From 739764ca8e6ca40d98fad6e51bb9ab6f16a2819d Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 19 Nov 2010 17:19:26 +0000
Subject: [PATCH] linear_dependent prints when not linearly dependent, like
 assert_equal.  Added linear_independent to check the opposite.

---
 gtsam/base/Matrix.cpp           | 44 ++++++++++++++++++++++++++++-----
 gtsam/base/Matrix.h             |  7 +++++-
 gtsam/base/tests/testMatrix.cpp |  2 +-
 3 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 08d09b809..56516d735 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -175,17 +175,49 @@ bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, doub
 }
 
 /* ************************************************************************* */
-bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
+static bool is_linear_dependent(const Matrix& A, const Matrix& B, double tol) {
+  // This local static function is used by linear_independent and
+  // linear_dependent just below.
   size_t n1 = A.size2(), m1 = A.size1();
   size_t n2 = B.size2(), m2 = B.size1();
 
-  if(m1!=m2 || n1!=n2) return false;
+  bool dependent = true;
+  if(m1!=m2 || n1!=n2) dependent = false;
 
-  for(size_t i=0; i<m1; i++) {
-  	if (!gtsam::linear_dependent(row_(A,i), row_(B,i), tol))
-  		return false;
+  for(size_t i=0; dependent && i<m1; i++) {
+    if (!gtsam::linear_dependent(row_(A,i), row_(B,i), tol))
+      dependent = false;
+  }
+
+  return dependent;
+}
+
+/* ************************************************************************* */
+bool linear_independent(const Matrix& A, const Matrix& B, double tol) {
+  if(!is_linear_dependent(A, B, tol))
+    return true;
+  else {
+    cout << "not linearly dependent:" << endl;
+    print(A,"A = ");
+    print(B,"B = ");
+    if(A.size1()!=B.size1() || A.size2()!=B.size2())
+      cout << A.size1() << "x" << A.size2() << " != " << B.size1() << "x" << B.size2() << endl;
+    return false;
+  }
+}
+
+/* ************************************************************************* */
+bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
+  if(is_linear_dependent(A, B, tol))
+    return true;
+  else {
+    cout << "not linearly dependent:" << endl;
+    print(A,"A = ");
+    print(B,"B = ");
+    if(A.size1()!=B.size1() || A.size2()!=B.size2())
+      cout << A.size1() << "x" << A.size2() << " != " << B.size1() << "x" << B.size2() << endl;
+    return false;
   }
-  return true;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 7815161b0..7219e1f17 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -99,7 +99,12 @@ bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
 bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol = 1e-9);
 
 /**
- * check whether the rows of two matrices are linear indepdent
+ * check whether the rows of two matrices are linear independent
+ */
+bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);
+
+/**
+ * check whether the rows of two matrices are linear dependent
  */
 bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
 
diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index 60e61781a..0785102db 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/gtsam/base/tests/testMatrix.cpp
@@ -882,7 +882,7 @@ TEST( matrix, linear_dependent3 )
 {
 	Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0);
 	Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0);
-	CHECK(!linear_dependent(A, B));
+	CHECK(linear_independent(A, B));
 }
 
 /* ************************************************************************* */