Remove feature type from Feature class

It makes sense to decouple the measurement type, referred to as feature
type or fType from the Feature class since a feature can be measured in
different ways throughout its lifetime. Better to keep knowledge of the
measurement type in the State class.

1 files changed, 5 insertions, 5 deletions

diff --git a/src/state.cpp b/src/state.cpp
index ab79086..47364f1 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -83,7 +83,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z )
                     exit(1);
                 }
                 fi = featureById( z[i].id );
-                h.block<4,9>(row,0) = fi->Hx(pos,q);
+                h.block<4,9>(row,0) = fi->Hx(pos,q).block<4,9>(0,0);
                 break;
 
             case HEIGHT:	
@@ -156,7 +156,7 @@ State::R ( const std::vector<measurement_t> &z )
 
             case MONO:	
                 fi = featureById(z[i].id);
-                r.block<4,4>(row,row) = fi->R();
+                r.block<4,4>(row,row) = fi->R().block<4,4>(0,0);
                 row += 4;
                 break;
 
@@ -237,8 +237,8 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q)
         } else if (mt==MONO) {
             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);
+            hi = fi->h(body->ned(), q).segment<4>(0);
+            zi = fi->measurement_vector(z[i].source);
             y.segment<4>(row) = zi-hi;
             row += 4;
         } else if (mt==HEIGHT) {
@@ -338,7 +338,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
         features.push_back(f);
 
         // Expand P
-        expandP(f->P0());
+        expandP(f->P0(i->z_type));
     }
     return ;
 }		/* -----  end of method State::addFeatures  ----- */