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
12/**
13 * @file VisualISAM2Example.cpp
14 * @brief A visualSLAM example for the structure-from-motion problem on a
15 * simulated dataset This version uses iSAM2 to solve the problem incrementally
16 * @author Duy-Nguyen Ta
17 */
18
19/**
20 * A structure-from-motion example with landmarks
21 * - The landmarks form a 10 meter cube
22 * - The robot rotates around the landmarks, always facing towards the cube
23 */
24
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
42#include <gtsam/nonlinear/NonlinearFactorGraph.h>
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.
50#include <gtsam/slam/ProjectionFactor.h>
51
52#include <vector>
53
54using namespace std;
55using namespace gtsam;
56
57/* ************************************************************************* */
58int 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(dim: 2, sigma: 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.
78 ISAM2Params parameters;
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
84 NonlinearFactorGraph graph;
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(pw: points[j]);
93 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
94 args&: measurement, args&: measurementNoise, args: Symbol('x', i), args: Symbol('l', j), args&: 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(wx: -0.1, wy: 0.2, wz: 0.25),
100 Point3(0.05, -0.10, 0.20));
101 initialEstimate.insert(j: Symbol('x', i), val: 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 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3))
111 .finished());
112 graph.addPrior(key: Symbol('x', 0), prior: poses[0], model: kPosePrior);
113
114 // Add a prior on landmark l0
115 static auto kPointPrior = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
116 graph.addPrior(key: Symbol('l', 0), prior: points[0], model: 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>(j: Symbol('l', j), val: points[j] + kDeltaPoint);
123
124 } else {
125 // Update iSAM with the new factors
126 isam.update(newFactors: graph, newTheta: 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(str: "Current estimate: ");
136
137 // Clear the factor graph and values for the next iteration
138 graph.resize(size: 0);
139 initialEstimate.clear();
140 }
141 }
142
143 return 0;
144}
145/* ************************************************************************* */
146