From cbc3a7a308d1edde429022a9492eeedce24c5fd9 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 8 Apr 2017 19:15:25 -0500 Subject: Add code to perform a guided measurement. The Vision class can template match in a search region. The search region result is masked by an ellipse related to the S matrix of the feature. --- src/state.h | 50 ++++++++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 22 deletions(-) (limited to 'src/state.h') diff --git a/src/state.h b/src/state.h index 55e2d86..8098415 100644 --- a/src/state.h +++ b/src/state.h @@ -9,8 +9,10 @@ #include "camera.h" #include "feature.h" #include "types.h" +#include "vision.h" -#define MAXFEATURES 30 +#define MAXFEATURES 40 +#define MINFEATURES 20 #define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ @@ -24,6 +26,7 @@ using Eigen::Vector3d; using std::cout; using std::cerr; using std::endl; +using std::vector; /* * ===================================================================================== @@ -45,20 +48,20 @@ class State Feature *featureById(int id); #if STATESIZE==13 MatrixXd F(const Vector3d &w, double dt); - MatrixXd H ( const std::vector &z ); - MatrixXd blockSI( const std::vector &z ); + MatrixXd H ( const vector &z ); + MatrixXd blockSI( const vector &z ); Quaterniond qhat(); - void featuresAsMeasurements(std::vector &yk); + void featuresAsMeasurements(vector &yk); #else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); - MatrixXd H ( const Quaterniond &q, const std::vector &z ); - MatrixXd blockSI( const std::vector &z, const Quaterniond &q); - void featuresAsMeasurements(std::vector &yk, + MatrixXd H ( const Quaterniond &q, const vector &z ); + MatrixXd blockSI( const vector &z, const Quaterniond &q); + void featuresAsMeasurements(vector &yk, const Quaterniond &q); #endif - int Hrows( const std::vector &z ); + int Hrows( const vector &z ); MatrixXd Q(double dt); - MatrixXd R( const std::vector &z ); + MatrixXd R( const vector &z ); Matrix Pxx(); Matrix Pxy(int id); Matrix Pyy(int id); @@ -69,26 +72,29 @@ class State void asVector(const Matrix &m); void pos(const UTM &utm); #if STATESIZE==13 - void handle_measurements( const std::vector & z); + void doMeasurements( vector &z, Vision &viz, Camera &cam); + void handle_measurements( const vector & z); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z); + const vector &z); void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z); + const vector &z); void Pkk1 ( const Vector3d &ang, const double dt); - void ransacUpdate(std::vector &z); + void ransacUpdate(vector &z); void quaternion_covariance(const Matrix &covq); void qhat(const Quaterniond &q); #else - void handle_measurements( const std::vector & z, const Quaterniond &q); + void doMeasurements( vector &z, Vision &viz, + const Quaterniond &q, Camera &cam); + void handle_measurements( const vector & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z, const Quaterniond &q); + const vector &z, const Quaterniond &q); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector &z, const Quaterniond &q ); + const vector &z, const Quaterniond &q ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); - void ransacUpdate(std::vector &z, const Quaterniond &q); + void ransacUpdate(vector &z, const Quaterniond &q); #endif void initializePi(int i, const Matrix &Pi); void position_covariance(const Matrix &p); @@ -99,9 +105,9 @@ class State /* ==================== OPERATORS ======================================= */ #if STATESIZE==13 - Matrix innovation( const std::vector &z); + Matrix innovation( const vector &z); #else - Matrix innovation( const std::vector &z, const Quaterniond &q); + Matrix innovation( const vector &z, const Quaterniond &q); #endif void unicsv(); @@ -113,9 +119,9 @@ class State private: /* ==================== METHODS ======================================= */ #if STATESIZE==13 - void addFeatures(std::vector &F, double z); + void addFeatures(vector &F, double z); #else - void addFeatures(std::vector &F, const Quaterniond &q, double z); + void addFeatures(vector &F, const Quaterniond &q, double z); #endif void expandP ( const Matrix3d &Pi); void shrinkP( int i ); @@ -127,7 +133,7 @@ class State char utm_c; int utm_i; std::list features; - std::vector blacklist; + vector blacklist; }; /* ----- end of class State ----- */ -- cgit v1.1