dataset.cpp
Go to the documentation of this file.
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 
7  * See LICENSE for the license information
8 
9  * -------------------------------------------------------------------------- */
10 
23 #include <gtsam/slam/dataset.h>
24 
25 #include <gtsam/geometry/Point3.h>
26 #include <gtsam/geometry/Pose2.h>
27 #include <gtsam/geometry/Rot3.h>
28 
31 
32 #include <gtsam/linear/Sampler.h>
33 
35 #include <gtsam/inference/Symbol.h>
36 
38 #include <gtsam/base/Lie.h>
39 #include <gtsam/base/Matrix.h>
40 #include <gtsam/base/Value.h>
41 #include <gtsam/base/Vector.h>
42 #include <gtsam/base/types.h>
43 
44 #include <optional>
45 
46 #include <cmath>
47 #include <fstream>
48 #include <iostream>
49 #include <stdexcept>
50 #include <string>
51 
52 #if defined(__GNUC__) && (__GNUC__ == 7)
53 #include <experimental/filesystem>
54 namespace fs = std::experimental::filesystem;
55 #else
56 #include <filesystem>
57 namespace fs = std::filesystem;
58 #endif
59 
61 
62 using std::cout;
63 using std::endl;
64 
65 #define LINESIZE 81920
66 
67 namespace gtsam {
68 
69 /* ************************************************************************* */
70 std::string findExampleDataFile(const std::string &name) {
71  // Search source tree and installed location
72  std::vector<std::string> rootsToSearch;
73 
74  // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt
75  rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR);
76  rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR);
77 
78  // Search for filename as given, and with .graph and .txt extensions
79  std::vector<std::string> namesToSearch;
80  namesToSearch.push_back(name);
81  namesToSearch.push_back(name + ".graph");
82  namesToSearch.push_back(name + ".txt");
83  namesToSearch.push_back(name + ".out");
84  namesToSearch.push_back(name + ".xml");
85  namesToSearch.push_back(name + ".g2o");
86 
87  // Find first name that exists
88  for (const fs::path root : rootsToSearch) {
89  for (const fs::path name : namesToSearch) {
90  if (fs::is_regular_file(root / name))
91  return (root / name).string();
92  }
93  }
94 
95  // If we did not return already, then we did not find the file
96  throw std::invalid_argument(
97  "gtsam::findExampleDataFile could not find a matching "
98  "file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
99  " or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" +
100  name + ", " + name + ".g2o, " + ", " + name + ".graph, or " + name +
101  ".txt");
102 }
103 
104 /* ************************************************************************* */
105 std::string createRewrittenFileName(const std::string &name) {
106  // Search source tree and installed location
107  if (!exists(fs::path(name))) {
108  throw std::invalid_argument(
109  "gtsam::createRewrittenFileName could not find a matching file in\n" +
110  name);
111  }
112 
113  fs::path p(name);
114  fs::path newpath = fs::path(p.parent_path().string()) /
115  fs::path(p.stem().string() + "-rewritten.txt");
116 
117  return newpath.string();
118 }
119 
120 /* ************************************************************************* */
121 // Type for parser functions used in parseLines below.
122 template <typename T>
123 using Parser =
124  std::function<std::optional<T>(std::istream &is, const std::string &tag)>;
125 
126 // Parse a file by calling the parse(is, tag) function for every line.
127 // The result of calling the function is ignored, so typically parse function
128 // works via a side effect, like adding a factor into a graph etc.
129 template <typename T>
130 static void parseLines(const std::string &filename, Parser<T> parse) {
131  std::ifstream is(filename.c_str());
132  if (!is)
133  throw std::invalid_argument("parse: can not find file " + filename);
134  std::string tag;
135  while (is >> tag) {
136  parse(is, tag); // ignore return value
137  is.ignore(LINESIZE, '\n');
138  }
139 }
140 
141 /* ************************************************************************* */
142 // Parse types T into a size_t-indexed map
143 template <typename T>
144 std::map<size_t, T> parseToMap(const std::string &filename, Parser<std::pair<size_t, T>> parse,
145  size_t maxIndex) {
146  std::map<size_t, T> result;
147  Parser<std::pair<size_t, T>> emplace = [&](std::istream &is, const std::string &tag) {
148  if (auto t = parse(is, tag)) {
149  if (!maxIndex || t->first <= maxIndex)
150  result.emplace(*t);
151  }
152  return std::nullopt;
153  };
154  parseLines(filename, emplace);
155  return result;
156 }
157 
158 /* ************************************************************************* */
159 // Parse a file and push results on a vector
160 template <typename T>
161 static std::vector<T> parseToVector(const std::string &filename, Parser<T> parse) {
162  std::vector<T> result;
163  Parser<T> add = [&result, parse](std::istream &is, const std::string &tag) {
164  if (auto t = parse(is, tag))
165  result.push_back(*t);
166  return std::nullopt;
167  };
168  parseLines(filename, add);
169  return result;
170 }
171 
172 /* ************************************************************************* */
173 std::optional<IndexedPose> parseVertexPose(std::istream &is, const std::string &tag) {
174  if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
175  size_t id;
176  double x, y, yaw;
177  if (!(is >> id >> x >> y >> yaw)) {
178  throw std::runtime_error("parseVertexPose encountered malformed line");
179  }
180  return IndexedPose(id, Pose2(x, y, yaw));
181  } else {
182  return std::nullopt;
183  }
184 }
185 
186 template <>
187 GTSAM_EXPORT std::map<size_t, Pose2> parseVariables<Pose2>(
188  const std::string &filename, size_t maxIndex) {
189  return parseToMap<Pose2>(filename, parseVertexPose, maxIndex);
190 }
191 
192 /* ************************************************************************* */
193 std::optional<IndexedLandmark> parseVertexLandmark(std::istream &is,
194  const std::string &tag) {
195  if (tag == "VERTEX_XY") {
196  size_t id;
197  double x, y;
198  if (!(is >> id >> x >> y)) {
199  throw std::runtime_error(
200  "parseVertexLandmark encountered malformed line");
201  }
202  return IndexedLandmark(id, Point2(x, y));
203  } else {
204  return std::nullopt;
205  }
206 }
207 
208 template <>
209 GTSAM_EXPORT std::map<size_t, Point2> parseVariables<Point2>(
210  const std::string &filename, size_t maxIndex) {
211  return parseToMap<Point2>(filename, parseVertexLandmark, maxIndex);
212 }
213 
214 /* ************************************************************************* */
215 // Interpret noise parameters according to flags
217  const Vector6 &v, bool smart, NoiseFormat noiseFormat,
218  KernelFunctionType kernelFunctionType) {
219  if (noiseFormat == NoiseFormatAUTO) {
220  // Try to guess covariance matrix layout
221  if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
222  v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
223  noiseFormat = NoiseFormatGRAPH;
224  } else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
225  v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
226  noiseFormat = NoiseFormatCOV;
227  } else {
228  throw std::invalid_argument(
229  "load2D: unrecognized covariance matrix format in dataset file. "
230  "Please specify the noise format.");
231  }
232  }
233 
234  // Read matrix and check that diagonal entries are non-zero
235  Matrix3 M;
236  switch (noiseFormat) {
237  case NoiseFormatG2O:
238  case NoiseFormatCOV:
239  // i.e., [v(0) v(1) v(2);
240  // v(1)' v(3) v(4);
241  // v(2)' v(4)' v(5) ]
242  if (v(0) == 0.0 || v(3) == 0.0 || v(5) == 0.0)
243  throw std::runtime_error(
244  "load2D::readNoiseModel looks like this is not G2O matrix order");
245  M << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5);
246  break;
247  case NoiseFormatTORO:
248  case NoiseFormatGRAPH:
249  // http://www.openslam.org/toro.html
250  // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
251  // i.e., [v(0) v(1) v(4);
252  // v(1)' v(2) v(5);
253  // v(4)' v(5)' v(3) ]
254  if (v(0) == 0.0 || v(2) == 0.0 || v(3) == 0.0)
255  throw std::invalid_argument(
256  "load2D::readNoiseModel looks like this is not TORO matrix order");
257  M << v(0), v(1), v(4), v(1), v(2), v(5), v(4), v(5), v(3);
258  break;
259  default:
260  throw std::runtime_error("load2D: invalid noise format");
261  }
262 
263  // Now, create a Gaussian noise model
264  // The smart flag will try to detect a simpler model, e.g., unit
266  switch (noiseFormat) {
267  case NoiseFormatG2O:
268  case NoiseFormatTORO:
269  // In both cases, what is stored in file is the information matrix
270  model = noiseModel::Gaussian::Information(M, smart);
271  break;
272  case NoiseFormatGRAPH:
273  case NoiseFormatCOV:
274  // These cases expect covariance matrix
275  model = noiseModel::Gaussian::Covariance(M, smart);
276  break;
277  default:
278  throw std::invalid_argument("load2D: invalid noise format");
279  }
280 
281  switch (kernelFunctionType) {
283  return model;
284  break;
288  break;
292  break;
293  default:
294  throw std::invalid_argument("load2D: invalid kernel function type");
295  }
296 }
297 
298 /* ************************************************************************* */
299 std::optional<IndexedEdge> parseEdge(std::istream &is, const std::string &tag) {
300  if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
301  (tag == "ODOMETRY")) {
302 
303  size_t id1, id2;
304  double x, y, yaw;
305  if (!(is >> id1 >> id2 >> x >> y >> yaw)) {
306  throw std::runtime_error("parseEdge encountered malformed line");
307  }
308  return IndexedEdge({id1, id2}, Pose2(x, y, yaw));
309  } else {
310  return std::nullopt;
311  }
312 }
313 
314 /* ************************************************************************* */
315 // Measurement parsers are implemented as a functor, as they have several
316 // options to filter and create the measurement model.
317 template <typename T> struct ParseMeasurement;
318 
319 /* ************************************************************************* */
320 // Converting from Measurement to BetweenFactor is generic
321 template <typename T> struct ParseFactor : ParseMeasurement<T> {
323  : ParseMeasurement<T>(parent) {}
324 
325  // We parse a measurement then convert
326  typename std::optional<typename BetweenFactor<T>::shared_ptr>
327  operator()(std::istream &is, const std::string &tag) {
328  if (auto m = ParseMeasurement<T>::operator()(is, tag))
329  return std::make_shared<BetweenFactor<T>>(
330  m->key1(), m->key2(), m->measured(), m->noiseModel());
331  else
332  return std::nullopt;
333  }
334 };
335 
336 /* ************************************************************************* */
337 // Pose2 measurement parser
338 template <> struct ParseMeasurement<Pose2> {
339  // The arguments
340  std::shared_ptr<Sampler> sampler;
341  size_t maxIndex;
342 
343  // For noise model creation
344  bool smart;
347 
348  // If this is not null, will use instead of parsed model:
350 
351  // The actual parser
352  std::optional<BinaryMeasurement<Pose2>> operator()(std::istream &is,
353  const std::string &tag) {
354  auto edge = parseEdge(is, tag);
355  if (!edge)
356  return std::nullopt;
357 
358  // parse noise model
359  Vector6 v;
360  is >> v(0) >> v(1) >> v(2) >> v(3) >> v(4) >> v(5);
361 
362  // optional filter
363  size_t id1, id2;
364  std::tie(id1, id2) = edge->first;
365  if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
366  return std::nullopt;
367 
368  // Get pose and optionally add noise
369  Pose2 &pose = edge->second;
370  if (sampler)
371  pose = pose.retract(sampler->sample());
372 
373  // emplace measurement
374  auto modelFromFile =
375  createNoiseModel(v, smart, noiseFormat, kernelFunctionType);
376  return BinaryMeasurement<Pose2>(id1, id2, pose,
377  model ? model : modelFromFile);
378  }
379 };
380 
381 /* ************************************************************************* */
382 // Create a sampler to corrupt a measurement
383 std::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
384  auto noise = std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
385  if (!noise)
386  throw std::invalid_argument("gtsam::load: invalid noise model for adding noise"
387  "(current version assumes diagonal noise model)!");
388  return std::shared_ptr<Sampler>(new Sampler(noise));
389 }
390 
391 /* ************************************************************************* */
392 // Implementation of parseMeasurements for Pose2
393 template <>
394 GTSAM_EXPORT
395 std::vector<BinaryMeasurement<Pose2>>
396 parseMeasurements(const std::string &filename,
398  size_t maxIndex) {
399  ParseMeasurement<Pose2> parse{model ? createSampler(model) : nullptr,
400  maxIndex, true, NoiseFormatAUTO,
401  KernelFunctionTypeNONE, nullptr};
402  return parseToVector<BinaryMeasurement<Pose2>>(filename, parse);
403 }
404 
405 /* ************************************************************************* */
406 // Implementation of parseMeasurements for Rot2, which converts from Pose2
407 
408 // Extract Rot2 measurement from Pose2 measurement
410  auto gaussian =
411  std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
412  if (!gaussian)
413  throw std::invalid_argument(
414  "parseMeasurements<Rot2> can only convert Pose2 measurements "
415  "with Gaussian noise models.");
416  const Matrix3 M = gaussian->covariance();
417  return BinaryMeasurement<Rot2>(p.key1(), p.key2(), p.measured().rotation(),
419 }
420 
421 template <>
422 GTSAM_EXPORT
423 std::vector<BinaryMeasurement<Rot2>>
424 parseMeasurements(const std::string &filename,
426  size_t maxIndex) {
427  auto poses = parseMeasurements<Pose2>(filename, model, maxIndex);
428  std::vector<BinaryMeasurement<Rot2>> result;
429  result.reserve(poses.size());
430  for (const auto &p : poses)
431  result.push_back(convert(p));
432  return result;
433 }
434 
435 /* ************************************************************************* */
436 // Implementation of parseFactors for Pose2
437 template <>
438 GTSAM_EXPORT
439 std::vector<BetweenFactor<Pose2>::shared_ptr>
440 parseFactors<Pose2>(const std::string &filename,
442  size_t maxIndex) {
443  ParseFactor<Pose2> parse({model ? createSampler(model) : nullptr, maxIndex,
445  nullptr});
446  return parseToVector<BetweenFactor<Pose2>::shared_ptr>(filename, parse);
447 }
448 
449 /* ************************************************************************* */
451 
452 template <> struct ParseMeasurement<BearingRange2D> {
453  // arguments
454  size_t maxIndex;
455 
456  // The actual parser
457  std::optional<BinaryMeasurement<BearingRange2D>>
458  operator()(std::istream &is, const std::string &tag) {
459  size_t id1, id2;
460  is >> id1 >> id2;
461  double bearing, range, bearing_std, range_std;
462 
463  if (tag == "BR") {
464  is >> bearing >> range >> bearing_std >> range_std;
465  } else if (tag == "LANDMARK") {
466  // A landmark measurement, converted to bearing-range
467  double lmx, lmy;
468  double v1, v2, v3;
469 
470  is >> lmx >> lmy >> v1 >> v2 >> v3;
471 
472  // Convert x,y to bearing,range
473  bearing = atan2(lmy, lmx);
474  range = sqrt(lmx * lmx + lmy * lmy);
475 
476  // In our experience, the x-y covariance on landmark sightings is not
477  // very good, so assume it describes the uncertainty at a range of 10m,
478  // and convert that to bearing/range uncertainty.
479  if (std::abs(v1 - v3) < 1e-4) {
480  bearing_std = sqrt(v1 / 10.0);
481  range_std = sqrt(v1);
482  } else {
483  // TODO(frank): we are ignoring the non-uniform covariance
484  bearing_std = 1;
485  range_std = 1;
486  }
487  } else {
488  return std::nullopt;
489  }
490 
491  // optional filter
492  if (maxIndex && id1 > maxIndex)
493  return std::nullopt;
494 
495  // Create noise model
496  auto measurementNoise = noiseModel::Diagonal::Sigmas(
497  (Vector(2) << bearing_std, range_std).finished());
498 
500  id1, L(id2), BearingRange2D(bearing, range), measurementNoise);
501  }
502 };
503 
504 /* ************************************************************************* */
505 GraphAndValues load2D(const std::string &filename, SharedNoiseModel model,
506  size_t maxIndex, bool addNoise, bool smart,
507  NoiseFormat noiseFormat,
508  KernelFunctionType kernelFunctionType) {
509 
510  // Single pass for poses and landmarks.
511  auto initial = std::make_shared<Values>();
512  Parser<int> insert = [maxIndex, &initial](std::istream &is, const std::string &tag) {
513  if (auto indexedPose = parseVertexPose(is, tag)) {
514  if (!maxIndex || indexedPose->first <= maxIndex)
515  initial->insert(indexedPose->first, indexedPose->second);
516  } else if (auto indexedLandmark = parseVertexLandmark(is, tag)) {
517  if (!maxIndex || indexedLandmark->first <= maxIndex)
518  initial->insert(L(indexedLandmark->first), indexedLandmark->second);
519  }
520  return 0;
521  };
522  parseLines(filename, insert);
523 
524  // Single pass for Pose2 and bearing-range factors.
525  auto graph = std::make_shared<NonlinearFactorGraph>();
526 
527  // Instantiate factor parser
528  ParseFactor<Pose2> parseBetweenFactor(
529  {addNoise ? createSampler(model) : nullptr, maxIndex, smart, noiseFormat,
530  kernelFunctionType, model});
531 
532  // Instantiate bearing-range parser
533  ParseMeasurement<BearingRange2D> parseBearingRange{maxIndex};
534 
535  // Combine in a single parser that adds factors to `graph`, but also inserts
536  // new variables into `initial` when needed.
537  Parser<int> parse = [&](std::istream &is, const std::string &tag) {
538  if (auto f = parseBetweenFactor(is, tag)) {
539  graph->push_back(*f);
540 
541  // Insert vertices if pure odometry file
542  Key key1 = (*f)->key<1>(), key2 = (*f)->key<2>();
543  if (!initial->exists(key1))
544  initial->insert(key1, Pose2());
545  if (!initial->exists(key2))
546  initial->insert(key2, initial->at<Pose2>(key1) * (*f)->measured());
547  } else if (auto m = parseBearingRange(is, tag)) {
548  Key key1 = m->key1(), key2 = m->key2();
549  BearingRange2D br = m->measured();
550  graph->emplace_shared<BearingRangeFactor<Pose2, Point2>>(key1, key2, br,
551  m->noiseModel());
552 
553  // Insert poses or points if they do not exist yet
554  if (!initial->exists(key1))
555  initial->insert(key1, Pose2());
556  if (!initial->exists(key2)) {
557  Pose2 pose = initial->at<Pose2>(key1);
558  Point2 local = br.bearing() * Point2(br.range(), 0);
559  Point2 global = pose.transformFrom(local);
560  initial->insert(key2, global);
561  }
562  }
563  return 0;
564  };
565 
566  parseLines(filename, parse);
567 
568  return {graph, initial};
569 }
570 
571 /* ************************************************************************* */
572 GraphAndValues load2D(std::pair<std::string, SharedNoiseModel> dataset,
573  size_t maxIndex, bool addNoise, bool smart,
574  NoiseFormat noiseFormat,
575  KernelFunctionType kernelFunctionType) {
576  return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart,
577  noiseFormat, kernelFunctionType);
578 }
579 
580 /* ************************************************************************* */
581 GraphAndValues load2D_robust(const std::string &filename,
582  const noiseModel::Base::shared_ptr &model,
583  size_t maxIndex) {
584  return load2D(filename, model, maxIndex);
585 }
586 
587 /* ************************************************************************* */
588 void save2D(const NonlinearFactorGraph &graph, const Values &config,
590  const std::string &filename) {
591 
592  std::fstream stream(filename.c_str(), std::fstream::out);
593 
594  // save poses
595  for (const auto &key_pose : config.extract<Pose2>()) {
596  const Pose2 &pose = key_pose.second;
597  stream << "VERTEX2 " << key_pose.first << " " << pose.x() << " " << pose.y()
598  << " " << pose.theta() << endl;
599  }
600 
601  // save edges
602  // TODO(frank): why don't we use model in factor?
603  Matrix3 R = model->R();
604  Matrix3 RR = R.transpose() * R;
605  for (auto f : graph) {
606  auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(f);
607  if (!factor)
608  continue;
609 
610  const Pose2 pose = factor->measured().inverse();
611  stream << "EDGE2 " << factor->key<2>() << " " << factor->key<1>() << " "
612  << pose.x() << " " << pose.y() << " " << pose.theta() << " "
613  << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2)
614  << " " << RR(0, 2) << " " << RR(1, 2) << endl;
615  }
616 
617  stream.close();
618 }
619 
620 /* ************************************************************************* */
621 GraphAndValues readG2o(const std::string &g2oFile, const bool is3D,
622  KernelFunctionType kernelFunctionType) {
623  if (is3D) {
624  return load3D(g2oFile);
625  } else {
626  // just call load2D
627  size_t maxIndex = 0;
628  bool addNoise = false;
629  bool smart = true;
630  return load2D(g2oFile, SharedNoiseModel(), maxIndex, addNoise, smart,
631  NoiseFormatG2O, kernelFunctionType);
632  }
633 }
634 
635 /* ************************************************************************* */
636 void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
637  const std::string &filename) {
638  std::fstream stream(filename.c_str(), std::fstream::out);
639 
640  // Use a lambda here to more easily modify behavior in future.
641  auto index = [](gtsam::Key key) { return Symbol(key).index(); };
642 
643  // save 2D poses
644  for (const auto &pair : estimate.extract<Pose2>()) {
645  const Pose2 &pose = pair.second;
646  stream << "VERTEX_SE2 " << index(pair.first) << " " << pose.x() << " "
647  << pose.y() << " " << pose.theta() << endl;
648  }
649 
650  // save 3D poses
651  for (const auto &pair : estimate.extract<Pose3>()) {
652  const Pose3 &pose = pair.second;
653  const Point3 t = pose.translation();
654  const auto q = pose.rotation().toQuaternion();
655  stream << "VERTEX_SE3:QUAT " << index(pair.first) << " " << t.x() << " "
656  << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
657  << q.z() << " " << q.w() << endl;
658  }
659 
660  // save 2D landmarks
661  for (const auto &pair : estimate.extract<Point2>()) {
662  const Point2 &point = pair.second;
663  stream << "VERTEX_XY " << index(pair.first) << " " << point.x() << " "
664  << point.y() << endl;
665  }
666 
667  // save 3D landmarks
668  for (const auto &pair : estimate.extract<Point3>()) {
669  const Point3 &point = pair.second;
670  stream << "VERTEX_TRACKXYZ " << index(pair.first) << " " << point.x()
671  << " " << point.y() << " " << point.z() << endl;
672  }
673 
674  // save edges (2D or 3D)
675  for (const auto &factor_ : graph) {
676  auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
677  if (factor) {
678  SharedNoiseModel model = factor->noiseModel();
679  auto gaussianModel =
680  std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
681  if (!gaussianModel) {
682  model->print("model\n");
683  throw std::invalid_argument("writeG2o: invalid noise model!");
684  }
685  Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R();
686  Pose2 pose = factor->measured(); //.inverse();
687  stream << "EDGE_SE2 " << index(factor->key<1>()) << " "
688  << index(factor->key<2>()) << " " << pose.x() << " " << pose.y()
689  << " " << pose.theta();
690  for (size_t i = 0; i < 3; i++) {
691  for (size_t j = i; j < 3; j++) {
692  stream << " " << Info(i, j);
693  }
694  }
695  stream << endl;
696  }
697 
698  auto factor3D = std::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
699 
700  if (factor3D) {
701  SharedNoiseModel model = factor3D->noiseModel();
702 
703  std::shared_ptr<noiseModel::Gaussian> gaussianModel =
704  std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
705  if (!gaussianModel) {
706  model->print("model\n");
707  throw std::invalid_argument("writeG2o: invalid noise model!");
708  }
709  Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R();
710  const Pose3 pose3D = factor3D->measured();
711  const Point3 p = pose3D.translation();
712  const auto q = pose3D.rotation().toQuaternion();
713  stream << "EDGE_SE3:QUAT " << index(factor3D->key<1>()) << " "
714  << index(factor3D->key<2>()) << " " << p.x() << " " << p.y() << " "
715  << p.z() << " " << q.x() << " " << q.y() << " " << q.z() << " "
716  << q.w();
717 
718  // g2o's EDGE_SE3:QUAT stores information/precision of Pose3 in t,R order, unlike GTSAM:
719  Matrix6 InfoG2o = I_6x6;
720  InfoG2o.block<3, 3>(0, 0) = Info.block<3, 3>(3, 3); // cov translation
721  InfoG2o.block<3, 3>(3, 3) = Info.block<3, 3>(0, 0); // cov rotation
722  InfoG2o.block<3, 3>(0, 3) = Info.block<3, 3>(3, 0); // off diagonal R,t -> t,R
723  InfoG2o.block<3, 3>(3, 0) = Info.block<3, 3>(0, 3); // off diagonal t,R -> R,t
724 
725  for (size_t i = 0; i < 6; i++) {
726  for (size_t j = i; j < 6; j++) {
727  stream << " " << InfoG2o(i, j);
728  }
729  }
730  stream << endl;
731  }
732  }
733  stream.close();
734 }
735 
736 /* ************************************************************************* */
737 // parse quaternion in x,y,z,w order, and normalize to unit length
738 std::istream &operator>>(std::istream &is, Quaternion &q) {
739  double x, y, z, w;
740  is >> x >> y >> z >> w;
741  const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
742  q = Quaternion(f * w, f * x, f * y, f * z);
743  return is;
744 }
745 
746 /* ************************************************************************* */
747 // parse Rot3 from roll, pitch, yaw
748 std::istream &operator>>(std::istream &is, Rot3 &R) {
749  double yaw, pitch, roll;
750  is >> roll >> pitch >> yaw; // notice order !
751  R = Rot3::Ypr(yaw, pitch, roll);
752  return is;
753 }
754 
755 /* ************************************************************************* */
756 std::optional<std::pair<size_t, Pose3>> parseVertexPose3(std::istream &is,
757  const std::string &tag) {
758  if (tag == "VERTEX3") {
759  size_t id;
760  double x, y, z;
761  Rot3 R;
762  is >> id >> x >> y >> z >> R;
763  return std::make_pair(id, Pose3(R, {x, y, z}));
764  } else if (tag == "VERTEX_SE3:QUAT") {
765  size_t id;
766  double x, y, z;
767  Quaternion q;
768  is >> id >> x >> y >> z >> q;
769  return std::make_pair(id, Pose3(q, {x, y, z}));
770  } else
771  return std::nullopt;
772 }
773 
774 template <>
775 GTSAM_EXPORT std::map<size_t, Pose3> parseVariables<Pose3>(
776  const std::string &filename, size_t maxIndex) {
777  return parseToMap<Pose3>(filename, parseVertexPose3, maxIndex);
778 }
779 
780 /* ************************************************************************* */
781 std::optional<std::pair<size_t, Point3>> parseVertexPoint3(std::istream &is,
782  const std::string &tag) {
783  if (tag == "VERTEX_TRACKXYZ") {
784  size_t id;
785  double x, y, z;
786  is >> id >> x >> y >> z;
787  return std::make_pair(id, Point3(x, y, z));
788  } else
789  return std::nullopt;
790 }
791 
792 template <>
793 GTSAM_EXPORT std::map<size_t, Point3> parseVariables<Point3>(
794  const std::string &filename, size_t maxIndex) {
795  return parseToMap<Point3>(filename, parseVertexPoint3, maxIndex);
796 }
797 
798 /* ************************************************************************* */
799 // Parse a symmetric covariance matrix (onlyupper-triangular part is stored)
800 std::istream &operator>>(std::istream &is, Matrix6 &m) {
801  for (size_t i = 0; i < 6; i++)
802  for (size_t j = i; j < 6; j++) {
803  is >> m(i, j);
804  m(j, i) = m(i, j);
805  }
806  return is;
807 }
808 
809 /* ************************************************************************* */
810 // Pose3 measurement parser
811 template <> struct ParseMeasurement<Pose3> {
812  // The arguments
813  std::shared_ptr<Sampler> sampler;
814  size_t maxIndex;
815 
816  // The actual parser
817  std::optional<BinaryMeasurement<Pose3>> operator()(std::istream &is,
818  const std::string &tag) {
819  if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT")
820  return std::nullopt;
821 
822  // parse ids and optionally filter
823  size_t id1, id2;
824  is >> id1 >> id2;
825  if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
826  return std::nullopt;
827 
828  Matrix6 m;
829  if (tag == "EDGE3") {
830  double x, y, z;
831  Rot3 R;
832  is >> x >> y >> z >> R >> m;
833  Pose3 T12(R, {x, y, z});
834  // optionally add noise
835  if (sampler)
836  T12 = T12.retract(sampler->sample());
837 
838  return BinaryMeasurement<Pose3>(id1, id2, T12,
840  } else if (tag == "EDGE_SE3:QUAT") {
841  double x, y, z;
842  Quaternion q;
843  is >> x >> y >> z >> q >> m;
844 
845  Pose3 T12(q, {x, y, z});
846  // optionally add noise
847  if (sampler)
848  T12 = T12.retract(sampler->sample());
849 
850  // g2o's EDGE_SE3:QUAT stores information/precision of Pose3 in t,R order, unlike GTSAM:
851  Matrix6 mgtsam;
852  mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // info rotation
853  mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // info translation
854  mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(0, 3); // off diagonal g2o t,R -> GTSAM R,t
855  mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(3, 0); // off diagonal g2o R,t -> GTSAM t,R
857 
859  id1, id2, T12, noiseModel::Gaussian::Information(mgtsam));
860  } else
861  return std::nullopt;
862  }
863 };
864 
865 /* ************************************************************************* */
866 // Implementation of parseMeasurements for Pose3
867 template <>
868 GTSAM_EXPORT
869 std::vector<BinaryMeasurement<Pose3>>
870 parseMeasurements(const std::string &filename,
872  size_t maxIndex) {
873  ParseMeasurement<Pose3> parse{model ? createSampler(model) : nullptr,
874  maxIndex};
875  return parseToVector<BinaryMeasurement<Pose3>>(filename, parse);
876 }
877 
878 /* ************************************************************************* */
879 // Implementation of parseMeasurements for Rot3, which converts from Pose3
880 
881 // Extract Rot3 measurement from Pose3 measurement
883  auto gaussian =
884  std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
885  if (!gaussian)
886  throw std::invalid_argument(
887  "parseMeasurements<Rot3> can only convert Pose3 measurements "
888  "with Gaussian noise models.");
889  const Matrix6 M = gaussian->covariance();
891  p.key1(), p.key2(), p.measured().rotation(),
892  noiseModel::Gaussian::Covariance(M.block<3, 3>(0, 0), true));
893 }
894 
895 template <>
896 GTSAM_EXPORT
897 std::vector<BinaryMeasurement<Rot3>>
898 parseMeasurements(const std::string &filename,
900  size_t maxIndex) {
901  auto poses = parseMeasurements<Pose3>(filename, model, maxIndex);
902  std::vector<BinaryMeasurement<Rot3>> result;
903  result.reserve(poses.size());
904  for (const auto &p : poses)
905  result.push_back(convert(p));
906  return result;
907 }
908 
909 /* ************************************************************************* */
910 // Implementation of parseFactors for Pose3
911 template <>
912 GTSAM_EXPORT
913 std::vector<BetweenFactor<Pose3>::shared_ptr>
914 parseFactors<Pose3>(const std::string &filename,
916  size_t maxIndex) {
917  ParseFactor<Pose3> parse({model ? createSampler(model) : nullptr, maxIndex});
918  return parseToVector<BetweenFactor<Pose3>::shared_ptr>(filename, parse);
919 }
920 
921 /* ************************************************************************* */
922 GraphAndValues load3D(const std::string &filename) {
923  auto graph = std::make_shared<NonlinearFactorGraph>();
924  auto initial = std::make_shared<Values>();
925 
926  // Instantiate factor parser. maxIndex is always zero for load3D.
927  ParseFactor<Pose3> parseFactor({nullptr, 0});
928 
929  // Single pass for variables and factors. Unlike 2D version, does *not* insert
930  // variables into `initial` if referenced but not present.
931  Parser<int> parse = [&](std::istream &is, const std::string &tag) {
932  if (auto indexedPose = parseVertexPose3(is, tag)) {
933  initial->insert(indexedPose->first, indexedPose->second);
934  } else if (auto indexedLandmark = parseVertexPoint3(is, tag)) {
935  initial->insert(L(indexedLandmark->first), indexedLandmark->second);
936  } else if (auto factor = parseFactor(is, tag)) {
937  graph->push_back(*factor);
938  }
939  return 0;
940  };
941  parseLines(filename, parse);
942 
943  return {graph, initial};
944 }
945 
946 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
948 parse2DFactors(const std::string &filename,
949  const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
950  return parseFactors<Pose2>(filename, model, maxIndex);
951 }
952 
954 parse3DFactors(const std::string &filename,
955  const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
956  return parseFactors<Pose3>(filename, model, maxIndex);
957 }
958 
959 } // namespace gtsam
const gtsam::Symbol key('X', 0)
const SharedNoiseModel & noiseModel() const
Matrix3f m
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Definition: dataset.h:143
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hp={}, OptionalJacobian< 3, 1 > Hr={})
Definition: Rot3.h:196
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
A insert(1, 2)=0
Typedefs for easier changing of types.
default: toro-style order, but covariance matrix !
Definition: dataset.h:68
std::function< std::optional< T >(std::istream &is, const std::string &tag)> Parser
Definition: dataset.cpp:124
std::optional< BinaryMeasurement< BearingRange2D > > operator()(std::istream &is, const std::string &tag)
Definition: dataset.cpp:458
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:226
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114
GraphAndValues load2D_robust(const std::string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:581
Vector v2
Vector v1
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
Definition: dataset.h:218
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition: dataset.h:74
noiseModel::Diagonal::shared_ptr model
std::ofstream out("Result.txt")
std::optional< typename BetweenFactor< T >::shared_ptr > operator()(std::istream &is, const std::string &tag)
Definition: dataset.cpp:327
#define LINESIZE
Definition: dataset.cpp:65
static SharedNoiseModel createNoiseModel(const Vector6 &v, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:216
Vector2 Point2
Definition: Point2.h:32
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
std::optional< std::pair< size_t, Pose3 > > parseVertexPose3(std::istream &is, const std::string &tag)
Definition: dataset.cpp:756
BetweenFactorPose3s parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:954
Rot2 R(Rot2::fromAngle(0.1))
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition: dataset.h:111
Values initial
BetweenFactorPose2s parse2DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:948
MatrixXd L
Definition: LLT_example.cpp:6
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:775
Try to guess covariance matrix layout.
Definition: dataset.h:70
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:704
NonlinearFactorGraph graph
std::shared_ptr< Sampler > createSampler(const SharedNoiseModel &model)
Definition: dataset.cpp:383
ParseFactor(const ParseMeasurement< T > &parent)
Definition: dataset.cpp:322
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
double y() const
get y
Definition: Pose2.h:249
static const Similarity3 id
std::optional< BinaryMeasurement< Pose3 > > operator()(std::istream &is, const std::string &tag)
Definition: dataset.cpp:817
NoiseFormat
Indicates how noise parameters are stored in file.
Definition: dataset.h:65
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:600
GTSAM_EXPORT std::map< size_t, Point2 > parseVariables< Point2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:209
Vector v3
std::pair< std::pair< size_t, size_t >, Pose2 > IndexedEdge
Definition: dataset.h:113
static std::map< timestep_t, std::vector< stereo_meas_t > > dataset
const SharedNoiseModel & noiseModel() const
access to the noise model
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition: dataset.h:67
Eigen::VectorXd Vector
Definition: Vector.h:38
std::optional< IndexedEdge > parseEdge(std::istream &is, const std::string &tag)
Definition: dataset.cpp:299
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Definition: Matrix.cpp:173
static shared_ptr Information(const Matrix &M, bool smart=true)
Definition: NoiseModel.cpp:97
const Symbol key1('v', 1)
Values result
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
KernelFunctionType kernelFunctionType
Definition: dataset.cpp:346
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
std::map< size_t, T > parseToMap(const std::string &filename, Parser< std::pair< size_t, T >> parse, size_t maxIndex)
Definition: dataset.cpp:144
GraphAndValues load3D(const std::string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:922
double theta() const
get theta
Definition: Pose2.h:252
Array< int, Dynamic, 1 > v
std::pair< size_t, Point2 > IndexedLandmark
Definition: dataset.h:112
static void parseLines(const std::string &filename, Parser< T > parse)
Definition: dataset.cpp:130
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EIGEN_DEVICE_FUNC const Scalar & q
sampling from a NoiseModel
void print(const std::string &name) const override
Definition: NoiseModel.cpp:134
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
Definition: dataset.cpp:588
GraphAndValues readG2o(const std::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:621
RowVector3d w
Base class and basic functions for Lie types.
std::shared_ptr< Sampler > sampler
Definition: dataset.cpp:813
std::uint64_t index() const
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
std::optional< BinaryMeasurement< Pose2 > > operator()(std::istream &is, const std::string &tag)
Definition: dataset.cpp:352
GTSAM_EXPORT std::map< size_t, Pose2 > parseVariables< Pose2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:187
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
BearingRange< Pose2, Point2 > BearingRange2D
Definition: dataset.cpp:450
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition: dataset.h:69
Information matrix I11, I12, I13, I22, I23, I33.
Definition: dataset.h:66
Non-linear factor base classes.
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
The quaternion class used to represent 3D orientations and rotations.
Double_ range(const Point2_ &p, const Point2_ &q)
Key L(std::uint64_t j)
std::optional< IndexedLandmark > parseVertexLandmark(std::istream &is, const std::string &tag)
Definition: dataset.cpp:193
const T & measured() const
Factor Graph Base Class.
3D Point
float * p
std::optional< std::pair< size_t, Point3 > > parseVertexPoint3(std::istream &is, const std::string &tag)
Definition: dataset.cpp:781
static std::vector< T > parseToVector(const std::string &filename, Parser< T > parse)
Definition: dataset.cpp:161
The base class for any variable that can be optimized or used in a factor.
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr > parseFactors< Pose2 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:440
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Annotation for function names.
Definition: attr.h:48
Vector3 Point3
Definition: Point3.h:38
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
Definition: dataset.h:212
std::shared_ptr< Sampler > sampler
Definition: dataset.cpp:340
2D Pose
GTSAM_EXPORT std::string createRewrittenFileName(const std::string &name)
Definition: dataset.cpp:105
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
std::optional< IndexedPose > parseVertexPose(std::istream &is, const std::string &tag)
Definition: dataset.cpp:173
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 x
#define abs(x)
Definition: datatypes.h:17
double x() const
get x
Definition: Pose2.h:246
GTSAM_EXPORT std::map< size_t, Point3 > parseVariables< Point3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:793
def parse(input_path, output_path, quiet=False, generate_xml_flag=True)
Definition: parse_xml.py:10
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
static BinaryMeasurement< Rot3 > convert(const BetweenFactor< Pose3 >::shared_ptr &f)
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Point2 t(10, 10)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr > parseFactors< Pose3 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:914
3D rotation represented as a rotation matrix or quaternion
const Symbol key2('v', 2)
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > parseMeasurements(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:396
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:09