Begin updating Feature to use FULLSTATE

2 files changed, 40 insertions, 5 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index a75fdf0..c9cdda1 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -130,10 +130,19 @@ Feature::findDepth ( const Quaterniond &qbw, double z, const Vector3d &xs,
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,6,6>
+#ifdef FULLSTATE
+Feature::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
 Feature::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
 {
+#ifdef FULLSTATE
+    Matrix<double,6,13> hx;
+#else
     Matrix<double,6,9> hx;
+#endif
     Matrix<double,6,3> hy;
     hx = Hx( pos, q);
     hy = Hy( pos, q);
@@ -341,17 +350,28 @@ Feature::x2p ( const Vector3d &x )
  * body.
  *--------------------------------------------------------------------------------------
  */
+#ifdef FULLSTATE
+Matrix<double,3,13>
+#else
 Matrix<double,3,9>
+#endif
 Feature::Fx ( double dt )
 {
     double y0,y1,y2;
     y0 = X[0];
     y1 = X[1];
     y2 = X[2];
+#ifdef FULLSTATE
+    Matrix<double,3,13> F;
+    F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0,0,0,0,0,
+         0.,0.,0.,   y1*y2, 0.,-y2, 0,0,0,0,0,0,0,
+         0,0,0,   y2*y2,    0,   0, 0,0,0,0,0,0,0;
+#else
     Matrix<double,3,9> F;
     F << 0., 0., 0., y0*y2,-y2, 0.,0,0,0,
          0.,0.,0.,   y1*y2, 0.,-y2, 0,0,0,
          0,0,0,   y2*y2,    0,   0, 0,0,0;
+#endif
     F *= dt;
     return F;
 }		/* -----  end of method Feature::Fx  ----- */
@@ -388,7 +408,11 @@ Feature::Fy ( const Vector3d &v, const Vector3d &w, double dt )
     return F;
 }		/* -----  end of method Feature::Fy  ----- */
 
+#ifdef FULLSTATE
+Matrix<double,6,13>
+#else
 Matrix<double,6,9>
+#endif
 Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
 {
     double xbw1,xbw2,xbw3;
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);