Filtering rtabmap localization jumps with robot_localization in 2D

classic Classic list List threaded Threaded
7 messages Options
Reply | Threaded
Open this post in threaded view
|

Filtering rtabmap localization jumps with robot_localization in 2D

matlabbe
Administrator
This post was updated on .
Here is a small example (working in 2D) to filter jumps caused by relocalization in localization mode. The idea is to smooth /map -> /odom transform with robot_localization package like a GPS (based on this example).

First, we need to disable tf published from rtabmap with publish_tf:=false, as it will be robot_localization that will publish /map -> /odom:
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   ...
   <param name="publish_tf" value="false"/>
   <param name="Reg/Force3DoF" value="true"/> <!-- 2D mode -->
</node>

Setup robot_localization like the GPS example:
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
      <param name="frequency" value="20"/>
      <param name="two_d_mode" value="true"/>

      <param name="map_frame" value="map"/>
      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="base_footprint"/>
      <param name="world_frame" value="map"/>

      <param name="odom0" value="odom"/>
      <param name="pose0" value="rtabmap/localization_pose"/> 

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, true,
                                      false, false, false,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam param="pose0_config">[
                                     true,   true,  false,
                                     false,  false, true,
                                     false,  false, false,
                                     false,  false, false,
                                     false,  false, false] </rosparam>  
      
      <param name="odom0_differential" value="true"/>
      <param name="pose0_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="pose0_relative" value="false"/>

      <param name="odom0_queue_size" value="5"/>
      <param name="pose0_queue_size" value="5"/> 

</node>

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

Sukhraj Kair
Hi Mathieu

I'm following this guide to filter out jumps coming from rtabmap localization but rtabmap is not publishing /rtabmap/localization_pose

I'm using rtabmap rgbd_odometry to publish odom->robot_footprint transform.

Here is my launch file for rtabmap:

<launch>
  <!-- Arguments for launch file with defaults provided -->
  <arg name="database_path"     default="rtabmap2.db"/>
  <arg name="rgb_topic"   default="/realsense/rgb/image_raw"/>
  <arg name="depth_topic" default="/realsense/depth/image_raw"/>
  <arg name="camera_info_topic" default="/realsense/rgb/camera_info"/> 
  <arg name="odom_topic" default="rgbd_odom"/>  
  <arg name="odom_frame_id" default="odom"/>

  <!--Launch rtabmap in localization mode-->
  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">

        <!-- Basic RTAB-Map Parameters -->
        <param name="database_path"       type="string" value="$(arg database_path)"/>
        <param name="frame_id"            type="string" value="robot_footprint"/>
        <param name="odom_frame_id"       type="string" value="$(arg odom_frame_id)"/> 
        <param name="subscribe_depth"     type="bool"   value="true"/>
        <param name="subscribe_scan"      type="bool"   value="true"/>

        <!-- RTAB-Map Inputs -->
        <remap from="scan" to="/scan"/>
        <remap from="rgb/image" to="$(arg rgb_topic)"/>
        <remap from="depth/image" to="$(arg depth_topic)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
        <remap from="odom" to="$(arg odom_topic)" />

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

        <!-- Rate (Hz) at which new nodes are added to map -->
        <param name="Rtabmap/DetectionRate" type="string" value="1"/>

        <!-- 2D SLAM -->
        <param name="Reg/Force3DoF" type="string" value="true"/>

        <!-- Loop Closure Detection -->
        <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE -->
        <param name="Kp/DetectorStrategy" type="string" value="0"/>

        <!-- Maximum visual words per image (bag-of-words) -->
        <param name="Kp/MaxFeatures" type="string" value="400"/>

        <!-- Used to extract more or less SURF features -->
        <param name="SURF/HessianThreshold" type="string" value="100"/>

        <!-- Loop Closure Constraint -->
        <!-- 0=Visual, 1=ICP (1 requires scan)-->
        <param name="Reg/Strategy" type="string" value="1"/>

        <!-- Minimum visual inliers to accept loop closure -->
        <param name="Vis/MinInliers" type="string" value="15"/>

        <!-- Enabling robust graph optimization using Vertigo-->
        <param name="RGBD/OptimizeStrategy" type="string" value="1"/> <!-- g2o=1, GTSAM=2 -->
        <param name="RGBD/OptimizeRobust" type="string" value="true"/>
        <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!-- should be 0 if RGBD/OptimizeRobust is true --> 

        <!-- localization mode -->
        <param name="Mem/IncrementalMemory" type="string" value="false"/>
        <param name="Mem/InitWMWithAllNodes" type="string" value="true"/> 
        <!--disable map->odom transform from rtabmap to use external localization package-->
        <param name="publish_tf" value="false"/>
    </node>
</launch>
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

matlabbe
Administrator
Hi,

/rtabmap/localization_pose is published only on loop closure (SLAM mode) or when localization happens (localization mode).

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

Artem Lobantsev
I had the issues with filter diverging exactly for the same case. The solution was founded in ignoring odometry absolute bose, but use only the twist information, Maybe it will help someone.

<rosparam param="odom0_config">[false,  false,  false,
                                false,  false,  false,
                                true,   true,   false,
                                false,  false,  true,
                                false,  false,  false]</rosparam>
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

matlabbe
Administrator

Note that in the code above the odometry is set differential and localization pose as absolute if this can change something:
<param name="odom0_differential" value="true"/>
<param name="pose0_differential" value="false"/>

<param name="odom0_relative" value="true"/>
<param name="pose0_relative" value="false"/>
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

derektan1995
This post was updated on .
Hi Mathieu,

I am trying out the approach you have posted using robot_localization via ekf to get rid of jumping localization poses. After running the ekf node, i noticed that my base_link tf jitters very significantly, even though my map and odom tf remains relatively still.

Here is a video of the jittering effects: https://drive.google.com/file/d/16h-7SrQsWLnX6InZ1AA2P5VU39tu0lW-/view?usp=sharing

I dug further and I realized that the rtabmap_ros node is still publishing tf data even though the parameter 'publish_tf' is set to false. I noticed this using the command 'rostopic info /tf', and received the output:



Any insights as tf is still being published even though 'publish_tf' is set to false? I suspect this could be the reason why the base_link is jittering so much.

p.s. I used your robot_localization ekf code as per what you have posted, without changing anything. Here is my launch file code ('localization' and 'smoothen_localization_using_ekf' parameters are set to true): https://drive.google.com/file/d/1DNw4DrwKxVhSeR2d58YsjvuwMXdCZxfl/view?usp=sharing

EDIT: It seems that if I set both odom0_differential and odom0_relative to false, the jitter effect is gone. However, there is no change the original localization jitters.

Much thanks,
Derek
Reply | Threaded
Open this post in threaded view
|

Re: Filtering rtabmap localization jumps with robot_localization in 2D

matlabbe
Administrator
Not sure about the localization jitters, but for publish_tf, rtabmap may still create the tf broadcaster when publish_tf is false, but normally the tf won't be published. It is maybe why tou can see that rtabmap is still a Tf publisher. You can also do "rosrun topic echo /tf" to make sure, or show the Tf tree.