SmartStereoProjectionFactorExample.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 
29 #include <gtsam/geometry/Pose3.h>
31 #include <gtsam/nonlinear/Values.h>
36 #include <gtsam/inference/Symbol.h>
37 #include <gtsam/slam/dataset.h>
38 
40 
41 #include <string>
42 #include <fstream>
43 #include <iostream>
44 
45 using namespace std;
46 using namespace gtsam;
48 
49 int main(int argc, char** argv){
50 
52 
53  bool output_poses = true;
54  string poseOutput("../../../examples/data/optimized_poses.txt");
55  string init_poseOutput("../../../examples/data/initial_poses.txt");
56  Values initial_estimate;
58  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
59  ofstream pose3Out, init_pose3Out;
60 
61  bool add_initial_noise = true;
62 
63  string calibration_loc = findExampleDataFile("VO_calibration.txt");
64  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
65  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
66 
67  //read camera calibration info from file
68  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
69  cout << "Reading calibration info" << endl;
70  ifstream calibration_file(calibration_loc.c_str());
71 
72  double fx, fy, s, u0, v0, b;
73  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
75 
76  cout << "Reading camera poses" << endl;
77  ifstream pose_file(pose_loc.c_str());
78 
79  int pose_id;
80  MatrixRowMajor m(4,4);
81  //read camera pose parameters and use to make initial estimates of camera poses
82  while (pose_file >> pose_id) {
83  for (int i = 0; i < 16; i++) {
84  pose_file >> m.data()[i];
85  }
86  if(add_initial_noise){
87  m(1,3) += (pose_id % 10)/10.0;
88  }
89  initial_estimate.insert(X(pose_id), Pose3(m));
90  }
91 
92  const auto initialPoses = initial_estimate.extract<Pose3>();
93  if (output_poses) {
94  init_pose3Out.open(init_poseOutput.c_str(), ios::out);
95  for (size_t i = 1; i <= initialPoses.size(); i++) {
96  init_pose3Out
97  << i << " "
98  << initialPoses.at(X(i)).matrix().format(
99  Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
100  }
101  }
102 
103  // camera and landmark keys
104  size_t x, l;
105 
106  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
107  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
108  double uL, uR, v, _X, Y, Z;
109  ifstream factor_file(factor_loc.c_str());
110  cout << "Reading stereo factors" << endl;
111 
112  //read stereo measurements and construct smart factors
113 
115  size_t current_l = 3; // hardcoded landmark ID from first measurement
116 
117  while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
118 
119  if(current_l != l) {
120  graph.push_back(factor);
122  current_l = l;
123  }
124  factor->add(StereoPoint2(uL,uR,v), X(x), K);
125  }
126 
127  Pose3 first_pose = initial_estimate.at<Pose3>(X(1));
128  //constrain the first pose such that it cannot change from its original value during optimization
129  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
130  // QR is much slower than Cholesky, but numerically more stable
132 
134  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
135  params.verbosity = NonlinearOptimizerParams::ERROR;
136 
137  cout << "Optimizing" << endl;
138  //create Levenberg-Marquardt optimizer to optimize the factor graph
139  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
140  Values result = optimizer.optimize();
141 
142  cout << "Final result sample:" << endl;
143  Values pose_values = utilities::allPose3s(result);
144  pose_values.print("Final camera poses:\n");
145 
146  if(output_poses){
147  pose3Out.open(poseOutput.c_str(),ios::out);
148  for(size_t i = 1; i<=pose_values.size(); i++){
149  pose3Out << i << " " << pose_values.at<Pose3>(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
150  " ", " ")) << endl;
151  }
152  cout << "Writing output" << endl;
153  }
154 
155  return 0;
156 }
utilities.h
Contains generic global functions designed particularly for the matlab interface.
SmartStereoProjectionPoseFactor.h
Smart stereo factor on poses, assuming camera calibration is fixed.
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
Eigen::IOFormat
Stores a set of parameters controlling the way matrices are printed.
Definition: IO.h:51
gtsam::Values::size
size_t size() const
Definition: Values.h:178
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
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
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
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
gtsam::SmartStereoProjectionPoseFactor
Definition: SmartStereoProjectionPoseFactor.h:46
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
l
static const Line3 l(Rot3(), 1, 1)
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Symbol.h
gtsam::Cal3_S2Stereo::shared_ptr
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
SmartFactor
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
Definition: ISAM2Example_SmartFactor.cpp:22
NonlinearEquality.h
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
out
std::ofstream out("Result.txt")
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
Eigen::StreamPrecision
@ StreamPrecision
Definition: IO.h:17
main
int main(int argc, char **argv)
Definition: SmartStereoProjectionFactorExample.cpp:49
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
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
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
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
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.
Pose3.h
3D Pose
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 Fri Nov 1 2024 03:35:31