12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Comments

release/4.3a0
dellaert 2014-05-06 01:33:09 -04:00
parent 779d6ad2af
commit 6c6c545d99
1 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/linear/GaussianConditional.cpp
View File

@ -119,14 +119,20 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const VectorValues GaussianConditional::solve(const VectorValues& x) const
{ {
// Solve matrix // Concatenate all vector values that correspond to parent variables
Vector xS = x.vector(FastVector<Key>(beginParents(), endParents())); Vector xS = x.vector(FastVector<Key>(beginParents(), endParents()));
// Update right-hand-side
xS = getb() - get_S() * xS; xS = getb() - get_S() * xS;
// Solve matrix
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Check for indeterminant solution // Check for indeterminant solution
if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); 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 // Insert solution into a VectorValues
VectorValues result; VectorValues result;
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
@ -142,9 +148,14 @@ namespace gtsam {
VectorValues GaussianConditional::solveOtherRHS( VectorValues GaussianConditional::solveOtherRHS(
const VectorValues& parents, const VectorValues& rhs) const const VectorValues& parents, const VectorValues& rhs) const
{ {
// Concatenate all vector values that correspond to parent variables
Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents())); Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents()));
// Instead of updating getb(), update the right-hand-side from the given rhs
const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals())); const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS; xS = rhsR - get_S() * xS;
// Solve Matrix
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS); Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
// Scale by sigmas // Scale by sigmas