rtabmap issue with loop closure
Hello, I have an rtabmap setup with:
- Stereo camera for mapping with rtab
- Wheel odometry only
In simulation and also real life I am getting incorrect loop closure that seem to mess up a good map. Is there any obvious stuff I'm doing wrong here?
Here is my rtabmap launch file:
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen"
args="--delete_db_on_start">
<remap from="left/image_rect" to="/stereo1/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo1/right/image_rect"/>
<remap from="left/camera_info" to="/stereo1/left/camera_info"/>
<remap from="right/camera_info" to="/stereo1/right/camera_info"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="grid_size" type="double" value="100"/>
<param name="queue_size" type="int" value="30"/>
<!-- all points below 20 cm are ground -->
<param name="Grid/MaxGroundHeight" type="string" value="0.5"/>
<!-- all points above 20 cm and below 2 m are obstacles -->
<!-- <param name="Grid/MaxObstacleHeight" type="string" value="3"/> -->
<!-- Use simple passthrough to filter the ground instead of normals segmentation -->
<!-- <param name="Grid/NormalsSegmentation" type="string" value="false"/> -->
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/>
<!-- kdTree -->
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<!-- 3D->2D (PnP) -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Grid/MaxGroundHeight" type="double" value="0.18"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="Viz/EstimationType" type="int" value="1"/>
<param name="Vis/MaxFeatures" type="int" value="1000"/>
<!-- http://bit.ly/2wdXVtj -->
<!-- Adding a despeckler -->
<param name="Stereo/WinWidth" type="int" value="15" />
<param name="Stereo/WinHeight" type="int" value="3" />
<param name="Stereo/Iterations" type="int" value="30" />
<param name="Stereo/MaxLevel" type="int" value="3" />
<param name="Stereo/MinDisparity" type="int" value="1" />
<param name="Stereo/MaxDisparity" type="int" value="128" />
<param name="Stereo/OpticalFlow" type="bool" value="False" />
<param name="Stereo/SSD" type="bool" value="true" />
<param name="Stereo/Eps" type="double" value="0.01" />
<param name="StereoBM/BlockSize" type="int" value="15" />
<param name="StereoBM/MinDisparity" type="int" value="0" />
<param name="StereoBM/NumDisparities" type="int" value="128" />
<param name="StereoBM/PreFilterSize" type="int" value="9" />
<param name="StereoBM/PreFilterCap" type="int" value="31" />
<param name="StereoBM/UniquenessRatio" type="int" value="15" />
<param name="StereoBM/TextureThreshold" type="int" value="2500" />
<param name="StereoBM/SpeckleWindowSize" type="int" value="1000" />
<param name="StereoBM/SpeckleRange" type="int" value="4" />
A video of the problem: https://youtu.be/tjO-dYugKbw
PS: This might be a simple issue having to do with my simulation cause the ground is very ...