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
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
GaussianISAM expected;
expected.insertRoot(bayesTree.roots().front());
GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);
// 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();
EXPECT(!conditional->get_model());
}
// Check whether BayesTree is correct
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));
// obtain solution
VectorValues e; // expected solution

23
tests/testGaussianISAM2.cpp
View File

@ -93,6 +93,9 @@ ISAM2 createSlamlikeISAM2(
goto done;
}
if(i > maxPoses)
goto done;
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
NonlinearFactorGraph newfactors;
@ -132,6 +135,9 @@ ISAM2 createSlamlikeISAM2(
goto done;
}
if(i > maxPoses)
goto done;
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
NonlinearFactorGraph newfactors;
@ -229,9 +235,12 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
bool isamEqual = assert_equal(expected, actual);
// Check information
ISAM2 expectedisam(isam.params());
expectedisam.update(fullgraph, fullinit);
bool isamTreeEqual = assert_equal(GaussianFactorGraph(expectedisam).augmentedHessian(), GaussianFactorGraph(isam).augmentedHessian());
GaussianFactorGraph isamGraph(isam);
isamGraph += isam.roots().front()->cachedFactor_;
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
ConsistencyVisitor visitor(isam);
@ -284,10 +293,10 @@ TEST(ISAM2, simple)
// These variables will be reused and accumulate factors and values
Values fullinit;
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
CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_));
}
}
@ -647,13 +656,13 @@ namespace {
// Check full marginalization
//cout << "treeEqual" << endl;
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;
bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 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);
//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;
return ok;