Removed namespace-functions for ISAM2 optimize* and gradient*, these were only there to try to maintain a consistent interface for linear but really were not correct anyway since ISAM2 is a nonlinear object. They instead caused confusion and made the code complicated since they used a complicated system for updating and caching various components that go into calculating the solution. Replaced all this with much simpler code directly inside updateDelta, which uses clearly-defined functions in the ISAM2::Impl class to calculate the components that go into the solution. Also removed the redundant deltaUpToDate flags - now just checks whether deltaReplacedMask is empty.

gtsam/nonlinear/ISAM2-impl.cpp

@@ -271,7 +271,8 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
 }
 
 /* ************************************************************************* */
-size_t ISAM2::Impl::UpdateDelta(const FastVector<ISAM2::sharedClique>& roots, FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
+size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
+    const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
 
   size_t lastBacksubVariableCount;
 
@@ -294,15 +295,12 @@ size_t ISAM2::Impl::UpdateDelta(const FastVector<ISAM2::sharedClique>& roots, Fa
 #endif
   }
 
-  // Clear replacedKeys
-  replacedKeys.clear();
-
   return lastBacksubVariableCount;
 }
 
 /* ************************************************************************* */
 namespace internal {
-void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys,
+void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys,
     const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) {
 
   // Check if any frontal or separator keys were recalculated, if so, we need
@@ -340,30 +338,37 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const Fast
 
     // Recurse to children
     BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
-      updateDoglegDeltas(child, replacedKeys, grad, RgProd, varsUpdated); }
+      updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); }
   }
 }
 }
 
 /* ************************************************************************* */
-size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet<Key>& replacedKeys,
+size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
-    VectorValues& deltaNewton, VectorValues& RgProd) {
+    const VectorValues& gradAtZero, VectorValues& RgProd) {
-
-  // Get gradient
-  VectorValues grad;
-  gradientAtZero(isam, grad);
 
   // Update variables
   size_t varsUpdated = 0;
-  BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots())
+  BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
-  {
+    internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated);
-    internal::updateDoglegDeltas(root, replacedKeys, grad, RgProd, varsUpdated);
-    optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, deltaNewton);
   }
 
-  replacedKeys.clear();
-
   return varsUpdated;
 }
   
+/* ************************************************************************* */
+VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero,
+                                     const VectorValues& RgProd)
+{
+  // Compute gradient squared-magnitude
+  const double gradientSqNorm = gradAtZero.dot(gradAtZero);
+  
+  // Compute minimizing step size
+  double RgNormSq = RgProd.vector().squaredNorm();
+  double step = -gradientSqNorm / RgNormSq;
+
+  // Compute steepest descent point
+  return step * gradAtZero;
+}
+
 }

gtsam/nonlinear/ISAM2-impl.h

@@ -123,11 +123,24 @@ struct GTSAM_EXPORT ISAM2::Impl {
       boost::optional<VectorValues&> invalidateIfDebug = boost::none,
       const KeyFormatter& keyFormatter = DefaultKeyFormatter);
 
-  static size_t UpdateDelta(const FastVector<ISAM2::sharedClique>& roots,
+  /**
-    FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
+   * Update the Newton's method step point, using wildfire
+   */
+  static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
+      const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
 
-  static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet<Key>& replacedKeys,
+  /**
-      VectorValues& deltaNewton, VectorValues& RgProd);
+   * Update the RgProd (R*g) incrementally taking into account which variables
+   * have been recalculated in \c replacedKeys.  Only used in Dogleg.
+   */
+  static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
+      const VectorValues& gradAtZero, VectorValues& RgProd);
+  
+  /**
+   * Compute the gradient-search point.  Only used in Dogleg.
+   */
+  static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
+                                            const VectorValues& RgProd);
 
 };
 

gtsam/nonlinear/ISAM2.cpp

@@ -151,15 +151,13 @@ void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) con
 }
 
 /* ************************************************************************* */
-ISAM2::ISAM2(const ISAM2Params& params):
+ISAM2::ISAM2(const ISAM2Params& params): params_(params) {
-    deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
 }
 
 /* ************************************************************************* */
-ISAM2::ISAM2():
+ISAM2::ISAM2() {
-    deltaDoglegUptodate_(true), deltaUptodate_(true) {
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
 }
@@ -745,8 +743,6 @@ ISAM2Result ISAM2::update(
     gttoc(remove_variables);
   }
   result.cliques = this->nodes().size();
-  deltaDoglegUptodate_ = false;
-  deltaUptodate_ = false;
 
   gttic(evaluate_error_after);
   if(params_.evaluateNonlinearError)
@@ -975,20 +971,36 @@ void ISAM2::updateDelta(bool forceFullSolve) const
         boost::get<ISAM2GaussNewtonParams>(params_.optimizationParams);
     const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold;
     gttic(Wildfire_update);
-    lastBacksubVariableCount = Impl::UpdateDelta(roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold);
+    lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(
+        roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold);
+    deltaReplacedMask_.clear();
     gttoc(Wildfire_update);
 
   } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) {
     // If using Dogleg, do a Dogleg step
     const ISAM2DoglegParams& doglegParams =
         boost::get<ISAM2DoglegParams>(params_.optimizationParams);
+    const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold;
 
     // Do one Dogleg iteration
     gttic(Dogleg_Iterate);
-    VectorValues dx_u = gtsam::optimizeGradientSearch(*this);
+
-    VectorValues dx_n = gtsam::optimize(*this);
+    // Compute Newton's method step
+    gttic(Wildfire_update);
+    lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold);
+    gttoc(Wildfire_update);
+    
+    // Compute steepest descent step
+    const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient
+    Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd
+    const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point
+    
+    // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_
+    deltaReplacedMask_.clear();
+    
+    // Compute dogleg point
     DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate(
-        *doglegDelta_, doglegParams.adaptationMode, dx_u, dx_n, *this, nonlinearFactors_,
+        *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_,
         theta_, nonlinearFactors_.error(theta_), doglegParams.verbose));
     gttoc(Dogleg_Iterate);
 
@@ -998,21 +1010,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const
     delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
     gttoc(Copy_dx_d);
   }
-
-  deltaUptodate_ = true;
 }
 
 /* ************************************************************************* */
 Values ISAM2::calculateEstimate() const {
   gttic(ISAM2_calculateEstimate);
-  gttic(Copy_Values);
-  Values ret(theta_);
-  gttoc(Copy_Values);
   const VectorValues& delta(getDelta());
   gttic(Expmap);
-  ret = ret.retract(delta);
+  return theta_.retract(delta);
   gttoc(Expmap);
-  return ret;
 }
 
 /* ************************************************************************* */
@@ -1023,7 +1029,8 @@ const Value& ISAM2::calculateEstimate(Key key) const {
 
 /* ************************************************************************* */
 Values ISAM2::calculateBestEstimate() const {
-  return theta_.retract(internal::linearAlgorithms::optimizeBayesTree(*this));
+  updateDelta(true); // Force full solve when updating delta_
+  return theta_.retract(delta_);
 }
 
 /* ************************************************************************* */
@@ -1033,7 +1040,7 @@ Matrix ISAM2::marginalCovariance(Key key) const {
 
 /* ************************************************************************* */
 const VectorValues& ISAM2::getDelta() const {
-  if(!deltaUptodate_)
+  if(!deltaReplacedMask_.empty())
     updateDelta();
   return delta_;
 }
@@ -1044,88 +1051,6 @@ double ISAM2::error(const VectorValues& x) const
   return GaussianFactorGraph(*this).error(x);
 }
 
-/* ************************************************************************* */
-VectorValues optimize(const ISAM2& isam) {
-  VectorValues delta;
-  optimizeInPlace(isam, delta);
-  return delta;
-}
-
-/* ************************************************************************* */
-void optimizeInPlace(const ISAM2& isam, VectorValues& delta)
-{
-  gttic(ISAM2_optimizeInPlace);
-  // We may need to update the solution calculations
-  if(!isam.deltaDoglegUptodate_) {
-    gttic(UpdateDoglegDeltas);
-    double wildfireThreshold = 0.0;
-    if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
-      wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
-    else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
-      wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
-    else
-      assert(false);
-    ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
-    isam.deltaDoglegUptodate_ = true;
-    gttoc(UpdateDoglegDeltas);
-  }
-
-  gttic(copy_delta);
-  delta = isam.deltaNewton_;
-  gttoc(copy_delta);
-}
-
-/* ************************************************************************* */
-
-VectorValues optimizeGradientSearch(const ISAM2& isam) {
-  VectorValues grad;
-  optimizeGradientSearchInPlace(isam, grad);
-  return grad;
-}
-
-/* ************************************************************************* */
-void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad)
-{
-  gttic(ISAM2_optimizeGradientSearchInPlace);
-  // We may need to update the solution calcaulations
-  if(!isam.deltaDoglegUptodate_) {
-    gttic(UpdateDoglegDeltas);
-    double wildfireThreshold = 0.0;
-    if(isam.params().optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
-      wildfireThreshold = boost::get<ISAM2GaussNewtonParams>(isam.params().optimizationParams).wildfireThreshold;
-    else if(isam.params().optimizationParams.type() == typeid(ISAM2DoglegParams))
-      wildfireThreshold = boost::get<ISAM2DoglegParams>(isam.params().optimizationParams).wildfireThreshold;
-    else
-      assert(false);
-    ISAM2::Impl::UpdateDoglegDeltas(isam, wildfireThreshold, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
-    isam.deltaDoglegUptodate_ = true;
-    gttoc(UpdateDoglegDeltas);
-  }
-
-  gttic(Compute_Gradient);
-  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
-  gradientAtZero(isam, grad);
-  double gradientSqNorm = grad.dot(grad);
-  gttoc(Compute_Gradient);
-
-  gttic(Compute_minimizing_step_size);
-  // Compute minimizing step size
-  double RgNormSq = isam.RgProd_.vector().squaredNorm();
-  double step = -gradientSqNorm / RgNormSq;
-  gttoc(Compute_minimizing_step_size);
-
-  gttic(Compute_point);
-  // Compute steepest descent point
-  grad *= step;
-  gttoc(Compute_point);
-}
-
-/* ************************************************************************* */
-
-VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) {
-  return GaussianFactorGraph(bayesTree).gradient(x0);
-}
-
 /* ************************************************************************* */
 static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root, VectorValues& g) {
   // Loop through variables in each clique, adding contributions
@@ -1147,13 +1072,16 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
 }
 
 /* ************************************************************************* */
-void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
+VectorValues ISAM2::gradientAtZero() const
-  // Zero-out gradient
+{
-  g.setZero();
+  // Create result
+  VectorValues g;
   
   // Sum up contributions for each clique
-  BOOST_FOREACH(const ISAM2::sharedClique& root, bayesTree.roots())
+  BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots())
   gradientAtZeroTreeAdder(root, g);
+  
+  return g;
 }
 
 }

gtsam/nonlinear/ISAM2.h

@@ -439,28 +439,14 @@ protected:
    */
   mutable VectorValues delta_;
 
-  mutable VectorValues deltaNewton_;
+  mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update
-  mutable VectorValues RgProd_;
+  mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally
-  mutable bool deltaDoglegUptodate_;
-
-  /** Indicates whether the current delta is up-to-date, only used
-   * internally - delta will always be updated if necessary when it is
-   * requested with getDelta() or calculateEstimate().
-   *
-   * This is \c mutable because it is used internally to not update delta_
-   * until it is needed.
-   */
-  mutable bool deltaUptodate_;
 
   /** A cumulative mask for the variables that were replaced and have not yet
    * been updated in the linear solution delta_, this is only used internally,
    * delta will always be updated if necessary when requested with getDelta()
    * or calculateEstimate().
    *
-   * This does not need to be permuted because any change in variable ordering
-   * that would cause a permutation will also mark variables as needing to be
-   * updated in this mask.
-   *
    * This is \c mutable because it is used internally to not update delta_
    * until it is needed.
    */
@@ -623,6 +609,14 @@ public:
   /** prints out clique statistics */
   void printStats() const { getCliqueData().getStats().print(); }
   
+  /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d
+   * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$.  See also
+   * gradient(const GaussianBayesNet&, const VectorValues&).
+   *
+   * @return A VectorValues storing the gradient.
+   */
+  VectorValues gradientAtZero() const;
+  
   /// @}
 
 protected:
@@ -633,20 +627,10 @@ protected:
 
   virtual boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
       const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
-  //  void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;
 
-  friend GTSAM_EXPORT void optimizeInPlace(const ISAM2&, VectorValues&);
-  friend GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&);
-
 }; // ISAM2
 
-/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
-GTSAM_EXPORT VectorValues optimize(const ISAM2& isam);
-
-/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
-GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
-
 /// Optimize the BayesTree, starting from the root.
 /// @param replaced Needs to contain
 /// all variables that are contained in the top of the Bayes tree that has been
@@ -666,66 +650,10 @@ template<class CLIQUE>
 size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
     double threshold, const FastSet<Key>& replaced, VectorValues& delta);
 
-/**
- * Optimize along the gradient direction, with a closed-form computation to
- * perform the line search.  The gradient is computed about \f$ \delta x=0 \f$.
- *
- * This function returns \f$ \delta x \f$ that minimizes a reparametrized
- * problem.  The error function of a GaussianBayesNet is
- *
- * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \f]
- *
- * with gradient and Hessian
- *
- * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
- *
- * This function performs the line search in the direction of the
- * gradient evaluated at \f$ g = g(\delta x = 0) \f$ with step size
- * \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g) \f$:
- *
- * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
- *
- * Optimizing by setting the derivative to zero yields
- * \f$ \hat \alpha = (-g^T g) / (g^T G g) \f$.  For efficiency, this function
- * evaluates the denominator without computing the Hessian \f$ G \f$, returning
- *
- * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
- */
-GTSAM_EXPORT VectorValues optimizeGradientSearch(const ISAM2& isam);
-
-/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
-GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad);
-
 /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
 template<class CLIQUE>
 int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
 
-/**
- * Compute the gradient of the energy function,
- * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
- * centered around \f$ x = x_0 \f$.
- * The gradient is \f$ R^T(Rx-d) \f$.
- * This specialized version is used with ISAM2, where each clique stores its
- * gradient contribution.
- * @param bayesTree The Gaussian Bayes Tree $(R,d)$
- * @param x0 The center about which to compute the gradient
- * @return The gradient as a VectorValues
- */
-GTSAM_EXPORT VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0);
-
-/**
- * Compute the gradient of the energy function,
- * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,
- * centered around zero.
- * The gradient about zero is \f$ -R^T d \f$.  See also gradient(const GaussianBayesNet&, const VectorValues&).
- * This specialized version is used with ISAM2, where each clique stores its
- * gradient contribution.
- * @param bayesTree The Gaussian Bayes Tree $(R,d)$
- * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
- * @return The gradient as a VectorValues
- */
-GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
-
 } /// namespace gtsam
 
 #include <gtsam/nonlinear/ISAM2-inl.h>

tests/testGaussianISAM2.cpp

@@ -274,8 +274,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
   // Check gradient
   VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
   VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
-  VectorValues actualGradient;
+  VectorValues actualGradient = isam.gradientAtZero();
-  gradientAtZero(isam, actualGradient);
   bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
   EXPECT(expectedGradOk);
   bool totalGradOk = assert_equal(expectedGradient, actualGradient);
@@ -503,8 +502,7 @@ TEST(ISAM2, swapFactors)
   // Check gradient
   VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
   VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
-  VectorValues actualGradient;
+  VectorValues actualGradient = isam.gradientAtZero();
-  gradientAtZero(isam, actualGradient);
   EXPECT(assert_equal(expectedGradient2, expectedGradient));
   EXPECT(assert_equal(expectedGradient, actualGradient));
 }
@@ -629,8 +627,7 @@ TEST(ISAM2, constrained_ordering)
   // Check gradient
   VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero();
   VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient));
-  VectorValues actualGradient;
+  VectorValues actualGradient = isam.gradientAtZero();
-  gradientAtZero(isam, actualGradient);
   EXPECT(assert_equal(expectedGradient2, expectedGradient));
   EXPECT(assert_equal(expectedGradient, actualGradient));
 }