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 ConcurrentCalibration.cpp
14* @brief First step towards estimating monocular calibration in concurrent
15* filter/smoother framework. To start with, just batch LM.
16* @date June 11, 2014
17* @author Chris Beall
18*/
19
20
21#include <gtsam/geometry/Pose3.h>
22#include <gtsam/nonlinear/Values.h>
23#include <gtsam/nonlinear/utilities.h>
24#include <gtsam/nonlinear/NonlinearEquality.h>
25#include <gtsam/nonlinear/NonlinearFactorGraph.h>
26#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
27#include <gtsam/inference/Symbol.h>
28#include <gtsam/slam/ProjectionFactor.h>
29#include <gtsam/slam/GeneralSFMFactor.h>
30#include <gtsam/slam/dataset.h>
31
32#include <string>
33#include <fstream>
34#include <iostream>
35
36using namespace std;
37using namespace gtsam;
38using symbol_shorthand::K;
39using symbol_shorthand::L;
40using symbol_shorthand::X;
41
42int main(int argc, char** argv){
43
44 Values initial_estimate;
45 NonlinearFactorGraph graph;
46 const auto model = noiseModel::Isotropic::Sigma(dim: 2,sigma: 1);
47
48 string calibration_loc = findExampleDataFile(name: "VO_calibration00s.txt");
49 string pose_loc = findExampleDataFile(name: "VO_camera_poses00s.txt");
50 string factor_loc = findExampleDataFile(name: "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(j: K(j: 0), val: noisy_K);
64
65 auto calNoise = noiseModel::Diagonal::Sigmas(sigmas: (Vector(5) << 500, 500, 1e-5, 100, 100).finished());
66 graph.addPrior(key: K(j: 0), prior: noisy_K, model: 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(j: Symbol('x', pose_id), val: Pose3(m));
79 }
80
81 auto poseNoise = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.01);
82 graph.addPrior(key: Symbol('x', pose_id), prior: Pose3(m), model: 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
96 graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(args: Point2(uL,v), args: model, args: X(j: x), args: L(j: l), args: K(j: 0));
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(j: L(j: l))) {
101 Pose3 camPose = initial_estimate.at<Pose3>(j: X(j: x));
102 //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
103 Point3 worldPoint = camPose.transformFrom(point: Point3(_X, Y, Z));
104 initial_estimate.insert(j: L(j: l), val: worldPoint);
105 }
106 }
107
108 Pose3 first_pose = initial_estimate.at<Pose3>(j: 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
112 graph.emplace_shared<NonlinearEquality<Pose3> >(args: Symbol('x',1),args&: first_pose);
113
114 cout << "Optimizing" << endl;
115 LevenbergMarquardtParams params;
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>(j: K(j: 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>(j: K(j: 0)).vector().transpose() << endl;
139
140 if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
141 } while(optimizer.iterations() < params.maxIterations &&
142 !checkConvergence(relativeErrorTreshold: params.relativeErrorTol, absoluteErrorTreshold: params.absoluteErrorTol,
143 errorThreshold: params.errorTol, currentError, newError: optimizer.error(), verbosity: params.verbosity));
144
145 Values result = optimizer.values();
146
147 cout << "Final result sample:" << endl;
148 Values pose_values = utilities::allPose3s(values: result);
149 pose_values.print(str: "Final camera poses:\n");
150
151 result.at<Cal3_S2>(j: K(j: 0)).print(s: "Final K\n");
152
153 noisy_K.print(s: "Initial noisy K\n");
154 true_K.print(s: "Initial correct K\n");
155
156 return 0;
157}
158