From bb8e5846bbe283796f37706a0b168745afd7fa3b Mon Sep 17 00:00:00 2001 From: hturner08 Date: Sat, 5 Mar 2022 18:58:24 +0000 Subject: [PATCH 1/4] Added rotation to fix flipped ZED Camera --- scripts/cone_detector.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/scripts/cone_detector.py b/scripts/cone_detector.py index d7be9e6..91d3254 100644 --- a/scripts/cone_detector.py +++ b/scripts/cone_detector.py @@ -2,12 +2,13 @@ import numpy as np import rospy +import imutils import cv2 from cv_bridge import CvBridge, CvBridgeError from sensor_msgs.msg import Image -from geometry_msgs.msg import Point #geometry_msgs not in CMake file +from geometry_msgs.msg import Point # geometry_msgs not in CMake file from visual_servoing.msg import ConeLocationPixel # import your color segmentation algorithm; call this function in ros_image_callback! @@ -20,15 +21,19 @@ class ConeDetector(): Subscribes to: /zed/zed_node/rgb/image_rect_color (Image) : the live RGB image from the onboard ZED camera. Publishes to: /relative_cone_px (ConeLocationPixel) : the coordinates of the cone in the image frame (units are pixels). """ + def __init__(self): # toggle line follower vs cone parker self.LineFollower = False # Subscribe to ZED camera RGB frames - self.cone_pub = rospy.Publisher("/relative_cone_px", ConeLocationPixel, queue_size=10) - self.debug_pub = rospy.Publisher("/cone_debug_img", Image, queue_size=10) - self.image_sub = rospy.Subscriber("/zed/zed_node/rgb/image_rect_color", Image, self.image_callback) - self.bridge = CvBridge() # Converts between ROS images and OpenCV Images + self.cone_pub = rospy.Publisher( + "/relative_cone_px", ConeLocationPixel, queue_size=10) + self.debug_pub = rospy.Publisher( + "/cone_debug_img", Image, queue_size=10) + self.image_sub = rospy.Subscriber( + "/zed/zed_node/rgb/image_rect_color", Image, self.image_callback) + self.bridge = CvBridge() # Converts between ROS images and OpenCV Images def image_callback(self, image_msg): # Apply your imported color segmentation function (cd_color_segmentation) to the image msg here @@ -45,7 +50,7 @@ def image_callback(self, image_msg): ################################# image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") - + image = imutils.rotate(image, 180) # ZED camers are upside down debug_msg = self.bridge.cv2_to_imgmsg(image, "bgr8") self.debug_pub.publish(debug_msg) From 8098dabf2c7e82e6d7d8f4bba5424d9f245ec4d5 Mon Sep 17 00:00:00 2001 From: cindywangg Date: Sat, 5 Mar 2022 22:53:19 +0000 Subject: [PATCH 2/4] add sift impl --- .../computer_vision/color_segmentation.pyc | Bin 2396 -> 1372 bytes scripts/computer_vision/sift_template.py | 14 ++++++++------ scripts/computer_vision/sift_template.pyc | Bin 4369 -> 3756 bytes 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/scripts/computer_vision/color_segmentation.pyc b/scripts/computer_vision/color_segmentation.pyc index 3d4dc9bff1ab8eaa8c750de74c57f3b33de4cf9c..b51e6178c340a2ea8c9e752008d9e83cc6d3ed4b 100644 GIT binary patch delta 339 zcmca3bcait`7Kmuq8P+2)=;Su+Aoz{2nh{eP>T>D3T;J6qf1pFs$|Z0YkQk}<908$ zrMpkz8$W@c$qy*c%*BE92Y~Hc&(6*~JG1lbT=TDfw|sc|Yap&a4g7zL$9#_=#Lp3t z5a&0tBhH$<18!it|P<%L=jRFEcCCxybAZDT(M z)DXl8CuAvw;PSwBc+t$#xxE8%?rZylTH0;Mp)xj4mxodGlTN~PZibXQd+K;Pge-V4 z0i^v|8ms->4^-gitN+p5?3+B;ztpDiqmfbhWr{>TH)&K*()%XJb!N?ekj7bIRX&0x z>4P*%bJ$N~l~|ul_p+s3$3C3Mh)-$^#sP3?_D@xd73BL{-ud%)>%C(iA?48PGJs4FB>kQImWlxl~(C6s}1@jj1 ztc$8As=g?DI1?xAiLfsw*m^^h8{*7v(n46nZ6RBy+GPK2QEpQ5Z(E|;64kb-c0_eo zlv|?Q5g%9gLk!ynOFT)U~96-ojNjFy*vR49X8JY|bKp^(Spbn!$E zhl|I%a{2htt{g57ARRn#h)_=C%NPi$G+g zCTJtIAMJ(45Z>&uvbAX@b3L8ivh2FtBlJB~N>jP?dkmIZVCIs=le>^S#$AIHT-qbzg6(5+^y=j+Z-lxXJU@G5 zX{RLP4#(B!9;Oq|Po|37#aP?9Hfn?u;4DBi!{!BA?N=W+ zjttksT=@*QDzXRCGh7YwcF4;Ru=jfJc&WTcVMTWP@wZfwg z$i6iPRmkaGs60^-9}#kQ(U3z4u8O_*J0Ng&oru@2eaj^3`U@s-GcL2 zPx02WqX7J+Tp4i@6;kthVmx568 diff --git a/scripts/computer_vision/sift_template.py b/scripts/computer_vision/sift_template.py index 980f0bb..b3571eb 100644 --- a/scripts/computer_vision/sift_template.py +++ b/scripts/computer_vision/sift_template.py @@ -67,12 +67,14 @@ def cd_sift_ransac(img, template): h, w = template.shape pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2) - ########## YOUR CODE STARTS HERE ########## - - x_min = y_min = x_max = y_max = 0 - - ########### YOUR CODE ENDS HERE ########### - + transformed_pts = cv2.perspectiveTransform(pts, M) + box = [np.int32(transformed_pts)] + x_s = [x[0][0] for x in box[0]] + y_s = [x[0][1] for x in box[0]] + x_min = min(x_s) + y_min = min(y_s) + x_max = max(x_s) + y_max = max(y_s) # Return bounding box return ((x_min, y_min), (x_max, y_max)) else: diff --git a/scripts/computer_vision/sift_template.pyc b/scripts/computer_vision/sift_template.pyc index bdf69d9cb6f187f6582ccc8e3ca8b47dbd14ace5..956fadf675958b86de18b768a92994094202d38a 100644 GIT binary patch delta 956 zcmbtTzfapx5dNM`9NTdM0umBPKnemGb%@lo65aX(l`idI1T2Nf*Z~4f0`>!fWQ&1H z-73+yv_t=cE+~IOJ9X%P(V;`9s_Im!_Z*>iK^@TZeedqO^F7_&dl%IY6rBs=L()Hh=&lT5UJQx5dpp_E|ixn>KD{$c4lI)fGCf1h8>2u zfE^$zz}Hb)+89SXfoLMJB(tgD@E41V*9U2Xpt2H|!0F$jOdL-pdMVMTz>8OW84si} za8wXYBAUXsf~btBA{F{pd;`neKt$y*EI;ux@U4Nv#EPBm&!jviK5Mr$g-UPXO}fZR zeVcG9C5aBN-{vZ5ZBmon?b*}fmHvK>XC&=%Z?_pfb;Etm$lguxBnhPk^I_t6;C908 zZqRP}4avzgTisp~*-NZmW5Av^La}MoZ#K`b#spgwzm53?Wt5GwIa=0WDFt;{^<<%0 zk~B#&@9z+fxmqz0r_p;2u3;|?e+YgGKEt_+rHvEu$!ryT6VyNIYHZ=KgZ0Rw7FvDV z+GqMR`)>m?;%By=k$1|;Nf;OPT+OoOp^iM&nH870*tjFry9DCxaQS96>m$Xpb;=YL z31H;aJj<(Ns-~8ec{WC(=_9JGlh2V;avl?KIpEGw`{2|Wm!=%cfxqP#v^)1DI0QDy RKM-E~)Mozw)*iBZe*lm=+Cu;U delta 1543 zcmbVMy>Ht_6n{s3QldUA+xm!=KsC@pP2mWzI;p6Ez+Xub*#YggK;b~3bhJ&IB31Ha zI{*m{>Q=Nsz@by;EL{o@ojMfl+94Ucbne_gzWVFpPEGL#vV z;X6c-_&g1@*$Ar>T5@LcB-Fy1^u*psy7eCq<=`4s2x@SsK!DLDQt;j51=toUEs9^U z{eYK%&tC_Mu-9CmgSg88GU~(O$To+jBQA4^$LxW1g&`}30*?V;Xo$h_x@)_hC{hic zs%<{T!xh)g>@3IhyM})%5@;XV#*rUANq&?i7z+njSJC&$YIIL4u=?fmWR-2xb&Op5 zscku?FUt_Ky(;49-}lC*NTKrR;m&67{(b4>)<6haj(f&=m$qg4&3g1#>c@_rL$|=M zfnzDp1uZ-hzMQ%Lx~?}eJY(ofKx=r^j{Z!y-<4QjsTH;sdFltPM2W2^8cX7%viFn{ zlTV@?E5rs0?xc)^K1b5;C5wJjKfkR(n1YLcKqTQL1w0Ns0i3}!1{+UaM(vC>A-ZKH zeN@RNje?;jb)*}b3=ZND<{->VS%3hMG&oIz-(*2vnnQwN4WvCxutT=&i4#EOv&^8Zf|{k zcXMm2x3jBvH#e}8FwJth#>FFdAT7@v2!pzzN!P2>{pxq4U$pR~L+U3e@NNM-(;wSH z^0Fr#L*wEL!=9-GN44nfouPZC+vule@(f|QB8^(V-}8mx30aAT>G0^&T>ahX+e!8j z5|8*rt~!ZVl_geZ^_a#Ml?6H1V+p1yOmOh&!Af9 From 92ea131333c80500e4e6eb86571b816668184cfd Mon Sep 17 00:00:00 2001 From: cindywangg Date: Sun, 6 Mar 2022 00:58:21 +0000 Subject: [PATCH 3/4] add template --- scripts/computer_vision/sift_template.py | 23 +++++++++++++++------- scripts/computer_vision/sift_template.pyc | Bin 3756 -> 4010 bytes 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/scripts/computer_vision/sift_template.py b/scripts/computer_vision/sift_template.py index b3571eb..14a8f81 100644 --- a/scripts/computer_vision/sift_template.py +++ b/scripts/computer_vision/sift_template.py @@ -30,6 +30,7 @@ def image_print(img): def cd_sift_ransac(img, template): """ Implement the cone detection using SIFT + RANSAC algorithm + Notes: needs to be good contrast between the feature and background/good for rotation changes Input: img: np.3darray; the input image with a cone to be detected Return: @@ -87,12 +88,16 @@ def cd_sift_ransac(img, template): def cd_template_matching(img, template): """ Implement the cone detection using template matching algorithm + Notes: doesn't work well with scale/rotation changes, good for detecting edges Input: img: np.3darray; the input image with a cone to be detected Return: bbox: ((x1, y1), (x2, y2)); the bounding box of the cone, unit in px (x1, y1) is the bottom left of the bbox and (x2, y2) is the top right of the bbox """ + # methods = ['cv.TM_CCOEFF', 'cv.TM_CCOEFF_NORMED', 'cv.TM_CCORR', + # 'cv.TM_CCORR_NORMED', 'cv.TM_SQDIFF', 'cv.TM_SQDIFF_NORMED'] + template_canny = cv2.Canny(template, 50, 200) # Perform Canny Edge detection on test image @@ -105,6 +110,8 @@ def cd_template_matching(img, template): # Keep track of best-fit match best_match = None + best_score = 0 + # Loop over different scales of image for scale in np.linspace(1.5, .5, 50): # Resize the image @@ -114,13 +121,15 @@ def cd_template_matching(img, template): if resized_template.shape[0] > img_height or resized_template.shape[1] > img_width: continue - ########## YOUR CODE STARTS HERE ########## - # Use OpenCV template matching functions to find the best match - # across template scales. + res = cv2.matchTemplate(img_canny, resized_template, cv2.TM_CCOEFF_NORMED) + res_score = np.max(res) + if res_score > best_score: + best_score = res_score - # Remember to resize the bounding box using the highest scoring scale - # x1,y1 pixel will be accurate, but x2,y2 needs to be correctly scaled - bounding_box = ((0,0),(0,0)) - ########### YOUR CODE ENDS HERE ########### + loc = np.where(res == res_score) + x_min, y_min = loc[0][0], loc[1][0] + #TODO: should we rescale w, h back? i think not + x_max, y_max = x_min + w, y_min + h + bounding_box = ((x_min, y_min), (x_max, y_max)) return bounding_box diff --git a/scripts/computer_vision/sift_template.pyc b/scripts/computer_vision/sift_template.pyc index 956fadf675958b86de18b768a92994094202d38a..d6928b77c37045106eea6ba72aaa26d7c3be6b99 100644 GIT binary patch delta 554 zcmXw#PiqrF7{;HO+3aRF`zLL(v;|{DiYy`p@lr(VA#HDMAQnVXLd|Z8X=61Ru!k86 z_U>iC4`J&;`~-fIegSWK*Y`~+ynh~k^UTaUbJ_W}8mK=>)T^!i2rj}b2M|zpDQBOB z`xU-AD?QjcPCG*T7zZe8D1DRy=L%bId|^ZAI>sT4L>WLwEXE$nI#x`G_BlC)u!N&I zR=D$8!Zu*z8|0C*h9eI%eS>(j&uDsfg^wEV0S$~BFdkc2$2@^ez+)va30(u5!cf%b zJL#~LM`r^;@}xMDb6Wxl?JR delta 277 zcmXwzPfEi;7{%XYlKGdJR;YmrDOLoDPz29VPoN8-=xz{=7|;fEk>H}>vS8;H>PEx^ zcm$UoLBW*=&>5kNAAJ1Yd+@amW3}#Y>&`iM!Vu$(eU>cWg*f_o^?V!nlb! mb?;`|jXI0GA7Yu&jAXP&8O@_D((^E}Lfuzqm+ZT|JO2ZuK{a6j From 1dd7fcd13ba331e6d221f07dbac7b0fce4e10a3b Mon Sep 17 00:00:00 2001 From: cindywangg Date: Wed, 9 Mar 2022 16:15:05 +0000 Subject: [PATCH 4/4] fix --- scripts/computer_vision/sift_template.py | 23 ++++++++++++++-------- scripts/computer_vision/sift_template.pyc | Bin 4010 -> 4177 bytes 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/scripts/computer_vision/sift_template.py b/scripts/computer_vision/sift_template.py index 14a8f81..2449f9f 100644 --- a/scripts/computer_vision/sift_template.py +++ b/scripts/computer_vision/sift_template.py @@ -121,14 +121,21 @@ def cd_template_matching(img, template): if resized_template.shape[0] > img_height or resized_template.shape[1] > img_width: continue - res = cv2.matchTemplate(img_canny, resized_template, cv2.TM_CCOEFF_NORMED) - res_score = np.max(res) - if res_score > best_score: - best_score = res_score - - loc = np.where(res == res_score) - x_min, y_min = loc[0][0], loc[1][0] - #TODO: should we rescale w, h back? i think not + # res = cv2.matchTemplate(img_canny, resized_template, cv2.TM_CCORR) + # res_score = np.max(res) + # if res_score > best_score: + # best_score = res_score + + # loc = np.where(res == res_score) + # x_min, y_min = loc[0][0], loc[1][0] + # x_max, y_max = x_min + w, y_min + h + # bounding_box = ((x_min, y_min), (x_max, y_max)) + res = cv2.matchTemplate(img_canny, resized_template, cv2.TM_CCORR) + min_v, max_v, min_pt, max_pt = cv2.minMaxLoc(res) + if max_v > best_score: + best_score = max_v + + x_min, y_min = max_pt[0], max_pt[1] x_max, y_max = x_min + w, y_min + h bounding_box = ((x_min, y_min), (x_max, y_max)) diff --git a/scripts/computer_vision/sift_template.pyc b/scripts/computer_vision/sift_template.pyc index d6928b77c37045106eea6ba72aaa26d7c3be6b99..9d76eafd86ca1d65e8b7d404ecd210e63ebb37ba 100644 GIT binary patch delta 541 zcmZ9IyKdA#6o${(j`!lVmt^BO5sR26VL_mD6iXE?4H5}bKmq|#hrH8& zovpnci9G94!8yw)O4{diC?%tb6qv>t8Uu6A1;r7k10LgX%4sYZ?Z?Svn95_pHfIk8 zGNq}+7{^l3WE6`b&-9u1^Zb^3&nBMEYj=VO04hd3qwGk3t@T$M1YJ0&09yd&0wZv0 z!`;2#pu6?AuK^BxP(GXzc-X7Q2MZYnFm zO6(+mAove#{2LY)mU=hBId{H$?-`i+sJxbS`P(u&>e`$Bqp(f!s~pUp)SAK!@tNGW zT>v_{P7dxx**GnhSg7Gbho{2R;4w^PG)^=o9*w&j+F0NgrVNn{PD)^;U=qe0B@kbw zA!Tfgq(#gDci<^liXCI(AEhXbCsMkBqR%>wB>9(7V?g|Y2z61 UJUF%FF+~@7F=xKmO;_07A5E=FZ~y=R