1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010-2020, 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 testTranslationRecovery.cpp
14 * @author Frank Dellaert, Akshay Krishnan
15 * @date March 2020
16 * @brief test recovering translations when rotations are given.
17 */
18
19#include <CppUnitLite/TestHarness.h>
20#include <gtsam/sfm/SfmData.h>
21#include <gtsam/sfm/TranslationRecovery.h>
22#include <gtsam/slam/dataset.h>
23
24using namespace std;
25using namespace gtsam;
26
27// Returns the Unit3 direction as measured in the binary measurement, but
28// computed from the input poses. Helper function used in the unit tests.
29Unit3 GetDirectionFromPoses(const Values& poses,
30 const BinaryMeasurement<Unit3>& unitTranslation) {
31 const Pose3 wTa = poses.at<Pose3>(j: unitTranslation.key1()),
32 wTb = poses.at<Pose3>(j: unitTranslation.key2());
33 const Point3 Ta = wTa.translation(), Tb = wTb.translation();
34 return Unit3(Tb - Ta);
35}
36
37/* ************************************************************************* */
38// We read the BAL file, which has 3 cameras in it, with poses. We then assume
39// the rotations are correct, but translations have to be estimated from
40// translation directions only. Since we have 3 cameras, A, B, and C, we can at
41// most create three relative measurements, let's call them w_aZb, w_aZc, and
42// bZc. These will be of type Unit3. We then call `recoverTranslations` which
43// sets up an optimization problem for the three unknown translations.
44TEST(TranslationRecovery, BAL) {
45 const string filename = findExampleDataFile(name: "dubrovnik-3-7-pre");
46 SfmData db = SfmData::FromBalFile(filename);
47
48 // Get camera poses, as Values
49 size_t j = 0;
50 Values poses;
51 for (auto camera : db.cameras) {
52 poses.insert(j: j++, val: camera.pose());
53 }
54
55 // Simulate measurements
56 const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
57 poses, edges: {{0, 1}, {0, 2}, {1, 2}});
58
59 // Check simulated measurements.
60 for (auto& unitTranslation : relativeTranslations) {
61 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
62 unitTranslation.measured()));
63 }
64
65 TranslationRecovery algorithm;
66 const auto graph = algorithm.buildGraph(relativeTranslations);
67 EXPECT_LONGS_EQUAL(3, graph.size());
68
69 // Run translation recovery
70 const double scale = 2.0;
71 const auto result = algorithm.run(relativeTranslations, scale);
72
73 // Check result for first two translations, determined by prior
74 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
75 EXPECT(assert_equal(
76 Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])),
77 result.at<Point3>(1)));
78
79 // Check that the third translations is correct
80 Point3 Ta = poses.at<Pose3>(j: 0).translation();
81 Point3 Tb = poses.at<Pose3>(j: 1).translation();
82 Point3 Tc = poses.at<Pose3>(j: 2).translation();
83 Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
84 EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
85
86 // TODO(frank): how to get stats back?
87 // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
88}
89
90TEST(TranslationRecovery, TwoPoseTest) {
91 // Create a dataset with 2 poses.
92 // __ __
93 // \/ \/
94 // 0 _____ 1
95 //
96 // 0 and 1 face in the same direction but have a translation offset.
97 Values poses;
98 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
99 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
100
101 auto relativeTranslations =
102 TranslationRecovery::SimulateMeasurements(poses, edges: {{0, 1}});
103
104 // Check simulated measurements.
105 for (auto& unitTranslation : relativeTranslations) {
106 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
107 unitTranslation.measured()));
108 }
109
110 TranslationRecovery algorithm;
111 const auto graph = algorithm.buildGraph(relativeTranslations);
112 EXPECT_LONGS_EQUAL(1, graph.size());
113
114 // Run translation recovery
115 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
116
117 // Check result for first two translations, determined by prior
118 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
119 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
120}
121
122TEST(TranslationRecovery, ThreePoseTest) {
123 // Create a dataset with 3 poses.
124 // __ __
125 // \/ \/
126 // 0 _____ 1
127 // \ __ /
128 // \\//
129 // 3
130 //
131 // 0 and 1 face in the same direction but have a translation offset. 3 is in
132 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
133
134 Values poses;
135 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
136 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
137 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
138
139 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
140 poses, edges: {{0, 1}, {1, 3}, {3, 0}});
141
142 // Check simulated measurements.
143 for (auto& unitTranslation : relativeTranslations) {
144 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
145 unitTranslation.measured()));
146 }
147
148 TranslationRecovery algorithm;
149 const auto graph = algorithm.buildGraph(relativeTranslations);
150 EXPECT_LONGS_EQUAL(3, graph.size());
151
152 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
153
154 // Check result
155 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
156 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
157 EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3), 1e-8));
158}
159
160TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
161 // Create a dataset with 3 poses.
162 // __ __
163 // \/ \/
164 // 0 _____ 1
165 // 2 <|
166 //
167 // 0 and 1 face in the same direction but have a translation offset. 2 is at
168 // the same point as 1 but is rotated, with little FOV overlap.
169 Values poses;
170 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
171 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
172 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(x: -M_PI / 2, y: 0, z: 0), Point3(2, 0, 0)));
173
174 auto relativeTranslations =
175 TranslationRecovery::SimulateMeasurements(poses, edges: {{0, 1}, {1, 2}});
176
177 // Check simulated measurements.
178 for (auto& unitTranslation : relativeTranslations) {
179 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
180 unitTranslation.measured()));
181 }
182
183 TranslationRecovery algorithm;
184 // Run translation recovery
185 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
186
187 // Check result
188 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
189 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
190 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2), 1e-8));
191}
192
193TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
194 // Create a dataset with 4 poses.
195 // __ __
196 // \/ \/
197 // 0 _____ 1
198 // \ __ 2 <|
199 // \\//
200 // 3
201 //
202 // 0 and 1 face in the same direction but have a translation offset. 2 is at
203 // the same point as 1 but is rotated, with very little FOV overlap. 3 is in
204 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
205
206 Values poses;
207 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
208 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
209 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(x: -M_PI / 2, y: 0, z: 0), Point3(2, 0, 0)));
210 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
211
212 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
213 poses, edges: {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
214
215 // Check simulated measurements.
216 for (auto& unitTranslation : relativeTranslations) {
217 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
218 unitTranslation.measured()));
219 }
220
221 TranslationRecovery algorithm;
222
223 // Run translation recovery
224 const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
225
226 // Check result
227 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
228 EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1), 1e-8));
229 EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2), 1e-8));
230 EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3), 1e-8));
231}
232
233TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
234 Values poses;
235 poses.insert<Pose3>(j: 0, val: Pose3(Rot3::RzRyRx(x: -M_PI / 6, y: 0, z: 0), Point3(0, 0, 0)));
236 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(0, 0, 0)));
237 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(M_PI / 6, y: 0, z: 0), Point3(0, 0, 0)));
238
239 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
240 poses, edges: {{0, 1}, {1, 2}, {2, 0}});
241
242 // Check simulated measurements.
243 for (auto& unitTranslation : relativeTranslations) {
244 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
245 unitTranslation.measured()));
246 }
247
248 TranslationRecovery algorithm;
249
250 // Run translation recovery
251 const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
252
253 // Check result
254 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
255 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1), 1e-8));
256 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
257}
258
259TEST(TranslationRecovery, ThreePosesWithOneSoftConstraint) {
260 // Create a dataset with 3 poses.
261 // __ __
262 // \/ \/
263 // 0 _____ 1
264 // \ __ /
265 // \\//
266 // 3
267 //
268 // 0 and 1 face in the same direction but have a translation offset. 3 is in
269 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
270
271 Values poses;
272 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
273 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
274 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
275
276 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
277 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
278
279 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
280 betweenTranslations.emplace_back(args: 0, args: 3, args: Point3(1, -1, 0),
281 args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 1e-2));
282
283 TranslationRecovery algorithm;
284 auto result =
285 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
286
287 // Check result
288 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
289 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
290 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
291}
292
293TEST(TranslationRecovery, ThreePosesWithOneHardConstraint) {
294 // Create a dataset with 3 poses.
295 // __ __
296 // \/ \/
297 // 0 _____ 1
298 // \ __ /
299 // \\//
300 // 3
301 //
302 // 0 and 1 face in the same direction but have a translation offset. 3 is in
303 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
304
305 Values poses;
306 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
307 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
308 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
309
310 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
311 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
312
313 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
314 betweenTranslations.emplace_back(args: 0, args: 1, args: Point3(2, 0, 0),
315 args: noiseModel::Constrained::All(dim: 3, mu: 1e2));
316
317 TranslationRecovery algorithm;
318 auto result =
319 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
320
321 // Check result
322 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
323 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
324 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
325}
326
327TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurements) {
328 // Checks that valid results are obtained when a between translation edge is
329 // provided with a node that does not have any other relative translations.
330 Values poses;
331 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
332 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
333 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
334 poses.insert<Pose3>(j: 4, val: Pose3(Rot3(), Point3(1, 2, 1)));
335
336 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
337 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
338
339 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
340 betweenTranslations.emplace_back(args: 0, args: 1, args: Point3(2, 0, 0),
341 args: noiseModel::Constrained::All(dim: 3, mu: 1e2));
342 // Node 4 only has this between translation prior, no relative translations.
343 betweenTranslations.emplace_back(args: 0, args: 4, args: Point3(1, 2, 1));
344
345 TranslationRecovery algorithm;
346 auto result =
347 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
348
349 // Check result
350 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
351 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
352 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
353 EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
354}
355
356/* *************************************************************************
357* Repeat all tests, but with the Bilinear Angle Translation Factor.
358* ************************************************************************* */
359
360
361/* ************************************************************************* */
362// We read the BAL file, which has 3 cameras in it, with poses. We then assume
363// the rotations are correct, but translations have to be estimated from
364// translation directions only. Since we have 3 cameras, A, B, and C, we can at
365// most create three relative measurements, let's call them w_aZb, w_aZc, and
366// bZc. These will be of type Unit3. We then call `recoverTranslations` which
367// sets up an optimization problem for the three unknown translations.
368TEST(TranslationRecovery, BALBATA) {
369 const string filename = findExampleDataFile(name: "dubrovnik-3-7-pre");
370 SfmData db = SfmData::FromBalFile(filename);
371
372 // Get camera poses, as Values
373 size_t j = 0;
374 Values poses;
375 for (auto camera : db.cameras) {
376 poses.insert(j: j++, val: camera.pose());
377 }
378
379 // Simulate measurements
380 const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
381 poses, edges: {{0, 1}, {0, 2}, {1, 2}});
382
383 // Check simulated measurements.
384 for (auto& unitTranslation : relativeTranslations) {
385 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
386 unitTranslation.measured()));
387 }
388
389 LevenbergMarquardtParams params;
390 TranslationRecovery algorithm(params, true);
391 const auto graph = algorithm.buildGraph(relativeTranslations);
392 EXPECT_LONGS_EQUAL(3, graph.size());
393
394 // Run translation recovery
395 const double scale = 2.0;
396 const auto result = algorithm.run(relativeTranslations, scale);
397
398 // Check result for first two translations, determined by prior
399 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
400 EXPECT(assert_equal(
401 Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])),
402 result.at<Point3>(1)));
403
404 // Check that the third translations is correct
405 Point3 Ta = poses.at<Pose3>(j: 0).translation();
406 Point3 Tb = poses.at<Pose3>(j: 1).translation();
407 Point3 Tc = poses.at<Pose3>(j: 2).translation();
408 Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
409 EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
410
411 // TODO(frank): how to get stats back?
412 // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
413}
414
415TEST(TranslationRecovery, TwoPoseTestBATA) {
416 // Create a dataset with 2 poses.
417 // __ __
418 // \/ \/
419 // 0 _____ 1
420 //
421 // 0 and 1 face in the same direction but have a translation offset.
422 Values poses;
423 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
424 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
425
426 auto relativeTranslations =
427 TranslationRecovery::SimulateMeasurements(poses, edges: {{0, 1}});
428
429 // Check simulated measurements.
430 for (auto& unitTranslation : relativeTranslations) {
431 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
432 unitTranslation.measured()));
433 }
434
435 LevenbergMarquardtParams params;
436 TranslationRecovery algorithm(params, true);
437 const auto graph = algorithm.buildGraph(relativeTranslations);
438 EXPECT_LONGS_EQUAL(1, graph.size());
439
440 // Run translation recovery
441 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
442
443 // Check result for first two translations, determined by prior
444 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
445 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
446}
447
448TEST(TranslationRecovery, ThreePoseTestBATA) {
449 // Create a dataset with 3 poses.
450 // __ __
451 // \/ \/
452 // 0 _____ 1
453 // \ __ /
454 // \\//
455 // 3
456 //
457 // 0 and 1 face in the same direction but have a translation offset. 3 is in
458 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
459
460 Values poses;
461 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
462 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
463 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
464
465 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
466 poses, edges: {{0, 1}, {1, 3}, {3, 0}});
467
468 // Check simulated measurements.
469 for (auto& unitTranslation : relativeTranslations) {
470 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
471 unitTranslation.measured()));
472 }
473
474 LevenbergMarquardtParams params;
475 TranslationRecovery algorithm(params, true);
476 const auto graph = algorithm.buildGraph(relativeTranslations);
477 EXPECT_LONGS_EQUAL(3, graph.size());
478
479 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
480
481 // Check result
482 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
483 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
484 EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3), 1e-8));
485}
486
487TEST(TranslationRecovery, ThreePosesIncludingZeroTranslationBATA) {
488 // Create a dataset with 3 poses.
489 // __ __
490 // \/ \/
491 // 0 _____ 1
492 // 2 <|
493 //
494 // 0 and 1 face in the same direction but have a translation offset. 2 is at
495 // the same point as 1 but is rotated, with little FOV overlap.
496 Values poses;
497 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
498 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
499 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(x: -M_PI / 2, y: 0, z: 0), Point3(2, 0, 0)));
500
501 auto relativeTranslations =
502 TranslationRecovery::SimulateMeasurements(poses, edges: {{0, 1}, {1, 2}});
503
504 // Check simulated measurements.
505 for (auto& unitTranslation : relativeTranslations) {
506 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
507 unitTranslation.measured()));
508 }
509
510 LevenbergMarquardtParams params;
511 TranslationRecovery algorithm(params, true);
512 // Run translation recovery
513 const auto result = algorithm.run(relativeTranslations, /*scale=*/3.0);
514
515 // Check result
516 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
517 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1), 1e-8));
518 EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2), 1e-8));
519}
520
521TEST(TranslationRecovery, FourPosesIncludingZeroTranslationBATA) {
522 // Create a dataset with 4 poses.
523 // __ __
524 // \/ \/
525 // 0 _____ 1
526 // \ __ 2 <|
527 // \\//
528 // 3
529 //
530 // 0 and 1 face in the same direction but have a translation offset. 2 is at
531 // the same point as 1 but is rotated, with very little FOV overlap. 3 is in
532 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
533
534 Values poses;
535 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
536 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
537 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(x: -M_PI / 2, y: 0, z: 0), Point3(2, 0, 0)));
538 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
539
540 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
541 poses, edges: {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
542
543 // Check simulated measurements.
544 for (auto& unitTranslation : relativeTranslations) {
545 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
546 unitTranslation.measured()));
547 }
548
549 LevenbergMarquardtParams params;
550 TranslationRecovery algorithm(params, true);
551
552 // Run translation recovery
553 const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
554
555 // Check result
556 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
557 EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1), 1e-8));
558 EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2), 1e-8));
559 EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3), 1e-8));
560}
561
562TEST(TranslationRecovery, ThreePosesWithZeroTranslationBATA) {
563 Values poses;
564 poses.insert<Pose3>(j: 0, val: Pose3(Rot3::RzRyRx(x: -M_PI / 6, y: 0, z: 0), Point3(0, 0, 0)));
565 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(0, 0, 0)));
566 poses.insert<Pose3>(j: 2, val: Pose3(Rot3::RzRyRx(M_PI / 6, y: 0, z: 0), Point3(0, 0, 0)));
567
568 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
569 poses, edges: {{0, 1}, {1, 2}, {2, 0}});
570
571 // Check simulated measurements.
572 for (auto& unitTranslation : relativeTranslations) {
573 EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
574 unitTranslation.measured()));
575 }
576
577 LevenbergMarquardtParams params;
578 TranslationRecovery algorithm(params, true);
579 // Run translation recovery
580 const auto result = algorithm.run(relativeTranslations, /*scale=*/4.0);
581
582 // Check result
583 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-8));
584 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(1), 1e-8));
585 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(2), 1e-8));
586}
587
588TEST(TranslationRecovery, ThreePosesWithOneSoftConstraintBATA) {
589 // Create a dataset with 3 poses.
590 // __ __
591 // \/ \/
592 // 0 _____ 1
593 // \ __ /
594 // \\//
595 // 3
596 //
597 // 0 and 1 face in the same direction but have a translation offset. 3 is in
598 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
599
600 Values poses;
601 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
602 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
603 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
604
605 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
606 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
607
608 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
609 betweenTranslations.emplace_back(args: 0, args: 3, args: Point3(1, -1, 0),
610 args: noiseModel::Isotropic::Sigma(dim: 3, sigma: 1e-2));
611
612 LevenbergMarquardtParams params;
613 TranslationRecovery algorithm(params, true);
614 auto result =
615 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
616
617 // Check result
618 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
619 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
620 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
621}
622
623TEST(TranslationRecovery, ThreePosesWithOneHardConstraintBATA) {
624 // Create a dataset with 3 poses.
625 // __ __
626 // \/ \/
627 // 0 _____ 1
628 // \ __ /
629 // \\//
630 // 3
631 //
632 // 0 and 1 face in the same direction but have a translation offset. 3 is in
633 // the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
634
635 Values poses;
636 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
637 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
638 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
639
640 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
641 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
642
643 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
644 betweenTranslations.emplace_back(args: 0, args: 1, args: Point3(2, 0, 0),
645 args: noiseModel::Constrained::All(dim: 3, mu: 1e2));
646
647 LevenbergMarquardtParams params;
648 TranslationRecovery algorithm(params, true);
649 auto result =
650 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
651
652 // Check result
653 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
654 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
655 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
656}
657
658TEST(TranslationRecovery, NodeWithBetweenFactorAndNoMeasurementsBATA) {
659 // Checks that valid results are obtained when a between translation edge is
660 // provided with a node that does not have any other relative translations.
661 Values poses;
662 poses.insert<Pose3>(j: 0, val: Pose3(Rot3(), Point3(0, 0, 0)));
663 poses.insert<Pose3>(j: 1, val: Pose3(Rot3(), Point3(2, 0, 0)));
664 poses.insert<Pose3>(j: 3, val: Pose3(Rot3(), Point3(1, -1, 0)));
665 poses.insert<Pose3>(j: 4, val: Pose3(Rot3(), Point3(1, 2, 1)));
666
667 auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
668 poses, edges: {{0, 1}, {0, 3}, {1, 3}});
669
670 std::vector<BinaryMeasurement<Point3>> betweenTranslations;
671 betweenTranslations.emplace_back(args: 0, args: 1, args: Point3(2, 0, 0),
672 args: noiseModel::Constrained::All(dim: 3, mu: 1e2));
673 // Node 4 only has this between translation prior, no relative translations.
674 betweenTranslations.emplace_back(args: 0, args: 4, args: Point3(1, 2, 1));
675
676 LevenbergMarquardtParams params;
677 TranslationRecovery algorithm(params, true);
678 auto result =
679 algorithm.run(relativeTranslations, /*scale=*/0.0, betweenTranslations);
680
681 // Check result
682 EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0), 1e-4));
683 EXPECT(assert_equal(Point3(2, 0, 0), result.at<Point3>(1), 1e-4));
684 EXPECT(assert_equal(Point3(1, -1, 0), result.at<Point3>(3), 1e-4));
685 EXPECT(assert_equal(Point3(1, 2, 1), result.at<Point3>(4), 1e-4));
686}
687
688
689
690/* ************************************************************************* */
691int main() {
692 TestResult tr;
693 return TestRegistry::runAllTests(result&: tr);
694}
695/* ************************************************************************* */
696