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

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<Eigen::Upper>().solve(I);
+  // (R' * R)^{-1} = R^{-1} * R^{-1}'
+  return (Rinv * Rinv.transpose());
+}
+
 /* ************************************************************************* */
 Vector Gaussian::sigmas() const {
-  Matrix R = thisR();
+  return Vector(covariance().diagonal()).cwiseSqrt();
-  // 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();
 }
 
 /* ************************************************************************* */

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 */