From c43fc2a95a878ab41dc3f23e0295e180d584181a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 9 Aug 2011 21:06:38 +0000
Subject: [PATCH] Added GaussianFactor::rhs(Permuted<VectorValues>) function
 for filling the rhs into the soln that works on permuted values.

---
 gtsam/linear/GaussianConditional.cpp | 13 +++++++++++++
 gtsam/linear/GaussianConditional.h   |  6 ++++++
 2 files changed, 19 insertions(+)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index 959cfb41f..3e2f4684c 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -155,6 +155,19 @@ void GaussianConditional::rhs(VectorValues& x) const {
 	x.range(beginFrontals(), endFrontals(), d);
 }
 
+/* ************************************************************************* */
+void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
+  // Copy the rhs into x, accounting for the permutation
+  Vector d = rhs();
+  size_t rhsPosition = 0;  // We walk through the rhs by variable
+  for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) {
+    // Get the segment of the rhs for this variable
+    x[*j] = d.segment(rhsPosition, this->dim(j));
+    // Increment the position
+    rhsPosition += this->dim(j);
+  }
+}
+
 /* ************************************************************************* */
 void GaussianConditional::solveInPlace(VectorValues& x) const {
   static const bool debug = false;
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index b0dab65f7..9d8379959 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -173,6 +173,12 @@ public:
 	 */
 	void rhs(VectorValues& x) const;
 
+  /**
+   * Adds the RHS to a given VectorValues for use in solve() functions.
+   * @param x is the values to be updated, assumed allocated
+   */
+  void rhs(Permuted<VectorValues>& x) const;
+
   /**
    * solves a conditional Gaussian and stores the result in x
    * This function works for multiple frontal variables.