From ab566b209a8da69b1691f326b17b82a653f3c136 Mon Sep 17 00:00:00 2001 From: Lukas Moungos Date: Fri, 12 May 2023 15:31:41 +0200 Subject: [PATCH] implement Sobel Filter for sub pixel accuracy --- CMakeLists.txt | 4 +- ID7.png | Bin 0 -> 15435 bytes MarkerTracking.cpp | 2 +- src/Detector.cpp | 13 ++++- src/Detector.hpp | 4 ++ src/SobelFilter.cpp | 125 ++++++++++++++++++++++++++++++++++++++++++++ src/SobelFilter.hpp | 21 ++++++++ 7 files changed, 166 insertions(+), 3 deletions(-) create mode 100644 ID7.png create mode 100644 src/SobelFilter.cpp create mode 100644 src/SobelFilter.hpp 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 0000000000000000000000000000000000000000..a0faa3a200bbc5a57095c11f6464b124697d135b GIT binary patch literal 15435 zcmeHOX;f3!7EZ1K18Fc!q5*6`ER%p16a^_)L?|djMWqUe+V>CyCDb7xH6)CRqJ3!9 zLYWl@1XN@Y)P$sBEmA4QQ$0%3mzKQ)s?P9I>)@!?Zf3R39`<}bcIeYJK z@AK_@b_V);Gqu^;6bgmu^Red=3I!3t-wwJa{E`rnJw%~!C_bL^m&UK^?`p3wcK(vq zrW|qaG%hc&pBTN^iFxKbmxKf9E!SJt^mt&EvXSAJdq?Oj4l1Bf5gOfqj_dIm^)b6> zzx_61%fZ<+5UHLKvZ;#N(vI1_>c?3e4xU4dBZYtNCJ5`Xp}t#{IS)Fm-kL)$qT|DF zhD2ZB-?eIsyled0f3s2oRl&Qrc}V|F4=<5n@Ye9D+H_G-RzlDLY{;vXr=_-Mu12bi z(O`?Oy1g{ST>YRIEmfx5bTWf3TJa{3sn+%7R1W^&+1itO3f}H(DQ|tD86IqAzrM2v zOI#DQVW-NwSd(;FeD6tBQ_S&*=L=@3Cxq@1V7b?Ko%T(Y&Gen{R*k(ri|-KKyUtZI znC4eKO})9RJyE6UnycG(V7VO=I#g4nB6AT*tiMB(jV}MPmD>)rn!3fP^1{iYf_}Ql zXm-bew=>d(b*IkkGn2$FcVD_d?Z7|L>7uz8CY5TBHn(N_@kT8V#@@Zv~^Q(sA<@Pzd zHy+Fi)2z2K2eJ9xVk@K?#8p^`>%Bzhd)&Gg+97WYAmi=#KfVvmK&5Y{<6EzO-eqsB z6KqnSYz`&BcQsa-Xt_#qbI;3rSl}T-Ly3{bE zqVQv?!ugC{rg@;PN>Y9Q0`EREBf9cbM*tK60YC3|(b`OR>S~q5UY>rr|BQlrsmRu9 z398JB*;P!xp{A+@ukCZi%T)X0|3UYn(rym^;%Zf%9*aM29jau;WMnxAcj=|)|_ zGj@r7$~!x$>vI|%Uk0zk4u)q4(Br6-F_3ywTV%P}CyTa?#vagA)UM0#-!CX|qx~Sj zjBguE)3uri{7uZoT@NIso!5d9rbAuj>WN8LT%%^z_0U;ZRYsD7Smw7$UL5tP=EAUF z;p5)RIw`y*G0_HIg-ZxkRT$ks&I-hylgt-i3JZ=e7@W$kMoIGqC=&KE{4Kq4Kfq_J0V!yBg$ zXY;PDbbuwZyyV>sUwL5>hp)L&_f=eJFprPr$)S!bLo%TK5YLijIObH+(xR|oC) zEc_H`rBko$a~Qen7lmuVc3(_hflnJed`W&_Dk^TBg1d_RsX~8HUNR_&^?HBle1Z6V zLPzVA#je67#HMPM8}b8sv)NkDpa5n{Vq53kwHCgw)^0yqyChcr9$h5#`$SK;7HV|K zgN8`^m=ku}r?p|pL)sC)96Y~oVoJy!ickbqGjsS!AB$yWsIvCT(bk4d6;B3}=N4Na zmk?mqT6pcDK<&Nug{^#Pc=bnz1WP;3S`#WPS}uFFMVr1ESi#BLjrfkQ3$+p2FfnER zi`AW4`?XcRR{po}wecfAR0g4iL8!8&w*kv-AHR&n!HDp{s>WaUFI=kfqv+9oB!|Ng zc=`dxXivwC$L16AO2REgxC@Qus{!6;d3L$dt!UADi?*q4TxYt-?P<(W8vfXftC=bj zms;&aPYd9D`83Rlpe--zfg!hRR4EtT^V!vC=PiH$Q}XzE2$Bb!r-F$rPbG zp`*a<(AGcjVt2y#EzgFI)((ot<`YSV5EOXNEi+D_4S1hxQT6Dcl&_m0bmZWgvlsf% zG70iHcd+=}spB=zz9P2L_Ryv9C;ltn+c+12MK_9`_8G8x;RNUS+8&R1Hs26nh6p?m zoz;K07c0$F+IvQ82gPHz!t|jP%9cab^)|rqg#~Rm1RX91;KPjKQ^GgY6{N`;g})qoh%hty&ZP(FK^qq^ zXy!PA-^-mDy=9q`Nkx5W=L~4$tb$IL1EwkJB)GbiarZxI)nMz~+QcldAR2oL=g&0R z1MA@5%||C9=Y($Yt6l@$u=6YyKU~h_S1b4v`QyhomE<$vjStx$`4H_hNo>=5Dz2{S zWXH00Zr!7t^-}2R#hY>Pi(+GZJwYYv=jT+Kre8s@yJgR(25UH8q*Ecbk11w=O?gX#rzk>XRK+RN~~S<-rT3iWAW%nEHUR zX##8riN(r@^}5cPG39w^p@Qpn3-cj5d5~Bl3WpGN^0=$#JvxU<727Hl`-pxdV8vAx(;0DfWtbr|qpafi?8RUWBdC`c=Nl_$!|7sB zE{?ykNtiBu9xAmYTBT4~91{M;4D~Q_$^;6JaKj@GN-%iEbBj)^PU!cwh3UL(tYK~T$@x$;x+m_)hibFlEi=T5gWG?* zs(QasmrYb3RDhl^D&fadLyiOK-KMAbnW)tDN;p5L)X$VgU#jgC>SXDpW=}$a0r1!3 z2QSW^1r-~dvD@K%=~e3;$AD97+U>FfPm)8XZ7@Y>`E(c+V6ru-sryvJxSIjGk@1$V znq$8|dpw0A^d+_{wMorm+7k1Z3}*VA(S&*v7@2Lr$n?Ij+E^sO8d!-POIC5q4wo+w zi1mq4JEa4yb;etH;@8&1Rwljj`+D{k7AMfUjobcB+ovk#EiMh_@1qQx>2s5)$_v~) zTY($xo^r_!7!RYhl`FqzZ2ao%7ATj^Vbr|1+ixcGqysvix-TqLd1Jl+J)$!AXTN7|;5yxa>)f1ZP#cIU!**O)Z|zXG?AlE; zRK62NJ^gmg%XMAM2G`YZ3<#r&@Y2P(E_Bv0gUGr_|HzZ7qko+T%-;g9IAjqGl`$OG zi{ZGQwQ=u{6M;))oDR0@(Wv$9bfMAb0oN0mKj-oA?uIqI@h1z+!KhCT%JxSEji(}C z5$>gxrRGY5iTS6>4EOzu34RV_2I-WfQ<6?eA|;8GBvPuHGi3TED!Kn_`X)ZV7e_(i z5oMYfScwf$u6V!w&fb@3RMs%aPw#;?B<~!Mmn~>?@|FpCy#~4!&DLKUzJf2Rvo zHE-|9u{dpZSf4C)q#;*4puQalD_IHMyaWDsEr}MLsvdBcRGR&Em6UZ1zV(k@xuHZY lBl?AvlS$#9u?ZvNy7ulotlvAoA1(<~d=~n9R(PyV{uR?e<9q-B literal 0 HcmV?d00001 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; +};