From 122edc1da42471cda8e14a833680f7e568520495 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 24 Nov 2020 11:57:07 -0500 Subject: [PATCH 1/4] Added test for subgraph preconditioner in shonan --- gtsam/sfm/tests/testShonanAveraging.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) 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); From b5a091998266e619ad6d99c56bbcc2e3ae26bbb6 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 24 Nov 2020 11:57:24 -0500 Subject: [PATCH 2/4] Refined error message in subgraphbuilder --- gtsam/linear/SubgraphBuilder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index c6b3ca15f..738c101db 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { const vector tree = buildTree(gfg, forward_ordering, weights); if (tree.size() != n - 1) { throw std::runtime_error( - "SubgraphBuilder::operator() failure: tree.size() != n-1"); + "SubgraphBuilder::operator() failure: tree.size() != n-1, might caused by disconnected graph"); } // Downweight the tree edges to zero. From 5cb45e7e2516ba8059c0a1c3410455754af5e271 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 30 Nov 2020 10:06:29 -0500 Subject: [PATCH 3/4] Fixed typo --- gtsam/linear/SubgraphBuilder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 738c101db..1919d38be 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const { const vector tree = buildTree(gfg, forward_ordering, weights); if (tree.size() != n - 1) { throw std::runtime_error( - "SubgraphBuilder::operator() failure: tree.size() != n-1, might caused by disconnected graph"); + "SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph"); } // Downweight the tree edges to zero. From a8f4f1eb08b917a6ef30c007980119138b2f7ba4 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 30 Nov 2020 17:35:43 -0500 Subject: [PATCH 4/4] Added more description to the toyExample.g2o --- gtsam/sfm/tests/testShonanAveraging.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index cf35a42cf..172166116 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -98,6 +98,9 @@ TEST(ShonanAveraging3, checkSubgraph) { gtsam::LevenbergMarquardtParams::CeresDefaults(), "SUBGRAPH"); ShonanAveraging3::Measurements measurements; + // The toyExample.g2o has 5 vertices, from 0-4 + // The edges are: 1-2, 2-3, 3-4, 3-1, 1-4, 0-1, + // which can build a connected graph auto subgraphShonan = fromExampleName("toyExample.g2o", params); // Create initial random estimation