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 SFMExample.cpp
14 * @brief A structure-from-motion problem on a simulated dataset
15 * @author Duy-Nguyen Ta
16 */
17
18// For loading the data, see the comments therein for scenario (camera rotates around cube)
19#include "SFMdata.h"
20
21// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
22#include <gtsam/geometry/Point2.h>
23
24// Each variable in the system (poses and landmarks) must be identified with a unique key.
25// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
26// Here we will use Symbols
27#include <gtsam/inference/Symbol.h>
28
29// In GTSAM, measurement functions are represented as 'factors'. Several common factors
30// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
31// Here we will use Projection factors to model the camera's landmark observations.
32// Also, we will initialize the robot at some location using a Prior factor.
33#include <gtsam/slam/ProjectionFactor.h>
34
35// When the factors are created, we will add them to a Factor Graph. As the factors we are using
36// are nonlinear factors, we will need a Nonlinear Factor Graph.
37#include <gtsam/nonlinear/NonlinearFactorGraph.h>
38
39// Finally, once all of the factors have been added to our factor graph, we will want to
40// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
41// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
42// trust-region method known as Powell's Dogleg
43#include <gtsam/nonlinear/DoglegOptimizer.h>
44
45// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
46// nonlinear functions around an initial linearization point, then solve the linear system
47// to update the linearization point. This happens repeatedly until the solver converges
48// to a consistent set of variable values. This requires us to specify an initial guess
49// for each variable, held in a Values container.
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 auto K = std::make_shared<Cal3_S2>(args: 50.0, args: 50.0, args: 0.0, args: 50.0, args: 50.0);
61
62 // Define the camera observation noise model
63 auto measurementNoise =
64 noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0); // one pixel in u and v
65
66 // Create the set of ground-truth landmarks
67 vector<Point3> points = createPoints();
68
69 // Create the set of ground-truth poses
70 vector<Pose3> poses = createPoses();
71
72 // Create a factor graph
73 NonlinearFactorGraph graph;
74
75 // Add a prior on pose x1. This indirectly specifies where the origin is.
76 auto poseNoise = noiseModel::Diagonal::Sigmas(
77 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3))
78 .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
79 graph.addPrior(key: Symbol('x', 0), prior: poses[0], model: poseNoise); // add directly to graph
80
81 // Simulated measurements from each camera pose, adding them to the factor
82 // graph
83 for (size_t i = 0; i < poses.size(); ++i) {
84 PinholeCamera<Cal3_S2> camera(poses[i], *K);
85 for (size_t j = 0; j < points.size(); ++j) {
86 Point2 measurement = camera.project(pw: points[j]);
87 graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
88 args&: measurement, args&: measurementNoise, args: Symbol('x', i), args: Symbol('l', j), args&: K);
89 }
90 }
91
92 // Because the structure-from-motion problem has a scale ambiguity, the
93 // problem is still under-constrained Here we add a prior on the position of
94 // the first landmark. This fixes the scale by indicating the distance between
95 // the first camera and the first landmark. All other landmark positions are
96 // interpreted using this scale.
97 auto pointNoise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
98 graph.addPrior(key: Symbol('l', 0), prior: points[0],
99 model: pointNoise); // add directly to graph
100 graph.print(str: "Factor Graph:\n");
101
102 // Create the data structure to hold the initial estimate to the solution
103 // Intentionally initialize the variables off from the ground truth
104 Values initialEstimate;
105 for (size_t i = 0; i < poses.size(); ++i) {
106 auto corrupted_pose = poses[i].compose(
107 g: Pose3(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25), Point3(0.05, -0.10, 0.20)));
108 initialEstimate.insert(
109 j: Symbol('x', i), val: corrupted_pose);
110 }
111 for (size_t j = 0; j < points.size(); ++j) {
112 Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
113 initialEstimate.insert<Point3>(j: Symbol('l', j), val: corrupted_point);
114 }
115 initialEstimate.print(str: "Initial Estimates:\n");
116
117 /* Optimize the graph and print results */
118 Values result = DoglegOptimizer(graph, initialEstimate).optimize();
119 result.print(str: "Final results:\n");
120 cout << "initial error = " << graph.error(values: initialEstimate) << endl;
121 cout << "final error = " << graph.error(values: result) << endl;
122
123 return 0;
124}
125/* ************************************************************************* */
126
127