From 4306ccf95bafd64aa528a7fc79b1dabe35092c0f Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 29 Mar 2017 13:31:35 -0500
Subject: Allow for the usage of REFLECTION and MONO features.

Feature methods were updated to accomodate both types of features.
---
 src/feature.h | 26 ++++++++++++++++----------
 1 file changed, 16 insertions(+), 10 deletions(-)

(limited to 'src/feature.h')

diff --git a/src/feature.h b/src/feature.h
index bddb50a..e9d1060 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -8,9 +8,11 @@
 #define VIEW_NOISE 1e-3             /*  */
 #define INITIAL_VIEW_NOISE 1e-3            /*  */
 #define REFLECTION_VIEW_NOISE 1e-2            /*  */
-#define FEATURECOVX 1e-3            /*  */
-#define FEATURECOVY 1e-3            /*  */
-#define FEATURECOVRHO 1e-2            /*  */
+#define FEATURECOVX 1e-4            /*  */
+#define FEATURECOVY 1e-4            /*  */
+#define FEATURECOVRHO 5e-3            /*  */
+#define FEATURECOVRHO_MONO 0.5            /*  */
+#define RHO_0 1./10.            /*  */
 
 using Eigen::Dynamic;
 using Eigen::Matrix;
@@ -35,6 +37,8 @@ class Feature
         /* ====================  LIFECYCLE     ======================================= */
         Feature ( int id, const Vector3d &xs, const Vector3d &xr,
                 const Vector3d &xbw, const Quaterniond &q, double z);
+        Feature ( int id, const Vector3d &xs, const Vector3d &xbw,
+                const Quaterniond &q);
 
         /* ====================  ACCESSORS     ======================================= */
         int id();
@@ -52,16 +56,17 @@ class Feature
                 const Vector3d &xr);
         Matrix<double,3,9> Fx( double dt );
         Matrix<double,3,3> Fy( const Vector3d &vel, const Vector3d &ang, double dt);
-        Matrix<double,Dynamic,9> Hx( const Vector3d &pos, const Quaterniond &q);
-        Matrix<double,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
-        Matrix<double,6,1> h( const Vector3d &x, const Quaterniond &q);
+        Matrix<double,Dynamic,9,0,6,9> Hx( const Vector3d &pos, const Quaterniond &q);
+        Matrix<double,Dynamic,3,0,6,3> Hy( const Vector3d &pos, const Quaterniond &q);
+        Matrix<double,Dynamic,1,0,6,1> h( const Vector3d &x, const Quaterniond &q);
         Matrix<double,3,6> L();
-        Matrix<double,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
+        Matrix<double,Dynamic,1,0,6,1> measurement_vector(const Vector3d &xs, const Vector3d &xr);
 
         Matrix<double,3,3> Q(const double dt);
-        Matrix<double,6,6> R();
-        Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
-            const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
+        Matrix<double,Dynamic,Dynamic,0,6,6> R();
+        Matrix<double,Dynamic,Dynamic,0,6,6> S ( const Matrix<double,9,9> &Pxx, 
+                const Matrix<double,9,3> &Pxy, const Matrix<double,3,3> &Pyy, 
+                const Vector3d &pos, const Quaterniond &q);
         Vector3d p2x(const Vector3d &p);
         Vector3d x2p(const Vector3d &x);
 
@@ -79,6 +84,7 @@ class Feature
         Vector3d X0;
         Quaterniond q0;
         Vector3d xb0w;
+        measurement_type fType;
 
 }; /* -----  end of class Feature  ----- */
 
-- 
cgit v1.1