Update statesize and remove quaternion input.

Rather than use FULLSTATE define, we define STATESIZE, which removes a
lot of the preprocessor ifdefs. This should be done in Body and Feature
also. Quaterniond was removed as an input to methods when STATESIZE==13
and it is instead accessed from the body state.

2 files changed, 139 insertions, 36 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 472ee66..393ffa2 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -20,7 +20,7 @@
 State::State ( )
 {
     body = new Body;
-    P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
+    P = Matrix<double,Dynamic,Dynamic>::Zero(STATESIZE,STATESIZE);
     P.block<3,3>(6,6) = COVBIAS*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
@@ -49,13 +49,20 @@ State::pos ( const UTM &utm )
  *--------------------------------------------------------------------------------------
  */
 MatrixXd
+#if STATESIZE==13
+State::H ( const std::vector<measurement_t> &z )
+#else
 State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     Vector3d pos;
     pos = body->ned();
     MatrixXd h;
     // Determine the size of H
-    int cols = 9 + 3*features.size();
+    int cols = STATESIZE + 3*features.size();
     int rows = Hrows(z);
     h = MatrixXd::Zero(rows,cols);
     for (int i=0,row=0; i<z.size(); ++i) {
@@ -71,7 +78,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
 
                 fi = featureById(z[i].id);
 
-                h.block<6,9>(row,0) = fi->Hx(pos,q);
+                h.block<6,STATESIZE>(row,0) = fi->Hx(pos,q);
                 h.block<6,3>(row,col) = fi->Hy(pos,q);
                 row += 6;
                 break;
@@ -83,11 +90,11 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                     exit(1);
                 }
                 fi = featureById( z[i].id );
-                h.block<4,9>(row,0) = fi->Hx(pos,q).block<4,9>(0,0);
+                h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(0,0);
                 break;
 
             case HEIGHT:	
-                h.block<1,9>(row,0) = body->H();
+                h.block<1,STATESIZE>(row,0) = body->H();
                 row += 1;
                 break;
 
@@ -174,9 +181,17 @@ State::R ( const std::vector<measurement_t> &z )
 }		/* -----  end of method State::R  ----- */
 
 MatrixXd
+#if STATESIZE==13
+State::partialUpdate( const MatrixXd &h, const MatrixXd &S, 
+        const std::vector<measurement_t> &z)
+#else
 State::partialUpdate( const MatrixXd &h, const MatrixXd &S, 
         const std::vector<measurement_t> &z, const Quaterniond &q )
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     MatrixXd K;
     // P^T is implied here since P is symmetric
     K = S.fullPivLu().solve(h*P).transpose();
@@ -188,10 +203,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
-    assert (dx.rows()==9+3*features.size());
-    body->dx(dx.segment<9>(0));
+    assert (dx.rows()==STATESIZE+3*features.size());
+    body->dx(dx.segment<STATESIZE>(0));
     {
-        int row=9;
+        int row=STATESIZE;
         for (auto i=features.begin(); i!=features.end(); ++i, row+=3) {
             (*i)->dx(dx.segment<3>(row));
         }
@@ -207,9 +222,17 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
  *--------------------------------------------------------------------------------------
  */
 void
+#if STATESIZE==13
+State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
+        const std::vector<measurement_t> &z)
+#else
 State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
         const std::vector<measurement_t> &z, const Quaterniond &q)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     MatrixXd K;
     K = partialUpdate(h,S,z,q);
 
@@ -229,8 +252,15 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,Dynamic,1>
+#if STATESIZE==13
+State::innovation( const std::vector<measurement_t> &z)
+#else
 State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     Matrix<double,Dynamic,1> y;
     int rows = Hrows(z);
     y = Matrix<double,Dynamic,1>::Zero(rows,1);
@@ -278,8 +308,15 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
  *--------------------------------------------------------------------------------------
  */
 void
+#if STATESIZE==13
+State::handle_measurements ( const std::vector<measurement_t> &z)
+#else
 State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     std::vector<measurement_t> featuresToAdd;
     std::vector<measurement_t> featuresToUpdate;
     double zmeas = body->ned()[2];
@@ -337,8 +374,15 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
 }		/* -----  end of method State::feature_update  ----- */
 
 MatrixXd
+#if STATESIZE==13
+State::blockSI ( const std::vector<measurement_t> &z)
+#else
 State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q )
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     int rows = Hrows(z);
     MatrixXd SI;
     SI = MatrixXd::Zero(rows,rows);
@@ -384,11 +428,11 @@ State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q )
  * Description:  Return the covariance block for feature with the given id.
  *--------------------------------------------------------------------------------------
  */
-Matrix<double,9,3>
+Matrix<double,STATESIZE,3>
 State::Pxy ( int id )
 {
     int col = rowById(id);
-    return P.block<9,3>(0,col) ;
+    return P.block<STATESIZE,3>(0,col) ;
 }		/* -----  end of method State::Pxy  ----- */
 
 Matrix<double,3,3>
@@ -399,8 +443,14 @@ State::Pyy ( int id )
 }		/* -----  end of method State::Pyy  ----- */
 
 void
-State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double z)
+#if STATESIZE==13
+#else
+State::addFeatures ( std::vector<measurement_t> &F, double z)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q =body->qhat();
+#endif
     // Add new features
     Vector3d pos = body->ned();
     for (auto i=F.begin(); i!=F.end(); ++i) {
@@ -439,7 +489,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
     void
 State::initializePi ( int i, const Matrix<double,3,3> &Pi )
 {
-    int pt=9+3*(i-1);
+    int pt=STATESIZE+3*(i-1);
     P.block(pt,0,3,P.cols()) = MatrixXd::Zero(3,P.cols());
     P.block(0,pt,P.rows(),3) = MatrixXd::Zero(P.rows(),3);
     P.block<3,3>(pt,pt) = Pi;
@@ -458,12 +508,12 @@ MatrixXd
 State::Q ( double dt )
 {
     MatrixXd q;
-    int rows = 9 + 3*features.size();
+    int rows = STATESIZE + 3*features.size();
     q = MatrixXd::Zero(rows,rows);
-    q.topLeftCorner<9,9>() = body->Q(dt);
+    q.topLeftCorner<STATESIZE,STATESIZE>() = body->Q(dt);
     { // limit i's scope
         auto i = features.begin();
-        for (int row=9; row<rows; row+=3, ++i) {
+        for (int row=STATESIZE; row<rows; row+=3, ++i) {
             q.block<3,3>(row,row) = (*i)->Q(dt);
         }
     }
@@ -478,22 +528,29 @@ State::Q ( double dt )
  *--------------------------------------------------------------------------------------
  */
 MatrixXd
+#if STATESIZE==13
+State::F ( const Vector3d &w, double dt )
+#else
 State::F ( const Quaterniond &q, const Vector3d &w, double dt )
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     Vector3d v;
     v = body->vel();
     // Allocate matrix F
     MatrixXd f;
-    int rows = 9 + 3*features.size();
+    int rows = STATESIZE + 3*features.size();
     f = MatrixXd::Zero(rows,rows);
 
     // Set body F
-    f.topLeftCorner<9,9>() = body->F(w,q,dt);
+    f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,q,dt);
     // Set Fxi Fyi
     { // limit i's scope
         auto i = features.begin();
-        for (int row=9; row<rows; row+=3,++i) {
-            f.block<3,9>(row,0) = (*i)->Fx(dt);
+        for (int row=STATESIZE; row<rows; row+=3,++i) {
+            f.block<3,STATESIZE>(row,0) = (*i)->Fx(dt);
             f.block<3,3>(row,row) = (*i)->Fy(v,w,dt);
         }
     }
@@ -509,8 +566,15 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt )
  *--------------------------------------------------------------------------------------
  */
 void
+#if STATESIZE==13
+State::Pkk1 ( const Vector3d &ang, const double dt)
+#else
 State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     MatrixXd f,fullQ;
     fullQ = Q(dt);
     f = F ( q, ang, dt );
@@ -521,9 +585,16 @@ State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 }		/* -----  end of method State::Pkk1  ----- */
 
 void
+#if STATESIZE==13
+State::motionModel ( const Vector3d &acc, const Vector3d &ang, const double dt)
+#else
 State::motionModel ( const Vector3d &acc, const Vector3d &ang,
     const Quaterniond &q, const double dt)
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     Pkk1(ang,q,dt);
     body->motionModel(acc,ang,q,dt);
 
@@ -710,7 +781,7 @@ State::exists ( int id )
 int
 State::rowById ( int id )
 {
-    int j=9;
+    int j=STATESIZE;
     for (auto i=features.begin(); i!=features.end(); ++i, j+=3) {
         if ((*i)->id()==id) return j;
     }
@@ -741,10 +812,10 @@ State::expandP ( const Matrix3d &Pi)
     return ;
 }		/* -----  end of method State::expandP  ----- */
 
-Matrix<double,9,9>
+Matrix<double,STATESIZE,STATESIZE>
 State::Pxx ( )
 {
-    return P.topLeftCorner<9,9>() ;
+    return P.topLeftCorner<STATESIZE,STATESIZE>() ;
 }		/* -----  end of method State::Pxx  ----- */
 
 /*
@@ -757,8 +828,8 @@ State::Pxx ( )
 void
 State::shrinkP ( int i )
 {
-    int N = 9 + 3*features.size();
-    int I = 9 + 3*i;
+    int N = STATESIZE + 3*features.size();
+    int I = STATESIZE + 3*i;
 
     P.block(I,0,N-I-3,I) = P.bottomLeftCorner(N-I-3,I);
     P.block(0,I,I,N-I-3) = P.topRightCorner(I,N-I-3);
@@ -769,8 +840,15 @@ State::shrinkP ( int i )
 }		/* -----  end of method State::shrinkP  ----- */
 
     void
+#if STATESIZE==13
+State::ransacUpdate ( std::vector<measurement_t> &z )
+#else
 State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q )
+#endif
 {
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
     Matrix<double,Dynamic,1> mu_old; // x_k|k-1
     mu_old = asVector();
     // Randomize the measurements
@@ -856,10 +934,10 @@ Matrix<double,Dynamic,1>
 State::asVector ( )
 {
     Matrix<double,Dynamic,1> m;
-    int rows = 9 + 3*features.size();
+    int rows = STATESIZE + 3*features.size();
     m = Matrix<double,Dynamic,1>::Zero(rows,1);
-    m.head<9>() = body->asVector();
-    int row = 9;
+    m.head<STATESIZE>() = body->asVector();
+    int row = STATESIZE;
     for (auto i=features.begin(); i!=features.end(); ++i) {
         m.segment<3>(row) = (*i)->asVector();
         row += 3;
@@ -871,11 +949,11 @@ State::asVector ( )
 void
 State::asVector ( const Matrix<double,Dynamic,1> &m )
 {
-    int rows = 9 + 3*features.size();
+    int rows = STATESIZE + 3*features.size();
     assert (rows==m.rows());
-    body->asVector(m.head<9>());
+    body->asVector(m.head<STATESIZE>());
 
-    int row = 9;
+    int row = STATESIZE;
     for (auto i=features.begin(); i!=features.end(); ++i) {
         (*i)->asVector(m.segment<3>(row));
         row += 3;
diff --git a/src/state.h b/src/state.h
index a7f7df5..f330b67 100644
--- a/src/state.h
+++ b/src/state.h
@@ -15,7 +15,7 @@
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define DORANSAC            /*  */
 //#define INLIERTEST            /*  */
-
+#define STATESIZE 13            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
 using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -44,14 +44,20 @@ class State
         int rowById(int id);
         int iById(int id);
         Feature *featureById(int id);
+#if STATESIZE==13
+        MatrixXd F(const Vector3d &w, double dt);
+        MatrixXd H ( const std::vector<measurement_t> &z );
+        MatrixXd blockSI( const std::vector<measurement_t> &z );
+#else
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
+        MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
+#endif
         int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
         MatrixXd R( const std::vector<measurement_t> &z );
-        MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
-        Matrix<double,9,9> Pxx();
-        Matrix<double,9,3> Pxy(int id);
+        Matrix<double,STATESIZE,STATESIZE> Pxx();
+        Matrix<double,STATESIZE,3> Pxy(int id);
         Matrix<double,3,3> Pyy(int id);
         Matrix<double,Dynamic,6> L();
 
@@ -59,16 +65,27 @@ class State
         void accelerometer_bias(const Vector3d &b);
         void asVector(const Matrix<double,Dynamic,1> &m);
         void pos(const UTM &utm);
+#if STATESIZE==13
+        void handle_measurements( const std::vector<measurement_t> & z);
+        void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
+                const std::vector<measurement_t> &z);
+        void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt);
+        MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, 
+                const std::vector<measurement_t> &z);
+        void Pkk1 ( const Vector3d &ang, const double dt);
+        void ransacUpdate(std::vector<measurement_t> &z);
+#else
         void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q);
-        void initializePi(int i, const Matrix<double,3,3> &Pi);
         void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
                 const std::vector<measurement_t> &z, const Quaterniond &q);
-        void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q);
         void motionModel(const Vector3d &acc, const Vector3d &ang,
                 const Quaterniond &q, const double dt);
         MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, 
                 const std::vector<measurement_t> &z, const Quaterniond &q );
         void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt);
+        void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q);
+#endif
+        void initializePi(int i, const Matrix<double,3,3> &Pi);
         void position_covariance(const Matrix<double,3,3> &p);
         std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, bool bl);
         std::list<Feature *>::iterator removeFeature ( int id, bool bl );
@@ -76,7 +93,11 @@ class State
         void vel(const Matrix<double,3,1> &v);
 
         /* ====================  OPERATORS     ======================================= */
+#if STATESIZE==13
+        Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z);
+#else
         Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q);
+#endif
         void unicsv();
 
     protected:
@@ -86,7 +107,11 @@ class State
 
     private:
         /* ====================  METHODS       ======================================= */
+#if STATESIZE==13
+        void addFeatures(std::vector<measurement_t> &F, double z);
+#else
         void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z);
+#endif
         void expandP ( const Matrix3d &Pi);
         void shrinkP( int i );