12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #611 from borglab/jing/subgraph 

Added test for subgraph preconditioner in shonan and refined error message
release/4.3a0
jingwuOUO 2020-11-30 18:57:24 -05:00 committed by GitHub
parent b0962ede05 a8f4f1eb08
commit 0b045fdc59
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/SubgraphBuilder.cpp
View File

@ -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.

21
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -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);