Added yet another configuration parameter to iSAM2. 'enablePartialRelinearizationCheck' checks variables for relinearization by descending the Bayes tree. Once a variable does not need to be relinearized, we stop checking that branch. A full check is still the default.

gtsam/nonlinear/ISAM2-impl.cpp

@@ -82,7 +82,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const N
 }
 
 /* ************************************************************************* */
-FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
+FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const Permuted<VectorValues>& delta, const Ordering& ordering,
     const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
   FastSet<Index> relinKeys;
 
@@ -109,6 +109,76 @@ FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& d
   return relinKeys;
 }
 
+/* ************************************************************************* */
+void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const Permuted<VectorValues>& 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<Eigen::Infinity>();
+    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<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const Permuted<VectorValues>& 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<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
+    const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
+
+  FastSet<Index> relinKeys;
+
+  if(relinearizeThreshold.type() == typeid(double)) {
+    CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
+
+  } else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
+    Ordering::InvertedMap decoder = ordering.invert();
+    CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
+  }
+
+  return relinKeys;
+}
+
 /* ************************************************************************* */
 void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
   static const bool debug = false;

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<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
+  static FastSet<Index> CheckRelinearizationFull(const Permuted<VectorValues>& 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<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const Permuted<VectorValues>& delta, const Ordering& ordering,
       const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
   /**

gtsam/nonlinear/ISAM2.cpp

@@ -588,8 +588,11 @@ ISAM2Result ISAM2::update(
     tic(5,"gather relinearize keys");
     vector<bool> 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(params_.enablePartialRelinearizationCheck)
-    if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
+      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) {

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) {}
 };
 
 /**

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<JacobianFactor> 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<JacobianFactor>(isam), expectedGradient);
+  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(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);}
 /* ************************************************************************* */