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-tp2469p5076.html

Thanks for your answer.

Maybe it's the best if I give you again a brief overview about our setup:

- we have a greyscale stereo camera which also can publish the disparity image via type sensor_msgs/Image
- we convert this disparity image into the type stereo_msgs/DisparityImage (with the now working python script, see appendix)
- than we use the rtabmap_ros/disparity_to_depth nodelet in order to use the RGBD mapping mode in RTAB-Map

Here are some outputs of the above steps:
1) The disparity map as type sensor_msgs/Image:

2) The depth map after using rtabmap_ros/disparity_to_depth:

3) Screenshot of RTAB-Map during the run (the 3D point cloud is wrong):


I think there's something wrong with the depth map.

We get the following warning during the run:

[ WARN] (2018-10-05 12:28:10.157) OdometryF2M.cpp:472::computeTransform() Registration failed: "Not enough inliers 7/20 (matches=46) between -1 and 3"
[ WARN] (2018-10-05 12:28:10.158) OdometryF2M.cpp:261::computeTransform() Failed to find a transformation with the provided guess (xyz=-0.035462,0.369827,-1.531558 rpy=-0.010178,-0.183696,0.056159), trying again without a guess.
[ WARN] (2018-10-05 12:28:10.251) OdometryF2M.cpp:481::computeTransform() Trial with no guess succeeded!

By the way, the ROS node of our camera says the following: "The stereo disparity map that is computed by SceneScan. The disparity map is transmitted as an image message with mono16 encoding. Please note that the disparity map has a 4-bit subpixel resolution. This means that its values have to be divided by 16 in order to receive the true pixel disparity"

Thats why we devide the disparity map by 16 in order to convert it into stereo_msgs/DisparityImage.

Can you again give us some hints in order to find our mistake?

Kind regards,

Felix

PS: Here's the obove mentioned python script:

#!/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

left_img = 0
disparity = 0
cam_info_left = 0
Q11 = 407.122375488
Tlr0 = -0.249977806853

def callback(left_image, disparity_map):
    
    global left_img;    
    global disparity;
    
    global Q11;
    global Tlr0;
    
    rospy.loginfo(rospy.get_caller_id() + ": Left image and disparity image received")
    left_img.publish(left_image)
    disparity_map = CvBridge().imgmsg_to_cv2(disparity_map, "32FC1")/16
    disparity_map = CvBridge().cv2_to_imgmsg(disparity_map,"passthrough")
    disparity.publish(header = left_image.header,image = disparity_map, f = Q11, T = Tlr0, min_disparity = 0, max_disparity = 127, delta_d = 0.0625)

def callback_cam_info(data):

    global cam_info_left;    
    global Q11;
    global Tlr0;
    
    rospy.loginfo(rospy.get_caller_id() + ": Camera info received")
    Q11 = data.Q[11]
    Tlr0 = data.T_left_right[0]
    cam_info_left.publish(data.left_info)
    
def listener():
    
    global left_img;
    global disparity;
    global cam_info_left; 
    
    rospy.init_node('disp2dispImage')  
    
    left_sub = message_filters.Subscriber('/nerian_stereo/left_image', Image)
    disp_sub = message_filters.Subscriber('/nerian_stereo/disparity_map', Image)
    rospy.Subscriber("/nerian_stereo/stereo_camera_info", StereoCameraInfo, callback_cam_info)

    ts = message_filters.TimeSynchronizer([left_sub, disp_sub], 10)
    ts.registerCallback(callback)
    
    left_img = rospy.Publisher('/left_image', Image, queue_size=10)
    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() 

Here we use two callback functions because our camera doesn't publish the camera info fast enough.