Export lidar point cloud with color

[VK]

Hi Mathieu,

I have a 435i realsense depth camera and a Velodyne lidar, rigidly mount with known extrinsics.
I would like to get odometry from lidar scans (I'm using icp_odometry node in rtabmap_ros) and the final point clouds from those scans with the color from the rgb sensor of D435 (i.e. for now I want to use the depth camera only for color).

Do do so (please correct if am wrong somewhere), I'm publishing the static TF betwwen Velodyne and D435 (camera_link) and launching rtab as below:

$ rosrun tf2_ros static_transform_publisher -0.075 0 0.178 0 0 0 1 camera_link velodyne
(velodyne is above D435 and "behind")

roslaunch rtabmap_ros rtabmap.launch \
   rtabmapviz:=true \
   use_sim_time:=true \
   frame_id:=camera_link \
   pm:=true \
   depth:=false \
   subscribe_depth:=true \
   subscribe_scan_cloud:=true \
   rgb_topic:=/camera/color/image_raw \
   depth_topic:=/camera/aligned_depth_to_color/image_raw \
   camera_info_topic:=/camera/color/camera_info \
   scan_cloud_topic:=/velodyne_points \
   icp_odometry:=true \
   approx_sync:=true \
   args:="-d  \
       --RGBD/CreateOccupancyGrid false \
       --Rtabmap/DetectionRate 0 \
       --Reg/Strategy 1 \
       --Odom/ScanKeyFrameThr 0.8 \
       --OdomF2M/ScanMaxSize 999900000000  \
       --OdomF2M/ScanSubtractRadius 0.05   \
       --Icp/PM true \
       --Icp/VoxelSize 0.05   \
       --Icp/MaxTranslation 2   \
       --Icp/MaxCorrespondenceDistance 0.1 \
       --Icp/PMOutlierRatio 0.7 \
       --Icp/Iterations 25 \
       --Icp/PointToPlane true \
       --Icp/PMMatcherKnn 25 \
       --Icp/PMMatcherIntensity true \
       --Icp/PMMatcherEpsilon 1 \
       --Icp/Epsilon 0.0001 \
       --Icp/PointToPlaneK 25 \
       --Icp/PointToPlaneRadius 0 \
       --Icp/CorrespondenceRatio 0.05 \
       --RGBD/NeighborLinkRefining true \
       --Optimizer/Iterations 50"

When I open the database viewer, both lidar and visual data are indeed strored in pose graph.
When I try to export the final point cloud, I select lidar as source and checking the  option for camera projection in order to add color in the cloud.

The weird thing is that there is an error when projecting the color (crearly visible in the Z axis)


(The vertical line indicates the zone that should be "brown", ie. the "color" should be moved up).

To me it seems that it is a TF problem, so I checked the TF between sensors but changing the values didn't make any sense (as the the best result I got was when I used  $ rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 1 camera_link velodyne that is not reasonable).

Any ideas??