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 TriangulationLOSTExample.cpp
14 * @author Akshay Krishnan
15 * @brief This example runs triangulation several times using 3 different
16 * approaches: LOST, DLT, and DLT with optimization. It reports the covariance
17 * and the runtime for each approach.
18 *
19 * @date 2022-07-10
20 */
21#include <gtsam/geometry/Cal3_S2.h>
22#include <gtsam/geometry/PinholeCamera.h>
23#include <gtsam/geometry/Point2.h>
24#include <gtsam/geometry/Point3.h>
25#include <gtsam/geometry/Pose3.h>
26#include <gtsam/geometry/Rot3.h>
27#include <gtsam/geometry/triangulation.h>
28
29#include <chrono>
30#include <iostream>
31#include <random>
32#include <optional>
33
34using namespace std;
35using namespace gtsam;
36
37static std::mt19937 rng(42);
38
39void PrintCovarianceStats(const Matrix& mat, const std::string& method) {
40 Matrix centered = mat.rowwise() - mat.colwise().mean();
41 Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
42 std::cout << method << " covariance: " << std::endl;
43 std::cout << cov << std::endl;
44 std::cout << "Trace sqrt: " << sqrt(x: cov.trace()) << std::endl << std::endl;
45}
46
47void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
48 const std::string& method) {
49 double nanoseconds = dur.count() / num_samples;
50 std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3
51 << std::endl;
52}
53
54void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
55 std::vector<Pose3>* poses, Point3* point,
56 Point2Vector* measurements) {
57 const double minXY = -10, maxXY = 10;
58 const double minZ = -20, maxZ = 0;
59 const int nrCameras = 500;
60 cameras->reserve(n: nrCameras);
61 poses->reserve(n: nrCameras);
62 measurements->reserve(n: nrCameras);
63 *point = Point3(0.0, 0.0, 10.0);
64
65 std::uniform_real_distribution<double> rand_xy(minXY, maxXY);
66 std::uniform_real_distribution<double> rand_z(minZ, maxZ);
67 Cal3_S2 identityK;
68
69 for (int i = 0; i < nrCameras; ++i) {
70 Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
71 Pose3 wTi(Rot3(), wti);
72
73 poses->push_back(x: wTi);
74 cameras->emplace_back(args&: wTi, args&: identityK);
75 measurements->push_back(x: cameras->back().project(pw: *point));
76 }
77}
78
79void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
80 std::vector<Pose3>* poses, Point3* point,
81 Point2Vector* measurements) {
82 Pose3 pose1;
83 Pose3 pose2(Rot3(), Point3(5., 0., -5.));
84 Cal3_S2 identityK;
85 PinholeCamera<Cal3_S2> camera1(pose1, identityK);
86 PinholeCamera<Cal3_S2> camera2(pose2, identityK);
87
88 *point = Point3(0, 0, 1);
89 cameras->push_back(x: camera1);
90 cameras->push_back(x: camera2);
91 *poses = {pose1, pose2};
92 *measurements = {camera1.project(pw: *point), camera2.project(pw: *point)};
93}
94
95Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
96 const double measurementSigma) {
97 std::normal_distribution<double> normal(0.0, measurementSigma);
98
99 Point2Vector noisyMeasurements;
100 noisyMeasurements.reserve(n: measurements.size());
101 for (const auto& p : measurements) {
102 noisyMeasurements.emplace_back(args: p.x() + normal(rng), args: p.y() + normal(rng));
103 }
104 return noisyMeasurements;
105}
106
107/* ************************************************************************* */
108int main(int argc, char* argv[]) {
109 CameraSet<PinholeCamera<Cal3_S2>> cameras;
110 std::vector<Pose3> poses;
111 Point3 landmark;
112 Point2Vector measurements;
113 GetLargeCamerasDataset(cameras: &cameras, poses: &poses, point: &landmark, measurements: &measurements);
114 // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements);
115 const double measurementSigma = 1e-2;
116 SharedNoiseModel measurementNoise =
117 noiseModel::Isotropic::Sigma(dim: 2, sigma: measurementSigma);
118
119 const long int nrTrials = 1000;
120 Matrix errorsDLT = Matrix::Zero(rows: nrTrials, cols: 3);
121 Matrix errorsLOST = Matrix::Zero(rows: nrTrials, cols: 3);
122 Matrix errorsDLTOpt = Matrix::Zero(rows: nrTrials, cols: 3);
123
124 double rank_tol = 1e-9;
125 std::shared_ptr<Cal3_S2> calib = std::make_shared<Cal3_S2>();
126 std::chrono::nanoseconds durationDLT{};
127 std::chrono::nanoseconds durationDLTOpt{};
128 std::chrono::nanoseconds durationLOST{};
129
130 for (int i = 0; i < nrTrials; i++) {
131 Point2Vector noisyMeasurements =
132 AddNoiseToMeasurements(measurements, measurementSigma);
133
134 auto lostStart = std::chrono::high_resolution_clock::now();
135 auto estimateLOST = triangulatePoint3<Cal3_S2>(
136 cameras, measurements: noisyMeasurements, rank_tol, optimize: false, model: measurementNoise, useLOST: true);
137 durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
138
139 auto dltStart = std::chrono::high_resolution_clock::now();
140 auto estimateDLT = triangulatePoint3<Cal3_S2>(
141 cameras, measurements: noisyMeasurements, rank_tol, optimize: false, model: measurementNoise, useLOST: false);
142 durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
143
144 auto dltOptStart = std::chrono::high_resolution_clock::now();
145 auto estimateDLTOpt = triangulatePoint3<Cal3_S2>(
146 cameras, measurements: noisyMeasurements, rank_tol, optimize: true, model: measurementNoise, useLOST: false);
147 durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
148
149 errorsLOST.row(i) = estimateLOST - landmark;
150 errorsDLT.row(i) = estimateDLT - landmark;
151 errorsDLTOpt.row(i) = estimateDLTOpt - landmark;
152 }
153 PrintCovarianceStats(mat: errorsLOST, method: "LOST");
154 PrintCovarianceStats(mat: errorsDLT, method: "DLT");
155 PrintCovarianceStats(mat: errorsDLTOpt, method: "DLT_OPT");
156
157 PrintDuration(dur: durationLOST, num_samples: nrTrials, method: "LOST");
158 PrintDuration(dur: durationDLT, num_samples: nrTrials, method: "DLT");
159 PrintDuration(dur: durationDLTOpt, num_samples: nrTrials, method: "DLT_OPT");
160}
161