testDataset.cpp
Go to the documentation of this file.
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 
19 #include <gtsam/slam/dataset.h>
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/inference/Symbol.h>
24 
25 #include <boost/algorithm/string.hpp>
26 
28 
29 #include <iostream>
30 #include <sstream>
31 
32 using namespace gtsam::symbol_shorthand;
33 using namespace std;
34 using namespace gtsam;
35 
36 /* ************************************************************************* */
38  const string expected_end = "examples/Data/example.graph";
39  const string actual = findExampleDataFile("example");
40  string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size());
41  boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash
42  EXPECT(assert_equal(expected_end, actual_end));
43 }
44 
45 /* ************************************************************************* */
47 {
48  const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
49  istringstream is(str);
50  string tag;
51  EXPECT(is >> tag);
52  const auto actual = parseVertexPose(is, tag);
53  EXPECT(actual);
54  if (actual) {
55  EXPECT_LONGS_EQUAL(1, actual->first);
56  EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
57  }
58 }
59 
60 /* ************************************************************************* */
62 {
63  const string str = "VERTEX_XY 1 2.000000 3.000000";
64  istringstream is(str);
65  string tag;
66  EXPECT(is >> tag);
67  const auto actual = parseVertexLandmark(is, tag);
68  EXPECT(actual);
69  if (actual) {
70  EXPECT_LONGS_EQUAL(1, actual->first);
71  EXPECT(assert_equal(Point2(2, 3), actual->second));
72  }
73 }
74 
75 /* ************************************************************************* */
76 TEST( dataSet, parseEdge)
77 {
78  const string str = "EDGE2 0 1 2.000000 3.000000 4.000000";
79  istringstream is(str);
80  string tag;
81  EXPECT(is >> tag);
82  const auto actual = parseEdge(is, tag);
83  EXPECT(actual);
84  if (actual) {
85  pair<size_t, size_t> expected(0, 1);
86  EXPECT(expected == actual->first);
87  EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
88  }
89 }
90 
91 /* ************************************************************************* */
92 TEST(dataSet, load2D) {
94  const string filename = findExampleDataFile("w100.graph");
97  boost::tie(graph, initial) = load2D(filename);
98  EXPECT_LONGS_EQUAL(300, graph->size());
99  EXPECT_LONGS_EQUAL(100, initial->size());
101  BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
102  model);
104  boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
105  EXPECT(assert_equal(expected, *actual));
106 
107  // Check binary measurements, Pose2
108  size_t maxIndex = 5;
109  auto measurements = parseMeasurements<Pose2>(filename, nullptr, maxIndex);
110  EXPECT_LONGS_EQUAL(5, measurements.size());
111 
112  // Check binary measurements, Rot2
113  auto measurements2 = parseMeasurements<Rot2>(filename);
114  EXPECT_LONGS_EQUAL(300, measurements2.size());
115 
116  // // Check factor parsing
117  const auto actualFactors = parseFactors<Pose2>(filename);
118  for (size_t i : {0, 1, 2, 3, 4, 5}) {
120  *boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
121  *actualFactors[i], 1e-5));
122  }
123 
124  // Check pose parsing
125  const auto actualPoses = parseVariables<Pose2>(filename);
126  for (size_t j : {0, 1, 2, 3, 4}) {
127  EXPECT(assert_equal(initial->at<Pose2>(j), actualPoses.at(j), 1e-5));
128  }
129 
130  // Check landmark parsing
131  const auto actualLandmarks = parseVariables<Point2>(filename);
132  EXPECT_LONGS_EQUAL(0, actualLandmarks.size());
133 }
134 
135 /* ************************************************************************* */
136 TEST(dataSet, load2DVictoriaPark) {
137  const string filename = findExampleDataFile("victoria_park.txt");
140 
141  // Load all
142  boost::tie(graph, initial) = load2D(filename);
143  EXPECT_LONGS_EQUAL(10608, graph->size());
144  EXPECT_LONGS_EQUAL(7120, initial->size());
145 
146  // Restrict keys
147  size_t maxIndex = 5;
148  boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
149  EXPECT_LONGS_EQUAL(5, graph->size());
150  EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
151  EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
152 }
153 
154 /* ************************************************************************* */
155 TEST( dataSet, Balbianello)
156 {
158  const string filename = findExampleDataFile("Balbianello");
159  SfmData mydata;
160  CHECK(readBundler(filename, mydata));
161 
162  // Check number of things
164  EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
165  const SfmTrack& track0 = mydata.tracks[0];
166  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
167 
168  // Check projection of a given point
169  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
170  const SfmCamera& camera0 = mydata.cameras[0];
171  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
172  EXPECT(assert_equal(expected,actual,1));
173 }
174 
175 /* ************************************************************************* */
176 TEST(dataSet, readG2o3D) {
177  const string g2oFile = findExampleDataFile("pose3example");
178  auto model = noiseModel::Isotropic::Precision(6, 10000);
179 
180  // Initialize 6 relative measurements with quaternion/point3 values:
181  const std::vector<Pose3> relative_poses = {
182  {{0.854230, 0.190253, 0.283162, -0.392318},
183  {1.001367, 0.015390, 0.004948}},
184  {{0.105373, 0.311512, 0.656877, -0.678505},
185  {0.523923, 0.776654, 0.326659}},
186  {{0.568551, 0.595795, -0.561677, 0.079353},
187  {0.910927, 0.055169, -0.411761}},
188  {{0.542221, -0.592077, 0.303380, -0.513226},
189  {0.775288, 0.228798, -0.596923}},
190  {{0.327419, -0.125250, -0.534379, 0.769122},
191  {-0.577841, 0.628016, -0.543592}},
192  {{0.083672, 0.104639, 0.627755, 0.766795},
193  {-0.623267, 0.086928, 0.773222}},
194  };
195 
196  // Initialize 5 poses with quaternion/point3 values:
197  const std::vector<Pose3> poses = {
198  {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
199  {{0.854230, 0.190253, 0.283162, -0.392318},
200  {1.001367, 0.015390, 0.004948}},
201  {{0.421446, -0.351729, -0.597838, 0.584174},
202  {1.993500, 0.023275, 0.003793}},
203  {{0.067024, 0.331798, -0.200659, 0.919323},
204  {2.004291, 1.024305, 0.018047}},
205  {{0.765488, -0.035697, -0.462490, 0.445933},
206  {0.999908, 1.055073, 0.020212}},
207  };
208 
209  // Specify connectivity:
210  using KeyPair = pair<Key, Key>;
211  std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
212 
213  // Created expected factor graph
214  size_t i = 0;
216  for (const auto& keys : edges) {
217  expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
218  keys.first, keys.second, relative_poses[i++], model);
219  }
220 
221  // Check factor parsing
222  const auto actualFactors = parseFactors<Pose3>(g2oFile);
223  for (size_t i : {0, 1, 2, 3, 4, 5}) {
225  *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
226  *actualFactors[i], 1e-5));
227  }
228 
229  // Check pose parsing
230  const auto actualPoses = parseVariables<Pose3>(g2oFile);
231  for (size_t j : {0, 1, 2, 3, 4}) {
232  EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
233  }
234 
235  // Check landmark parsing
236  const auto actualLandmarks = parseVariables<Point3>(g2oFile);
237  for (size_t j : {0, 1, 2, 3, 4}) {
238  EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
239  }
240 
241  // Check graph version
243  Values::shared_ptr actualValues;
244  bool is3D = true;
245  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
246  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
247  for (size_t j : {0, 1, 2, 3, 4}) {
248  EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
249  }
250 }
251 
252 /* ************************************************************************* */
253 TEST( dataSet, readG2o3DNonDiagonalNoise)
254 {
255  const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt");
257  Values::shared_ptr actualValues;
258  bool is3D = true;
259  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
260 
261  Values expectedValues;
262  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
263  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
264  expectedValues.insert(0, Pose3(R0, p0));
265 
266  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
267  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
268  expectedValues.insert(1, Pose3(R1, p1));
269 
270  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
271 
272  Matrix Info = Matrix(6,6);
273  for (int i = 0; i < 6; i++){
274  for (int j = i; j < 6; j++){
275  if(i==j)
276  Info(i, j) = 10000;
277  else{
278  Info(i, j) = i+1; // arbitrary nonzero number
279  Info(j, i) = i+1;
280  }
281  }
282  }
285  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
286  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
287  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
288 
289  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
290 }
291 
292 /* ************************************************************************* */
293 TEST(dataSet, readG2oCheckDeterminants) {
294  const string g2oFile = findExampleDataFile("toyExample.g2o");
295 
296  // Check determinants in factors
299  for (const auto& factor : factors) {
300  const Rot3 R = factor->measured().rotation();
301  EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
302  }
303 
304  // Check determinants in initial values
305  const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
306  EXPECT_LONGS_EQUAL(5, poses.size());
307  for (const auto& key_value : poses) {
308  const Rot3 R = key_value.second.rotation();
309  EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
310  }
311  const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
312  EXPECT_LONGS_EQUAL(0, landmarks.size());
313 }
314 
315 /* ************************************************************************* */
316 TEST(dataSet, readG2oLandmarks) {
317  const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
318 
319  // Check number of poses and landmarks. Should be 8 each.
320  const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
321  EXPECT_LONGS_EQUAL(8, poses.size());
322  const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
323  EXPECT_LONGS_EQUAL(8, landmarks.size());
324 
325  auto graphAndValues = load3D(g2oFile);
326  EXPECT(graphAndValues.second->exists(L(0)));
327 }
328 
329 /* ************************************************************************* */
333  g.emplace_shared<Factor>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
334  g.emplace_shared<Factor>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
335  g.emplace_shared<Factor>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
336  g.emplace_shared<Factor>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
337  g.emplace_shared<Factor>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
338  g.emplace_shared<Factor>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
339  g.emplace_shared<Factor>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
340  g.emplace_shared<Factor>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
341  g.emplace_shared<Factor>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
342  g.emplace_shared<Factor>(9, 10, Pose2(1.003350, 0.022250, -0.195918), model);
343  g.emplace_shared<Factor>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
344  g.emplace_shared<Factor>(3, 10, Pose2(0.044020, 0.988477, -1.553511), model);
345  return g;
346 }
347 
348 /* ************************************************************************* */
349 TEST(dataSet, readG2o) {
350  const string g2oFile = findExampleDataFile("pose2example");
352  Values::shared_ptr actualValues;
353  boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
354 
356  Vector3(44.721360, 44.721360, 30.901699));
357  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
358 
359  Values expectedValues;
360  expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
361  expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
362  expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
363  expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
364  expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
365  expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
366  expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
367  expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
368  expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
369  expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
370  expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
371  EXPECT(assert_equal(expectedValues, *actualValues, 1e-5));
372 }
373 
374 /* ************************************************************************* */
375 TEST(dataSet, readG2oHuber) {
376  const string g2oFile = findExampleDataFile("pose2example");
378  Values::shared_ptr actualValues;
379  bool is3D = false;
380  boost::tie(actualGraph, actualValues) =
381  readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
382 
383  auto baseModel = noiseModel::Diagonal::Precisions(
384  Vector3(44.721360, 44.721360, 30.901699));
386  noiseModel::mEstimator::Huber::Create(1.345), baseModel);
387 
388  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
389 }
390 
391 /* ************************************************************************* */
392 TEST(dataSet, readG2oTukey) {
393  const string g2oFile = findExampleDataFile("pose2example");
395  Values::shared_ptr actualValues;
396  bool is3D = false;
397  boost::tie(actualGraph, actualValues) =
398  readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
399 
400  auto baseModel = noiseModel::Diagonal::Precisions(
401  Vector3(44.721360, 44.721360, 30.901699));
403  noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
404 
405  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
406 }
407 
408 /* ************************************************************************* */
409 TEST( dataSet, writeG2o)
410 {
411  const string g2oFile = findExampleDataFile("pose2example");
413  Values::shared_ptr expectedValues;
414  boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
415 
416  const string filenameToWrite = createRewrittenFileName(g2oFile);
417  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
418 
420  Values::shared_ptr actualValues;
421  boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
422  EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
423  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
424 }
425 
426 /* ************************************************************************* */
427 TEST( dataSet, writeG2o3D)
428 {
429  const string g2oFile = findExampleDataFile("pose3example");
431  Values::shared_ptr expectedValues;
432  bool is3D = true;
433  boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
434 
435  const string filenameToWrite = createRewrittenFileName(g2oFile);
436  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
437 
439  Values::shared_ptr actualValues;
440  boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
441  EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
442  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
443 }
444 
445 /* ************************************************************************* */
446 TEST( dataSet, writeG2o3DNonDiagonalNoise)
447 {
448  const string g2oFile = findExampleDataFile("pose3example-offdiagonal");
450  Values::shared_ptr expectedValues;
451  bool is3D = true;
452  boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
453 
454  const string filenameToWrite = createRewrittenFileName(g2oFile);
455  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
456 
458  Values::shared_ptr actualValues;
459  boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
460  EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
461  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
462 }
463 
464 /* ************************************************************************* */
465 TEST( dataSet, readBAL_Dubrovnik)
466 {
468  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
469  SfmData mydata;
470  CHECK(readBAL(filename, mydata));
471 
472  // Check number of things
474  EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
475  const SfmTrack& track0 = mydata.tracks[0];
476  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
477 
478  // Check projection of a given point
479  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
480  const SfmCamera& camera0 = mydata.cameras[0];
481  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
482  EXPECT(assert_equal(expected,actual,12));
483 }
484 
485 /* ************************************************************************* */
486 TEST( dataSet, openGL2gtsam)
487 {
488  Vector3 rotVec(0.2, 0.7, 1.1);
489  Rot3 R = Rot3::Expmap(rotVec);
490  Point3 t = Point3(0.0,0.0,0.0);
491  Pose3 poseGTSAM = Pose3(R,t);
492 
493  Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
494 
495  Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
496  Rot3 cRw(
497  r1.x(), r2.x(), r3.x(),
498  -r1.y(), -r2.y(), -r3.y(),
499  -r1.z(), -r2.z(), -r3.z());
500  Rot3 wRc = cRw.inverse();
501  Pose3 actual = Pose3(wRc,t);
502 
503  EXPECT(assert_equal(expected,actual));
504 }
505 
506 /* ************************************************************************* */
507 TEST( dataSet, gtsam2openGL)
508 {
509  Vector3 rotVec(0.2, 0.7, 1.1);
510  Rot3 R = Rot3::Expmap(rotVec);
511  Point3 t = Point3(1.0,20.0,10.0);
512  Pose3 actual = Pose3(R,t);
513  Pose3 poseGTSAM = openGL2gtsam(R, t.x(), t.y(), t.z());
514 
515  Pose3 expected = gtsam2openGL(poseGTSAM);
516  EXPECT(assert_equal(expected,actual));
517 }
518 
519 /* ************************************************************************* */
520 TEST( dataSet, writeBAL_Dubrovnik)
521 {
523  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
524  SfmData readData;
525  readBAL(filenameToRead, readData);
526 
527  // Write readData to file filenameToWrite
528  const string filenameToWrite = createRewrittenFileName(filenameToRead);
529  CHECK(writeBAL(filenameToWrite, readData));
530 
531  // Read what we wrote
532  SfmData writtenData;
533  CHECK(readBAL(filenameToWrite, writtenData));
534 
535  // Check that what we read is the same as what we wrote
536  EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras());
537  EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks());
538 
539  for (size_t i = 0; i < readData.number_cameras(); i++){
540  PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
541  PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
542  EXPECT(assert_equal(expectedCamera,actualCamera));
543  }
544 
545  for (size_t j = 0; j < readData.number_tracks(); j++){
546  // check point
547  SfmTrack expectedTrack = writtenData.tracks[j];
548  SfmTrack actualTrack = readData.tracks[j];
549  Point3 expectedPoint = expectedTrack.p;
550  Point3 actualPoint = actualTrack.p;
551  EXPECT(assert_equal(expectedPoint,actualPoint));
552 
553  // check rgb
554  Point3 expectedRGB = Point3( expectedTrack.r, expectedTrack.g, expectedTrack.b );
555  Point3 actualRGB = Point3( actualTrack.r, actualTrack.g, actualTrack.b);
556  EXPECT(assert_equal(expectedRGB,actualRGB));
557 
558  // check measurements
559  for (size_t k = 0; k < actualTrack.number_measurements(); k++){
560  EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first);
561  EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
562  }
563  }
564 }
565 
566 
567 /* ************************************************************************* */
568 TEST( dataSet, writeBALfromValues_Dubrovnik){
569 
571  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
572  SfmData readData;
573  readBAL(filenameToRead, readData);
574 
575  Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
576 
577  Values value;
578  for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
579  Pose3 pose = poseChange.compose(readData.cameras[i].pose());
580  value.insert(X(i), pose);
581  }
582  for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
583  Point3 point = poseChange.transformFrom( readData.tracks[j].p );
584  value.insert(P(j), point);
585  }
586 
587  // Write values and readData to a file
588  const string filenameToWrite = createRewrittenFileName(filenameToRead);
589  writeBALfromValues(filenameToWrite, readData, value);
590 
591  // Read the file we wrote
592  SfmData writtenData;
593  readBAL(filenameToWrite, writtenData);
594 
595  // Check that the reprojection errors are the same and the poses are correct
596  // Check number of things
597  EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
598  EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
599  const SfmTrack& track0 = writtenData.tracks[0];
600  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
601 
602  // Check projection of a given point
603  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
604  const SfmCamera& camera0 = writtenData.cameras[0];
605  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
606  EXPECT(assert_equal(expected,actual,12));
607 
608  Pose3 expectedPose = camera0.pose();
609  Pose3 actualPose = value.at<Pose3>(X(0));
610  EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
611 
612  Point3 expectedPoint = track0.p;
613  Point3 actualPoint = value.at<Point3>(P(0));
614  EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
615 }
616 
617 
618 /* ************************************************************************* */
619 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
620 /* ************************************************************************* */
vector< MFAS::KeyPair > edges
Definition: testMFAS.cpp:25
Provides additional testing facilities for common data structures.
size_t size() const
Definition: FactorGraph.h:306
#define CHECK(condition)
Definition: Test.h:109
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:630
Point3 r1() const
first column
Definition: Rot3M.cpp:224
std::vector< BetweenFactor< Pose2 >::shared_ptr > parseFactors< Pose2 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:432
const Pose3 & pose() const
return pose
bool writeBAL(const string &filename, SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure...
Definition: dataset.cpp:1140
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:116
std::vector< BetweenFactor< Pose3 >::shared_ptr > parseFactors< Pose3 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:916
Define the structure for SfM data.
Definition: dataset.h:326
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3f p1
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Matrix expected
Definition: testMatrix.cpp:974
Key P(std::uint64_t j)
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:500
Vector2 Point2
Definition: Point2.h:27
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition: dataset.cpp:972
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
Definition: dataset.cpp:294
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
#define M_PI
Definition: main.h:78
Values initial
Definition: Half.h:150
Vector3f p0
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
Matrix3 matrix() const
Definition: Rot3M.cpp:219
Key X(std::uint64_t j)
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
Definition: NoiseModel.h:323
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:665
NonlinearFactorGraph graph
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:162
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 > Hr=boost::none)
Definition: Rot3.h:199
void g(const string &key, int i)
Definition: testBTree.cpp:43
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:608
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:311
std::vector< SfmTrack > tracks
Sparse set of points.
Definition: dataset.h:328
Point3 point(10, 0,-5)
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel &model)
string findExampleDataFile(const string &name)
Definition: dataset.cpp:65
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:207
boost::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:60
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:298
size_t number_tracks() const
The number of reconstructed 3D points.
Definition: dataset.h:333
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
boost::shared_ptr< This > shared_ptr
Class compose(const Class &g) const
Definition: Lie.h:47
Define the structure for the 3D points.
Definition: dataset.h:220
const ValueType at(Key j) const
Definition: Values-inl.h:342
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition: Values.h:96
Point3 r2() const
second column
Definition: Rot3M.cpp:227
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Definition: pytypes.h:928
Point3 r3() const
third column
Definition: Rot3M.cpp:230
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
#define EXPECT(condition)
Definition: Test.h:151
int main()
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double r2
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Definition: Rot3.h:377
size_t number_measurements() const
Total number of measurements in this track.
Definition: dataset.h:233
string createRewrittenFileName(const string &name)
Definition: dataset.cpp:100
static const double r3
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
std::vector< SfmCamera > cameras
Set of cameras.
Definition: dataset.h:327
static const double r1
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:155
Key R(std::uint64_t j)
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition: dataset.cpp:963
std::map< size_t, Point3 > parseVariables< Point3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:798
Key L(std::uint64_t j)
Point3 p
3D position of the point
Definition: dataset.h:224
float b
RGB color of the 3D point.
Definition: dataset.h:225
TEST(LPInitSolver, InfiniteLoopSingleVar)
std::map< size_t, Pose2 > parseVariables< Pose2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:182
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:780
boost::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:189
static Rot3 R0
Vector3 Point3
Definition: Point3.h:35
const KeyVector keys
2D Pose
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:615
bool readBundler(const string &filename, SfmData &data)
This function parses a bundler output file and stores the data into a SfmData structure.
Definition: dataset.cpp:986
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:924
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
Definition: NoiseModel.h:559
std::map< size_t, Point2 > parseVariables< Point2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:204
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
Definition: dataset.cpp:188
utility functions for loading datasets
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition: dataset.h:226
std::ptrdiff_t j
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Point2 t(10, 10)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
Definition: dataset.cpp:168
bool readBAL(const string &filename, SfmData &data)
This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfmData...
Definition: dataset.cpp:1073
bool writeBALfromValues(const string &filename, const SfmData &data, Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition: dataset.cpp:1216
size_t number_cameras() const
Definition: dataset.h:329


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:25