1/* ----------------------------------------------------------------------------
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 * Atlanta, Georgia 30332-0415
4 * All Rights Reserved
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 * See LICENSE for the license information
7 * -------------------------------------------------------------------------- */
8
9/**
10 * @file testSmartStereoFactor_iSAM2.cpp
11 * @brief Unit tests for ProjectionFactor Class
12 * @author Jose Luis Blanco-Claraco
13 * @date May 2019
14 *
15 * @note Originally based on ISAM2_SmartFactorStereo.cpp by Nghia Ho
16 */
17
18#include <CppUnitLite/TestHarness.h>
19
20#include <gtsam/base/debug.h>
21#include <gtsam/nonlinear/ISAM2.h>
22#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
23#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
24
25#include <array>
26#include <fstream>
27#include <iostream>
28#include <sstream>
29#include <string>
30#include <vector>
31
32// Set to 1 to enable verbose output of intermediary results
33#define TEST_VERBOSE_OUTPUT 0
34
35#if TEST_VERBOSE_OUTPUT
36#define TEST_COUT(ARGS_) std::cout << ARGS_
37#else
38#define TEST_COUT(ARGS_) void(0)
39#endif
40
41// Tolerance for ground-truth pose comparison:
42static const double tol = 1e-3;
43
44// Synthetic dataset generated with rwt
45// (https://github.com/jlblancoc/recursive-world-toolkit)
46// Camera parameters
47const double fx = 200.0;
48const double fy = 150.0;
49const double cx = 512.0;
50const double cy = 384.0;
51const double baseline = 0.2; // meters
52
53using timestep_t = std::size_t;
54using lm_id_t = int;
55
56struct stereo_meas_t {
57 stereo_meas_t(lm_id_t id, double lu, double ru, double v_lr)
58 : lm_id{id}, left_u{lu}, right_u{ru}, v{v_lr} {}
59
60 lm_id_t lm_id{-1}; // landmark id
61 double left_u{0}, right_u{0}, v{0};
62};
63
64static std::map<timestep_t, std::vector<stereo_meas_t>> dataset = {
65 {0,
66 {{0, 911.99993896, 712.00000000, 384.0},
67 {159, 311.99996948, 211.99996948, 384.0},
68 {3, 378.66665649, 312.00000000, 384.0},
69 {2, 645.33331299, 578.66662598, 384.0},
70 {157, 111.99994659, 11.99993896, 384.0},
71 {4, 578.66662598, 545.33331299, 384.0},
72 {5, 445.33331299, 412.00000000, 384.0},
73 {6, 562.00000000, 537.00000000, 384.0}}},
74 {1,
75 {{0, 1022.06353760, 762.57519531, 384.0},
76 {159, 288.30487061, 177.80273438, 384.0},
77 {2, 655.30645752, 583.12127686, 384.0},
78 {3, 368.60937500, 297.43176270, 384.0},
79 {4, 581.82666016, 547.16766357, 384.0},
80 {5, 443.66183472, 409.23681641, 384.0},
81 {6, 564.35980225, 538.62115479, 384.0},
82 {7, 461.66418457, 436.05477905, 384.0},
83 {8, 550.32220459, 531.75256348, 384.0},
84 {9, 476.17767334, 457.67541504, 384.0}}},
85 {2,
86 {{159, 257.97128296, 134.26287842, 384.0},
87 {2, 666.87255859, 588.07275391, 384.0},
88 {3, 356.53823853, 280.10061646, 384.0},
89 {4, 585.10949707, 548.99212646, 384.0},
90 {5, 441.66403198, 406.05108643, 384.0},
91 {6, 566.75402832, 540.21868896, 384.0},
92 {7, 461.16207886, 434.90002441, 384.0},
93 {8, 552.28387451, 533.30230713, 384.0},
94 {9, 476.63549805, 457.79418945, 384.0},
95 {10, 546.48394775, 530.53009033, 384.0}}},
96 {3,
97 {{159, 218.10592651, 77.30914307, 384.0},
98 {2, 680.54644775, 593.68103027, 384.0},
99 {3, 341.92507935, 259.28231812, 384.0},
100 {4, 588.53289795, 550.80499268, 384.0},
101 {5, 439.29989624, 402.39105225, 384.0},
102 {6, 569.18627930, 541.78991699, 384.0},
103 {7, 460.47863770, 433.51678467, 384.0},
104 {8, 554.24902344, 534.82952881, 384.0},
105 {9, 477.00451660, 457.80438232, 384.0},
106 {10, 548.33770752, 532.07501221, 384.0},
107 {11, 483.58688354, 467.47830200, 384.0},
108 {12, 542.36785889, 529.29321289, 384.0}}},
109 {4,
110 {{2, 697.09454346, 600.18432617, 384.0},
111 {3, 324.03643799, 233.97094727, 384.0},
112 {4, 592.11877441, 552.60449219, 384.0},
113 {5, 436.52197266, 398.19531250, 384.0},
114 {6, 571.66101074, 543.33209229, 384.0},
115 {7, 459.59658813, 431.88333130, 384.0},
116 {8, 556.21801758, 536.33258057, 384.0},
117 {9, 477.27893066, 457.69882202, 384.0},
118 {10, 550.18920898, 533.60003662, 384.0},
119 {11, 484.24472046, 467.86862183, 384.0},
120 {12, 544.14727783, 530.86157227, 384.0},
121 {13, 491.26141357, 478.11267090, 384.0},
122 {14, 541.29949951, 529.57086182, 384.0},
123 {15, 494.58111572, 482.95935059, 384.0}}}};
124
125// clang-format off
126/*
127% Ground truth path of the SENSOR, and the ROBOT
128% STEP X Y Z QR QX QY QZ | X Y Z QR QX QY QZ
129 ----------------------------------------------------------------------------------------------------------------------------------------
130 0 0.000000 0.000000 0.000000 0.500000 -0.500000 0.500000 -0.500000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000
131 1 0.042019 -0.008403 0.000000 0.502446 -0.502446 0.497542 -0.497542 0.042019 -0.008403 0.000000 0.999988 0.000000 0.000000 0.004905
132 2 0.084783 -0.016953 0.000000 0.504879 -0.504879 0.495073 -0.495073 0.084783 -0.016953 0.000000 0.999952 0.000000 0.000000 0.009806
133 3 0.128305 -0.025648 0.000000 0.507299 -0.507299 0.492592 -0.492592 0.128305 -0.025648 0.000000 0.999892 0.000000 0.000000 0.014707
134 4 0.172605 -0.034490 0.000000 0.509709 -0.509709 0.490098 -0.490098 0.172605 -0.034490 0.000000 0.999808 0.000000 0.000000 0.019611
135*/
136// clang-format on
137
138// Ground truth using camera pose = vehicle frame
139// The table above uses:
140// camera +x = vehicle -y
141// camera +y = vehicle -z
142// camera +z = vehicle +x
143static const std::map<timestep_t, gtsam::Point3> gt_positions = {
144 {0, {0.000000, 0.000000, 0.0}},
145 {1, {0.042019, -0.008403, 0.0}},
146 {2, {0.084783, -0.016953, 0.0}},
147 {3, {0.128305, -0.025648, 0.0}},
148 {4, {0.172605, -0.034490, 0.0}}};
149
150// Batch version, to compare against iSAM2 solution.
151TEST(testISAM2SmartFactor, Stereo_Batch) {
152 TEST_COUT("============ Running: Batch ============\n");
153
154 using namespace gtsam;
155 using symbol_shorthand::V;
156 using symbol_shorthand::X;
157
158 const auto K =
159 std::make_shared<Cal3_S2Stereo>(args: fx, args: fy, args: .0, args: cx, args: cy, args: baseline);
160
161 // Pose prior - at identity
162 auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
163 sigmas: (Vector(6) << Vector3::Constant(value: 0.2), Vector3::Constant(value: 0.2)).finished());
164
165 // Map: landmark_id => smart_factor_index inside iSAM2
166 std::map<lm_id_t, FactorIndex> lm2factor;
167
168 // Storage of smart factors:
169 std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
170
171 NonlinearFactorGraph batch_graph;
172 Values batch_values;
173
174 // Run one timestep at once:
175 for (const auto &entries : dataset) {
176 // 1) Process new observations:
177 // ------------------------------
178 const auto kf_id = entries.first;
179 const std::vector<stereo_meas_t> &obs = entries.second;
180
181 for (const stereo_meas_t &stObs : obs) {
182 if (smartFactors.count(x: stObs.lm_id) == 0) {
183 auto noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
184 SmartProjectionParams parm(HESSIAN, ZERO_ON_DEGENERACY);
185
186 smartFactors[stObs.lm_id] =
187 std::make_shared<SmartStereoProjectionPoseFactor>(args&: noise, args&: parm);
188
189 batch_graph.push_back(factor: smartFactors[stObs.lm_id]);
190 }
191
192 TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
193 << stObs.lm_id << "\n");
194
195 smartFactors[stObs.lm_id]->add(
196 measured: StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), poseKey: X(j: kf_id), K);
197 }
198
199 // prior, for the first keyframe:
200 if (kf_id == 0) {
201 batch_graph.addPrior(key: X(j: kf_id), prior: Pose3::Identity(), model: priorPoseNoise);
202 }
203
204 batch_values.insert(j: X(j: kf_id), val: Pose3::Identity());
205 }
206
207 LevenbergMarquardtParams parameters;
208#if TEST_VERBOSE_OUTPUT
209 parameters.verbosity = NonlinearOptimizerParams::LINEAR;
210 parameters.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
211#endif
212
213 LevenbergMarquardtOptimizer lm(batch_graph, batch_values, parameters);
214
215 Values finalEstimate = lm.optimize();
216#if TEST_VERBOSE_OUTPUT
217 finalEstimate.print("LevMarq estimate:");
218#endif
219
220 // GT:
221 // camera +x = vehicle -y
222 // camera +y = vehicle -z
223 // camera +z = vehicle +x
224 for (const auto &gt : gt_positions) {
225 const Pose3 p = finalEstimate.at<Pose3>(j: X(j: gt.first));
226 EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
227 EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
228 EXPECT(assert_equal(p.z(), gt.second.x(), tol));
229 }
230}
231
232TEST(testISAM2SmartFactor, Stereo_iSAM2) {
233 TEST_COUT("======= Running: iSAM2 ==========\n");
234
235#if TEST_VERBOSE_OUTPUT
236 SETDEBUG("ISAM2 update", true);
237 // SETDEBUG("ISAM2 update verbose",true);
238#endif
239
240 using namespace gtsam;
241 using symbol_shorthand::V;
242 using symbol_shorthand::X;
243
244 const auto K =
245 std::make_shared<Cal3_S2Stereo>(args: fx, args: fy, args: .0, args: cx, args: cy, args: baseline);
246
247 ISAM2Params parameters;
248 parameters.relinearizeThreshold = 0.01;
249 parameters.evaluateNonlinearError = true;
250
251 // Do not cache smart factors:
252 parameters.cacheLinearizedFactors = false;
253
254 // Important: must set relinearizeSkip=1 to additional calls to update() to
255 // have a real effect.
256 parameters.relinearizeSkip = 1;
257
258 ISAM2 isam(parameters);
259
260 // Pose prior - at identity
261 auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
262 sigmas: (Vector(6) << Vector3::Constant(value: 0.2), Vector3::Constant(value: 0.2)).finished());
263
264 // Map: landmark_id => smart_factor_index inside iSAM2
265 std::map<lm_id_t, FactorIndex> lm2factor;
266
267 // Storage of smart factors:
268 std::map<lm_id_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
269
270 Pose3 lastKeyframePose = Pose3::Identity();
271
272 // Run one timestep at once:
273 for (const auto &entries : dataset) {
274 // 1) Process new observations:
275 // ------------------------------
276 const auto kf_id = entries.first;
277 const std::vector<stereo_meas_t> &obs = entries.second;
278
279 // Special instructions for using iSAM2 + smart factors:
280 // Must fill factorNewAffectedKeys:
281 FastMap<FactorIndex, KeySet> factorNewAffectedKeys;
282 NonlinearFactorGraph newFactors;
283
284 // Map: factor index in the internal graph of iSAM2 => landmark_id
285 std::map<FactorIndex, lm_id_t> newFactor2lm;
286
287 for (const stereo_meas_t &stObs : obs) {
288 if (smartFactors.count(x: stObs.lm_id) == 0) {
289 auto noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
290 SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
291
292 smartFactors[stObs.lm_id] =
293 std::make_shared<SmartStereoProjectionPoseFactor>(args&: noise, args&: params);
294 newFactor2lm[newFactors.size()] = stObs.lm_id;
295 newFactors.push_back(factor: smartFactors[stObs.lm_id]);
296 } else {
297 // Only if the factor *already* existed:
298 factorNewAffectedKeys[lm2factor.at(k: stObs.lm_id)].insert(x: X(j: kf_id));
299 }
300
301 TEST_COUT("Adding stereo observation from KF #" << kf_id << " for LM #"
302 << stObs.lm_id << "\n");
303
304 smartFactors[stObs.lm_id]->add(
305 measured: StereoPoint2(stObs.left_u, stObs.right_u, stObs.v), poseKey: X(j: kf_id), K);
306 }
307
308 // prior, for the first keyframe:
309 if (kf_id == 0) {
310 newFactors.addPrior(key: X(j: kf_id), prior: Pose3::Identity(), model: priorPoseNoise);
311 }
312
313 // 2) Run iSAM2:
314 // ------------------------------
315 Values newValues;
316 newValues.insert(j: X(j: kf_id), val: lastKeyframePose);
317
318 TEST_COUT("Running iSAM2 for frame: " << kf_id << "\n");
319
320 ISAM2UpdateParams updateParams;
321 updateParams.newAffectedKeys = std::move(factorNewAffectedKeys);
322
323 ISAM2Result res = isam.update(newFactors, newTheta: newValues, updateParams);
324
325 for (const auto &f2l : newFactor2lm)
326 lm2factor[f2l.second] = res.newFactorsIndices.at(n: f2l.first);
327
328 TEST_COUT("error before1: " << res.errorBefore.value() << "\n");
329 TEST_COUT("error after1 : " << res.errorAfter.value() << "\n");
330
331 // Additional refining steps:
332 ISAM2Result res2 = isam.update();
333 TEST_COUT("error before2: " << res2.errorBefore.value() << "\n");
334 TEST_COUT("error after2 : " << res2.errorAfter.value() << "\n");
335
336 Values currentEstimate = isam.calculateEstimate();
337#if TEST_VERBOSE_OUTPUT
338 currentEstimate.print("currentEstimate:");
339#endif
340
341 // Keep last KF pose as initial pose of the next one, to reduce the need
342 // to run more non-linear iterations:
343 lastKeyframePose = currentEstimate.at(j: X(j: kf_id)).cast<Pose3>();
344
345 } // end for each timestep
346
347 Values finalEstimate = isam.calculateEstimate();
348
349 // GT:
350 // camera +x = vehicle -y
351 // camera +y = vehicle -z
352 // camera +z = vehicle +x
353 for (const auto &gt : gt_positions) {
354 const Pose3 p = finalEstimate.at<Pose3>(j: X(j: gt.first));
355 EXPECT(assert_equal(p.x(), -gt.second.y(), tol));
356 EXPECT(assert_equal(p.y(), -gt.second.z(), tol));
357 EXPECT(assert_equal(p.z(), gt.second.x(), tol));
358 }
359}
360
361/* *************************************************************************
362 */
363int main() {
364 TestResult tr;
365 return TestRegistry::runAllTests(result&: tr);
366}
367