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.

6 files changed, 301 insertions, 42 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 041d971..1e1f57d 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -251,3 +251,16 @@ Body::vel (  )
     return X.segment<3>(3);
 }		/* -----  end of method Body::vel  ----- */
 
+Matrix<double,9,1>
+Body::asVector (  )
+{
+    return X;
+}		/* -----  end of method Body::asVector  ----- */
+
+void
+Body::asVector ( const Matrix<double,9,1> &m )
+{
+    X = m;
+    return ;
+}		/* -----  end of method Body::asVector  ----- */
+
diff --git a/src/body.h b/src/body.h
index f9d3157..90e1998 100644
--- a/src/body.h
+++ b/src/body.h
@@ -3,7 +3,7 @@
 #include <Eigen/Dense>
 #include <iostream>
 #include "types.h"
-#define R_HEIGHT 1e-2            /* measurement noise of height measurement */
+#define R_HEIGHT 2.5e-3            /* measurement noise of height measurement */
 #define IMU_NOISE 800e-6            /* IMU process noise */
 #define IMU_RANDOMWALK 50e-6      /* Bias process noise */
 using Eigen::Matrix;
@@ -23,11 +23,13 @@ class Body
         Body (){};                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        Matrix<double,9,1> asVector();
         Vector3d ned();
         Vector3d vel();
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
+        void asVector(const Matrix<double,9,1> &m);
         void pos( const UTM &utm);
         void dx( const Matrix<double,9,1> &del);
         void vel(const Matrix<double,3,1> &v);
diff --git a/src/feature.cpp b/src/feature.cpp
index 8b1ffe6..caa56f0 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -629,3 +629,59 @@ Feature::inFOV ( )
     return rv;
 }		/* -----  end of method Feature::inFOV  ----- */
 
+bool
+Feature::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,Dynamic,Dynamic,0,6,6> s;
+    Matrix<double,Dynamic,1,0,6,1> hi, zi, y;
+    s = S(Pxx, Pxy, Pyy, pos, q);
+    hi = h(pos,q);
+    if (z.z_type==MONO) {
+        s = s.topLeftCorner<4,4>();
+        hi = hi.head<4>();
+        zi = measurement_vector(z.source);
+    } else {
+        zi = measurement_vector(z.source, z.reflection);
+    }
+    double Y;
+    y = zi-hi;
+    Y =  y.transpose()*s.inverse()*y;
+
+    return (Y<thr) ? true : false;
+}		/* -----  end of method Feature::isInlier  ----- */
+
+bool
+Feature::isRansacInlier(const measurement_t &z, const Vector3d &pos,
+        const Quaterniond &q)
+{
+    Matrix<double,Dynamic,1,0,6,1> hi, zi, y;
+    hi = h(pos,q);
+    if (z.z_type==MONO) {
+        hi = hi.head<4>();
+        zi = measurement_vector(z.source);
+    } else {
+        zi = measurement_vector(z.source, z.reflection);
+    }
+    double Y;
+    y = zi-hi;
+    Y = y.transpose()*y;
+
+    return (Y<RANSAC_LI_THRESHOLD) ? true : false;
+}
+
+Vector3d
+Feature::asVector (  )
+{
+    return X;
+}		/* -----  end of method Feature::asVector  ----- */
+
+
+void
+Feature::asVector ( const Vector3d &m )
+{
+    X = m;
+    return ;
+}		/* -----  end of method Feature::asVector  ----- */
+
diff --git a/src/feature.h b/src/feature.h
index ce42d9f..2754f89 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -4,15 +4,19 @@
 #include <iostream>
 #include "types.h"
 
-#define FEATURE_NOISE  1           /* Feature process noise */
-#define VIEW_NOISE .33             /*  */
-#define INITIAL_VIEW_NOISE .33           /*  */
-#define REFLECTION_VIEW_NOISE 1.1            /*  */
-#define FEATURECOVX .33            /*  */
-#define FEATURECOVY .33           /*  */
-#define FEATURECOVRHO 5e-2            /*  */
+#define FEATURE_NOISE  1e-3           /* Feature process noise */
+#define VIEW_NOISE 5e-3             /*  */
+#define INITIAL_VIEW_NOISE 1e-1           /*  */
+#define REFLECTION_VIEW_NOISE 5e-3            /*  */
+#define FEATURECOVX .01            /*  */
+#define FEATURECOVY .01           /*  */
+#define FEATURECOVRHO 25e-4            /*  */
 #define FEATURECOVRHO_MONO 0.5            /*  */
 #define RHO_0 1./10.            /*  */
+#define INLIER_THRESHOLD 5.9915            /*  */
+#define RANSAC_LI_THRESHOLD 4e-5            /*  */
+#define RANSAC_HI_THRESHOLD 5e-2            /*  */
+
 
 using Eigen::Dynamic;
 using Eigen::Matrix;
@@ -43,14 +47,20 @@ class Feature
         /* ====================  ACCESSORS     ======================================= */
         int id();
         Matrix<double,3,3> P0(measurement_type t);
+        Vector3d asVector();
         UTM utm(Vector3d &xbw, Quaterniond &q); 
 
         /* ====================  MUTATORS      ======================================= */
+        void asVector(const Vector3d &m);
         void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
         void dx( const Vector3d &del);
 
         /* ====================  OPERATORS     ======================================= */
         bool sane();
+        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);
+        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);
diff --git a/src/state.cpp b/src/state.cpp
index 5121e18..a9a55ae 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -59,13 +59,13 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
     int rows = Hrows(z);
     h = MatrixXd::Zero(rows,cols);
     for (int i=0,row=0; i<z.size(); ++i) {
+        int col;
+        Feature *fi;
         switch ( z[i].z_type ) {
-            int col;
-            Feature *fi;
             case REFLECTION:	
                 col = rowById(z[i].id);
                 if (col==-1) {
-                    fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
+                    fprintf(stderr, "Reflection feature %d not found, quitting.\n", z[i].id);
                     exit(1);
                 }
 
@@ -173,37 +173,22 @@ State::R ( const std::vector<measurement_t> &z )
     return r ;
 }		/* -----  end of method State::R  ----- */
 
-/*
- *--------------------------------------------------------------------------------------
- *       Class:  State
- *      Method:  State :: kalmanUpdate
- * Description:  Performs the kalman update. 
- *--------------------------------------------------------------------------------------
- */
-void
-State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
-        const std::vector<measurement_t> &z, const Quaterniond &q)
+MatrixXd
+State::partialUpdate( const MatrixXd &h, const MatrixXd &S, 
+        const std::vector<measurement_t> &z, const Quaterniond &q )
 {
     MatrixXd K;
     // P^T is implied here since P is symmetric
     K = S.fullPivLu().solve(h*P).transpose();
 
-    // Compute P_k|k
-    P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
-    P = 0.5*(P+P.transpose());
-
     // Compute the innovation or error
     Matrix<double,Dynamic,1> y;
-    Eigen::ArrayXd ya;
     y = innovation(z,q);
-    ya = y;
-
-    if ( (ya < -0.3).any() ) return;
-    if ( (ya > 0.3).any() ) return;
-
+    
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
+    assert (dx.rows()==9+3*features.size());
     body->dx(dx.segment<9>(0));
     {
         int row=9;
@@ -211,6 +196,27 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S,
             (*i)->dx(dx.segment<3>(row));
         }
     }
+    return K;
+}
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: kalmanUpdate
+ * Description:  Performs the kalman update. 
+ *--------------------------------------------------------------------------------------
+ */
+void
+State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, 
+        const std::vector<measurement_t> &z, const Quaterniond &q)
+{
+    MatrixXd K;
+    K = partialUpdate(h,S,z,q);
+
+
+    // Compute P_k|k
+    P = (MatrixXd::Identity(P.rows(),P.rows())-K*h)*P;
+    P = 0.5*(P+P.transpose());
     
     return ;
 }		/* -----  end of method State::kalmanUpdate  ----- */
@@ -257,6 +263,7 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
             row += 1;
         } else {
             fprintf(stderr, "Unknown measurement type\n");
+            exit(1);
         }
     }
 
@@ -281,7 +288,12 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
             featuresToUpdate.push_back(*i);
             zmeas = i->height;
         } else if (exists(i->id)) {
-            featuresToUpdate.push_back(*i);
+            Feature *ft;
+            ft = featureById(i->id);
+            if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, INLIER_THRESHOLD)) 
+            {
+                featuresToUpdate.push_back(*i);
+            } 
         } else {
             featuresToAdd.push_back(*i);
         }
@@ -291,9 +303,10 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
     int feati=0;
     while (i!=features.end()) {
         bool measured=false;
-        for (auto j=z.begin(); j!=z.end(); ++j) {
-            if (j->z_type!=REFLECTION) continue;
-            if (j->id==(*i)->id()) {
+        for (auto j=featuresToUpdate.begin(); j!=featuresToUpdate.end(); ++j) {
+            if (j->z_type==HEIGHT) {
+                continue;
+            } else if (j->id==(*i)->id()) {
                 measured=true;
                 break;
             }
@@ -305,8 +318,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
             i=removeFeature(i,feati);
         }
     }
-    
     if (featuresToUpdate.size()>1) {
+        ransacUpdate(featuresToUpdate,q);
+    } else if (featuresToUpdate.size()>1) {
         MatrixXd h;
         h = H(q,featuresToUpdate);
         MatrixXd S;
@@ -387,6 +401,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
     Vector3d pos = body->ned();
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
+        if (std::find(blacklist.begin(),blacklist.end(), i->id)!=blacklist.end()) continue;
         // Create feature
         Feature *f;
         if (i->z_type==REFLECTION) {
@@ -544,7 +559,7 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang,
                 ++i;
                 ++j;
             } else {
-                i=removeFeature(i,j);
+                i=removeFeature(i,false);
             }
         }
     }
@@ -556,16 +571,55 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang,
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
+ *      Method:  State :: iById
+ * Description:  Returns the the feature position in the features vector.
+ * Returns -1 if not found.
+ *--------------------------------------------------------------------------------------
+ */
+int
+State::iById ( int id )
+{
+    int i=0;
+    for (auto f=features.begin(); f!=features.end(); ++f, ++i) {
+        if ((*f)->id()==id) return i;
+    }
+    return -1;
+}		/* -----  end of method State::iById  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
  *      Method:  State :: removeFeature
  * Description:  Remove the Feature i and Pj in a clean fashion.
  *--------------------------------------------------------------------------------------
  */
 std::list<Feature *>::iterator
-State::removeFeature ( std::list<Feature *>::iterator &i, int j )
+State::removeFeature ( std::list<Feature *>::iterator &i, bool bl )
 {
     // It is important to shrink P first, because it depends on knowing the
     // current size of the feature vector.
+    int j = iById((*i)->id());
+    assert (j!=-1);
     shrinkP(j);
+    if (bl) { // add to blacklist
+        blacklist.push_back((*i)->id());
+    }
+    delete *i;
+    i = features.erase(i);
+    return i;
+}		/* -----  end of method State::removeFeature  ----- */
+
+std::list<Feature *>::iterator
+State::removeFeature ( int id, bool bl )
+{
+    int j = iById(id);
+    assert (j!=-1);
+    shrinkP(j);
+    if (bl) {
+        blacklist.push_back(id);
+    }
+    Feature *f = featureById(id);
+    auto i = std::find(features.begin(), features.end(), f);
     delete *i;
     i = features.erase(i);
     return i;
@@ -707,3 +761,118 @@ State::shrinkP ( int i )
     return ;
 }		/* -----  end of method State::shrinkP  ----- */
 
+    void
+State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q )
+{
+    Matrix<double,Dynamic,1> mu_old; // x_k|k-1
+    mu_old = asVector();
+    // Randomize the measurements
+    std::random_shuffle ( z.begin(), z.end() );
+
+    int N0 = z.size();
+    int N = z.size();
+    std::vector<measurement_t> inliers;
+    for (int i=0; i<N && i<N0; ++i) {
+        measurement_t zi = z[i];
+        std::vector<measurement_t> Z;
+        Z.push_back(zi);
+        MatrixXd h;
+        h = H(q,Z);
+        MatrixXd S;
+        S = h*P*h.transpose() + R(Z);
+        partialUpdate(h,S,Z,q);
+
+        for (auto j=z.begin(); j!=z.end(); ++j) {
+            if (j->id==zi.id) continue;
+            if (j->z_type==HEIGHT) {
+                Z.push_back(*j);
+                continue;
+            }
+            Feature *ft = featureById(j->id);
+            if (ft->isRansacInlier(*j, body->ned(), q)) {
+                Z.push_back(*j);
+            }
+        }
+        if (Z.size()>inliers.size()) {
+            inliers.clear();
+            for (auto j=Z.begin(); j!=Z.end(); ++j) {
+                inliers.push_back(*j);
+            }
+            double eps = 1. - (double) inliers.size() / (double) z.size();
+            //N = ceil(log(0.01)/log(eps));
+        }
+        // Restore original state before trying again.
+        asVector(mu_old);
+    }
+
+    // Perform a full update using the inliers.
+    MatrixXd h;
+    h = H(q,inliers);
+    MatrixXd S;
+    S = h*P*h.transpose() + R(inliers);
+    kalmanUpdate(h, S, inliers, q);
+
+    // High innovation update
+    std::vector<measurement_t> hi_inliers;
+    for (auto i=z.begin(); i!=z.end(); ++i) {
+        bool found = false;
+        for (auto j=inliers.begin(); j!=inliers.end(); ++j) {
+            if (i->id==j->id) {
+                found = true;
+                break;
+            }
+        }
+        if (!found) {
+            Feature *ft = featureById(i->id);
+            if (ft->isInlier(*i, Pxx(), Pxy(i->id), Pyy(i->id), body->ned(), q, RANSAC_HI_THRESHOLD)) {
+                hi_inliers.push_back(*i);
+            } else {
+                removeFeature(i->id, true);
+            }
+        }
+    }
+
+    h = H(q,hi_inliers);
+    S = h*P*h.transpose() + R(hi_inliers);
+    kalmanUpdate(h, S, hi_inliers, q);
+
+    /*
+    cout << "Z: " << z.size()
+         << " LI: " << inliers.size()
+         << " HI: " << hi_inliers.size() << endl;
+         */
+
+    return ;
+}		/* -----  end of method State::ransacUpdate  ----- */
+
+Matrix<double,Dynamic,1>
+State::asVector ( )
+{
+    Matrix<double,Dynamic,1> m;
+    int rows = 9 + 3*features.size();
+    m = Matrix<double,Dynamic,1>::Zero(rows,1);
+    m.head<9>() = body->asVector();
+    int row = 9;
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        m.segment<3>(row) = (*i)->asVector();
+        row += 3;
+    }
+
+    return m ;
+}		/* -----  end of method State::asVector  ----- */
+
+void
+State::asVector ( const Matrix<double,Dynamic,1> &m )
+{
+    int rows = 9 + 3*features.size();
+    assert (rows==m.rows());
+    body->asVector(m.head<9>());
+
+    int row = 9;
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        (*i)->asVector(m.segment<3>(row));
+        row += 3;
+    }
+    return ;
+}		/* -----  end of method State::asVector  ----- */
+
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  ----- */