Update State::motionModel and addFeatures

Add the feature motion model update to the state method. It is
implemented in two ways: one is the for loop that operates on each
feature instance directly, which is how it has been implemented in the
past. The second method is to compose a large L matrix for all features
and compute the per feature dx in one go. It is expected that the second
method is faster, but it is not yet tested for speed.

1 files changed, 70 insertions, 14 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 959cf22..a5a09fd 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -47,7 +47,7 @@ State::update ( const Matrix<double,1,1> &z )
     Matrix<double,1,9> H;
     H=body->H();
     Matrix<double,1,1> S;
-    S=body->S(P);
+    S=body->S(Pxx());
     Matrix<double,9,1> K;
     K = P*H.transpose()*S.inverse();
     P = (Matrix<double,9,9>::Identity()-K*H)*P;
@@ -78,9 +78,6 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
 void
 State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 {
-    // Expand P matrix all at once
-    expandP(F.size());
-    //
     // Add new features
     for (auto i=F.begin(); i!=F.end(); ++i) {
         // Estimate initial depth
@@ -88,7 +85,14 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 
         // Create feature
         Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
+        if (!f->sane()) {
+            delete f;
+            continue;
+        }
         features.push_back(f);
+
+        // Expand P
+        expandP();
         
         // Initialize P values
         int j=features.size();
@@ -110,8 +114,6 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q)
 State::initializePi ( int i, const Matrix<double,3,3> &Pi )
 {
     int pt=9+3*(i-1);
-    std::cout << pt << std::endl;
-    std::cout << P.cols() << std::endl;
     P.block(pt,0,3,P.cols()) = MatrixXd::Zero(3,P.cols());
     P.block(0,pt,P.rows(),3) = MatrixXd::Zero(P.rows(),3);
     P.block<3,3>(pt,pt) = Pi;
@@ -196,11 +198,63 @@ void
 State::motionModel ( const Vector3d &acc, const Vector3d &ang,
     const Quaterniond &q, const double dt)
 {
-    Pkk1(ang,q,dt);
+    //Pkk1(ang,q,dt);
     body->motionModel(acc,ang,q,dt);
+
+    Vector3d vel;
+    vel = body->vel();
+    
+#ifdef  FASTMOTIONMODEL
+    Matrix<double,Dynamic,6> fullL;
+    fullL = L();
+
+    Matrix<double,6,1> V;
+    V << vel[1], vel[2], vel[0], ang[1], ang[2], ang[0];
+
+    Matrix<double,Dynamic,1> f;
+    f = fullL*V*dt;
+
+    // Update features' states
+    {
+        int rows = 3*features.size();
+        auto i = features.begin();
+        for (int row=0; row<rows; row+=3, ++i) {
+            Vector3d dx;
+            dx = f.segment<3>(row);
+            (*i)->dx(dx);
+        }
+    }
+    
+#else      /* -----  not FASTMOTIONMODEL  ----- */
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        (*i)->motionModel(ang,vel,dt);
+    }
+#endif     /* -----  not FASTMOTIONMODEL  ----- */
     return ;
 }		/* -----  end of method State::motionModel  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: L
+ * Description:  Return the composition of all Li's of the features
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,Dynamic,6>
+State::L (  )
+{
+    Matrix<double,Dynamic,6> l;
+    int rows = 3*features.size();
+    l = Matrix<double,Dynamic,6>::Zero(rows,6);
+    {
+        auto i=features.begin();
+        for (int row=0; row<rows; row+=3, ++i) {
+            l.block<3,6>(row,0) = (*i)->L();
+        }
+    }
+    return l;
+}		/* -----  end of method State::L  ----- */
+
 void
 State::position_covariance ( const Matrix<double,3,3> &p )
 {
@@ -249,17 +303,19 @@ State::exists ( int id )
  *--------------------------------------------------------------------------------------
  *       Class:  State
  *      Method:  State :: expandP
- * Description:  Grows P so that Nnew new features can be added. This could mean
- * explicitly resizing the P matrix or P being a block of a preallocated matrix.
+ * Description:  Expand the P matrix to fit one more feature.
  *--------------------------------------------------------------------------------------
  */
 void
-State::expandP ( int Nnew )
+State::expandP ( )
 {
-    int curRows, newRows;
-    curRows = P.rows();
-    newRows = curRows + 3*Nnew;
-    P.conservativeResize(newRows,newRows);
+    P.conservativeResize(3+P.rows(),3+P.cols());
     return ;
 }		/* -----  end of method State::expandP  ----- */
 
+Matrix<double,9,9>
+State::Pxx ( )
+{
+    return P.topLeftCorner<9,9>() ;
+}		/* -----  end of method State::Pxx  ----- */
+