0
$\begingroup$

that is my code

#!/usr/bin/env python
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import PointStamped, Point
from std_msgs.msg import Header

class CircleDetection:
    def __init__(self):
        rospy.init_node('circle_detection_node', anonymous=True)
        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_callback)
        self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.depth_callback)
        self.camera_info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.camera_info_callback)
        self.center_pub = rospy.Publisher("/circle_center_3d", PointStamped, queue_size=1)
        self.bridge = CvBridge()
        self.K = None  # 内参矩阵
        self.depth_image = None

    def camera_info_callback(self, data):
        # 保存内参矩阵
        self.K = np.array(data.K).reshape((3, 3))

    def depth_callback(self, data):
        try:
            # 将深度图像转换为OpenCV格式
            self.depth_image = self.bridge.imgmsg_to_cv2(data, desired_encoding='passthrough')
        except CvBridgeError as e:
            print(e)

    def image_callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)
            return

        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        gray = cv2.GaussianBlur(gray, (15, 15), 0)

        circles = cv2.HoughCircles(
            gray, cv2.HOUGH_GRADIENT, dp=1, minDist=20, param1=50, param2=30, minRadius=0, maxRadius=50
        )

        if circles is not None and self.K is not None and self.depth_image is not None:
            circles = np.uint16(np.around(circles))
            for i in circles[0, :]:
                center = (i[0], i[1])
                cv2.circle(cv_image, center, 1, (0, 100, 100), 3)
                cv2.circle(cv_image, center, i[2], (255, 0, 0), 2)

                # 获取深度信息
                depth_value = self.depth_image[i[1], i[0]]

                # 使用内参矩阵进行坐标转换
                pixel_coordinates = np.array([[i[0], i[1]]], dtype='float32')
                undistorted_pixel = cv2.undistortPoints(pixel_coordinates, self.K, None)
                u, v = undistorted_pixel[0, 0]

                # 计算三维坐标
                x = (u - self.K[0, 2]) * depth_value / self.K[0, 0]
                y = (v - self.K[1, 2]) * depth_value / self.K[1, 1]
                z = depth_value

                # 发布三维坐标
                header = Header()
                header.stamp = rospy.Time.now()
                point_stamped = PointStamped(header=header, point=Point(x, y, z))
                self.center_pub.publish(point_stamped)

        cv2.imshow("Circle Detection", cv_image)
        cv2.waitKey(3)

if __name__ == '__main__':
    try:
        circle_detection = CircleDetection()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

During runtime, the target can be detected, but when printed to the terminal, it cannot be converted.

header: 
  seq: 6
  stamp: 
    secs: 292
    nsecs: 277000000
  frame_id: ''
point: 
  x: nan
  y: nan
  z: nan

I am a beginner, and I would be extremely grateful if you could provide assistance.

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.