Allow for the usage of REFLECTION and MONO features.

Feature methods were updated to accomodate both types of features.

1 files changed, 38 insertions, 7 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 8ad1d9c..ab79086 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -146,17 +146,18 @@ State::R ( const std::vector<measurement_t> &z )
     r = MatrixXd::Zero(rows,rows);
 
     for (int i=0,row=0; i<z.size(); ++i) {
+        Feature *fi;
         switch ( z[i].z_type ) {
             case REFLECTION:	
-                Feature *fi;
                 fi = featureById(z[i].id);
                 r.block<6,6>(row,row) = fi->R();
                 row += 6;
                 break;
 
             case MONO:	
-                fprintf(stderr, "mono points not supported.\n");
-                exit(1);
+                fi = featureById(z[i].id);
+                r.block<4,4>(row,row) = fi->R();
+                row += 4;
                 break;
 
             case HEIGHT:	
@@ -224,9 +225,9 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
     y = Matrix<double,Dynamic,1>::Zero(rows,1);
 
     for (int i=0,row=0; i<z.size(); ++i) {
+        Feature *fi;
         measurement_type mt=z[i].z_type;
         if (mt==REFLECTION) {
-            Feature *fi;
             fi = featureById(z[i].id);
             Matrix<double,6,1> hi, zi;
             hi = fi->h(body->ned(), q);
@@ -234,8 +235,12 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
             y.segment<6>(row) = zi-hi;
             row += 6;
         } else if (mt==MONO) {
-            fprintf(stderr, "mono points not supported.\n");
-            exit(1);
+            fi = featureById(z[i].id);
+            Matrix<double,4,1> hi, zi;
+            hi = fi->h(body->ned(), q);
+            zi = fi->measurement_vector(z[i].source, z[i].reflection);
+            y.segment<4>(row) = zi-hi;
+            row += 4;
         } else if (mt==HEIGHT) {
             Matrix<double,1,1> zi, hi;
             zi[0] = z[i].height;
@@ -276,6 +281,26 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni
             featuresToAdd.push_back(*i);
         }
     }
+    // Remove features that don't have measurements in this timestep.
+    auto i=features.begin();
+    int feati=0;
+    while (i!=features.end()) {
+        bool measured=false;
+        for (auto j=z.begin(); j!=z.end(); ++j) {
+            if (j->z_type!=REFLECTION) continue;
+            if (j->id==(*i)->id()) {
+                measured=true;
+                break;
+            }
+        }
+        if (measured==true) {
+            ++i;
+            ++feati;
+        } else {
+            i=removeFeature(i,feati);
+        }
+    }
+    
     if (featuresToUpdate.size()>1) {
 #ifdef  FULLS
         MatrixXd h;
@@ -300,7 +325,12 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
     for (auto i=F.begin(); i!=F.end(); ++i) {
         if (features.size()>MAXFEATURES) break;
         // Create feature
-        Feature *f = new Feature(i->id, i->source, i->reflection, pos, q, z);
+        Feature *f;
+        if (i->z_type==REFLECTION) {
+            f = new Feature(i->id, i->source, i->reflection, pos, q, z);
+        } else {
+            f = new Feature(i->id, i->source, pos, q);
+        }
         if (!f->sane()) {
             delete f;
             continue;
@@ -517,6 +547,7 @@ void
 State::unicsv ( )
 {
     body->unicsv();
+    printf(",%d\n", features.size());
     return ;
 }		/* -----  end of method State::unicsv  ----- */