Begin updating Feature to use FULLSTATE

1 files changed, 16 insertions, 5 deletions

diff --git a/src/feature.h b/src/feature.h
index db4f05e..6260d71 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -58,16 +58,30 @@ class Feature
 
         /* ====================  OPERATORS     ======================================= */
         bool sane();
+#ifdef FULLSTATE
+        bool isInlier(const measurement_t &z, const Matrix<double,13,13> &Pxx,
+                const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy,
+                const Vector3d &pos, const Quaterniond &q, double thr);
+        Matrix<double,3,13> Fx( double dt );
+        Matrix<double,6,13> Hx( const Vector3d &pos, const Quaterniond &q);
+        Matrix<double,6,6> S ( const Matrix<double,13,13> &Pxx, 
+                const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy, 
+                const Vector3d &pos, const Quaterniond &q);
+#else
         bool isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx,
                 const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy,
                 const Vector3d &pos, const Quaterniond &q, double thr);
+        Matrix<double,3,9> Fx( double dt );
+        Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
+        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);
+#endif
         bool isRansacInlier(const measurement_t &z, const Vector3d &pos, const Quaterniond &q);
         bool inFOV();
         Vector3d findDepth( const Quaterniond &q, double z, const Vector3d &xs,
                 const Vector3d &xr);
-        Matrix<double,3,9> Fx( double dt );
         Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
-        Matrix<double,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
         Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
         Matrix<double,3,6> L();
@@ -76,9 +90,6 @@ class Feature
 
         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);
         Vector3d p2x(const Vector3d &p);
         Vector3d x2p(const Vector3d &x);