From 1a5bbf751ee0e924b514989f859084ebcd733dc6 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 9 Apr 2017 15:59:29 -0500
Subject: Full state estimation and vision process are working.

---
 src/vision.cpp | 30 +++++++++++++++++++++---------
 1 file changed, 21 insertions(+), 9 deletions(-)

(limited to 'src/vision.cpp')

diff --git a/src/vision.cpp b/src/vision.cpp
index 0f89b35..4d4ba2e 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -41,19 +41,16 @@ Vision::Vision ()
 void
 Vision::open ( const char *fn, const Camera &cam )
 {
-    Mat bayered, unbayered ;
+    Mat bayered, unbayered, undistorted ;
     bayered = cv::imread(fn, -1);
     if (!bayered.data) {
         fprintf(stderr, "Could not read %s.\n", fn);
         exit(1);
     }
     cv::cvtColor(bayered, unbayered, CV_BayerBG2BGR ,3);
-    unbayered.convertTo(original, CV_32FC3);
-    Mat luv;
+    cv::undistort( unbayered, undistorted, cam.K(1), cam.d(1));
+    undistorted.convertTo(original, CV_32FC3);
     cv::cvtColor(original, gray, CV_BGR2GRAY);
-    //vector<Mat> luv_channels;
-    //cv::split(luv,luv_channels);
-    //gray = luv_channels[0];
     display = unbayered.clone();
     return ;
 }		/* -----  end of method Vision::open  ----- */
@@ -173,6 +170,7 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
     for (auto i=zin.begin(); i!=zin.end(); ++i) {
         measurement_t z;
         z = *i;
+        measurement_type mt = z.z_type;
         if (i->z_type==MONO || i->z_type==BOTH) {
             measurement_t zins, zouts;
             zins = *i;
@@ -180,6 +178,13 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
             measure(cam,zins, zouts);
             z.xcorrmax = zouts.xcorrmax;
             z.source = zouts.source;
+            if (z.xcorrmax<0.9) { //downgrade
+                if (mt==BOTH) {
+                    mt=REFLECTION;
+                } else if (mt==MONO) {
+                    continue;
+                }
+            }
         }
         if (i->z_type==REFLECTION || i->z_type==BOTH) {
             measurement_t zinr, zoutr;
@@ -187,9 +192,16 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
             zinr.z_type = REFLECTION;
             measure(cam, zinr, zoutr);
             z.reflection = zoutr.reflection;
+            if (zoutr.xcorrmax<0.4) { //downgrade
+                if (mt==BOTH) {
+                    mt = MONO;
+                } else if (mt==REFLECTION) {
+                    continue;
+                }
+            }
         }
-        if (z.xcorrmax>0.90)
-            zout.push_back(z);
+        z.z_type = BOTH;
+        zout.push_back(z);
     }
     return ;
 }		/* -----  end of method Vision::measurements  ----- */
@@ -287,7 +299,7 @@ Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRec
     if (z.z_type==MONO) {
         xis = cam.body2img(z.source);
     } else {
-        xis = cam.body2img(z.source);
+        xis = cam.body2img(z.reflection);
     }
     //xis /= xis[2];
     cv::Point ps;
-- 
cgit v1.1