Cleanup in linear solving:

- No longer need to fill solution vector with rhs
 - Removed rhs functions
 - Combined/removed redundant optimize functions for GaussianConditional, GaussianBayesNet, and GaussianBayesTree
 - Renamed some to optimizeInPlace and optimizeWildfire
 - Moved BayesTree optimize functions from GaussianISAM(2) to GaussianBayesTree.

gtsam/linear/GaussianBayesNet.cpp

@@ -83,33 +83,14 @@ boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn)
 
 /* ************************************************************************* */
 VectorValues optimize(const GaussianBayesNet& bn) {
-  return *optimize_(bn);
+	VectorValues x = *allocateVectorValues(bn);
-}
+	optimizeInPlace(bn, x);
-
-/* ************************************************************************* */
-boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn)
-{
-	// get the RHS as a VectorValues to initialize system
-	boost::shared_ptr<VectorValues> result(new VectorValues(rhs(bn)));
-
-  /** solve each node in turn in topological sort order (parents first)*/
-	BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn)
-		cg->solveInPlace(*result); // solve and store solution in same step
-
-	return result;
-}
-
-/* ************************************************************************* */
-VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y) {
-	VectorValues x(y);
-	backSubstituteInPlace(bn,x);
 	return x;
 }
 
 /* ************************************************************************* */
 // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
-void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
+void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
-	VectorValues& x = y;
 	/** solve each node in turn in topological sort order (parents first)*/
 	BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
 		// i^th part of R*x=y, x=inv(R)*y
@@ -194,15 +175,6 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn)  {
   return make_pair(R,d);
 }
 
-/* ************************************************************************* */
-VectorValues rhs(const GaussianBayesNet& bn) {
-	boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
-  BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn)
-  	cg->rhs(*result);
-
-  return *result;
-}
-
 /* ************************************************************************* */
 double determinant(const GaussianBayesNet& bayesNet) {
 	double logDet = 0.0;

gtsam/linear/GaussianBayesNet.h

@@ -55,26 +55,19 @@ namespace gtsam {
 	boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn);
 
 	/**
-	 * optimize, i.e. return x = inv(R)*d
+	 * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
+	 * back-substitution.
 	 */
-	VectorValues optimize(const GaussianBayesNet&);
+	VectorValues optimize(const GaussianBayesNet& bn);
 
-	/**
+  /**
-	 * shared pointer version
+   * Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
-	 */
+   * back-substitution, writes the solution \f$ x \f$ into a pre-allocated
-	boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn);
+   * VectorValues.  You can use allocateVectorValues(const GaussianBayesNet&)
-
+   * allocate it.  See also optimize(const GaussianBayesNet&), which does not
-	/**
+   * require pre-allocation.
-	 * Backsubstitute
+   */
-	 * (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
+  void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x);
-	 * @param y is the RHS of the system
-	 */
-	VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y);
-
-	/**
-	 * Backsubstitute in place, y starts as RHS and is replaced with solution
-	 */
-	void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y);
 
 	/**
 	 * Transpose Backsubstitute
@@ -91,12 +84,6 @@ namespace gtsam {
 	 */
 	std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
 
-  /**
-   * Return RHS d as a VectorValues
-   * Such that backSubstitute(bn,d) = optimize(bn)
-   */
-  VectorValues rhs(const GaussianBayesNet&);
-
   /**
    * Computes the determinant of a GassianBayesNet
    * A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix

gtsam/linear/GaussianBayesTree-inl.h

@@ -0,0 +1,55 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+ */
+
+#pragma once
+
+#include <boost/foreach.hpp>
+
+#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
+
+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.h

@@ -0,0 +1,40 @@
+/* ----------------------------------------------------------------------------
+
+ * 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.h
+ * @brief   Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
+ * @brief   GaussianBayesTree
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/inference/BayesTree.h>
+#include <gtsam/linear/GaussianConditional.h>
+
+namespace gtsam {
+
+typedef BayesTree<GaussianConditional> GaussianBayesTree;
+
+/// optimize the BayesTree, starting from the root
+template<class CLIQUE>
+VectorValues optimize(const BayesTree<GaussianConditional, CLIQUE>& bayesTree);
+
+/// recursively optimize this conditional and all subtrees
+template<class CLIQUE>
+void optimizeInPlace(const BayesTree<GaussianConditional, CLIQUE>& clique, VectorValues& result);
+
+}
+
+#include <gtsam/linear/GaussianBayesTree-inl.h>
+

gtsam/linear/GaussianConditional.cpp

@@ -31,19 +31,19 @@ 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<typename ITERATOR>
+template<class VALUES, typename ITERATOR>
-static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
+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.dim(*j);
+    dim += values[*j].rows();
 
   // Copy vectors
   Vector ret(dim);
   int varStart = 0;
   for(ITERATOR j = first; j != last; ++j) {
-    ret.segment(varStart, values.dim(*j)) = values[*j];
+    ret.segment(varStart, values[*j].rows()) = values[*j];
-    varStart += values.dim(*j);
+    varStart += values[*j].rows();
   }
   return ret;
 }
@@ -52,13 +52,13 @@ static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR fir
 // 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, typename ITERATOR>
+template<class VECTOR, class VALUES, typename ITERATOR>
-static void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
+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.dim(*j));
+    values[*j] = vector.segment(varStart, values[*j].rows());
-    varStart += values.dim(*j);
+    varStart += values[*j].rows();
   }
   assert(varStart == vector.rows());
 }
@@ -221,78 +221,34 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
 }
 
 /* ************************************************************************* */
-void GaussianConditional::rhs(VectorValues& x) const {
+template<class VALUES>
-  writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals());
+inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) {
-}
 
-/* ************************************************************************* */
+  // Helper function to solve-in-place on a VectorValues or Permuted<VectorValues>,
-void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
+  // called by GaussianConditional::solveInPlace(VectorValues&) and by
-  // Copy the rhs into x, accounting for the permutation
+  // GaussianConditional::solveInPlace(Permuted<VectorValues>&).
-  Vector d = get_d();
+
-  size_t rhsPosition = 0;  // We walk through the rhs by variable
+  static const bool debug = false;
-  for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) {
+  if(debug) conditional.print("Solving conditional in place");
-    // Get the segment of the rhs for this variable
+  Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
-    x[*j] = d.segment(rhsPosition, this->dim(j));
+  xS = conditional.get_d() - conditional.get_S() * xS;
-    // Increment the position
+  Vector soln = conditional.permutation().transpose() *
-    rhsPosition += this->dim(j);
+      conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
+  if(debug) {
+    gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
+    gtsam::print(soln, "full back-substitution solution: ");
   }
+  writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
 }
 
 /* ************************************************************************* */
 void GaussianConditional::solveInPlace(VectorValues& x) const {
-  static const bool debug = false;
+  doSolveInPlace(*this, x); // Call helper version above
-  if(debug) print("Solving conditional in place");
-  Vector rhs = extractVectorValuesSlices(x, beginFrontals(), endFrontals());
-	for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
-		rhs += -get_S(parent) * x[*parent];
-	}
-	Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
-	if(debug) {
-		gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
-		gtsam::print(rhs, "rhs: ");
-	  gtsam::print(soln, "full back-substitution solution: ");
-	}
-	writeVectorValuesSlices(soln, x, beginFrontals(), endFrontals());
 }
 
 /* ************************************************************************* */
 void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
-  static const bool debug = false;
+  doSolveInPlace(*this, x); // Call helper version above
-  if(debug) print("Solving conditional in place (permuted)");
-	// Extract RHS from values - inlined from VectorValues
-	size_t s = 0;
-	for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it)
-		s += x[*it].size();
-	Vector rhs(s); size_t start = 0;
-	for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) {
-		SubVector v = x[*it];
-		const size_t d = v.size();
-		rhs.segment(start, d) = v;
-		start += d;
-	}
-
-	// apply parents to rhs
-	for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
-		rhs += -get_S(parent) * x[*parent];
-	}
-
-	// solve system - backsubstitution
-	Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
-
-	// apply solution: inlined manually due to permutation
-  size_t solnStart = 0;
-  for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
-  	const size_t d = this->dim(frontal);
-  	x[*frontal] = soln.segment(solnStart, d);
-  	solnStart += d;
-  }
-}
-
-/* ************************************************************************* */
-VectorValues GaussianConditional::solve(const VectorValues& x) const {
-	VectorValues result = x;
-	solveInPlace(result);
-	return result;
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianConditional.h

@@ -197,46 +197,39 @@ public:
 	 */
 	boost::shared_ptr<JacobianFactor> toFactor() const;
 
-	/**
-	 * Adds the RHS to a given VectorValues for use in solve() functions.
-	 * @param x is the values to be updated, assumed allocated
-	 */
-	void rhs(VectorValues& x) const;
-
   /**
-   * Adds the RHS to a given VectorValues for use in solve() functions.
+   * Solves a conditional Gaussian and writes the solution into the entries of
-   * @param x is the values to be updated, assumed allocated
+   * \c x for each frontal variable of the conditional.  The parents are
-   */
+   * assumed to have already been solved in and their values are read from \c x.
-  void rhs(Permuted<VectorValues>& x) const;
-
-  /**
-   * solves a conditional Gaussian and stores the result in x
    * This function works for multiple frontal variables.
-   * NOTE: assumes that the RHS for the frontals is stored in x, and
-   * then replaces the RHS with the partial result for this conditional,
-   * assuming that parents have been solved already.
    *
-   * @param x values structure with solved parents, and the RHS for this conditional
+   * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
-   * @return solution \f$ x = R \ (d - Sy - Tz - ...) \f$ for each frontal variable
+   * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
+   * variables of this conditional, this solve function computes
+   * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
+   *
+   * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
+   * solution \f$ x_f \f$ will be written.
    */
   void solveInPlace(VectorValues& x) const;
 
   /**
-   * solves a conditional Gaussian and stores the result in x
+   * Solves a conditional Gaussian and writes the solution into the entries of
-   * Identical to solveInPlace() above, with a permuted x
+   * \c x for each frontal variable of the conditional (version for permuted
+   * VectorValues).  The parents are assumed to have already been solved in
+   * and their values are read from \c x.  This function works for multiple
+   * frontal variables.
+   *
+   * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
+   * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
+   * variables of this conditional, this solve function computes
+   * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
+   *
+   * @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
+   * solution \f$ x_f \f$ will be written.
    */
   void solveInPlace(Permuted<VectorValues>& x) const;
 
-  /**
-   * Solves a conditional Gaussian and returns a new VectorValues
-   * This function works for multiple frontal variables, but should
-   * only be used for testing as it copies the input vector values
-   *
-   * Assumes, as in solveInPlace, that the RHS has been stored in x
-   * for all frontal variables
-   */
-  VectorValues solve(const VectorValues& x) const;
-
   // functions for transpose backsubstitution
 
   /**

gtsam/linear/GaussianDensity.cpp

@@ -42,7 +42,6 @@ namespace gtsam {
 		Index k = firstFrontalKey();
 		// a VectorValues that only has a value for k: cannot be printed if k<>0
 		x.insert(k, Vector(sigmas_.size()));
-		rhs(x);
 		solveInPlace(x);
 		return x[k];
 	}

gtsam/linear/GaussianISAM.cpp

@@ -17,6 +17,7 @@
 
 #include <gtsam/3rdparty/Eigen/Eigen/Dense>
 #include <gtsam/linear/GaussianISAM.h>
+#include <gtsam/linear/GaussianBayesTree.h>
 
 #include <gtsam/inference/ISAM-inl.h>
 
@@ -50,50 +51,14 @@ Matrix GaussianISAM::marginalCovariance(Index j) const {
 	return Super::jointBayesNet(key1, key2, &EliminateQR);
 }
 
-/* ************************************************************************* */
-void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
-	// parents are assumed to already be solved and available in result
-	// RHS for current conditional should already be in place in result
-  clique->conditional()->solveInPlace(result);
-
-	BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
-		optimize(child, result);
-}
-
-/* ************************************************************************* */
-void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
-  clique->conditional()->rhs(result);
-	BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
-		treeRHS(child, result);
-}
-
-/* ************************************************************************* */
-VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims) {
-	VectorValues result;
-	if(dims)
-	  result = VectorValues(*dims);
-	else
-	  result = *allocateVectorValues(bayesTree); // allocate
-	treeRHS(bayesTree.root(), result);    // recursively fill
-	return result;
-}
-
 /* ************************************************************************* */
 VectorValues optimize(const GaussianISAM& isam) {
-  VectorValues result = rhs(isam, isam.dims_);
+  VectorValues result(isam.dims_);
-  // starting from the root, call optimize on each conditional
+  // Call optimize for BayesTree
-  optimize(isam.root(), result);
+  optimizeInPlace((const BayesTree<GaussianConditional>&)isam, result);
   return result;
 }
 
-/* ************************************************************************* */
-VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree) {
-	VectorValues result = rhs(bayesTree);
-	// starting from the root, call optimize on each conditional
-	optimize(bayesTree.root(), result);
-	return result;
-}
-
 /* ************************************************************************* */
 BayesNet<GaussianConditional> GaussianISAM::shortcut(sharedClique clique, sharedClique root) {
 	return clique->shortcut(root,&EliminateQR);

gtsam/linear/GaussianISAM.h

@@ -90,19 +90,7 @@ public:
 
 }; // \class GaussianISAM
 
-/** load a VectorValues with the RHS of the system for backsubstitution */
-VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims = boost::none);
-
-/** recursively load RHS for system */
-void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
-
-// recursively optimize this conditional and all subtrees
-void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
-
 // optimize the BayesTree, starting from the root
 VectorValues optimize(const GaussianISAM& isam);
 
-// optimize the BayesTree, starting from the root
-VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree);
-
 } // \namespace gtsam

gtsam/linear/GaussianJunctionTree.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/inference/ClusterTree.h>
 #include <gtsam/inference/JunctionTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/GaussianBayesTree.h>
 
 #include <vector>
 
@@ -33,32 +34,6 @@ namespace gtsam {
 
 	using namespace std;
 
-	/* ************************************************************************* */
-	void GaussianJunctionTree::btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const {
-		// solve the bayes net in the current node
-	  current->conditional()->solveInPlace(config);
-
-	  //		GaussianBayesNet::const_reverse_iterator it = current->rbegin();
-//		for (; it!=current->rend(); ++it) {
-//			(*it)->solveInPlace(config); // solve and store result
-//
-////			Vector x = (*it)->solve(config); // Solve for that variable
-////			config[(*it)->key()] = x;   // store result in partial solution
-//		}
-
-		// solve the bayes nets in the child nodes
-		BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) {
-			btreeBackSubstitute(child, config);
-		}
-	}
-
-	/* ************************************************************************* */
-	void GaussianJunctionTree::btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const {
-		current->conditional()->rhs(config);
-		BOOST_FOREACH(const BTClique::shared_ptr& child, current->children())
-			btreeRHS(child, config);
-	}
-
 	/* ************************************************************************* */
 	VectorValues GaussianJunctionTree::optimize(Eliminate function) const {
 	  tic(1, "GJT eliminate");
@@ -71,12 +46,11 @@ namespace gtsam {
     vector<size_t> dims(rootClique->conditional()->back()+1, 0);
 		countDims(rootClique, dims);
 		VectorValues result(dims);
-		btreeRHS(rootClique, result);
     toc(2, "allocate VectorValues");
 
 		// back-substitution
     tic(3, "back-substitute");
-		btreeBackSubstitute(rootClique, result);
+		internal::optimizeInPlace(rootClique, result);
     toc(3, "back-substitute");
 		return result;
 	}

gtsam/linear/GaussianJunctionTree.h

@@ -45,13 +45,6 @@ namespace gtsam {
 		typedef Base::sharedClique sharedClique;
 		typedef GaussianFactorGraph::Eliminate Eliminate;
 
-	protected:
-		// back-substitute in topological sort order (parents first)
-		void btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const;
-
-		// find the RHS for the system in order to perform backsubstitution
-		void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const;
-
 	public :
 
 		/** Default constructor */

gtsam/linear/GaussianSequentialSolver.cpp

@@ -78,7 +78,8 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
 
   tic(2,"optimize");
   // Back-substitute
-  VectorValues::shared_ptr solution(gtsam::optimize_(*bayesNet));
+  VectorValues::shared_ptr solution(
+      new VectorValues(gtsam::optimize(*bayesNet)));
   toc(2,"optimize");
 
   if(debug) solution->print("GaussianSequentialSolver, solution ");

gtsam/linear/tests/testGaussianConditional.cpp

@@ -144,48 +144,6 @@ TEST( GaussianConditional, equals )
   EXPECT( expected.equals(actual) );
 }
 
-/* ************************************************************************* */
-TEST( GaussianConditional, rhs_permuted )
-{
-  // Test filling the rhs when the VectorValues is permuted
-
-  // Create a VectorValues
-  VectorValues unpermuted(5, 2);
-  unpermuted[0] << 1, 2;
-  unpermuted[1] << 3, 4;
-  unpermuted[2] << 5, 6;
-  unpermuted[3] << 7, 8;
-  unpermuted[4] << 9, 10;
-
-  // Create a permutation
-  Permutation permutation(5);
-  permutation[0] = 4;
-  permutation[1] = 3;
-  permutation[2] = 2;
-  permutation[3] = 1;
-  permutation[4] = 0;
-
-  // Permuted VectorValues
-  Permuted<VectorValues> permuted(permutation, unpermuted);
-
-  // Expected VectorValues
-  VectorValues expected(5, 2);
-  expected[0] << 1, 2;
-  expected[1] << 3, 4;
-  expected[2] << 5, 6;
-  expected[3] << 7, 8;
-  expected[4] << 11, 12;
-
-  // GaussianConditional
-  Vector d(2);  d << 11, 12;
-  GaussianConditional conditional(0, d, Matrix::Identity(2,2), Vector::Ones(2));
-
-  // Fill rhs, conditional is on index 0, which should fill slot 4 of the values
-  conditional.rhs(permuted);
-
-  EXPECT(assert_equal(expected, unpermuted));
-}
-
 /* ************************************************************************* */
 TEST( GaussianConditional, solve )
 {
@@ -208,8 +166,7 @@ TEST( GaussianConditional, solve )
 
   Vector tau = ones(2);
 
-  // RHS is different than the one in the solution vector
+  GaussianConditional cg(_x_, d, R, _x1_, A1, _l1_, A2, tau);
-  GaussianConditional cg(_x_,ones(2), R, _x1_, A1, _l1_, A2, tau);
 
   Vector sx1(2);
   sx1(0) = 1.0; sx1(1) = 1.0;
@@ -218,21 +175,16 @@ TEST( GaussianConditional, solve )
   sl1(0) = 1.0; sl1(1) = 1.0;
 
   VectorValues solution(vector<size_t>(3, 2));
-  solution[_x_]  = d;   // RHS
+  solution[_x_]  = d;
   solution[_x1_] = sx1; // parents
   solution[_l1_] = sl1;
 
-  // NOTE: the solve functions assume the RHS is passed as the initialization of
-  // the solution.
   VectorValues expected(vector<size_t>(3, 2));
   expected[_x_] = expectedX;
   expected[_x1_] = sx1;
   expected[_l1_] = sl1;
-
-  VectorValues copy_result = cg.solve(solution);
   cg.solveInPlace(solution);
 
-  EXPECT(assert_equal(expected, copy_result, 0.0001));
   EXPECT(assert_equal(expected, solution, 0.0001));
 }
 
@@ -240,12 +192,11 @@ TEST( GaussianConditional, solve )
 TEST( GaussianConditional, solve_simple )
 {
 	// no pivoting from LDL, so R matrix is not permuted
-	// RHS is deliberately not the same as below
 	Matrix full_matrix = Matrix_(4, 7,
-			1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.0,
+			1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
-			0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0,
+			0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
-			0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,
+			0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
-			0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0);
+			0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
 
 	// solve system as a non-multifrontal version first
 	// 2 variables, frontal has dim=4
@@ -261,7 +212,6 @@ TEST( GaussianConditional, solve_simple )
 	// elimination order; _x_, _x1_
 	vector<size_t> vdim; vdim += 4, 2;
 	VectorValues actual(vdim);
-	actual[_x_]  = Vector_(4, 0.1, 0.2, 0.3, 0.4); // d
 	actual[_x1_] = sx1; // parent
 
 	VectorValues expected(vdim);
@@ -283,10 +233,10 @@ TEST( GaussianConditional, solve_multifrontal )
 	// create full system, 3 variables, 2 frontals, all 2 dim
 	// no pivoting from LDL, so R matrix is not permuted
 	Matrix full_matrix = Matrix_(4, 7,
-			1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
+			1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
-			0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
+			0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
-			0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
+			0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
-			0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
+			0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
 
 	// 3 variables, all dim=2
 	vector<size_t> dims; dims += 2, 2, 2, 1;
@@ -295,15 +245,13 @@ TEST( GaussianConditional, solve_multifrontal )
 	vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
 	GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
 
-	EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
+	EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
 
 	// partial solution
 	Vector sl1 = Vector_(2, 9.0, 10.0);
 
 	// elimination order; _x_, _x1_, _l1_
 	VectorValues actual(vector<size_t>(3, 2));
-	actual[_x_]  = Vector_(2, 0.1, 0.2); // rhs
-	actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
 	actual[_l1_] = sl1; // parent
 
 	VectorValues expected(vector<size_t>(3, 2));
@@ -327,10 +275,10 @@ TEST( GaussianConditional, solve_multifrontal_permuted )
   // create full system, 3 variables, 2 frontals, all 2 dim
   // no pivoting from LDL, so R matrix is not permuted
   Matrix full_matrix = Matrix_(4, 7,
-      1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
+      1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
-      0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
+      0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
-      0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
+      0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
-      0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
+      0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
 
   // 3 variables, all dim=2
   vector<size_t> dims; dims += 2, 2, 2, 1;
@@ -339,7 +287,7 @@ TEST( GaussianConditional, solve_multifrontal_permuted )
   vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
   GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
 
-  EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
+  EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
 
   // partial solution
   Vector sl1 = Vector_(2, 9.0, 10.0);

gtsam/nonlinear/GaussianISAM2-inl.h

@@ -25,8 +25,9 @@ namespace gtsam {
   using namespace std;
 
   /* ************************************************************************* */
+  namespace internal {
   template<class CLIQUE>
-  void optimize2(const boost::shared_ptr<CLIQUE>& clique, double threshold,
+  void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
       vector<bool>& changed, const vector<bool>& replaced, Permuted<VectorValues>& delta, int& count) {
     // if none of the variables in this clique (frontal and separator!) changed
     // significantly, then by the running intersection property, none of the
@@ -63,7 +64,6 @@ namespace gtsam {
       }
 
       // Back-substitute
-      (*clique)->rhs(delta);
       (*clique)->solveInPlace(delta);
       count += (*clique)->nrFrontals();
 
@@ -97,42 +97,19 @@ namespace gtsam {
 
       // Recurse to children
       BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
-        optimize2(child, threshold, changed, replaced, delta, count);
+        optimizeWildfire(child, threshold, changed, replaced, delta, count);
       }
     }
   }
-
-  /* ************************************************************************* */
-  // fast full version without threshold
-  template<class CLIQUE>
-  void optimize2(const boost::shared_ptr<CLIQUE>& clique, VectorValues& delta) {
-
-    // parents are assumed to already be solved and available in result
-    (*clique)->rhs(delta);
-    (*clique)->solveInPlace(delta);
-
-    // Solve chilren recursively
-    BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
-      optimize2(child, delta);
-    }
   }
 
-  ///* ************************************************************************* */
-  //boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
-  //	boost::shared_ptr<VectorValues> delta(new VectorValues());
-  //	set<Key> changed;
-  //	// starting from the root, call optimize on each conditional
-  //	optimize2(root, delta);
-  //	return delta;
-  //}
-
   /* ************************************************************************* */
   template<class CLIQUE>
-  int optimize2(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
+  int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
     vector<bool> changed(keys.size(), false);
     int count = 0;
     // starting from the root, call optimize on each conditional
-    optimize2(root, threshold, changed, keys, delta, count);
+    internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
     return count;
   }
 

gtsam/nonlinear/GaussianISAM2.h

@@ -63,10 +63,6 @@ public:
 
 };
 
-/** optimize the BayesTree, starting from the root */
-template<class CLIQUE>
-void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
-
 /// optimize the BayesTree, starting from the root; "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
@@ -76,7 +72,7 @@ void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
 /// variables are contained in the subtree.
 /// returns the number of variables that were solved for
 template<class CLIQUE>
-int optimize2(const boost::shared_ptr<CLIQUE>& root,
+int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
     double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
 
 /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -15,6 +15,8 @@
  * @author  Michael Kaess, Richard Roberts
  */
 
+#include <gtsam/linear/GaussianBayesTree.h>
+
 namespace gtsam {
 
 using namespace std;
@@ -374,7 +376,7 @@ size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2
     VectorValues newDelta(dims);
 
     // Optimize full solution delta
-    optimize2(root, newDelta);
+    internal::optimizeInPlace(root, newDelta);
 
     // Copy solution into delta
     delta.permutation() = Permutation::Identity(delta.size());
@@ -384,7 +386,7 @@ size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2
 
   } else {
     // Optimize with wildfire
-    lastBacksubVariableCount = optimize2(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
+    lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
 
 #ifndef NDEBUG
     for(size_t j=0; j<delta.container().size(); ++j)

gtsam/nonlinear/ISAM2-inl.h

@@ -634,7 +634,7 @@ const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
 template<class CONDITIONAL, class GRAPH>
 VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
   VectorValues delta = *allocateVectorValues(isam);
-  optimize2(isam.root(), delta);
+  internal::optimizeInPlace(isam.root(), delta);
   return delta;
 }
 

tests/testGaussianBayesNet.cpp

@@ -119,64 +119,25 @@ TEST( GaussianBayesNet, optimize2 )
 }
 
 /* ************************************************************************* */
-TEST( GaussianBayesNet, backSubstitute )
+TEST( GaussianBayesNet, optimize3 )
 {
 	// y=R*x, x=inv(R)*y
-	// 2 = 1 1  -1
+	// 9 = 1 1   4
-	// 3     1   3
+	// 5     1   5
 	// NOTE: we are supplying a new RHS here
   GaussianBayesNet cbn = createSmallGaussianBayesNet();
 
-  VectorValues y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
+  VectorValues expected(vector<size_t>(2,1)), x(vector<size_t>(2,1));
-  y[_x_] = Vector_(1,2.);
+  expected[_x_] = Vector_(1, 4.);
-  y[_y_] = Vector_(1,3.);
+  expected[_y_] = Vector_(1, 5.);
-  x[_x_] = Vector_(1,-1.);
-  x[_y_] = Vector_(1, 3.);
 
   // test functional version
-  VectorValues actual = backSubstitute(cbn,y);
+  VectorValues actual = optimize(cbn);
-  EXPECT(assert_equal(x,actual));
+  EXPECT(assert_equal(expected,actual));
 
   // test imperative version
-  backSubstituteInPlace(cbn,y);
+  optimizeInPlace(cbn,x);
-  EXPECT(assert_equal(x,y));
+  EXPECT(assert_equal(expected,x));
-}
-
-/* ************************************************************************* */
-TEST( GaussianBayesNet, rhs )
-{
-	// y=R*x, x=inv(R)*y
-	// 2 = 1 1  -1
-	// 3     1   3
-  GaussianBayesNet cbn = createSmallGaussianBayesNet();
-	VectorValues expected = gtsam::optimize(cbn);
-	VectorValues d = rhs(cbn);
-	VectorValues actual = backSubstitute(cbn, d);
-	EXPECT(assert_equal(expected, actual));
-}
-
-/* ************************************************************************* */
-TEST( GaussianBayesNet, rhs_with_sigmas )
-{
-	Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
-	Matrix R22 = Matrix_(1, 1, 1.0);
-	Vector d1(1), d2(1);
-	d1(0) = 9;
-	d2(0) = 5;
-	Vector tau(1);
-	tau(0) = 0.25;
-
-	// define nodes and specify in reverse topological sort (i.e. parents last)
-	GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11,
-			_y_, S12, tau)), Py(new GaussianConditional(_y_, d2, R22, tau));
-	GaussianBayesNet cbn;
-	cbn.push_back(Px_y);
-	cbn.push_back(Py);
-
-	VectorValues expected = gtsam::optimize(cbn);
-	VectorValues d = rhs(cbn);
-	VectorValues actual = backSubstitute(cbn, d);
-	EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */

tests/testGaussianISAM2.cpp

@@ -16,6 +16,7 @@ using namespace boost::assign;
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/nonlinear/GaussianISAM2.h>
 #include <gtsam/slam/smallExample.h>
 #include <gtsam/slam/planarSLAM.h>
@@ -167,15 +168,13 @@ TEST(ISAM2, optimize2) {
 
   // Expected vector
   VectorValues expected(1, 3);
-  conditional->rhs(expected);
   conditional->solveInPlace(expected);
 
   // Clique
   GaussianISAM2<>::sharedClique clique(
       GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
   VectorValues actual(theta.dims(ordering));
-  conditional->rhs(actual);
+  internal::optimizeInPlace(clique, actual);
-  optimize2(clique, actual);
 
 //  expected.print("expected: ");
 //  actual.print("actual: ");