fixed a bug in matrix solve function

cpp/Matrix.cpp

@@ -216,11 +216,18 @@ Matrix sub(const Matrix& A, size_t i1, size_t i2, size_t j1, size_t j2) {
 /* ************************************************************************* */
 void solve(Matrix& A, Matrix& B)
 {
-  // perform LU-factorization
-  ublas::lu_factorize(A);
+	typedef ublas::permutation_matrix<std::size_t> pmatrix;
+	// create a working copy of the input
+	Matrix A_(A);
+	// create a permutation matrix for the LU-factorization
+	pmatrix pm(A_.size1());
 
-  // backsubstitute
-  ublas::lu_substitute<const Matrix, Matrix>(A, B);
+	// perform LU-factorization
+	int res = lu_factorize(A_,pm);
+	if( res != 0 ) throw runtime_error ("Matrix::solve: lu_factorize failed!");
+
+	// backsubstitute to get the inverse
+	lu_substitute(A_, pm, B);
 }
 
 /* ************************************************************************* */

cpp/testMatrix.cpp

@@ -428,6 +428,24 @@ TEST( matrix, inverse )
   CHECK(assert_equal(expected, Ainv, 1e-4));
 }
 
+/* ************************************************************************* */
+TEST( matrix, inverse2 )
+{
+  Matrix A(3,3);
+  A(0,0)= 0;  A(0,1)=-1; A(0,2)=1;
+  A(1,0)= 1;  A(1,1)= 0; A(1,2)=2;
+  A(2,0)= 0;  A(2,1)= 0; A(2,2)=1;
+
+  Matrix Ainv = inverse(A);
+
+  Matrix expected(3,3);
+  expected(0,0)= 0;   expected(0,1)=1; expected(0,2)=-2;
+  expected(1,0)=-1;   expected(1,1)=0; expected(1,2)= 1;
+  expected(2,0)= 0;   expected(2,1)=0; expected(2,2)= 1;
+
+  CHECK(assert_equal(expected, Ainv, 1e-4));
+}
+
 /* ************************************************************************* */
 TEST( matrix, backsubtitution )
 {

cpp/testPose2.cpp

@@ -203,6 +203,8 @@ TEST( Pose2, matrix )
   // check inverse pose
   Matrix _1Mg = matrix(inverse(gT1));
   CHECK(assert_equal(inverse(gM1),_1Mg));
+  CHECK(assert_equal(eye(3),inverse(_1Mg)*_1Mg));
+  CHECK(assert_equal(eye(3),inverse(gM1)*gM1));
 }
 
 /* ************************************************************************* */