diff options
| -rw-r--r-- | src/state.cpp | 59 | ||||
| -rw-r--r-- | src/state.h | 8 |
2 files changed, 61 insertions, 6 deletions
diff --git a/src/state.cpp b/src/state.cpp index a5a09fd..dae6a4d 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -80,9 +80,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q) { // Add new features for (auto i=F.begin(); i!=F.end(); ++i) { - // Estimate initial depth - Vector3d xib; - + if (features.size()>MAXFEATURES) break; // Create feature Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44); if (!f->sane()) { @@ -230,12 +228,46 @@ State::motionModel ( const Vector3d &acc, const Vector3d &ang, (*i)->motionModel(ang,vel,dt); } #endif /* ----- not FASTMOTIONMODEL ----- */ + + // Remove features that leave FOV + { + auto i = features.begin(); + int j=0; + while (i!=features.end()) { + if ((*i)->inFOV()) { + ++i; + ++j; + } else { + i=removeFeature(i,j); + } + } + } + + return ; } /* ----- end of method State::motionModel ----- */ /* *-------------------------------------------------------------------------------------- * Class: State + * Method: State :: removeFeature + * Description: Remove the Feature i and Pj in a clean fashion. + *-------------------------------------------------------------------------------------- + */ +std::list<Feature *>::iterator +State::removeFeature ( std::list<Feature *>::iterator &i, int j ) +{ + // It is important to shrink P first, because it depends on knowing the + // current size of the feature vector. + shrinkP(j); + delete *i; + i = features.erase(i); + return i; +} /* ----- end of method State::removeFeature ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: State * Method: State :: L * Description: Return the composition of all Li's of the features *-------------------------------------------------------------------------------------- @@ -319,3 +351,24 @@ State::Pxx ( ) return P.topLeftCorner<9,9>() ; } /* ----- end of method State::Pxx ----- */ +/* + *-------------------------------------------------------------------------------------- + * Class: State + * Method: State :: shrinkP + * Description: Delete the ith feature from P + *-------------------------------------------------------------------------------------- + */ +void +State::shrinkP ( int i ) +{ + int N = 9 + 3*features.size(); + int I = 9 + 3*i; + + P.block(I,0,N-I-3,I) = P.bottomLeftCorner(N-I-3,I); + P.block(0,I,I,N-I-3) = P.topRightCorner(I,N-I-3); + P.block(I,I,N-I-3,N-I-3) = P.bottomRightCorner(N-I-3,N-I-3); + P.conservativeResize(N-3,N-3); + + return ; +} /* ----- end of method State::shrinkP ----- */ + diff --git a/src/state.h b/src/state.h index 67b235f..a0ebc45 100644 --- a/src/state.h +++ b/src/state.h @@ -9,7 +9,7 @@ #include "feature.h" #include "types.h" -#define MAXFEATURES 50 +#define MAXFEATURES 30 #define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once using Eigen::Matrix; using Eigen::MatrixXd; @@ -45,10 +45,11 @@ class State void initializePi(int i, const Matrix<double,3,3> &Pi); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); + 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); void velocity_covariance(const Matrix<double,3,3> &p); void vel(const Matrix<double,3,1> &v); - void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); void update ( const Matrix<double,1,1> &z); /* ==================== OPERATORS ======================================= */ @@ -61,8 +62,9 @@ class State private: /* ==================== METHODS ======================================= */ - void expandP(); void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q); + void expandP(); + void shrinkP( int i ); /* ==================== DATA MEMBERS ======================================= */ Body *body; |