doc/Code/VisualISAMExample.cpp
Go to the documentation of this file.
2 NonlinearISAM isam(relinearizeInterval);
3 
4 // ... first frame initialization omitted ...
5 
6 // Loop over the different poses, adding the observations to iSAM
7 for (size_t i = 1; i < poses.size(); ++i) {
8 
9  // Add factors for each landmark observation
10  NonlinearFactorGraph graph;
11  for (size_t j = 0; j < points.size(); ++j) {
12  graph.add(
13  GenericProjectionFactor<Pose3, Point3, Cal3_S2>
14  (z[i][j], noise,Symbol('x', i), Symbol('l', j), K)
15  );
16  }
17 
18  // Add an initial guess for the current pose
19  Values initialEstimate;
20  initialEstimate.insert(Symbol('x', i), initial_x[i]);
21 
22  // Update iSAM with the new factors
23  isam.update(graph, initialEstimate);
24  }
int relinearizeInterval
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
NonlinearFactorGraph graph
NonlinearISAM isam(relinearizeInterval)
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:45