1/**
2 * @file ISAM2Example_SmartFactor.cpp
3 * @brief test of iSAM with smart factors, led to bitbucket issue #367
4 * @author Alexander (pumaking on BitBucket)
5 */
6
7#include <gtsam/geometry/PinholeCamera.h>
8#include <gtsam/geometry/Cal3_S2.h>
9#include <gtsam/nonlinear/ISAM2.h>
10#include <gtsam/slam/BetweenFactor.h>
11#include <gtsam/slam/SmartProjectionPoseFactor.h>
12
13#include <iostream>
14#include <vector>
15
16using namespace std;
17using namespace gtsam;
18using symbol_shorthand::P;
19using symbol_shorthand::X;
20
21// Make the typename short so it looks much cleaner
22typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
23
24int main(int argc, char* argv[]) {
25 Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
26
27 auto measurementNoise =
28 noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0); // one pixel in u and v
29
30 Vector6 sigmas;
31 sigmas << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3);
32 auto noise = noiseModel::Diagonal::Sigmas(sigmas);
33
34 ISAM2Params parameters;
35 parameters.relinearizeThreshold = 0.01;
36 parameters.relinearizeSkip = 1;
37 parameters.cacheLinearizedFactors = false;
38 parameters.enableDetailedResults = true;
39 parameters.print();
40 ISAM2 isam(parameters);
41
42 // Create a factor graph
43 NonlinearFactorGraph graph;
44 Values initialEstimate;
45
46 Point3 point(0.0, 0.0, 1.0);
47
48 // Intentionally initialize the variables off from the ground truth
49 Pose3 delta(Rot3::Rodrigues(wx: 0.0, wy: 0.0, wz: 0.0), Point3(0.05, -0.10, 0.20));
50
51 Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0));
52 Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0));
53 Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0));
54 Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0));
55 Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0));
56
57 vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
58
59 // Add first pose
60 graph.addPrior(key: X(j: 0), prior: poses[0], model: noise);
61 initialEstimate.insert(j: X(j: 0), val: poses[0].compose(g: delta));
62
63 // Create smart factor with measurement from first pose only
64 SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
65 smartFactor->add(measured: PinholePose<Cal3_S2>(poses[0], K).project(pw: point), key: X(j: 0));
66 graph.push_back(factor: smartFactor);
67
68 // loop over remaining poses
69 for (size_t i = 1; i < 5; i++) {
70 cout << "****************************************************" << endl;
71 cout << "i = " << i << endl;
72
73 // Add prior on new pose
74 graph.addPrior(key: X(j: i), prior: poses[i], model: noise);
75 initialEstimate.insert(j: X(j: i), val: poses[i].compose(g: delta));
76
77 // "Simulate" measurement from this pose
78 PinholePose<Cal3_S2> camera(poses[i], K);
79 Point2 measurement = camera.project(pw: point);
80 cout << "Measurement " << i << "" << measurement << endl;
81
82 // Add measurement to smart factor
83 smartFactor->add(measured: measurement, key: X(j: i));
84
85 // Update iSAM2
86 ISAM2Result result = isam.update(newFactors: graph, newTheta: initialEstimate);
87 result.print();
88
89 cout << "Detailed results:" << endl;
90 for (auto& [key, status] : result.detail->variableStatus) {
91 PrintKey(key);
92 cout << " {" << endl;
93 cout << "reeliminated: " << status.isReeliminated << endl;
94 cout << "relinearized above thresh: " << status.isAboveRelinThreshold
95 << endl;
96 cout << "relinearized involved: " << status.isRelinearizeInvolved << endl;
97 cout << "relinearized: " << status.isRelinearized << endl;
98 cout << "observed: " << status.isObserved << endl;
99 cout << "new: " << status.isNew << endl;
100 cout << "in the root clique: " << status.inRootClique << endl;
101 cout << "}" << endl;
102 }
103
104 Values currentEstimate = isam.calculateEstimate();
105 currentEstimate.print(str: "Current estimate: ");
106
107 auto pointEstimate = smartFactor->point(values: currentEstimate);
108 if (pointEstimate) {
109 cout << *pointEstimate << endl;
110 } else {
111 cout << "Point degenerate." << endl;
112 }
113
114 // Reset graph and initial estimate for next iteration
115 graph.resize(size: 0);
116 initialEstimate.clear();
117 }
118
119 return 0;
120}
121