Full state estimation and vision process are working.

1 files changed, 23 insertions, 7 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 70dc32c..32801f8 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -172,7 +172,7 @@ State::R ( const vector<measurement_t> &z )
     for (int i=0,row=0; i<z.size(); ++i) {
         Feature *fi;
         switch ( z[i].z_type ) {
-            case REFLECTION:	
+            case BOTH:	
                 fi = featureById(z[i].id);
                 r.block<6,6>(row,row) = fi->R();
                 row += 6;
@@ -184,6 +184,12 @@ State::R ( const vector<measurement_t> &z )
                 row += 4;
                 break;
 
+            case REFLECTION:
+                fi = featureById(z[i].id);
+                r.block<4,4>(row,row) = fi->R().block<4,4>(2,2);
+                row += 4;
+                break;
+
             case HEIGHT:	
                 r.block<1,1>(row,row) = body->R();
                 row += 1;
@@ -291,7 +297,7 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q)
     for (int i=0,row=0; i<z.size(); ++i) {
         Feature *fi;
         measurement_type mt=z[i].z_type;
-        if (mt==REFLECTION) {
+        if (mt==BOTH) {
             fi = featureById(z[i].id);
             Matrix<double,6,1> hi, zi;
             hi = fi->h(body->ned(), q);
@@ -305,6 +311,13 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q)
             zi = fi->measurement_vector(z[i].source);
             y.segment<4>(row) = zi-hi;
             row += 4;
+        } else if (mt==REFLECTION) {
+            fi = featureById(z[i].id);
+            Matrix<double,4,1> hi, zi;
+            hi = fi->h(body->ned(), q).segment<4>(2);
+            zi = fi->measurement_vector(z[i].source, z[i].reflection).segment<4>(2);
+            y.segment<4>(row) = zi-hi;
+            row += 4;
         } else if (mt==HEIGHT) {
             Matrix<double,1,1> zi, hi;
             zi[0] = z[i].height;
@@ -422,6 +435,8 @@ State::blockSI ( const vector<measurement_t> &z, const Quaterniond &q )
 #if STATESIZE==13
     Quaterniond q = body->qhat();
 #endif
+    fprintf(stderr, "blockSI not implemented\n");
+    exit(1);
     int rows = Hrows(z);
     MatrixXd SI;
     SI = MatrixXd::Zero(rows,rows);
@@ -498,7 +513,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z)
         if (std::find(blacklist.begin(),blacklist.end(), i->id)!=blacklist.end()) continue;
         // Create feature
         Feature *f;
-        if (i->z_type==REFLECTION) {
+        if (i->z_type==BOTH) {
 #ifdef  INITDEPTH
             f = new Feature(i->id, i->source, i->reflection, pos, q, z, i->patch);
 #else      /* -----  not INITDEPTH  ----- */
@@ -507,7 +522,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z)
         } else if (i->z_type==MONO) {
             f = new Feature(i->id, i->source, pos, q, i->patch);
         } else {
-            fprintf(stderr, "Measurement type: %d not known, quitting\n", i->z_type);
+            fprintf(stderr, "Measurement type: %d not supported by addFeatures, quitting\n", i->z_type);
             exit(1);
         }
         if (!f->sane()) {
@@ -1088,7 +1103,7 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam,
         z.id = (*i)->id();
         z.source = (*i)->p2x(pbs);
         z.reflection = (*i)->p2x(pbr);
-        z.z_type = MONO;
+        z.z_type = BOTH;
         z.patch = (*i)->warpedPatch(cam, q);
         z.refpatch = (*i)->reflectedPatch(cam, q);
 
@@ -1163,8 +1178,9 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
 #else
     featuresAsMeasurements(updatedFeatures, cam, q);
 #endif
-    viz.drawMeasurements(z, cam, cv::Scalar(0,0,100), cv::Scalar(0,0,0), false);
-    viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,0,0), true);
+    cerr << z.size() << endl;
+    viz.drawMeasurements(z, cam, cv::Scalar(0,0,200), cv::Scalar(0,0,200), false);
+    viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,200,0), true);
     viz.show();
 
     return ;