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 
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::createPoints
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
Definition: SFMdata.h:43
Camera
Definition: camera.h:36
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
main
int main(int argc, char *argv[])
Definition: SFMExample_SmartFactor.cpp:41
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
SmartProjectionPoseFactor.h
Smart factor on poses, assuming camera calibration is fixed.
gtsam::NonlinearOptimizer::iterations
size_t iterations() const
return number of iterations in current optimizer state
Definition: NonlinearOptimizer.cpp:53
gtsam::createPoses
std::vector< Pose3 > createPoses(const Pose3 &init=Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), const Pose3 &delta=Pose3(Rot3::Ypr(0, -M_PI_4, 0), {sin(M_PI_4) *30, 0, 30 *(1 - sin(M_PI_4))}), int steps=8)
Definition: SFMdata.h:62
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Camera
PinholePose< Cal3_S2 > Camera
Definition: SFMExample_SmartFactor.cpp:38
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::PinholePose
Definition: PinholePose.h:239
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
SFMdata.h
Simple example for the structure-from-motion problems.
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
measurement
static Point2 measurement(323.0, 240.0)
gtsam::CalibratedCamera::project
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:188
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: SFMExample_SmartFactor.cpp:35
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactorGraph.cpp:55


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:11