Localization issue

[aviadoz]

Hi,

I am using kinetic distro and rtabmap for navigation in our lab.
I mapped the lab with realSense 435 depth camera and used encoders to publish odometry (I tried without the encoders and the results are the same). I used "depthimage_to_laserscan" pkg to build the occupancy2d map. The 2D map is better if I use fake laser scan.
I know that my localization is not good (I heard to "localization_pose" topic and it is not good).
I tried many things to improve the localization but without a success.
Now I was working with Rtabmapviz and I noticed that there is a different in the frames of the loop closure windows (screenshot image).
I think that here is the problem. I also saw that the robot axes is jump a little when it is standing.
I will glad if someone will help me with that issue.

Tnx

 Capture.JPG 

[matlabbe]

Hi,

The loop closure seems normal. If you stand still and localizations are detected, the robot may jump a little on place (the /map -> /odom TF frame), because the localization transformations are not perfect. If you have very large jumps, try recording a video of the problem.

cheers,
Mathieu

[aviadoz]

Hi Mathieu,

So, I made a video which describe my issue.
I have a  previous map with DetectRate  = 0, my odom>base_link was published by node which I created and not from another pkg or robot_localization for example.

Here my launch file:

 

<?xml version="1.0"?>
<launch>
<arg name="vo"              default="false" />
<arg name="amcl"              default="false" />
<arg name="move_base"         default="true" />
<arg name="mapping"         default="false" />
<arg name="outdoor"         default="false" />
<arg name="urdf"         default="false" />
<!--load platform 3d model-->
<include if="$(arg urdf)" file="$(find arl_description)/launch/newurdf.launch" />
<!--<node pkg="tf" type="static_transform_publisher" name="map_odom" args="0 0 0 0 0 0 map odom 30" />
<node pkg="tf" type="static_transform_publisher" name="odom_base_footprint" args="0 0 0 0 0 0 odom base_footprint 30" />-->
<!--<node pkg="tf" type="static_transform_publisher" name="base1" args="0 0 0 0 0 0 base_footprint base_link 30" />-->
 <!-- bringup realsense camera -->

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

<!--outdoor configuration for realsense-->
<node if="$(arg outdoor)" name="dynamic_reconfigure_load" pkg="dynamic_reconfigure" type="dynparam" args="load /camera/realsense2_camera_manager $(find realsense2_camera)/launch/includes/test_yaml.yaml" >
</node>

<!--camera tf-->
<node pkg="tf" type="static_transform_publisher" name="camera_link_arl" args="0.15 0 0.2 0 0 0 base_link camera_link 30" />
 
<!-- Laser topic -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
	<remap from="image" to="/camera/depth/image_rect_raw"/>
	<remap from="camera_info" to="/camera/depth/camera_info"/>
 	<param name="range_min" value="0.1"/>
	<param name="range_max" value="5"/>
	
		<!--<param name="frame_id" value="camera_link" />--> 
	  </node>
<!-- Arduino connection -->
 <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="57600"/>
  </node>

<!-- encoders odometry -->
	<node unless="$(arg vo)" pkg="arl_description" type="physical_odometry" name="physical_odometry" output="screen"/>

<!-- efk for localization 
<include file="$(find robot_localization)/launch/ekf_template.launch" />-->	

<!--load rtabmap mapping
<include  file="$(find arl_description)/launch/rtabmap_realsense.launch" unless="$(arg amcl)" >-->
<include  file="$(find arl_description)/launch/rtabmap_realsense.launch" output="screen" > 

<arg name="vo"              default="$(arg vo)" />
<arg name="mapping"                 value="$(arg mapping)" />
<arg if="$(arg mapping)" name="localization" value="false" />
<arg unless="$(arg mapping)" name="localization" value="true" />

 <arg name="rtabmapviz"              default="false" /> 
 <arg name="rviz"                    default="true" />
  <arg name="frame_id"               value="base_link" />
  <arg name="rgb_topic"               default="/camera/color/image_raw" />
  <arg name="depth_topic" 	      default="/camera/depth/image_rect_raw" /><!--was aligned-->
  <arg name="camera_info_topic"       default="/camera/color/camera_info" />
 </include>


 <!-- ROS navigation stack move_base -->
     <node if="$(arg move_base)" pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
       <!-- <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
	<rosparam file="$(find arl_description)/launch/nav_data/global_planner_params.yaml" command="load" />-->
	<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
<!--<rosparam file="$(find arl_description)/launch/nav_data/planner.yaml" command="load"/>
	<rosparam file="$(find arl_description)/launch/nav_data/dwa_local_planner.yaml" command="load" /> 
        <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
	<rosparam file="$(find arl_description)/launch/nav_data/dwa_local_planner_params.yaml" command="load" />-->

    	<rosparam file="$(find arl_description)/launch/nav_data/costmap_common_params.yaml" command="load" ns="global_costmap" />
     	<rosparam file="$(find arl_description)/launch/nav_data/costmap_common_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find arl_description)/launch/nav_data/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="$(find arl_description)/launch/nav_data/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find arl_description)/launch/nav_data/base_local_planner_params.yaml" command="load" />
<param name="recovery_behavior_enabled" value="false" />
<remap from="cmd_vel" to="cmd_vel"/>
<remap from="odom" to="odom"/>
<!--<remap from="move_base_simple/goal" to="rtabmap/goal"/>-->
<remap from="scan" to="scan"/>
<remap from="map"     to="/grid_map"/>
     </node>


<!--AMCL-->
	<node if="$(arg amcl)"	pkg="amcl"	type="amcl"	name="amcl"	>
	<remap from="map" to="/grid_map"/>
	<!--<remap from="initialpose" to="/localization_pose"/>-->
	<param	name="odom_model_type"	value="diff"	/>
	<param	name="odom_frame_id"	value="odom"	/>
<!--	<param name="global_frame_id" value="odom"/>-->
	<param name="base_frame_id" value="base_link"/>
	<param name="use_map_topic" value="true"/>
	<param name="tf_broadcast" value="false"/>
	
</node>

<arg name="rviz_cfg"                default="$(find arl_description)/launch/realsenseARL.rviz" />
<!--<node if="$(arg amcl)"  pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>-->

</launch>

also, I decrease the controller frequency parameter to 1.

Here it is one more video which describe the jumps of the robot (in localization mode and after a little driving)
https://www.youtube.com/watch?v=IkXk45l28t0

Thank you

[matlabbe]

Hi,

I cannot see how rtabmap_realsense.launch is configured, but the scan is created from the camera depth image, which seems to have a lot of distortion far from the camera. The resulting scan is very noisy, which is likely causing those jumps. Limit the range of the scan generated or explicitly set Reg/Strategy=0 (visual) for rtabmap node (by default if rtabmap is subscribed to a laser scan, it sets Reg/Strategy=1).

cheers,
Mathieu

[aviadoz]

Hi,
Thank you for your reply.
I will test it like you said and will update about the results.

Thanks again,
Aviad

[aviadoz]

Hi Mathieu,

So,I did what you suggested and I can't see any change of these jumps. The "odom" frame jumps a lot.
I reduce the laser range for 3 meter instead 5 meter which was before and of course the Reg param also.
I am putting here the "rtabmap_realsense.launch" that I am using in my main launch file. maybe you can see something which can help. Also, there is no change if I use visual_odometry or my odometry by the encoders.
Do you think that I need to calibrate my realsense camera?should I need to use robot_localization pkg?

<?xml version="1.0"?>
<launch>
  
  <arg name="vo" default="false" />		
  <arg name="visitors" default="false" />
  <arg name="mapping" default="true" />
  <arg if="$(arg mapping)" name="delete_db" value="--delete_db_on_start" />
  <arg unless="$(arg mapping)" name="delete_db" value="" />
     
  <!-- Choose between RGB-D and stereo -->      
  <arg name="stereo"          default="false"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find arl_description)/launch/realsenseARL.rviz" />
  <arg name="rviz_visitors_cfg"                default="$(find arl_description)/launch/visitorsRviz.rviz" />
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default="odom"/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="50"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/color/image_raw" />
  <arg name="depth_topic"             default="/camera/aligned_depth_to_color/image_raw" />
  <arg name="camera_info_topic"       default="/camera/color/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
  
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="scan_normal_k"           default="0"/>
   
  <arg name="visual_odometry"          default="false"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>  
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->



    
    <node  name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen"  args="$(arg delete_db)">
      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
      <param name="gen_scan" type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="false"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="Rtabmap/DetectionRate" type="string" value="0"/>
      <param name="Reg/Strategy" type="string" value="0"/>
      <param name="frame_id" type="string" value="$(arg frame_id)"/>
     <!--  <param name="publish_tf" type="bool" value="$(arg publish_map_tf)" /> -->     
	
        <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="odom"/><!-- to get the odometry from the encoders and not from the kinect-->
 	<remap from="scan" to="/scan"/>
	<remap from="scan_map" to="cloud_map"/>
	<!--<param name="RGBD/OptimizeStrategy" type="string" value="1"/>
	<remap from="grid_map" to="/map"/>-->
	
       

         <!-- RTAB-Map's parameters -->

        <!--
  <param name="RGBD/AngularUpdate" type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate" type="string" value="0.01"/>
	  <param name="RGBD/OptimizeSlam2d"          type="string" value="true"/>
          <param name="queue_size" type="int" value="50"/>
          <param name="RGBD/PoseScanMatching"        type="string" value="true"/>
	  <param name="RGBD/PlanStuckIterations"        type="int" value="10"/>
          <param name="use_action_for_goal" type="bool" value="true"/> 
	  <param name="publish_tf" type="bool" value="true"/>
          <param name="Rtabmap/TimeThr" type="string" value="700"/>
          <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
	  <param name="RGBD/PoseScanMatching" type="string" value="true"/>
	  <param name="RGBD/OptimizeStrategy" type="string" value="1"/> 
	  <param name="RGBD/OptimizeRobust" type="string" value="true"/>
	  <param name="RGBD/OptimizeMaxError" type="string" value="0"/> 
          <param name="RGBD/OptimizeRobust" type="string" value="true"/>
	  <param name="Rtabmap/DetectionRate" type="string" value="0"/>
	  <param name="grid_size" type="double" value="10"/> 
	  <param name="cloud_noise_filtering_radius" value="0.05"/>
	  <param name="cloud_noise_filtering_min_neighbors" value="2"/>
	  <param name="proj_max_ground_angle" value="45"/>
	  <param name="proj_max_ground_height" value="0.1"/>
	  <param name="proj_max_height" value="2.0"/>
          <param name="approx_sync" type="bool" value="true"/>
	  <param name="proj_min_cluster_size" value="20"/>
-->
        </node>
	  
    

 <!-- RGB-D Odometry -->
        <node   if="$(arg vo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" >
          <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="rgbd_image"      to="$(arg rgb_topic)"/>
          <!--<remap from="odom"            to="$(arg odom_topic)"/>-->
      
          <param name="frame_id"                    type="string" value="base_link"/>
          <param name="odom_frame_id"               type="string" value="odom"/>
          <param name="wait_for_transform_duration" type="double" value="0.01"/>
          <param name="approx_sync"                 type="bool"   value="true"/>
          <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
          <param name="subscribe_rgbd"              type="bool"   value="false"/>
    	  <param name="publish_tf" type="bool" value="true"/>

</node>
  



  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg visitors)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_visitors_cfg)"/>


<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="$(arg output)">

	<param name="subscribe_rgbd"       type="bool"   value="false"/>
	<param name="subscribe_scan"       type="bool"   value="true"/>
	<param name="frame_id"             type="string" value="base_link"/>
        <param name="odom_frame_id"        type="string" value="odom"/>
 	<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="/scan"                   to="/scan"/>
       <!-- <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
        <remap from="odom"                   to="$(arg odom_topic)"/>-->
 </node>
</launch>

[matlabbe]

Hi,

You could try robot_localization package to filter localization jumps. Not sure which rtabmap version you are using, but set  to make sure localization is in 2D, then you can try the approach described here.

cheers,
Mathieu