Methods added to State to get H

Added a method to return an H matrix for the provided measurements. This
should be a versatile method that can work for the full H update or the
indpendent update depending on what measurements are passed.

Some helper methods were also added for determining a features
location in the state vector and for return a feature pointer by id.

1 files changed, 5 insertions, 1 deletions

diff --git a/src/state.h b/src/state.h
index db45ba2..43342a7 100644
--- a/src/state.h
+++ b/src/state.h
@@ -11,6 +11,7 @@
 
 #define MAXFEATURES 30
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
+#define FULLS // Comment out to treat each measurement independently.
 using Eigen::Matrix;
 using Eigen::MatrixXd;
 using Eigen::Dynamic;
@@ -33,7 +34,10 @@ class State
 
         /* ====================  ACCESSORS     ======================================= */
         bool exists(int id);
+        int rowById(int id);
+        Feature *featureById(int id);
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
+        MatrixXd H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
         Matrix<double,9,9> Pxx();
         Matrix<double,Dynamic,6> L();
@@ -50,7 +54,7 @@ class State
         std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j);
         void velocity_covariance(const Matrix<double,3,3> &p);
         void vel(const Matrix<double,3,1> &v);
-        void update ( const Matrix<double,1,1> &z);
+        void update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z );
 
         /* ====================  OPERATORS     ======================================= */
         void unicsv();