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
12/**
13* @file SmartProjectionFactorExample.cpp
14* @brief A stereo visual odometry example
15* @date May 30, 2014
16* @author Stephen Camp
17* @author Chris Beall
18*/
19
20
21/**
22 * A smart projection factor example based on stereo data, throwing away the
23 * measurement from the right camera
24 * -robot starts at origin
25 * -moves forward, taking periodic stereo measurements
26 * -makes monocular observations of many landmarks
27 */
28
29#include <gtsam/geometry/Pose3.h>
30#include <gtsam/geometry/Cal3_S2Stereo.h>
31#include <gtsam/nonlinear/Values.h>
32#include <gtsam/nonlinear/utilities.h>
33#include <gtsam/nonlinear/NonlinearEquality.h>
34#include <gtsam/nonlinear/NonlinearFactorGraph.h>
35#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
36#include <gtsam/inference/Symbol.h>
37#include <gtsam/slam/dataset.h>
38
39#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
40
41#include <string>
42#include <fstream>
43#include <iostream>
44
45using namespace std;
46using namespace gtsam;
47using symbol_shorthand::X;
48
49int main(int argc, char** argv){
50
51 typedef SmartStereoProjectionPoseFactor SmartFactor;
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;
57 NonlinearFactorGraph graph;
58 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(dim: 3,sigma: 1);
59 ofstream pose3Out, init_pose3Out;
60
61 bool add_initial_noise = true;
62
63 string calibration_loc = findExampleDataFile(name: "VO_calibration.txt");
64 string pose_loc = findExampleDataFile(name: "VO_camera_poses_large.txt");
65 string factor_loc = findExampleDataFile(name: "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;
74 const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(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(j: X(j: pose_id), val: Pose3(m));
90 }
91
92 const auto initialPoses = initial_estimate.extract<Pose3>();
93 if (output_poses) {
94 init_pose3Out.open(s: init_poseOutput.c_str(), mode: ios::out);
95 for (size_t i = 1; i <= initialPoses.size(); i++) {
96 init_pose3Out
97 << i << " "
98 << initialPoses.at(k: X(j: i)).matrix().format(
99 fmt: 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
114 SmartFactor::shared_ptr factor(new SmartFactor(model));
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);
121 factor = SmartFactor::shared_ptr(new SmartFactor(model));
122 current_l = l;
123 }
124 factor->add(measured: StereoPoint2(uL,uR,v), poseKey: X(j: x), K);
125 }
126
127 Pose3 first_pose = initial_estimate.at<Pose3>(j: X(j: 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
131 graph.emplace_shared<NonlinearEquality<Pose3> >(args: X(j: 1),args&: first_pose);
132
133 LevenbergMarquardtParams params;
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(values: result);
144 pose_values.print(str: "Final camera poses:\n");
145
146 if(output_poses){
147 pose3Out.open(s: poseOutput.c_str(),mode: ios::out);
148 for(size_t i = 1; i<=pose_values.size(); i++){
149 pose3Out << i << " " << pose_values.at<Pose3>(j: X(j: i)).matrix().format(fmt: Eigen::IOFormat(Eigen::StreamPrecision, 0,
150 " ", " ")) << endl;
151 }
152 cout << "Writing output" << endl;
153 }
154
155 return 0;
156}
157