Merge pull request #611 from borglab/jing/subgraph

Added test for subgraph preconditioner in shonan and refined error message

gtsam/linear/SubgraphBuilder.cpp

@@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
   const vector<size_t> 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 be caused by disconnected graph");
   }
 
   // Downweight the tree edges to zero.

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -91,6 +91,27 @@ 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;
+
+  // 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
+  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);