/rtabmap/grid_map not aligned with 2d scan

[seanxu112]

I am currently trying to use both t265 and d415 with rtabmap, though not using intel's provided 3d printed mount. We have a good estimate of the TF between the two cameras and laser_frame, but the grid map seems to be shifted translationally. As shown in the image below, is it just that we need a very precise TF or is there anything wrong with our rtabmap setup?



<launch>
  <include file="$(find realsense2_camera)/launch/rs_rgbd_t265_lidar.launch"/>

  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="rtabmap_args" value="--delete_db_on_start"/>
    <arg name="depth_topic" value="/camera/aligned_depth_to_color/image_raw"/>
    <arg name="rgb_topic" value="/camera/color/image_raw"/>
    <arg name="camera_info_topic" value="/camera/color/camera_info"/>
    <arg name="approx_sync" value="true"/>
    <arg name="visual_odometry" value="false"/>
    <arg name="odom_topic" value="/odometry"/>
    <arg name="odom_frame_id" value="t265_odom_frame"/>
    <arg name="frame_id" value="t265_link"/>
   
   
    <arg name="subscribe_scan" value="true"/>
    <arg name="scan_topic" value="/scan"/>

    <para m name="RGBD/NeighborLinkRefining" type="string" value="true"/>
    <para m name="RGBD/ProximityBySpace"     type="string" value="true"/>
    <para m name="RGBD/AngularUpdate"        type="string" value="0.01"/>
    <para m name="RGBD/LinearUpdate"         type="string" value="0.01"/>
    <para m name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    <para m name="Grid/FromDepth"            type="string" value="false"/>
    <para m name="Reg/Force3DoF"             type="string" value="true"/>
    <para m name="Reg/Strategy"              type="string" value="1"/> 

   
    <para m name="Icp/VoxelSize"                 type="string" value="0.05"/>
    <para m name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
  </include>
</launch>

rs_rgbd_t265_lidar.launch:
  everything is the same as the example launch file provided by realsense2_camera but these tfs'

<launch>

  ....

  <node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="-0.06 0 -0.18 0 -0.1745 0 /t265_link /camera_link 100"/>

 
  <include file="$(find ydlidar)/launch/lidar.launch"/>

  <node pkg="tf" type="static_transform_publisher" name="t265_to_ydlidar" args="-0.025 0 -0.07 0 -0.058 -0.055 /t265_link /laser_frame 100"/>
</launch>

[matlabbe]

Can you make small rosbag of couple of seconds or a database to share? The ray tracing seems matching the red points (which I assume is the lidar), but not the black cells, which have an offset. Normally, both empty and obstacle cells should be relative to each other. I don't see a problem with the launch file.

[seanxu112]

Sorry for the late reply, here is the bag that I used. I changed a few things from when I posted.
https://drive.google.com/open?id=1UhIhi5OCNbsN7KOyLjxHa9HUdr0-RfmB

One thing to note is that my t265 is tilted upwards, not sure if that is causing the problem.

[matlabbe]

Hi,

I replayed the bag (with map frame filtered using this script to avoid tf conflict with re-running rtabmap on that bag, note also that I launched in different namespace rtabmap2 to avoid conflicts with rtabmap topics recorded in the bag) like this:

$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Reg/Strategy 1" \
   namespace:="rtabmap2" \
   visual_odometry:=false \
   odom_frame_id:=t265_odom_frame \
   frame_id:=t265_rectified_pose_frame \
   rgbd_sync:=true \
   depth_topic:=/camera/aligned_depth_to_color/image_raw \
   rgb_topic:=/camera/color/image_raw \
   camera_info_topic:=/camera/color/camera_info \
   subscribe_scan:=true \
   scan_topic:=/ydlidar/scan \
   use_sim_time:=true \
   approx_rgbd_sync:=false \
   queue_size:=100

$ python filterBagTF.py #Change input.bag to rtabmap_tilt.bag and output.bag to rtabmap_tilt_out.bag
$ rosbag play --clock --pause rtabmap_tilt_out.bag



So I cannot reproduce the strange occupancy grid of the bag.

cheers,
Mathieu