Begin Vision class.

This class can display measurements and featuers and find new features.
When complete it will also perform measurements.

9 files changed, 285 insertions, 20 deletions

diff --git a/src/camera.cpp b/src/camera.cpp
index f9a3ccf..7e1c1b3 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -73,7 +73,7 @@ Camera::Camera (const char *fin)
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,3>
-Camera::Rc2b (  )
+Camera::Rc2b (  ) const
 {
 #ifdef DOYAWCORRECT
     double yaw;
@@ -94,7 +94,7 @@ Camera::Rc2b (  )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,4,4>
-Camera::Rc2b4 ( )
+Camera::Rc2b4 ( ) const
 {
 #ifdef DOYAWCORRECT
     double yaw;
@@ -121,7 +121,7 @@ Camera::Rc2b4 ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,4,4>
-Camera::K4 ( )
+Camera::K4 ( ) const
 {
     Matrix<double,4,4> K4;
     K4 = Matrix<double,4,4>::Identity();
@@ -138,7 +138,7 @@ Camera::K4 ( )
  *--------------------------------------------------------------------------------------
  */
 Matrix<double,3,3>
-Camera::K (  )
+Camera::K (  ) const
 {
     return _K ;
 }		/* -----  end of method Camera::K  ----- */
@@ -151,7 +151,7 @@ Camera::K (  )
  *--------------------------------------------------------------------------------------
  */
 Vector4d
-Camera::d ( )
+Camera::d ( ) const
 {
     return _d;
 }		/* -----  end of method Camera::d  ----- */
@@ -166,7 +166,7 @@ Camera::d ( )
  *--------------------------------------------------------------------------------------
  */
 Vector3d
-Camera::img2body ( Vector3d &xi )
+Camera::img2body ( const Vector3d &xi ) const
 {
     Vector3d xc, xb;
     xc = K().inverse()*xi;
@@ -176,3 +176,18 @@ Camera::img2body ( Vector3d &xi )
     return xb;
 }		/* -----  end of method Camera::img2body  ----- */
 
+Vector3d
+Camera::body2img ( const Vector3d &xb ) const
+{
+    Vector3d xc,xi;
+    xc = Rc2b().transpose()*xb;
+    xi = K()*xc;
+    return xi;
+}		/* -----  end of method Camera::body2img  ----- */
+
+Vector3d
+Camera::principalPoint (  ) const
+{
+    return _K.col(2);
+}		/* -----  end of method Camera::principalPoint  ----- */
+
diff --git a/src/camera.h b/src/camera.h
index d038069..3de64a6 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -28,16 +28,18 @@ class Camera
         Camera (const char *fin);                             /* constructor */
 
         /* ====================  ACCESSORS     ======================================= */
-        Matrix<double,3,3> K();
-        Matrix<double,4,4> K4();
-        Vector4d d();
-        Matrix<double,3,3> Rc2b();
-        Matrix<double,4,4> Rc2b4();
+        Vector4d d() const;
+        Matrix<double,3,3> K() const;
+        Matrix<double,4,4> K4() const;
+        Vector3d principalPoint() const;
+        Matrix<double,3,3> Rc2b() const;
+        Matrix<double,4,4> Rc2b4() const;
 
         /* ====================  MUTATORS      ======================================= */
 
         /* ====================  OPERATORS     ======================================= */
-        Vector3d img2body(Vector3d &xi);
+        Vector3d img2body(const Vector3d &xi) const;
+        Vector3d body2img(const Vector3d &xb) const;
 
     protected:
         /* ====================  METHODS       ======================================= */
diff --git a/src/feature.h b/src/feature.h
index eaf7349..d5e84b4 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -5,18 +5,18 @@
 #include "types.h"
 
 #define FEATURE_NOISE  1e-3           /* Feature process noise */
-#define VIEW_NOISE 5e-3             /*  */
+#define VIEW_NOISE 5e-2             /*  */
 #define INITIAL_VIEW_NOISE 1e-1           /*  */
-#define REFLECTION_VIEW_NOISE 5e-3            /*  */
+#define REFLECTION_VIEW_NOISE 5e-2            /*  */
 #define FEATURECOVX .01            /*  */
 #define FEATURECOVY .01           /*  */
 #define FEATURECOVRHO 25e-4            /*  */
 #define FEATURECOVRHO_MONO 0.5            /*  */
 #define RHO_0 1./10.            /*  */
-#define INLIER_THRESHOLD 2.9915            /*  */
+#define INLIER_THRESHOLD 5.9915            /*  */
 #define RANSAC_LI_THRESHOLD 4e-5            /*  */
 #define RANSAC_HI_THRESHOLD 5e-2            /*  */
-//#define INITDEPTH
+#define INITDEPTH
 
 using Eigen::Dynamic;
 using Eigen::Matrix;
diff --git a/src/main.h b/src/main.h
index 94be379..fb11776 100644
--- a/src/main.h
+++ b/src/main.h
@@ -14,7 +14,7 @@
 #include "ourerr.hpp"
 #include "state.h"
 #include "types.h"
-
+#include "vision.h"
 //#define ROS_PUBLISH          /* Uncomment to publish ROS node */
 //#define ROS_SUBSCRIBE            /* Uncomment to subscribe to ROS */
 
@@ -37,7 +37,7 @@
 #define ACCBIASY 0.01465135
 #define ACCBIASZ -1*-0.00709229
 
-#define CANOECENTER 0.95            /* center of gravity of canoe  */
+#define CANOECENTER 0.88            /* center of gravity of canoe  */
 #define CANOEHEIGHT -0.40
 //#define CANOEHEIGHT -0.75
 #define DOWNSAMPLE 1            /*  */
diff --git a/src/state.cpp b/src/state.cpp
index 937a653..8a31535 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -204,7 +204,7 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S,
     // Get the update
     Matrix<double,Dynamic,1> dx;
     dx = K*y;
-    if (dx.segment<3>(3).norm()>0.04) {
+    if (dx.segment<3>(3).norm()>1) {
         cerr << "dx: " << dx.head<STATESIZE>().transpose() << endl;
         return K;
     }
@@ -1035,3 +1035,49 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq )
 }		/* -----  end of method State::quaternion_covariance  ----- */
 
 #endif
+
+/*
+ *--------------------------------------------------------------------------------------
+ *       Class:  State
+ *      Method:  State :: featuresAsMeasurements
+ * Description:  Writes the feature locations to a measurement_t vector in body
+ * coordinates.
+ *--------------------------------------------------------------------------------------
+ */
+void
+#if STATESIZE==13
+State::featuresAsMeasurements ( std::vector<measurement_t> &yk )
+#else
+State::featuresAsMeasurements ( std::vector<measurement_t> &yk,
+        const Quaterniond &q)
+#endif
+{
+#if STATESIZE==13
+    Quaterniond q = body->qhat();
+#endif
+    for (auto i=features.begin(); i!=features.end(); ++i) {
+        // Get the predicted measurement
+        Matrix<double,6,1> h;
+        h = (*i)->h(body->ned(), q);
+
+        Vector3d pbs, pbr;
+        pbs << h(0), h(1), 1;
+        pbr << h(4), h(5), 1;
+
+        measurement_t z;
+        z.id = 0;
+        z.source = (*i)->p2x(pbs);
+        z.reflection = (*i)->p2x(pbr);
+        z.z_type = REFLECTION;
+
+        // Get ellipse
+        Matrix<double,6,6> s;
+        s = (*i)->S(Pxx(), Pxy((*i)->id()), Pyy((*i)->id()), body->ned(), q);
+        z.Ssrc << s(0,0), s(1,1);
+        z.Sref << s(4,4), s(5,5);
+
+        yk.push_back(z);
+    }
+    return ;
+}		/* -----  end of method State::featuresAsMeasurements  ----- */
+
diff --git a/src/state.h b/src/state.h
index cc10263..55e2d86 100644
--- a/src/state.h
+++ b/src/state.h
@@ -10,7 +10,7 @@
 #include "feature.h"
 #include "types.h"
 
-#define MAXFEATURES 50
+#define MAXFEATURES 30
 #define COVBIAS 2e-5
 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once
 #define DORANSAC            /*  */
@@ -48,10 +48,13 @@ class State
         MatrixXd H ( const std::vector<measurement_t> &z );
         MatrixXd blockSI( const std::vector<measurement_t> &z );
         Quaterniond qhat();
+        void featuresAsMeasurements(std::vector<measurement_t> &yk);
 #else
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z );
         MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q);
+        void featuresAsMeasurements(std::vector<measurement_t> &yk,
+                const Quaterniond &q);
 #endif
         int Hrows( const std::vector<measurement_t> &z );
         MatrixXd Q(double dt);
diff --git a/src/types.h b/src/types.h
index 1248b80..8b0e8a2 100644
--- a/src/types.h
+++ b/src/types.h
@@ -8,6 +8,7 @@
 using Eigen::Matrix;
 using Eigen::Matrix3d;
 using Eigen::Quaterniond;
+using Eigen::Vector2d;
 using Eigen::Vector3d;
 using Eigen::Vector4d;
 
@@ -18,6 +19,7 @@ typedef struct {
     int id;
     Vector3d source, reflection;
     double height;
+    Vector2d Ssrc, Sref;
 } measurement_t;
 
 // A struct for storing PVA covariance.
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;
diff --git a/src/vision.h b/src/vision.h
new file mode 100644
index 0000000..6f324c1
--- /dev/null
+++ b/src/vision.h
@@ -0,0 +1,57 @@
+#ifndef  vision_INC
+#define  vision_INC
+#include <cv.h>
+#include <Eigen/Dense>
+#include <iostream>
+#include <opencv2/highgui/highgui.hpp>
+#include <vector>
+
+#include "camera.h"
+#include "types.h"
+
+using cv::Mat;
+using Eigen::Vector2d;
+using Eigen::Vector3d;
+using Eigen::Matrix;
+using std::vector;
+
+/*
+ * =====================================================================================
+ *        Class:  Vision
+ *  Description:  
+ * =====================================================================================
+ */
+class Vision
+{
+    public:
+        /* ====================  LIFECYCLE     ======================================= */
+        Vision ();                             /* constructor */
+
+        /* ====================  ACCESSORS     ======================================= */
+        void drawMeasurements( const vector<measurement_t> &z, const Camera &cam,
+                const cv::Scalar &colsrc, const cv::Scalar &colref,
+                bool drawEllipse);
+
+        void show();
+
+        /* ====================  MUTATORS      ======================================= */
+
+        /* ====================  OPERATORS     ======================================= */
+        void open(const char *fn, const Camera &cam);
+        void acquireFeatures(const Camera &cam, vector<measurement_t> &z);
+
+    protected:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+
+    private:
+        /* ====================  METHODS       ======================================= */
+
+        /* ====================  DATA MEMBERS  ======================================= */
+        Mat original, display;
+        static int _id;
+
+}; /* -----  end of class Vision  ----- */
+
+#endif   /* ----- #ifndef vision_INC  ----- */