diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index c6bd3967b..a5c651a44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -119,14 +119,20 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { - // Solve matrix + // Concatenate all vector values that correspond to parent variables Vector xS = x.vector(FastVector(beginParents(), endParents())); + + // Update right-hand-side xS = getb() - get_S() * xS; + + // Solve matrix Vector soln = get_R().triangularView().solve(xS); // Check for indeterminant solution if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); + // TODO, do we not have to scale by sigmas here? Copy/paste bug + // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; @@ -142,9 +148,14 @@ namespace gtsam { VectorValues GaussianConditional::solveOtherRHS( const VectorValues& parents, const VectorValues& rhs) const { + // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); + + // Instead of updating getb(), update the right-hand-side from the given rhs const Vector rhsR = rhs.vector(FastVector(beginFrontals(), endFrontals())); xS = rhsR - get_S() * xS; + + // Solve Matrix Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas