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;
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 /* ************************************************************************* */
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::createPoints
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
Definition: SFMdata.h:43
gtsam::NonlinearISAM::update
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
Definition: NonlinearISAM.cpp:35
Point2.h
2D Point
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Definition: FactorGraph.h:367
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Pose3
Definition: Pose3.h:37
main
int main(int argc, char *argv[])
Definition: VisualISAM2Example.cpp:58
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
isam
NonlinearISAM isam(relinearizeInterval)
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
Symbol.h
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::createPoses
std::vector< Pose3 > createPoses(const Pose3 &init=Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), const Pose3 &delta=Pose3(Rot3::Ypr(0, -M_PI_4, 0), {sin(M_PI_4) *30, 0, 30 *(1 - sin(M_PI_4))}), int steps=8)
Definition: SFMdata.h:62
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
camera
static const CalibratedCamera camera(kDefaultPose)
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
SFMdata.h
Simple example for the structure-from-motion problems.
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
measurement
static Point2 measurement(323.0, 240.0)
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:43