From f21a53aa32f9a005258b129c1e0ef5c7d71890df Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Fri, 31 Mar 2017 15:54:02 -0500
Subject: 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.
---
 src/feature.cpp | 56 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 56 insertions(+)

(limited to 'src/feature.cpp')

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  ----- */
+
-- 
cgit v1.1