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

make covariance() use fast triangular inverse and use that for sigmas

release/4.3a0
Varun Agrawal 2020-03-27 23:22:42 -04:00
parent a59786c809
commit 44ec8ba8c0
2 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/linear/NoiseModel.cpp
View File

@ -141,13 +141,20 @@ bool Gaussian::equals(const Base& expected, double tol) const {
return equal_with_abs_tol(R(), p->R(), sqrt(tol));
}
/* ************************************************************************* */
Matrix Gaussian::covariance() const {
// Uses a fast version of `covariance = information().inverse();`
Matrix R = this->R();
Matrix I = Matrix::Identity(R.rows(), R.cols());
// Fast inverse of upper-triangular matrix R using forward-substitution
Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
// (R' * R)^{-1} = R^{-1} * R^{-1}'
return (Rinv * Rinv.transpose());
}
/* ************************************************************************* */
Vector Gaussian::sigmas() const {
Matrix R = thisR();
// fast inverse of upper-triangular matrix
// Alternate for Matrix Rinv = R.inverse();
Matrix Rinv = R.triangularView<Eigen::Upper>().solve(Matrix::Identity(R.rows(), R.cols()));
return Vector((Rinv * Rinv.transpose()).diagonal()).cwiseSqrt();
return Vector(covariance().diagonal()).cwiseSqrt();
}
/* ************************************************************************* */

2
gtsam/linear/NoiseModel.h
View File

@ -257,7 +257,7 @@ namespace gtsam {
virtual Matrix information() const { return R().transpose() * R(); }
/// Compute covariance matrix
virtual Matrix covariance() const { return information().inverse(); }
virtual Matrix covariance() const;
private:
/** Serialization function */