diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9a3c797b2..cd05b4f98 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -122,45 +122,6 @@ TEST( dataSet, Balbianello) EXPECT(assert_equal(expected,actual,1)); } -/* ************************************************************************* */ -TEST( dataSet, readG2o) -{ - const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile); - - 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(Vector3(44.721360, 44.721360, 30.901699)); - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); -} - /* ************************************************************************* */ TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); @@ -273,59 +234,82 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) } /* ************************************************************************* */ -TEST( dataSet, readG2oHuber) -{ - const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - bool is3D = false; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); - - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); - SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); - - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); +static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) { + NonlinearFactorGraph g; + using Factor = BetweenFactor; + g.emplace_shared(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + g.emplace_shared(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + g.emplace_shared(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + g.emplace_shared(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + g.emplace_shared(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + g.emplace_shared(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + g.emplace_shared(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + g.emplace_shared(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + g.emplace_shared(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + g.emplace_shared(9, 10, Pose2(1.003350, 0.022250, -0.195918), model); + g.emplace_shared(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + g.emplace_shared(3, 10, Pose2(0.044020, 0.988477, -1.553511), model); + return g; } /* ************************************************************************* */ -TEST( dataSet, readG2oTukey) -{ +TEST(dataSet, readG2o) { + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile); + + auto model = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); + + 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)); +} + +/* ************************************************************************* */ +TEST(dataSet, readG2oHuber) { const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = false; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); + boost::tie(actualGraph, actualValues) = + readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); - SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + auto baseModel = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), baseModel); - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); +} + +/* ************************************************************************* */ +TEST(dataSet, readG2oTukey) { + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = false; + boost::tie(actualGraph, actualValues) = + readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); + + auto baseModel = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); } /* ************************************************************************* */