SFMExample_SmartFactor.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 
20 // In GTSAM, measurement functions are represented as 'factors'.
21 // The factor we used here is SmartProjectionPoseFactor.
22 // Every smart factor represent a single landmark, seen from multiple cameras.
23 // The SmartProjectionPoseFactor only optimizes for the poses of a camera,
24 // not the calibration, which is assumed known.
26 
27 // For an explanation of these headers, see SFMExample.cpp
28 #include "SFMdata.h"
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 // Make the typename short so it looks much cleaner
36 
37 // create a typedef to the camera type
39 
40 /* ************************************************************************* */
41 int main(int argc, char* argv[]) {
42 
43  // Define the camera calibration parameters
44  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
45 
46  // Define the camera observation noise model
47  auto measurementNoise =
48  noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
49 
50  // Create the set of ground-truth landmarks and poses
51  vector<Point3> points = createPoints();
52  vector<Pose3> poses = createPoses();
53 
54  // Create a factor graph
56 
57  // Simulated measurements from each camera pose, adding them to the factor graph
58  for (size_t j = 0; j < points.size(); ++j) {
59 
60  // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
61  SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
62 
63  for (size_t i = 0; i < poses.size(); ++i) {
64 
65  // generate the 2D measurement
66  Camera camera(poses[i], K);
67  Point2 measurement = camera.project(points[j]);
68 
69  // call add() function to add measurement into a single factor, here we need to add:
70  // 1. the 2D measurement
71  // 2. the corresponding camera's key
72  // 3. camera noise model
73  // 4. camera calibration
74  smartfactor->add(measurement, i);
75  }
76 
77  // insert the smart factor in the graph
78  graph.push_back(smartfactor);
79  }
80 
81  // Add a prior on pose x0. This indirectly specifies where the origin is.
82  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
83  auto noise = noiseModel::Diagonal::Sigmas(
84  (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
85  graph.addPrior(0, poses[0], noise);
86 
87  // Because the structure-from-motion problem has a scale ambiguity, the problem is
88  // still under-constrained. Here we add a prior on the second pose x1, so this will
89  // fix the scale by indicating the distance between x0 and x1.
90  // Because these two are fixed, the rest of the poses will be also be fixed.
91  graph.addPrior(1, poses[1], noise); // add directly to graph
92 
93  graph.print("Factor Graph:\n");
94 
95  // Create the initial estimate to the solution
96  // Intentionally initialize the variables off from the ground truth
97  Values initialEstimate;
98  Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
99  for (size_t i = 0; i < poses.size(); ++i)
100  initialEstimate.insert(i, poses[i].compose(delta));
101  initialEstimate.print("Initial Estimates:\n");
102 
103  // Optimize the graph and print results
104  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
105  Values result = optimizer.optimize();
106  result.print("Final results:\n");
107 
108  // A smart factor represent the 3D point inside the factor, not as a variable.
109  // The 3D position of the landmark is not explicitly calculated by the optimizer.
110  // To obtain the landmark's 3D position, we use the "point" method of the smart factor.
111  Values landmark_result;
112  for (size_t j = 0; j < points.size(); ++j) {
113 
114  // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
115  SmartFactor::shared_ptr smart = std::dynamic_pointer_cast<SmartFactor>(graph[j]);
116  if (smart) {
117  // The output of point() is in std::optional<Point3>, as sometimes
118  // the triangulation operation inside smart factor will encounter degeneracy.
119  auto point = smart->point(result);
120  if (point) // ignore if std::optional return nullptr
121  landmark_result.insert(j, *point);
122  }
123  }
124 
125  landmark_result.print("Landmark results:\n");
126  cout << "final error: " << graph.error(result) << endl;
127  cout << "number of iterations: " << optimizer.iterations() << endl;
128 
129  return 0;
130 }
131 /* ************************************************************************* */
132 
Smart factor on poses, assuming camera calibration is fixed.
virtual const Values & optimize()
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Vector2 Point2
Definition: Point2.h:32
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
size_t iterations() const
return number of iterations in current optimizer state
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
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
Values result
int main(int argc, char *argv[])
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
double error(const Values &values) const
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
PinholePose< Cal3_S2 > Camera
Vector3 Point3
Definition: Point3.h:38
Definition: camera.h:36
static const CalibratedCamera camera(kDefaultPose)
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
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