Add Feature::S() method.

This is a method for computing S for an independent measurement. It can
provide a speed improvement over computing a full, correlated S, but may
reduce quality if correlated measurements are treated as independent.

1 files changed, 5 insertions, 7 deletions

diff --git a/src/feature.h b/src/feature.h
index 971c952..2a492c2 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -5,6 +5,7 @@
 
 using Eigen::Vector3d;
 using Eigen::Matrix;
+using Eigen::MatrixXd;
 using Eigen::Quaterniond;
 
 /*
@@ -32,13 +33,10 @@ class Feature
         Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         Matrix<double,3,6> L();
 
-        /*
-        Matrix<double,9,9> Q(double dt);
-        Matrix<double,1,1> R();
-        Matrix<double,1,1> S(const Matrix<double,9,9> &P);
-        void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
-        void unicsv();
-        */
+        Matrix<double,3,3> Q(const double dt);
+        Matrix<double,6,6> R();
+        Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
+            const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
 
     protected:
         /* ====================  METHODS       ======================================= */