Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

[seanxu112]

Hi,

Sorry to have another problem so soon. Localization with laser and ZEDm camera works great right now. However, it seems like the global map has a lot of salt and pepper noise. I tried to solve it by post processing it with rtabmap-databaseViewer, but it seems like we cannot save the new processed data base after refining links and detect loop closure. I also applied cloud filtering with 0.1meter of 100 points. There is just one single point on top of the map, and move_base is registering it as an obstacle and trying to avoid it. Also, my cloud map topic and cloud obstacles topic seems to be publishing the same thing.

I thought about using mark_threshold in costmap params yaml file. Here is my local and global costmap yaml:

local_costmap:
  global_frame: /map
  robot_base_frame: /base_link
  update_frequency: 2.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

global_costmap:
  global_frame: /map
  robot_base_frame: /base_link
  update_frequency: 2.0
  static_map: true

costmap_common_params:
obstacle_range: 2.5
raytrace_range: 3.0
max_obstacle_height: 1.0
footprint: [[0.36, 0.29], [0.36, -0.29], [-0.31, -0.29], [-0.31, 0.29]]
#robot_radius: ir_of_robot
inflation_radius: 0.05

mark_threshold: 10

transform_tolerance: 0.5
observation_sources: laser_scan_sensor point_cloud_sensor #laser_scan_sensor point_cloud_sensor

laser_scan_sensor: {sensor_frame: laser_frame, data_type: LaserScan, topic: /scan, marking: true, clearing: true}

point_cloud_sensor: {sensor_frame: zed_camera_center, data_type: PointCloud2, topic: /rtabmap/cloud_map, marking: true, clearing: true}


Is there any way to solve this with move_base or post-processing the database?

Sincerely,

Sean

[matlabbe]

Hi,

There are two approaches to filter that noise.

1) Adjusting "Grid/" parameters and regenerate the local grids

In rtabmap-databaseViewer, show the "Core Parameters" and "3D view" panels. Click on "Grid" checkbox in 3D view. This will show the local grid used to generate the map for the frame selected (with the main sliders). Under Core Parameters panel, select Grid sub section. To filter pepper noise obstacles, tune NoiseFilteringRadius and NoiseFilteringMinNeighbors. Click on "Edit->Regenerate local grid maps (selected)". The local grid will be regenerated. If you are satisfied, you can do "Edit->Regenerate local grid maps" to regenerate all local grids with those parameters. This will regenerate the global map as well. You can also write those parameters to your launch file to be used in next sessions.

2) If you restart in Localization mode, edit directly the global map

In rtabmap-databaseViewer, do "File -> Edit optimized map...". See right-click menu to add/remove obstacles. On close the map is saved back to database. When reloading the map in Localization mode (Mem/IncrementalMemory=false), that modified map should be used. If you start in SLAM mode, the map will be regenerated, removing the changes made (in this case, it is preferred to use approach 1 above). Rtabmap >=0.19 required for this approach.

cheers,
Mathieu

[seanxu112]

Hi Mathieu,

Thank you for the response. For the first solution, since we are using the laser sensor, When I tried to filter the grid map it seems like it is only editing the laser data. And it does not seem to change a lot.

Currently I am using 0.17 is there anyway to upgrade rtabmap-databaseViewer?

Also I am reading your tutorial from 2016, specifically this one: http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation

Should I still follow this tutorial?
If so, since I am not using openni, I set local_costmap_param to subscribe to topic: /rtabmap/cloud_obstacles, but it seems like the local costmap is not publishing anything anymore.

Thank you for any response,

Sincerely,
Sean

[matlabbe]

Hi Sean,

I don't know what is your configuration (launch fiels, robot, sensors...), but if you have a lidar but you want the map created from the camera, set Grid/FromDepth to true.

From that tutorial, you should not subscribe to /rtabmap/cloud_obstacles. The local costmap in the tutorial is connected to output of obstacles_detection nodelet (obstacles derived directly from the camera). The local costmap should not subscribe to any topic from rtabmap node (like in that tutorial).

cheers,
Mathieu

[seanxu112]

Hi Mathieu,

Thank you for the quick response. Do we need to use openni for the tutorial? Is the topic "cloudXYZ" an openni subscribed topic?

Since we are using ZED camera, we do not have disparity/camera_info, should we just use depth/camera_info?

On a different topic, since I am using a custom robot, the robot is not rotating w.r.t its center of mass. Does the navigation stack make the assumption that the robot rotate w.r.t the base_footprint frame? I understand this is a rtabmap forum, so it is perfectly fine if you do not answer this question.

Thank you for answering all my questions.

Sincerely,
Sean

[matlabbe]

point_cloud_xyz can use depth image as input. The zed camera should provide a depth image, thus use the corresponding camera_info. See http://wiki.ros.org/rtabmap_ros#rtabmap_ros.2BAC8-point_cloud_xyz for more info.

cloudXYZ is a topic created from point_cloud_xyz  nodelet.

Example for zed:

     <!-- Generate a point cloud from the depth image -->
      <node pkg="nodelet" type="nodelet" name="depth2cloud" args="standalone rtabmap_ros/point_cloud_xyz">
         <remap from="depth/image"       to="/zed/zed_node/depth/depth_registered"/>
         <remap from="depth/camera_info" to="/zed/zed_node/rgb/camera_info"/>
         <remap from="cloud"                to="/voxel_cloud"/>
         
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="4"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

      <!-- Create point cloud for the local planner -->
      <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="standalone rtabmap_ros/obstacles_detection">
         <remap from="cloud" to="/voxel_cloud"/>
         <remap from="obstacles" to="/obstacles"/>

         <param name="frame_id" type="string" value="base_footprint"/>
         <param name="map_frame_id" type="string" value="map"/>
         <param name="min_cluster_size" type="int" value="20"/>
         <param name="max_obstacles_height" type="double" value="0.0"/>
       </node>

The local costmap config:

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 2.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.025
  origin_x: -2.0
  origin_y: -2.0

  observation_sources: point_cloud_sensor

  # assuming receiving a cloud from rtabmap_ros/obstacles_detection node
  point_cloud_sensor: {
    sensor_frame: base_footprint,
    data_type: PointCloud2, 
    topic: /obstacles, 
    expected_update_rate: 0.5, 
    marking: true, 
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 99999.0}

cheers,
Mathieu

[matlabbe]

If your TF are correctly set, navigation/map should be able to correctly transform data in the correct frame.

[seanxu112]

When I added rtabmap point_cloud_xyz and obstacle detection by nodelet, the error in my other post appeared again:
OccupancyGrid.cpp:357::createLocalMap() Cannot create local map, scan is empty (node=668).

Is there a potential problem? I am running move_base in another launch file, and the local costmap is published by move_base.

[matlabbe]

Which node is telling "OccupancyGrid.cpp:357::createLocalMap() Cannot create local map, scan is empty"? Is it obstacles_detection or rtabmap? You can disable the launch of rtabmap temporary to see if it is coming from obstacles_detection. Does voxel_cloud topic from point_cloud_xyz look good in RVIZ?

[seanxu112]

Voxel_cloud topic from point_cloud_xyz both look good but when I commented out point_cloud_sensor in local_costmap_params.yaml and depth2cloud and obstacles_detection in rtabmapwithlidar.launch I still got that error.

Thank you!

[matlabbe]

Hi,

You could check if the output topic of obstacles_detection look good too. If so, the local costmap should work.

The error seems coming from rtabmap node. Can you share (google drive/dropbox link) the resulting ~/.ros/rtabmap.db file?

cheers,
Mathieu

[seanxu112]

Hi Mathieu,

It turns out that I set the subscribe_rgbd to false during localization since it seems to be giving better result. I think I didn't understand your answer in the other post that well. Thank you so much for your help.

Sincerely,
Sean

[qzho]

Hi Mathieu,

I am trying to remove a garbage bin in the map, which is no long at its original place. I tried the approach (2). I was able to do "File ->Edit the optimized 2D map" --> right click to "remove obstacle" --> save map. However, the 2D map was not updated in the Graph view. In Localization mode later, the obstacle is still there. Is there anything that I missed?

Thank you,
Qian

[matlabbe]

Hi Qian,

The graph view shows always the map reconstructed from scratch. To see the difference, open RVIZ and add Map display on the output /rtabma/grid_map. It should have your changes.

cheers,
Mathieu

[qzho]

Hi Mathieu,

Thanks for your response! After I cleared an obstacle and saved changes in databaseViewer and re-open the database to check the 2D map, I can see an updated map with no obstacle. However, the obstacle was still there in /rtabmap/grid_map in RVIZ. And if I then closes the RVIZ and reopen the 2D map in databaseViewer, the removed obstacle was added back (I set "Mem/IncrementalMemory" to false). I am thinking if it were because the database contains point cloud and it will project all the obstacle to the grid map. I recorded a video to show the problem here.

Sorry for this specific question. Thank you very much for the help!

Qian

[matlabbe]

Hi,

I opened your database, edited it like in your video, then re-opened in rviz in localization mode:

$ mv ~/Downloads/rtabmap.db ~/.ros/rtabmap.db
$ roslaunch rtabmap_ros rtabmap.launch localization:=true rviz:=true
$ rosservice call /rtabmap/publish_map 1 1 0

It seems ok. However, from the video, it seems that the map is reconstructed at each iteration when receiving new data. What parameters are you using in your launch file for rtabmap node?

I tried to reproduce the problem with demo_mapping.bag from here:
1) Mapping the first 30 sec of the bag:

roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
rosbag play --clock demo_mapping.bag

2) Edit the map in rtabmap-databaseViewer (File->Edit optimized 2D map...)


3) Relaunch the demo launch file in localization mode:

$ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false localization:=true
$ rosbag play --clock demo_mapping.bag

[qzho]

Thank you! I will look into the demo files. I uploaded the .launch file I used here. Really appreciate all the help!

[matlabbe]

Hi,

It is because memory management is enabled, set Rtabmap/TimeThr to 0. When memory management is enabled, if there are nodes transferred to or retrieved from Long-Term Memory (LTM), we have to reconstruct the grid.

cheers,
Mathieu

[qzho]

Mathieu, thank you very much!