From 15bb00683a9952b4c5a2d95dd36bc3f2786fa8df Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 30 Dec 2009 13:20:16 +0000
Subject: [PATCH] transpose backsubstitute on upper-triangular matrix

---
 cpp/Matrix.cpp     | 36 ++++++++++++++++++++++++++++--------
 cpp/Matrix.h       | 18 ++++++++++++++----
 cpp/testMatrix.cpp | 20 ++++++++++----------
 3 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 98b90b895..50e0274c2 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -414,11 +414,31 @@ Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit) {
 
 	Vector result(n);
 	for (size_t i = n; i > 0; i--) {
-		double tmp = b(i-1);
+		double zi = b(i-1);
 		for (size_t j = i+1; j <= n; j++)
-			tmp -= U(i-1,j-1) * result(j-1);
-		if (!unit) tmp /= U(i-1,i-1);
-		result(i-1) = tmp;
+			zi -= U(i-1,j-1) * result(j-1);
+		if (!unit) zi /= U(i-1,i-1);
+		result(i-1) = zi;
+	}
+
+	return result;
+}
+
+/* ************************************************************************* */
+Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit) {
+	size_t m = U.size1(), n = U.size2();
+#ifndef NDEBUG
+	if (m!=n)
+		throw invalid_argument("backSubstituteUpper: U must be square");
+#endif
+
+	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 -= U(j-1,i-1) * result(j-1);
+		if (!unit) zi /= U(i-1,i-1);
+		result(i-1) = zi;
 	}
 
 	return result;
@@ -434,11 +454,11 @@ Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit) {
 
 	Vector result(n);
 	for (size_t i = 1; i <= n; i++) {
-		double tmp = b(i-1);
+		double zi = b(i-1);
 		for (size_t j = 1; j < i; j++)
-			tmp -= L(i-1,j-1) * result(j-1);
-		if (!unit) tmp /= L(i-1,i-1);
-		result(i-1) = tmp;
+			zi -= L(i-1,j-1) * result(j-1);
+		if (!unit) zi /= L(i-1,i-1);
+		result(i-1) = zi;
 	}
 
 	return result;
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index 072afb5c0..202369885 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -209,18 +209,28 @@ void householder(Matrix& A, size_t k);
 /**
  * backSubstitute U*x=b
  * @param U an upper triangular matrix
- * @param b a RHS vector
- * @param unit, set tru if unit triangular
+ * @param b an RHS vector
+ * @param unit, set true if unit triangular
  * @return the solution x of U*x=b
  * TODO: use boost
  */
 Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
 
+/**
+ * backSubstitute x'*U=b'
+ * @param U an upper triangular matrix
+ * @param b an RHS vector
+ * @param unit, set true if unit triangular
+ * @return the solution x of x'*U=b'
+ * TODO: use boost
+ */
+Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
+
 /**
  * backSubstitute L*x=b
  * @param L an lower triangular matrix
- * @param b a RHS vector
- * @param unit, set tru if unit triangular
+ * @param b an RHS vector
+ * @param unit, set true if unit triangular
  * @return the solution x of L*x=b
  * TODO: use boost
  */ 
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 283588129..df8d8c1bc 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -391,29 +391,29 @@ TEST( matrix, backsubtitution )
 {
 	// TEST ONE  2x2 matrix U1*x=b1
 	Vector expected1 = Vector_(2, 3.6250, -0.75);
-	Matrix U1 = Matrix_(2, 2,
+	Matrix U22 = Matrix_(2, 2,
 			2., 3.,
 			0., 4.);
-	Vector b1 = U1*expected1;
-	CHECK( assert_equal(expected1 , backSubstituteUpper(U1, b1), 0.000001));
+	Vector b1 = U22*expected1;
+	CHECK( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
 
 	// TEST TWO  3x3 matrix U2*x=b2
 	Vector expected2 = Vector_(3, 5.5, -8.5, 5.);
-	Matrix U2 = Matrix_(3, 3,
+	Matrix U33 = Matrix_(3, 3,
 			3., 5., 6.,
 			0., 2., 3.,
 			0., 0., 1.);
-	Vector b2 = U2*expected2;
-	CHECK( assert_equal(expected2 , backSubstituteUpper(U2, b2), 0.000001));
+	Vector b2 = U33*expected2;
+	CHECK( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
 
 	// TEST THREE  Lower triangular 3x3 matrix L3*x=b3
 	Vector expected3 = Vector_(3, 1., 1., 1.);
-	Matrix L3 = Matrix_(3, 3,
-			3., 0., 0.,
-			5., 2., 0.,
-			6., 3., 1.);
+	Matrix L3 = trans(U33);
 	Vector b3 = L3*expected3;
 	CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
+
+	// TEST FOUR Try the above with transpose backSubstituteUpper
+	CHECK( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
 }
 
 /* ************************************************************************* */