ConcurrentCalibration.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 
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/nonlinear/Values.h>
27 #include <gtsam/inference/Symbol.h>
30 #include <gtsam/slam/dataset.h>
31 
32 #include <string>
33 #include <fstream>
34 #include <iostream>
35 
36 using namespace std;
37 using namespace gtsam;
41 
42 int main(int argc, char** argv){
43 
44  Values initial_estimate;
46  const auto model = noiseModel::Isotropic::Sigma(2,1);
47 
48  string calibration_loc = findExampleDataFile("VO_calibration00s.txt");
49  string pose_loc = findExampleDataFile("VO_camera_poses00s.txt");
50  string factor_loc = findExampleDataFile("VO_stereo_factors00s.txt");
51 
52  //read camera calibration info from file
53  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
54  double fx, fy, s, u0, v0, b;
55  ifstream calibration_file(calibration_loc.c_str());
56  cout << "Reading calibration info" << endl;
57  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
58 
59  //create stereo camera calibration object
60  const Cal3_S2 true_K(fx,fy,s,u0,v0);
61  const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10);
62 
63  initial_estimate.insert(K(0), noisy_K);
64 
65  auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
66  graph.addPrior(K(0), noisy_K, calNoise);
67 
68 
69  ifstream pose_file(pose_loc.c_str());
70  cout << "Reading camera poses" << endl;
71  int pose_id;
72  MatrixRowMajor m(4,4);
73  //read camera pose parameters and use to make initial estimates of camera poses
74  while (pose_file >> pose_id) {
75  for (int i = 0; i < 16; i++) {
76  pose_file >> m.data()[i];
77  }
78  initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
79  }
80 
81  auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
82  graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise);
83 
84  // camera and landmark keys
85  size_t x, l;
86 
87  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
88  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
89  double uL, uR, v, _X, Y, Z;
90  ifstream factor_file(factor_loc.c_str());
91  cout << "Reading stereo factors" << endl;
92  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
93  while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
94 // graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, X(x), L(l), K);
95 
97 
98 
99  //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
100  if (!initial_estimate.exists(L(l))) {
101  Pose3 camPose = initial_estimate.at<Pose3>(X(x));
102  //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
103  Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z));
104  initial_estimate.insert(L(l), worldPoint);
105  }
106  }
107 
108  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
109  //constrain the first pose such that it cannot change from its original value during optimization
110  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
111  // QR is much slower than Cholesky, but numerically more stable
113 
114  cout << "Optimizing" << endl;
116  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
117  params.verbosity = NonlinearOptimizerParams::ERROR;
118 
119  //create Levenberg-Marquardt optimizer to optimize the factor graph
120  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate,params);
121 // Values result = optimizer.optimize();
122 
123  string K_values_file = "K_values.txt";
124  ofstream stream_K(K_values_file.c_str());
125 
126  double currentError;
127 
128 
129  stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
130 
131 
132  // Iterative loop
133  do {
134  // Do next iteration
135  currentError = optimizer.error();
136  optimizer.iterate();
137 
138  stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
139 
140  if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
141  } while(optimizer.iterations() < params.maxIterations &&
142  !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
143  params.errorTol, currentError, optimizer.error(), params.verbosity));
144 
145  Values result = optimizer.values();
146 
147  cout << "Final result sample:" << endl;
148  Values pose_values = utilities::allPose3s(result);
149  pose_values.print("Final camera poses:\n");
150 
151  result.at<Cal3_S2>(K(0)).print("Final K\n");
152 
153  noisy_K.print("Initial noisy K\n");
154  true_K.print("Initial correct K\n");
155 
156  return 0;
157 }
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
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
gtsam::LevenbergMarquardtOptimizer::iterate
GaussianFactorGraph::shared_ptr iterate() override
Definition: LevenbergMarquardtOptimizer.cpp:273
gtsam::NonlinearOptimizer::values
const Values & values() const
return values in current optimizer state
Definition: NonlinearOptimizer.cpp:57
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::GeneralSFMFactor2
Definition: GeneralSFMFactor.h:208
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
GeneralSFMFactor.h
a general SFM factor with an unknown calibration
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::NonlinearOptimizer::error
double error() const
return error in current optimizer state
Definition: NonlinearOptimizer.cpp:49
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Definition: NonlinearOptimizer.cpp:182
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
dataset.h
utility functions for loading datasets
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
l
static const Line3 l(Rot3(), 1, 1)
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
L
MatrixXd L
Definition: LLT_example.cpp:6
Symbol.h
camPose
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
NonlinearEquality.h
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
gtsam::NonlinearOptimizer::iterations
size_t iterations() const
return number of iterations in current optimizer state
Definition: NonlinearOptimizer.cpp:53
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::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
main
int main(int argc, char **argv)
Definition: ConcurrentCalibration.cpp:42
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
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::Cal3_S2::print
void print(const std::string &s="Cal3_S2") const override
print with optional string
Definition: Cal3_S2.cpp:34
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
Z
#define Z
Definition: icosphere.cpp:21
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
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::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09