implement Sobel Filter for sub pixel accuracy

CMakeLists.txt

@@ -12,6 +12,8 @@ include_directories( ${OpenCV_INCLUDE_DIRS} )
 set(SOURCES
         MarkerTracking.cpp
         src/Detector.hpp
-        src/Detector.cpp)
+        src/Detector.cpp
+        src/SobelFilter.hpp
+        src/SobelFilter.cpp)
 add_executable(MarkerTracking ${SOURCES})
 target_link_libraries (MarkerTracking ${OpenCV_LIBS})

MarkerTracking.cpp

@@ -68,7 +68,7 @@ int main() {
     }
   }
 
-  destroyWindow(contoursWindow);
+  destroyAllWindows();
 
   return (0);
 }

src/Detector.cpp

@@ -65,11 +65,22 @@ void Detector::drawRectangle(Mat &output, contour_t contour){
           double px = dx * j + contour[i].x;
           double py = dy * j + contour[i].y;
 
+
           Point p;
           p.x = (int)px;
           p.y = (int)py;
 
-          circle(output, p, 3, CV_RGB(0, 0, 255), -1);
-        }
+
+          cv::Point2d correction;
+          correction = sobelFilter.getSubPixelPoint(dx, dy, contour.at(i), p, output);
+
+          correction.x += p.x;
+          correction.y += p.y;
+
+          circle(output, correction, 3, Scalar(255,0,0), -1);
+          circle(output, p, 1, CV_RGB(0, 0, 255), -1);
+        }
+
+        sobelFilter.drawStrips(output);
       }
 }

src/Detector.hpp

@@ -1,11 +1,15 @@
+#pragma once
 #include <opencv2/opencv.hpp>
+#include "SobelFilter.hpp"
 
 #define THICKNESS_VALUE 4
 
 class Detector{
     public:
+        cv::Mat sobelDebug;
         void detect(cv::Mat thresholdFrame, cv::Mat &output);
     private:
+        SobelFilter sobelFilter;
         // List of points
         typedef std::vector<cv::Point> contour_t;
         // List of contours

src/SobelFilter.cpp

@@ -0,0 +1,125 @@
+#include "SobelFilter.hpp"
+
+cv::Point2d SobelFilter::getSubPixelPoint(float dx, float dy, cv::Point edge, cv::Point delimiter,cv::Mat input){
+    SampleStrip sample;
+
+    double diffLength = sqrt ( dx*dx+dy*dy );
+    int stripeLength = (int)(0.8* diffLength);
+
+    if (stripeLength < 5)
+        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) {
+            cv::Point2f subPixel;
+            subPixel.x = sample.vecX.x * m + sample.vecY.x * n;
+            subPixel.y = sample.vecX.y * m + sample.vecY.y * n;
+
+            subPixel.x += delimiter.x;
+            subPixel.y += delimiter.y;
+
+
+            //cv::circle(input, subPixel, 2, cv::Scalar(0,0,255), -1);
+            int pixel = samplePixel(input, subPixel);
+            int w = m + 1;
+            int h = n + (stripeLength >> 1);
+
+            iplStripe.at<uchar>(h,w) = (uchar)pixel;
+        }
+    }
+
+    //std::cout << p1.x << "||" << std::fabs(p1.x) << std::endl;
+
+    cv::Sobel(iplStripe, iplStripe, CV_8U, 0, 1);
+
+    return findSubPixelPoint(iplStripe, sample);
+}
+
+int SobelFilter::samplePixel(const cv::Mat &pSrc, const cv::Point2f &p){
+    int x = int( floorf ( p.x ) );
+    int y = int( floorf ( p.y ) );
+
+    if ( x < 0 || x >= pSrc.cols - 1 || y < 0 || y >= pSrc.rows - 1 )
+        return 127;
+
+    int dx = int ( 256 * ( p.x - floorf ( p.x ) ) );
+    int dy = int ( 256 * ( p.y - floorf ( p.y ) ) );
+
+    unsigned char* i = ( unsigned char* ) ( ( pSrc.data + y * pSrc.step ) + x );
+    int a = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
+    i += pSrc.step;
+    int b = i[ 0 ] + ( ( dx * ( i[ 1 ] - i[ 0 ] ) ) >> 8 );
+    return a + ( ( dy * ( b - a) ) >> 8 );
+}
+
+cv::Point2d SobelFilter::findSubPixelPoint(cv::Mat iplStripe, SampleStrip strip){
+
+    double maxIntensity = -1;
+
+    int maxIntensityIndex = 0;
+
+    // Finding the max value
+
+    for (size_t i = 0; i < iplStripe.cols; ++i) {
+        uchar intensity = iplStripe.at<uchar>(i,1);
+        if(intensity > maxIntensity){
+            maxIntensity = intensity;
+            maxIntensityIndex = i;
+        }
+    }
+
+    double y0, y1, y2;
+
+    // Point before and after
+    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);
+    y1 = iplStripe.at<uchar>(maxIntensityIndex,1);
+    // TASK: Why do we need this if statement?
+    y2 = (maxIntensityIndex >= strip.length - 3) ? 0 : iplStripe.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):
+    // xv = x1 + (d / 2) * (y2 - y0)/(2*y1 - y0 - y2)
+
+    // d = 1 because of the normalization and x1 will be added later
+    double pos = (y2 - y0) / (4 * y1 - 2 * y0 - 2 * y2);
+
+    cv::Point2d edgeCenter;
+
+    // TASK: What happens when there is no solution
+    if(std::isnan(pos)){
+        return edgeCenter;
+    }
+
+    edgeCenter.x = pos * (strip.vecX.x + strip.vecY.x);
+    edgeCenter.y = pos * (strip.vecX.y + strip.vecY.y);
+
+    return edgeCenter;
+}
+
+void SobelFilter::drawStrips(cv::Mat output){
+    sampleStrips.clear();
+    samples.clear();
+}

src/SobelFilter.hpp

@@ -0,0 +1,21 @@
+#pragma once
+#include <opencv2/opencv.hpp>
+
+struct SampleStrip{
+    float length;
+    cv::Point2f vecX;
+    cv::Point2f vecY;
+};
+
+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);
+
+    private:
+        int samplePixel(const cv::Mat &pSrc, const cv::Point2f &p);
+        cv::Point2d findSubPixelPoint(cv::Mat iplPixel, SampleStrip strip);
+        std::vector<SampleStrip> sampleStrips;
+        std::vector<char> samples;
+};