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 VisualISAMExample.cpp
14 * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
15 * This version uses iSAM to solve the problem incrementally
16 * @author Duy-Nguyen Ta
17 * @author Frank Dellaert
18 */
19
20/**
21 * A structure-from-motion example with landmarks
22 * - The landmarks form a 10 meter cube
23 * - The robot rotates around the landmarks, always facing towards the cube
24 */
25
26// For loading the data
27#include "SFMdata.h"
28
29// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
30#include <gtsam/geometry/Point2.h>
31
32// Each variable in the system (poses and landmarks) must be identified with a unique key.
33// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
34// Here we will use Symbols
35#include <gtsam/inference/Symbol.h>
36
37// In GTSAM, measurement functions are represented as 'factors'. Several common factors
38// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
39// Here we will use Projection factors to model the camera's landmark observations.
40// Also, we will initialize the robot at some location using a Prior factor.
41#include <gtsam/slam/ProjectionFactor.h>
42
43// We want to use iSAM to solve the structure-from-motion problem incrementally, so
44// include iSAM here
45#include <gtsam/nonlinear/NonlinearISAM.h>
46
47// iSAM requires as input a set set of new factors to be added stored in a factor graph,
48// and initial guesses for any new variables used in the added factors
49#include <gtsam/nonlinear/NonlinearFactorGraph.h>
50#include <gtsam/nonlinear/Values.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
63 auto noise = noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0); // one pixel in u and v
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 a NonlinearISAM object which will relinearize and reorder the variables
72 // every "relinearizeInterval" updates
73 int relinearizeInterval = 3;
74 NonlinearISAM isam(relinearizeInterval);
75
76 // Create a Factor Graph and Values to hold the new data
77 NonlinearFactorGraph graph;
78 Values initialEstimate;
79
80 // Loop over the different poses, adding the observations to iSAM incrementally
81 for (size_t i = 0; i < poses.size(); ++i) {
82 // Add factors for each landmark observation
83 for (size_t j = 0; j < points.size(); ++j) {
84 // Create ground truth measurement
85 PinholeCamera<Cal3_S2> camera(poses[i], *K);
86 Point2 measurement = camera.project(pw: points[j]);
87 // Add measurement
88 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(args&: measurement, args&: noise,
89 args: Symbol('x', i), args: Symbol('l', j), args&: K);
90 }
91
92 // Intentionally initialize the variables off from the ground truth
93 Pose3 noise(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25), Point3(0.05, -0.10, 0.20));
94 Pose3 initial_xi = poses[i].compose(g: noise);
95
96 // Add an initial guess for the current pose
97 initialEstimate.insert(j: Symbol('x', i), val: initial_xi);
98
99 // If this is the first iteration, add a prior on the first pose to set the coordinate frame
100 // and a prior on the first landmark to set the scale
101 // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
102 // adding it to iSAM.
103 if (i == 0) {
104 // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
105 auto poseNoise = noiseModel::Diagonal::Sigmas(
106 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3)).finished());
107 graph.addPrior(key: Symbol('x', 0), prior: poses[0], model: poseNoise);
108
109 // Add a prior on landmark l0
110 auto pointNoise =
111 noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
112 graph.addPrior(key: Symbol('l', 0), prior: points[0], model: pointNoise);
113
114 // Add initial guesses to all observed landmarks
115 Point3 noise(-0.25, 0.20, 0.15);
116 for (size_t j = 0; j < points.size(); ++j) {
117 // Intentionally initialize the variables off from the ground truth
118 Point3 initial_lj = points[j] + noise;
119 initialEstimate.insert(j: Symbol('l', j), val: initial_lj);
120 }
121
122 } else {
123 // Update iSAM with the new factors
124 isam.update(newFactors: graph, initialValues: initialEstimate);
125 Values currentEstimate = isam.estimate();
126 cout << "****************************************************" << endl;
127 cout << "Frame " << i << ": " << endl;
128 currentEstimate.print(str: "Current estimate: ");
129
130 // Clear the factor graph and values for the next iteration
131 graph.resize(size: 0);
132 initialEstimate.clear();
133 }
134 }
135
136 return 0;
137}
138/* ************************************************************************* */
139