Assorted cleanup to remove ublas references, switch more Vector/Matrix utility functions to use Eigen in templates, reimplemented backsubstitution with Eigen

gtsam/3rdparty/Eigen/Makefile.am

@@ -4,7 +4,6 @@
 SUBDIRS = src
 
 # And the corresponding libraries produced
-# TODO add sublibs when something in 3rdparty actually needs compilation
 SUBLIBS = 
 
 # use nostdinc to turn off -I. and -I.., we do not need them because 

gtsam/base/Matrix.cpp

@@ -522,67 +522,27 @@ void householder(MatrixColMajor& A, size_t k) {
 
 /* ************************************************************************* */
 Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
-	size_t n = L.cols();
+	// @return the solution x of L*x=b
-#ifndef NDEBUG
+	assert(L.rows() == L.cols());
-	size_t m = L.rows();
+	if (unit)
-	if (m!=n)
+		return L.triangularView<Eigen::UnitLower>().solve(b);
-		throw invalid_argument("backSubstituteLower: L must be square");
+	else
-#endif
+		return L.triangularView<Eigen::Lower>().solve(b);
-
-	Vector result(n);
-	for (size_t i = 1; i <= n; i++) {
-		double zi = b(i-1);
-		for (size_t j = 1; j < i; j++)
-			zi -= L(i-1,j-1) * result(j-1);
-#ifndef NDEBUG
-		if(!unit && fabs(L(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
-			stringstream ss;
-			ss << "backSubstituteUpper: L is singular,\n";
-			print(L, "L: ", ss);
-			throw invalid_argument(ss.str());
-		}
-#endif
-		if (!unit) zi /= L(i-1,i-1);
-		result(i-1) = zi;
-	}
-
-	return result;
 }
 
 /* ************************************************************************* */
 Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
 	// @return the solution x of U*x=b
-  size_t n = U.cols();
+	assert(U.rows() == U.cols());
-#ifndef NDEBUG
+	if (unit)
-  size_t m = U.rows();
+		return U.triangularView<Eigen::UnitUpper>().solve(b);
-  if (m!=n)
+	else
-    throw invalid_argument("backSubstituteUpper: U must be square");
+		return U.triangularView<Eigen::Upper>().solve(b);
-#endif
-
-  Vector result(n);
-  for (size_t i = n; i > 0; i--) {
-    double zi = b(i-1);
-    for (size_t j = i+1; j <= n; j++)
-      zi -= U(i-1,j-1) * result(j-1);
-#ifndef NDEBUG
-    if(!unit && fabs(U(i-1,i-1)) <= numeric_limits<double>::epsilon()) {
-      stringstream ss;
-      ss << "backSubstituteUpper: U is singular,\n";
-      print(U, "U: ", ss);
-      throw invalid_argument(ss.str());
-    }
-#endif
-    if (!unit) zi /= U(i-1,i-1);
-    result(i-1) = zi;
-  }
-
-  return result;
 }
 
 /* ************************************************************************* */
 Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
 	// @return the solution x of x'*U=b'
-
   size_t n = U.cols();
 #ifndef NDEBUG
   size_t m = U.rows();

gtsam/base/Matrix.h

@@ -30,16 +30,9 @@
 
 /**
  * Matrix is a *global* typedef
- * wrap-matlab does this typedef as well
  * we use the default < double,row_major,unbounded_array<double> >
  */
 
-// FIXME: replace to handle matlab wrapper
-//#if ! defined (MEX_H)
-//typedef boost::numeric::ublas::matrix<double> Matrix;
-//typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> MatrixColMajor;
-//#endif
-
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Matrix;
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixColMajor;
 
@@ -373,6 +366,7 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
  * @return the solution x of x'*U=b'
  */
 //FIXME: add back expression form
+//TODO: is this function necessary? it isn't used
 //template<class VECTOR, class MATRIX>
 //Vector backSubstituteUpper(const VECTOR& b, const MATRIX& U, bool unit=false);
 Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);

gtsam/base/Vector.cpp

@@ -11,7 +11,7 @@
 
 /**
  * @file    Vector.cpp
- * @brief   typedef and functions to augment Boost's ublas::vector<double>
+ * @brief   typedef and functions to augment Eigen's Vectors
  * @author  Kai Ni
  * @author  Frank Dellaert
  */
@@ -312,29 +312,6 @@ double max(const Vector &a) {
 	return a.maxCoeff();
 }
 
-/* ************************************************************************* */
-double dot(const Vector& a, const Vector& b) {
-	assert (b.size()==a.size());
-	return a.dot(b);
-}
-
-/* ************************************************************************* */
-void scal(double alpha, Vector& x) {
-	x *= alpha;
-}
-
-/* ************************************************************************* */
-void axpy(double alpha, const Vector& x, Vector& y) {
-	assert (y.size()==x.size());
-	y += alpha * x;
-}
-
-/* ************************************************************************* */
-void axpy(double alpha, const Vector& x, SubVector y) {
-	assert (y.size()==x.size());
-	y += alpha * x;
-}
-
 /* ************************************************************************* */
 // imperative version, pass in x
 double houseInPlace(Vector &v) {

gtsam/base/Vector.h

@@ -11,7 +11,7 @@
 
 /**
  * @file    Vector.h
- * @brief   typedef and functions to augment Boost's ublas::vector<double>
+ * @brief   typedef and functions to augment Eigen's VectorXd
  * @author  Kai Ni
  * @author  Frank Dellaert
  */
@@ -26,11 +26,6 @@
 #include <boost/random/linear_congruential.hpp>
 
 // Vector is a *global* typedef
-// wrap-matlab does this typedef as well
-// TODO: fix matlab wrapper
-//#if ! defined (MEX_H)
-//typedef boost::numeric::ublas::vector<double> Vector;
-//#endif
 
 // Typedef arbitary length vector
 typedef Eigen::VectorXd Vector;
@@ -263,20 +258,41 @@ Vector abs(const Vector& v);
  */
 double max(const Vector &a);
 
-/** Dot product */
+/**
-double dot(const Vector &a, const Vector& b);
+ * Dot product
+ */
+template<class V1, class V2>
+inline double dot(const V1 &a, const V2& b) {
+	assert (b.size()==a.size());
+	return a.dot(b);
+}
+
+/** compatibility version for ublas' inner_prod() */
+template<class V1, class V2>
+inline double inner_prod(const V1 &a, const V2& b) {
+	assert (b.size()==a.size());
+	return a.dot(b);
+}
 
-// TODO: remove simple blas functions - these are one-liners with Eigen
 /**
  * BLAS Level 1 scal: x <- alpha*x
+ * @DEPRECIATED: use operators instead
  */
-void scal(double alpha, Vector& x);
+inline void scal(double alpha, Vector& x) { x *= alpha; }
 
 /**
  * BLAS Level 1 axpy: y <- alpha*x + y
+ * @DEPRECIATED: use operators instead
  */
-void axpy(double alpha, const Vector& x, Vector& y);
+template<class V1, class V2>
-void axpy(double alpha, const Vector& x, SubVector y);
+inline void axpy(double alpha, const V1& x, V2& y) {
+	assert (y.size()==x.size());
+	y += alpha * x;
+}
+inline void axpy(double alpha, const Vector& x, SubVector y) {
+	assert (y.size()==x.size());
+	y += alpha * x;
+}
 
 /**
  * house(x,j) computes HouseHolder vector v and scaling factor beta

gtsam/base/blockMatrices.h

@@ -243,8 +243,7 @@ public:
   template<class RHS>
   VerticalBlockView<MATRIX>& assignNoalias(const RHS& rhs) {
     copyStructureFrom(rhs);
-//    boost::numeric::ublas::noalias(matrix_) = rhs.full();
+    matrix_.noalias() = rhs.full();
-    matrix_ = rhs.full(); // FIXME: check if noalias is necessary here
     return *this;
   }
 
@@ -530,8 +529,7 @@ public:
   template<class RHSMATRIX>
   SymmetricBlockView<MATRIX>& assignNoalias(const SymmetricBlockView<RHSMATRIX>& rhs) {
     copyStructureFrom(rhs);
-//    boost::numeric::ublas::noalias(matrix_) = rhs.range(0, rhs.nBlocks(), 0, rhs.nBlocks());
+    matrix_.noalias() = rhs.matrix_.block(0, 0, rhs.nBlocks(), rhs.nBlocks());
-    matrix_ = rhs.matrix_.block(0, 0, rhs.nBlocks(), rhs.nBlocks());
     return *this;
   }
 

gtsam/base/tests/testCholesky.cpp

@@ -20,55 +20,7 @@
 #include <gtsam/base/cholesky.h>
 #include <CppUnitLite/TestHarness.h>
 
-//#include <boost/numeric/ublas/triangular.hpp>
-//#include <boost/numeric/ublas/matrix_proxy.hpp>
-//#include <boost/numeric/ublas/symmetric.hpp>
-
 using namespace gtsam;
-//namespace ublas = boost::numeric::ublas;
-
-///* ************************************************************************* */
-//TEST(cholesky, cholesky_inplace) {
-//  Matrix I = Matrix_(5,5,
-//         1.739214152118470,   0.714164147286383,   1.893437990040579,   1.469485110232169,   1.549910412077052,
-//         0.714164147286383,   0.587767128354794,   0.738548568260523,   0.588775778746414,   0.645015863153235,
-//         1.893437990040579,   0.738548568260523,   2.165530165155413,   1.631203698494913,   1.617901992621506,
-//         1.469485110232169,   0.588775778746414,   1.631203698494913,   1.591363675348797,   1.584779118350080,
-//         1.549910412077052,   0.645015863153235,   1.617901992621506,   1.584779118350080,   1.928887183904716);
-//
-//  Matrix expected = Matrix_(5,5,
-//      1.318792687316119,   0.541528743793524,   1.435735888021887,   1.114265437142152,   1.175249473995251,
-//                    0.0,   0.542691208699940,  -0.071760299365346,  -0.026960052875681,   0.015818372803848,
-//                    0.0,                 0.0,   0.314711112667495,   0.093667363355578,  -0.217058504307151,
-//                    0.0,                 0.0,                 0.0,   0.583331630832890,   0.507424929100112,
-//                    0.0,                 0.0,                 0.0,                 0.0,   0.492779041656733);
-//
-//  MatrixColMajor actualColMaj = I;
-//  cholesky_inplace(actualColMaj);
-//  Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColMaj);
-//  CHECK(assert_equal(expected, actual, 1e-7));
-//}
-
-///* ************************************************************************* */
-//TEST(cholesky, choleskyFactorUnderdetermined) {
-//
-//  Matrix Ab = Matrix_(3,7,
-//      0.0357,    0.6787,    0.3922,    0.7060,    0.0462,    0.6948,    0.0344,
-//      0.8491,    0.7577,    0.6555,    0.0318,    0.0971,    0.3171,    0.4387,
-//      0.9340,    0.7431,    0.1712,    0.2769,    0.8235,    0.9502,    0.3816);
-//
-//  Matrix expected = Matrix_(3,7,
-//      1.2628,    1.0784,    0.5785,    0.2462,    0.6757,    0.9357,    0.5782,
-//         0.0,    0.6513,    0.4089,    0.6811,   -0.0180,    0.6280,    0.0244,
-//         0.0,       0.0,    0.3332,   -0.2273,   -0.4825,   -0.4652,    0.0660);
-//
-//  MatrixColMajor actualColmaj = Ab;
-//  choleskyFactorUnderdetermined(actualColmaj, 3);
-//
-//  Matrix actual = ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(actualColmaj);
-//
-//  CHECK(assert_equal(expected, actual, 1e-3));
-//}
 
 /* ************************************************************************* */
 TEST(cholesky, choleskyPartial) {
@@ -91,12 +43,8 @@ TEST(cholesky, choleskyPartial) {
   // See the function comment for choleskyPartial, this decomposition should hold.
   MatrixColMajor R1 = RSL.transpose();
   MatrixColMajor R2 = RSL;
-//  ublas::project(R1, ublas::range(3,7), ublas::range(3,7)) = eye(4,4);
   R1.block(3, 3, 4, 4).setIdentity();
 
-//  ublas::project(R2, ublas::range(3,7), ublas::range(3,7)) =
-//      ublas::symmetric_adaptor<const ublas::matrix_range<Matrix>,ublas::upper>(
-//          ublas::project(R2, ublas::range(3,7), ublas::range(3,7)));
   R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>();
 
   MatrixColMajor actual = R1 * R2;

gtsam/base/tests/testVector.cpp

@@ -239,10 +239,13 @@ TEST( TestVector, dot )
 TEST( TestVector, axpy )
 {
   Vector x = Vector_(3,10.,20.,30.);
-  Vector y = Vector_(3,2.0,5.0,6.0);
+  Vector y0 = Vector_(3,2.0,5.0,6.0);
-  axpy(0.1,x,y);
+  Vector y1 = y0, y2 = y0;
+  axpy(0.1,x,y1);
+  axpy(0.1,x,y2.head(3));
   Vector expected = Vector_(3,3.0,7.0,9.0);
-  EXPECT(assert_equal(expected,y));
+  EXPECT(assert_equal(expected,y1));
+  EXPECT(assert_equal(expected,Vector(y2)));
 }
 
 /* ************************************************************************* */

gtsam/base/tests/timeMatrix.cpp

@@ -16,13 +16,11 @@
  */
 
 #include <iostream>
-#include <boost/numeric/ublas/matrix_proxy.hpp>
 #include <boost/timer.hpp>
 #include <gtsam/base/Matrix.h>
 
 using namespace std;
 using namespace gtsam;
-namespace ublas = boost::numeric::ublas;
 
 /*
  * Results:

gtsam/inference/Conditional.h

@@ -34,8 +34,6 @@ namespace gtsam {
  * provides storage for the keys involved in a conditional, and iterators and
  * access to the frontal and separator keys.
  *
- * todo:  Move permutation functions to IndexConditional.
- *
  * Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
  * to the associated factor type and shared_ptr type of the derived class.  See
  * IndexConditional and GaussianConditional for examples.

gtsam/inference/Factor.h

@@ -40,9 +40,6 @@ template<class KEY> class Conditional;
  * class, using Index keys.  This class does not store any data other than its
  * keys.  Derived classes store data such as matrices and probability tables.
  *
- * todo:  Make NonlinearFactor derive from this too, which requires moving
- * Combine, eliminate*, permute* and the sorted key invariant to IndexFactor.
- *
  * Note that derived classes *must* redefine the ConditionalType and shared_ptr
  * typedefs to refer to the associated conditional and shared_ptr types of the
  * derived class.  See IndexFactor, JacobianFactor, etc. for examples.

gtsam/linear/Errors.cpp

@@ -103,7 +103,8 @@ double dot(const Errors& a, const Errors& b) {
 }
 
 /* ************************************************************************* */
-void axpy(double alpha, const Errors& x, Errors& y) {
+template<>
+void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
 	Errors::const_iterator it = x.begin();
   BOOST_FOREACH(Vector& yi, y)
 		axpy(alpha,*(it++),yi);

gtsam/linear/Errors.h

@@ -60,7 +60,8 @@ namespace gtsam {
   /**
    * BLAS level 2 style
    */
-  void axpy(double alpha, const Errors& x, Errors& y);
+  template <>
+  void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y);
 
   /** print with optional string */
   void print(const Errors& a, const std::string& s = "Error");

gtsam/linear/GaussianConditional.cpp

@@ -148,7 +148,6 @@ Vector GaussianConditional::solve(const VectorValues& x) const {
 	Vector rhs(get_d());
 	for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
 		rhs += -get_S(parent) * x[*parent];
-//  ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false);
 	}
 	if(debug) gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
 	if(debug) gtsam::print(rhs, "rhs: ");

gtsam/linear/GaussianConditional.h

@@ -47,7 +47,6 @@ public:
 	typedef boost::shared_ptr<GaussianConditional> shared_ptr;
 
 	/** Store the conditional matrix as upper-triangular column-major */
-//	typedef boost::numeric::ublas::triangular_matrix<double, boost::numeric::ublas::upper, boost::numeric::ublas::column_major> AbMatrix;
 	typedef MatrixColMajor AbMatrix;
 	typedef VerticalBlockView<AbMatrix> rsd_type;
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -275,7 +275,7 @@ namespace gtsam {
 	}
 
   /* ************************************************************************* */
-  //static
+  static
   FastMap<Index, SlotEntry> findScatterAndDims
   (const FactorGraph<GaussianFactor>& factors) {
 

gtsam/linear/GaussianFactorGraph.h

@@ -61,9 +61,6 @@ namespace gtsam {
     return e;
   }
 
-  /** DEBUG ONLY: TODO: hide again later */
-  FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors);
-
   /**
    * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    *   Factor == GaussianFactor

gtsam/linear/GaussianISAM.cpp

@@ -24,8 +24,6 @@ using namespace gtsam;
 #include <gtsam/inference/ISAM-inl.h>
 template class ISAM<GaussianConditional>;
 
-//namespace ublas = boost::numeric::ublas;
-
 namespace gtsam {
 
 /* ************************************************************************* */

gtsam/linear/GaussianMultifrontalSolver.cpp

@@ -72,7 +72,6 @@ std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalCovariance(Index j
   GaussianConditional::shared_ptr conditional = EliminateQR(fg,1).first->front();
   Matrix R = conditional->get_R();
   return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
-//  return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
 }
 
 }

gtsam/linear/GaussianSequentialSolver.cpp

@@ -88,7 +88,6 @@ std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j)
 	GaussianConditional::shared_ptr conditional = EliminateQR(fg, 1).first->front();
 	Matrix R = conditional->get_R();
 	return make_pair(conditional->get_d(), (R.transpose() * R).inverse());
-//	return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R),R)));
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.cpp

@@ -105,8 +105,6 @@ HessianFactor::HessianFactor(Index j1, Index j2,
 HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
 	JacobianFactor jf(cg);
 	info_.copyStructureFrom(jf.Ab_);
-	//    ublas::noalias(ublas::symmetric_adaptor<MatrixColMajor,ublas::upper>(matrix_)) =
-	//        ublas::prod(ublas::trans(jf.matrix_), jf.matrix_);
 	matrix_.noalias() = jf.matrix_.transpose() * jf.matrix_;
 	assertInvariants();
 }
@@ -175,8 +173,6 @@ void HessianFactor::print(const std::string& s) const {
 	for(const_iterator key=this->begin(); key!=this->end(); ++key)
 		cout << *key << "(" << this->getDim(key) << ") ";
 	cout << "\n";
-	//    gtsam::print(ublas::symmetric_adaptor<const constBlock,ublas::upper>(
-	//        info_.range(0,info_.nBlocks(), 0,info_.nBlocks())), "Ab^T * Ab: ");
 	gtsam::print(MatrixColMajor(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView<Eigen::Upper>()), "Ab^T * Ab: ");
 }
 
@@ -186,10 +182,8 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
 		return false;
 	else {
 		MatrixColMajor thisMatrix = this->info_.full().selfadjointView<Eigen::Upper>();
-		//      MatrixColMajor thisMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(this->info_.full());
 		thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
 		MatrixColMajor rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView<Eigen::Upper>();
-		//      MatrixColMajor rhsMatrix = ublas::symmetric_adaptor<const MatrixColMajor,ublas::upper>(static_cast<const HessianFactor&>(lf).info_.full());
 		rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
 		return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
 	}
@@ -444,7 +438,6 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector<Index>& key
 		}
 
 		tic(2, "construct cond");
-		//    const ublas::scalar_vector<double> sigmas(varDim, 1.0);
 		Vector sigmas = Vector::Ones(varDim);
 		conditionals->push_back(boost::make_shared<ConditionalType>(keys.begin()+j, keys.end(), 1, Ab, sigmas));
 		toc(2, "construct cond");

gtsam/linear/HessianFactor.h

@@ -25,6 +25,8 @@
 
 // Forward declarations for friend unit tests
 class ConversionConstructorHessianFactorTest;
+class Constructor1HessianFactorTest;
+
 
 namespace gtsam {
 
@@ -118,11 +120,6 @@ namespace gtsam {
     /** returns the full linear term - g from the constructors */
     constColumn linear_term() const;
 
-    /** const access to the full matrix DEBUG ONLY*/
-    const InfoMatrix& raw_matrix() const { return matrix_; }
-
-    const BlockInfo& raw_info() const { return info_; } /// DEBUG ONLY
-
     /**
      * Permutes the GaussianFactor, but for efficiency requires the permutation
      * to already be inverted.  This acts just as a change-of-name for each
@@ -133,6 +130,7 @@ namespace gtsam {
 
     // Friend unit test classes
     friend class ::ConversionConstructorHessianFactorTest;
+    friend class ::Constructor1HessianFactorTest;
 
     // Friend JacobianFactor for conversion
     friend class JacobianFactor;

gtsam/linear/JacobianFactor.cpp

@@ -164,7 +164,6 @@ namespace gtsam {
     Ab_.assignNoalias(factor.info_);
     size_t maxrank = choleskyCareful(matrix_).first;
     // FIXME: replace with triangular system
-//    matrix_ = ublas::triangular_adaptor<AbMatrix, ublas::upper>(matrix_);
     Ab_.rowEnd() = maxrank;
     model_ = noiseModel::Unit::Create(maxrank);
 

gtsam/linear/JacobianFactor.h

@@ -32,6 +32,7 @@
 // Forward declarations of friend unit tests
 class Combine2GaussianFactorTest;
 class eliminateFrontalsGaussianFactorTest;
+class constructor2GaussianFactorTest;
 
 namespace gtsam {
 
@@ -183,18 +184,6 @@ namespace gtsam {
      */
     Matrix matrix_augmented(bool weight = true) const;
 
-    /**
-     * Returns full dense matrix underying factor
-     * TESTING ONLY
-     */
-    const AbMatrix& raw_matrix() const { return matrix_; }
-
-    /**
-     * Returns the block indexing view
-     * TESTING ONLY
-     */
-    const BlockAb& Ab() const { return Ab_; }
-
     /**
      * Return vector of i, j, and s to generate an m-by-n sparse matrix
      * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
@@ -219,6 +208,7 @@ namespace gtsam {
     // Friend unit tests (see also forward declarations above)
     friend class ::Combine2GaussianFactorTest;
     friend class ::eliminateFrontalsGaussianFactorTest;
+    friend class ::constructor2GaussianFactorTest;
 
     /* Used by ::CombineJacobians for sorting */
     struct _RowSource {

gtsam/linear/Makefile.am

@@ -38,7 +38,7 @@ headers += iterative-inl.h
 sources += iterative.cpp SubgraphPreconditioner.cpp
 headers += IterativeSolver.h IterativeOptimizationParameters.h 
 headers += SubgraphSolver.h SubgraphSolver-inl.h
-#sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp
+#sources += BayesNetPreconditioner.cpp 
 #check_PROGRAMS += tests/testBayesNetPreconditioner
 
 # Timing tests

gtsam/linear/tests/testGaussianFactor.cpp

@@ -59,7 +59,7 @@ TEST(GaussianFactor, constructor2)
   EXPECT(assert_equal(*key0, _x0_));
   EXPECT(assert_equal(*key1, _x1_));
   EXPECT(!actual.empty());
-  EXPECT_LONGS_EQUAL(3, actual.Ab().nBlocks());
+  EXPECT_LONGS_EQUAL(3, actual.Ab_.nBlocks());
 
   Matrix actualA0 = actual.getA(key0);
   Matrix actualA1 = actual.getA(key1);

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -334,13 +334,6 @@ TEST(GaussianFactor, eliminateFrontals)
 
   // Create first factor (from pieces of Ab)
   list<pair<Index, Matrix> > terms1;
-//  terms1 +=
-//      make_pair( 3, Matrix(ublas::project(Ab, ublas::range(0,4), ublas::range(0,2)))),
-//      make_pair( 5, ublas::project(Ab, ublas::range(0,4), ublas::range(2,4))),
-//      make_pair( 7, ublas::project(Ab, ublas::range(0,4), ublas::range(4,6))),
-//      make_pair( 9, ublas::project(Ab, ublas::range(0,4), ublas::range(6,8))),
-//      make_pair(11, ublas::project(Ab, ublas::range(0,4), ublas::range(8,10)));
-//  Vector b1 = ublas::project(ublas::column(Ab, 10), ublas::range(0,4));
 
   terms1 +=
       make_pair( 3, Matrix(Ab.block(0, 0, 4, 2))),
@@ -348,33 +341,33 @@ TEST(GaussianFactor, eliminateFrontals)
       make_pair( 7, Matrix(Ab.block(0, 4, 4, 2))),
       make_pair( 9, Matrix(Ab.block(0, 6, 4, 2))),
       make_pair(11, Matrix(Ab.block(0, 8, 4, 2)));
-  Vector b1 = Ab.col(10).segment(0, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(0,4));
+  Vector b1 = Ab.col(10).segment(0, 4);
   JacobianFactor::shared_ptr factor1(new JacobianFactor(terms1, b1, sharedSigma(4, 0.5)));
 
   // Create second factor
   list<pair<Index, Matrix> > terms2;
   terms2 +=
-      make_pair(5, Matrix(Ab.block(4, 2, 4, 2))), //make_pair(5, ublas::project(Ab, ublas::range(4,8), ublas::range(2,4))),
+      make_pair(5, Matrix(Ab.block(4, 2, 4, 2))),
-      make_pair(7, Matrix(Ab.block(4, 4, 4, 2))), //make_pair(7, ublas::project(Ab, ublas::range(4,8), ublas::range(4,6))),
+      make_pair(7, Matrix(Ab.block(4, 4, 4, 2))),
-      make_pair(9, Matrix(Ab.block(4, 6, 4, 2))), //make_pair(9, ublas::project(Ab, ublas::range(4,8), ublas::range(6,8))),
+      make_pair(9, Matrix(Ab.block(4, 6, 4, 2))),
-      make_pair(11,Matrix(Ab.block(4, 8, 4, 2))); //make_pair(11,ublas::project(Ab, ublas::range(4,8), ublas::range(8,10)));
+      make_pair(11,Matrix(Ab.block(4, 8, 4, 2)));
-  Vector b2 = Ab.col(10).segment(4, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(4,8));
+  Vector b2 = Ab.col(10).segment(4, 4);
   JacobianFactor::shared_ptr factor2(new JacobianFactor(terms2, b2, sharedSigma(4, 0.5)));
 
   // Create third factor
   list<pair<Index, Matrix> > terms3;
   terms3 +=
-      make_pair(7, Matrix(Ab.block(8, 4, 4, 2))),  //make_pair(7, ublas::project(Ab, ublas::range(8,12), ublas::range(4,6))),
+      make_pair(7, Matrix(Ab.block(8, 4, 4, 2))),
-      make_pair(9, Matrix(Ab.block(8, 6, 4, 2))),  //make_pair(9, ublas::project(Ab, ublas::range(8,12), ublas::range(6,8))),
+      make_pair(9, Matrix(Ab.block(8, 6, 4, 2))),
-      make_pair(11,Matrix(Ab.block(8, 8, 4, 2)));  //make_pair(11,ublas::project(Ab, ublas::range(8,12), ublas::range(8,10)));
+      make_pair(11,Matrix(Ab.block(8, 8, 4, 2)));
-  Vector b3 = Ab.col(10).segment(8, 4); //ublas::project(ublas::column(Ab, 10), ublas::range(8,12));
+  Vector b3 = Ab.col(10).segment(8, 4);
   JacobianFactor::shared_ptr factor3(new JacobianFactor(terms3, b3, sharedSigma(4, 0.5)));
 
   // Create fourth factor
   list<pair<Index, Matrix> > terms4;
   terms4 +=
-  		make_pair(11, Matrix(Ab.block(12, 8, 2, 2))); //make_pair(11, ublas::project(Ab, ublas::range(12,14), ublas::range(8,10)));
+  		make_pair(11, Matrix(Ab.block(12, 8, 2, 2)));
-  Vector b4 = Ab.col(10).segment(12, 2); //ublas::project(ublas::column(Ab, 10), ublas::range(12,14));
+  Vector b4 = Ab.col(10).segment(12, 2);
   JacobianFactor::shared_ptr factor4(new JacobianFactor(terms4, b4, sharedSigma(2, 0.5)));
 
   // Create factor graph
@@ -424,7 +417,7 @@ TEST(GaussianFactor, eliminateFrontals)
       make_pair(7, sub(R, 2,4, 4,6)),
       make_pair(9, sub(R, 2,4, 6,8)),
       make_pair(11,sub(R, 2,4, 8,10));
-  Vector d2 = R.col(10).segment(2,2); //ublas::project(ublas::column(R, 10), ublas::range(2,4));
+  Vector d2 = R.col(10).segment(2,2);
   GaussianConditional::shared_ptr cond2(new GaussianConditional(5, d2, R2, cterms2, ones(2)));
 
   // Expected conditional on third variable from next 2 rows of R
@@ -433,7 +426,7 @@ TEST(GaussianFactor, eliminateFrontals)
   cterms3 +=
       make_pair(9,  sub(R, 4,6, 6,8)),
       make_pair(11, sub(R, 4,6, 8,10));
-  Vector d3 = R.col(10).segment(4,2); //ublas::project(ublas::column(R, 10), ublas::range(4,6));
+  Vector d3 = R.col(10).segment(4,2);
   GaussianConditional::shared_ptr cond3(new GaussianConditional(7, d3, R3, cterms3, ones(2)));
 
   // Create expected Bayes net fragment from three conditionals above
@@ -445,7 +438,7 @@ TEST(GaussianFactor, eliminateFrontals)
   // Get expected matrices for remaining factor
   Matrix Ae1 = sub(R, 6,10, 6,8);
   Matrix Ae2 = sub(R, 6,10, 8,10);
-  Vector be = R.col(10).segment(6, 4); //blas::project(ublas::column(R, 10), ublas::range(6,10));
+  Vector be = R.col(10).segment(6, 4);
 
   // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!!
   GaussianBayesNet actualFragment_QR = *actualFactor_QR.eliminate(3);

gtsam/linear/tests/testHessianFactor.cpp

@@ -113,7 +113,7 @@ TEST(HessianFactor, Constructor1)
   HessianFactor factor(0, G, g, f);
 
   // extract underlying parts
-  MatrixColMajor info_matrix = factor.raw_info().range(0, 1, 0, 1);
+  MatrixColMajor info_matrix = factor.info_.range(0, 1, 0, 1);
   EXPECT(assert_equal(MatrixColMajor(G), info_matrix));
   EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10);
   EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10));

gtsam/linear/tests/testNoiseModel.cpp

@@ -231,12 +231,12 @@ TEST(NoiseModel, Cholesky)
   SharedDiagonal expected = noiseModel::Unit::Create(4);
   MatrixColMajor Ab = exampleQR::Ab; // otherwise overwritten !
   SharedDiagonal actual = exampleQR::diagonal->Cholesky(Ab, 4); // ASSERTION FAILURE: access out of bounds
-//  EXPECT(assert_equal(*expected,*actual));
+  EXPECT(assert_equal(*expected,*actual));
   // Ab was modified in place !!!
-//  MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
+  MatrixColMajor actualRd = Ab.block(0, 0, actual->dim(), Ab.cols()).triangularView<Eigen::Upper>();
 //      ublas::project(ublas::triangular_adaptor<MatrixColMajor, ublas::upper>(Ab),
 //          ublas::range(0,actual->dim()), ublas::range(0, Ab.cols()));
-//  EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4));
+  EXPECT(linear_dependent(exampleQR::Rd,actualRd,1e-4)); // FIXME: enable test
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testVectorBTree.cpp

@@ -19,7 +19,6 @@
 #include <sstream>
 #include <limits>
 
-#include <boost/numeric/ublas/vector_proxy.hpp>
 #include <boost/assign/std/vector.hpp>
 using namespace boost::assign; // bring 'operator+=()' into scope
 

gtsam/nonlinear/NonlinearFactor.h

@@ -36,14 +36,6 @@
 
 #include <gtsam/nonlinear/Ordering.h>
 
-// FIXME: is this necessary?
-#define INSTANTIATE_NONLINEAR_FACTOR1(C,J) \
-  template class gtsam::NonlinearFactor1<C,J>;
-#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,J2) \
-    template class gtsam::NonlinearFactor2<C,J1,J2>;
-#define INSTANTIATE_NONLINEAR_FACTOR3(C,J1,J2,J3) \
-    template class gtsam::NonlinearFactor3<C,J1,J2,J3>;
-
 namespace gtsam {
 
 	/**