first commit

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)