Add 1-Point RANSAC.

1-Pt RANSAC is a method for detecting outlier measurements in the EKF
framework. This algorithm is as described in Civera 2010.

1 files changed, 13 insertions, 4 deletions

diff --git a/src/state.h b/src/state.h
index d223d0f..2383706 100644
--- a/src/state.h
+++ b/src/state.h
@@ -1,5 +1,6 @@
 #ifndef  state_INC
 #define  state_INC
+#include <algorithm>
 #include <Eigen/Dense>
 #include <list>
 #include <vector>
@@ -12,9 +13,8 @@
 #define MAXFEATURES 50
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
-#define FULLS // Comment out to treat each measurement independently.
-//#define BLOCKSI
-//#define MEAS1
+#define DORANSAC            /*  */
+
 using Eigen::Dynamic;
 using Eigen::Matrix;
 using Eigen::MatrixXd;
@@ -24,6 +24,7 @@ using Eigen::Vector3d;
 using std::cout;
 using std::cerr;
 using std::endl;
+
 /*
  * =====================================================================================
  *        Class:  State
@@ -37,8 +38,10 @@ class State
         State ();                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        Matrix<double,Dynamic,1> asVector();
         bool exists(int id);
         int rowById(int id);
+        int iById(int id);
         Feature *featureById(int id);
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
@@ -53,16 +56,21 @@ class State
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
+        void asVector(const Matrix<double,Dynamic,1> &m);
         void pos(const UTM &utm);
         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 position_covariance(const Matrix<double,3,3> &p);
-        std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
+        std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, bool bl);
+        std::list<Feature *>::iterator removeFeature ( int id, bool bl );
         void velocity_covariance(const Matrix<double,3,3> &p);
         void vel(const Matrix<double,3,1> &v);
 
@@ -88,6 +96,7 @@ class State
         char utm_c;
         int utm_i;
         std::list<Feature *> features;
+        std::vector<int> blacklist;
 
 
 }; /* -----  end of class State  ----- */