rtabmap mapping mapping publish strange tf

[harryhe]

Hi there, I was trying to simulate rtab with gazebo simulation, this works with simple environment. When I tried on some other 'real' gazebo world, the rtab will sometimes publish strange tf which make all maps messed up. the odom from gazebo is fused with imu so the odom to baselink has barely any drift. IDK what is the reason for the tf error. I suppose it about loop closure issues, wish someone could help me figure it out. The pictures and launch code are shown below.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160

<?xml version="1.0"?> <launch> <!-- Backward compatibility launch file, use rtabmap.launch instead --> <!-- Your RGB-D sensor should be already started with "depth_registration:=true". Examples: $ roslaunch freenect_launch freenect.launch depth_registration:=true $ roslaunch openni2_launch openni2.launch depth_registration:=true --> <!-- <include file="$(find floam)/launch/floam_velodyne.launch"/> --> <!-- choose visualization --> <arg name="rviz" default="true" /> <arg name="rtabmapviz" default="false" /> <!-- Localization-only mode --> <arg name="localization" default="false"/> <!-- <arg if="$(arg localization)" name="args" default=""/> --> <!-- <arg unless="$(arg localization)" name="args" default="-delete_db_on_start"/> --> <!-- Corresponding config files --> <arg name="rtabmapviz_cfg" default="~/.ros/rtabmap_gui.ini" /> <arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/turtlebot_navigation.rviz" /> <arg name="args" default="--delete_db_on_start" /> <arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <arg name="database_path" default="/home/open/rtabmap_zed.db"/> <!--switch to other db if needed e.g. rtabmap_zed.db--> <arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes --> <arg name="approx_sync" default="false"/> <!-- if timestamps of the input topics are not synchronized --> <arg name="rgb_topic" default="/zed/rgb/image_raw" /> <arg name="depth_registered_topic" default="/zed/depth/image_raw" /> <arg name="camera_info_topic" default="/zed/rgb/camera_info" /> <arg name="compressed" default="false"/> <arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF --> <arg name="scan_topic" default="/lms511_scan"/> <arg name="subscribe_scan_cloud" default="false"/> <!-- Assuming 3D scan if set --> <arg name="scan_cloud_topic" default="/velodyne_points"/> <arg name="queue_size" default="30"/> <arg name="visual_odometry" default="false"/> <!-- Generate visual odometry --> <!-- Odometry topic used if visual_odometry is false --> <arg name="odom_topic" default="odom_combined"/> <!-- If set, TF is used to get odometry instead of the topic --> <arg name="odom_frame_id" default="odom_combined"/> <arg name="namespace" default="rtabmap"/> <arg name="wait_for_transform" default="0.2"/> <remap from="/rtabmap/grid_map" to="/map"/> <include file="$(find rtabmap_ros)/launch/rtabmap.launch"> <arg name="rtabmapviz" value="$(arg rtabmapviz)" /> <arg name="rviz" value="$(arg rviz)" /> <arg name="localization" value="$(arg localization)"/> <arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" /> <arg name="rviz_cfg" value="$(arg rviz_cfg)" /> <arg name="frame_id" value="$(arg frame_id)"/> <arg name="namespace" value="$(arg namespace)"/> <arg name="database_path" value="$(arg database_path)"/> <arg name="wait_for_transform" value="$(arg wait_for_transform)"/> <arg name="rtabmap_args" value="$(arg rtabmap_args)"/> <arg name="launch_prefix" value="$(arg launch_prefix)"/> <arg name="approx_sync" value="$(arg approx_sync)"/> <arg name="rgb_topic" value="$(arg rgb_topic)" /> <arg name="depth_topic" value="$(arg depth_registered_topic)" /> <arg name="camera_info_topic" value="$(arg camera_info_topic)" /> <arg name="compressed" value="$(arg compressed)"/> <arg name="subscribe_scan" value="$(arg subscribe_scan)"/> <arg name="scan_topic" value="$(arg scan_topic)"/> <arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/> <arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/> <arg name="queue_size" value="$(arg queue_size)"/> <arg name="visual_odometry" value="$(arg visual_odometry)"/> <arg name="odom_topic" value="$(arg odom_topic)"/> <arg name="odom_frame_id" value="$(arg odom_frame_id)"/> <arg name="odom_args" value="$(arg rtabmap_args)"/> <group ns="rtabmap" > <remap from="move_base" to="/move_base"/> <remap from="grid_map" to="/map"/> <param name="use_action_for_goal" type="bool" value="false"/> <param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/> <param name="subscribe_stereo" type="bool" value="$(arg subscribe_stereo)"/> <param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/> <param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="database_path" type="string" value="$(arg database_path)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="config_path" type="string" value=""/> <param name="queue_size" type="int" value="30"/> <remap from="rgb/image" to="$(arg rgb_topic)"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/> <remap from="scan" to="$(arg scan_topic)"/> <remap from="scan_cloud" to="$(arg scan_cloud_topic)"/> <remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/> <!-- 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)"/> <!-- RTAB-Map's parameters --> <param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM --> <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom --> <param name="Kp/DetectorStrategy" type="string" value="2"/> <param name="Kp/MaxDepth" type="string" value="0.0"/> <!--10--> <param name="Kp/MaxFeatures" type="string" value="0"/> <!--1000--> <param name="SURF/HessianThreshold" type="string" value="150"/> <param name="Kp/BadSignRatio" type="string" value="0.3"/> <param name="Grid/FootprintLength" type="string" value="1.5"/> <!-- footprint length used to filter points over the footprint of the robot --> <param name="Grid/FootprintWidth" type="string" value="0.8"/> <!-- Footprint width used to filter points over the footprint of the robot --> <param name="Grid/FootprintHeight" type="string" value="1.5"/> <!-- Footprint height used to filter points over the footprint of the robot. --> <param name="Grid/CellSize" type="string" value="0.05"/> <!-- Resolution of the occupancy grid. --> <param name="Grid/ScanDecimation" type="int" value="1"/> <!-- Decimation of the laser scan before creating cloud --> <param name="Grid/MaxObstacleHeight" type="string" value="1.5"/> <!-- Maximum obstacles height (0=disabled) --> <param name="Grid/MinGroundHeight" type="string" value="-0.15"/> <!-- Minimum ground height (0=disabled) --> <param name="Grid/MaxGroundHeight" type="string" value="0.15"/> <!-- Maximum ground height (0=disabled) --> <!-- Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles. --> <param name="Grid/MaxGroundAngle" type="string" value="45"/> <param name="Grid/GroundIsObstacle" type="bool" value="false"/> <param name="Grid/RayTracing " type="bool" value="true"/> <!-- <param name="Grid/NoiseFilteringRadius" type="string" value="0.1"/> --> <param name="GridGlobal/Eroded" type="string" value="true"/> <!-- Erode obstacle cells --> <param name="Grid/FromDepth" type="string" value="false"/> <param name="Rtabmap/ComputeRMSE" type="string" value="false"/> <param name="Mem/RawDescriptorsKept" type="string" value="false"/> <!-- Kp, MaxDepth, --> <!-- 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="Icp/CorrespondenceRatio" type="string" value="0.3"/> <param name="Vis/MinInliers" type="string" value="5"/> <!--#15 3D visual words minimum inliers to accept loop closure 100--> <param name="Vis/InlierDistance" type="string" value="0.05"/> <!--0.1 3D visual words correspondence distance --> <param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving --> <param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving --> <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/> <param name="RGBD/OptimizeMaxError" type="string" value="0"/> <!--increase to accepet larger loop closure error 0->accept all--> <param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Mem/RehearsalSimilarity" type="string" value="0.30"/> <param name="Optimizer/Slam2D" type="string" value="true"/> <param name="Reg/Force3DoF" type="string" value="true"/> <!-- change to false --> <param name="Optimizer/Strategy" type="string" value="0"/> <!--Optimization approach used: 0=TORO, 1=g2o and 2=GTSAM--> <param name="Kp/NndrRatio" type="string" value="0.8"/> <!--Added/Modified by Sihan.--> <!-- <param name="Odom/Strategy" type="string" value="1"/> Frame-to-Frame odometry --> <!-- <param name="Odom/GuessMotion" type="string" value="true" /> --> <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP --> <param name="Reg/Strategy" type="string" value="1"/> </group> </include> </launch>

[matlabbe]

What is the twist covariance set in odom_combined ? With repetitive textures, rtabmap is likely to find wrong loop closures. The only way for rtabmap to reject those loop closures is to have a covariance in odometry not over estimated.

Note that with

<!-- If set, TF is used to get odometry instead of the topic -->         
<arg name="odom_frame_id"           default="odom_combined"/>

You should set

<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 -->

to match your odometry covariance. With wheel odometry, you could set them as:

<arg name="odom_tf_angular_variance" default="0.005"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance"  default="0.001"/>             <!-- If TF is used to get odometry, this is the default linear variance -->

Enable graph check rejection (this can detect wrong loop closures):

<param name="RGBD/OptimizeMaxError"           type="string" value="3"/>

cheers,
Mathieu

[harryhe]

Thanks man, that helps a lot. While it still sometimes messes up but for a simple environment it is good.