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, 17 insertions, 7 deletions

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