54 lines
2.3 KiB
Python
54 lines
2.3 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2024/12/26 16:24
|
|||
|
|
# @Author : hjw
|
|||
|
|
# @File : Mechanical_arm_error_test.py.py
|
|||
|
|
'''
|
|||
|
|
from camera_coordinate_dete import Detection
|
|||
|
|
from Trace.handeye_calibration import *
|
|||
|
|
from Vision.tool.CameraPe_depth2color import camera_pe as camera_pe_depth2color
|
|||
|
|
import cv2
|
|||
|
|
|
|||
|
|
def dete_center_point_circle(img):
|
|||
|
|
gay_img = cv2.cvtColor(img, cv2.COLOR_BGRA2GRAY)
|
|||
|
|
img = cv2.medianBlur(gay_img, 7) # 进行中值模糊，去噪点
|
|||
|
|
# 要对图像进行滤波去除噪点并转为灰度图像，滤波方法视图像而定
|
|||
|
|
circles = cv2.HoughCircles(img, cv2.HOUGH_GRADIENT, 1, 60, param1=100, param2=30, minRadius=0, maxRadius=0)
|
|||
|
|
# dp: 图像解析的反向比例。1为原始大小，2为原始大小的一半
|
|||
|
|
# minDist: 圆心之间的最小距离。过小会增加圆的误判，过大会丢失存在的圆
|
|||
|
|
# param1: Canny检测器的高阈值
|
|||
|
|
# param2: 检测阶段圆心的累加器阈值。越小的话，会增加不存在的圆；越大的话，则检测到的圆就更加接近完美的圆形
|
|||
|
|
# minRadius: 检测的最小圆的半径
|
|||
|
|
# maxRadius: 检测的最大圆的半径
|
|||
|
|
|
|||
|
|
print(np.shape(circles))
|
|||
|
|
circles = np.uint16(np.around(circles))
|
|||
|
|
print(circles)
|
|||
|
|
|
|||
|
|
for i in circles[0, :]: # 遍历矩阵每一行的数据
|
|||
|
|
cv2.circle(img, (i[0], i[1]), i[2], (0, 255, 0), 2)
|
|||
|
|
cv2.circle(img, (i[0], i[1]), 2, (0, 0, 255), 3)
|
|||
|
|
print('圆心：', (i[0], i[1]))
|
|||
|
|
cv2.imshow('circle', img)
|
|||
|
|
return i[0], i[1]
|
|||
|
|
|
|||
|
|
detection = Detection(cameraType = 'Pe')
|
|||
|
|
camera = camera_pe_depth2color()
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
ret, img, pm, _depth_align = camera.get_img_and_point_map()
|
|||
|
|
if ret == 1:
|
|||
|
|
center_x, center_y = dete_center_point_circle(img)
|
|||
|
|
point_x, point_y, point_z = pm[center_y, center_x]
|
|||
|
|
|
|||
|
|
# print('xyz点云坐标：', xyz)
|
|||
|
|
# print('nx_ny_nz法向量：', nx_ny_nz)
|
|||
|
|
# print('矩形框四顶点：', box)
|
|||
|
|
# img = cv2.resize(img,(720, 540))
|
|||
|
|
transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0) # (x, y, z, u, v, w)
|
|||
|
|
target_position, noraml_base = getPosition(point_x, point_y, point_z, 1, 1, 1, transformation_matrix, None)
|
|||
|
|
print("target_position:", target_position)
|
|||
|
|
print("noraml_base", noraml_base)
|
|||
|
|
cv2.imshow('img', img)
|
|||
|
|
cv2.waitKey(0)
|