Move feature depth calculation to Feature class.

And also fixed F computation for features.

I realized that the features were transformed from image to body frame
prior to being passed to State. This makes sense because the State
really doesn't need to know about Camera objects. Since the camera was
no longer necessary inside of State it made sense to move the depth
computation from Camera::ref2body() into a method in the Feature class
that does not rely on camera parameters.

Additionally, the depth solver was changed from a simple matrix
inversion to a least squares calculation.

Fx in the feature class was fixed to take into account dt.

Updating Pkk1 is in the process of being modified to handle features.

1 files changed, 47 insertions, 11 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 9a2a6b3..f2344f0 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -60,26 +60,36 @@ State::update ( const Matrix<double,1,1> &z )
 }
 
 void
-State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q )
+State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &q)
 {
     std::vector<measurement_t> featuresToAdd;
     for (auto i=z.begin(); i!=z.end(); ++i) {
         if (exists(i->id)) {
-            printf("Exists!\n");
+            ;
         } else {
             featuresToAdd.push_back(*i);
         }
     }
+    addFeatures( featuresToAdd, q);
 
-    // Expand P matrix
-    expandP(featuresToAdd.size());
+    return ;
+}		/* -----  end of method State::feature_update  ----- */
 
+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=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) {
+    for (auto i=F.begin(); i!=F.end(); ++i) {
+        // Estimate initial depth
+        Vector3d xib;
+
         // Create feature
-        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q);
+        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q, -0.44);
         features.push_back(f);
-
+        
         // Initialize P values
         int j=features.size();
         Matrix<double,3,3> Pi;
@@ -87,7 +97,7 @@ State::feature_update ( const std::vector<measurement_t> &z, const Quaterniond &
         initializePi(j,Pi);
     }
     return ;
-}		/* -----  end of method State::feature_update  ----- */
+}		/* -----  end of method State::addFeatures  ----- */
 
 /*
  *--------------------------------------------------------------------------------------
@@ -109,6 +119,30 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
     return ;
 }		/* -----  end of method State::initializePi  ----- */
 
+MatrixXd
+State::F ( const Quaterniond &q, const Vector3d &w, double dt )
+{
+    Vector3d v;
+    v = body->vel();
+    // Allocate matrix F
+    MatrixXd f;
+    int rows = 9 + 3*features.size();
+    f = MatrixXd::Zero(rows,rows);
+
+    // Set body F
+    f.topLeftCorner<9,9>() = body->F(w,q,dt);
+    // Set Fxi Fyi
+    { // limit i's scope
+        auto i = features.begin();
+        for (int row=9; row<rows; row+=3,++i) {
+            f.block<3,9>(row,0) = (*i)->Fx(dt);
+            f.block<3,3>(row,row) = (*i)->Fy(v,w,dt);
+        }
+    }
+
+    return f ;
+}		/* -----  end of method State::F  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
@@ -119,10 +153,12 @@ State::initializePi ( int i, const Matrix<double,3,3> &Pi )
 void
 State::Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt)
 {
-    Matrix<double,9,9> F,Q;
+    Matrix<double,9,9> Q;
     Q = body->Q(dt);
-    F = body->F(ang,q,dt);
-    P = F*P*F.transpose()+Q;
+    MatrixXd f;
+    f = F ( q, ang, dt );
+    //cout << f << endl;
+    P = f*P*f.transpose()+Q;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
     return ;