Re: Clarification on RGB and Depth image has input from Stereo camera

Posted by FeKl on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Clarification-on-RGB-and-Depth-image-has-input-from-Stereo-camera-tp2469p5050.html

Thanks for your answer. I tried to implement a python script in order to convert the disparity from sensor_msgs/Image to stereo/DisparityImage:

#!/usr/bin/env python

# This script converts an disparity image from "sensor_msgs/Image" to "stereo_msgs/DisparityImage"
import rospy
import message_filters
from nerian_stereo.msg import StereoCameraInfo
from sensor_msgs.msg import Image
from stereo_msgs.msg import DisparityImage
from cv_bridge import CvBridge
from sensor_msgs.msg import CameraInfo

disparity = 0
cam_info_left = 0

def callback(disparity_map, stereo_camera_info):
    
    global disparity;
    global cam_info_left;
    
    rospy.loginfo(rospy.get_caller_id() + ": Disparity image and camera info received")
    disparity_map = CvBridge().imgmsg_to_cv2(disparity_map, "32FC1")
    disparity_map = CvBridge().cv2_to_imgmsg(disparity_map,"passthrough")
    disparity.publish(header = disparity_map.header, image = disparity_map, f = stereo_camera_info.Q[11], T = stereo_camera_info.T_left_right[0], min_disparity = 0, max_disparity = 127, delta_d = 0.0625)
    cam_info_left.publish(stereo_camera_info.left_info)
    
def listener():

    global disparity;
    global cam_info_left;
    
    rospy.init_node('disp2dispImage')
    
    image_sub = message_filters.Subscriber('/nerian_stereo/disparity_map', Image)
    info_sub = message_filters.Subscriber('/nerian_stereo/stereo_camera_info', StereoCameraInfo)

    ts = message_filters.TimeSynchronizer([image_sub, info_sub], 10)
    ts.registerCallback(callback)
    
    disparity = rospy.Publisher('/disparity', DisparityImage, queue_size=10)
    cam_info_left = rospy.Publisher('/stereo_camera_info', CameraInfo, queue_size=10)
 
    rospy.spin()

if __name__ == '__main__':
    listener() 

Then I tried to use the nodelet rtabmap_ros/disparity_to_depth. But I didn't manage it to work. Can you give me a hint in order to find my mistake? Maybe there is something wrong with the conversion "32FC1"...?

Kind regards

Felix