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 EssentialViewGraphExample.cpp
14 * @brief View-graph calibration with essential matrices.
15 * @author Frank Dellaert
16 * @date October 2024
17 */
18
19#include <gtsam/geometry/Cal3f.h>
20#include <gtsam/geometry/EssentialMatrix.h>
21#include <gtsam/geometry/PinholeCamera.h>
22#include <gtsam/geometry/Point2.h>
23#include <gtsam/geometry/Point3.h>
24#include <gtsam/geometry/Pose3.h>
25#include <gtsam/inference/EdgeKey.h>
26#include <gtsam/inference/Symbol.h>
27#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
29#include <gtsam/nonlinear/Values.h>
30#include <gtsam/sfm/TransferFactor.h> // Contains EssentialTransferFactorK
31
32#include <vector>
33
34#include "SFMdata.h" // For createPoints() and posesOnCircle()
35
36using namespace std;
37using namespace gtsam;
38using namespace symbol_shorthand; // For K(symbol)
39
40// Main function
41int main(int argc, char* argv[]) {
42 // Define the camera calibration parameters
43 Cal3f cal(50.0, 50.0, 50.0);
44
45 // Create the set of 8 ground-truth landmarks
46 vector<Point3> points = createPoints();
47
48 // Create the set of 4 ground-truth poses
49 vector<Pose3> poses = posesOnCircle(num_cameras: 4, R: 30);
50
51 // Calculate ground truth essential matrices, 1 and 2 poses apart
52 auto E1 = EssentialMatrix::FromPose3(1P2_: poses[0].between(g: poses[1]));
53 auto E2 = EssentialMatrix::FromPose3(1P2_: poses[0].between(g: poses[2]));
54
55 // Simulate measurements from each camera pose
56 std::array<std::array<Point2, 8>, 4> p;
57 for (size_t i = 0; i < 4; ++i) {
58 PinholeCamera<Cal3f> camera(poses[i], cal);
59 for (size_t j = 0; j < 8; ++j) {
60 p[i][j] = camera.project(pw: points[j]);
61 }
62 }
63
64 // Create the factor graph
65 NonlinearFactorGraph graph;
66 using Factor = EssentialTransferFactorK<Cal3f>;
67
68 for (size_t a = 0; a < 4; ++a) {
69 size_t b = (a + 1) % 4; // Next camera
70 size_t c = (a + 2) % 4; // Camera after next
71
72 // Vectors to collect tuples for each factor
73 std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
74
75 // Collect data for the three factors
76 for (size_t j = 0; j < 8; ++j) {
77 tuples1.emplace_back(args&: p[a][j], args&: p[b][j], args&: p[c][j]);
78 tuples2.emplace_back(args&: p[a][j], args&: p[c][j], args&: p[b][j]);
79 tuples3.emplace_back(args&: p[c][j], args&: p[b][j], args&: p[a][j]);
80 }
81
82 // Add transfer factors between views a, b, and c.
83 graph.emplace_shared<Factor>(args: EdgeKey(a, c), args: EdgeKey(b, c), args&: tuples1);
84 graph.emplace_shared<Factor>(args: EdgeKey(a, b), args: EdgeKey(b, c), args&: tuples2);
85 graph.emplace_shared<Factor>(args: EdgeKey(a, c), args: EdgeKey(a, b), args&: tuples3);
86 }
87
88 // Formatter for printing keys
89 auto formatter = [](Key key) {
90 if (Symbol(key).chr() == 'k') {
91 return (string)Symbol(key);
92 } else {
93 EdgeKey edge(key);
94 return (std::string)edge;
95 }
96 };
97
98 graph.print(str: "Factor Graph:\n", keyFormatter: formatter);
99
100 // Create a delta vector to perturb the ground truth (small perturbation)
101 Vector5 delta;
102 delta << 1, 1, 1, 1, 1;
103 delta *= 1e-2;
104
105 // Create the initial estimate for essential matrices
106 Values initialEstimate;
107 for (size_t a = 0; a < 4; ++a) {
108 size_t b = (a + 1) % 4; // Next camera
109 size_t c = (a + 2) % 4; // Camera after next
110 initialEstimate.insert(j: EdgeKey(a, b), val: E1.retract(xi: delta));
111 initialEstimate.insert(j: EdgeKey(a, c), val: E2.retract(xi: delta));
112 }
113
114 // Insert initial calibrations (using K symbol)
115 for (size_t i = 0; i < 4; ++i) {
116 initialEstimate.insert(j: K(j: i), val: cal);
117 }
118
119 initialEstimate.print(str: "Initial Estimates:\n", keyFormatter: formatter);
120 graph.printErrors(values: initialEstimate, str: "Initial Errors:\n", keyFormatter: formatter);
121
122 // Optimize the graph and print results
123 LevenbergMarquardtParams params;
124 params.setlambdaInitial(1000.0); // Initialize lambda to a high value
125 params.setVerbosityLM("SUMMARY");
126 Values result =
127 LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
128
129 cout << "Initial error = " << graph.error(values: initialEstimate) << endl;
130 cout << "Final error = " << graph.error(values: result) << endl;
131
132 result.print(str: "Final Results:\n", keyFormatter: formatter);
133
134 E1.print(s: "Ground Truth E1:\n");
135 E2.print(s: "Ground Truth E2:\n");
136
137 return 0;
138}