diff options
Diffstat (limited to 'src/state.h')
| -rw-r--r-- | src/state.h | 17 |
1 files changed, 13 insertions, 4 deletions
diff --git a/src/state.h b/src/state.h index d223d0f..2383706 100644 --- a/src/state.h +++ b/src/state.h @@ -1,5 +1,6 @@ #ifndef state_INC #define state_INC +#include <algorithm> #include <Eigen/Dense> #include <list> #include <vector> @@ -12,9 +13,8 @@ #define MAXFEATURES 50 #define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once -#define FULLS // Comment out to treat each measurement independently. -//#define BLOCKSI -//#define MEAS1 +#define DORANSAC /* */ + using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -24,6 +24,7 @@ using Eigen::Vector3d; using std::cout; using std::cerr; using std::endl; + /* * ===================================================================================== * Class: State @@ -37,8 +38,10 @@ class State State (); /* constructor */ /* ==================== ACCESSORS ======================================= */ + Matrix<double,Dynamic,1> asVector(); bool exists(int id); int rowById(int id); + int iById(int id); Feature *featureById(int id); MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); @@ -53,16 +56,21 @@ class State /* ==================== MUTATORS ======================================= */ void accelerometer_bias(const Vector3d &b); + void asVector(const Matrix<double,Dynamic,1> &m); void pos(const UTM &utm); void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); void initializePi(int i, const Matrix<double,3,3> &Pi); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, const std::vector<measurement_t> &z, const Quaterniond &q); + void ransacUpdate(std::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 ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); void position_covariance(const Matrix<double,3,3> &p); - std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, int j); + std::list<Feature *>::iterator removeFeature(std::list<Feature *>::iterator &i, bool bl); + std::list<Feature *>::iterator removeFeature ( int id, bool bl ); void velocity_covariance(const Matrix<double,3,3> &p); void vel(const Matrix<double,3,1> &v); @@ -88,6 +96,7 @@ class State char utm_c; int utm_i; std::list<Feature *> features; + std::vector<int> blacklist; }; /* ----- end of class State ----- */ |