diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 3ecb274de..915550d22 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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().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().solve(Matrix::Identity(R.rows(), R.cols())); - return Vector((Rinv * Rinv.transpose()).diagonal()).cwiseSqrt(); + return Vector(covariance().diagonal()).cwiseSqrt(); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6c42fc7ba..dda780a36 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -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 */