[!WARNING]
Hey there, I'm facing issue while converting 2d image pixel pose to 3d point pose, using rgb and depth data.
Can you tell me how to do it, or if am I mistaking something? Also, I can't find any parameter related to conversion of depth value to meters.
Here's my code that I use to convert the point from image space to 3d space:
import rospy
import numpy as np
import cv2
from sensor_msgs.msg import CompressedImage, CameraInfo
from vision_msgs.msg import Detection2DArray
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker, MarkerArray
class BoundingBox3DPose:
def __init__(self):
rospy.init_node('bounding_box_3d_pose', anonymous=True)
self.bridge = CvBridge()
self.camera_info = None
self.camera_matrix = None
self.depth_image = None
self.detection_sub = rospy.Subscriber("/yolov7/detect", Detection2DArray, self.detection_callback)
self.depth_image_sub = rospy.Subscriber("/camera/depth/image/compressed", CompressedImage, self.depth_image_callback)
self.camera_info_sub = rospy.Subscriber("/camera/depth/camera_info", CameraInfo, self.camera_info_callback)
self.marker_pub = rospy.Publisher("/object_markers", MarkerArray, queue_size=10)
rospy.spin()
def camera_info_callback(self, msg):
self.camera_info = msg
self.camera_matrix = np.array(self.camera_info.K).reshape(3, 3)
def depth_image_callback(self, msg):
try:
self.depth_image = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="passthrough")[:,:,0]
except CvBridgeError as e:
rospy.logerr(e)
def detection_callback(self, msg):
if self.depth_image is None or self.camera_matrix is None:
return
markers = MarkerArray()
for i, detection in enumerate(msg.detections):
bbox = detection.bbox/
xmin = int(bbox.center.x - bbox.size_x / 4)
xmax = int(bbox.center.x + bbox.size_x / 4)
ymin = int(bbox.center.y - bbox.size_y / 4)
ymax = int(bbox.center.y + bbox.size_y / 4)
# Ensure bounding box is within image bounds
xmin = max(0, xmin)
xmax = min(self.depth_image.shape[1], xmax)
ymin = max(0, ymin)
ymax = min(self.depth_image.shape[0], ymax)
# Extract the region of interest
depth_roi = self.depth_image[ymin:ymax, xmin:xmax]
non_zero_depths = depth_roi[depth_roi > 0]
if len(non_zero_depths) == 0: # Handle cases where no valid depth data is available
continue
# Compute mean depth
mean_depth = np.mean(non_zero_depths)
center_x = int(bbox.center.x)
center_y = int(bbox.center.y)
# Convert 2D center coordinates to 3D world coordinates using mean depth
center_x3d = 0.01*(center_x - self.camera_matrix[0, 2]) * mean_depth / self.camera_matrix[0, 0]
center_y3d = 0.01*(center_y - self.camera_matrix[1, 2]) * mean_depth / self.camera_matrix[1, 1]
center_z3d = 0.01*mean_depth
# Create Marker message
marker = Marker()
marker.header = detection.source_img.header
marker.ns = "object_markers"
marker.id = i
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = center_x3d
marker.pose.position.y = center_y3d
marker.pose.position.z = center_z3d
marker.pose.orientation.w = 1.0 # Identity quaternion for no rotation
marker.scale.x = 0.1 # Scale the sphere to an appropriate size
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0 # Alpha channel for visibility
marker.color.r = 1.0 # Red color for the markers
marker.color.g = 0.0
marker.color.b = 0.0
markers.markers.append(marker)
self.marker_pub.publish(markers)
if __name__ == '__main__':
try:
BoundingBox3DPose()
except rospy.ROSInterruptException:
pass
here's an rviz's screenshot that shows the bad alignment between depth point cloud and markers:
Thanks a lot for your attention, a help on this issue would mean a lot to me
Can you tell me how to do it, or if am I mistaking something? Also, I can't find any parameter related to conversion of depth value to meters. Here's my code that I use to convert the point from image space to 3d space:
here's an rviz's screenshot that shows the bad alignment between depth point cloud and markers:
Thanks a lot for your attention, a help on this issue would mean a lot to me