Merge pull request #2408 from DanMcGann/feature/risam-isam2-updates

iSAM2 Updates to Support riSAM Implementation

Author: dellaert

gtsam/inference/BayesTree-inst.h

@@ -573,5 +573,34 @@ namespace gtsam {
     return cliques;
   }
 
+  /* *********************************************************************** */
+  template <class CLIQUE>
+  void BayesTree<CLIQUE>::collectAffectedPathKeys(
+      gtsam::KeySet& traversedKeys, const sharedClique& clique) const {
+    // base case is nullptr, if so we do nothing and return empties above
+    if (clique) {
+      // traverse me
+      traversedKeys.insert(clique->conditional()->frontals().begin(),
+                           clique->conditional()->frontals().end());
+      // traverse path above me
+      this->collectAffectedPathKeys(traversedKeys, clique->parent_.lock());
+    }
+  }
+
+  /* *********************************************************************** */
+  template <class CLIQUE>
+  gtsam::KeySet BayesTree<CLIQUE>::collectAffectedKeys(
+      const gtsam::KeyVector& keys) const {
+    gtsam::KeySet traversedKeys;
+    // process each key of the new factor
+    for (const gtsam::Key& j : keys) {
+      typename Nodes::const_iterator node = nodes_.find(j);
+      if (node != nodes_.end()) {
+        // traverse path from clique to root
+        this->collectAffectedPathKeys(traversedKeys, node->second);
+      }
+    }
+    return traversedKeys;
+  }
 }
 /// namespace gtsam

gtsam/inference/BayesTree.h

@@ -245,7 +245,17 @@ namespace gtsam {
     /** Add all cliques in this BayesTree to the specified factor graph */
     void addFactorsToGraph(FactorGraph<FactorType>* graph) const;
 
-  protected:
+    /**
+     * @brief Returns the set of keys from the tree that are affected by a
+     * update to 'keys'
+     * @param keys: The keys updated and in-turn affecting the tree
+     * @returns The set of keys from the tree that are affected
+     * @note Note: Return matches contents of BayesNet from removeTop without
+     * affecting the tree
+     */
+    gtsam::KeySet collectAffectedKeys(const gtsam::KeyVector& keys) const;
+
+   protected:
 
     /** private helper method for saving the Tree to a text file in GraphViz format */
     void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
@@ -260,6 +270,11 @@ namespace gtsam {
     /** Fill the nodes index for a subtree */
     void fillNodesIndex(const sharedClique& subtree);
 
+    /// @brief Helper for collectAffectedKeys that recursively aggregates
+    /// affected keys from a path from 'clique' to the root of tree
+    void collectAffectedPathKeys(gtsam::KeySet& traversedKeys,
+                                 const sharedClique& clique) const;
+
     // Friend JunctionTree because it directly fills roots and nodes index.
     template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
 

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -252,4 +252,124 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
   return result;
 }
 
+/** This class contains an extension of the Dogleg Algorithm where a line search
+ * is performed across the Dogleg arc (interpolation of gradient and
+ * Gauss-Newton directions). This algorithm is applicable to cases where the
+ * trust region is not correlated between algorithm iterations.
+ */
+struct GTSAM_EXPORT DoglegLineSearchImpl {
+  struct Params {
+    double minDelta;  ///< Minimum allowed small delta
+    double maxDelta;  ///< Maximum allowed delta
+    double stepSize;  ///< Increase of trust region for outward steps
+    double sufficientDecreaseCoeff;  ///< Coefficient for suff. decrease check
+    bool verbose;                    ///< Whether to print debug information
+  };
+
+  /**
+   * Compute the update point for one iteration of the Dogleg Line Search
+   * algorithm, starting with a trust region of |N| * minDelta the algorithm
+   * searches trust regions from |N| * minDelta to maxDelta where |N| is the
+   * number of variables in the system. The algorithm returns the search point
+   * with minimum cost that meets the Wolfe Conditions. Evaluation points for
+   * the line search are computed according to a geometric series step_{k+1} =
+   * stepSize * step_k.
+   *
+   *
+   * @tparam M The type of the Bayes' net or tree, currently
+   * either BayesNet<GaussianConditional> (or GaussianBayesNet) or
+   * BayesTree<GaussianConditional>.
+   * @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>.
+   * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate
+   * the error function.
+   * @param params The parameters for dogleg line search
+   * @param Rd The Bayes' net or tree as described above.
+   * @param f The original nonlinear factor graph with which to evaluate the
+   * accuracy of \f$ M(\delta x) \f$ to adjust \f$ \delta \f$.
+   * @param x0 The linearization point about which \f$ \bayesNet \f$ was created
+   * @param verbose Flag to write debug information.
+   * @return A DoglegIterationResult containing the new \c delta, the linear
+   * update \c dx_d, and the resulting nonlinear error \c f_error.
+   */
+  template <class M, class F, class VALUES>
+  static DoglegOptimizerImpl::IterationResult Iterate(const Params& params,
+                                                      const VectorValues& dx_u,
+                                                      const VectorValues& dx_n,
+                                                      const M& Rd, const F& f,
+                                                      const VALUES& x0);
+};
+
+/* ************************************************************************* */
+template <class M, class F, class VALUES>
+typename DoglegOptimizerImpl::IterationResult DoglegLineSearchImpl::Iterate(
+    const Params& params, const VectorValues& dx_u, const VectorValues& dx_n,
+    const M& Rd, const F& f, const VALUES& x0) {
+  if (params.verbose)
+    std::cout << "DoglegLineSearch | minDelta: " << params.minDelta
+              << " maxDelta: " << params.maxDelta
+              << " stepSize: " << params.stepSize << std::endl;
+  const double fInitError = f.error(x0);
+  const double gnStep = dx_n.norm();
+  const size_t numVars = dx_n.size();
+
+  double maxStep, step;
+  /// The search bounds are scaled by the number of variables in the system
+  maxStep = std::min(params.maxDelta * numVars, gnStep);
+  step = std::min(params.minDelta * numVars, gnStep);
+  step = std::min(step, maxStep);  // Edge case min_step > maxStep
+
+  if (params.verbose)
+    std::cout << "Search Region: [ " << step << " -> " << maxStep << "]"
+              << std::endl;
+
+  DoglegOptimizerImpl::IterationResult result;
+  result.dx_d =
+      DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, params.verbose);
+  result.f_error = f.error(x0.retract(result.dx_d));
+  result.delta = step;
+  if (params.verbose)
+    std::cout << "Initial Step Error: " << result.f_error << std::endl;
+
+  // Validate Step size will terminate search
+  if (step < 1e-12 || params.stepSize < 1.0)
+    throw std::runtime_error(
+        "Invalid DoglegLineSearch configuration. Would cause infinite search.");
+
+  // Search Increase delta
+  double eps = std::numeric_limits<double>::epsilon();
+  while (step < maxStep - eps) {
+    // Compute the trust region for the evaluation point according to the
+    // Geometric Series
+    step = std::min(maxStep, step * params.stepSize);
+
+    // Compute the Evaluation Point and evaluate the error
+    VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(
+        step, dx_u, dx_n, params.verbose);
+    VALUES x_d(x0.retract(dx_d));
+    double fNewError = f.error(x_d);
+
+    if (params.verbose)
+      std::cout << "Step: " << step << " | Error: " << fNewError << std::endl;
+
+    // Check step acceptance conditions
+    bool updateDecreasedError = fNewError < result.f_error;
+    bool updateHasSufficientDecrease =
+        fNewError < (fInitError - params.sufficientDecreaseCoeff * step);
+
+    if (params.verbose)
+      std::cout << "Decrease Error: " << updateDecreasedError
+                << " | Suff. Dec.: " << updateHasSufficientDecrease
+                << std::endl;
+
+    if (updateDecreasedError && updateHasSufficientDecrease) {
+      result.f_error = fNewError;
+      result.dx_d = dx_d;
+      result.delta = step;
+
+      if (params.verbose) std::cout << "Step Accepted" << std::endl;
+    }
+  }
+
+  return result;
+}
 }

gtsam/nonlinear/ISAM2.cpp

@@ -431,7 +431,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors,
   addVariables(newTheta, result.details());
 
   // Update delta if we need it to check relinearization later
-  if (update.relinarizationNeeded(update_count_))
+  if (update.relinarizationNeeded(update_count_) && !deltaReplacedMask_.empty())
     updateDelta(updateParams.forceFullSolve);
 
   // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
@@ -756,9 +756,9 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
     const VectorValues gradAtZero = this->gradientAtZero();  // Compute gradient
     DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero,
                             &RgProd_);  // Update RgProd
-    const VectorValues dx_u = DeltaImpl::ComputeGradientSearch(
-        gradAtZero, RgProd_);  // Compute gradient search point
-
+    const VectorValues dx_u =
+        VectorValues::Zero(deltaNewton_)
+            .update(DeltaImpl::ComputeGradientSearch(gradAtZero, RgProd_));
     // Clear replaced keys mask because now we've updated deltaNewton_ and
     // RgProd_
     deltaReplacedMask_.clear();
@@ -777,6 +777,39 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
     // Copy the VectorValues containing with the linear solution
     delta_ = doglegResult.dx_d;
     gttoc(Copy_dx_d);
+  } else if (std::holds_alternative<ISAM2DoglegLineSearchParams>(
+                 params_.optimizationParams)) {
+    const ISAM2DoglegLineSearchParams& isamDllsParams =
+        std::get<ISAM2DoglegLineSearchParams>(params_.optimizationParams);
+    const double effectiveWildfireThreshold =
+        forceFullSolve ? 0.0 : isamDllsParams.getWildfireThreshold();
+
+    // Timer start
+    gttic(DoglegLineSearch_Iterate);
+
+    // Compute Newton's method step
+    gttic(Wildfire_update);
+    DeltaImpl::UpdateGaussNewtonDelta(
+        roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_);
+    gttoc(Wildfire_update);
+
+    // Compute steepest descent step
+    const VectorValues gradAtZero = this->gradientAtZero();
+    DeltaImpl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, &RgProd_);
+    const VectorValues dx_u =
+        VectorValues::Zero(deltaNewton_)
+            .update(DeltaImpl::ComputeGradientSearch(gradAtZero, RgProd_));
+    deltaReplacedMask_.clear();
+
+    // Do the DogLeg Line Search
+    DoglegOptimizerImpl::IterationResult doglegResult(
+        DoglegLineSearchImpl::Iterate(isamDllsParams.dllsParams, dx_u,
+                                      deltaNewton_, *this, nonlinearFactors_,
+                                      theta_));
+
+    // Update Delta and linear step
+    delta_ = doglegResult.dx_d;
+    gttoc(DoglegLineSearch_Iterate);
   } else {
     throw std::runtime_error("iSAM2: unknown ISAM2Params type");
   }
@@ -832,4 +865,41 @@ VectorValues ISAM2::gradientAtZero() const {
   return g;
 }
 
+/* ************************************************************************* */
+std::pair<KeySet, bool> ISAM2::predictUpdateInfo(
+    const NonlinearFactorGraph& newFactors, const Values& newTheta,
+    const ISAM2UpdateParams& updateParams) const {
+  // Temp variables required by inputs below
+  ISAM2Result result;
+  UpdateImpl update(params_, updateParams);
+
+  if (update.relinarizationNeeded(update_count_) && !deltaReplacedMask_.empty())
+    updateDelta(updateParams.forceFullSolve);
+
+  // Gather the Keys involved from update params
+  update.computeUnusedKeys(newFactors, variableIndex_,
+                           result.keysWithRemovedFactors, &result.unusedKeys);
+  update.gatherInvolvedKeys(newFactors, nonlinearFactors_,
+                            result.keysWithRemovedFactors, &result.markedKeys);
+  update.updateKeys(result.markedKeys, &result);
+
+  // Gather keys involved due to relinearization threshold
+  // Note: increment by one as we are performing a one step look ahead
+  KeySet relinKeys;
+  if (update.relinarizationNeeded(update_count_ + 1)) {
+    relinKeys = update.gatherRelinearizeKeys(roots_, delta_, fixedVariables_,
+                                             &result.markedKeys);
+    if (!relinKeys.empty())
+      update.findFluid(roots_, relinKeys, &result.markedKeys, result.details());
+  }
+  // Get the top of the bayes tree affected by all the involved keys
+  KeySet affectedKeys = collectAffectedKeys(
+      KeyVector(result.markedKeys.begin(), result.markedKeys.end()));
+  // Add the new keys to get the entire set of keys affected by the update
+  KeySet newKeys = newFactors.keys();
+  affectedKeys.insert(newKeys.begin(), newKeys.end());
+  // Return the affected keys and whether or not this will be a batch update
+  return {affectedKeys, affectedKeys.size() >= theta_.size() * 0.65};
+}
+
 }  // namespace gtsam

gtsam/nonlinear/ISAM2.h

@@ -290,6 +290,19 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
    */
   VectorValues gradientAtZero() const;
 
+  /** @brief Predicts the updated variables for a hypothetical update.
+   * @param newFactors The factors for the hypothetical update
+   * @param newTheta The estimates for new variables in the hypothetical update
+   * @param updateParams The update params for the hypothetical update
+   * @returns The set of all affected keys, and a flag indicating if this would
+   * be a batch update
+   *
+   * NOTE: Update may mutate the mutable field delta_
+   */
+  std::pair<KeySet, bool> predictUpdateInfo(
+      const NonlinearFactorGraph& newFactors, const Values& newTheta,
+      const ISAM2UpdateParams& updateParams) const;
+
   /// @}
 
  protected:

gtsam/nonlinear/ISAM2Params.h

@@ -126,6 +126,69 @@ struct GTSAM_EXPORT ISAM2DoglegParams {
       const std::string& adaptationMode) const;
 };
 
+/**
+ * @ingroup isam2
+ * Parameters for ISAM2 using Dogleg Line Search optimization.  Either this
+ * class, ISAM2GaussNewtonParams, or ISAM2DoglegParams should be specified as
+ * the optimizationParams in ISAM2Params, which should in turn be passed to
+ * ISAM2(const ISAM2Params&).
+ */
+struct GTSAM_EXPORT ISAM2DoglegLineSearchParams {
+  DoglegLineSearchImpl::Params dllsParams;  ///< Params for DoglegLineSearch
+  double wildfireThreshold;  ///< Update delta when changes are above thresh
+
+  /** Specify parameters as constructor arguments */
+  ISAM2DoglegLineSearchParams(double minDelta = 0.02, double maxDelta = 0.5,
+                              double stepSize = 1.5,
+                              double sufficientDecreaseCoeff = 1e-3,
+                              bool verbose = false,
+                              double wildfireThreshold = 1e-4)
+      : dllsParams{minDelta, maxDelta, stepSize, sufficientDecreaseCoeff,
+                   verbose},
+        wildfireThreshold(wildfireThreshold) {
+    if (minDelta < 1e-12 || maxDelta < 1e-12 || stepSize < 1.0) {
+      throw std::invalid_argument(
+          "ISAM2DoglegLineSearchParams constructed with invalid configuration. "
+          "Search Bounds [minDelta, maxDelta] ~ 0 or stepSize < 1.0");
+    }
+  }
+
+  void print(const std::string str = "") const {
+    using std::cout;
+    cout << str << "type:                      ISAM2DoglegLineSearchParams\n";
+    cout << str << "minDelta:                 " << dllsParams.minDelta << "\n";
+    cout << str << "maxDelta:                 " << dllsParams.maxDelta << "\n";
+    cout << str << "stepSize:                 " << dllsParams.stepSize << "\n";
+    cout << str
+         << "sufficientDecreaseCoeff: " << dllsParams.sufficientDecreaseCoeff
+         << "\n";
+    cout << str << "verbose:        " << dllsParams.verbose << "\n";
+    cout << str << "wildfireThreshold:        " << wildfireThreshold << "\n";
+    cout.flush();
+  }
+  /** Getters **/
+  double getMinDelta() const { return dllsParams.minDelta; }
+  double getMaxDelta() const { return dllsParams.maxDelta; }
+  double getStepSize() const { return dllsParams.stepSize; }
+  double getSufficientDecreaseCoeff() const {
+    return dllsParams.sufficientDecreaseCoeff;
+  }
+  bool isVerbose() const { return dllsParams.verbose; }
+  double getWildfireThreshold() const { return wildfireThreshold; }
+
+  /** Setters */
+  void setMinDelta(double minDelta) { this->dllsParams.minDelta = minDelta; }
+  void setMaxDelta(double maxDelta) { this->dllsParams.maxDelta = maxDelta; }
+  void setStepSize(double stepSize) { this->dllsParams.stepSize = stepSize; }
+  void setSufficientDecreaseCoeff(double sufficientDecreaseCoeff) {
+    this->dllsParams.sufficientDecreaseCoeff = sufficientDecreaseCoeff;
+  }
+  void setVerbose(bool verbose) { this->dllsParams.verbose = verbose; }
+  void setWildfireThreshold(double wildfireThreshold) {
+    this->wildfireThreshold = wildfireThreshold;
+  }
+};
+
 /**
  * @ingroup isam2
  * Parameters for the ISAM2 algorithm.  Default parameter values are listed
@@ -134,9 +197,11 @@ struct GTSAM_EXPORT ISAM2DoglegParams {
 typedef FastMap<char, Vector> ISAM2ThresholdMap;
 typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
 struct GTSAM_EXPORT ISAM2Params {
-  typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
+  typedef std::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams,
+                       ISAM2DoglegLineSearchParams>
       OptimizationParams;  ///< Either ISAM2GaussNewtonParams or
-                           ///< ISAM2DoglegParams
+                           ///< ISAM2DoglegParams or
+                           ///< ISAM2DoglegLineSearchParams
   typedef std::variant<double, FastMap<char, Vector> >
       RelinearizationThreshold;  ///< Either a constant relinearization
                                  ///< threshold or a per-variable-type set of