Fix Sobel Filtering

src/Detector.cpp

@@ -36,16 +36,16 @@ void Detector::detect(Mat thresholdFrame, Mat &output){
       //DEBUG
       //rectangle(filteredFrame, r, Scalar(0,0,255),4);
 
-      drawRectangle(output, approx_contour);
+      drawRectangle(output, thresholdFrame, approx_contour);
     }
   }
 
-void Detector::drawRectangle(Mat &output, contour_t contour){
+void Detector::drawRectangle(Mat &output, Mat threshold, contour_t contour){
       // -> Cleaning done!
 
+      //polylines(output, contour, true, Scalar(0,0,255), THICKNESS_VALUE);
       // 1 -> 1 contour, we have a closed contour, true -> closed , 4 ->
       // thickness
-      polylines(output, contour, true, Scalar(0,0,255), THICKNESS_VALUE);
 
       // -----------------------------
 
@@ -59,6 +59,9 @@ void Detector::drawRectangle(Mat &output, contour_t contour){
         double dx = (double)(contour[(i+1)%4].x - contour[i].x) / 7;
         double dy = (double)(contour[(i+1)%4].y - contour[i].y) / 7;
 
+        SampleStrip strip = sobelFilter.getSampleStrip(dx, dy);
+
+
         // First point already rendered, now the other 6 points
         for (size_t j = 1; j < 7; j++) {
           // Position calculation
@@ -70,9 +73,8 @@ void Detector::drawRectangle(Mat &output, contour_t contour){
           p.x = (int)px;
           p.y = (int)py;
 
-
           cv::Point2d correction;
-          correction = sobelFilter.getSubPixelPoint(dx, dy, contour.at(i), p, output);
+          correction = sobelFilter.getSubPixelPoint(p, strip, threshold);
 
           correction.x += p.x;
           correction.y += p.y;

src/Detector.hpp

@@ -14,5 +14,5 @@ class Detector{
         typedef std::vector<cv::Point> contour_t;
         // List of contours
         typedef std::vector<contour_t> contour_vector_t;
-        void drawRectangle(cv::Mat &output, contour_t contour);
+        void drawRectangle(cv::Mat &output, cv::Mat threshold ,contour_t contour);
 };

src/SobelFilter.cpp

@@ -1,33 +1,11 @@
 #include "SobelFilter.hpp"
 
-cv::Point2d SobelFilter::getSubPixelPoint(float dx, float dy, cv::Point edge, cv::Point delimiter,cv::Mat input){
+cv::Point2d SobelFilter::getSubPixelPoint(cv::Point delimiter, SampleStrip sample,cv::Mat input){
-    SampleStrip sample;
 
-    double diffLength = sqrt ( dx*dx+dy*dy );
+    int nStart = sample.nStart;
-    int stripeLength = (int)(0.8* diffLength);
+    int nStop = sample.nStop;
-
+    int stripeLength = sample.length;
-    if (stripeLength < 5)
+    cv::Mat iplStripe = sample.iplStripe;
-        stripeLength = 5;
-
-    sample.length = stripeLength;
-
-    cv::Point2f p1;
-    cv::Point2f p2;
-
-    p1.x = dx / diffLength;
-    p1.y = dy / diffLength;
-
-    p2.x = p1.y;
-    p2.y = -p1.x;
-
-    sample.vecX = p1;
-    sample.vecY = p2;
-
-    int nStop = stripeLength / 2;
-    int nStart = -nStop;
-
-    cv::Size size(3,stripeLength);
-    cv::Mat iplStripe(size, CV_8UC1);
 
     for (int m = -1; m <= 1; ++m) {
         for (int n = nStart; n <= nStop; ++n) {
@@ -46,11 +24,52 @@ cv::Point2d SobelFilter::getSubPixelPoint(float dx, float dy, cv::Point edge, cv
         }
     }
 
-    cv::Sobel(iplStripe, iplStripe, CV_8U, 1, 0);
+    cv::Mat grad_y;
+    cv::Sobel(iplStripe, grad_y, CV_8U, 1,0);
 
-    return findSubPixelPoint(iplStripe, sample);
+    return findSubPixelPoint(grad_y, sample);
 }
 
+
+SampleStrip SobelFilter::getSampleStrip(float dx, float dy){
+     SampleStrip sample;
+
+    double diffLength = sqrt ( dx*dx+dy*dy );
+    int stripeLength = (int)(0.8* diffLength);
+
+    if (stripeLength < 5)
+        stripeLength = 5;
+
+    if(stripeLength % 2 == 0){
+        stripeLength++;
+    }
+
+    cv::Point2f p1;
+    cv::Point2f p2;
+
+    p1.x = dx / diffLength;
+    p1.y = dy / diffLength;
+
+    p2.x = p1.y;
+    p2.y = -p1.x;
+
+    int nStop = stripeLength / 2;
+    int nStart = -nStop;
+
+    cv::Size size(3,stripeLength);
+    cv::Mat iplStripe(size, CV_8UC1);
+
+    sample.length = stripeLength;
+    sample.vecX = p1;
+    sample.vecY = p2;
+    sample.nStart = nStart;
+    sample.nStop = nStop;
+    sample.iplStripe = iplStripe;
+
+    return sample;
+}
+
+
 int SobelFilter::samplePixel(const cv::Mat &pSrc, const cv::Point2f &p){
     int x = int( floorf ( p.x ) );
     int y = int( floorf ( p.y ) );
@@ -68,7 +87,7 @@ int SobelFilter::samplePixel(const cv::Mat &pSrc, const cv::Point2f &p){
     return a + ( ( dy * ( b - a) ) >> 8 );
 }
 
-cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip){
+cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat input, SampleStrip strip){
 
     double maxIntensity = -1;
 
@@ -76,8 +95,8 @@ cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip)
 
     // Finding the max value
 
-    for (size_t i = 0; i < iplStripe.cols; ++i) {
+    for (size_t i = 0; i < input.cols - 2; ++i) {
-        uchar intensity = iplStripe.at<uchar>(i,1);
+        uchar intensity = input.at<uchar>(i,1);
         if(intensity > maxIntensity){
             maxIntensity = intensity;
             maxIntensityIndex = i;
@@ -90,10 +109,10 @@ cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip)
     unsigned int max1 = maxIntensityIndex - 1, max2 = maxIntensityIndex + 1;
 
     // TASK: Why do we need this if statement?
-    y0 = (maxIntensityIndex <= 0) ? 0 : iplStripe.at<uchar>(max1,1);
+    y0 = (maxIntensityIndex <= 0) ? 0 : input.at<uchar>(max1,1);
-    y1 = iplStripe.at<uchar>(maxIntensityIndex,1);
+    y1 = input.at<uchar>(maxIntensityIndex,1);
     // TASK: Why do we need this if statement?
-    y2 = (maxIntensityIndex >= strip.length - 3) ? 0 : iplStripe.at<uchar>(max2,1);
+    y2 = (maxIntensityIndex >= strip.length - 3) ? 0 : input.at<uchar>(max2,1);
 
     // Formula for calculating the x-coordinate of the vertex of a parabola, given 3 points with equal distances
     // (xv means the x value of the vertex, d the distance between the points):
@@ -109,8 +128,10 @@ cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip)
         return edgeCenter;
     }
 
-    edgeCenter.x = pos + (strip.vecX.x + strip.vecY.x);
+    int maxIndexShift = maxIntensityIndex - (strip.length >> 1);
-    edgeCenter.y = pos + (strip.vecX.y + strip.vecY.y);
+
+    edgeCenter.x = ((double)maxIndexShift + pos) * strip.vecX.x;
+    edgeCenter.y = ((double)maxIndexShift + pos) * strip.vecY.y;
 
     return edgeCenter;
 }

src/SobelFilter.hpp

@@ -2,16 +2,22 @@
 #include <opencv2/opencv.hpp>
 
 struct SampleStrip{
-    float length;
+    int length;
     cv::Point2f vecX;
     cv::Point2f vecY;
+
+    int nStart;
+    int nStop;
+
+    cv::Mat iplStripe;
 };
 
 class SobelFilter{
 
     public:
         void drawStrips(cv::Mat output);
-        cv::Point2d getSubPixelPoint(float dx, float dy,const cv::Point edge, cv::Point delimiter,cv::Mat input);
+        cv::Point2d getSubPixelPoint(cv::Point delimiter, SampleStrip sample,cv::Mat input);
+        SampleStrip getSampleStrip(float dx, float dy);
 
     private:
         int samplePixel(const cv::Mat &pSrc, const cv::Point2f &p);