diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 509143b0b..f37856679 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -82,7 +82,7 @@ FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, +FastSet ISAM2::Impl::CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; @@ -109,6 +109,76 @@ FastSet ISAM2::Impl::CheckRelinearization(const Permuted& d return relinKeys; } +/* ************************************************************************* */ +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const Permuted& delta, const ISAM2Clique::shared_ptr& clique) { + + // Check the current clique for relinearization + bool relinearize = false; + BOOST_FOREACH(Index var, clique->conditional()->keys()) { + double maxDelta = delta[var].lpNorm(); + if(maxDelta >= threshold) { + relinKeys.insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if(relinearize) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + } + } +} + +/* ************************************************************************* */ +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const Permuted& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) { + + // Check the current clique for relinearization + bool relinearize = false; + BOOST_FOREACH(Index var, clique->conditional()->keys()) { + + // Lookup the key associated with this index + Key key = decoder.at(var); + + // Find the threshold for this variable type + const Vector& threshold = thresholds.find(Symbol(key).chr())->second; + + // Verify the threshold vector matches the actual variable size + if(threshold.rows() != delta[var].rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); + + // Check for relinearization + if((delta[var].array().abs() > threshold.array()).any()) { + relinKeys.insert(var); + relinearize = true; + } + } + + // If this node was relinearized, also check its children + if(relinearize) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child); + } + } +} + +/* ************************************************************************* */ +FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { + + FastSet relinKeys; + + if(relinearizeThreshold.type() == typeid(double)) { + CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); + + } else if(relinearizeThreshold.type() == typeid(FastMap)) { + Ordering::InvertedMap decoder = ordering.invert(); + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, decoder, root); + } + + return relinKeys; +} + /* ************************************************************************* */ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f2f03fb3a..0aafb3f35 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -68,7 +68,21 @@ struct ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, + static FastSet CheckRelinearizationFull(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /** + * Find the set of variables to be relinearized according to relinearizeThreshold. + * This check is performed recursively, starting at the top of the tree. Once a + * variable in the tree does not need to be relinearized, no further checks in + * that branch are performed. This is an approximation of the Full version, designed + * to save time at the expense of accuracy. + * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging + * @return The set of variable indices in delta whose magnitude is greater than or + * equal to relinearizeThreshold + */ + static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e2ab825b3..07e0f518d 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -588,8 +588,11 @@ ISAM2Result ISAM2::update( tic(5,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging + if(params_.enablePartialRelinearizationCheck) + relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); + else + relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index c118cb44e..5804640df 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -125,6 +125,13 @@ struct ISAM2Params { bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) + /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false). + * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate + * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance + * is desired over correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -139,7 +146,7 @@ struct ISAM2Params { relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false) {} + enableDetailedResults(false), enablePartialRelinearizationCheck(false) {} }; /** diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 80bd651c8..2a5bf7962 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -1242,6 +1242,129 @@ TEST(ISAM2, constrained_ordering) EXPECT(assert_equal(expectedGradient, actualGradient)); } +/* ************************************************************************* */ +TEST(ISAM2, slamlike_solution_partial_relinearization_check) +{ + + // These variables will be reused and accumulate factors and values + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam(params); + Values fullinit; + planarSLAM::Graph fullgraph; + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update isam + { + planarSLAM::Graph newfactors; + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((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.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 5 to 6 and landmark measurement at time 5 + { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); + fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + + isam.update(newfactors, init); + ++ i; + } + + // Add odometry from time 6 to time 10 + for( ; i<10; ++i) { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + + isam.update(newfactors, init); + } + + // Add odometry from time 10 to 11 and landmark measurement at time 10 + { + planarSLAM::Graph newfactors; + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + fullgraph.push_back(newfactors); + + Values init; + init.insert((i+1), Pose2(6.9, 0.1, 0.01)); + fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); + + isam.update(newfactors, init); + ++ i; + } + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam)); + + // Check gradient at each node + typedef ISAM2::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);} /* ************************************************************************* */