From 9404d4be6d465082a41900b866dcb87b3c0a9e73 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 20 Sep 2023 02:44:26 +0300 Subject: [PATCH] Use image_geometry library in red circle following example --- clover/examples/red_circle.py | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/clover/examples/red_circle.py b/clover/examples/red_circle.py index 9fc726df..fec548e2 100644 --- a/clover/examples/red_circle.py +++ b/clover/examples/red_circle.py @@ -17,6 +17,7 @@ from cv_bridge import CvBridge from clover import long_callback, srv import tf2_ros import tf2_geometry_msgs +import image_geometry rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c @@ -32,21 +33,14 @@ mask_pub = rospy.Publisher('~mask', Image, queue_size=1) point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1) # read camera info -camera_info = rospy.wait_for_message('main_camera/camera_info', CameraInfo) -camera_matrix = np.float64(camera_info.K).reshape(3, 3) -distortion = np.float64(camera_info.D).flatten() +camera_model = image_geometry.PinholeCameraModel() +camera_model.fromCameraInfo(rospy.wait_for_message('main_camera/camera_info', CameraInfo)) def img_xy_to_point(xy, dist): - xy = cv2.undistortPoints(xy, camera_matrix, distortion, P=camera_matrix)[0][0] - - # Shift points to center - xy -= camera_info.width // 2, camera_info.height // 2 - - fx = camera_matrix[0, 0] - fy = camera_matrix[1, 1] - - return Point(x=xy[0] * dist / fx, y=xy[1] * dist / fy, z=dist) + xy_rect = camera_model.rectifyPoint(xy) + ray = camera_model.projectPixelTo3dRay(xy_rect) + return Point(x=ray[0] * dist, y=ray[1] * dist, z=dist) def get_center_of_mass(mask): M = cv2.moments(mask)