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 ViewGraphExample.cpp
14 * @brief View-graph calibration on a simulated dataset, a la Sweeney 2015
15 * @author Frank Dellaert
16 * @date October 2024
17 */
18
19#include <gtsam/geometry/Cal3_S2.h>
20#include <gtsam/geometry/PinholeCamera.h>
21#include <gtsam/geometry/Point2.h>
22#include <gtsam/geometry/Point3.h>
23#include <gtsam/geometry/Pose3.h>
24#include <gtsam/inference/EdgeKey.h>
25#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
26#include <gtsam/nonlinear/NonlinearFactorGraph.h>
27#include <gtsam/nonlinear/Values.h>
28#include <gtsam/sfm/TransferFactor.h>
29
30#include <vector>
31
32#include "SFMdata.h"
33#include "gtsam/inference/Key.h"
34
35using namespace std;
36using namespace gtsam;
37
38/* ************************************************************************* */
39int main(int argc, char* argv[]) {
40 // Define the camera calibration parameters
41 Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0);
42
43 // Create the set of 8 ground-truth landmarks
44 vector<Point3> points = createPoints();
45
46 // Create the set of 4 ground-truth poses
47 vector<Pose3> poses = posesOnCircle(num_cameras: 4, R: 30);
48
49 // Calculate ground truth fundamental matrices, 1 and 2 poses apart
50 auto F1 = FundamentalMatrix(cal.K(), poses[0].between(g: poses[1]), cal.K());
51 auto F2 = FundamentalMatrix(cal.K(), poses[0].between(g: poses[2]), cal.K());
52
53 // Simulate measurements from each camera pose
54 std::array<std::array<Point2, 8>, 4> p;
55 for (size_t i = 0; i < 4; ++i) {
56 PinholeCamera<Cal3_S2> camera(poses[i], cal);
57 for (size_t j = 0; j < 8; ++j) {
58 p[i][j] = camera.project(pw: points[j]);
59 }
60 }
61
62 // This section of the code is inspired by the work of Sweeney et al.
63 // [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph
64 // calibration. The graph is made up of transfer factors that enforce the
65 // epipolar constraint between corresponding points across three views, as
66 // described in the paper. Rather than adding one ternary error term per point
67 // in a triplet, we add three binary factors for sparsity during optimization.
68 // In this version, we only include triplets between 3 successive cameras.
69 NonlinearFactorGraph graph;
70 using Factor = TransferFactor<FundamentalMatrix>;
71
72 for (size_t a = 0; a < 4; ++a) {
73 size_t b = (a + 1) % 4; // Next camera
74 size_t c = (a + 2) % 4; // Camera after next
75
76 // Vectors to collect tuples for each factor
77 std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
78
79 // Collect data for the three factors
80 for (size_t j = 0; j < 8; ++j) {
81 tuples1.emplace_back(args&: p[a][j], args&: p[b][j], args&: p[c][j]);
82 tuples2.emplace_back(args&: p[a][j], args&: p[c][j], args&: p[b][j]);
83 tuples3.emplace_back(args&: p[c][j], args&: p[b][j], args&: p[a][j]);
84 }
85
86 // Add transfer factors between views a, b, and c. Note that the EdgeKeys
87 // are crucial in performing the transfer in the right direction. We use
88 // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
89 // matrices we will optimize for.
90 graph.emplace_shared<Factor>(args: EdgeKey(a, c), args: EdgeKey(b, c), args&: tuples1);
91 graph.emplace_shared<Factor>(args: EdgeKey(a, b), args: EdgeKey(b, c), args&: tuples2);
92 graph.emplace_shared<Factor>(args: EdgeKey(a, c), args: EdgeKey(a, b), args&: tuples3);
93 }
94
95 auto formatter = [](Key key) {
96 EdgeKey edge(key);
97 return (std::string)edge;
98 };
99
100 graph.print(str: "Factor Graph:\n", keyFormatter: formatter);
101
102 // Create a delta vector to perturb the ground truth
103 // We can't really go far before convergence becomes problematic :-(
104 Vector7 delta;
105 delta << 1, 2, 3, 4, 5, 6, 7;
106 delta *= 1e-5;
107
108 // Create the data structure to hold the initial estimate to the solution
109 Values initialEstimate;
110 for (size_t a = 0; a < 4; ++a) {
111 size_t b = (a + 1) % 4; // Next camera
112 size_t c = (a + 2) % 4; // Camera after next
113 initialEstimate.insert(j: EdgeKey(a, b), val: F1.retract(delta));
114 initialEstimate.insert(j: EdgeKey(a, c), val: F2.retract(delta));
115 }
116 initialEstimate.print(str: "Initial Estimates:\n", keyFormatter: formatter);
117 graph.printErrors(values: initialEstimate, str: "errors: ", keyFormatter: formatter);
118
119 /* Optimize the graph and print results */
120 LevenbergMarquardtParams params;
121 params.setlambdaInitial(1000.0); // Initialize lambda to a high value
122 params.setVerbosityLM("SUMMARY");
123 Values result =
124 LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
125
126 cout << "initial error = " << graph.error(values: initialEstimate) << endl;
127 cout << "final error = " << graph.error(values: result) << endl;
128
129 result.print(str: "Final results:\n", keyFormatter: formatter);
130
131 cout << "Ground Truth F1:\n" << F1.matrix() << endl;
132 cout << "Ground Truth F2:\n" << F2.matrix() << endl;
133
134 return 0;
135}
136/* ************************************************************************* */
137