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 SelfCalibrationExample.cpp
14 * @brief Based on VisualSLAMExample, but with unknown (yet fixed) calibration.
15 * @author Frank Dellaert
16 */
17
18/*
19 * See the detailed documentation in Visual SLAM.
20 * The only documentation below with deal with the self-calibration.
21 */
22
23// For loading the data
24#include "SFMdata.h"
25
26// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
27#include <gtsam/geometry/Point2.h>
28
29// Inference and optimization
30#include <gtsam/inference/Symbol.h>
31#include <gtsam/nonlinear/NonlinearFactorGraph.h>
32#include <gtsam/nonlinear/DoglegOptimizer.h>
33#include <gtsam/nonlinear/Values.h>
34
35// SFM-specific factors
36#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
37
38// Standard headers
39#include <vector>
40
41using namespace std;
42using namespace gtsam;
43
44int main(int argc, char* argv[]) {
45 // Create the set of ground-truth
46 vector<Point3> points = createPoints();
47 vector<Pose3> poses = createPoses();
48
49 // Create the factor graph
50 NonlinearFactorGraph graph;
51
52 // Add a prior on pose x1.
53 auto poseNoise = noiseModel::Diagonal::Sigmas(
54 sigmas: (Vector(6) << Vector3::Constant(value: 0.1), Vector3::Constant(value: 0.3))
55 .finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
56 graph.addPrior(key: Symbol('x', 0), prior: poses[0], model: poseNoise);
57
58 // Simulated measurements from each camera pose, adding them to the factor
59 // graph
60 Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
61 auto measurementNoise =
62 noiseModel::Isotropic::Sigma(dim: 2, sigma: 1.0);
63 for (size_t i = 0; i < poses.size(); ++i) {
64 for (size_t j = 0; j < points.size(); ++j) {
65 PinholeCamera<Cal3_S2> camera(poses[i], K);
66 Point2 measurement = camera.project(pw: points[j]);
67 // The only real difference with the Visual SLAM example is that here we
68 // use a different factor type, that also calculates the Jacobian with
69 // respect to calibration
70 graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
71 args&: measurement, args&: measurementNoise, args: Symbol('x', i), args: Symbol('l', j),
72 args: Symbol('K', 0));
73 }
74 }
75
76 // Add a prior on the position of the first landmark.
77 auto pointNoise =
78 noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
79 graph.addPrior(key: Symbol('l', 0), prior: points[0],
80 model: pointNoise); // add directly to graph
81
82 // Add a prior on the calibration.
83 auto calNoise = noiseModel::Diagonal::Sigmas(
84 sigmas: (Vector(5) << 500, 500, 0.1, 100, 100).finished());
85 graph.addPrior(key: Symbol('K', 0), prior: K, model: calNoise);
86
87 // Create the initial estimate to the solution
88 // now including an estimate on the camera calibration parameters
89 Values initialEstimate;
90 initialEstimate.insert(j: Symbol('K', 0), val: Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
91 for (size_t i = 0; i < poses.size(); ++i)
92 initialEstimate.insert(
93 j: Symbol('x', i), val: poses[i].compose(g: Pose3(Rot3::Rodrigues(wx: -0.1, wy: 0.2, wz: 0.25),
94 Point3(0.05, -0.10, 0.20))));
95 for (size_t j = 0; j < points.size(); ++j)
96 initialEstimate.insert<Point3>(j: Symbol('l', j),
97 val: points[j] + Point3(-0.25, 0.20, 0.15));
98
99 /* Optimize the graph and print results */
100 Values result = DoglegOptimizer(graph, initialEstimate).optimize();
101 result.print(str: "Final results:\n");
102
103 return 0;
104}
105
106