Consistent 'optimize', 'optimizeInPlace', 'optimizeGradientSearch', and 'optimizeGradientSearchInPlace' functions for GBN, GBT, and ISAM2. Reorganized some existing ones and added some new ones to do this.

gtsam/linear/GaussianBayesNet.cpp

@@ -122,6 +122,42 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
 	return gy;
 }
 
+/* ************************************************************************* */
+VectorValues optimizeGradientSearch(const GaussianBayesNet& Rd) {
+  tic(0, "Allocate VectorValues");
+  VectorValues grad = *allocateVectorValues(Rd);
+  toc(0, "Allocate VectorValues");
+
+  optimizeGradientSearchInPlace(Rd, grad);
+
+  return grad;
+}
+
+/* ************************************************************************* */
+void optimizeGradientSearchInPlace(const GaussianBayesNet& Rd, VectorValues& grad) {
+  tic(1, "Compute Gradient");
+  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
+  gradientAtZero(Rd, grad);
+  double gradientSqNorm = grad.dot(grad);
+  toc(1, "Compute Gradient");
+
+  tic(2, "Compute R*g");
+  // Compute R * g
+  FactorGraph<JacobianFactor> Rd_jfg(Rd);
+  Errors Rg = Rd_jfg * grad;
+  toc(2, "Compute R*g");
+
+  tic(3, "Compute minimizing step size");
+  // Compute minimizing step size
+  double step = -gradientSqNorm / dot(Rg, Rg);
+  toc(3, "Compute minimizing step size");
+
+  tic(4, "Compute point");
+  // Compute steepest descent point
+  scal(step, grad);
+  toc(4, "Compute point");
+}
+
 /* ************************************************************************* */  
 pair<Matrix,Vector> matrix(const GaussianBayesNet& bn)  {
 

gtsam/linear/GaussianBayesNet.h

@@ -69,6 +69,43 @@ namespace gtsam {
    */
   void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x);
 
+  /**
+   * 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]
+   *
+   * @param bn The GaussianBayesNet on which to perform this computation
+   * @return The resulting \f$ \delta x \f$ as described above
+   */
+  VectorValues optimizeGradientSearch(const GaussianBayesNet& bn);
+
+  /** In-place version of optimizeGradientSearch(const GaussianBayesNet&) requiring pre-allocated VectorValues \c grad
+   *
+   * @param bn The GaussianBayesNet on which to perform this computation
+   * @param [out] grad The resulting \f$ \delta x \f$ as described in optimizeGradientSearch(const GaussianBayesNet&)
+   * */
+  void optimizeGradientSearchInPlace(const GaussianBayesNet& bn, VectorValues& grad);
+
 	/**
 	 * Transpose Backsubstitute
 	 * gy=inv(L)*gx by solving L*gy=gx.

gtsam/linear/GaussianBayesTree-inl.h

@@ -25,31 +25,4 @@
 
 namespace gtsam {
 
-/* ************************************************************************* */
-namespace internal {
-template<class CLIQUE>
-inline static void optimizeInPlace(const boost::shared_ptr<CLIQUE>& clique, VectorValues& result) {
-  // parents are assumed to already be solved and available in result
-  clique->conditional()->solveInPlace(result);
-
-  // starting from the root, call optimize on each conditional
-  BOOST_FOREACH(const boost::shared_ptr<CLIQUE>& child, clique->children_)
-    optimizeInPlace(child, result);
-}
-}
-
-/* ************************************************************************* */
-template<class CLIQUE>
-VectorValues optimize(const BayesTree<GaussianConditional, CLIQUE>& bayesTree) {
-  VectorValues result = *allocateVectorValues(bayesTree);
-  internal::optimizeInPlace(bayesTree.root(), result);
-  return result;
-}
-
-/* ************************************************************************* */
-template<class CLIQUE>
-void optimizeInPlace(const BayesTree<GaussianConditional, CLIQUE>& bayesTree, VectorValues& result) {
-  internal::optimizeInPlace(bayesTree.root(), result);
-}
-
 }

gtsam/linear/GaussianBayesTree.cpp

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GaussianBayesTree.cpp
+ * @brief   Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
+ * @brief   GaussianBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/linear/GaussianBayesTree.h>
+#include <gtsam/linear/JacobianFactor.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+namespace internal {
+void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result) {
+  // parents are assumed to already be solved and available in result
+  clique->conditional()->solveInPlace(result);
+
+  // starting from the root, call optimize on each conditional
+  BOOST_FOREACH(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& child, clique->children_)
+    optimizeInPlace(child, result);
+}
+}
+
+/* ************************************************************************* */
+VectorValues optimize(const GaussianBayesTree& bayesTree) {
+  VectorValues result = *allocateVectorValues(bayesTree);
+  internal::optimizeInPlace(bayesTree.root(), result);
+  return result;
+}
+
+/* ************************************************************************* */
+VectorValues optimizeGradientSearch(const GaussianBayesTree& Rd) {
+  tic(0, "Allocate VectorValues");
+  VectorValues grad = *allocateVectorValues(Rd);
+  toc(0, "Allocate VectorValues");
+
+  optimizeGradientSearchInPlace(Rd, grad);
+
+  return grad;
+}
+
+/* ************************************************************************* */
+void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& grad) {
+  tic(1, "Compute Gradient");
+  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
+  gradientAtZero(Rd, grad);
+  double gradientSqNorm = grad.dot(grad);
+  toc(1, "Compute Gradient");
+
+  tic(2, "Compute R*g");
+  // Compute R * g
+  FactorGraph<JacobianFactor> Rd_jfg(Rd);
+  Errors Rg = Rd_jfg * grad;
+  toc(2, "Compute R*g");
+
+  tic(3, "Compute minimizing step size");
+  // Compute minimizing step size
+  double step = -gradientSqNorm / dot(Rg, Rg);
+  toc(3, "Compute minimizing step size");
+
+  tic(4, "Compute point");
+  // Compute steepest descent point
+  scal(step, grad);
+  toc(4, "Compute point");
+}
+
+/* ************************************************************************* */
+void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
+  internal::optimizeInPlace(bayesTree.root(), result);
+}
+
+/* ************************************************************************* */
+VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) {
+  return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
+}
+
+/* ************************************************************************* */
+void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) {
+  gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
+}
+
+}
+
+
+
+

gtsam/linear/GaussianBayesTree.h

@@ -21,18 +21,73 @@
 
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianFactor.h>
 
 namespace gtsam {
 
 typedef BayesTree<GaussianConditional> GaussianBayesTree;
 
 /// optimize the BayesTree, starting from the root
-template<class CLIQUE>
+VectorValues optimize(const GaussianBayesTree& bayesTree);
-VectorValues optimize(const BayesTree<GaussianConditional, CLIQUE>& bayesTree);
 
 /// recursively optimize this conditional and all subtrees
-template<class CLIQUE>
+void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result);
-void optimizeInPlace(const BayesTree<GaussianConditional, CLIQUE>& clique, VectorValues& result);
+
+namespace internal {
+void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result);
+}
+
+/**
+ * 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]
+ */
+VectorValues optimizeGradientSearch(const GaussianBayesTree& bn);
+
+/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
+void optimizeGradientSearchInPlace(const GaussianBayesTree& bn, VectorValues& grad);
+
+/**
+ * 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$.
+ * @param bayesTree The Gaussian Bayes Tree $(R,d)$
+ * @param x0 The center about which to compute the gradient
+ * @return The gradient as a VectorValues
+ */
+VectorValues gradient(const GaussianBayesTree& 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&).
+ * @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
+ */
+void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g);
 
 }
 

gtsam/linear/GaussianConditional.h

@@ -43,7 +43,7 @@ class JacobianFactor;
 /**
  * A conditional Gaussian functions as the node in a Bayes network
  * It has a set of parents y,z, etc. and implements a probability density on x.
- * The negative log-probability is given by \f$ |Rx - (d - Sy - Tz - ...)|^2 \f$
+ * The negative log-probability is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$
  */
 class GaussianConditional : public IndexConditional {
 

gtsam/nonlinear/DoglegOptimizerImpl.h

@@ -115,29 +115,6 @@ struct DoglegOptimizerImpl {
    */
   static VectorValues ComputeDoglegPoint(double Delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false);
 
-  /** Compute the minimizer \f$ \delta x_u \f$ of the line search along the gradient direction \f$ g \f$ of
-   * the function
-   * \f[
-   * M(\delta x) = (R \delta x - d)^T (R \delta x - d)
-   * \f]
-   * where \f$ R \f$ is an upper-triangular matrix and \f$ d \f$ is a vector.
-   * Together \f$ (R,d) \f$ are either a Bayes' net or a Bayes' tree.
-   *
-   * The same quadratic error function written as a Taylor expansion of the original
-   * non-linear error function is
-   * \f[
-   * M(\delta x) = f(x_0) + g(x_0) + \frac{1}{2} \delta x^T G(x_0) \delta x,
-   * \f]
-   * @tparam M The type of the Bayes' net or tree, currently
-   * either BayesNet<GaussianConditional> (or GaussianBayesNet) or BayesTree<GaussianConditional>.
-   * @param Rd The Bayes' net or tree \f$ (R,d) \f$ as described above, currently
-   * this must be of type BayesNet<GaussianConditional> (or GaussianBayesNet) or
-   * BayesTree<GaussianConditional>.
-   * @return The minimizer \f$ \delta x_u \f$ along the gradient descent direction.
-   */
-  template<class M>
-  static VectorValues ComputeSteepestDescentPoint(const M& Rd);
-
   /** Compute the point on the line between the steepest descent point and the
    * Newton's method point intersecting the trust region boundary.
    * Mathematically, computes \f$ \tau \f$ such that \f$ 0<\tau<1 \f$ and
@@ -159,7 +136,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
 
   // Compute steepest descent and Newton's method points
   tic(0, "Steepest Descent");
-  VectorValues dx_u = ComputeSteepestDescentPoint(Rd);
+  VectorValues dx_u = optimizeGradientSearch(Rd);
   toc(0, "Steepest Descent");
   tic(1, "optimize");
   VectorValues dx_n = optimize(Rd);
@@ -274,33 +251,4 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
   return result;
 }
 
-/* ************************************************************************* */
-template<class M>
-VectorValues DoglegOptimizerImpl::ComputeSteepestDescentPoint(const M& Rd) {
-
-  tic(0, "Compute Gradient");
-  // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
-  VectorValues grad = *allocateVectorValues(Rd);
-  gradientAtZero(Rd, grad);
-  double gradientSqNorm = grad.dot(grad);
-  toc(0, "Compute Gradient");
-
-  tic(1, "Compute R*g");
-  // Compute R * g
-  FactorGraph<JacobianFactor> Rd_jfg(Rd);
-  Errors Rg = Rd_jfg * grad;
-  toc(1, "Compute R*g");
-
-  tic(2, "Compute minimizing step size");
-  // Compute minimizing step size
-  double step = -gradientSqNorm / dot(Rg, Rg);
-  toc(2, "Compute minimizing step size");
-
-  tic(3, "Compute point");
-  // Compute steepest descent point
-  scal(step, grad);
-  toc(3, "Compute point");
-  return grad;
-}
-
 }

gtsam/nonlinear/GaussianISAM2-inl.h

@@ -114,6 +114,44 @@ namespace gtsam {
     return count;
   }
 
+  /* ************************************************************************* */
+  template<class GRAPH>
+  VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam) {
+    tic(0, "Allocate VectorValues");
+    VectorValues grad = *allocateVectorValues(isam);
+    toc(0, "Allocate VectorValues");
+
+    optimizeGradientSearchInPlace(isam, grad);
+
+    return grad;
+  }
+
+  /* ************************************************************************* */
+  template<class GRAPH>
+  void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& Rd, VectorValues& grad) {
+    tic(1, "Compute Gradient");
+    // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
+    gradientAtZero(Rd, grad);
+    double gradientSqNorm = grad.dot(grad);
+    toc(1, "Compute Gradient");
+
+    tic(2, "Compute R*g");
+    // Compute R * g
+    FactorGraph<JacobianFactor> Rd_jfg(Rd);
+    Errors Rg = Rd_jfg * grad;
+    toc(2, "Compute R*g");
+
+    tic(3, "Compute minimizing step size");
+    // Compute minimizing step size
+    double step = -gradientSqNorm / dot(Rg, Rg);
+    toc(3, "Compute minimizing step size");
+
+    tic(4, "Compute point");
+    // Compute steepest descent point
+    scal(step, grad);
+    toc(4, "Compute point");
+  }
+
   /* ************************************************************************* */
   template<class CLIQUE>
   void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {

gtsam/nonlinear/GaussianISAM2.cpp

@@ -26,16 +26,6 @@ using namespace gtsam;
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  VectorValues gradient(const BayesTree<GaussianConditional>& bayesTree, const VectorValues& x0) {
-    return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
-  }
-
-  /* ************************************************************************* */
-  void gradientAtZero(const BayesTree<GaussianConditional>& bayesTree, VectorValues& g) {
-    gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
-  }
-
   /* ************************************************************************* */
   VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique<GaussianConditional> >& bayesTree, const VectorValues& x0) {
     return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);

gtsam/nonlinear/GaussianISAM2.h

@@ -63,44 +63,65 @@ public:
 
 };
 
-/// optimize the BayesTree, starting from the root; "replaced" needs to contain
+/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
+template<class GRAPH>
+VectorValues optimize(const ISAM2<GaussianConditional, GRAPH>& isam) {
+  VectorValues delta = *allocateVectorValues(isam);
+  internal::optimizeInPlace(isam.root(), delta);
+  return 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
-/// redone; "delta" is the current solution, an offset from the linearization
+/// redone.
-/// point; "threshold" is the maximum change against the PREVIOUS delta for
+/// @param delta The current solution, an offset from the linearization
+/// point.
+/// @param threshold The maximum change against the PREVIOUS delta for
 /// non-replaced variables that can be ignored, ie. the old delta entry is kept
 /// and recursive backsubstitution might eventually stop if none of the changed
 /// variables are contained in the subtree.
-/// returns the number of variables that were solved for
+/// @return The number of variables that were solved for
 template<class CLIQUE>
 int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
     double threshold, const std::vector<bool>& replaced, Permuted<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]
+ */
+template<class GRAPH>
+VectorValues optimizeGradientSearch(const ISAM2<GaussianConditional, GRAPH>& isam);
+
+/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
+template<class GRAPH>
+void optimizeGradientSearchInPlace(const ISAM2<GaussianConditional, GRAPH>& 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$.
- * @param bayesTree The Gaussian Bayes Tree $(R,d)$
- * @param x0 The center about which to compute the gradient
- * @return The gradient as a VectorValues
- */
-VectorValues gradient(const BayesTree<GaussianConditional>& 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&).
- * @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
- */
-void gradientAtZero(const BayesTree<GaussianConditional>& bayesTree, VectorValues& g);
-
 /**
  * Compute the gradient of the energy function,
  * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 \f$,

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -361,6 +361,18 @@ ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
   return result;
 }
 
+/* ************************************************************************* */
+namespace internal {
+inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& clique, VectorValues& result) {
+  // parents are assumed to already be solved and available in result
+  clique->conditional()->solveInPlace(result);
+
+  // starting from the root, call optimize on each conditional
+  BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique<GaussianConditional> >& child, clique->children_)
+    optimizeInPlace(child, result);
+}
+}
+
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold) {

gtsam/nonlinear/ISAM2-inl.h

@@ -557,7 +557,6 @@ ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
   return result;
 }
 
-
 /* ************************************************************************* */
 template<class CONDITIONAL, class GRAPH>
 void ISAM2<CONDITIONAL, GRAPH>::updateDelta(bool forceFullSolve) const {
@@ -630,13 +629,5 @@ const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
   return delta_;
 }
 
-/* ************************************************************************* */
-template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
-  VectorValues delta = *allocateVectorValues(isam);
-  internal::optimizeInPlace(isam.root(), delta);
-  return delta;
-}
-
 }
 /// namespace gtsam

gtsam/nonlinear/ISAM2.h

@@ -462,10 +462,6 @@ private:
 
 }; // ISAM2
 
-/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
-template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
-
 } /// namespace gtsam
 
 #include <gtsam/nonlinear/ISAM2-inl.h>

tests/testDoglegOptimizer.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/slam/smallExample.h>
@@ -102,7 +103,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
   VectorValues expected = gradientValues;  scal(step, expected);
 
   // Compute the steepest descent point with the dogleg function
-  VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn);
+  VectorValues actual = optimizeGradientSearch(gbn);
 
   // Check that points agree
   EXPECT(assert_equal(expected, actual, 1e-5));
@@ -290,7 +291,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
   expectedFromBN[4] = Vector_(2, 0.300134, 0.423233);
 
   // Compute the steepest descent point with the dogleg function
-  VectorValues actual = DoglegOptimizerImpl::ComputeSteepestDescentPoint(bt);
+  VectorValues actual = optimizeGradientSearch(bt);
 
   // Check that points agree
   EXPECT(assert_equal(expected, actual, 1e-5));
@@ -324,7 +325,7 @@ TEST(DoglegOptimizer, ComputeBlend) {
       4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
 
   // Compute steepest descent point
-  VectorValues xu = DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn);
+  VectorValues xu = optimizeGradientSearch(gbn);
 
   // Compute Newton's method point
   VectorValues xn = optimize(gbn);
@@ -362,18 +363,18 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
   // Compute dogleg point for different deltas
 
   double Delta1 = 0.5;  // Less than steepest descent
-  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
   DOUBLES_EQUAL(Delta1, actual1.vector().norm(), 1e-5);
 
   double Delta2 = 1.5;  // Between steepest descent and Newton's method
-  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
-  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
   DOUBLES_EQUAL(Delta2, actual2.vector().norm(), 1e-5);
   EXPECT(assert_equal(expected2, actual2));
 
   double Delta3 = 5.0;  // Larger than Newton's method point
   VectorValues expected3 = optimize(gbn);
-  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, DoglegOptimizerImpl::ComputeSteepestDescentPoint(gbn), optimize(gbn));
+  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn));
   EXPECT(assert_equal(expected3, actual3));
 }