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, 56 insertions, 0 deletions

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  ----- */
+