summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMartin Miller2017-03-25 17:22:44 -0500
committerMartin Miller2017-03-25 17:22:44 -0500
commitf409d322fd400b412c632b74f76c6c9794fbf6c0 (patch)
treef500feab7fc99693def2dfca8bdc1a397d088810
parentb44be266412246a50078f73a188e6d506a3fb5ae (diff)
downloadrefslam-f409d322fd400b412c632b74f76c6c9794fbf6c0.zip
refslam-f409d322fd400b412c632b74f76c6c9794fbf6c0.tar.gz
Add code to State for removing features.
After updating the state according to the motion model we check if features are still in the field of view. If they are not they are cleanly removed. Removal requires deletion of the feature, removal from the features list and shrinkng the P matrix.
-rw-r--r--src/state.cpp59
-rw-r--r--src/state.h8
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;