some matlab changes and modified Guassian:QR to mimic Constrained::QR and deleted DenseQR related stuff

base/Makefile.am

@@ -18,7 +18,6 @@ sources += Vector.cpp svdcmp.cpp Matrix.cpp
 check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix 
 
 if USE_LAPACK
-sources += DenseQR.cpp DenseQRUtil.cpp
 check_PROGRAMS += tests/testDenseQRUtil
 endif
 

gtsam-broken.h

@@ -21,13 +21,8 @@ class GaussianFactor {
 			Matrix A3,
 			Vector b_in,
 			const SharedDiagonal& model);
-	void print(string s) const;
-	bool equals(const GaussianFactor& lf, double tol) const;
-	bool empty() const;
-	Vector get_b() const;
-	Matrix get_A(string key) const;
-	double error(const VectorValues& c) const;
 	bool involves(string key) const;
+	Matrix getA(string key) const;
 	pair<Matrix,Vector> matrix(const Ordering& ordering) const;
 	pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
 };

gtsam.h

@@ -17,11 +17,20 @@ class Ordering {
 
 class VectorValues {
   VectorValues();
+  VectorValues(size_t nVars, size_t varDim);
   void print(string s) const;
   bool equals(const VectorValues& expected, double tol) const;
   size_t size() const;
 };
 
+class GaussianFactor {
+	void print(string s) const;
+	bool equals(const GaussianFactor& lf, double tol) const;
+	bool empty() const;
+	Vector getb() const;
+	double error(const VectorValues& c) const;
+};
+
 class GaussianFactorSet {
   GaussianFactorSet();
   void push_back(GaussianFactor* factor);

linear/NoiseModel.cpp

@@ -120,7 +120,7 @@ void Gaussian::WhitenInPlace(MatrixColMajor& H) const {
 }
 
 // General QR, see also special version in Constrained
-SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZeroRows) const {
+/*SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZeroRows) const {
 
 	// get size(A) and maxRank
 	// TODO: really no rank problems ?
@@ -141,8 +141,83 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<vector<long>&> firstZero
 #endif
 
 return Unit::Create(maxRank);
+}*/
+
+// Special version of QR for Constrained calls slower but smarter code
+// that deals with possibly zero sigmas
+// It is Gram-Schmidt orthogonalization rather than Householder
+// Previously Diagonal::QR
+SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firstZeroRows) const {
+
+	WhitenInPlace(Ab);
+	// get size(A) and maxRank
+	size_t m = Ab.size1(), n = Ab.size2()-1;
+	size_t maxRank = min(m,n);
+
+	// create storage for [R d]
+	typedef boost::tuple<size_t, Vector, double> Triple;
+	list<Triple> Rd;
+
+	Vector pseudo(m); // allocate storage for pseudo-inverse
+	Vector weights = ones(m); // calculate weights once
+
+	// We loop over all columns, because the columns that can be eliminated
+	// are not necessarily contiguous. For each one, estimate the corresponding
+	// scalar variable x as d-rS, with S the separator (remaining columns).
+	// Then update A and b by substituting x with d-rS, zero-ing out x's column.
+	for (size_t j=0; j<n; ++j) {
+		// extract the first column of A
+		// ublas::matrix_column is slower ! TODO Really, why ????
+		//  AGC: if you use column() you will automatically call ublas, use
+		//      column_() to actually use the one in our library
+		Vector a(column(Ab, j));
+
+		// Calculate weighted pseudo-inverse and corresponding precision
+		double precision = weightedPseudoinverse(a, weights, pseudo);
+
+		// If precision is zero, no information on this column
+		// This is actually not limited to constraints, could happen in Gaussian::QR
+		// In that case, we're probably hosed. TODO: make sure Householder is rank-revealing
+		if (precision < 1e-8) continue;
+
+		// create solution [r d], rhs is automatically r(n)
+		Vector rd(n+1); // uninitialized !
+		rd(j)=1.0; // put 1 on diagonal
+		for (size_t j2=j+1; j2<n+1; ++j2) // and fill in remainder with dot-products
+			rd(j2) = inner_prod(pseudo, ublas::matrix_column<Matrix>(Ab, j2));
+
+		// construct solution (r, d, sigma)
+		Rd.push_back(boost::make_tuple(j, rd, precision));
+
+		// exit after rank exhausted
+		if (Rd.size()>=maxRank) break;
+
+		// update Ab, expensive, using outer product
+		updateAb(Ab, j, a, rd);
 	}
 
+	// Create storage for precisions
+	Vector precisions(Rd.size());
+
+	// Write back result in Ab, imperative as we are
+	// TODO: test that is correct if a column was skipped !!!!
+	size_t i = 0; // start with first row
+	bool mixed = false;
+	BOOST_FOREACH(const Triple& t, Rd) {
+		const size_t& j  = t.get<0>();
+		const Vector& rd = t.get<1>();
+		precisions(i)    = t.get<2>();
+		if (precisions(i)==inf) mixed = true;
+		for (size_t j2=0; j2<j; ++j2) Ab(i,j2) = 0.0; // fill in zeros below diagonal anway
+		for (size_t j2=j; j2<n+1; ++j2) // copy the j-the row TODO memcpy
+			Ab(i,j2) = rd(j2);
+		i+=1;
+	}
+
+	return Unit::Create(maxRank);
+}
+
+
 // General QR, see also special version in Constrained
 SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
 

matlab/tests/createZeroDelta.m

@@ -3,7 +3,7 @@ function c = createZeroDelta()
 v_x1 = [0; 0];
 v_x2 = [0; 0];
 v_l1 = [0; 0];
-c = VectorConfig();
+c = VectorValues();
 c.insert('x1', v_x1);
 c.insert('x2', v_x2);
 c.insert('l1', v_l1);