1 files changed, 140 insertions, 0 deletions
diff --git a/src/vision.cpp b/src/vision.cpp
new file mode 100644
index 0000000..7d1931d
--- /dev/null
+++ b/src/vision.cpp
@@ -0,0 +1,140 @@
+/*
+ * =====================================================================================
+ *
+ *       Filename:  vision.cpp
+ *
+ *    Description:  Class for vision tasks such as opening images, plotting
+ *    points in the image frame, performing measurements.
+ *
+ *        Version:  1.0
+ *        Created:  04/07/2017 11:34:16 AM
+ *       Revision:  none
+ *       Compiler:  gcc
+ *
+ *         Author:  Martin Miller (MHM), miller7@illinois.edu
+ *   Organization:  Aerospace Robotics and Controls Lab (ARC)
+ *
+ * =====================================================================================
+ */
+#include "vision.h"
+
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Vision
+ *      Method:  Vision
+ * Description:  constructor
+ *--------------------------------------------------------------------------------------
+ */
+Vision::Vision ()
+{
+    ;
+}  /* -----  end of method Vision::Vision  (constructor)  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Vision
+ *      Method:  Vision :: open
+ * Description:  Opens the given image, undistorts.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Vision::open ( const char *fn, const Camera &cam )
+{
+    Mat bayered ;
+    bayered = cv::imread(fn, -1);
+    if (!bayered.data) {
+        fprintf(stderr, "Could not read %s.\n", fn);
+        exit(1);
+    }
+    cv::cvtColor(bayered, original, CV_BayerBG2BGR ,3);
+    display = original.clone();
+    return ;
+}		/* -----  end of method Vision::open  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Vision
+ *      Method:  Vision :: drawMeasurements
+ * Description:  Draws the measured points onto the image.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam,
+        const cv::Scalar &colsrc, const cv::Scalar &colref, bool drawEllipse)
+{
+    cv::Scalar ellcol(0,0,200);
+    for (auto i=z.begin(); i!=z.end(); ++i) {
+        if (i->z_type==HEIGHT) continue;
+        cerr << i->id << endl;
+        Vector3d xis, xir;
+        xis = cam.body2img(i->source);
+        xis /= xis[2];
+        xir = cam.body2img(i->reflection);
+        xir /= xir[2];
+        cv::Point ps(xis[0],xis[1]);
+        cv::Point pr(xir[0],xir[1]);
+
+        cv::circle(display, ps, 3, colsrc,2);
+        cv::circle(display, pr, 3, colref);
+
+        if (drawEllipse) {
+            double xs, ys, xr, yr;
+            xs = 2*sqrt(i->Ssrc(0));
+            ys = 2*sqrt(i->Ssrc(1));
+            xr = 2*sqrt(i->Sref(0));
+            yr = 2*sqrt(i->Sref(1));
+            
+            // Create rotated rect for ellipse
+            cv::RotatedRect rrs(ps, cv::Size(2*5.9*xs, 2*5.9*ys), 0.);
+            cv::RotatedRect rrr(pr, cv::Size(2*5.9*xr, 2*5.9*yr), 0.);
+            cv::ellipse(display, rrs, ellcol );
+            cv::ellipse(display, rrr, ellcol);
+        }
+    }
+    return ;
+}		/* -----  end of method Vision::drawMeasurements  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Vision
+ *      Method:  Vision :: show
+ * Description:  Show the display image.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Vision::show (  )
+{
+    cv::imshow("Image view", display);
+    cv::waitKey(1);
+    return ;
+}		/* -----  end of method Vision::show  ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  Vision
+ *      Method:  Vision :: acquireFeatures
+ * Description:  Finds new features, assigns IDs, extracts descriptors.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z )
+{
+    vector<cv::Point2f> corners;
+    Mat gray;
+    cv::cvtColor(original, gray, CV_BGR2GRAY);
+    cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20);
+    for (auto i=corners.begin(); i!=corners.end(); ++i) {
+        measurement_t m;
+        m.id = _id++;
+        m.z_type = MONO;
+        Vector3d xi;
+        xi << i->x, i->y, 1.;
+        m.source = cam.img2body(xi);
+        m.reflection << 0,0,1;
+        z.push_back(m);
+    }
+    return ;
+}		/* -----  end of method Vision::acquireFeatures  ----- */
+
+int Vision::_id = 1;