Merge remote-tracking branch 'origin/feature/optimization_params' into feature/more_noise_wrapped

cython/gtsam/tests/test_NonlinearOptimizer.py

@@ -0,0 +1,71 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for IMU testing scenarios.
+Author: Frank Dellaert & Duy Nguyen Ta (Python)
+"""
+# pylint: disable=invalid-name, no-name-in-module
+
+from __future__ import print_function
+
+import unittest
+
+import gtsam
+from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
+                   GaussNewtonParams, LevenbergMarquardtOptimizer,
+                   LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
+                   Point2, PriorFactorPoint2, Values)
+
+KEY1 = 1
+KEY2 = 2
+
+
+class TestScenario(unittest.TestCase):
+    def test_optimize(self):
+        """Do trivial test with three optimizer variants."""
+        fg = NonlinearFactorGraph()
+        model = gtsam.noiseModel_Unit.Create(2)
+        fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
+
+        # test error at minimum
+        xstar = Point2(0, 0)
+        optimal_values = Values()
+        optimal_values.insert(KEY1, xstar)
+        self.assertEqual(0.0, fg.error(optimal_values), 0.0)
+
+        # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
+        x0 = Point2(3, 3)
+        initial_values = Values()
+        initial_values.insert(KEY1, x0)
+        self.assertEqual(9.0, fg.error(initial_values), 1e-3)
+
+        # optimize parameters
+        ordering = Ordering()
+        ordering.push_back(KEY1)
+
+        # Gauss-Newton
+        gnParams = GaussNewtonParams()
+        gnParams.setOrdering(ordering)
+        actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual1))
+
+        # Levenberg-Marquardt
+        lmParams = LevenbergMarquardtParams.CeresDefaults()
+        lmParams.setOrdering(ordering)
+        actual2 = LevenbergMarquardtOptimizer(
+            fg, initial_values, lmParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual2))
+
+        # Dogleg
+        dlParams = DoglegParams()
+        dlParams.setOrdering(ordering)
+        actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
+        self.assertAlmostEqual(0, fg.error(actual3))
+
+
+if __name__ == "__main__":
+    unittest.main()

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

@@ -2019,10 +2019,12 @@ virtual class NonlinearOptimizerParams {
   void setVerbosity(string s);
 
   string getLinearSolverType() const;
-
   void setLinearSolverType(string solver);
-  void setOrdering(const gtsam::Ordering& ordering);
+
   void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
+  void setOrdering(const gtsam::Ordering& ordering);
+  string getOrderingType() const;
+  void setOrderingType(string ordering);
 
   bool isMultifrontal() const;
   bool isSequential() const;
@@ -2043,15 +2045,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
 virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
   LevenbergMarquardtParams();
 
-  double getlambdaInitial() const;
+  bool getDiagonalDamping() const;
   double getlambdaFactor() const;
+  double getlambdaInitial() const;
+  double getlambdaLowerBound() const;
   double getlambdaUpperBound() const;
+  bool getUseFixedLambdaFactor();
+  string getLogFile() const;
   string getVerbosityLM() const;
-
+  
-  void setlambdaInitial(double value);
+  void setDiagonalDamping(bool flag);
   void setlambdaFactor(double value);
+  void setlambdaInitial(double value);
+  void setlambdaLowerBound(double value);
   void setlambdaUpperBound(double value);
+  void setUseFixedLambdaFactor(bool flag);
+  void setLogFile(string s);
   void setVerbosityLM(string s);
+
+  static gtsam::LevenbergMarquardtParams LegacyDefaults();
+  static gtsam::LevenbergMarquardtParams CeresDefaults();
+
+  static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
+      gtsam::LevenbergMarquardtParams params,
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::LevenbergMarquardtParams ReplaceOrdering(
+      gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
 };
 
 #include <gtsam/nonlinear/DoglegOptimizer.h>
@@ -2497,6 +2516,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/nonlinear/LevenbergMarquardtParams.h

@@ -122,24 +122,35 @@ public:
   virtual ~LevenbergMarquardtParams() {}
   void print(const std::string& str = "") const override;
 
-  /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++.
+  /// @name Getters/Setters, mainly for wrappers. Use fields above in C++.
   /// @{
   bool getDiagonalDamping() const { return diagonalDamping; }
   double getlambdaFactor() const { return lambdaFactor; }
   double getlambdaInitial() const { return lambdaInitial; }
   double getlambdaLowerBound() const { return lambdaLowerBound; }
   double getlambdaUpperBound() const { return lambdaUpperBound; }
+  bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
   std::string getLogFile() const { return logFile; }
   std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
+  
   void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
   void setlambdaFactor(double value) { lambdaFactor = value; }
   void setlambdaInitial(double value) { lambdaInitial = value; }
   void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
   void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
-  void setLogFile(const std::string& s) { logFile = s; }
   void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
+  void setLogFile(const std::string& s) { logFile = s; }
   void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
   // @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  boost::shared_ptr<NonlinearOptimizerParams> clone() const {
+    return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
+  }
+
+  /// @}
 };
 
 }

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -54,47 +54,24 @@ public:
   }
   virtual void print(const std::string& str = "") const;
 
-  size_t getMaxIterations() const {
+  size_t getMaxIterations() const { return maxIterations; }
-    return maxIterations;
+  double getRelativeErrorTol() const { return relativeErrorTol; }
-  }
+  double getAbsoluteErrorTol() const { return absoluteErrorTol; }
-  double getRelativeErrorTol() const {
+  double getErrorTol() const { return errorTol; }
-    return relativeErrorTol;
+  std::string getVerbosity() const { return verbosityTranslator(verbosity); }
-  }
-  double getAbsoluteErrorTol() const {
-    return absoluteErrorTol;
-  }
-  double getErrorTol() const {
-    return errorTol;
-  }
-  std::string getVerbosity() const {
-    return verbosityTranslator(verbosity);
-  }
 
-  void setMaxIterations(int value) {
+  void setMaxIterations(int value) { maxIterations = value; }
-    maxIterations = value;
+  void setRelativeErrorTol(double value) { relativeErrorTol = value; }
-  }
+  void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
-  void setRelativeErrorTol(double value) {
+  void setErrorTol(double value) { errorTol = value; }
-    relativeErrorTol = value;
+  void setVerbosity(const std::string& src) {
-  }
-  void setAbsoluteErrorTol(double value) {
-    absoluteErrorTol = value;
-  }
-  void setErrorTol(double value) {
-    errorTol = value;
-  }
-  void setVerbosity(const std::string &src) {
     verbosity = verbosityTranslator(src);
   }
 
   static Verbosity verbosityTranslator(const std::string &s) ;
   static std::string verbosityTranslator(Verbosity value) ;
 
-  // Successive Linearization Parameters
-
-public:
-
   /** See NonlinearOptimizerParams::linearSolverType */
-
   enum LinearSolverType {
     MULTIFRONTAL_CHOLESKY,
     MULTIFRONTAL_QR,
@@ -168,13 +145,9 @@ public:
 
 private:
   std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
-
   LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
-
   std::string orderingTypeTranslator(Ordering::OrderingType type) const;
-
   Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
-
 };
 
 // For backward compatibility:

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));
 }
 
 /* ************************************************************************* */