warp source and reflection patches

10 files changed, 195 insertions, 45 deletions

diff --git a/Makefile b/Makefile
index 861c404..e7f3390 100644
--- a/Makefile
+++ b/Makefile
@@ -12,7 +12,7 @@ slam: ${OBJECT}
 	$(CXX) -o $@ $^ $(CXXFLAGS) $(CPPFLAGS) ${LIBS}
 
 #.cpp.o:
-%.o: %.cpp %.h
+%.o: %.cpp %.h src/types.h
 	$(CXX) $(CXXFLAGS) $(CPPFLAGS) ${LIBS} -c $< -o $@
 
 .PHONY: clean
diff --git a/src/camera.cpp b/src/camera.cpp
index 415bf35..2040641 100644
--- a/src/camera.cpp
+++ b/src/camera.cpp
@@ -192,3 +192,69 @@ Camera::principalPoint (  ) const
     return _K.col(2);
 }		/* -----  end of method Camera::principalPoint  ----- */
 
+cv::Mat
+Camera::warpPatch ( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
+{
+    cv::Mat w;
+    Matrix<double,3,3> R;
+    R = Rc2b().transpose() * q1.toRotationMatrix().transpose() *
+        q0.toRotationMatrix() * Rc2b();
+    cv::Matx23d Rmat;
+    Rmat << R(0,0), R(0,1), R(0,2),
+            R(1,0), R(1,1), R(1,2);
+            //R(2,0), R(2,1), R(2,2);
+    cv::Size sz(PATCHSIZE,PATCHSIZE);
+    cv::warpAffine(p, w, Rmat, sz);
+
+    return w;
+}		/* -----  end of method Camera::warpPatch  ----- */
+
+cv::Mat
+Camera::reflectPatch( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const
+{
+    using Eigen::AngleAxisd;
+    cv::Mat w1, w2, w3;
+    Matrix<double,3,3> R1, R2;
+    Matrix<double,3,3> J;
+    Matrix<double,3,3> Rbw, Rb0w;
+    double roll0, pitch0;
+    double roll1, pitch1;
+
+    roll0 = roll(q0);
+    pitch0 = pitch(q0);
+    roll1 = roll(q1);
+    pitch1 = pitch(q1);
+
+    Rb0w = AngleAxisd(roll0, Vector3d::UnitX())
+      *  AngleAxisd(pitch0, Vector3d::UnitY());
+    Rbw = AngleAxisd(roll1, Vector3d::UnitX())
+      *  AngleAxisd(pitch1, Vector3d::UnitY());
+
+    R1 = Rc2b().transpose() * Rb0w * Rc2b();
+    R2 = Rc2b().transpose() * Rbw.transpose() * Rc2b();
+    cv::Matx23d Rmat1, Rmat2;
+
+    Rmat1 << R1(0,0), R1(0,1), R1(0,2),
+            R1(1,0), R1(1,1), R1(1,2);
+    Rmat2 << R2(0,0), R2(0,1), R2(0,2),
+            R2(1,0), R2(1,1), R2(1,2);
+
+    cv::Size sz(PATCHSIZE,PATCHSIZE);
+    cv::warpAffine(p, w1, Rmat1, sz);
+    cv::flip(w1, w2, 0);
+    cv::warpAffine(w2, w3, Rmat2, sz);
+
+    return w3;
+}		/* -----  end of method Camera::warpPatch  ----- */
+
+double
+Camera::roll(const Quaterniond &q) const
+{
+    return atan2( 2*(q.w()*q.x() + q.y()*q.z()), 1-2*(q.x()*q.x() + q.y()+q.y()));
+}
+
+double
+Camera::pitch(const Quaterniond &q) const
+{
+    return asin( 2*(q.w()*q.y() - q.z()*q.x()));
+}
diff --git a/src/camera.h b/src/camera.h
index 3de64a6..51ba9a6 100644
--- a/src/camera.h
+++ b/src/camera.h
@@ -1,9 +1,12 @@
 #ifndef  camera_INC
 #define  camera_INC
+#include <cv.h>
 #include <Eigen/Dense>
 #include <iostream>
+#include <opencv2/highgui/highgui.hpp>
 #include <yaml-cpp/yaml.h>
 
+#include "types.h"
 #define BINNING 0.5 // set the binning factor
 #define YAWCORRECT 2.0
 //#define DOYAWCORRECT
@@ -40,9 +43,13 @@ class Camera
         /* ====================  OPERATORS     ======================================= */
         Vector3d img2body(const Vector3d &xi) const;
         Vector3d body2img(const Vector3d &xb) const;
+        cv::Mat warpPatch(const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1) const;
+        cv::Mat reflectPatch( const cv::Mat &p, const Quaterniond &q0, const Quaterniond &q1 ) const;
 
     protected:
         /* ====================  METHODS       ======================================= */
+        double roll(const Quaterniond &q) const;
+        double pitch(const Quaterniond &q) const;
 
         /* ====================  DATA MEMBERS  ======================================= */
 
diff --git a/src/feature.cpp b/src/feature.cpp
index 66b0677..fbb1376 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -1119,3 +1119,15 @@ Feature::patch ( )
     return _patch ;
 }		/* -----  end of method Feature::patch  ----- */
 
+Mat
+Feature::warpedPatch ( const Camera &cam, const Quaterniond &q1 )
+{
+    return cam.warpPatch(_patch, q0, q1);
+}		/* -----  end of method Feature::warpedPatch  ----- */
+
+Mat
+Feature::reflectedPatch( const Camera &cam, const Quaterniond &q1 )
+{
+    return cam.reflectPatch(_patch, q0, q1);
+}
+
diff --git a/src/feature.h b/src/feature.h
index 1c0718d..65850af 100644
--- a/src/feature.h
+++ b/src/feature.h
@@ -3,14 +3,16 @@
 #include <cv.h>
 #include <Eigen/Dense>
 #include <iostream>
+
+#include "camera.h"
 #include "types.h"
 
 #define FEATURE_NOISE  1e-3           /* Feature process noise */
 #define VIEW_NOISE 5e-2             /*  */
 #define INITIAL_VIEW_NOISE 1e-1           /*  */
 #define REFLECTION_VIEW_NOISE 5e-2            /*  */
-#define FEATURECOVX .01            /*  */
-#define FEATURECOVY .01           /*  */
+#define FEATURECOVX .001            /*  */
+#define FEATURECOVY .001           /*  */
 #define FEATURECOVRHO 25e-4            /*  */
 #define FEATURECOVRHO_MONO 0.5            /*  */
 #define RHO_0 1./10.            /*  */
@@ -51,6 +53,8 @@ class Feature
         Vector3d asVector();
         UTM utm(Vector3d &xbw, Quaterniond &q); 
         Mat patch();
+        Mat warpedPatch(const Camera &cam, const Quaterniond &q1);
+        Mat reflectedPatch(const Camera &cam, const Quaterniond &q1);
 
         /* ====================  MUTATORS      ======================================= */
         void asVector(const Vector3d &m);
diff --git a/src/state.cpp b/src/state.cpp
index d43be62..5a334c5 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -1049,9 +1049,9 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq )
  */
 void
 #if STATESIZE==13
-State::featuresAsMeasurements ( vector<measurement_t> &yk )
+State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam )
 #else
-State::featuresAsMeasurements ( vector<measurement_t> &yk,
+State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam,
         const Quaterniond &q)
 #endif
 {
@@ -1071,8 +1071,9 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk,
         z.id = (*i)->id();
         z.source = (*i)->p2x(pbs);
         z.reflection = (*i)->p2x(pbr);
-        z.z_type = REFLECTION;
-        z.patch = (*i)->patch();
+        z.z_type = MONO;
+        z.patch = (*i)->warpedPatch(cam, q);
+        z.refpatch = (*i)->reflectedPatch(cam, q);
 
         // Get ellipse
         Matrix<double,6,6> s;
@@ -1101,13 +1102,35 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
         viz.acquireFeatures(cam, newFeatures);
     }
     // Measure existing features
-    vector<measurement_t> currentFeatures, featureZ;
+    vector<measurement_t> currentFeatures;
 #if STATESIZE==13
-    featuresAsMeasurements(currentFeatures);
+    featuresAsMeasurements(currentFeatures,cam);
 #else
-    featuresAsMeasurements(currentFeatures,q);
+    featuresAsMeasurements(currentFeatures,cam,q);
+#endif
+    viz.measurements(cam, currentFeatures, z);
+
+#ifdef DORANSAC
+#if STATESIZE==13
+    ransacUpdate(z);
+#else
+    ransacUpdate(z,q);
+#endif
+#else      /* -----  not DORANSAC  ----- */
+        MatrixXd h;
+#if STATESIZE==13
+        h = H(z);
+#else
+        h = H(q,z);
+#endif
+        MatrixXd S;
+        S = h*P*h.transpose() + R(z);
+#if STATESIZE==13
+        kalmanUpdate(h,S,z);
+#else
+        kalmanUpdate(h,S,z,q);
+#endif
 #endif
-    viz.measurements(cam, currentFeatures, featureZ);
 
     // Initialize new features
     if (features.size()<MINFEATURES) {
@@ -1117,6 +1140,15 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz,
         addFeatures(newFeatures, q, hgtmeas);
 #endif
     }
+    vector<measurement_t> updatedFeatures;
+#if STATESIZE==13
+    featuresAsMeasurements(updatedFeatures, cam);
+#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);
+    viz.show();
 
     return ;
 }		/* -----  end of method State::doMeasurements  ----- */
diff --git a/src/state.h b/src/state.h
index 8098415..08d71ee 100644
--- a/src/state.h
+++ b/src/state.h
@@ -51,12 +51,12 @@ class State
         MatrixXd H ( const vector<measurement_t> &z );
         MatrixXd blockSI( const vector<measurement_t> &z );
         Quaterniond qhat();
-        void featuresAsMeasurements(vector<measurement_t> &yk);
+        void featuresAsMeasurements(vector<measurement_t> &yk, const Camera &cam);
 #else
         MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt);
         MatrixXd H ( const Quaterniond &q, const vector<measurement_t> &z );
         MatrixXd blockSI( const vector<measurement_t> &z, const Quaterniond &q);
-        void featuresAsMeasurements(vector<measurement_t> &yk,
+        void featuresAsMeasurements(vector<measurement_t> &yk, const Camera &cam,
                 const Quaterniond &q);
 #endif
         int Hrows( const vector<measurement_t> &z );
diff --git a/src/types.h b/src/types.h
index d264cdc..d43fdf2 100644
--- a/src/types.h
+++ b/src/types.h
@@ -3,10 +3,12 @@
 
 #include <cv.h>
 #include <Eigen/Dense>
+#include <exception>
 #define MAXLINE 8192
 #define MAXFILENAME 1024
 #define STATESIZE 9            /* Set to 13 for quaternion estimation, or 9 for quaternion as input  */
 #define INLIER_THRESHOLD 12.
+#define PATCHSIZE 51            /* must be odd */
 using Eigen::Matrix;
 using Eigen::Matrix3d;
 using Eigen::Quaterniond;
@@ -22,7 +24,7 @@ typedef struct {
     Vector3d source, reflection;
     double height;
     Vector2d Ssrc, Sref;
-    cv::Mat patch;
+    cv::Mat patch, refpatch;
     double xcorrmax;
 } measurement_t;
 
diff --git a/src/vision.cpp b/src/vision.cpp
index 857bd0e..5d46f02 100644
--- a/src/vision.cpp
+++ b/src/vision.cpp
@@ -93,8 +93,12 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam,
             // Create rotated rect for ellipse
             cv::RotatedRect rrs(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.);
             cv::RotatedRect rrr(pr, cv::Size(2*INLIER_THRESHOLD*xr, 2*INLIER_THRESHOLD*yr), 0.);
-            cv::ellipse(display, rrs, ellcol );
-            cv::ellipse(display, rrr, ellcol);
+            try {
+                cv::ellipse(display, rrs, ellcol );
+                cv::ellipse(display, rrr, ellcol);
+            } catch (...) {
+                cerr << "caught and error drawing ellipse" << endl;
+            }
         }
     }
     return ;
@@ -169,7 +173,8 @@ 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);
-        zout.push_back(z);
+        if (z.xcorrmax>0.90)
+            zout.push_back(z);
     }
     return ;
 }		/* -----  end of method Vision::measurements  ----- */
@@ -184,8 +189,6 @@ Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin,
 void
 Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zout )
 {
-    zout.id = zin.id;
-
     // Create ROIs
     RotatedRect searchrr; // Rotated rect describing search ellipse
     Rect searchr; // Rect describing search ellipse
@@ -193,34 +196,59 @@ Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zo
     searchr = searchrr.boundingRect();
 
     // Add margin around search region
+    Rect imrect(cv::Point(0,0), gray.size());
     cv::Point offset(PATCHSIZE/2,PATCHSIZE/2);
     cv::Size enlarge(2*(PATCHSIZE/2),2*(PATCHSIZE/2));
     Rect roi = (searchr-offset) + enlarge;
-    Mat sr = gray(roi);
-
-    // 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_CCORR_NORMED);
-
-    // Mask the ellipse around the result.
-    Mat mask = Mat::zeros(result.size(), CV_8UC1);
-    RotatedRect ellipserr;
-    cv::Size sz = searchr.size();
-    ellipserr.size = sz;
-    ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height);
-    ellipserr.angle = 0;
-    cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 );
-    double minval, maxval;
-    int minidx[2], maxidx[2];
-    cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask);
-    cv::Point maxpt(maxidx[0], maxidx[1]);
-
-    cv::Point zpi = maxpt+searchr.tl();
-    Vector3d zi;
-    zi << zpi.x, zpi.y, 1;
-    zout.source = cam.img2body(zi);
-    zout.xcorrmax = maxval;
+    if ((roi & imrect)==roi) {
+        Mat sr = gray(roi);
+
+        // 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);
+
+        // Mask the ellipse around the result.
+        Mat mask = Mat::zeros(result.size(), CV_8UC1);
+        RotatedRect ellipserr;
+        cv::Size sz = searchr.size();
+        ellipserr.size = sz;
+        ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height);
+        ellipserr.angle = 0;
+        cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 );
+        double minval, maxval;
+        int minidx[2], maxidx[2];
+        cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask);
+        cv::Point maxpt(maxidx[1], maxidx[0]);
+
+        cv::Point zpi = maxpt+searchr.tl();
+        Vector3d zi;
+        zi << zpi.x, zpi.y, 1;
+
+        zout.source = cam.img2body(zi);
+        zout.xcorrmax = maxval;
+
+        Mat sr8, patch8, rpatch8;
+        sr.convertTo(sr8, CV_8UC1);
+        zin.patch.convertTo(patch8, CV_8UC1);
+        zin.refpatch.convertTo(rpatch8, CV_8UC1);
+        cv::circle(sr8, maxpt+offset, 4, cv::Scalar(255), 1);
+        cv::Point spt(PATCHSIZE/2, PATCHSIZE/2);
+        cv::circle(patch8, spt, 4, cv::Scalar(255), 1);
+        cv::circle(display, maxpt+searchr.tl(), 4, cv::Scalar(20,240,0),2);
+        cv::imshow("s region", sr8);
+        cv::imshow("patch", patch8);
+        cv::imshow("rpatch", rpatch8);
+        cv::imshow("result", result);
+        cv::imshow("mask", mask);
+        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  ----- */
 
diff --git a/src/vision.h b/src/vision.h
index a8ed318..aaa982b 100644
--- a/src/vision.h
+++ b/src/vision.h
@@ -8,7 +8,6 @@
 
 #include "camera.h"
 #include "types.h"
-#define PATCHSIZE 51            /* must be odd */
 using cv::Mat;
 using cv::RotatedRect;
 using cv::Rect;