1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file timeShonanFactor.cpp
14 * @brief time ShonanFactor with BAL file
15 * @author Frank Dellaert
16 * @date 2019
17 */
18
19#include <gtsam/base/timing.h>
20#include <gtsam/geometry/Pose3.h>
21#include <gtsam/linear/NoiseModel.h>
22#include <gtsam/linear/PCGSolver.h>
23#include <gtsam/linear/SubgraphPreconditioner.h>
24#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
25#include <gtsam/nonlinear/NonlinearEquality.h>
26#include <gtsam/nonlinear/NonlinearFactorGraph.h>
27#include <gtsam/nonlinear/Values.h>
28#include <gtsam/slam/PriorFactor.h>
29#include <gtsam/slam/dataset.h>
30#include <gtsam/sfm/ShonanFactor.h>
31
32#include <iostream>
33#include <random>
34#include <string>
35#include <vector>
36
37using namespace std;
38using namespace gtsam;
39
40static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(dim: 2);
41
42int main(int argc, char* argv[]) {
43 // primitive argument parsing:
44 if (argc > 3) {
45 throw runtime_error("Usage: timeShonanFactor [g2oFile]");
46 }
47
48 string g2oFile;
49 try {
50 if (argc > 1)
51 g2oFile = argv[argc - 1];
52 else
53 g2oFile = findExampleDataFile(name: "sphere_smallnoise.graph");
54 } catch (const exception& e) {
55 cerr << e.what() << '\n';
56 exit(status: 1);
57 }
58
59 // Read G2O file
60 const auto measurements = parseMeasurements<Rot3>(filename: g2oFile);
61 const auto poses = parseVariables<Pose3>(filename: g2oFile);
62
63 // Build graph
64 NonlinearFactorGraph graph;
65 // graph.add(NonlinearEquality<SOn>(0, SOn::Identity(4)));
66 auto priorModel = noiseModel::Isotropic::Sigma(dim: 6, sigma: 10000);
67 graph.add(factorOrContainer: PriorFactor<SOn>(0, SOn::Identity(n: 4), priorModel));
68 auto G = std::make_shared<Matrix>(args: SOn::VectorizedGenerators(n: 4));
69 for (const auto &m : measurements) {
70 const auto &keys = m.keys();
71 const Rot3 &Rij = m.measured();
72 const auto &model = m.noiseModel();
73 graph.emplace_shared<ShonanFactor3>(
74 args: keys[0], args: keys[1], args: Rij, args: 4, args: model, args&: G);
75 }
76
77 std::mt19937 rng(42);
78
79 // Set parameters to be similar to ceres
80 LevenbergMarquardtParams params;
81 LevenbergMarquardtParams::SetCeresDefaults(&params);
82 params.setLinearSolverType("MULTIFRONTAL_QR");
83 // params.setVerbosityLM("SUMMARY");
84 // params.linearSolverType = LevenbergMarquardtParams::Iterative;
85 // auto pcg = std::make_shared<PCGSolverParameters>();
86 // pcg->preconditioner =
87 // std::make_shared<SubgraphPreconditionerParameters>();
88 // std::make_shared<BlockJacobiPreconditionerParameters>();
89 // params.iterativeParams = pcg;
90
91 // Optimize
92 for (size_t i = 0; i < 100; i++) {
93 gttic_(optimize);
94 Values initial;
95 initial.insert(j: 0, val: SOn::Identity(n: 4));
96 for (size_t j = 1; j < poses.size(); j++) {
97 initial.insert(j, val: SOn::Random(rng, n: 4));
98 }
99 LevenbergMarquardtOptimizer lm(graph, initial, params);
100 Values result = lm.optimize();
101 cout << "cost = " << graph.error(values: result) << endl;
102 }
103
104 tictoc_finishedIteration_();
105 tictoc_print_();
106
107 return 0;
108}
109