testTranslationRecovery.cpp
Go to the documentation of this file.
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 
19 #include <CppUnitLite/TestHarness.h>
20 #include <gtsam/sfm/SfmData.h>
21 #include <gtsam/sfm/TranslationRecovery.h>
22 #include <gtsam/slam/dataset.h>
23 
24 using namespace std;
25 using 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.
29 Unit3 GetDirectionFromPoses(const Values& poses,
30  const BinaryMeasurement<Unit3>& unitTranslation) {
31  const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
32  wTb = poses.at<Pose3>(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.
44 TEST(TranslationRecovery, BAL) {
45  const string filename = findExampleDataFile("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++, camera.pose());
53  }
54 
55  // Simulate measurements
56  const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
57  poses, {{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>(0).translation();
81  Point3 Tb = poses.at<Pose3>(1).translation();
82  Point3 Tc = poses.at<Pose3>(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 
90 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
99  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
100 
101  auto relativeTranslations =
102  TranslationRecovery::SimulateMeasurements(poses, {{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 
122 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
136  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
137  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
138 
139  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
140  poses, {{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 
160 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
171  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
172  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
173 
174  auto relativeTranslations =
175  TranslationRecovery::SimulateMeasurements(poses, {{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 
193 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
208  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
209  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
210  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
211 
212  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
213  poses, {{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 
233 TEST(TranslationRecovery, ThreePosesWithZeroTranslation) {
234  Values poses;
235  poses.insert<Pose3>(0, Pose3(Rot3::RzRyRx(-M_PI / 6, 0, 0), Point3(0, 0, 0)));
236  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(0, 0, 0)));
237  poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(M_PI / 6, 0, 0), Point3(0, 0, 0)));
238 
239  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
240  poses, {{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 
259 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
273  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
274  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
275 
276  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
277  poses, {{0, 1}, {0, 3}, {1, 3}});
278 
279  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
280  betweenTranslations.emplace_back(0, 3, Point3(1, -1, 0),
281  noiseModel::Isotropic::Sigma(3, 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 
293 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
307  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
308  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
309 
310  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
311  poses, {{0, 1}, {0, 3}, {1, 3}});
312 
313  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
314  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
315  noiseModel::Constrained::All(3, 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 
327 TEST(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>(0, Pose3(Rot3(), Point3(0, 0, 0)));
332  poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
333  poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
334  poses.insert<Pose3>(4, Pose3(Rot3(), Point3(1, 2, 1)));
335 
336  auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
337  poses, {{0, 1}, {0, 3}, {1, 3}});
338 
339  std::vector<BinaryMeasurement<Point3>> betweenTranslations;
340  betweenTranslations.emplace_back(0, 1, Point3(2, 0, 0),
341  noiseModel::Constrained::All(3, 1e2));
342  // Node 4 only has this between translation prior, no relative translations.
343  betweenTranslations.emplace_back(0, 4, 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 int main() {
358  TestResult tr;
359  return TestRegistry::runAllTests(tr);
360 }
361 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::TranslationRecovery
Definition: TranslationRecovery.h:51
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::TranslationRecovery::buildGraph
NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 >> &relativeTranslations) const
Build the factor graph to do the optimization.
Definition: TranslationRecovery.cpp:99
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition: SfmData.h:39
GetDirectionFromPoses
Unit3 GetDirectionFromPoses(const Values &poses, const BinaryMeasurement< Unit3 > &unitTranslation)
Definition: testTranslationRecovery.cpp:29
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
dataset.h
utility functions for loading datasets
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
Definition: gnuplot_common_settings.hh:54
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
relicense.filename
filename
Definition: relicense.py:57
gtsam::Pose3
Definition: Pose3.h:37
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
TEST
TEST(TranslationRecovery, BAL)
Definition: testTranslationRecovery.cpp:44
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
gtsam::BinaryMeasurement::key1
Key key1() const
Definition: BinaryMeasurement.h:58
TestResult
Definition: TestResult.h:26
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
SfmData.h
Data structure for dealing with Structure from Motion data.
TranslationRecovery.h
Recovering translations in an epipolar graph when rotations are given.
gtsam::BinaryMeasurement
Definition: BinaryMeasurement.h:36
gtsam::BinaryMeasurement::key2
Key key2() const
Definition: BinaryMeasurement.h:59
gtsam::TranslationRecovery::run
Values run(const TranslationEdges &relativeTranslations, const double scale=1.0, const std::vector< BinaryMeasurement< Point3 >> &betweenTranslations={}, const Values &initialValues=Values()) const
Build and optimize factor graph.
Definition: TranslationRecovery.cpp:177
gtsam
traits
Definition: chartTesting.h:28
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition: SfmData.h:40
gtsam::Values
Definition: Values.h:65
gtsam::internal::translation
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:84
std
Definition: BFloat16.h:88
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
test_TranslationRecovery.SimulateMeasurements
def SimulateMeasurements(gt_poses, graph_edges)
Definition: test_TranslationRecovery.py:20
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
camera
static const CalibratedCamera camera(kDefaultPose)
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
main
int main()
Definition: testTranslationRecovery.cpp:357

gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:10:38