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.

1 files changed, 31 insertions, 6 deletions

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 );