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