/rtabmap/grid_map not aligned with 2d scan

Posted by seanxu112 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/rtabmap-grid-map-not-aligned-with-2d-scan-tp6595.html

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>