Re: Filtering rtabmap localization jumps with robot_localization in 2D

Posted by Sukhraj Kair on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Filtering-rtabmap-localization-jumps-with-robot-localization-in-2D-tp5931p6935.html

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>