ISAM2Example_SmartFactor.cpp
Go to the documentation of this file.
1 
12 
13 #include <iostream>
14 #include <vector>
15 
16 using namespace std;
17 using namespace gtsam;
20 
21 // Make the typename short so it looks much cleaner
23 
24 int 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(2, 1.0); // one pixel in u and v
29 
30  Vector6 sigmas;
31  sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
32  auto noise = noiseModel::Diagonal::Sigmas(sigmas);
33 
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
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(0.0, 0.0, 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(X(0), poses[0], noise);
61  initialEstimate.insert(X(0), poses[0].compose(delta));
62 
63  // Create smart factor with measurement from first pose only
64  SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
65  smartFactor->add(PinholePose<Cal3_S2>(poses[0], K).project(point), X(0));
66  graph.push_back(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(X(i), poses[i], noise);
75  initialEstimate.insert(X(i), poses[i].compose(delta));
76 
77  // "Simulate" measurement from this pose
78  PinholePose<Cal3_S2> camera(poses[i], K);
79  Point2 measurement = camera.project(point);
80  cout << "Measurement " << i << "" << measurement << endl;
81 
82  // Add measurement to smart factor
83  smartFactor->add(measurement, X(i));
84 
85  // Update iSAM2
86  ISAM2Result result = isam.update(graph, 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("Current estimate: ");
106 
107  auto pointEstimate = smartFactor->point(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(0);
116  initialEstimate.clear();
117  }
118 
119  return 0;
120 }
const gtsam::Symbol key('X', 0)
void print(const std::string str="") const
Print results.
Definition: ISAM2Result.h:172
void clear()
Definition: Values.h:298
Smart factor on poses, assuming camera calibration is fixed.
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
void print(const std::string &str="") const
print iSAM2 parameters
Definition: ISAM2Params.h:253
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Base class for all pinhole cameras.
static Point2 measurement(323.0, 240.0)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Values result
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
static Pose3 pose3(rt3, pt3)
NonlinearISAM isam(relinearizeInterval)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
static ConjugateGradientParameters parameters
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
RelinearizationThreshold relinearizeThreshold
Definition: ISAM2Params.h:169
traits
Definition: chartTesting.h:28
virtual void resize(size_t size)
Definition: FactorGraph.h:389
static Pose3 pose2
void insert(Key j, const Value &val)
Definition: Values.cpp:155
void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
Utility function to print one key with optional prefix.
Definition: Key.cpp:41
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
#define X
Definition: icosphere.cpp:20
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Values calculateEstimate() const
Definition: ISAM2.cpp:766
std::optional< DetailedResults > detail
Definition: ISAM2Result.h:156
The most common 5DOF 3D->2D calibration.
int main(int argc, char *argv[])


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:26