Compiles and passes unit tests

.project

@@ -31,7 +31,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildLocation</key>
-					<value>${workspace_loc:/gtsam/build-timing}</value>
+					<value>${ProjDirPath}/build</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>

gtsam/linear/GaussianConditional.cpp

@@ -194,7 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
 
   static const bool debug = false;
   if(debug) conditional.print("Solving conditional in place");
-  Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
+  Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
   xS = conditional.get_d() - conditional.get_S() * xS;
   Vector soln = conditional.permutation().transpose() *
       conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
@@ -202,7 +202,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
     gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
     gtsam::print(soln, "full back-substitution solution: ");
   }
-  writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
+  internal::writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
 }
 
 /* ************************************************************************* */
@@ -217,7 +217,7 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
 
 /* ************************************************************************* */
 void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
-	Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
+	Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
 	// TODO: verify permutation
 	frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
 	GaussianConditional::const_iterator it;
@@ -225,14 +225,14 @@ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
 		const Index i = *it;
 		transposeMultiplyAdd(-1.0,get_S(it),frontalVec,gy[i]);
 	}
-	writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
+	internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
 }
 
 /* ************************************************************************* */
 void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
-	Vector frontalVec = extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
+	Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
 	frontalVec = emul(frontalVec, get_sigmas());
-	writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
+	internal::writeVectorValuesSlices(frontalVec, gy, beginFrontals(), endFrontals());
 }
 
 }

gtsam/linear/VectorValues.cpp

@@ -156,3 +156,19 @@ void VectorValues::operator+=(const VectorValues& c) {
 	assert(this->hasSameStructure(c));
 	this->values_ += c.values_;
 }
+
+/* ************************************************************************* */
+VectorValues& VectorValues::operator=(const Permuted<VectorValues>& rhs) {
+  if(this->size() != rhs.size())
+    throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
+  for(size_t j=0; j<this->size(); ++j) {
+    if(exists(j)) {
+      SubVector& l(this->at(j));
+      const SubVector& r(rhs[j]);
+      if(l.rows() != r.rows())
+        throw std::invalid_argument("VectorValues assignment from Permuted<VectorValues> requires pre-allocation, see documentation.");
+      l = r;
+    }
+  }
+  return *this;
+}

gtsam/linear/VectorValues.h

@@ -19,6 +19,7 @@
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/types.h>
+#include <gtsam/inference/Permutation.h>
 
 #include <boost/foreach.hpp>
 #include <boost/shared_ptr.hpp>
@@ -273,6 +274,11 @@ namespace gtsam {
      */
     void operator+=(const VectorValues& c);
 
+    /** Assignment operator from Permuted<VectorValues>, requires the dimensions
+     * of the assignee to already be properly pre-allocated.
+     */
+    VectorValues& operator=(const Permuted<VectorValues>& rhs);
+
     /// @}
 
   private:

gtsam/nonlinear/ISAM2-impl.cpp

@@ -22,8 +22,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void ISAM2::Impl::AddVariables(
-    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta,
-    Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch,
+    Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& deltaGradSearch, vector<bool>& replacedKeys,
     Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
   const bool debug = ISDEBUG("ISAM2 AddVariables");
 
@@ -48,6 +48,8 @@ void ISAM2::Impl::AddVariables(
     Index nextVar = originalnVars;
     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
       delta.permutation()[nextVar] = nextVar;
+      deltaNewton.permutation()[nextVar] = nextVar;
+      deltaGradSearch.permutation()[nextVar] = nextVar;
       ordering.insert(key_value.key, nextVar);
       if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
       ++ nextVar;
@@ -301,21 +303,47 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
 
 /* ************************************************************************* */
 namespace internal {
-size_t updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
+void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vector<bool>& replacedKeys,
-    const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, vector<bool>& updated) {
+    const VectorValues& grad, Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd, size_t& varsUpdated) {
 
+  // Check if any frontal or separator keys were recalculated, if so, we need
+  // update deltas and recurse to children, but if not, we do not need to
+  // recurse further because of the running separator property.
+  bool anyReplaced = false;
+  BOOST_FOREACH(Index j, *clique->conditional()) {
+    if(replacedKeys[j]) {
+      anyReplaced = true;
+      break;
+    }
+  }
 
+  if(anyReplaced) {
+    // 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());
 
-  // Update the current variable
+    // Compute R*g and S*g for this clique
-  // Get VectorValues slice corresponding to current variables
+    Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
-  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
+    // Write into RgProd vector
-  Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS;
+    internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals());
 
-  // Write into RgProd vector
+    // Now solve the part of the Newton's method point for this clique (back-substitution)
-  internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->begin(), (*clique)->end());
+    (*clique)->solveInPlace(deltaNewton);
+
+    // If debugging, set recalculated keys to false so we can check them later
+#ifndef NDEBUG
+    BOOST_FOREACH(Index j, (*clique)->frontals()) {
+      replacedKeys[j] = false; }
+#endif
+
+    varsUpdated += (*clique)->nrFrontals();
+
+    // Recurse to children
+    BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
+      updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); }
+  }
 }
 }
 
@@ -323,15 +351,23 @@ size_t updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vec
 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);
+  size_t varsUpdated = 0;
+  internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated);
+
+  // Make sure all were updated
+#ifndef NDEBUG
+  for(size_t j=0; j<replacedKeys.size(); ++j)
+    assert(!replacedKeys[j]);
+#else
+  replacedKeys.assign(replacedKeys.size(), false);
+#endif
+
+  return varsUpdated;
 }
 
 }

gtsam/nonlinear/ISAM2-impl.h

@@ -127,7 +127,7 @@ 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);
+      Permuted<VectorValues>& deltaNewton, Permuted<VectorValues>& RgProd);
 
 };
 

gtsam/nonlinear/ISAM2.cpp

@@ -38,7 +38,8 @@ static const double batchThreshold = 0.65;
 
 /* ************************************************************************* */
 ISAM2::ISAM2(const ISAM2Params& params):
-    delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) {
+    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
+    deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@@ -46,7 +47,8 @@ ISAM2::ISAM2(const ISAM2Params& params):
 
 /* ************************************************************************* */
 ISAM2::ISAM2():
-    delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) {
+    delta_(deltaUnpermuted_), deltaNewton_(deltaNewtonUnpermuted_), RgProd_(RgProdUnpermuted_),
+    deltaDoglegUptodate_(true), deltaUptodate_(true) {
   // See note in gtsam/base/boost_variant_with_workaround.h
   if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
     doglegDelta_ = boost::get<ISAM2DoglegParams>(params_.optimizationParams).initialDelta;
@@ -344,6 +346,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     toc(3,"permute global variable index");
     tic(4,"permute delta");
     delta_.permute(partialSolveResult.fullReordering);
+    deltaNewton_.permute(partialSolveResult.fullReordering);
+    RgProd_.permute(partialSolveResult.fullReordering);
     toc(4,"permute delta");
     tic(5,"permute ordering");
     ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
@@ -441,7 +445,7 @@ ISAM2Result ISAM2::update(
 
   tic(2,"add new variables");
   // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
-  Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_);
+  Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_);
   toc(2,"add new variables");
 
   tic(3,"evaluate error before");
@@ -543,6 +547,7 @@ ISAM2Result ISAM2::update(
   toc(10,"evaluate error after");
 
   result.cliques = this->nodes().size();
+  deltaDoglegUptodate_ = false;
   deltaUptodate_ = false;
 
   return result;
@@ -575,9 +580,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
     doglegDelta_ = doglegResult.Delta;
     delta_.permutation() = Permutation::Identity(delta_.size());  // Dogleg solves for the full delta so there is no permutation
     delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution
-
-    // Clear replaced mask
-    deltaReplacedMask_.assign(deltaReplacedMask_.size(), false);
   }
 
   deltaUptodate_ = true;
@@ -618,9 +620,17 @@ VectorValues optimize(const ISAM2& isam) {
 
 /* ************************************************************************* */
 void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
-  tic(1, "optimizeInPlace");
+  // We may need to update the solution calcaulations
-  internal::optimizeInPlace<ISAM2::Base>(isam.root(), delta);
+  if(!isam.deltaDoglegUptodate_) {
-  toc(1, "optimizeInPlace");
+    tic(1, "UpdateDoglegDeltas");
+    ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
+    isam.deltaDoglegUptodate_ = true;
+    toc(1, "UpdateDoglegDeltas");
+  }
+
+  tic(2, "copy delta");
+  delta = isam.deltaNewton_;
+  toc(2, "copy delta");
 }
 
 /* ************************************************************************* */
@@ -635,27 +645,30 @@ VectorValues optimizeGradientSearch(const ISAM2& isam) {
 }
 
 /* ************************************************************************* */
-void optimizeGradientSearchInPlace(const ISAM2& Rd, VectorValues& grad) {
+void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
+  // We may need to update the solution calcaulations
+  if(!isam.deltaDoglegUptodate_) {
+    tic(1, "UpdateDoglegDeltas");
+    ISAM2::Impl::UpdateDoglegDeltas(isam, isam.deltaReplacedMask_, isam.deltaNewton_, isam.RgProd_);
+    isam.deltaDoglegUptodate_ = true;
+    toc(1, "UpdateDoglegDeltas");
+  }
+
   tic(1, "Compute Gradient");
   // Compute gradient (call gradientAtZero function, which is defined for various linear systems)
-  gradientAtZero(Rd, grad);
+  gradientAtZero(isam, grad);
   double gradientSqNorm = grad.dot(grad);
   toc(1, "Compute Gradient");
 
-  tic(2, "Compute R*g");
+  tic(2, "Compute minimizing step size");
-  // 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);
+  double RgNormSq = isam.RgProd_.container().vector().squaredNorm();
-  toc(3, "Compute minimizing step size");
+  double step = -gradientSqNorm / RgNormSq;
+  toc(2, "Compute minimizing step size");
 
   tic(4, "Compute point");
   // Compute steepest descent point
-  scal(step, grad);
+  grad.vector() *= step;
   toc(4, "Compute point");
 }
 

gtsam/nonlinear/ISAM2.h

@@ -296,6 +296,7 @@ protected:
   mutable Permuted<VectorValues> deltaNewton_;
   VectorValues RgProdUnpermuted_;
   mutable Permuted<VectorValues> RgProd_;
+  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
@@ -359,6 +360,11 @@ public:
     newISAM2->variableIndex_ = variableIndex_;
     newISAM2->deltaUnpermuted_ = deltaUnpermuted_;
     newISAM2->delta_ = delta_;
+    newISAM2->deltaNewtonUnpermuted_ = deltaNewtonUnpermuted_;
+    newISAM2->deltaNewton_ = deltaNewton_;
+    newISAM2->RgProdUnpermuted_ = RgProdUnpermuted_;
+    newISAM2->RgProd_ = RgProd_;
+    newISAM2->deltaDoglegUptodate_ = deltaDoglegUptodate_;
     newISAM2->deltaUptodate_ = deltaUptodate_;
     newISAM2->deltaReplacedMask_ = deltaReplacedMask_;
     newISAM2->nonlinearFactors_ = nonlinearFactors_;
@@ -459,6 +465,9 @@ private:
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;
 
+  friend void optimizeInPlace(const ISAM2&, VectorValues&);
+  friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&);
+
 }; // ISAM2
 
 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */

tests/testGaussianISAM2.cpp

@@ -29,7 +29,7 @@ using boost::shared_ptr;
 const double tol = 1e-4;
 
 /* ************************************************************************* */
-TEST(ISAM2, AddVariables) {
+TEST_UNSAFE(ISAM2, AddVariables) {
 
   // Create initial state
   Values theta;
@@ -48,6 +48,26 @@ TEST(ISAM2, AddVariables) {
 
   Permuted<VectorValues> delta(permutation, deltaUnpermuted);
 
+  VectorValues deltaNewtonUnpermuted;
+  deltaNewtonUnpermuted.insert(0, Vector_(3, .1, .2, .3));
+  deltaNewtonUnpermuted.insert(1, Vector_(2, .4, .5));
+
+  Permutation permutationNewton(2);
+  permutationNewton[0] = 1;
+  permutationNewton[1] = 0;
+
+  Permuted<VectorValues> deltaNewton(permutationNewton, deltaNewtonUnpermuted);
+
+  VectorValues deltaRgUnpermuted;
+  deltaRgUnpermuted.insert(0, Vector_(3, .1, .2, .3));
+  deltaRgUnpermuted.insert(1, Vector_(2, .4, .5));
+
+  Permutation permutationRg(2);
+  permutationRg[0] = 1;
+  permutationRg[1] = 0;
+
+  Permuted<VectorValues> deltaRg(permutationRg, deltaRgUnpermuted);
+
   vector<bool> replacedKeys(2, false);
 
   Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
@@ -78,6 +98,30 @@ TEST(ISAM2, AddVariables) {
 
   Permuted<VectorValues> deltaExpected(permutationExpected, deltaUnpermutedExpected);
 
+  VectorValues deltaNewtonUnpermutedExpected;
+  deltaNewtonUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
+  deltaNewtonUnpermutedExpected.insert(1, Vector_(2, .4, .5));
+  deltaNewtonUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
+
+  Permutation permutationNewtonExpected(3);
+  permutationNewtonExpected[0] = 1;
+  permutationNewtonExpected[1] = 0;
+  permutationNewtonExpected[2] = 2;
+
+  Permuted<VectorValues> deltaNewtonExpected(permutationNewtonExpected, deltaNewtonUnpermutedExpected);
+
+  VectorValues deltaRgUnpermutedExpected;
+  deltaRgUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3));
+  deltaRgUnpermutedExpected.insert(1, Vector_(2, .4, .5));
+  deltaRgUnpermutedExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
+
+  Permutation permutationRgExpected(3);
+  permutationRgExpected[0] = 1;
+  permutationRgExpected[1] = 0;
+  permutationRgExpected[2] = 2;
+
+  Permuted<VectorValues> deltaRgExpected(permutationRgExpected, deltaRgUnpermutedExpected);
+
   vector<bool> replacedKeysExpected(3, false);
 
   Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
@@ -86,11 +130,15 @@ TEST(ISAM2, AddVariables) {
           3, ISAM2::sharedClique());
 
   // Expand initial state
-  ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
+  ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes);
 
   EXPECT(assert_equal(thetaExpected, theta));
   EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
   EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation()));
+  EXPECT(assert_equal(deltaNewtonUnpermutedExpected, deltaNewtonUnpermuted));
+  EXPECT(assert_equal(deltaNewtonExpected.permutation(), deltaNewton.permutation()));
+  EXPECT(assert_equal(deltaRgUnpermutedExpected, deltaRgUnpermuted));
+  EXPECT(assert_equal(deltaRgExpected.permutation(), deltaRg.permutation()));
   EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys));
   EXPECT(assert_equal(orderingExpected, ordering));
 }