How to prevent previously-seen map regions from updating?

[olamarre]

Hi! We have a ground robot equipped with a stereo camera pair and are generating a point cloud which is further converted into an elevation map. The point cloud and odometry messages of our robot are both published at 10 Hz.

As we drive our robot, the elevation map is successfully generated and grown from the point cloud messages. However, when the entire map updates (including regions outside the current field of view of the robot), previously-seen obstacles disappear and are replaced by a more or less smooth map. And sometimes, they temporally re-appear.

As an example, the following snapshots illustrate this issue. The current field of view of the robot is overlaid with the current point cloud, while older regions of the map (towards the bottom of the images) keep being updated even if no points from the point cloud are published in these locations. A previously obstacle (circled in red) keeps on disappearing and reappearing:

We would like to keep previously-seen obstacles intact so that our path planner does not find a trajectory that goes through them as soon as they fall outside of the robot's field of view. How can this be obtained?

This is our current elevation_mapping configuration:

# Map parameters
length_in_x: 10.0
length_in_y: 10.0
resolution: 0.1
min_variance: 0.1 #0.00001
max_variance: 10
mahalanobis_distance_threshold: 2.5
multi_height_noise: 0.0000001

# Robot parameters
point_cloud_topic: /navcam/points/color
robot_pose_with_covariance_topic: /ekf/pose_with_covariance
sensor_frame_id: NCAML
map_frame_id: map
robot_base_frame_id: base_link

robot_pose_cache_size: 10000
track_point_frame_id: base_link
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

scanning_duration: 0.1
min_update_rate: 2.0
fused_map_publishing_rate: 2.0
relocate_rate: 3.0
enable_visibility_cleanup: true
visibility_cleanup_rate: 0.1
time_tolerance: 0.5

and our sensor_processor parameters:

type: stereo
p_1:   0.03287
p_2:  -0.0001276
p_3:   0.4850
p_4: 399.1046
p_5:   0.000006735
lateral_factor: 0.001376915
depth_to_disparity_factor: 47.3

ignore_points_above: 0.5
ignore_points_below: -1.0

After varying every parameter documented on the README, we suspect that this issue is related to the map and sensor variances tuning, but we're unsure how to go about those. Also, the max_variance parameter is currently very high for debugging purposes (a small one will erase every map region not in the current field of view of the robot).

Any help would be appreciated. Thanks!

[maximilianwulf]

Hi @olamarre, I also suspect that the map fusion procedure introduces the behavior. Do you see the same in the elevation_map_raw topic? The raw topic should really just reflect the point cloud as-is.

Another thing that you could check is the visibility cleanup, but whereas it should only update visible areas.

If you want you can share a rosbag and we have a look at it.

[olamarre]

Thanks for the reply @maximilianwulf! The elevation_map_raw topic looks pretty good indeed, and keeps the obstacles previously seen. Our localization and point clouds are reasonably accurate, so that might have contributed to it. But the raw one is definitely more noisy. Here's a comparison of both:

We played quite a bit with the visibility cleanup (enabled it, disabled it, changed the rate), but this did not prevent the obstacles outside of the current field of view from being smoothed out. I feel like the smoothing out really comes from a growing/accumulating uncertainty in the system. Perhaps the disappearing and re-appearing of the obstacles behind the robot happens when the uncertainty becomes so large that the outcome of the fusion is unpredictable? According to my understanding of elevation_mapping, the uncertainty of the map behind the robot should increase with distance, influenced by both the sensor noise model and the robot pose covariance, right?

We're working with our HR department to get a rosbag released, which hopefully could help you better see our situation and ultimately also help the community. Thanks again and we'll keep you in the loop.

[maximilianwulf]

According to my understanding of elevation_mapping, the uncertainty of the map behind the robot should increase with distance, influenced by both the sensor noise model and the robot pose covariance, right?

The uncertainty of the height values that are not seen again is only affected by the uncertainty of the input pose. Did you have a look at the covariance of your input pose? For a more detailed description, I can really recommend the original thesis of P. Fankhauser, chapter 3.2 covers the details of the elevation mapping and uncertainty propagation.

If you only care about a smoother map based on the raw elevation map, you could smooth it with a gaussian filter, which is available in the grid_map package. What I can also recommend is to convert it to a OpenCV Mat object and use filters from computer vision. The conversion is also available in the grid_map package.

We're working with our HR department to get a rosbag released, which hopefully could help you better see our situation and ultimately also help the community. Thanks again and we'll keep you in the loop.

Sounds great.

[olamarre]

Perfect, I'll keep you posted! Thanks a lot.

[olamarre]

@maximilianwulf is there a specific branch I should look at in the grid_map_filters package? I can't seem to find the gaussian filter. Or is this a filter I should implement myself using other filters (sliding window, math expression, etc.)?

[maximilianwulf]

My bad, I thought we have a gaussian filter, but you can use the MinInRadiusFilter to filter out the noise. If you transform the grid_map to an OpenCV Matrix you can use cv::GaussianBlur().

[Yaru-Gu]

Hello @olamarre, have you solved this problem? I've met the same problem when I'm trying to apply this elevation_mapping package to my legged walking robot. I noticed the display of the map is inaccurate when that particular part is out of the robot's sight.