diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 6df7e8450..9f2d84a90 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -15,6 +15,8 @@ * @author Michael Kaess, Richard Roberts */ +#include +#include #include #include @@ -163,7 +165,7 @@ ISAM2::Impl::PartialSolveResult ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { - static const bool debug = ISDEBUG("ISAM2 recalculate"); + const bool debug = ISDEBUG("ISAM2 recalculate"); PartialSolveResult result; @@ -200,9 +202,15 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, vector cmember(affectedKeysSelector.size(), 0); if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) { assert(reorderingMode.constrainedKeys); - if(keys.size() > reorderingMode.constrainedKeys->size()) { - BOOST_FOREACH(Index var, *reorderingMode.constrainedKeys) { - cmember[affectedKeysSelectorInverse[var]] = 1; + if(!reorderingMode.constrainedKeys->empty()) { + typedef std::pair Index_Group; + if(keys.size() > reorderingMode.constrainedKeys->size()) { // Only if some variables are unconstrained + BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { + cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second; } + } else { + int minGroup = *boost::range::min_element(boost::adaptors::values(*reorderingMode.constrainedKeys)); + BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { + cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second - minGroup; } } } } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 351a371df..9c1f28043 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -36,7 +36,7 @@ struct ISAM2::Impl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional&> constrainedKeys; + boost::optional > constrainedKeys; }; /** diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 58af039dd..8888a8259 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -18,6 +18,8 @@ #include #include // for operator += using namespace boost::assign; +#include +#include #include #include @@ -148,11 +150,11 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, - const boost::optional >& constrainKeys, ISAM2Result& result) { + const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. - static const bool debug = ISDEBUG("ISAM2 recalculate"); + const bool debug = ISDEBUG("ISAM2 recalculate"); // Input: BayesTree(this), newFactors @@ -224,13 +226,21 @@ boost::shared_ptr > ISAM2::recalculate( tic(1,"CCOLAMD"); // Do a batch step - reorder and relinearize all variables vector cmember(theta_.size(), 0); - FastSet constrainedKeysSet; - if(constrainKeys) - constrainedKeysSet = *constrainKeys; - else - constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); - if(theta_.size() > constrainedKeysSet.size()) { - BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } + if(constrainKeys) { + if(!constrainKeys->empty()) { + typedef std::pair Index_Group; + if(theta_.size() > constrainKeys->size()) { // Only if some variables are unconstrained + BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { + cmember[index_group.first] = index_group.second; } + } else { + int minGroup = *boost::range::min_element(boost::adaptors::values(*constrainKeys)); + BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { + cmember[index_group.first] = index_group.second - minGroup; } + } + } + } else { + if(theta_.size() > newKeys.size()) // Only if some variables are unconstrained + BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } } Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); @@ -331,10 +341,12 @@ boost::shared_ptr > ISAM2::recalculate( reorderingMode.nFullSystemVars = ordering_.nVars(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; - if(constrainKeys) + if(constrainKeys) { reorderingMode.constrainedKeys = *constrainKeys; - else - reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); + } else { + reorderingMode.constrainedKeys = FastMap(); + BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } + } Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); toc(2,"PartialSolve"); @@ -395,10 +407,10 @@ boost::shared_ptr > ISAM2::recalculate( /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, bool force_relinearize) { + const boost::optional >& constrainedKeys, bool force_relinearize) { - static const bool debug = ISDEBUG("ISAM2 update"); - static const bool verbose = ISDEBUG("ISAM2 update verbose"); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); static int count = 0; count++; @@ -521,11 +533,12 @@ ISAM2Result ISAM2::update( tic(9,"recalculate"); // 8. Redo top of Bayes tree // Convert constrained symbols to indices - boost::optional > constrainedIndices; + boost::optional > constrainedIndices; if(constrainedKeys) { - constrainedIndices.reset(FastSet()); - BOOST_FOREACH(Key key, *constrainedKeys) { - constrainedIndices->insert(ordering_[key]); + constrainedIndices = FastMap(); + typedef pair Key_Group; + BOOST_FOREACH(Key_Group key_group, *constrainedKeys) { + constrainedIndices->insert(make_pair(ordering_[key_group.first], key_group.second)); } } boost::shared_ptr > replacedKeys; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index b967eadc4..64408b520 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -405,7 +405,7 @@ public: */ ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); /** Access the current linearization point */ @@ -464,7 +464,7 @@ private: boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, - const boost::optional >& constrainKeys, ISAM2Result& result); + const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index a990d2ded..1c2290e8c 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -837,7 +837,9 @@ TEST(ISAM2, constrained_ordering) planarSLAM::Graph fullgraph; // We will constrain x3 and x4 to the end - FastSet constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4)); + FastMap constrained; + constrained.insert(make_pair(planarSLAM::PoseKey(3), 1)); + constrained.insert(make_pair(planarSLAM::PoseKey(4), 2)); // i keeps track of the time step size_t i = 0; @@ -926,8 +928,7 @@ TEST(ISAM2, constrained_ordering) EXPECT(isam_check(fullgraph, fullinit, isam)); // Check that x3 and x4 are last, but either can come before the other - EXPECT((isam.getOrdering()[planarSLAM::PoseKey(3)] == 12 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 13) || - (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12)); + EXPECT(isam.getOrdering()[planarSLAM::PoseKey(3)] == 12 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 13); // Check gradient at each node typedef ISAM2::sharedClique sharedClique;