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

Cut out middle-man, added noalias for better performance

release/4.3a0
Frank Dellaert 2019-04-10 22:26:34 -04:00
parent d8570ce09b
commit 35d9f65d9c
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/GaussianConditional.cpp
View File

@ -172,13 +172,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); frontalVec = get_R().triangularView<Eigen::Upper>().solve<Eigen::OnTheLeft>(frontalVec);
// Check for indeterminant solution // Check for indeterminant solution
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++) for (const_iterator it = beginParents(); it!= endParents(); it++)
gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec;
// Scale by sigmas // Scale by sigmas
if (model_) if (model_)