diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index 17c24e23a..0c3136e9d 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -32,9 +32,9 @@ struct ISAM2::Impl { struct ReorderingMode { size_t nFullSystemVars; - enum { AS_ADDED, COLAMD } algorithm; - enum { NO_CONSTRAINT, LATEST_LAST } constrain; - boost::optional&> latestKeys; + enum { /*AS_ADDED,*/ COLAMD } algorithm; + enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; + boost::optional&> constrainedKeys; }; /** @@ -296,10 +296,10 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& facto toc(2,"variable index"); tic(3,"ccolamd"); vector cmember(affectedKeysSelector.size(), 0); - if(reorderingMode.constrain == ReorderingMode::LATEST_LAST) { - assert(reorderingMode.latestKeys); - if(keys.size() > reorderingMode.latestKeys->size()) { - BOOST_FOREACH(Index var, *reorderingMode.latestKeys) { + 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; } } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 343ec7a56..cdac57b7f 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -37,8 +37,6 @@ using namespace std; static const bool disableReordering = false; static const double batchThreshold = 0.65; -static const bool latestLast = true; -static const bool structuralLast = false; /* ************************************************************************* */ template @@ -155,7 +153,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { template boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastSet& structuralKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result) { + const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -181,26 +180,6 @@ boost::shared_ptr > ISAM2::recalculat counter++; #endif - FastSet affectedStructuralKeys; - if(structuralLast) { - tic(0, "affectedStructuralKeys"); - affectedStructuralKeys.insert(structuralKeys.begin(), structuralKeys.end()); - // For each structural variable, collect the variables up the path to the root, - // which will be constrained to the back of the ordering. - BOOST_FOREACH(Index key, structuralKeys) { - sharedClique clique = this->nodes_[key]; - while(clique) { - affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); -#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique - clique = boost::dynamic_pointer_cast(clique->parent_.lock()); -#else - clique = boost::static_pointer_cast(clique->parent_.lock()); -#endif - } - } - toc(0, "affectedStructuralKeys"); - } - if(debug) { cout << "markedKeys: "; BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } @@ -208,12 +187,6 @@ boost::shared_ptr > ISAM2::recalculat cout << "newKeys: "; BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; } cout << endl; - cout << "structuralKeys: "; - BOOST_FOREACH(const Index key, structuralKeys) { cout << key << " "; } - cout << endl; - cout << "affectedStructuralKeys: "; - BOOST_FOREACH(const Index key, affectedStructuralKeys) { cout << key << " "; } - cout << endl; } // 1. Remove top of Bayes tree and convert to a factor graph: @@ -257,16 +230,13 @@ boost::shared_ptr > ISAM2::recalculat tic(1,"CCOLAMD"); // Do a batch step - reorder and relinearize all variables vector cmember(theta_.size(), 0); - if(structuralLast) { - if(theta_.size() > affectedStructuralKeys.size()) { - BOOST_FOREACH(Index var, affectedStructuralKeys) { cmember[var] = 1; } - if(latestLast) { BOOST_FOREACH(Index var, newKeys) { cmember[var] = 2; } } - } - } else if(latestLast) { - FastSet newKeysSet(newKeys.begin(), newKeys.end()); - if(theta_.size() > newKeysSet.size()) { - BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; } - } + 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; } } Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); @@ -364,8 +334,11 @@ boost::shared_ptr > ISAM2::recalculat typename Impl::ReorderingMode reorderingMode; reorderingMode.nFullSystemVars = ordering_.nVars(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; - reorderingMode.constrain = Impl::ReorderingMode::LATEST_LAST; - reorderingMode.latestKeys = FastSet(newKeys.begin(), newKeys.end()); + reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; + if(constrainKeys) + reorderingMode.constrainedKeys = *constrainKeys; + else + reorderingMode.constrainedKeys = FastSet(newKeys.begin(), newKeys.end()); typename Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); toc(2,"PartialSolve"); @@ -424,7 +397,8 @@ boost::shared_ptr > ISAM2::recalculat /* ************************************************************************* */ template ISAM2Result ISAM2::update( - const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, bool force_relinearize) { + const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, + const boost::optional >& constrainedKeys, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -488,8 +462,6 @@ ISAM2Result ISAM2::update( // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them - FastSet structuralKeys; - if(structuralLast) structuralKeys = markedKeys; // If we're using structural-last ordering, make another copy toc(3,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold @@ -543,9 +515,17 @@ ISAM2Result ISAM2::update( tic(8,"recalculate"); // 8. Redo top of Bayes tree + // Convert constrained symbols to indices + boost::optional > constrainedIndices; + if(constrainedKeys) { + constrainedIndices.reset(FastSet()); + BOOST_FOREACH(const Symbol& key, *constrainedKeys) { + constrainedIndices->insert(ordering_[key]); + } + } boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors, result); + replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); toc(8,"recalculate"); tic(9,"solve"); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 544031a93..fdb42cfc8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -346,7 +346,9 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), const FastVector& removeFactorIndices = FastVector(), + ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), + const FastVector& removeFactorIndices = FastVector(), + const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); /** Access the current linearization point */ @@ -403,8 +405,9 @@ private: FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys) const; FactorGraph getCachedBoundaryFactors(Cliques& orphans); - boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& structuralKeys, - const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, ISAM2Result& result); + boost::shared_ptr > recalculate(const FastSet& markedKeys, + const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); }; // ISAM2 diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 5c59f8c28..d28844cd9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -766,6 +766,149 @@ TEST(ISAM2, removeFactors) EXPECT(assert_equal(expectedGradient, actualGradient)); } +/* ************************************************************************* */ +TEST(ISAM2, constrained_ordering) +{ + +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + + // Pose and landmark key types from planarSLAM + typedef planarSLAM::PoseKey PoseKey; + typedef planarSLAM::PointKey PointKey; + + // Set up parameters + SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); + SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + + // These variables will be reused and accumulate factors and values + GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + planarSLAM::Values fullinit; + planarSLAM::Graph fullgraph; + + // We will constrain x3 and x4 to the end + FastSet constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4)); + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + + isam.update(newfactors, init); + } + + CHECK(isam_check(fullgraph, fullinit, isam)); + + // Add odometry from time 0 to time 5 + for( ; i<5; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + if(i >= 3) + isam.update(newfactors, init, FastVector(), constrained); + else + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + planarSLAM::Values init; + init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init, FastVector(), constrained); + ++ i; + } + + // Compare solutions + 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)); + + // Check gradient at each node + typedef GaussianISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + // Compute expected gradient + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(jfg, expectedGradient); + // Compare with actual gradients + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const int dim = clique->conditional()->dim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + EXPECT(assert_equal(expectedGradient[*jit], actual)); + variablePosition += dim; + } + LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); + } + + // Check gradient + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); + gradientAtZero(isam, actualGradient); + EXPECT(assert_equal(expectedGradient2, expectedGradient)); + EXPECT(assert_equal(expectedGradient, actualGradient)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */