Disparity topic is not available in the ROS topic list

[cpsarras]

Hello Mathieu,

how can i subscribe to the disparity and depth topics in RTAB-Map ROS? I can see that the topics are published in the code but RVIZ does not show them.

Thank you in advance,
Chris

[matlabbe]

Hi, which part of the code you are referring? Normally, rtabmap doesn't republish rgb/depth/disparity images, just MapData topic that could contain these stuff for the added node in the map. MapData can be visualized in RVIZ with rtabmap_ros/MapCloud rviz plugin.

To see in RVIZ the rgb/depth/disparity images of MapData like in rtabmapviz, it is not possible. Note that you can subscribe in RVIZ to topics coming from the camera directly.

cheers,
Mathieu

[cpsarras]

I am sorry for the vagueness.

I was using RTAB-Map ROS with a handheld ZED stereo camera outside, with a 1080p resolution. I noticed a pattern, when I was under direct sunlight, enough inliers were detected and the odometry was very precise. Although, when there were clouds and sunlight was sparse, the odometry had a relatively large error (1m+) and not enough inliers were found, so odometry was lost pretty soon after recording started.
I wanted to start investigating this by examining the disparity calculated by RTAB-Map. I went to coreWrapper.cpp and added an image publisher in line 1030 (after the if(stereoToDepth_) statement, where disparity was computed). The publisher was initialized in the onInit function. After compilation, the publisher is visible in RVIZ, but nothing is published in the topic, because the program does not get into the if-statement. So the variable stereoToDepth_ is false, but i am using the stereo_mapping.launch. Shouldn't RTAB-Map be calculating the disparity from the stereo image pair? Isn't that done in coreWrapper.cpp?

Thank you very much,
Chris

[matlabbe]

Hi,

With the ZED camera, you may use RGB-D example, as ZED SDK provides already a depth image (using their own dense disparity approach). Follow ZED subsections here: http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping

If you want to use ZED as a stereo camera (as in this tutorial) or you would want another dense disparity computation approach (like cv::StereoBM), there are indeed different approaches on how the disparity is computed.

1) By default, dense disparity is not used for transformation estimation, just for visualization and creating occupancy grid. See Stereo B setup. An optical flow approach is used to get disparity of each extracted feature (not all pixels). Parameters are:

$ rtabmap --params | grep Stereo/
Param: Stereo/Eps = "0.01"                                 [[Stereo/OpticalFlow=true] Epsilon stop criterion.]
Param: Stereo/Iterations = "30"                            [Maximum iterations.]
Param: Stereo/MaxDisparity = "128"                         [Maximum disparity.]
Param: Stereo/MaxLevel = "3"                               [Maximum pyramid level.]
Param: Stereo/MinDisparity = "1"                           [Minimum disparity.]
Param: Stereo/OpticalFlow = "true"                         [Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.]
Param: Stereo/SSD = "true"                                 [[Stereo/OpticalFlow=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.]
Param: Stereo/WinHeight = "3"                              [Window height.]
Param: Stereo/WinWidth = "15"                              [Window width.]

As you can see, maximum disparity is set at 128. With 1080p images, you may want to increase it to 300 or more for example. Maybe set pyramid level to 4 too. The window size could be also increased. You can also try the simple block matcher algorithm (Stereo/OpticalFlow=false). The dense disparity images used for visualization are computed using opencv StereoBM algorithm. Parameters are here:

$ rtabmap --params | grep StereoBM/
Param: StereoBM/BlockSize = "15"                           [See cv::StereoBM]
Param: StereoBM/MinDisparity = "0"                         [See cv::StereoBM]
Param: StereoBM/NumDisparities = "128"                     [See cv::StereoBM]
Param: StereoBM/PreFilterCap = "31"                        [See cv::StereoBM]
Param: StereoBM/PreFilterSize = "9"                        [See cv::StereoBM]
Param: StereoBM/SpeckleRange = "4"                         [See cv::StereoBM]
Param: StereoBM/SpeckleWindowSize = "100"                  [See cv::StereoBM]
Param: StereoBM/TextureThreshold = "10"                    [See cv::StereoBM]
Param: StereoBM/UniquenessRatio = "15"                     [See cv::StereoBM]

2) A second approach is to compute disparity image before rgbd_odometry and rtabmap nodes. See Stereo setup A. You will then get a depth image to use it like an RGB-D camera. This approach uses also StereoBM from stereo_image_proc package. This tutorial can be used to tune stereo matcher parameters.

3) The stereoToDepth_ parameter is true only if we set parameter "stereo_to_depth" to true. In this setup, stereo_odometry still compute transformation estimation with the "Stereo/" parameters above, but rtabmap node will generate a disparity image (like stereo_image_proc) using StereoBM parameters before doing the map update (then doing RGB-D update instead of stereo update). This is very similar to using stereo_image_proc before rtabmap node, but the advantage is that a disparity image is computed only at the rate of rtabmap update (default 1 Hz) instead for all images of the camera (which would be 30 Hz), thus saving a lot of computing resources.

EDIT 
For odometry getting lost often, while choosing good stereo parameters, I recommend also to use "Vis/EstimationType" set to 1 for stereo cameras.

$ roslaunch rtabmap_ros stereo_mapping.launch rtabmap_args:="--delete_db_on_start --Vis/EstimationType 1"

Note that you can set parameters above the same way using "rtabmap_args" argument for convenience.

cheers,
Mathieu

[cpsarras]

Thank you very much for the detailed answer. It is now clear how disparity is calculated and by whom.

Your recommendations on MaxDisparity, MaxLevel, WinHeight and WinWidth were spot on. I used a MaxDisparity of 1000 (I suppose disparity is in pixels and is not normalized) as the camera was sometimes very close to the ground and facing it. I also doubled WinHeight and WinWidth and set MaxLevel to 4. This setup seems to work just fine with the 1920x1080 camera feed.

PS. I noticed very bad odometry calculation using the default GFTT/BRIEF algorithm for feature extraction. I changed it to SURF, which yields exceptional results.

Chris

[matlabbe]

Hi,

Thank you for the SURF vs GFTT/BRIEF comparison. GFTT has also parameters not scaling with the image size:

./rtabmap --params | grep GFTT/
Param: GFTT/BlockSize = "3"                                []
Param: GFTT/K = "0.04"                                     []
Param: GFTT/MinDistance = "5"                              []
Param: GFTT/QualityLevel = "0.01"                          []
Param: GFTT/UseHarrisDetector = "false"                    []

For example, GFTT/MinDistance and GFTT/BlockSize could be increased for larger images. But anyway, I'll give a try with our ZED camera to find out what is the problem with GFTT/BRIEF on large images.

cheers,
Mathieu

[matlabbe]

Update: To use GFTT/BRIEF with 1080p images, we should also double GFTT/BlockSize and GFTT/MinDistance.

[cpsarras]

I can confirm good results with those GFTT settings. (if not identical to default SURF)