diff options
Diffstat (limited to 'src/state.h')
-rw-r--r-- | src/state.h | 50 |
1 files changed, 28 insertions, 22 deletions
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<measurement_t> &z ); - MatrixXd blockSI( const std::vector<measurement_t> &z ); + MatrixXd H ( const vector<measurement_t> &z ); + MatrixXd blockSI( const vector<measurement_t> &z ); Quaterniond qhat(); - void featuresAsMeasurements(std::vector<measurement_t> &yk); + void featuresAsMeasurements(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, + MatrixXd H ( const Quaterniond &q, const vector<measurement_t> &z ); + MatrixXd blockSI( const vector<measurement_t> &z, const Quaterniond &q); + void featuresAsMeasurements(vector<measurement_t> &yk, const Quaterniond &q); #endif - int Hrows( const std::vector<measurement_t> &z ); + int Hrows( const vector<measurement_t> &z ); MatrixXd Q(double dt); - MatrixXd R( const std::vector<measurement_t> &z ); + MatrixXd R( const vector<measurement_t> &z ); Matrix<double,STATESIZE,STATESIZE> Pxx(); Matrix<double,STATESIZE,3> Pxy(int id); Matrix<double,3,3> Pyy(int id); @@ -69,26 +72,29 @@ class State void asVector(const Matrix<double,Dynamic,1> &m); void pos(const UTM &utm); #if STATESIZE==13 - void handle_measurements( const std::vector<measurement_t> & z); + void doMeasurements( vector<measurement_t> &z, Vision &viz, Camera &cam); + void handle_measurements( const vector<measurement_t> & z); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z); + const vector<measurement_t> &z); void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z); + const vector<measurement_t> &z); void Pkk1 ( const Vector3d &ang, const double dt); - void ransacUpdate(std::vector<measurement_t> &z); + void ransacUpdate(vector<measurement_t> &z); void quaternion_covariance(const Matrix<double,4,4> &covq); void qhat(const Quaterniond &q); #else - void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); + void doMeasurements( vector<measurement_t> &z, Vision &viz, + const Quaterniond &q, Camera &cam); + void handle_measurements( const vector<measurement_t> & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q); + const vector<measurement_t> &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<measurement_t> &z, const Quaterniond &q ); + const vector<measurement_t> &z, const Quaterniond &q ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); - void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q); + void ransacUpdate(vector<measurement_t> &z, const Quaterniond &q); #endif void initializePi(int i, const Matrix<double,3,3> &Pi); void position_covariance(const Matrix<double,3,3> &p); @@ -99,9 +105,9 @@ class State /* ==================== OPERATORS ======================================= */ #if STATESIZE==13 - Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z); + Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z); #else - Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); + Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z, const Quaterniond &q); #endif void unicsv(); @@ -113,9 +119,9 @@ class State private: /* ==================== METHODS ======================================= */ #if STATESIZE==13 - void addFeatures(std::vector<measurement_t> &F, double z); + void addFeatures(vector<measurement_t> &F, double z); #else - void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z); + void addFeatures(vector<measurement_t> &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<Feature *> features; - std::vector<int> blacklist; + vector<int> blacklist; }; /* ----- end of class State ----- */ |