MONO handling in State was missing Hy!

3 files changed, 47 insertions, 9 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 5a334c5..05c3b30 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -91,6 +91,7 @@ State::H ( const Quaterniond &q, const vector<measurement_t> &z )
                 }
                 fi = featureById( z[i].id );
                 h.block<4,STATESIZE>(row,0) = fi->Hx(pos,q).block<4,STATESIZE>(0,0);
+                h.block<4,3>(row,col) = fi->Hy(pos,q).block<4,3>(0,0);
                 break;
 
             case HEIGHT:	
diff --git a/src/types.h b/src/types.h
index d43fdf2..955e3bf 100644
--- a/src/types.h
+++ b/src/types.h
@@ -17,7 +17,7 @@ using Eigen::Vector3d;
 using Eigen::Vector4d;
 
 // A struct for storing measurements.
-typedef enum {REFLECTION,MONO,HEIGHT} measurement_type;
+typedef enum {BOTH,REFLECTION,MONO,HEIGHT} measurement_type;
 typedef struct {
     measurement_type z_type;
     int id;
diff --git a/src/vision.cpp b/src/vision.cpp
index 5d46f02..0f89b35 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -172,7 +172,22 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
 {
     for (auto i=zin.begin(); i!=zin.end(); ++i) {
         measurement_t z;
-        measure(cam,*i, z);
+        z = *i;
+        if (i->z_type==MONO || i->z_type==BOTH) {
+            measurement_t zins, zouts;
+            zins = *i;
+            zins.z_type = MONO;
+            measure(cam,zins, zouts);
+            z.xcorrmax = zouts.xcorrmax;
+            z.source = zouts.source;
+        }
+        if (i->z_type==REFLECTION || i->z_type==BOTH) {
+            measurement_t zinr, zoutr;
+            zinr = *i;
+            zinr.z_type = REFLECTION;
+            measure(cam, zinr, zoutr);
+            z.reflection = zoutr.reflection;
+        }
         if (z.xcorrmax>0.90)
             zout.push_back(z);
     }
@@ -189,6 +204,7 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
 void
 Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zout )
 {
+    assert((zin.z_type==MONO) || (zin.z_type==REFLECTION));
     // Create ROIs
     RotatedRect searchrr; // Rotated rect describing search ellipse
     Rect searchr; // Rect describing search ellipse
@@ -206,7 +222,11 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
         // Match template
         Mat result;
         result.create( sr.cols-zin.patch.cols+1, sr.rows-zin.patch.rows+1, CV_32FC1 );
-        matchTemplate( sr, zin.patch, result, CV_TM_CCOEFF_NORMED);
+        if (zin.z_type==MONO) {
+            matchTemplate( sr, zin.patch, result, CV_TM_CCOEFF_NORMED);
+        } else {
+            matchTemplate( sr, zin.refpatch, result, CV_TM_CCOEFF_NORMED);
+        }
 
         // Mask the ellipse around the result.
         Mat mask = Mat::zeros(result.size(), CV_8UC1);
@@ -225,9 +245,17 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
         Vector3d zi;
         zi << zpi.x, zpi.y, 1;
 
-        zout.source = cam.img2body(zi);
+        zout.id = zin.id;
+        if (zin.z_type==MONO) {
+            zout.source = cam.img2body(zi);
+            zout.z_type = MONO;
+        } else {
+            zout.reflection = cam.img2body(zi);
+            zout.z_type = REFLECTION;
+        }
         zout.xcorrmax = maxval;
 
+        /*
         Mat sr8, patch8, rpatch8;
         sr.convertTo(sr8, CV_8UC1);
         zin.patch.convertTo(patch8, CV_8UC1);
@@ -244,27 +272,36 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
         cv::imshow("display2", display);
         cerr << maxval << endl;
         cv::waitKey(0);
+        */
     } else {
         zout.xcorrmax = -1.;
     }
-    zout.id = zin.id;
-    zout.z_type = MONO;
     return ;
 }		/* -----  end of method Vision::measure  ----- */
 
 void
 Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRect &rr )
 {
+    assert((z.z_type==MONO) || (z.z_type==REFLECTION));
     Vector3d xis;
-    xis = cam.body2img(z.source);
+    if (z.z_type==MONO) {
+        xis = cam.body2img(z.source);
+    } else {
+        xis = cam.body2img(z.source);
+    }
     //xis /= xis[2];
     cv::Point ps;
     ps.x = xis[0];
     ps.y = xis[1];
 
     double xs, ys;
-    xs = 2*sqrt(z.Ssrc(0));
-    ys = 2*sqrt(z.Ssrc(1));
+    if (z.z_type==MONO) {
+        xs = 2*sqrt(z.Ssrc(0));
+        ys = 2*sqrt(z.Ssrc(1));
+    } else {
+        xs = 2*sqrt(z.Sref(0));
+        ys = 2*sqrt(z.Sref(1));
+    }
     
     // Create rotated rect for ellipse
     rr = cv::RotatedRect(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.);