设为首页收藏本站
首页 > 论坛 > Mind+

[教程]基于Opencv和行空板完成大疆EP自瞄-装甲板检测

Cm4iHKyN3ptj | 学徒 | 创造力： | 帖子： | 发消息 | 串个门 | 加好友 | 打招呼
2022-9-7 16:41:53 [显示全部楼层]
3554浏览
1回复
1收藏
查看: 3554|回复: 1

[教程] 基于Opencv和行空板完成大疆EP自瞄-装甲板检测

[复制链接]
本帖最后由 Cm4iHKyN3ptj 于 2022-9-7 16:43 编辑

去年年底就关注到DF的行空板，从接口和处理器以及价钱来看是一款不错的硬件，因为自己手上也有比较多的DF传感器，所以一直想入手，在漂流活动中，成功的获取到了行空板，自己又在做自瞄，就想尝试一下行空板，刚刚行空板也有货了，所以就果断入手了几块，为了完成任务，就写了这个帖子，有什么不对的地方，请大家多多指教。
整个程序主要分为5部分，分别为图像获取——图像处理——轮廓检测——数据下发——EP控制，这一篇文章先完成前面四部分。
用的硬件主要有DF行空板，Microsoft摄像头，EP机器人，主要用到的语言为python

1.图像获取
因为是才用摄像头和opencv，所以图像获取是采用以下程序。
  1. cap = cv2.VideoCapture(VIDEO_INDEX) # 打开摄像头

  2.     frame_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) # 480图像的高

  3.     frame_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) # 640图像的宽

  4.     gun_point = (int(frame_w/2),int(frame_h/2) + GUN_ERROR)#图像的中心
复制代码
2.图像处理
图像处理主要的内容为摄像头的畸变矫正，以及颜色过滤。
  1. areas = []

  2.         ret, frame = cap.read()#视频信息处理

  3.         newcameramtx, roi=cv2.getOptimalNewCameraMatrix(left_camera_matrix,left_distortion,(frame_w,frame_h),1,(frame_w,frame_h))#<span style="color: rgb(77, 77, 77); font-family: -apple-system, "SF UI Text", Arial, "PingFang SC", "Hiragino Sans GB", "Microsoft YaHei", "WenQuanYi Micro Hei", sans-serif; font-size: 16px; font-variant-ligatures: no-common-ligatures; background-color: rgb(255, 255, 255);">去</span>除畸<span style="color: rgb(77, 77, 77); font-family: -apple-system, "SF UI Text", Arial, "PingFang SC", "Hiragino Sans GB", "Microsoft YaHei", "WenQuanYi Micro Hei", sans-serif; font-size: 16px; font-variant-ligatures: no-common-ligatures; background-color: rgb(255, 255, 255);">变矫正后图像四周黑色的区域</span>

  4.         frame = cv2.undistort(frame, left_camera_matrix, left_distortion, None, newcameramtx)#<span style="background-color: rgb(255, 255, 255); color: rgb(79, 79, 79); font-family: "PingFang SC", "Microsoft YaHei", SimHei, Arial, SimSun; font-size: 18px; font-variant-ligatures: no-common-ligatures;">像素坐标去畸变</span>

  5.         gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)#灰度处理

  6.         blur = cv2.GaussianBlur(gray, (3,3), 0)#高斯滤波，降噪

  7.         ret,imged = cv2.threshold(blur,HIGHTLIGHT_THR,255, cv2.THRESH_BINARY)#图像阈值处理

  8.         kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 10)) # (5,7)

  9.         highlight_img = cv2.dilate(imged, kernel)#图像膨胀
复制代码
  1. COLOR_TYPE=0

  2.         if COLOR_TYPE == 0: # 红方

  3.             sub_rg = cv2.subtract(r, g)

  4.             sub_rb = cv2.subtract(r, b)

  5.             ret,res_rg = cv2.threshold(sub_rg,COLOR_THR,255, cv2.THRESH_BINARY)

  6.             ret,res_rb = cv2.threshold(sub_rb,COLOR_THR,255, cv2.THRESH_BINARY)

  7.             final = cv2.bitwise_and(res_rg,res_rb)   

  8.         else: # 蓝方

  9.             sub_bg = cv2.subtract(b, g)

  10.             sub_br = cv2.subtract(b, r)

  11.             ret,res_bg = cv2.threshold(sub_bg,COLOR_THR,255, cv2.THRESH_BINARY)

  12.             ret,res_br = cv2.threshold(sub_br,COLOR_THR,255, cv2.THRESH_BINARY)

  13.             final = cv2.bitwise_and(res_bg,res_br)#主要完成红蓝颜色的过滤
复制代码
  1. kernelx = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 8))

  2.         color_img = cv2.dilate(final, kernelx)

  3. 

  4.         kernele = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))

  5.         preprocessed = cv2.bitwise_and(color_img,highlight_img)

  6.         preprocessed = cv2.dilate(preprocessed, kernele)

复制代码

3.轮廓检测
这个部分主要对过滤完的图片信息进行轮廓提取并进行大小区域判断，画出装甲板的框。
  1. contours, hierarchy = cv2.findContours(preprocessed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)#查找轮廓

  2.         cv2.drawContours(frame,contours,-1,(255,0,0),2)#<font color="#2a2b2e" face="PingFang SC, Segoe UI, Arial, Microsoft YaHei, 微软雅黑, 宋体, Malgun Gothic, sans-serif"><span style="font-size: 12px; background-color: rgb(255, 255, 255);">绘制轮廓</span></font>

  3.         

  4.         for contour in contours:#遍历检测出来的轮廓

  5.             areas.append(cv2.contourArea(contour))

  6.             try:

  7.                 max_id = areas.index(max(areas))

  8.             except:

  9.                 continue

  10.             x, y, w, h = cv2.boundingRect(contours[max_id])#轮廓面积判断，根据装甲板的大小

  11.             if ( w * h > 1000 ) :

  12.                 thetaX = degrees(atan2(x-left_camera_matrix[0][2],left_camera_matrix[0][0]))

  13.                 thetaY = degrees(atan2(y-left_camera_matrix[1][2],left_camera_matrix[1][1]))

  14.                 dist = distance_to_camera(w)

  15.                 yield(thetaX,thetaY,dist)

  16.                 # 绘制

  17.                 cv2.rectangle(frame, (x,y), (x+w,y+h), (0,255,255), 2)#绘制轮廓

  18. 

  19.         cv2.circle(frame,gun_point,5,(0,0,255),-1)

  20.         cv2.imshow("orgin_pic", frame) 
复制代码
4.数据下发
主要的下发的内容是检测出的装甲板的中心点和距离机器人视角的距离，以及距离装甲板的距离
  1.  for (yaw,pitch,dist) in aim():

  2.         yaw2=yaw

  3.         pitch=pitch-2

  4.         pitch2=pitch

  5.         if ( abs(yaw) < 1.5 ) :

  6.             yaw = 0

  7.         if ( abs(pitch) < 1.5 ):

  8.             pitch = 0 

  9.         if abs(yaw2) and abs(pitch2)<2.5:

  10.             fire = 1

  11.         else:

  12.             fire = 0

  13.         print("%d,%.2f,%.2f,%.2f,%d" % (COLOR_TYPE,yaw,pitch,dist,fire))
复制代码
5.实现效果

6。完整代码
  1. import cv2

  2. #import serial

  3. from math import *

  4. from user_config import *

  5. #import RPi.GPIO as GPIO

  6. 

  7. # 距离计算函数 

  8. def distance_to_camera(perWidth):  

  9.     return (ARMOUR_WIDTH * FOCAL_LENS) / perWidth      

  10.     

  11. def aim():

  12.     global DEBUG_MODE,COLOR_TYPE,HIGHTLIGHT_THR,COLOR_THR

  13.     cap = cv2.VideoCapture(VIDEO_INDEX) # 打开摄像头

  14.     frame_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) # 480

  15.     frame_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) # 640

  16.     gun_point = (int(frame_w/2),int(frame_h/2) + GUN_ERROR)

  17.     if DEBUG_MODE == True:

  18.         def get_pixel(event,x,y,flags,param):

  19.             if event == cv2.EVENT_LBUTTONDBLCLK:

  20.                 print(x,y)

  21.         cv2.setMouseCallback('orgin_pic',get_pixel)

  22.         def nothing():

  23.             pass

  24.         cv2.namedWindow("win_control")

  25.         cv2.createTrackbar('hl_thr','win_control',0,255,nothing)

  26.         cv2.createTrackbar('cl_thr','win_control',0,255,nothing)

  27.         cv2.setTrackbarPos('hl_thr','win_control',HIGHTLIGHT_THR)

  28.         cv2.setTrackbarPos('cl_thr','win_control',COLOR_THR)

  29.     while True:

  30.         areas = []

  31.         ret, frame = cap.read() 

  32.         newcameramtx, roi=cv2.getOptimalNewCameraMatrix(left_camera_matrix,left_distortion,(frame_w,frame_h),1,(frame_w,frame_h))

  33.        

  34. 

  35.         if DEBUG_MODE == True:

  36.             COLOR_THR = cv2.getTrackbarPos('cl_thr','win_control')

  37.             HIGHTLIGHT_THR = cv2.getTrackbarPos('hl_thr','win_control')

  38.         # matRotate = cv2.getRotationMatrix2D((frame_w*0.5, frame_h*0.5), 180, 1.0)

  39.         # frame = cv2.warpAffine(frame, matRotate, (frame_w,frame_h))

  40.         gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

  41.         blur = cv2.GaussianBlur(gray, (3,3), 0)

  42.         ret,imged = cv2.threshold(blur,HIGHTLIGHT_THR,255, cv2.THRESH_BINARY)

  43.         kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 10)) # (5,7)

  44.         highlight_img = cv2.dilate(imged, kernel)

  45. 

  46.         (b,g,r) = cv2.split(frame)

  47.         #COLOR_TYPE = GPIO.input(DETECT_GPIO_PIN)#设置引脚模式：

  48.         COLOR_TYPE=0

  49.         if COLOR_TYPE == 0: # 红方

  50.             sub_rg = cv2.subtract(r, g)

  51.             sub_rb = cv2.subtract(r, b)

  52.             ret,res_rg = cv2.threshold(sub_rg,COLOR_THR,255, cv2.THRESH_BINARY)

  53.             ret,res_rb = cv2.threshold(sub_rb,COLOR_THR,255, cv2.THRESH_BINARY)

  54.             final = cv2.bitwise_and(res_rg,res_rb)   

  55.         else: # 蓝方

  56.             sub_bg = cv2.subtract(b, g)

  57.             sub_br = cv2.subtract(b, r)

  58.             ret,res_bg = cv2.threshold(sub_bg,COLOR_THR,255, cv2.THRESH_BINARY)

  59.             ret,res_br = cv2.threshold(sub_br,COLOR_THR,255, cv2.THRESH_BINARY)

  60.             final = cv2.bitwise_and(res_bg,res_br)

  61. 

  62.         kernelx = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 8))

  63.         color_img = cv2.dilate(final, kernelx)

  64. 

  65.         kernele = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))

  66.         preprocessed = cv2.bitwise_and(color_img,highlight_img)

  67.         preprocessed = cv2.dilate(preprocessed, kernele)

  68. 

  69.         contours, hierarchy = cv2.findContours(preprocessed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

  70.         cv2.drawContours(frame,contours,-1,(255,0,0),2)

  71.         

  72.         for contour in contours:

  73.             areas.append(cv2.contourArea(contour))

  74.             try:

  75.                 max_id = areas.index(max(areas))

  76.             except:

  77.                 continue

  78.             x, y, w, h = cv2.boundingRect(contours[max_id])

  79.             if ( w * h > 1000 ) :

  80.                 thetaX = degrees(atan2(x-left_camera_matrix[0][2],left_camera_matrix[0][0]))

  81.                 thetaY = degrees(atan2(y-left_camera_matrix[1][2],left_camera_matrix[1][1]))

  82.                 dist = distance_to_camera(w)

  83.                 yield(thetaX,thetaY,dist)

  84.                 # 绘制

  85.                 cv2.rectangle(frame, (x,y), (x+w,y+h), (0,255,255), 2)

  86. 

  87.         cv2.circle(frame,gun_point,5,(0,0,255),-1)

  88.         cv2.imshow("orgin_pic", frame) 

  89.         if DEBUG_MODE == True:

  90.             cv2.imshow("hl_pic",highlight_img)

  91.             cv2.imshow("cl_pic",color_img)

  92.             cv2.imshow("final_pic", preprocessed) 

  93.             cv2.waitKey(1)

  94.         if cv2.waitKey(1) & 0xFF == ord('q'):

  95.             break

  96.         

  97. 

  98. if __name__ == "__main__":

  99.     

  100.     #ser = serial.Serial(PORT_NAME,BAUD_RATE,timeout=50)

  101.     fire = 0

  102.     #GPIO.setmode(GPIO.BOARD)#设置引脚模式：

  103.     #GPIO.setup(DETECT_GPIO_PIN, GPIO.IN)#设置引脚为输入：

  104.     for (yaw,pitch,dist) in aim():

  105.         yaw2=yaw

  106.         pitch=pitch-2

  107.         pitch2=pitch

  108.         if ( abs(yaw) < 1.5 ) :

  109.             yaw = 0

  110.         if ( abs(pitch) < 1.5 ):

  111.             pitch = 0 

  112.         if abs(yaw2) and abs(pitch2)<2.5:

  113.             fire = 1

  114.         else:

  115.             fire = 0

  116.         print("%d,%.2f,%.2f,%.2f,%d" % (COLOR_TYPE,yaw,pitch,dist,fire))import cv2

  117. #import serial

  118. from math import *

  119. from user_config import *

  120. #import RPi.GPIO as GPIO

  121. 

  122. # 距离计算函数 

  123. def distance_to_camera(perWidth):  

  124.     return (ARMOUR_WIDTH * FOCAL_LENS) / perWidth      

  125.     

  126. def aim():

  127.     global DEBUG_MODE,COLOR_TYPE,HIGHTLIGHT_THR,COLOR_THR

  128.     cap = cv2.VideoCapture(VIDEO_INDEX) # 打开摄像头

  129.     frame_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) # 480

  130.     frame_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) # 640

  131.     gun_point = (int(frame_w/2),int(frame_h/2) + GUN_ERROR)

  132.     if DEBUG_MODE == True:

  133.         def get_pixel(event,x,y,flags,param):

  134.             if event == cv2.EVENT_LBUTTONDBLCLK:

  135.                 print(x,y)

  136.         cv2.setMouseCallback('orgin_pic',get_pixel)

  137.         def nothing():

  138.             pass

  139.         cv2.namedWindow("win_control")

  140.         cv2.createTrackbar('hl_thr','win_control',0,255,nothing)

  141.         cv2.createTrackbar('cl_thr','win_control',0,255,nothing)

  142.         cv2.setTrackbarPos('hl_thr','win_control',HIGHTLIGHT_THR)

  143.         cv2.setTrackbarPos('cl_thr','win_control',COLOR_THR)

  144.     while True:

  145.         areas = []

  146.         ret, frame = cap.read() 

  147.         newcameramtx, roi=cv2.getOptimalNewCameraMatrix(left_camera_matrix,left_distortion,(frame_w,frame_h),1,(frame_w,frame_h))

  148.        

  149. 

  150.         if DEBUG_MODE == True:

  151.             COLOR_THR = cv2.getTrackbarPos('cl_thr','win_control')

  152.             HIGHTLIGHT_THR = cv2.getTrackbarPos('hl_thr','win_control')

  153.         # matRotate = cv2.getRotationMatrix2D((frame_w*0.5, frame_h*0.5), 180, 1.0)

  154.         # frame = cv2.warpAffine(frame, matRotate, (frame_w,frame_h))

  155.         gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

  156.         blur = cv2.GaussianBlur(gray, (3,3), 0)

  157.         ret,imged = cv2.threshold(blur,HIGHTLIGHT_THR,255, cv2.THRESH_BINARY)

  158.         kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 10)) # (5,7)

  159.         highlight_img = cv2.dilate(imged, kernel)

  160. 

  161.         (b,g,r) = cv2.split(frame)

  162.         #COLOR_TYPE = GPIO.input(DETECT_GPIO_PIN)#设置引脚模式：

  163.         COLOR_TYPE=0

  164.         if COLOR_TYPE == 0: # 红方

  165.             sub_rg = cv2.subtract(r, g)

  166.             sub_rb = cv2.subtract(r, b)

  167.             ret,res_rg = cv2.threshold(sub_rg,COLOR_THR,255, cv2.THRESH_BINARY)

  168.             ret,res_rb = cv2.threshold(sub_rb,COLOR_THR,255, cv2.THRESH_BINARY)

  169.             final = cv2.bitwise_and(res_rg,res_rb)   

  170.         else: # 蓝方

  171.             sub_bg = cv2.subtract(b, g)

  172.             sub_br = cv2.subtract(b, r)

  173.             ret,res_bg = cv2.threshold(sub_bg,COLOR_THR,255, cv2.THRESH_BINARY)

  174.             ret,res_br = cv2.threshold(sub_br,COLOR_THR,255, cv2.THRESH_BINARY)

  175.             final = cv2.bitwise_and(res_bg,res_br)

  176. 

  177.         kernelx = cv2.getStructuringElement(cv2.MORPH_RECT, (8, 8))

  178.         color_img = cv2.dilate(final, kernelx)

  179. 

  180.         kernele = cv2.getStructuringElement(cv2.MORPH_RECT, (7, 7))

  181.         preprocessed = cv2.bitwise_and(color_img,highlight_img)

  182.         preprocessed = cv2.dilate(preprocessed, kernele)

  183. 

  184.         contours, hierarchy = cv2.findContours(preprocessed, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

  185.         cv2.drawContours(frame,contours,-1,(255,0,0),2)

  186.         

  187.         for contour in contours:

  188.             areas.append(cv2.contourArea(contour))

  189.             try:

  190.                 max_id = areas.index(max(areas))

  191.             except:

  192.                 continue

  193.             x, y, w, h = cv2.boundingRect(contours[max_id])

  194.             if ( w * h > 1000 ) :

  195.                 thetaX = degrees(atan2(x-left_camera_matrix[0][2],left_camera_matrix[0][0]))

  196.                 thetaY = degrees(atan2(y-left_camera_matrix[1][2],left_camera_matrix[1][1]))

  197.                 dist = distance_to_camera(w)

  198.                 yield(thetaX,thetaY,dist)

  199.                 # 绘制

  200.                 cv2.rectangle(frame, (x,y), (x+w,y+h), (0,255,255), 2)

  201. 

  202.         cv2.circle(frame,gun_point,5,(0,0,255),-1)

  203.         cv2.imshow("orgin_pic", frame) 

  204.         if DEBUG_MODE == True:

  205.             cv2.imshow("hl_pic",highlight_img)

  206.             cv2.imshow("cl_pic",color_img)

  207.             cv2.imshow("final_pic", preprocessed) 

  208.             cv2.waitKey(1)

  209.         if cv2.waitKey(1) & 0xFF == ord('q'):

  210.             break

  211.         

  212. 

  213. if __name__ == "__main__":

  214.     

  215.     #ser = serial.Serial(PORT_NAME,BAUD_RATE,timeout=50)

  216.     fire = 0

  217.     #GPIO.setmode(GPIO.BOARD)#设置引脚模式：

  218.     #GPIO.setup(DETECT_GPIO_PIN, GPIO.IN)#设置引脚为输入：

  219.     for (yaw,pitch,dist) in aim():

  220.         yaw2=yaw

  221.         pitch=pitch-2

  222.         pitch2=pitch

  223.         if ( abs(yaw) < 1.5 ) :

  224.             yaw = 0

  225.         if ( abs(pitch) < 1.5 ):

  226.             pitch = 0 

  227.         if abs(yaw2) and abs(pitch2)<2.5:

  228.             fire = 1

  229.         else:

  230.             fire = 0

  231.         print("%d,%.2f,%.2f,%.2f,%d" % (COLOR_TYPE,yaw,pitch,dist,fire))
复制代码
  1. import numpy as np

  2. 

  3. PORT_NAME = "COM8" # 串口名称

  4. BAUD_RATE = 115200

  5. VIDEO_INDEX = 0 # 摄像头索引

  6. 

  7. DEBUG_MODE = False

  8. COLOR_TYPE = 0 # 灯条颜色参数，0为红，1为蓝

  9. HIGHTLIGHT_THR = 190 # 高光通道阈值

  10. COLOR_THR = 80 # 颜色通道饱和度阈值

  11. 

  12. ARMOUR_WIDTH =  55 # 装甲板宽度，mm

  13. ARMOUR_HEIGHT = 35 # 装甲板高度，mm

  14. RATE = ARMOUR_WIDTH / ARMOUR_HEIGHT # 长宽比

  15. ERROR = 0.1 # 识别到的长宽比误差

  16. RATE_RANGE = ( RATE - ERROR , RATE + ERROR ) # 误差范围

  17. FOCAL_LENS = 618.34872 # 焦距，mm

  18. 

  19. GUN_ERROR = -60 # 枪口pitch轴误差

  20. 

  21. DETECT_GPIO_PIN = 23 # 检测的GPIO引脚

  22. 

  23. #  内参矩阵

  24. left_camera_matrix = np.array([[618.34872, 0., 315.38874], 

  25.                                [0., 613.77151, 166.87870], 

  26.                                [0., 0., 1.]])

  27. # 畸变系数

  28. left_distortion = np.array([[-0.39063 , 0.18552 , 0.00857 , -0.00777, 0.00000]])

复制代码


WedSeptember-202209072548..png
WedSeptember-202209078790..png
WedSeptember-202209079085..png
回复

使用道具 举报

精彩评论(1)

帅猫  高级技师

发表于 2022-9-9 20:18:13

EP不需要使用额外的摄像头，通过wifi连接后可以通过TCP获取视频流数据。
还有EP的车轮螺丝是红色的，你这个不支持二次开发的S1不是EP，至于为什么带拓展基板我就不知道了，或许你还藏着另一台EP或者厂家给你发了黑色螺丝？
回复

使用道具 举报

高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
撰写新帖
关于楼主

楼主的其它帖子

background 创造力兑换 background 心愿单 background 创客导航
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail