search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
1k stars 559 forks source link

distorted Point cloud #607

Open Haydar1990 opened 3 years ago

Haydar1990 commented 3 years ago

Hello every one, I'm trying to build a 3d map using Gazebo & RTAB-Map it works but in rviz the point clouds looks disrtorted, would be nice if someone can help me my Launch File:

<launch>
  <arg name="database_path" default="~/Documents/RTAB-Map/rtabmap.db"/>
  <arg name="localization" default="false"/>
  <arg name="args" default=""/>

  <arg name="wait_for_transform" default="0.2"/>

  <!-- Navigation stuff (move_base)-->
  <!--include file="$(find mir_navigation)/launch/mir_nav/move_base.xml"/-->

  <!-- Mapping -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
          <param name="database_path"  type="string" value="$(arg database_path)"/>
          <param name="frame_id"       type="string" value="base_footprint"/>
          <param name="odom_frame_id"       type="string" value="odom_comb"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="subscribe_depth"     type="bool"   value="true"/>
          <param name="subscribe_scan"      type="bool"   value="true"/>

          <!-- inputs -->
          <remap from="scan"            to="/scan"/>
          <remap from="rgb/image"       to="/camera/color/image_raw"/>
          <remap from="depth/image"     to="/camera/depth/image_raw"/>
          <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

          <!-- output -->
          <remap from="grid_map" to="/map"/>

          <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available 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/MaxDepth"                  type="string" value="4.0"/>
          <param name="Reg/Strategy"                 type="string" value="1"/>      <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
          <param name="Icp/CorrespondenceRatio"      type="string" value="0.4"/>
          <param name="Vis/MinInliers"               type="string" value="6"/>      <!-- 3D visual words minimum inliers to accept loop closure -->
          <param name="Vis/InlierDistance"           type="string" value="0.1"/>    <!-- 3D visual words correspondence distance -->
          <param name="RGBD/AngularUpdate"           type="string" value="0.01"/>    <!-- Update map only if the robot is moving -->
          <param name="RGBD/LinearUpdate"            type="string" value="0.01"/>    <!-- Update map only if the robot is moving -->
          <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"/>
      <param name="cloud_noise_filtering_radius" value="0.05"/>
      <param name="cloud_noise_filtering_min_neighbors" value="2"/>
      <param name="RGBD/OptimizeMaxError"          type="string" value="3"/>      
      <param name="RGBD/NeighborLinkRefining"      type="string" value="true"/>

          <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
          <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
    </node>
  </group>
</launch>

The Point Cloud in Rviz:

Screenshot from 2021-07-12 15-24-01

SUMMARY

PARAMETERS

NODES /rtabmap/ rtabmap (rtabmap_ros/rtabmap)

ROS_MASTER_URI=http://localhost:11311

process[rtabmap/rtabmap-1]: started with pid [23739] [ INFO] [1626095761.415170281]: Starting node... [ INFO] [1626095761.461916189]: Initializing nodelet with 4 worker threads. [ INFO] [1626095761.617569139]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000 [ INFO] [1626095761.617605750]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000 [ INFO] [1626095761.617620264]: /rtabmap/rtabmap(maps): map_cleanup = true [ INFO] [1626095761.617635355]: /rtabmap/rtabmap(maps): map_always_update = false [ INFO] [1626095761.617643055]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true [ INFO] [1626095761.617651281]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true [ INFO] [1626095761.617661619]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false [ INFO] [1626095761.617672611]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [ INFO] [1626095761.618054229]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16 [ INFO] [1626095761.647192756]: rtabmap: frame_id = base_footprint [ INFO] [1626095761.648201488]: rtabmap: odom_frame_id = odom_comb [ INFO] [1626095761.648279594]: rtabmap: map_frame_id = map [ INFO] [1626095761.648332506]: rtabmap: use_action_for_goal = false [ INFO] [1626095761.648386178]: rtabmap: tf_delay = 0.050000 [ INFO] [1626095761.648586468]: rtabmap: tf_tolerance = 0.100000 [ INFO] [1626095761.648821633]: rtabmap: odom_sensor_sync = false [ INFO] [1626095761.649282014]: rtabmap: gen_scan = false [ INFO] [1626095761.649361833]: rtabmap: gen_depth = false [ INFO] [1626095761.768974635, 50.660000000]: Setting RTAB-Map parameter "Icp/CorrespondenceRatio"="0.4" [ INFO] [1626095761.818548330, 50.684000000]: Setting RTAB-Map parameter "Kp/MaxDepth"="4.0" [ INFO] [1626095761.859099633, 50.701000000]: Setting RTAB-Map parameter "Mem/RehearsalSimilarity"="0.30" [ INFO] [1626095761.887043589, 50.715000000]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.01" [ INFO] [1626095761.891659244, 50.719000000]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.01" [ INFO] [1626095761.901920639, 50.726000000]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="true" [ INFO] [1626095761.903039631, 50.727000000]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false" [ INFO] [1626095761.903277753, 50.727000000]: Setting RTAB-Map parameter "RGBD/OptimizeMaxError"="3" [ INFO] [1626095761.906989560, 50.730000000]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true" [ INFO] [1626095761.917320723, 50.737000000]: Setting RTAB-Map parameter "Reg/Force3DoF"="true" [ INFO] [1626095761.918693110, 50.738000000]: Setting RTAB-Map parameter "Reg/Strategy"="1" [ INFO] [1626095761.950807442, 50.755000000]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="700" [ INFO] [1626095762.044416429, 50.810000000]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1" [ INFO] [1626095762.055144033, 50.815000000]: Setting RTAB-Map parameter "Vis/MinInliers"="6" [ WARN] [1626095762.175426362, 50.890000000]: Rtabmap: Parameter name changed: "Optimizer/Slam2D" -> "Reg/Force3DoF". Please update your launch file accordingly. Value "true" is still set to the new parameter name. [ WARN] [1626095762.219828808, 50.915000000]: Parameter "cloud_noise_filtering_radius" has moved from rtabmap_ros to rtabmap library. Use parameter "Grid/NoiseFilteringRadius" instead. The value is still copied to new parameter name. [ WARN] [1626095762.220812796, 50.916000000]: Parameter "cloud_noise_filtering_min_neighbors" has moved from rtabmap_ros to rtabmap library. Use parameter "Grid/NoiseFilteringMinNeighbors" instead. The value is still copied to new parameter name. [ WARN] [1626095762.226103525, 50.920000000]: Setting "Grid/FromDepth" parameter to false (default true) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true. The occupancy grid map will be constructed from laser scans. To get occupancy grid map from cloud projection, set "Grid/FromDepth" to true. To suppress this warning, add [ INFO] [1626095762.226237459, 50.920000000]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true and Grid/FromDepth is false. [ WARN] [1626095762.226338532, 50.920000000]: Setting "RGBD/ProximityPathMaxNeighbors" parameter to 10 (default 0) as "subscribe_scan" is true and "Reg/Strategy" uses ICP. Proximity detection by space will be also done by merging close scans. To disable, set "RGBD/ProximityPathMaxNeighbors" to 0. To suppress this warning, add [ INFO] [1626095762.228641987, 50.922000000]: Update RTAB-Map parameter "Optimizer/Epsilon"="0.00001" from database [ INFO] [1626095762.228810023, 50.922000000]: Update RTAB-Map parameter "Optimizer/GravitySigma"="0.0" from database [ INFO] [1626095762.228849413, 50.922000000]: Update RTAB-Map parameter "Optimizer/Iterations"="100" from database [ INFO] [1626095762.228880225, 50.922000000]: Update RTAB-Map parameter "Optimizer/Strategy"="0" from database [ INFO] [1626095762.228953908, 50.922000000]: Update RTAB-Map parameter "Vis/BundleAdjustment"="0" from database [ INFO] [1626095762.228980778, 50.922000000]: Update RTAB-Map parameter "Vis/PnPRefineIterations"="1" from database [ INFO] [1626095762.229376955, 50.922000000]: RTAB-Map detection rate = 1.000000 Hz [ INFO] [1626095762.229486102, 50.923000000]: rtabmap: Using database from "/home/rosmatch/Documents/RTAB-Map/rtabmap.db" (2366 MB). [ INFO] [1626095768.107523168, 53.804000000]: rtabmap: Working Memory = 2421, Local map = 0. [ INFO] [1626095768.107820546, 53.804000000]: rtabmap: Database version = "0.20.11". [ INFO] [1626095768.107914978, 53.804000000]: rtabmap: SLAM mode (Mem/IncrementalMemory=true) [ INFO] [1626095768.146269392, 53.817000000]: /rtabmap/rtabmap: subscribe_depth = true [ INFO] [1626095768.146513792, 53.817000000]: /rtabmap/rtabmap: subscribe_rgb = true [ INFO] [1626095768.146585356, 53.817000000]: /rtabmap/rtabmap: subscribe_stereo = false [ INFO] [1626095768.146601516, 53.817000000]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1) [ INFO] [1626095768.146616085, 53.817000000]: /rtabmap/rtabmap: subscribe_odom_info = false [ INFO] [1626095768.146628954, 53.817000000]: /rtabmap/rtabmap: subscribe_user_data = false [ INFO] [1626095768.146643959, 53.817000000]: /rtabmap/rtabmap: subscribe_scan = true [ INFO] [1626095768.146659224, 53.817000000]: /rtabmap/rtabmap: subscribe_scan_cloud = false [ INFO] [1626095768.146672146, 53.817000000]: /rtabmap/rtabmap: subscribe_scan_descriptor = false [ INFO] [1626095768.146685446, 53.817000000]: /rtabmap/rtabmap: queue_size = 10 [ INFO] [1626095768.146699227, 53.817000000]: /rtabmap/rtabmap: approx_sync = true [ INFO] [1626095768.146858366, 53.817000000]: Setup depth callback [ INFO] [1626095768.194924105, 53.845000000]: /rtabmap/rtabmap subscribed to (approx sync): /camera/color/image_raw \ /camera/depth/image_raw \ /camera/color/camera_info \ /scan [ INFO] [1626095768.344755375, 53.930000000]: rtabmap 0.20.13 started... [ WARN] (2021-07-12 15:16:08.980) Rtabmap.cpp:3166::process() Rejecting all added loop closures (1, first is 9685 <-> 9505) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.399891 (edge 8591->9281, type=2, abs error=0.035352 m, stddev=0.010398). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:08.981) Rtabmap.cpp:3200::process() Loop closure 9685->9505 rejected!

[ WARN] (2021-07-12 15:16:10.247) Rtabmap.cpp:3166::process() Rejecting all added loop closures (1, first is 9686 <-> 8718) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.261691 (edge 8591->9281, type=2, abs error=0.033915 m, stddev=0.010398). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:10.247) Rtabmap.cpp:3200::process() Loop closure 9686->8718 rejected!

[ WARN] (2021-07-12 15:16:11.826) Rtabmap.cpp:3166::process() Rejecting all added loop closures (1, first is 9687 <-> 8725) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.342206 (edge 8591->9281, type=2, abs error=0.034752 m, stddev=0.010398). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:11.826) Rtabmap.cpp:3200::process() Loop closure 9687->8725 rejected!

[ WARN] [1626095773.287864231, 57.281000000]: Many occupancy grids should be loaded (~304), this may take a while to update the map(s)... [ WARN] [1626095774.827647678, 58.134000000]: Map(s) updated! (1.539849 s)

[ WARN] (2021-07-12 15:16:15.608) Rtabmap.cpp:3166::process() Rejecting all added loop closures (3, first is 9689 <-> 9600) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 4.129570 (edge 9277->9688, type=1, abs error=0.038164 m, stddev=0.009242). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:15.608) Rtabmap.cpp:3200::process() Loop closure 9689->9600 rejected! [ WARN] (2021-07-12 15:16:15.608) Rtabmap.cpp:3200::process() Loop closure 9689->9316 rejected! [ WARN] (2021-07-12 15:16:15.608) Rtabmap.cpp:3200::process() Loop closure 9689->8718 rejected!

[ WARN] (2021-07-12 15:16:17.260) Rtabmap.cpp:3166::process() Rejecting all added loop closures (3, first is 9690 <-> 9278) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.884472 (edge 8591->9281, type=2, abs error=0.040390 m, stddev=0.010398). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:17.260) Rtabmap.cpp:3190::process() Rejecting all added loop closures (3, first is 9690 <-> 9278) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.943795 (edge 8728->9690, type=2, abs error=0.537563 deg, stddev=0.002379). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:17.260) Rtabmap.cpp:3200::process() Loop closure 9690->9278 rejected! [ WARN] (2021-07-12 15:16:17.260) Rtabmap.cpp:3200::process() Loop closure 9690->8728 rejected! [ WARN] (2021-07-12 15:16:17.260) Rtabmap.cpp:3200::process() Loop closure 9690->9506 rejected!

[ WARN] (2021-07-12 15:16:18.994) Rtabmap.cpp:3166::process() Rejecting all added loop closures (3, first is 9691 <-> 9316) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 5.736555 (edge 8723->9280, type=2, abs error=0.063425 m, stddev=0.011056). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:18.994) Rtabmap.cpp:3200::process() Loop closure 9691->9316 rejected! [ WARN] (2021-07-12 15:16:18.994) Rtabmap.cpp:3200::process() Loop closure 9691->9503 rejected! [ WARN] (2021-07-12 15:16:18.994) Rtabmap.cpp:3200::process() Loop closure 9691->8725 rejected!

[ WARN] [1626095779.713280117, 61.243000000]: Messages of type 3 arrived out of order (will print only once) [ WARN] (2021-07-12 15:16:20.667) Rtabmap.cpp:3166::process() Rejecting all added loop closures (3, first is 9692 <-> 9589) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 4.024592 (edge 8591->9281, type=2, abs error=0.041847 m, stddev=0.010398). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:20.667) Rtabmap.cpp:3190::process() Rejecting all added loop closures (3, first is 9692 <-> 9589) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.550694 (edge 8725->9283, type=2, abs error=0.562417 deg, stddev=0.002765). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2021-07-12 15:16:20.667) Rtabmap.cpp:3200::process() Loop closure 9692->9589 rejected! [ WARN] (2021-07-12 15:16:20.667) Rtabmap.cpp:3200::process() Loop closure 9692->9280 rejected! [ WARN] (2021-07-12 15:16:20.667) Rtabmap.cpp:3200::process() Loop closure 9692->8718 rejected!

Thank you in advance

matlabbe commented 3 years ago 
[ INFO] [1626095762.229486102, 50.923000000]: rtabmap: Using database from "/home/rosmatch/Documents/RTAB-Map/rtabmap.db" (2366 MB).

You may want to add:

<arg if="$(arg localization)"name="args" default=""/>
<arg unless="$(arg localization)"name="args" default="-d"/> <!-- delete previous database  on start-->

Then for the drift, it seems coming from your odometry odom_comb:

<param name="odom_frame_id"       type="string" value="odom_comb"/>