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) { if(marginalizeEntireClique) {
// Remove the whole clique and its subtree, and keep the marginal factor. // Remove the whole clique and its subtree, and keep the marginal factor.
GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor();
marginalFactor->print("marginalFactor: ");
// We do not need the marginal factors associated with this clique // We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new // because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the // 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> { class GTSAM_EXPORT ISAM2: public BayesTree<ISAM2Clique> {
protected: public:
/** The current linearization point */ /** The current linearization point */
Values theta_; Values theta_;

15
tests/testGaussianISAM2.cpp
View File

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