SFMExample.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 
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.
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.
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 Degleg
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 
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
63  auto measurementNoise =
64  noiseModel::Isotropic::Sigma(2, 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
74 
75  // Add a prior on pose x1. This indirectly specifies where the origin is.
76  auto poseNoise = noiseModel::Diagonal::Sigmas(
77  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
78  .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
79  graph.addPrior(Symbol('x', 0), poses[0], 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(points[j]);
88  measurement, measurementNoise, Symbol('x', i), Symbol('l', j), 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(3, 0.1);
98  graph.addPrior(Symbol('l', 0), points[0],
99  pointNoise); // add directly to graph
100  graph.print("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  Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
108  initialEstimate.insert(
109  Symbol('x', i), 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>(Symbol('l', j), corrupted_point);
114  }
115  initialEstimate.print("Initial Estimates:\n");
116 
117  /* Optimize the graph and print results */
118  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
119  result.print("Final results:\n");
120  cout << "initial error = " << graph.error(initialEstimate) << endl;
121  cout << "final error = " << graph.error(result) << endl;
122 
123  return 0;
124 }
125 /* ************************************************************************* */
126 
virtual const Values & optimize()
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
int main(int argc, char *argv[])
Definition: SFMExample.cpp:58
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
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
Values result
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
double error(const Values &values) const
Reprojection of a LANDMARK to a 2D point.
traits
Definition: chartTesting.h:28
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
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
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:35:42