From c6da9d6e19de44b0c294e2caafd6ccddb1835540 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 15 Aug 2011 20:24:32 +0000
Subject: [PATCH] Added unit test for solving with permuted solution vector

---
 gtsam/linear/GaussianConditional.cpp          |  2 +-
 .../linear/tests/testGaussianConditional.cpp  | 50 +++++++++++++++++++
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 3e2f4684c..959f2d751 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -212,7 +212,7 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
 	// apply solution: inlined manually due to permutation
   size_t solnStart = 0;
   for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
-  	const size_t d = dim(frontal);
+  	const size_t d = this->dim(frontal);
   	x[*frontal] = soln.segment(solnStart, d);
   	solnStart += d;
   }
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index e8eb57d05..a04562653 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -294,6 +294,56 @@ TEST( GaussianConditional, solve_multifrontal )
 
 }
 
+/* ************************************************************************* */
+TEST( GaussianConditional, solve_multifrontal_permuted )
+{
+  // create full system, 3 variables, 2 frontals, all 2 dim
+  // no pivoting from LDL, so R matrix is not permuted
+  Matrix full_matrix = Matrix_(4, 7,
+      1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
+      0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
+      0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
+      0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
+
+  // 3 variables, all dim=2
+  vector<size_t> dims; dims += 2, 2, 2, 1;
+  GaussianConditional::rsd_type matrices(full_matrix, dims.begin(), dims.end());
+  Vector sigmas = ones(4);
+  vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
+  GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
+
+  EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
+
+  // partial solution
+  Vector sl1 = Vector_(2, 9.0, 10.0);
+
+  // elimination order; _x_, _x1_, _l1_
+  VectorValues actualUnpermuted(vector<size_t>(3, 2));
+  Permutation permutation(3);
+  permutation[0] = 2;
+  permutation[1] = 0;
+  permutation[2] = 1;
+  Permuted<VectorValues> actual(permutation, actualUnpermuted);
+  actual[_x_]  = Vector_(2, 0.1, 0.2); // rhs
+  actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
+  actual[_l1_] = sl1; // parent
+
+  VectorValues expectedUnpermuted(vector<size_t>(3, 2));
+  Permuted<VectorValues> expected(permutation, expectedUnpermuted);
+  expected[_x_] = Vector_(2, -3.1,-3.4);
+  expected[_x1_] = Vector_(2, -11.9,-13.2);
+  expected[_l1_] = sl1;
+
+  // verify indices/size
+  EXPECT_LONGS_EQUAL(3, cg.size());
+  EXPECT_LONGS_EQUAL(4, cg.dim());
+
+  // solve and verify
+  cg.solveInPlace(actual);
+  EXPECT(assert_equal(expected.container(), actual.container(), tol));
+
+}
+
 /* ************************************************************************* */
 TEST( GaussianConditional, solveTranspose ) {
 	static const Index _y_=1;