diff --git a/.cproject b/.cproject index 02aba4f6f..c89bab640 100644 --- a/.cproject +++ b/.cproject @@ -2073,14 +2073,6 @@ true true - - make - -j5 - testDataset.run - true - true - true - make -j5 @@ -2502,6 +2494,14 @@ true true + + make + -j5 + testDataset.run + true + true + true + make -j5 diff --git a/examples/Data/dubrovnik-3-7-pre-rewritten.txt b/examples/Data/dubrovnik-3-7-pre-rewritten.txt index 12c9f4db4..7dea43c9e 100644 --- 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 diff --git a/examples/Data/pose2example-rewritten.txt b/examples/Data/pose2example-rewritten.txt new file mode 100644 index 000000000..6c8fe38a2 --- /dev/null +++ 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 diff --git a/examples/Data/pose2example.txt b/examples/Data/pose2example.txt new file mode 100644 index 000000000..c6c69c881 --- /dev/null +++ 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 \ No newline at end of file diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 1029d7615..853503ad4 100644 --- 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; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 7bbc88f70..2f73db37f 100644 --- 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 }; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index b79312d43..f4bcdded0 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -21,6 +21,8 @@ #include #include + +#include #include 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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); + expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); + expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); + expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); + expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); + expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); + expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); + expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); + expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); + expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); + expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); + expectedGraph.add(BetweenFactor(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) {