Ask Your Question
0

incorrect rtabmap parameters, loop closure problem

asked 2020-01-14 02:20:46 -0600

june2473 gravatar image

updated 2020-01-15 23:39:10 -0600

Hello! I am looking at some amazing rtabmap mappings such as

https://www.youtube.com/watch?v=Fvhxd...

https://www.youtube.com/watch?v=j8OYl...

and my mapping does not look good at all, I dont know why exactly, distance for mapping is too big or odometry is not enough good, there is a lot of shifts and inexactitudes.

here it is: https://yadi.sk/i/89HN-ixWRIJvvw (if there is no play button, try to reload page)

I am using only two cameras, d435 and t265 firmly attached to each other, and walking indoor with laptop in one hand and those cameras in other hand.

Can anyone take a glance on my rtabmap parameters and tell me what is definitely incorrect there?

<launch>
  <arg name="use_zed"         default="false"  doc="Set to false when using Intel Realsense D435"/>
  <arg name="localization"    default="false" doc="Localization mode for navigation"/>
  <arg name="database_path"   default="rtabmap.db"/>
  <arg name="rviz"            default="false"/>
  <arg name="rate"            default="1.0"/>
  <arg name="camera_frame"    default="d400"/>

  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
  <arg     if="$(arg localization)" name="rviz_config"   default="$(find turtlebot3_slam_3d)/config/navigation.rviz"/>
  <arg unless="$(arg localization)" name="rviz_config"   default="$(find turtlebot3_slam_3d)/config/mapping.rviz"/>

  <arg name="input_scan"       default="/scan"/>
  <arg name="input_odom"       default="/t265/odom/sample"/>
  <arg     if="$(arg use_zed)" name="input_image"        default="/stereo_camera/left/image_rect_color"/>
  <arg unless="$(arg use_zed)" name="input_image"        default="/d400/color/image_raw"/>
  <arg     if="$(arg use_zed)" name="input_depth"        default="/stereo_camera/depth/depth_registered"/>
  <arg unless="$(arg use_zed)" name="input_depth"        default="/d400/aligned_depth_to_color/image_raw"/>
  <arg     if="$(arg use_zed)" name="input_camera_info"  default="/stereo_camera/left/camera_info"/>
  <arg unless="$(arg use_zed)" name="input_camera_info"  default="/d400/color/camera_info"/>

 <!-- RTAB=Map node -->
  <group ns="rtabmap">

    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/d400/color/image_raw"/>
      <remap from="depth/image"     to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/d400/color/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics
          (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="false"/>
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">

      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="frame_id"            type="string" value="t265_pose_frame"/>
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_rgbd"      type="bool"   value="true"/>
      <param name="subscribe_scan"      type="bool"   value="false"/>

      <!-- Input topics -->
      <remap from="scan"                to="$(arg input_scan)"/>
      <remap from="odom"                to="$(arg input_odom)"/>
      <remap from="rgb/image"           to="$(arg input_image)"/>
      <remap from="depth/image"         to="$(arg input_depth)"/>
      <remap from="rgb/camera_info"     to="$(arg input_camera_info)"/>
      <remap from="rgbd_image"          to="rgbd_image"/>

      <!-- Output topics -->
      <remap from="grid_map" to="/map"/>

      <param name ...
(more)
edit retag flag offensive close merge delete

Comments

1

The D400 depth is not very accurate farther than 3-4 meters, I would set a maximum depth in MapCloud plugin in RVIZ. Why t265odom_to_map is used? Can you show the TF tree (rosrun tf view_frames)?

matlabbe gravatar imagematlabbe ( 2020-01-14 12:25:42 -0600 )edit

I have set maximum depth in RVIZ to 2 meters. How can I configure rtabmap to not build the map far than 2 meters?

june2473 gravatar imagejune2473 ( 2020-01-14 22:50:05 -0600 )edit

t265 was because I forgot to set frame_id at first. Now I have removed that line and frames pdf is here.

Nothing actually changed, mapping in rviz does look well.

june2473 gravatar imagejune2473 ( 2020-01-14 22:54:07 -0600 )edit

I changed frame_id from t265_pose_frame to t265_link.

And I have made new test with long run of big room: https://yadi.sk/i/w9Ci5sri45WPFA

What do you think, what is wrong in my configuration? and what can I do to improve mapping?

june2473 gravatar imagejune2473 ( 2020-01-15 04:49:19 -0600 )edit
1

Can you share that database, it would be useful to debug where the drift is coming from (T265? or loop closures). What is the fixed transform between T265 and D435? To limit the range of the occupancy grid, set Grid/RangeMax to 2.

matlabbe gravatar imagematlabbe ( 2020-01-15 14:29:52 -0600 )edit

you mean rtabmap.db file? I have it. I dont have bag file of that video because I haven't recorded it. I can make new passage and record bag file.

june2473 gravatar imagejune2473 ( 2020-01-15 23:49:42 -0600 )edit

fixed transform between t265 and d435 is:

<node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>

it's from rs_d400_and_t265.launch file

june2473 gravatar imagejune2473 ( 2020-01-16 00:37:28 -0600 )edit

I have made new test! it looks far more better than previous.

https://yadi.sk/d/zIoELo4EGLUO6A

what do you think?

june2473 gravatar imagejune2473 ( 2020-01-17 00:38:59 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-01-17 16:55:25 -0600

matlabbe gravatar image

From the database you shared in the comments, try this:

Kp/MaxDepth=0                   # Should be 0 in general
Vis/MaxDepth=0                  # Should be 0 in general
RGBD/NeighborLinkRefining=false # Don't refine T265 odometry, it should be already accurate
Optimizer/GravitySigma=0.1      #  Force graph optimization with gravity constraints
Mem/UseOdomGravity=true         # Get gravity constraints from odometry (assuming VIO from T265)

Example reprocessing the database with those parameters:

$ rtabmap-reprocess --Kp/MaxDepth 0 --Vis/MaxDepth 0 --RGBD/NeighborLinkRefining false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true rtabmap.db output.db

Also, it is better to disable relocalisation feature of T265 if possible when using rtabmap. This issue is explained here.

You should also adjust the transform between the cameras based on your setup (for better loop closure optimizations):

<node pkg="tf" type="static_transform_publisher" name="t265_to_d400" args="0 0 0 0 0 0 /$(arg tf_prefix_camera1)_link /$(arg tf_prefix_camera2)_link 100"/>

cheers,
Mathieu

edit flag offensive delete link more

Comments

Thank you!

Here is what I got: https://yadi.sk/d/H9N_5g8f5TMYFg

june2473 gravatar imagejune2473 ( 2020-01-20 01:02:15 -0600 )edit

Most of the drift seems caused by visual odometry. rtabmap seems correcting large loops, but if the camera drifted locally over some meters, that won't be fixed. Here is a video showing some results that I would expect from a T265+D435 setup: https://www.youtube.com/watch?v=iHQOp...

matlabbe gravatar imagematlabbe ( 2020-01-20 15:22:58 -0600 )edit

you mean, I ought to move t265 better, trying not to drift t265 odometry?

Can you suggest recommended movements of t265, which won't cause such drifts?

june2473 gravatar imagejune2473 ( 2020-01-20 23:06:16 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-01-14 02:20:46 -0600

Seen: 22 times

Last updated: Jan 17