diff --git a/CMakeLists.txt b/CMakeLists.txt index 326c131..5ca656b 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/ID7.png b/ID7.png new file mode 100644 index 0000000..a0faa3a Binary files /dev/null and b/ID7.png differ diff --git a/MarkerTracking.cpp b/MarkerTracking.cpp index 950e542..ace8b05 100644 --- a/MarkerTracking.cpp +++ b/MarkerTracking.cpp @@ -68,7 +68,7 @@ int main() { } } - destroyWindow(contoursWindow); + destroyAllWindows(); return (0); } diff --git a/src/Detector.cpp b/src/Detector.cpp index 8496fcb..f76e728 100644 --- a/src/Detector.cpp +++ b/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); } } diff --git a/src/Detector.hpp b/src/Detector.hpp index 61a9bfb..de4fa01 100644 --- a/src/Detector.hpp +++ b/src/Detector.hpp @@ -1,11 +1,15 @@ +#pragma once #include +#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 contour_t; // List of contours diff --git a/src/SobelFilter.cpp b/src/SobelFilter.cpp new file mode 100644 index 0000000..247499f --- /dev/null +++ b/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(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(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(max1,1); + y1 = iplStripe.at(maxIntensityIndex,1); + // TASK: Why do we need this if statement? + y2 = (maxIntensityIndex >= strip.length - 3) ? 0 : iplStripe.at(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(); +} diff --git a/src/SobelFilter.hpp b/src/SobelFilter.hpp new file mode 100644 index 0000000..3f99af8 --- /dev/null +++ b/src/SobelFilter.hpp @@ -0,0 +1,21 @@ +#pragma once +#include + +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 sampleStrips; + std::vector samples; +};