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 SFMExample_SmartFactorPCG.cpp
14 * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient
15 * @author Frank Dellaert
16 */
17
18// For an explanation of these headers, see SFMExample_SmartFactor.cpp
19#include "SFMdata.h"
20#include <gtsam/slam/SmartProjectionPoseFactor.h>
21
22// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <gtsam/linear/Preconditioner.h>
25#include <gtsam/linear/PCGSolver.h>
26
27using namespace std;
28using namespace gtsam;
29
30// Make the typename short so it looks much cleaner
31typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
32
33// create a typedef to the camera type
34typedef PinholePose<Cal3_S2> Camera;
35
36/* ************************************************************************* */
37int main(int argc, char* argv[]) {
38 // Define the camera calibration parameters
39 Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
40
41 // Define the camera observation noise model
42 auto measurementNoise =
43 noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0); // one pixel in u and v
44
45 // Create the set of ground-truth landmarks and poses
46 vector<Point3> points = createPoints();
47 vector<Pose3> poses = createPoses();
48
49 // Create a factor graph
50 NonlinearFactorGraph graph;
51
52 // Simulated measurements from each camera pose, adding them to the factor graph
53 for (size_t j = 0; j < points.size(); ++j) {
54 // every landmark represent a single landmark, we use shared pointer to init
55 // the factor, and then insert measurements.
56 SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
57
58 for (size_t i = 0; i < poses.size(); ++i) {
59 // generate the 2D measurement
60 Camera camera(poses[i], K);
61 Point2 measurement = camera.project(pw: points[j]);
62
63 // call add() function to add measurement into a single factor
64 smartfactor->add(measured: measurement, key: i);
65 }
66
67 // insert the smart factor in the graph
68 graph.push_back(factor: smartfactor);
69 }
70
71 // Add a prior on pose x0. This indirectly specifies where the origin is.
72 // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
73 auto noise = noiseModel::Diagonal::Sigmas(
74 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3)).finished());
75 graph.addPrior(key: 0, prior: poses[0], model: noise);
76
77 // Fix the scale ambiguity by adding a prior
78 graph.addPrior(key: 1, prior: poses[0], model: noise);
79
80 // Create the initial estimate to the solution
81 Values initialEstimate;
82 Pose3 delta(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25), Point3(0.05, -0.10, 0.20));
83 for (size_t i = 0; i < poses.size(); ++i)
84 initialEstimate.insert(j: i, val: poses[i].compose(g: delta));
85
86 // We will use LM in the outer optimization loop, but by specifying
87 // "Iterative" below We indicate that an iterative linear solver should be
88 // used. In addition, the *type* of the iterativeParams decides on the type of
89 // iterative solver, in this case the SPCG (subgraph PCG)
90 LevenbergMarquardtParams parameters;
91 parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
92 parameters.absoluteErrorTol = 1e-10;
93 parameters.relativeErrorTol = 1e-10;
94 parameters.maxIterations = 500;
95 PCGSolverParameters::shared_ptr pcg =
96 std::make_shared<PCGSolverParameters>();
97 pcg->preconditioner = std::make_shared<BlockJacobiPreconditionerParameters>();
98 // Following is crucial:
99 pcg->epsilon_abs = 1e-10;
100 pcg->epsilon_rel = 1e-10;
101 parameters.iterativeParams = pcg;
102
103 LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
104 Values result = optimizer.optimize();
105
106 // Display result as in SFMExample_SmartFactor.run
107 result.print(str: "Final results:\n");
108 Values landmark_result;
109 for (size_t j = 0; j < points.size(); ++j) {
110 auto smart = std::dynamic_pointer_cast<SmartFactor>(r: graph[j]);
111 if (smart) {
112 std::optional<Point3> point = smart->point(values: result);
113 if (point) // ignore if std::optional return nullptr
114 landmark_result.insert(j, val: *point);
115 }
116 }
117
118 landmark_result.print(str: "Landmark results:\n");
119 cout << "final error: " << graph.error(values: result) << endl;
120 cout << "number of iterations: " << optimizer.iterations() << endl;
121
122 return 0;
123}
124/* ************************************************************************* */
125
126