Kinect For Azure / L515 - ICP (lighting invariant mapping)

[matlabbe]

Hello,

EDIT: Config files for K4A and L515 and be downloaded on v0.20.8 release.

With the L515 and Kinect For Azure, I've seen some great scans but I didn't see any scans in pitch dark using LiDAR capability advantage of those cameras. Here is a config file for Kinect For Azure with ICP mode using this config file: config_k4a_icp_v3.ini. RTAB-Map should be built with libpointmatcher, GTSAM and Kinect For Azure driver. I don't have a L515 to test with, but from what I saw, I think similar parameters could be used. Updated with L515 example below! For quick reference, here are the RTAB-Map parameters that I have modified:

Icp/CorrespondenceRatio=           0.2  (default=0.1)
Icp/PMOutlierRatio=                0.65  (default=0.95)
Icp/Epsilon=                       0.005  (default=0)
Icp/PointToPlaneMinComplexity=     0  (default=0.02)
Icp/VoxelSize=                     0  (default=0.05)
Odom/ScanKeyFrameThr=              0.7  (default=0.9)
OdomF2M/ScanMaxSize=               15000  (default=2000)
Optimizer/GravitySigma=            0.3  (default=0.0)
RGBD/ProximityPathMaxNeighbors=    1  (default=0)
Reg/Strategy=                      1  (default=0)

The Preferences->Source parameters. Note that I use IR mode for Kinect 4 Azure to use the raw LiDAR data (without distortion that could be added when registering to RGB camera and to have the whole LiDAR Field-Of-View) for best accuracy:

For RealSense L515 (this commit required):

Here we generate a downsampled 3D laser scan from the depth image of Kinect to be able to use ICP:

LiDAR intensity (voxel size=1cm):

Normals colored along x-axis:

Normals colored along y-axis:

Normals colored along z-axis:

Colored height ramp (with EDL shader):

Regards,
Mathieu

[David9696]

Hi matlabbe,

Thanks for sharing your result.

I have tried the standalone rtabmap to build a map with L515+T265. I could get similar result that you have shown.

Is it possible to implement the same setup in ros environment? I tried to build a 3d map with the same cameras but I can't reproduce the same result as the standalone rtbamap. For the same setup, any guideline for the launch file and param setup?

Hope you could help to reproduce similar result in ROS.

Thanks.

[quizzy5889]

hi matlabbe,
thank you first of all for this post. I was able to try and produce results similar to those proposed.

I wonder if there is a possibility to include the RGB information in the final pointcloud?

Thanks a lot

[matlabbe]

@David9696 You could do the same in ROS. Launch rtabmap node with T265 odometry as input, and feed L515 data for RGB/depth images. Make sure to adjust static TF between L515 and T265.

@quizzy5889 To get RGB color in point cloud while doing ICP for odometry, just set "IR mode" checkbox to false under RealSense2 group in Source panel.

[matlabbe]

With L515, here is a ROS example based on the standalone example above (ref):

roslaunch realsense2_camera rs_camera.launch\
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
    enable_accel:=true

rosrun imu_filter_madgwick imu_filter_node\
     _use_mag:=false\
     _publish_tf:=false\
     _world_frame:="enu"\
     /imu/data_raw:=/camera/imu\
     /imu/data:=/rtabmap/imu

rosrun nodelet nodelet standalone rtabmap_ros/point_cloud_xyz \
    _approx_sync:=false  \
    /depth/image:=/camera/depth/image_rect_raw \
    /depth/camera_info:=/camera/depth/camera_info \
    _decimation:=4

roslaunch rtabmap_ros rtabmap.launch\
    rtabmap_args:="\
      --delete_db_on_start \
      --Icp/VoxelSize 0.05 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/PointToPlaneK 20 \
      --Icp/CorrespondenceRatio 0.2 \
      --Icp/PMOutlierRatio 0.65 \
      --Icp/Epsilon 0.005 \
      --Icp/PointToPlaneMinComplexity 0 \
      --Odom/ScanKeyFrameThr 0.7 \
      --OdomF2M/ScanMaxSize 15000 \
      --Optimizer/GravitySigma 0.3 \
      --RGBD/ProximityPathMaxNeighbors 1 \
      --Reg/Strategy 1" \
    icp_odometry:=true \
    scan_cloud_topic:=/cloud \
    subscribe_scan_cloud:=true \
    depth_topic:=/camera/aligned_depth_to_color/image_raw \
    rgb_topic:=/camera/color/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    approx_sync:=false \
    wait_imu_to_init:=true \
    imu_topic:=/rtabmap/imu