SmartProjectionFactorExample.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 
30 #include <gtsam/slam/dataset.h>
32 #include <gtsam/nonlinear/Values.h>
37 #include <gtsam/inference/Symbol.h>
38 
39 #include <string>
40 #include <fstream>
41 #include <iostream>
42 
43 using namespace std;
44 using namespace gtsam;
45 
46 int main(int argc, char** argv){
48 
49  Values initial_estimate;
51  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
52 
53  string calibration_loc = findExampleDataFile("VO_calibration.txt");
54  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
55  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
56 
57  //read camera calibration info from file
58  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
59  cout << "Reading calibration info" << endl;
60  ifstream calibration_file(calibration_loc.c_str());
61 
62  double fx, fy, s, u0, v0, b;
63  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
64  const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0));
65 
66  cout << "Reading camera poses" << endl;
67  ifstream pose_file(pose_loc.c_str());
68 
69  int pose_index;
70  MatrixRowMajor m(4,4);
71  //read camera pose parameters and use to make initial estimates of camera poses
72  while (pose_file >> pose_index) {
73  for (int i = 0; i < 16; i++)
74  pose_file >> m.data()[i];
75  initial_estimate.insert(pose_index, Pose3(m));
76  }
77 
78  // landmark keys
79  size_t landmark_key;
80 
81  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
82  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
83  double uL, uR, v, X, Y, Z;
84  ifstream factor_file(factor_loc.c_str());
85  cout << "Reading stereo factors" << endl;
86 
87  //read stereo measurements and construct smart factors
88 
90  size_t current_l = 3; // hardcoded landmark ID from first measurement
91 
92  while (factor_file >> pose_index >> landmark_key >> uL >> uR >> v >> X >> Y >> Z) {
93 
94  if(current_l != landmark_key) {
95  graph.push_back(factor);
97  current_l = landmark_key;
98  }
99  factor->add(Point2(uL,v), pose_index);
100  }
101 
102  Pose3 firstPose = initial_estimate.at<Pose3>(1);
103  //constrain the first pose such that it cannot change from its original value during optimization
104  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
105  // QR is much slower than Cholesky, but numerically more stable
107 
109  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
110  params.verbosity = NonlinearOptimizerParams::ERROR;
111 
112  cout << "Optimizing" << endl;
113  //create Levenberg-Marquardt optimizer to optimize the factor graph
114  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
115  Values result = optimizer.optimize();
116 
117  cout << "Final result sample:" << endl;
118  Values pose_values = utilities::allPose3s(result);
119  pose_values.print("Final camera poses:\n");
120 
121  return 0;
122 }
utilities.h
Contains generic global functions designed particularly for the matlab interface.
gtsam::utilities::allPose3s
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:150
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
s
RealScalar s
Definition: level1_cplx_impl.h:126
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
b
Scalar * b
Definition: benchVecAdd.cpp:17
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
X
#define X
Definition: icosphere.cpp:20
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::NonlinearEquality
Definition: NonlinearEquality.h:43
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
dataset.h
utility functions for loading datasets
main
int main(int argc, char **argv)
Definition: SmartProjectionFactorExample.cpp:46
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SmartProjectionPoseFactor
Definition: SmartProjectionPoseFactor.h:45
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
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
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: ISAM2Example_SmartFactor.cpp:22
NonlinearEquality.h
SmartProjectionPoseFactor.h
Smart factor on poses, assuming camera calibration is fixed.
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
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
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
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
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
gtsam::SmartProjectionPoseFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactor.h:59
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
Z
#define Z
Definition: icosphere.cpp:21
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48


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