Added GaussianFactor::rhs(Permuted<VectorValues>) function for filling the rhs into the soln that works on permuted values.

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;

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.