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

Temporary debugging for marginalizeLeaves

release/4.3a0
Richard Roberts 2013-08-10 15:52:00 +00:00
parent 67431ba8ad
commit 63e89656d2
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/nonlinear/ISAM2.cpp
View File

@ -781,6 +781,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList)
if(marginalizeEntireClique) {
// Remove the whole clique and its subtree, and keep the marginal factor.
GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor();
marginalFactor->print("marginalFactor: ");
// We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the

2
gtsam/nonlinear/ISAM2.h
View File

@ -399,7 +399,7 @@ private:
*/
class GTSAM_EXPORT ISAM2: public BayesTree<ISAM2Clique> {
protected:
public:
/** The current linearization point */
Values theta_;

15
tests/testGaussianISAM2.cpp
View File

@ -623,16 +623,15 @@ namespace {
toKeep.push_back(j);
// Calculate expected marginal from iSAM2 tree
expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep)->augmentedHessian();
expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian();
//// Calculate expected marginal from cached linear factors
//assert(isam.params().cacheLinearizedFactors);
//GaussianSequentialSolver solver2(isam.linearFactors_, isam.params().factorization == ISAM2Params::QR);
//expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian();
// Calculate expected marginal from cached linear factors
assert(isam.params().cacheLinearizedFactors);
Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian();
// Calculate expected marginal from original nonlinear factors
expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint())
->marginal(toKeep)->augmentedHessian();
->marginal(toKeep, EliminateQR)->augmentedHessian();
// Do marginalization
isam.marginalizeLeaves(leafKeys);
@ -648,7 +647,7 @@ namespace {
// Check full marginalization
//cout << "treeEqual" << endl;
bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6);
//bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6);
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);
@ -656,7 +655,7 @@ namespace {
//cout << "nonlinCorrect" << endl;
actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); 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;
}
}