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

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_crop
        result[:] = img
        for 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.1
            result_y = (i[1] + y1)/resize_scale/0.1
            result_r = i[2]/resize_scale/0.1
            points_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()