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

Fixed ISAM(2) unit tests

release/4.3a0
Richard Roberts 2013-08-11 00:40:04 +00:00
parent 211542fc0e
commit 3228895c26
2 changed files with 19 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
tests/testGaussianISAM.cpp
View File

@ -53,18 +53,16 @@ TEST( ISAM, iSAM_smoother )
} }
// Create expected Bayes Tree by solving smoother with "natural" ordering // Create expected Bayes Tree by solving smoother with "natural" ordering
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
GaussianISAM expected;
expected.insertRoot(bayesTree.roots().front());
// Verify sigmas in the bayes tree // Verify sigmas in the bayes tree
BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree.nodes() | br::map_values) { BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, expected.nodes() | br::map_values) {
GaussianConditional::shared_ptr conditional = clique->conditional(); GaussianConditional::shared_ptr conditional = clique->conditional();
EXPECT(!conditional->get_model()); EXPECT(!conditional->get_model());
} }
// Check whether BayesTree is correct // Check whether BayesTree is correct
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
// obtain solution // obtain solution
VectorValues e; // expected solution VectorValues e; // expected solution

23
tests/testGaussianISAM2.cpp
View File

@ -93,6 +93,9 @@ ISAM2 createSlamlikeISAM2(
goto done; goto done;
} }
if(i > maxPoses)
goto done;
// Add odometry from time 5 to 6 and landmark measurement at time 5 // Add odometry from time 5 to 6 and landmark measurement at time 5
{ {
NonlinearFactorGraph newfactors; NonlinearFactorGraph newfactors;
@ -132,6 +135,9 @@ ISAM2 createSlamlikeISAM2(
goto done; goto done;
} }
if(i > maxPoses)
goto done;
// Add odometry from time 10 to 11 and landmark measurement at time 10 // Add odometry from time 10 to 11 and landmark measurement at time 10
{ {
NonlinearFactorGraph newfactors; NonlinearFactorGraph newfactors;
@ -229,9 +235,12 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
bool isamEqual = assert_equal(expected, actual); bool isamEqual = assert_equal(expected, actual);
// Check information // Check information
ISAM2 expectedisam(isam.params()); GaussianFactorGraph isamGraph(isam);
expectedisam.update(fullgraph, fullinit); isamGraph += isam.roots().front()->cachedFactor_;
bool isamTreeEqual = assert_equal(GaussianFactorGraph(expectedisam).augmentedHessian(), GaussianFactorGraph(isam).augmentedHessian()); Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian();
Matrix actualHessian = isamGraph.augmentedHessian();
expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1);
bool isamTreeEqual = assert_equal(expectedHessian, actualHessian);
// Check consistency // Check consistency
ConsistencyVisitor visitor(isam); ConsistencyVisitor visitor(isam);
@ -284,10 +293,10 @@ TEST(ISAM2, simple)
// These variables will be reused and accumulate factors and values // These variables will be reused and accumulate factors and values
Values fullinit; Values fullinit;
NonlinearFactorGraph fullgraph; NonlinearFactorGraph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.0), 0.0, 0, false), i); ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i);
// Compare solutions // Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
} }
} }
@ -647,13 +656,13 @@ namespace {
// Check full marginalization // Check full marginalization
//cout << "treeEqual" << endl; //cout << "treeEqual" << endl;
bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
//cout << "nonlinEqual" << endl; //cout << "nonlinEqual" << endl;
bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6);
//actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6);
//cout << "nonlinCorrect" << endl; //cout << "nonlinCorrect" << endl;
actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6);
bool ok = treeEqual && linEqual && nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; bool ok = treeEqual && linEqual && nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect;
return ok; return ok;