diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 1200c8ebb..cf35a42cf 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -91,6 +91,24 @@ TEST(ShonanAveraging3, checkOptimality) { EXPECT(!kShonan.checkOptimality(random)); } +/* ************************************************************************* */ +TEST(ShonanAveraging3, checkSubgraph) { + // Create parameter with solver set to SUBGRAPH + auto params = ShonanAveragingParameters3( + gtsam::LevenbergMarquardtParams::CeresDefaults(), "SUBGRAPH"); + ShonanAveraging3::Measurements measurements; + + auto subgraphShonan = fromExampleName("toyExample.g2o", params); + + // Create initial random estimation + Values initial; + initial = subgraphShonan.initializeRandomly(kRandomNumberGenerator); + + // Run Shonan with SUBGRAPH solver + auto result = subgraphShonan.run(initial, 3, 3); + EXPECT_DOUBLES_EQUAL(1e-11, subgraphShonan.cost(result.first), 1e-4); +} + /* ************************************************************************* */ TEST(ShonanAveraging3, tryOptimizingAt3) { const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);