summaryrefslogtreecommitdiff
path: root/src/state.h
diff options
context:
space:
mode:
Diffstat (limited to 'src/state.h')
-rw-r--r--src/state.h50
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 ----- */