Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

classic Classic list List threaded Threaded
12 messages Options
Reply | Threaded
Open this post in threaded view
|

Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

seanxu112
This post was updated on .
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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
If your TF are correctly set, navigation/map should be able to correctly transform data in the correct frame.
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

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.
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
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?
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

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!
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

matlabbe
Administrator
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
Reply | Threaded
Open this post in threaded view
|

Re: Noisy Global Map using ZEDm causing obstacle avoidance navigation problem

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