Begin Vision class.

This class can display measurements and featuers and find new features.
When complete it will also perform measurements.

1 files changed, 4 insertions, 1 deletions

diff --git a/src/state.h b/src/state.h
index cc10263..55e2d86 100644
--- a/src/state.h
+++ b/src/state.h
@@ -10,7 +10,7 @@
 #include "feature.h"
 #include "types.h"
 
-#define MAXFEATURES 50
+#define MAXFEATURES 30
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define DORANSAC            /*  */
@@ -48,10 +48,13 @@ class State
         MatrixXd H ( const std::vector<measurement_t> &z );
         MatrixXd blockSI( const std::vector<measurement_t> &z );
         Quaterniond qhat();
+        void featuresAsMeasurements(std::vector<measurement_t> &yk);
 #else
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
         MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
+        void featuresAsMeasurements(std::vector<measurement_t> &yk,
+                const Quaterniond &q);
 #endif
         int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);