diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index da0a997c2..b1034a594 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -65,7 +65,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors) { + GaussianFactorGraph& linearFactors, FastSet& fixedVariables) { // Get indices of unused keys vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); @@ -113,12 +113,13 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli nodes.swap(newNodes); } - // Reorder and remove from ordering and solution + // Reorder and remove from ordering, solution, and fixed keys ordering.permuteInPlace(unusedToEnd); BOOST_REVERSE_FOREACH(Key key, unusedKeys) { Ordering::value_type removed = ordering.pop_back(); assert(removed.first == key); theta.erase(key); + fixedVariables.erase(key); } // Finally, permute references to variables diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index a39b321d8..57dc881ad 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -57,7 +57,7 @@ struct ISAM2::Impl { static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors); + GaussianFactorGraph& linearFactors, FastSet& fixedVariables); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 53d0ed0ff..367b88833 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -31,7 +31,8 @@ using namespace boost::assign; #include #include - +#include +#include namespace gtsam { @@ -673,6 +674,9 @@ ISAM2Result ISAM2::update( if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed + BOOST_FOREACH(Key key, fixedVariables_) { + relinKeys.erase(ordering_[key]); + } if(noRelinKeys) { BOOST_FOREACH(Key key, *noRelinKeys) { relinKeys.erase(ordering_[key]); @@ -765,7 +769,7 @@ ISAM2Result ISAM2::update( if(!unusedKeys.empty()) { gttic(remove_variables); Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); + deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -780,6 +784,134 @@ ISAM2Result ISAM2::update( return result; } +/* ************************************************************************* */ +void ISAM2::experimentalMarginalizeLeaves(const FastList& leafKeys) +{ + // Convert set of keys into a set of indices + FastSet indices; + BOOST_FOREACH(Key key, leafKeys) { + indices.insert(ordering_[key]); + } + + // For each clique containing variables to be marginalized, we need to + // reeliminate the marginalized variables and add their linear contribution + // factor into the factor graph. This results in some extra work if we are + // marginalizing multiple cliques on the same path. + + // To do this, first build a map containing all cliques containing variables to be + // marginalized that determines the last-ordered variable eliminated in that + // clique. + FastMap cliquesToMarginalize; + BOOST_FOREACH(Index jI, indices) + { + assert(nodes_[jI]); // Assert that the nodes structure contains this index + pair::iterator, bool> insertResult = + cliquesToMarginalize.insert(make_pair(nodes_[jI], jI)); + // If insert found a map entry existing, see if our current index is + // eliminated later and modify the entry if so. + if(!insertResult.second && jI > insertResult.first->second) + insertResult.first->second = jI; + } + +#ifndef NDEBUG + // Check that the cliques have sorted frontal keys - we assume that here. + for(FastMap::const_iterator c = cliquesToMarginalize.begin(); + c != cliquesToMarginalize.end(); ++c) + { + FastSet keys(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals()); + assert(std::equal(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals(), keys.begin())); + } +#endif + + // Now loop over the indices + FastSet factorIndicesToRemove; + GaussianFactorGraph factorsToAdd; + for(FastSet::iterator jI = indices.begin(); jI != indices.end(); ) + { + const Index j = *jI; + + // Retrieve the clique and the latest-ordered index corresponding to this index + FastMap::iterator clique_lastIndex = cliquesToMarginalize.find(nodes_[*jI]); + assert(clique_lastIndex != cliquesToMarginalize.end()); // Assert that we indexed the clique + + // Check that the clique has no children + if(!clique_lastIndex->first->children().empty()) + throw MarginalizeNonleafException(ordering_.key(*jI), params_.keyFormatter); + + // Mark factors to be removed + BOOST_FOREACH(size_t i, variableIndex_[*jI]) { + factorIndicesToRemove.insert(i); + } + + // Check that all previous variables in the clique are also being eliminated and no later ones. + // At the same time, remove the indices marginalized with this clique from the indices set. + // This is where the iterator j is advanced. + size_t nFrontals = 0; + { + bool foundLast = false; + BOOST_FOREACH(Index cliqueVar, *clique_lastIndex->first->conditional()) { + if(!foundLast && indices.find(cliqueVar) == indices.end()) + throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter); + if(foundLast && indices.find(cliqueVar) != indices.end()) + throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter); + if(!foundLast) { + ++ nFrontals; + if(cliqueVar == *jI) + indices.erase(jI++); // j gets advanced here + else + indices.erase(cliqueVar); + } + if(cliqueVar == clique_lastIndex->second) + foundLast = true; + } + } + + // Reeliminate the factored conditional + marginal that was previously making up the clique + GaussianFactorGraph cliqueGraph; + cliqueGraph.push_back(clique_lastIndex->first->conditional()->toFactor()); + assert(clique_lastIndex->first->cachedFactor()); // Assert that we have the cached marginal piece + cliqueGraph.push_back(clique_lastIndex->first->cachedFactor()); + pair eliminationResult = + params_.factorization==ISAM2Params::QR ? + EliminateQR(cliqueGraph, nFrontals) : + EliminatePreferCholesky(cliqueGraph, nFrontals); + + // Add the marginal into the factor graph + factorsToAdd.push_back(eliminationResult.second); + + // Remove the clique + this->removeClique(clique_lastIndex->first); + } + + // Remove any factors touching the marginalized-out variables + vector removedFactorIndices; + SymbolicFactorGraph removedFactors; + BOOST_FOREACH(size_t i, factorIndicesToRemove) { + removedFactorIndices.push_back(i); + removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_)); + nonlinearFactors_.remove(i); + if(params_.cacheLinearizedFactors) + linearFactors_.remove(i); + } + + // Add the new factors and fix linearization points of involved variables + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorsToAdd) { + nonlinearFactors_.push_back(boost::make_shared( + factor, ordering_)); + if(params_.cacheLinearizedFactors) + linearFactors_.push_back(factor); + BOOST_FOREACH(Index factorIndex, *factor) { + fixedVariables_.insert(ordering_.key(factorIndex)); + } + } + variableIndex_.augment(factorsToAdd); // Augment the variable index + + // Remove the marginalized variables + variableIndex_.remove(removedFactorIndices, removedFactors); + Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_); +} + /* ************************************************************************* */ void ISAM2::updateDelta(bool forceFullSolve) const { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3e74c9767..7f9fec87b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -494,6 +494,10 @@ protected: /** The current Dogleg Delta (trust region radius) */ mutable boost::optional doglegDelta_; + /** Set of variables that are involved with linear factors from marginalized + * variables and thus cannot have their linearization points changed. */ + FastSet fixedVariables_; + public: typedef ISAM2 This; ///< This class @@ -545,6 +549,8 @@ public: const boost::optional >& noRelinKeys = boost::none, bool force_relinearize = false); + void experimentalMarginalizeLeaves(const FastList& leafKeys); + /** Access the current linearization point */ const Values& getLinearizationPoint() const { return theta_; } diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h new file mode 100644 index 000000000..6f512f65f --- /dev/null +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file nonlinearExceptions.h + * @brief Exceptions that may be thrown by nonlinear optimization components + * @author Richard Roberts + * @date Aug 17, 2012 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + Thrown when requesting to marginalize out variables from ISAM2 that are not + leaves. To make the variables you would like to marginalize be leaves, their + ordering should be constrained using the constrainedKeys argument to + ISAM2::update(). + */ + class MarginalizeNonleafException : public std::exception { + Key key_; + KeyFormatter formatter_; + mutable std::string what_; + public: + MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() : + key_(key), formatter_(formatter) {} + virtual ~MarginalizeNonleafException() throw() {} + Key key() const { return key_; } + virtual const char* what() const throw() { + if(what_.empty()) + what_ = +"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\ +is not a leaf. To make the variables you would like to marginalize be leaves,\n\ +their ordering should be constrained using the constrainedKeys argument to\n\ +ISAM2::update().\n"; + return what_.c_str(); + } + }; + +} diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index befb66ef0..96bbd5949 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -286,8 +286,9 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); variableIndex.remove(removedFactorsI, removedFactors); GaussianFactorGraph linearFactors; + FastSet fixedVariables; ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, - replacedKeys, ordering, nodes, linearFactors); + replacedKeys, ordering, nodes, linearFactors, fixedVariables); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(variableIndexExpected, variableIndex)); @@ -820,7 +821,6 @@ TEST(ISAM2, constrained_ordering) /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_partial_relinearization_check) { - // These variables will be reused and accumulate factors and values Values fullinit; NonlinearFactorGraph fullgraph; @@ -832,6 +832,43 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, marginalizeLeaves) +{ + // Create isam2 + ISAM2 isam = createSlamlikeISAM2(); + + // Get linearization point + Values soln = isam.calculateBestEstimate(); + + // Calculate expected marginal + GaussianFactorGraph isamAsGraph(isam); + GaussianSequentialSolver solver(isamAsGraph); + vector toKeep; + const Index lastVar = isam.getOrdering().size() - 1; + for(Index i=0; i<=lastVar; ++i) + if(i != isam.getOrdering()[0]) + toKeep.push_back(i); + GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep); + vector toFrontI; + toFrontI.push_back(isam.getOrdering()[0]); + Permutation toFront = Permutation::PullToFront(toFrontI, lastVar+1); + marginalgfg.permuteWithInverse(*toFront.inverse()); + Matrix expectedAugmentedHessian = marginalgfg.augmentedHessian(); + + // Marginalize + FastList marginalizeKeys; + marginalizeKeys.push_back(isam.getOrdering().key(0)); + isam.experimentalMarginalizeLeaves(marginalizeKeys); + + // Check + GaussianFactorGraph actualMarginalGraph(isam); + Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); + + LONGS_EQUAL(lastVar-1, isam.getOrdering().size()-1); + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */