VisualISAM2Example.cpp
Go to the documentation of this file.
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 
25 // For loading the data
26 #include "SFMdata.h"
27 
28 // Camera observations of landmarks will be stored as Point2 (x, y).
29 #include <gtsam/geometry/Point2.h>
30 
31 // Each variable in the system (poses and landmarks) must be identified with a
32 // unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
33 // (X1, X2, L1). Here we will use Symbols
34 #include <gtsam/inference/Symbol.h>
35 
36 // We want to use iSAM2 to solve the structure-from-motion problem
37 // incrementally, so include iSAM2 here
38 #include <gtsam/nonlinear/ISAM2.h>
39 
40 // iSAM2 requires as input a set of new factors to be added stored in a factor
41 // graph, and initial guesses for any new variables used in the added factors
43 #include <gtsam/nonlinear/Values.h>
44 
45 // In GTSAM, measurement functions are represented as 'factors'. Several common
46 // factors have been provided with the library for solving robotics/SLAM/Bundle
47 // Adjustment problems. Here we will use Projection factors to model the
48 // camera's landmark observations. Also, we will initialize the robot at some
49 // location using a Prior factor.
51 
52 #include <vector>
53 
54 using namespace std;
55 using namespace gtsam;
56 
57 /* ************************************************************************* */
58 int main(int argc, char* argv[]) {
59  // Define the camera calibration parameters
60  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
61 
62  // Define the camera observation noise model, 1 pixel stddev
63  auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
64 
65  // Create the set of ground-truth landmarks
66  vector<Point3> points = createPoints();
67 
68  // Create the set of ground-truth poses
69  vector<Pose3> poses = createPoses();
70 
71  // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
72  // to maintain proper linearization and efficient variable ordering, iSAM2
73  // performs partial relinearization/reordering at each step. A parameter
74  // structure is available that allows the user to set various properties, such
75  // as the relinearization threshold and type of linear solver. For this
76  // example, we we set the relinearization threshold small so the iSAM2 result
77  // will approach the batch result.
79  parameters.relinearizeThreshold = 0.01;
80  parameters.relinearizeSkip = 1;
81  ISAM2 isam(parameters);
82 
83  // Create a Factor Graph and Values to hold the new data
85  Values initialEstimate;
86 
87  // Loop over the poses, adding the observations to iSAM incrementally
88  for (size_t i = 0; i < poses.size(); ++i) {
89  // Add factors for each landmark observation
90  for (size_t j = 0; j < points.size(); ++j) {
91  PinholeCamera<Cal3_S2> camera(poses[i], *K);
92  Point2 measurement = camera.project(points[j]);
94  measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
95  }
96 
97  // Add an initial guess for the current pose
98  // Intentionally initialize the variables off from the ground truth
99  static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
100  Point3(0.05, -0.10, 0.20));
101  initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
102 
103  // If this is the first iteration, add a prior on the first pose to set the
104  // coordinate frame and a prior on the first landmark to set the scale Also,
105  // as iSAM solves incrementally, we must wait until each is observed at
106  // least twice before adding it to iSAM.
107  if (i == 0) {
108  // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
109  static auto kPosePrior = noiseModel::Diagonal::Sigmas(
110  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
111  .finished());
112  graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
113 
114  // Add a prior on landmark l0
115  static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
116  graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
117 
118  // Add initial guesses to all observed landmarks
119  // Intentionally initialize the variables off from the ground truth
120  static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
121  for (size_t j = 0; j < points.size(); ++j)
122  initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
123 
124  } else {
125  // Update iSAM with the new factors
126  isam.update(graph, initialEstimate);
127  // Each call to iSAM2 update(*) performs one iteration of the iterative
128  // nonlinear solver. If accuracy is desired at the expense of time,
129  // update(*) can be called additional times to perform multiple optimizer
130  // iterations every step.
131  isam.update();
132  Values currentEstimate = isam.calculateEstimate();
133  cout << "****************************************************" << endl;
134  cout << "Frame " << i << ": " << endl;
135  currentEstimate.print("Current estimate: ");
136 
137  // Clear the factor graph and values for the next iteration
138  graph.resize(0);
139  initialEstimate.clear();
140  }
141  }
142 
143  return 0;
144 }
145 /* ************************************************************************* */
void clear()
Definition: Values.h:298
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
Vector2 Point2
Definition: Point2.h:32
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
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::vector< gtsam::Point3 > createPoints()
Definition: SFMdata.h:39
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
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
NonlinearISAM isam(relinearizeInterval)
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
Reprojection of a LANDMARK to a 2D point.
static ConjugateGradientParameters parameters
RelinearizationThreshold relinearizeThreshold
Definition: ISAM2Params.h:169
traits
Definition: chartTesting.h:28
virtual void resize(size_t size)
Definition: FactorGraph.h:389
int main(int argc, char *argv[])
Simple example for the structure-from-motion problems.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
2D Point
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Values calculateEstimate() const
Definition: ISAM2.cpp:766
std::ptrdiff_t j
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4) *30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
Definition: SFMdata.h:56


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