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

const reference and minor updates

release/4.3a0
Varun Agrawal 2020-05-07 15:18:48 -04:00
parent 44ec8ba8c0
commit 620ca2bd3a
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/NoiseModel.cpp
View File

@ -136,20 +136,18 @@ bool Gaussian::equals(const Base& expected, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&expected); const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
if (p == NULL) return false; if (p == NULL) return false;
if (typeid(*this) != typeid(*p)) return false; if (typeid(*this) != typeid(*p)) return false;
//TODO(Alex);
//if (!sqrt_information_) return true;
return equal_with_abs_tol(R(), p->R(), sqrt(tol)); return equal_with_abs_tol(R(), p->R(), sqrt(tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Gaussian::covariance() const { Matrix Gaussian::covariance() const {
// Uses a fast version of `covariance = information().inverse();` // Uses a fast version of `covariance = information().inverse();`
Matrix R = this->R(); const Matrix& R = this->R();
Matrix I = Matrix::Identity(R.rows(), R.cols()); Matrix I = Matrix::Identity(R.rows(), R.cols());
// Fast inverse of upper-triangular matrix R using forward-substitution // Fast inverse of upper-triangular matrix R using forward-substitution
Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I); Matrix Rinv = R.triangularView<Eigen::Upper>().solve(I);
// (R' * R)^{-1} = R^{-1} * R^{-1}' // (R' * R)^{-1} = R^{-1} * R^{-1}'
return (Rinv * Rinv.transpose()); return Rinv * Rinv.transpose();
} }
/* ************************************************************************* */ /* ************************************************************************* */