diff --git a/Vision/Mechanical_arm_error_test.py b/Vision/Mechanical_arm_error_test.py new file mode 100644 index 0000000..4be2881 --- /dev/null +++ b/Vision/Mechanical_arm_error_test.py @@ -0,0 +1,54 @@ +#!/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) \ No newline at end of file