Closed ranjitkathiriya closed 3 years ago
MartyG-RealSense
commented
3 years ago Hi @ranjitkathiriya On a case about converting 2D coordinates to 3D in ROS, a script in Python is provided (see the link below). Would this meet your needs please?
https://github.com/IntelRealSense/realsense-ros/issues/1342#issuecomment-681172015
ranjitkathiriya
commented
3 years ago Hey,
Thanks for this link and I have tried the code from that link and I am getting this error.
---- 1. The code below is the ros version of this code right : https://github.com/IntelRealSense/librealsense/blob/jupyter/notebooks/distance_to_object.ipynb
[ERROR] [1608207612.676933]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7f6656897e10>> Traceback (most recent call last): File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback cb(msg) File "/opt/ros/noetic/lib/python3/dist-packages/message_filters/init.py", line 76, in callback self.signalMessage(msg) File "/opt/ros/noetic/lib/python3/dist-packages/message_filters/init.py", line 58, in signalMessage cb((msg + args)) File "/opt/ros/noetic/lib/python3/dist-packages/message_filters/init.py", line 330, in add self.signalMessage(msgs) File "/opt/ros/noetic/lib/python3/dist-packages/message_filters/init.py", line 58, in signalMessage cb(*(msg + args)) File "/home/kuka-ai/catkin_ws/src/ai_milking_ros/scripts/new_method.py", line 116, in callback self.function_box(img, im0s, self.half, self.model, self.view_img, self.names,cv_image_depth,camerainfo) File "/home/kuka-ai/catkin_ws/src/ai_milking_ros/scripts/new_method.py", line 163, in function_box results = self.convert_depth_to_phys_coord_using_realsense(int((xyxy[0]+ xyxy[2])/2),int((xyxy[1]+ xyxy[3])/2), depth, camerainfo) File "/home/kuka-ai/catkin_ws/src/ai_milking_ros/scripts/new_method.py", line 203, in convert_depth_to_phys_coord_using_realsense result = pyrealsense2.rs2_deproject_pixel_to_point(_intrinsics, [x, y], depth) TypeError: rs2_deproject_pixel_to_point(): incompatible function arguments. The following argument types are supported:
Invoked with: [ 640x480 p[316.961 246.175] f[611.322 611.844] Brown Conrady [0 0 0 0 0] ], [630, 404], array([[[255, 150, 0], [255, 155, 0], [255, 155, 0],
and my code is :
results = self.convert_depth_to_phys_coord_using_realsense(int((xyxy[0]+ xyxy[2])/2),int((xyxy[1]+ xyxy[3])/2), depth, camerainfo)
def convert_depth_to_phys_coord_using_realsense(self,x, y, depth, cameraInfo):
_intrinsics = pyrealsense2.intrinsics()
_intrinsics.width = cameraInfo.width
_intrinsics.height = cameraInfo.height
_intrinsics.ppx = cameraInfo.K[2]
_intrinsics.ppy = cameraInfo.K[5]
_intrinsics.fx = cameraInfo.K[0]
_intrinsics.fy = cameraInfo.K[4]
#_intrinsics.model = cameraInfo.distortion_model
# _intrinsics.model = pyrealsense2.distortion.none
if cameraInfo.distortion_model == 'plumb_bob':
_intrinsics.model = pyrealsense2.distortion.brown_conrady
elif cameraInfo.distortion_model == 'equidistant':
_intrinsics.model = pyrealsense2.distortion.kannala_brandt4
_intrinsics.coeffs = [i for i in cameraInfo.D]
result = pyrealsense2.rs2_deproject_pixel_to_point(_intrinsics, [x, y], depth)
#result[0]: right, result[1]: down, result[2]: forward
return result[2], -result[0], -result[1]I don't have any insight about the errors in the above script unfortunately. I will refer it to the RealSense ROS wrapper developer @doronhi to seek his advice. Thanks for your patience in the meantime.
ranjitkathiriya
commented
3 years ago I am right? because I am having only X,Y coordinate and I need Z as well.
MartyG-RealSense
commented
3 years ago The script posted by @doronhi in https://github.com/IntelRealSense/realsense-ros/issues/1342#issuecomment-681172015 is a modification of a script called show_center_depth.py, whilst the Jupyter script is distance_to_object.ipynb.
ranjitkathiriya
commented
3 years ago Sorry! My question is by giving X and Y positions we can get the Z (Distance) in mm?
Am I correct or not?
Thanks,
MartyG-RealSense
commented
3 years ago It is certainly possible to convert 2D XY coordinates to 3D XYZ, though I am more familiar with doing so in librealsense than with ROS. I wonder if it is going to be easier to just generate a pointcloud in ROS though and get Z from the pointcloud. Here is an example:
https://answers.ros.org/question/310996/how-to-get-xyz-and-rgb-of-each-pixel-from-a-sensormsg-image/
ranjitkathiriya
commented
3 years ago Actually, I have tried with point cloud but I think I am not able to get the accurate distance.
#!/usr/bin/env python
import rospy
import struct
import argparse
import numpy as np
import matplotlib.pyplot as plt
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from ai_milking_ros.msg import BoundingBox, BoundingBoxes
from geometry_msgs.msg import Vector3
bbox = BoundingBox()
pre_bbox= BoundingBox()
pc = PointCloud2()
center = Vector3()
point = Vector3()
points = []
depth = 0.0
def bbox_callback(msg):
num_box = len(msg.bounding_boxes)
if num_box>0:
b = msg.bounding_boxes[0]
bbox.xmin = b.xmin
bbox.xmax = b.xmax
bbox.ymin = b.ymin
bbox.ymax = b.ymax
def point_callback(msg):
global points
global bbox
global pre_bbox
pc.header = msg.header
pc.height = msg.height
pc.width = msg.width
pc.fields = msg.fields # channels and their layout
pc.is_bigendian = msg.is_bigendian
pc.point_step = msg.point_step
pc.row_step = msg.row_step
pc.data = msg.data # Actual point data, size is (row_step*height)
resolution = (pc.height, pc.width)
if bbox.xmin==pre_bbox.xmin and \
bbox.xmin==pre_bbox.xmin and \
bbox.xmin==pre_bbox.xmin and \
bbox.xmin==pre_bbox.xmin:
pass
else:
points = [ ]
for p in pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z")):
points.append(p[2])
if len(points) == pc.width*pc.height:
z_points = np.array(points, dtype=np.float32)
z = z_points.reshape(resolution)
if not (bbox.xmin==0 and bbox.xmax==0):
print('Box: {}, {}'.format(bbox.xmin, bbox.xmax))
z_box = z[bbox.xmin:bbox.xmax, bbox.ymin:bbox.ymax]
z_value = z_box[~np.isnan(z_box)]
distance = min(z_value)
print('Distance: {}'.format(distance))
pre_bbox.xmin = bbox.xmin
pre_bbox.xmax = bbox.xmax
pre_bbox.ymin = bbox.ymin
pre_bbox.ymax = bbox.ymax
def main(args):
rospy.init_node('ObjectDepth', anonymous=True)
point_sub = rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback)
bbox_sub = rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
freq = 10
rate = rospy.Rate(freq)
while not rospy.is_shutdown():
pass
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Output Depth of an Target Object')
args = parser.parse_args()
main(args)I continued researching possible solutions to getting Z. Using a point cloud remained the most suitable option in my opinion though. Like in this case:
MartyG-RealSense
commented
3 years ago Hi @ranjitkathiriya Do you have an update that you can provide about this case please? Thanks!
ranjitkathiriya
commented
3 years ago Thanks for help
ravijo
commented
3 years ago Although this issue is old, however, I am posting my comment here. Just in case, it could be helpful for someone. Please read below:
Given a pixel location (x, y) and its corresponding depth (z), the 3D coordinate in camera space, can be calculated by using camera intrinsic parameters. Luckily, these parameters are provided by the IntelRealSense camera and hence we can directly plug these values to get the 3D point.
RodrigoFBernardo
commented
2 years ago Actually, I have tried with point cloud but I think I am not able to get the accurate distance.
#!/usr/bin/env python import rospy import struct import argparse import numpy as np import matplotlib.pyplot as plt import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import PointCloud2 from ai_milking_ros.msg import BoundingBox, BoundingBoxes from geometry_msgs.msg import Vector3 bbox = BoundingBox() pre_bbox= BoundingBox() pc = PointCloud2() center = Vector3() point = Vector3() points = [] depth = 0.0 def bbox_callback(msg): num_box = len(msg.bounding_boxes) if num_box>0: b = msg.bounding_boxes[0] bbox.xmin = b.xmin bbox.xmax = b.xmax bbox.ymin = b.ymin bbox.ymax = b.ymax def point_callback(msg): global points global bbox global pre_bbox pc.header = msg.header pc.height = msg.height pc.width = msg.width pc.fields = msg.fields # channels and their layout pc.is_bigendian = msg.is_bigendian pc.point_step = msg.point_step pc.row_step = msg.row_step pc.data = msg.data # Actual point data, size is (row_step*height) resolution = (pc.height, pc.width) if bbox.xmin==pre_bbox.xmin and \ bbox.xmin==pre_bbox.xmin and \ bbox.xmin==pre_bbox.xmin and \ bbox.xmin==pre_bbox.xmin: pass else: points = [ ] for p in pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z")): points.append(p[2]) if len(points) == pc.width*pc.height: z_points = np.array(points, dtype=np.float32) z = z_points.reshape(resolution) if not (bbox.xmin==0 and bbox.xmax==0): print('Box: {}, {}'.format(bbox.xmin, bbox.xmax)) z_box = z[bbox.xmin:bbox.xmax, bbox.ymin:bbox.ymax] z_value = z_box[~np.isnan(z_box)] distance = min(z_value) print('Distance: {}'.format(distance)) pre_bbox.xmin = bbox.xmin pre_bbox.xmax = bbox.xmax pre_bbox.ymin = bbox.ymin pre_bbox.ymax = bbox.ymax def main(args): rospy.init_node('ObjectDepth', anonymous=True) point_sub = rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) bbox_sub = rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback) freq = 10 rate = rospy.Rate(freq) while not rospy.is_shutdown(): pass if __name__ == '__main__': parser = argparse.ArgumentParser(description='Output Depth of an Target Object') args = parser.parse_args() main(args)
I am trying to adapt this example, but I want to calculate the z value based on the image taken by the /camera/color/image/raw topic.
What I am doing is: I calculate the centroid of my point of interest and convert it to a 3D point but the z value is not correct. Do I need to calculate the centroid from the image coming from the topic '/camera/depth/image_rect_raw' ?
MartyG-RealSense
commented
2 years ago Hi @RodrigoCodeBernardo As you are using darknet_ros, it may be worth investigating whether creating a 3D bounding box with darknet_ros_3d can meet your needs if you have not looked at it already. There are versions for ROS1 and ROS2.
ROS1 Melodic https://github.com/IntelligentRoboticsLabs/gb_visual_detection_3d/tree/melodic
ROS1 Noetic https://github.com/IntelligentRoboticsLabs/gb_visual_detection_3d/tree/noetic
ROS2 Eloquent https://github.com/IntelligentRoboticsLabs/gb_visual_detection_3d/tree/master
RodrigoFBernardo
commented
2 years ago Hi @MartyG-RealSense.
It might be a good solution. I'll explore.
Thanks for your help
ranjitkathiriya
commented
2 years ago
Hi there,
I am using intel realsense d435i, and I have x,y,x+w, and y+h positions and I want to calculate distance based on this using python. Can you suggest the best approach for calculating this? I am using Ros-noetic and if possible can you provide sample links of code that can be helpful for the integration.
thanks and regards, Ranjit