added many unit test for g2o read/write

2014-05-20 17:59:49 -04:00 · 2014-05-20 17:59:49 -04:00 · 16571a9a95
parent a668b2a8e4
commit 16571a9a95
7 changed files with 179 additions and 20 deletions
--- a/.cproject
+++ b/.cproject
@ -2073,14 +2073,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testDataset.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDataset.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@ -2502,6 +2494,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDataset.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
--- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt
+++ b/examples/Data/dubrovnik-3-7-pre-rewritten.txt
@ -20,9 +20,9 @@
 1 6 543.18011474609375 294.80999755859375
 2 6 -58.419979095458984375 110.8300018310546875

-0.29656188120312942935
+  0.29656188120312942935
 -0.035318354384285870207
-0.31252101755032046793
+  0.31252101755032046793
 0.47230274932665988752
 -0.3572340863744113415
 -2.0517704282499575896
@ -30,21 +30,21 @@
 -7.5572756941255647689e-08
 3.2377570134516087119e-14

-0.28532097381985194184
+ 0.28532097381985194184
 -0.27699838370789808817
 0.048601169984112867206
-1.2598695987143850861
+  -1.2598695987143850861
 -0.049063798188844320869
-1.9586867140445654023
+  -1.9586867140445654023
 1432.137451171875
 -7.3171918302250560373e-08
 3.1759419042137054801e-14

 0.057491325683772541433
-0.34853090049579965592
-0.47985129303736057116
-8.1963904289063389541
-6.5146840788718787252
+ 0.34853090049579965592
+ 0.47985129303736057116
+ 8.1963904289063389541
+ 6.5146840788718787252
 -3.8392804395897406344
 1572.047119140625
 -1.5962623223231275915e-08
--- a/examples/Data/pose2example-rewritten.txt
+++ b/examples/Data/pose2example-rewritten.txt
@ -0,0 +1,23 @@
+VERTEX_SE2 0 0 0 0
+VERTEX_SE2 1 1.03039 0.01135 -0.081596
+VERTEX_SE2 2 2.03614 -0.129733 -0.301887
+VERTEX_SE2 3 3.0151 -0.442395 -0.345514
+VERTEX_SE2 4 3.34395 0.506678 1.21471
+VERTEX_SE2 5 3.68449 1.46405 1.18379
+VERTEX_SE2 6 4.06463 2.41478 1.17633
+VERTEX_SE2 7 4.42978 3.30018 1.25917
+VERTEX_SE2 8 4.12888 2.32148 -1.82539
+VERTEX_SE2 9 3.88465 1.32751 -1.95302
+VERTEX_SE2 10 3.53107 0.388263 -2.14893
+EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017
+EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017
--- a/examples/Data/pose2example.txt
+++ b/examples/Data/pose2example.txt
@ -0,0 +1,23 @@
+VERTEX_SE2 0 0.000000 0.000000 0.000000
+VERTEX_SE2 1 1.030390 0.011350 -0.081596
+VERTEX_SE2 2 2.036137 -0.129733 -0.301887
+VERTEX_SE2 3 3.015097 -0.442395 -0.345514
+VERTEX_SE2 4 3.343949 0.506678 1.214715
+VERTEX_SE2 5 3.684491 1.464049 1.183785
+VERTEX_SE2 6 4.064626 2.414783 1.176333
+VERTEX_SE2 7 4.429778 3.300180 1.259169
+VERTEX_SE2 8 4.128877 2.321481 -1.825391
+VERTEX_SE2 9 3.884653 1.327509 -1.953016
+VERTEX_SE2 10 3.531067 0.388263 -2.148934
+EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
+EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@ -554,7 +554,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
    if(! (is >> tag))
      break;

-    if (tag == "VERTEX_SE2") {
+    if (tag == "VERTEX_SE2" || tag == "VERTEX2") {
      int id;
      double x, y, yaw;
      is >> id >> x >> y >> yaw;
@ -571,7 +571,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
    if(! (is >> tag))
      break;

-    if (tag == "EDGE_SE2") {
+    if (tag == "EDGE_SE2" || tag == "EDGE2") {
      int id1, id2;
      double x, y, yaw;
      double I11, I12, I13, I22, I23, I33;
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@ -121,7 +121,7 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
 * @brief This function parses a g2o file and stores the measurements into a
 * NonlinearFactorGraph and the initial guess in a Values structure
 * @param filename The name of the g2o file
- * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
+ * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
 * @return initial Values containing the initial guess (VERTEX_SE2)
 */
 enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@ -21,6 +21,8 @@

 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/inference/Symbol.h>
+
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>

 using namespace std;
@ -70,6 +72,117 @@ TEST( dataSet, Balbianello)
  EXPECT(assert_equal(expected,actual,1));
 }

+/* ************************************************************************* */
+TEST( dataSet, readG2o)
+{
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph actualGraph;
+  Values actualValues;
+  readG2o(g2oFile, actualGraph, actualValues);
+
+  Values expectedValues;
+  expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
+  expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
+  expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
+  expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
+  expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
+  expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
+  expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
+  expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
+  expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
+  expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
+  expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
+  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
+  NonlinearFactorGraph expectedGraph;
+  expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
+  expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
+  expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
+  expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
+  expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
+  expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
+  expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
+  expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
+  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( dataSet, readG2oHuber)
+{
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph actualGraph;
+  Values actualValues;
+  readG2o(g2oFile, actualGraph, actualValues, HUBER);
+
+  noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
+  SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
+
+  NonlinearFactorGraph expectedGraph;
+  expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
+  expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
+  expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
+  expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
+  expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
+  expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
+  expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
+  expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
+  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( dataSet, readG2oTukey)
+{
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph actualGraph;
+  Values actualValues;
+  readG2o(g2oFile, actualGraph, actualValues, TUKEY);
+
+  noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
+  SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
+
+  NonlinearFactorGraph expectedGraph;
+  expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
+  expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
+  expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
+  expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
+  expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
+  expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
+  expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
+  expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
+  expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
+  expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
+  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( dataSet, writeG2o)
+{
+  const string g2oFile = findExampleDataFile("pose2example");
+  NonlinearFactorGraph expectedGraph;
+  Values expectedValues;
+  readG2o(g2oFile, expectedGraph, expectedValues);
+
+  const string filenameToWrite = findExampleDataFile("pose2example-rewritten");
+  writeG2o(filenameToWrite, expectedGraph, expectedValues);
+
+  NonlinearFactorGraph actualGraph;
+  Values actualValues;
+  readG2o(filenameToWrite, actualGraph, actualValues);
+  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+}
+
 /* ************************************************************************* */
 TEST( dataSet, readBAL_Dubrovnik)
 {