Merged in feature/datasets (pull request #395)

parse3DFactors

Approved-by: Chris Beall <chrisbeall@gmail.com>

cython/gtsam/tests/test_dataset.py

@@ -0,0 +1,44 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for testing dataset access.
+Author: Frank Dellaert
+"""
+# pylint: disable=invalid-name, no-name-in-module, no-member
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import BetweenFactorPose3, BetweenFactorPose3s
+
+
+class TestDataset(unittest.TestCase):
+    """Tests for datasets.h wrapper."""
+
+    def setUp(self):
+        """Get some common paths."""
+        self.pose3_example_g2o_file = gtsam.findExampleDataFile(
+            "pose3example.txt")
+
+    def test_readG2o3D(self):
+        """Test reading directly into factor graph."""
+        is3D = True
+        graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
+        self.assertEqual(graph.size(), 6)
+        self.assertEqual(initial.size(), 5)
+
+    def test_parse3Dfactors(self):
+        """Test parsing into data structure."""
+        factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
+        self.assertEqual(factors.size(), 6)
+        self.assertIsInstance(factors.at(0), BetweenFactorPose3)
+
+
+if __name__ == '__main__':
+    unittest.main()

gtsam.h

@@ -2479,6 +2479,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+class BetweenFactorPose3s
+{
+  size_t size() const;
+  gtsam::BetweenFactorPose3* at(size_t i) const;
+};
+
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
 void writeG2o(const gtsam::NonlinearFactorGraph& graph,

gtsam/slam/dataset.cpp

@@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
 
 /* ************************************************************************* */
 GraphAndValues readG2o(const string& g2oFile, const bool is3D,
-    KernelFunctionType kernelFunctionType) {
+                       KernelFunctionType kernelFunctionType) {
-  // just call load2D
+  if (is3D) {
-  int maxID = 0;
-  bool addNoise = false;
-  bool smart = true;
-
-  if(is3D)
     return load3D(g2oFile);
-
+  } else {
-  return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+    // just call load2D
-      NoiseFormatG2O, kernelFunctionType);
+    int maxID = 0;
+    bool addNoise = false;
+    bool smart = true;
+    return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+                  NoiseFormatG2O, kernelFunctionType);
+  }
 }
 
 /* ************************************************************************* */
@@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
 }
 
 /* ************************************************************************* */
-GraphAndValues load3D(const string& filename) {
+std::map<Key, Pose3> parse3DPoses(const string& filename) {
-
   ifstream is(filename.c_str());
   if (!is)
-    throw invalid_argument("load3D: can not find file " + filename);
+    throw invalid_argument("parse3DPoses: can not find file " + filename);
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
 
+  std::map<Key, Pose3> poses;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) {
       Key id;
       double x, y, z, roll, pitch, yaw;
       ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
+      poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
     }
     if (tag == "VERTEX_SE3:QUAT") {
       Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
+      poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
-      Point3 t = Point3(x, y, z);
-      initial->insert(id, Pose3(R,t));
     }
   }
-  is.clear(); /* clears the end-of-file and error flags */
+  return poses;
-  is.seekg(0, ios::beg);
+}
 
+/* ************************************************************************* */
+BetweenFactorPose3s parse3DFactors(const string& filename) {
+  ifstream is(filename.c_str());
+  if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
+
+  std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
   while (!is.eof()) {
     char buf[LINESIZE];
     is.getline(buf, LINESIZE);
@@ -557,44 +556,55 @@ GraphAndValues load3D(const string& filename) {
       Key id1, id2;
       double x, y, z, roll, pitch, yaw;
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
-      Rot3 R = Rot3::Ypr(yaw,pitch,roll);
+      Matrix m(6, 6);
-      Point3 t = Point3(x, y, z);
+      for (size_t i = 0; i < 6; i++)
-      Matrix m = I_6x6;
+        for (size_t j = i; j < 6; j++) ls >> m(i, j);
-      for (int i = 0; i < 6; i++)
-        for (int j = i; j < 6; j++)
-          ls >> m(i, j);
       SharedNoiseModel model = noiseModel::Gaussian::Information(m);
-      NonlinearFactor::shared_ptr factor(
+      factors.emplace_back(new BetweenFactor<Pose3>(
-          new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
+          id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
-      graph->push_back(factor);
     }
     if (tag == "EDGE_SE3:QUAT") {
-      Matrix m = I_6x6;
       Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
-      Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
+      Matrix m(6, 6);
-      Point3 t = Point3(x, y, z);
+      for (size_t i = 0; i < 6; i++) {
-      for (int i = 0; i < 6; i++){
+        for (size_t j = i; j < 6; j++) {
-        for (int j = i; j < 6; j++){
           double mij;
           ls >> mij;
           m(i, j) = mij;
           m(j, i) = mij;
         }
       }
-      Matrix mgtsam = I_6x6;
+      Matrix mgtsam(6, 6);
 
-      mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation
+      mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
-      mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation
+      mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
-      mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
+      mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
-      mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
+      mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal
 
       SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
-      NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
+      factors.emplace_back(new BetweenFactor<Pose3>(
-      graph->push_back(factor);
+          id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
     }
   }
+  return factors;
+}
+
+/* ************************************************************************* */
+GraphAndValues load3D(const string& filename) {
+  const auto factors = parse3DFactors(filename);
+  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
+  for (const auto& factor : factors) {
+    graph->push_back(factor);
+  }
+
+  const auto poses = parse3DPoses(filename);
+  Values::shared_ptr initial(new Values);
+  for (const auto& key_pose : poses) {
+    initial->insert(key_pose.first, key_pose.second);
+  }
+
   return make_pair(graph, initial);
 }
 

gtsam/slam/dataset.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
@@ -34,6 +35,7 @@
 #include <utility> // for pair
 #include <vector>
 #include <iosfwd>
+#include <map>
 
 namespace gtsam {
 
@@ -153,9 +155,14 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
 GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
     const Values& estimate, const std::string& filename);
 
-/**
+/// Parse edges in 3D TORO graph file into a set of BetweenFactors.
- * Load TORO 3D Graph
+using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
- */
+GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
+
+/// Parse vertices in 3D TORO graph file into a map of Pose3s.
+GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
+
+/// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
 /// A measurement with its camera index

gtsam/slam/tests/testDataset.cpp

@@ -16,8 +16,8 @@
  */
 
 
-#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>
@@ -162,65 +162,74 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2o3D)
+TEST(dataSet, readG2o3D) {
-{
   const string g2oFile = findExampleDataFile("pose3example");
+  auto model = noiseModel::Isotropic::Precision(6, 10000);
+
+  // Initialize 6 relative measurements with quaternion/point3 values:
+  const std::vector<Pose3> relative_poses = {
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.105373, 0.311512, 0.656877, -0.678505},
+       {0.523923, 0.776654, 0.326659}},
+      {{0.568551, 0.595795, -0.561677, 0.079353},
+       {0.910927, 0.055169, -0.411761}},
+      {{0.542221, -0.592077, 0.303380, -0.513226},
+       {0.775288, 0.228798, -0.596923}},
+      {{0.327419, -0.125250, -0.534379, 0.769122},
+       {-0.577841, 0.628016, -0.543592}},
+      {{0.083672, 0.104639, 0.627755, 0.766795},
+       {-0.623267, 0.086928, 0.773222}},
+  };
+
+  // Initialize 5 poses with quaternion/point3 values:
+  const std::vector<Pose3> poses = {
+      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.421446, -0.351729, -0.597838, 0.584174},
+       {1.993500, 0.023275, 0.003793}},
+      {{0.067024, 0.331798, -0.200659, 0.919323},
+       {2.004291, 1.024305, 0.018047}},
+      {{0.765488, -0.035697, -0.462490, 0.445933},
+       {0.999908, 1.055073, 0.020212}},
+  };
+
+  // Specify connectivity:
+  using KeyPair = pair<Key, Key>;
+  std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
+
+  // Created expected factor graph
+  size_t i = 0;
+  NonlinearFactorGraph expectedGraph;
+  for (const auto& keys : edges) {
+    expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
+        keys.first, keys.second, relative_poses[i++], model);
+  }
+
+  // Check factor parsing
+  const auto actualFactors = parse3DFactors(g2oFile);
+  for (size_t i : {0, 1, 2, 3, 4, 5}) {
+    EXPECT(assert_equal(
+        *boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
+        *actualFactors[i], 1e-5));
+  }
+
+  // Check pose parsing
+  const auto actualPoses = parse3DPoses(g2oFile);
+  for (size_t j : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
+  }
+
+  // Check graph version
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = true;
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
-
+  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
-  Values expectedValues;
+  for (size_t j : {0, 1, 2, 3, 4}) {
-  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
+    EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
-  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
+  }
-  expectedValues.insert(0, Pose3(R0, p0));
-
-  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
-  expectedValues.insert(1, Pose3(R1, p1));
-
-  Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
-  Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
-  expectedValues.insert(2, Pose3(R2, p2));
-
-  Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
-  Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
-  expectedValues.insert(3, Pose3(R3, p3));
-
-  Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
-  Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
-  expectedValues.insert(4, Pose3(R4, p4));
-
-  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
-
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
-  NonlinearFactorGraph expectedGraph;
-
-  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
-
-  Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
-  Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
-
-  Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
-  Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
-
-  Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
-  Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
-
-  Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
-  Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
-
-  Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
-  Rot3 R30 = Rot3::Quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
-
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */