From 0c8912c5dc1ec3c752f8648ca2a8181275ba2654 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 4 Apr 2017 14:07:24 -0500
Subject: Use STATESIZE for Feature

Replace FULLSTATE define with STATESIZE.
---
 src/feature.cpp | 67 +++++++++++++++------------------------------------------
 1 file changed, 17 insertions(+), 50 deletions(-)

(limited to 'src/feature.cpp')

diff --git a/src/feature.cpp b/src/feature.cpp
index 8f6fd12..9c6bf84 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -130,19 +130,10 @@ 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,
+Feature::S ( const Matrix<double,STATESIZE,STATESIZE> &Pxx, const Matrix<double,STATESIZE,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,STATESIZE> hx;
     Matrix<double,6,3> hy;
     hx = Hx( pos, q);
     hy = Hy( pos, q);
@@ -350,28 +341,18 @@ Feature::x2p ( const Vector3d &x )
  * body.
  *--------------------------------------------------------------------------------------
  */
-#ifdef FULLSTATE
-Matrix<double,3,13>
-#else
-Matrix<double,3,9>
-#endif
+Matrix<double,3,STATESIZE>
 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
+    Matrix<double,3,STATESIZE> F;
+    F = Matrix<double,3,STATESIZE>::Zero();
+    F.block<3,3>(0,3) << y0*y2,-y2, 
+                         y1*y2, 0.,-y2, 
+                         y2*y2,  0,   0;
     F *= dt;
     return F;
 }		/* -----  end of method Feature::Fx  ----- */
@@ -408,11 +389,7 @@ 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
+Matrix<double,6,STATESIZE>
 Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
 {
     double xbw1,xbw2,xbw3;
@@ -444,13 +421,8 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
     qb0w3=q0.z();
     qb0w4=q0.w();
 
-#ifdef FULLSTATE
-    Matrix<double,Dynamic,13,0,6,13> H;
-    H = Matrix<double,6,13>::Zero();
-#else
-    Matrix<double,Dynamic,9,0,6,9> H;
-    H = Matrix<double,6,9>::Zero();
-#endif
+    Matrix<double,Dynamic,STATESIZE,0,6,STATESIZE> H;
+    H = Matrix<double,6,STATESIZE>::Zero();
 
     // Matlab generated C code.
     double t2,t3,t4,t5,t6,t7,t8,t9,t10,t11,t12,t13,t14,t15,t16,t17,t18,t19,t20,
@@ -705,7 +677,7 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
     H(2,0) = -t47/(t24+t26+t28+t46-t12*(t30+t32+t11*(t22-qbw2*qbw4*2.0))-pib1*t12*t39)-t7*t60*t75;
     H(2,1) = t52*t76-t9*t60*t75;
     H(2,2) = -t50*t76-t11*t60*t75;
-#ifdef FULLSTATE
+#if STATESIZE==13
     H(2,6) = -t76*(t12*t87+pib1*t12*t84+pib2*t12*t97)-t60*t75*t137;
     H(2,7) = t76*(t12*t84-pib1*t12*t87+pib2*t12*t101)-t60*t75*t146;
     H(2,8) = -t76*(-t12*t97+pib2*t12*t87+pib1*t12*t101)-t60*t75*t157;
@@ -714,21 +686,21 @@ Feature::Hx ( const Vector3d &pos, const Quaterniond &q )
     H(3,0) = -t76*t113-t7*t60*t130;
     H(3,1) = t76*t114-t9*t60*t130;
     H(3,2) = t76*t115-t11*t60*t130;
-#ifdef FULLSTATE
+#if STATESIZE==13
     H(3,6) = -t76*(-t12*t143+pib1*t12*t140+pib2*t12*t150)-t60*t130*t137;
     H(3,7) = t76*(t12*t140+pib1*t12*t143-pib2*t12*t154)-t60*t130*t146;
     H(3,8) = t76*(t12*t150+pib2*t12*t143+pib1*t12*t154)-t60*t130*t157;
     H(3,9) = -t76*(t12*t154+pib2*t12*t140-pib1*t12*t150)-t60*t130*t163;
 #endif
     H(4,2) = t181*(qbw1*qbw4*4.0+qbw2*qbw3*4.0)+t182*t185*t213;
-#ifdef FULLSTATE
+#if STATESIZE==13
     H(4,6) = -t181*(t203+t204-qbw4*t167*2.0+t36*t188-t34*t191+t37*t195)+t182*t185*t220;
     H(4,7) = t181*(t197+t198-qbw1*t176*2.0-t37*t188+t36*t195-t34*t202)-t182*t185*t225;
     H(4,8) = t181*(t210+t211-qbw3*t171*2.0+t37*t191+t34*t195+t36*t202)+t182*t185*t229;
     H(4,9) = t181*(t207+t208+t209+t34*t188+t36*t191-t37*t202)-t182*t185*t232;
 #endif
     H(5,2) = -t181*(t14*2.0+t15*2.0-t16*2.0-t17*2.0)+t182*t213*t217;
-#ifdef FULLSTATE
+#if STATESIZE==13
     H(5,6) = -t181*(t207+t208+t209+t40*t188+t42*t191+t44*t195)+t182*t217*t220;
     H(5,7) = -t181*(t210+t211-qbw3*t171*2.0+t44*t188-t40*t195-t42*t202)-t182*t217*t225;
     H(5,8) = t181*(t197+t198-t226+t44*t191-t42*t195+t40*t202)+t182*t217*t229;
@@ -1080,13 +1052,8 @@ Feature::inFOV ( )
 }		/* -----  end of method Feature::inFOV  ----- */
 
 bool
-#ifdef FULLSTATE
-Feature::isInlier(const measurement_t &z, const Matrix<double,13,13> &Pxx,
-        const Matrix<double,13,3> &Pxy, const Matrix<double,3,3> &Pyy,
-#else
-Feature::isInlier(const measurement_t &z, const Matrix<double,9,9> &Pxx,
-        const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy,
-#endif
+Feature::isInlier(const measurement_t &z, const Matrix<double,STATESIZE,STATESIZE> &Pxx,
+        const Matrix<double,STATESIZE,3> &Pxy, const Matrix<double,3,3> &Pyy,
         const Vector3d &pos, const Quaterniond &q, double thr)
 {
     Matrix<double,Dynamic,Dynamic,0,6,6> s;
-- 
cgit v1.1