In progress - updating dogleg computations incrementally

gtsam/linear/GaussianConditional.cpp

@@ -27,42 +27,6 @@ using namespace std;
 
 namespace gtsam {
 
-/* ************************************************************************* */
-// Helper function used only in this file - extracts vectors with variable indices
-// in the first and last iterators, and concatenates them in that order into the
-// output.
-template<class VALUES, typename ITERATOR>
-static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
-  // Find total dimensionality
-  int dim = 0;
-  for(ITERATOR j = first; j != last; ++j)
-    dim += values[*j].rows();
-
-  // Copy vectors
-  Vector ret(dim);
-  int varStart = 0;
-  for(ITERATOR j = first; j != last; ++j) {
-    ret.segment(varStart, values[*j].rows()) = values[*j];
-    varStart += values[*j].rows();
-  }
-  return ret;
-}
-
-/* ************************************************************************* */
-// Helper function used only in this file - writes to the variables in values
-// with indices iterated over by first and last, interpreting vector as the
-// concatenated vectors to write.
-template<class VECTOR, class VALUES, typename ITERATOR>
-static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
-  // Copy vectors
-  int varStart = 0;
-  for(ITERATOR j = first; j != last; ++j) {
-    values[*j] = vector.segment(varStart, values[*j].rows());
-    varStart += values[*j].rows();
-  }
-  assert(varStart == vector.rows());
-}
-
 /* ************************************************************************* */
 GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
 

gtsam/linear/VectorValues.h

@@ -402,4 +402,42 @@ namespace gtsam {
 #endif
   }
 
+  namespace internal {
+  /* ************************************************************************* */
+  // Helper function, extracts vectors with variable indices
+  // in the first and last iterators, and concatenates them in that order into the
+  // output.
+  template<class VALUES, typename ITERATOR>
+  Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
+    // Find total dimensionality
+    int dim = 0;
+    for(ITERATOR j = first; j != last; ++j)
+      dim += values[*j].rows();
+
+    // Copy vectors
+    Vector ret(dim);
+    int varStart = 0;
+    for(ITERATOR j = first; j != last; ++j) {
+      ret.segment(varStart, values[*j].rows()) = values[*j];
+      varStart += values[*j].rows();
+    }
+    return ret;
+  }
+
+  /* ************************************************************************* */
+  // Helper function, writes to the variables in values
+  // with indices iterated over by first and last, interpreting vector as the
+  // concatenated vectors to write.
+  template<class VECTOR, class VALUES, typename ITERATOR>
+  void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
+    // Copy vectors
+    int varStart = 0;
+    for(ITERATOR j = first; j != last; ++j) {
+      values[*j] = vector.segment(varStart, values[*j].rows());
+      varStart += values[*j].rows();
+    }
+    assert(varStart == vector.rows());
+  }
+  }
+
 } // \namespace gtsam

gtsam/nonlinear/ISAM2-impl.cpp

@@ -299,4 +299,39 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
   return lastBacksubVariableCount;
 }
 
+/* ************************************************************************* */
+namespace internal {
+size_t updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
+    const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, vector<bool>& updated) {
+
+
+
+  // Update the current variable
+  // Get VectorValues slice corresponding to current variables
+  Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals());
+  Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents());
+
+  // Compute R*g and S*g for this clique
+  Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
+
+  // Write into RgProd vector
+  internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end());
+}
+}
+
+/* ************************************************************************* */
+size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, std::vector<bool>& replacedKeys,
+    Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd) {
+
+  // Keep a set of flags for whether each variable has been updated.
+  vector<bool> updated(replacedKeys.size());
+
+  // Get gradient
+  VectorValues grad = *allocateVectorValues(isam);
+  gradientAtZero(isam, grad);
+
+  // Update variables
+  return internal::updateDoglegDeltas(root, replacedKeys, grad, deltaNewton, RgProd, updated);
+}
+
 }

gtsam/nonlinear/ISAM2-impl.h

@@ -126,6 +126,9 @@ struct ISAM2::Impl {
 
   static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
 
+  static size_t UpdateDoglegDeltas(const ISAM2& isam, std::vector<bool>& replacedKeys,
+      const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
+
 };
 
 }

gtsam/nonlinear/ISAM2.cpp

@@ -660,7 +660,7 @@ void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) {
 }
 
 /* ************************************************************************* */
-VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, const VectorValues& x0) {
+VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) {
   return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
 }
 
@@ -682,7 +682,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
 }
 
 /* ************************************************************************* */
-void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, VectorValues& g) {
+void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
   // Zero-out gradient
   g.setZero();
 

gtsam/nonlinear/ISAM2.h

@@ -293,9 +293,9 @@ protected:
   mutable Permuted<VectorValues> delta_;
 
   VectorValues deltaNewtonUnpermuted_;
-  mutable Permuted<VectorValues> deltaNewtonUnpermuted_;
+  mutable Permuted<VectorValues> deltaNewton_;
-  VectorValues deltaGradSearchUnpermuted_;
+  VectorValues RgProdUnpermuted_;
-  mutable Permuted<VectorValues> deltaGradSearchUnpermuted_;
+  mutable Permuted<VectorValues> RgProd_;
 
   /** Indicates whether the current delta is up-to-date, only used
    * internally - delta will always be updated if necessary when it is
@@ -527,7 +527,7 @@ int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
  * @param x0 The center about which to compute the gradient
  * @return The gradient as a VectorValues
  */
-VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree, const VectorValues& x0);
+VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0);
 
 /**
  * Compute the gradient of the energy function,
@@ -540,7 +540,7 @@ VectorValues gradient(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTr
  * @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, ISAM2Clique>& bayesTree, VectorValues& g);
+void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
 
 } /// namespace gtsam