Problems interfacing RTAB-Map and V-Rep

[PierHarper]

Hello to everybody.
I'm using RTAB-Map with the aim to create a 3D dense point-cloud map of a simulated environment.I'm using V-Rep to simulate as the environment as the mobile robot in it.The mobile robot is equipped with a kinect (the one available in V-Rep) which streams color images (in rgb8 format) and depth images (in rgb8 format, so i'm forced to convert the images in 16UC1 encoding using cv_bridge) .Unfortunately, when i try to visualize (in rviz) the voxel_cloud generated by the "points_xyzrgb" nodelet, it appears to be deformated ,more precisely, the scene object reconstructed appears to be magnified, bigger than theirs counterparts in the native scene (imagine an object reconstructed in an higher scale). Moreover MapCloud seems to be unable to build the 3D dense map. All the code has been tested also on a real robot in my laboratory and works fine without the mentioned problems. Someone knows why, with simulated scenes i got these troubles?

[matlabbe]

Hi,

Verify that you have a camera_info simlar to a real camera and print your converted 16UC1 values to make sure they match your simulated scene scale. Note that 16UC1 format is in mm.

cheers

[PierHarper]

Hi matlabbe, thanks for your help.
These are the simulated kinect matrix:
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [589.3667059626796, 0.0, 320.0, 0.0, 589.3667059626796, 240.0, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [589.3667059626796, 0.0, 320.0, 0.0, 0.0, 589.3667059626796, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0]

On the other hand the real kinect matrix:
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [570.3422241210938, 0.0, 319.5, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [570.3422241210938, 0.0, 319.5, 0.0, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0]

There are no other difference between the two cameras.
Then
 i've printed the 16UC1 values.The data field contains values up to
60.Are these ones in mm? If so there is an error because the obstacle is
 at least one meter far from kinect.
Have you got any idea?
Thanks 😉

[matlabbe]

Hi,

Maybe there's a problem with the conversion from rgb8 to 16UC1. It is strange that the depth image is in rgb8 format at first. Is that depth image a grayscale image of the depth in 8 bits? If so, it doesn't contain the actual depth values (in meters or mm).

In this post, it seems that  simros_strmcmd_get_vision_sensor_depth_buffer returns float32 data

vrep_common::VisionSensorDepthBuff::data (Float32MultiArray): the depth values. Values are in the range of 0-1 (0=closest to sensor, 1=farthest from sensor).

If values are between 0 and 1, you would have to convert this message to meters (FLOAT32) according to maximum range of the camera.

cheers

[PierHarper]

Hi, thanks for your last reply, it helped me a lot!
Unfortunately i got another problem.
The message's data field streamed by simros_strmcmd_get_vision_sensor_depth_buffer contains 480*640*1=307200 elements while the brand new sensor_msgs/Image i've created contains 480*640*4*1=1228800 elements in its data field.
When i put these 307200 depth elements ,multiplied for 300 in order to consider my kinect's range (3m), in the data field of the new image, i noticed that remains 1228800-307200=921600 empty elements.Obviously, if i left empty these 921600 elements and try to run rtabmap and visualize on rviz i got nothing in the sense that there is no 3D map reconstruction. I also got same result if i firstly build an image in 8UC1 format in which i put the depth_values (with no one left empty) and then convert it in 16UC1 format and then in 32FC1 format using cv_bridge (the 3D map is not build and i get some warnings like "util3d_registration.cpp:184::transformFromXYZCorrespondences() RANSAC refineModel: Refinement failed: got an empty set of inliers!" , "[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow." and errors:"[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.") .
How can i solve it?

[matlabbe]

Hi,

Your sensor_msgs/Image should have "32FC1" or "16UC1" encoding. Using cv_bridge, you can create such image like this:

cv::Mat depthData(480, 640, CV_32FC1); // your input data from VREP (values scaled between 0 and 1)

cv_bridge::CvImage depth;
depth.header.frame_id = "camera";
depth.header.stamp = ros::time::now();
depth.encoding = "bgr8";
depth.image = depthData * 3.0f; // scale to 3 meters
depthPub.publish(depth.toImageMsg());

You may want to use DepthCloud Plugin in RVIZ to visualize the cloud generated by depth and rgb topics.

cheers

[PierHarper]

Hi, i've followed your instructions. Thanks a lot! It works fine!
There is only another little problem.
The simulated  V-rep kinect "puts" black everything is not in the camera range. For example, if there are no objects in kinect's range , the camera sees everything black. I really don't want these black pixels reconstructed in my 3D map by rtabmap but only the scene objects (walls,doors,people,other robots) kinect actually sees. Is there a way to filter these black pixels and, so, have a 3D map with only what is actually in kinect's range?
Thanks

[matlabbe]

Hi,

Black pixels are added because they have valid corresponding depth values (you could have a black couch for example). If you don't want them, set their corresponding pixel in the depth image to 0 meter.

cheers