Add feature handling to state

10 files changed, 178 insertions, 11 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 2a907a1..caa7cb9 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -207,7 +207,6 @@ Body::skewSymmetric ( const Vector3d &x )
     return y;
 }		/* -----  end of method Body::skewSymmetric  ----- */
 
-
 void
 Body::unicsv ( )
 {
@@ -225,3 +224,16 @@ Body::accelerometer_bias ( const Vector3d &b )
     return ;
 }		/* -----  end of method State::accelerometer_bias  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Body
+ *      Method:  Body :: ned
+ * Description:  Returns the body position in NED coordinates.
+ *--------------------------------------------------------------------------------------
+ */
+Vector3d
+Body::ned (  )
+{
+    return X.segment(0,3) ;
+}		/* -----  end of method Body::ned  ----- */
+
diff --git a/src/body.h b/src/body.h
index 8c3dca3..f99c9e8 100644
--- a/src/body.h
+++ b/src/body.h
@@ -21,6 +21,7 @@ class Body
         Body (){};                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        Vector3d ned();
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias( const Vector3d &b);
diff --git a/src/camera.cpp b/src/camera.cpp
index dcc5a44..b698925 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -90,3 +90,14 @@ Camera::d ( )
     return _d;
 }		/* -----  end of method Camera::d  ----- */
 
+Vector3d
+Camera::img2body ( Vector3d &xi )
+{
+    Vector3d xc, xb;
+    xc = K().inverse()*xi;
+    // Normalize
+    xc /= xc.norm();
+    xb = Rc2b()*xc;
+    return xb;
+}		/* -----  end of method Camera::img2body  ----- */
+
diff --git a/src/camera.h b/src/camera.h
index 54c47b9..328db25 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -6,6 +6,8 @@
 #define BINNING 0.5 // set the binning factor
 using Eigen::Matrix;
 using Eigen::Vector4d;
+using Eigen::Vector3d;
+
 /*
  * =====================================================================================
  *        Class:  Camera
@@ -26,6 +28,7 @@ class Camera
         /* ====================  MUTATORS      ======================================= */
 
         /* ====================  OPERATORS     ======================================= */
+        Vector3d img2body(Vector3d &xi);
 
     protected:
         /* ====================  METHODS       ======================================= */
diff --git a/src/feature.cpp b/src/feature.cpp
index 2afd5a0..fd4a85f 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -25,13 +25,18 @@
  * Description:  constructor
  *--------------------------------------------------------------------------------------
  */
-Feature::Feature (const Vector3d &pib, const Vector3d &xbw, const Quaterniond q )  /* constructor */
+Feature::Feature ( int id, const Vector3d &xib, const Vector3d &xpb,
+        const Vector3d &xbw, const Quaterniond &q )  /* constructor */
 {
-    X = pib;
+    _id = id;
+    X = x2p(xib);
+    X[2] = 1./20.;
     xb0w = xbw;
     q0 = q;
 }  /* -----  end of method Feature::Feature  (constructor)  ----- */
 
+Feature::Feature () {;}
+
 
 /*
  *--------------------------------------------------------------------------------------
@@ -368,3 +373,28 @@ Feature::Hy (  const Vector3d &pos, const Quaterniond &q )
     return H ;
 }		/* -----  end of method Feature::Hy  ----- */
 
+int
+Feature::id ( )
+{
+    return _id;
+}		/* -----  end of method Feature::id  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Feature
+ *      Method:  Feature :: P0
+ * Description:  Returns the initial P matrix.
+ *--------------------------------------------------------------------------------------
+ */
+Matrix<double,3,3>
+Feature::P0 ( )
+{
+    Matrix<double,3,3> P;
+    double p0 = 1e-4;
+    double p1 = 1e-2;
+    P << p0, 0., 0., 
+         0., p0, 0.,
+         0., 0., p1;
+    return P;
+}		/* -----  end of method Feature::P0  ----- */
+
diff --git a/src/feature.h b/src/feature.h
index fc8fe2f..eb3e5bd 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -18,9 +18,12 @@ class Feature
 {
     public:
         /* ====================  LIFECYCLE     ======================================= */
-        Feature (const Vector3d &pib, const Vector3d &xbw, const Quaterniond q );                             /* constructor */
+        Feature ();
+        Feature ( int id, const Vector3d &xib, const Vector3d &xpb, const Vector3d &xbw, const Quaterniond &q );                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        int id();
+        Matrix<double,3,3> P0();
 
         /* ====================  MUTATORS      ======================================= */
         void motionModel ( const Vector3d &ang, const Vector3d &vel, const double dt);
@@ -37,6 +40,8 @@ class Feature
         Matrix<double,6,6> R();
         Matrix<double,6,6> S ( const Matrix<double,9,9> &Pxx, const Matrix<double,9,3> &Pxy,
             const Matrix<double,3,3> &Pyy, const Vector3d &pos, const Quaterniond &q);
+        Vector3d p2x(const Vector3d &p);
+        Vector3d x2p(const Vector3d &x);
 
     protected:
         /* ====================  METHODS       ======================================= */
@@ -45,10 +50,9 @@ class Feature
 
     private:
         /* ====================  METHODS       ======================================= */
-        Vector3d p2x(const Vector3d &p);
-        Vector3d x2p(const Vector3d &x);
 
         /* ====================  DATA MEMBERS  ======================================= */
+        int _id;
         Vector3d X;
         Quaterniond q0;
         Vector3d xb0w;
diff --git a/src/main.cpp b/src/main.cpp
index af602c5..390a57e 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -43,8 +43,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q)
 }
 
 void
-imgCallback(message &msg)
+imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
 {
+    std::vector<measurement_t> z;
     int id,sx,sy,rx,ry;
     strcat(msg.image_names[0],".txt");
     FILE *fin;
@@ -52,11 +53,21 @@ imgCallback(message &msg)
         err_sys("fopen: %s", msg.image_names[0]);
     }
     while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) {
-        printf("id: %d source: (%d, %d) reflection: (%d, %d)\n", id, sx, sy, rx, ry);
+        measurement_t m;
+        m.id = id;
+        // Points in the image frame
+        Vector3d xi, xir;
+        xi << sx, sy, 1.;
+        xir << rx, ry, 1.;
+        // Points in the camera frame
+        m.source = cam.img2body(xi);
+        m.reflection = cam.img2body(xir);
+        z.push_back(m);
     }
     if (fclose(fin)==EOF) {
         err_sys("fclose");
     }
+    mu.feature_update(z,q);
 
     return;
 }
@@ -261,7 +272,7 @@ while (scanf("%s", line)!=EOF) {
             break;
 
         case IMG:	
-            imgCallback(msg);
+            imgCallback(msg, mu, cam, qbw);
             break;
 
         case INSPVAS:	
diff --git a/src/main.h b/src/main.h
index 174ab0e..0698e68 100644
--- a/src/main.h
+++ b/src/main.h
@@ -7,6 +7,7 @@
 #include <Eigen/Dense>
 #include <istream>
 #include <iostream>
+#include <vector>
 
 #include "body.h"
 #include "camera.h"
@@ -24,7 +25,7 @@ int parseLine(char *line, message *msg);
 timestamp update_dt(const timestamp t, timestamp *t_old);
 
 void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaterniond &q);
-void imgCallback(message &msg);
+void imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q);
 void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt);
 void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q);
 void utmCallback(const message &msg, State &mu);
diff --git a/src/state.cpp b/src/state.cpp
index 84aa565..9a2a6b3 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -59,6 +59,56 @@ State::update ( const Matrix<double,1,1> &z )
     body->update(dx);
 }
 
+void
+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);
+        }
+    }
+
+    // Expand P matrix
+    expandP(featuresToAdd.size());
+
+    // Add new features
+    for (auto i=featuresToAdd.begin(); i!=featuresToAdd.end(); ++i) {
+        // Create feature
+        Feature *f = new Feature(i->id, i->source, i->reflection, body->ned(), q);
+        features.push_back(f);
+
+        // Initialize P values
+        int j=features.size();
+        Matrix<double,3,3> Pi;
+        Pi = f->P0();
+        initializePi(j,Pi);
+    }
+    return ;
+}		/* -----  end of method State::feature_update  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: initializePi
+ * Description:  Zeros out the off-diagonal blocks and sets the Pii block to Pi.
+ *--------------------------------------------------------------------------------------
+ */
+    void
+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;
+
+    return ;
+}		/* -----  end of method State::initializePi  ----- */
+
 /*
  *--------------------------------------------------------------------------------------
  *       Class:  State
@@ -115,3 +165,37 @@ State::accelerometer_bias ( const Vector3d &b)
     return ;
 }		/* -----  end of method State::accelerometer_bias  ----- */
 
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: exists
+ * Description:  Tests if the id is a current feature.
+ *--------------------------------------------------------------------------------------
+ */
+bool
+State::exists ( int id )
+{
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        if ((*i)->id()==id) return true;
+    }
+    return false;
+}		/* -----  end of method State::exists  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       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.
+ *--------------------------------------------------------------------------------------
+ */
+void
+State::expandP ( int Nnew )
+{
+    int curRows, newRows;
+    curRows = P.rows();
+    newRows = curRows + 3*Nnew;
+    P.conservativeResize(newRows,newRows);
+    return ;
+}		/* -----  end of method State::expandP  ----- */
+
diff --git a/src/state.h b/src/state.h
index c91c1ae..8a9d318 100644
--- a/src/state.h
+++ b/src/state.h
@@ -1,11 +1,16 @@
 #ifndef  state_INC
 #define  state_INC
 #include <Eigen/Dense>
+#include <list>
+#include <vector>
+
 #include "body.h"
+#include "feature.h"
 #include "types.h"
 
 #define MAXFEATURES 30
 using Eigen::Matrix;
+using Eigen::MatrixXd;
 using Eigen::Dynamic;
 using Eigen::Vector3d;
 using Eigen::Quaterniond;
@@ -22,19 +27,22 @@ class State
         State ();                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
+        bool exists(int id);
 
         /* ====================  MUTATORS      ======================================= */
         void accelerometer_bias(const Vector3d &b);
         void enu(const UTM &utm);
+        void feature_update( const std::vector<measurement_t> & z, const Quaterniond &q);
+        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 position_covariance(const Matrix<double,3,3> &p);
         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     ======================================= */
-        void update ( const Matrix<double,1,1> &z);
         void unicsv();
 
     protected:
@@ -44,12 +52,14 @@ class State
 
     private:
         /* ====================  METHODS       ======================================= */
+        void expandP(int Nnew);
 
         /* ====================  DATA MEMBERS  ======================================= */
         Body *body;
         Matrix<double, Dynamic, Dynamic, 0, 9+3*MAXFEATURES, 9+3*MAXFEATURES> P;
         char utm_c;
         int utm_i;
+        std::list<Feature *> features;
 
 
 }; /* -----  end of class State  ----- */