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

Only call iSAM2 marginalize function if there are actually keys to marginalizae out

release/4.3a0
Stephen Williams 2013-03-18 22:28:14 +00:00
parent 9e1a07fcec
commit 396732ff6a
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -102,16 +102,15 @@ bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
isam.getLinearizationPoint(), isam.getOrdering())->augmentedHessian();
assert(
actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all());
assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite<double>)).all());
// Check full marginalization
std::cout << "Checking treeEqual..." << std::endl;
bool treeEqual = assert_equal(expectedAugmentedHessian,
actualAugmentedHessian, 1e-6);
//bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
std::cout << "Checking nonlinEqual..." << std::endl;
bool nonlinEqual = assert_equal(expected3AugmentedHessian,
actualAugmentedHessian, 1e-6);
@ -121,6 +120,8 @@ bool checkMarginalizeLeaves(ISAM2& isam, const FastList<Key>& leafKeys) {
actual3AugmentedHessian.bottomRightCorner(1, 1) =
expected3AugmentedHessian.bottomRightCorner(1, 1);
std::cout << "Checking afterNonlinCorrect..." << std::endl;
bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian,
actual3AugmentedHessian, 1e-6);
@ -185,9 +186,11 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
}
// Marginalize out any needed variables
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
// isam_.experimentalMarginalizeLeaves(leafKeys);
checkMarginalizeLeaves(isam_, leafKeys);
if(marginalizableKeys.size() > 0) {
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
// isam_.experimentalMarginalizeLeaves(leafKeys);
checkMarginalizeLeaves(isam_, leafKeys);
}
// Remove marginalized keys from the KeyTimestampMap
eraseKeyTimestampMap(marginalizableKeys);