二维和三维联合进行圆孔空间定位-编程知识

二维和三维联合进行圆孔空间定位

0.任务描述

对空间圆孔进行三维空间的定位，方便后续的抓取或装配
流程：使用二维图与opencv霍夫圆检测进行二维上的定位，再从深度图上查询深度信息，结合相机内参计算出相机坐标系下圆孔的三维坐标信息，并在点云上进行标注

1.二维定位与圆周点获取

使用opencv霍夫圆检测，下段程序可以鼠标拖动来调整参数，检测到的圆心坐标和半径以x,y,r的顺序打印出来
在图片相同路径下生成同名的txt文档，一共四行，分别是圆心x坐标，圆心y坐标，半径，圆周上若干点的坐标（格式x,y;x,y;）

import cv2
import numpy as np
from math import sin,cos# 更新阈值
def updateThreshold(x):# 缩放参数resize_scale = cv2.getTrackbarPos('resize_scale', 'param_select')# 裁减的坐标参数x1 = cv2.getTrackbarPos('x1', 'param_select')x2 = cv2.getTrackbarPos('x2', 'param_select')y1 = cv2.getTrackbarPos('y1', 'param_select')y2 = cv2.getTrackbarPos('y2', 'param_select')# 滤波参数blur_type = cv2.getTrackbarPos('blur_type', 'param_select')# 霍夫圆检测参数dp = cv2.getTrackbarPos('dp', 'param_select')minDist = cv2.getTrackbarPos('minDist', 'param_select')param1 = cv2.getTrackbarPos('param1', 'param_select')param2 = cv2.getTrackbarPos('param2', 'param_select')minRadius = cv2.getTrackbarPos('minRadius', 'param_select')maxRadius = cv2.getTrackbarPos('maxRadius', 'param_select')#在圆周上画出点num = cv2.getTrackbarPos('num', 'param_select')# 缩放操作img_resize = cv2.resize(img,None,fx=resize_scale*0.1,fy=resize_scale*0.1) cv2.imshow('resize', img_resize)# 裁减操作img_crop = img_resize[y1:y2, x1:x2]cv2.imshow('crop', img_crop)# 灰度化和滤波img_gray = cv2.cvtColor(img_crop, cv2.COLOR_BGR2GRAY)  img_blur = cv2.blur(img_gray, (2 * blur_type + 1, 2 * blur_type + 1))  cv2.imshow('img_blur', img_blur)# 检测主函数circles = cv2.HoughCircles(img_blur, cv2.HOUGH_GRADIENT, dp=dp, minDist=minDist, param1=param1, param2=param2, minRadius=minRadius, maxRadius=maxRadius)# 用于循环显示更新pro_result = np.zeros_like(img_crop)result = np.zeros_like(img)points_datas = datas+'.txt'if circles is not None:points_datas = open(points_datas,'w')circles = np.uint16(np.around(circles))print('circles numbers:', circles.shape[1])print('draw on image!') # 用于更新pro_result[:] = img_cropresult[:] = imgfor i in circles[0, :]:# 在预处理的图片上标记cv2.circle(pro_result, (i[0], i[1]), i[2], (0, 255, 0), 2)cv2.circle(pro_result, (i[0], i[1]), 2, (0, 0, 255), 3)# 计算坐标和半径result_x = (i[0] + x1)/resize_scale/0.1result_y = (i[1] + y1)/resize_scale/0.1result_r = i[2]/resize_scale/0.1points_datas.write(str(int(result_x))+'\n')points_datas.write(str(int(result_y))+'\n')points_datas.write(str(int(result_r))+'\n')#计算圆周上的点for i in range(num):points_x,points_y = int(result_r * cos(360/num*i) + result_x), int(result_r * sin(360/num*i) + result_y)cv2.circle(result, (int(points_x), int(points_y)), 2, (0, 0, 255), 10)# print(i,points_x,points_y)points_datas.write(str(int(points_x))+',' + str(int(points_y)) + ';')points_datas.write('\n')# 在原图片上标记cv2.circle(result, (int(result_x), int(result_y)), int(result_r), (0, 255, 8), 1)cv2.circle(result, (int(result_x), int(result_y)), 2, (0, 0, 255), 10)# 输出坐标和半径print(int(result_x), int(result_y), result_r)points_datas.close()else:print("No circles detected.")cv2.imshow('pro_result', pro_result)cv2.imshow('result', result)if __name__ == "__main__":# 载入图片datas = 'detectimages/example/depth_image_00000'img = cv2.imread(datas + '.png')# 创建显示框cv2.namedWindow('param_select', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)cv2.namedWindow('resize', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)cv2.namedWindow('crop', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)cv2.namedWindow('img_blur', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)cv2.namedWindow('pro_result', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)cv2.namedWindow('result', flags= cv2.WINDOW_NORMAL | cv2.WINDOW_FREERATIO)# 创建滑动条 当滑动条有变化时调用updateThreshold函数cv2.createTrackbar('resize_scale','param_select',1,10,updateThreshold)cv2.createTrackbar('x1','param_select',0,3072,updateThreshold)cv2.createTrackbar('x2','param_select',3072,3072,updateThreshold)cv2.createTrackbar('y1','param_select',0,2048,updateThreshold)cv2.createTrackbar('y2','param_select',2048,2048,updateThreshold)cv2.createTrackbar('blur_type','param_select',3,5,updateThreshold)cv2.createTrackbar('dp','param_select',1,15,updateThreshold)cv2.createTrackbar('minDist','param_select',40,255,updateThreshold)cv2.createTrackbar('param1','param_select',70,255,updateThreshold)cv2.createTrackbar('param2','param_select',23,255,updateThreshold)cv2.createTrackbar('minRadius','param_select',10,255,updateThreshold)cv2.createTrackbar('maxRadius','param_select',30,255,updateThreshold)cv2.createTrackbar('num','param_select',1,360,updateThreshold)# 循环更新显示 空格退出while cv2.waitKey(0) != ord(' '):continue# 关闭所有窗口cv2.destroyAllWindows()