Height drift compensation not updating unseen points

[itaouil]

Hi :),

Thanks for making open-source such an amazing package.

When enabling the the height drift compensation I noticed that it only updates the cells that are hit by the rays of the sensors, but the remaining cells are not (as shown in the pic below), leading to a behavior where some of the feet start floating.

Is there a way to enable the update for all cells or maybe I am missing something?

[mktk1117]

Hi, thanks for using this!
The way drift compensation works is, first calculate the height error, and then apply that error to all cells.
So I suspect that the state estimation is drifting but the height error correction is not correcting 100%
Currently the height error is only calculated on a flat surface by checking the traversability value.

Do you have a screenshot with a point cloud visualization?

[itaouil]

Hi,

Here are some pics:

Ok. So the height drift compensation should actually add the height error compensation brought by the state estimation drift (based on the cells that are traversable and flat).

Do you have any idea on what could make the height error correction not work as expected? Maybe I should be using the corrected TF published somehow? I also copied the parameters.yaml file down if that can help.

#### Mapping parameters ########
resolution: 0.02                                # resolution in m.
map_length: 2                                  # map's size in m.
sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
mahalanobis_thresh: 1.0                         # points outside this distance is outlier.
outlier_variance: 0.01                          # if point is outlier, add this value to the cell.
drift_compensation_variance_inler: 0.05         # cells under this value is used for drift compensation.
max_drift: 0.1                                  # drift compensation happens only the drift is smaller than this value (for safety)
drift_compensation_alpha: 0.1                   # drift compensation alpha for smoother update of drift compensation
time_variance: 0.0001                           # add this value when update_variance is called.
max_variance: 100.0                             # maximum variance for each cell.
initial_variance: 1000.0                        # initial variance for each cell.
traversability_inlier: 0.9                      # cells with higher traversability are used for drift compensation.
dilation_size: 3                                # dilation filter size before traversability filter.
wall_num_thresh: 20                             # if there are more points than this value, only higher points than the current height are used to make the wall more sharp.
min_height_drift_cnt: 100                       # drift compensation only happens if the valid cells are more than this number.
position_noise_thresh: 0.01                     # if the position change is bigger than this value, the drift compensation happens.
orientation_noise_thresh: 0.01                  # if the orientation change is bigger than this value, the drift compensation happens.
position_lowpass_alpha: 0.2                     # lowpass filter alpha used for detecting movements.
orientation_lowpass_alpha: 0.2                  # lowpass filter alpha used for detecting movements.
min_valid_distance: 0.06                         # points with shorter distance will be filtered out.
max_height_range: 1.0                           # points higher than this value from sensor will be filtered out to disable ceiling.
ramped_height_range_a: 0.3                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
ramped_height_range_b: 1.0                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
ramped_height_range_c: 0.2                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
update_variance_fps: 30.0                      # fps for updating variance.
update_pose_fps: 200.0                          # fps for updating pose and shift the center of map.
time_interval: 0.1                              # Time layer is updated with this interval.
map_acquire_fps: 5.0                          # Raw map is fetched from GPU memory in this fps.
publish_statistics_fps: 1.0                     # Publish statistics topic in this fps.

max_ray_length: 2.0                             # maximum length for ray tracing.
cleanup_step: 0.1                               # subtitute this value from validity layer at visibiltiy cleanup.
cleanup_cos_thresh: 0.1                         # subtitute this value from validity layer at visibiltiy cleanup.

safe_thresh: 0.7                                # if traversability is smaller, it is counted as unsafe cell.
safe_min_thresh: 0.4                            # polygon is unsafe if there exists lower traversability than this.
max_unsafe_n: 10                                # if the number of cells under safe_thresh exceeds this value, polygon is unsafe.

overlap_clear_range_xy: 4.0                     # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
overlap_clear_range_z: 2.0                      # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)

map_frame: 'odom'                               # The map frame where the odometry source uses.
base_frame: 'base'                              # The robot's base frame. This frame will be a center of the map.
corrected_map_frame: 'corrected_odom'

#### Feature toggles ########
enable_edge_sharpen: false
enable_visibility_cleanup: false
enable_drift_compensation: true
enable_overlap_clearance: false
enable_pointcloud_publishing: false
enable_drift_corrected_TF_publishing: true
enable_normal_color: false                      # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.

#### Traversability filter ########
use_chainer: false                              # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster.
weight_file: '$(rospack find elevation_mapping_cupy)/config/weights.dat'               # Weight file for traversability filter

#### Upper bound ########
use_only_above_for_upper_bound: false

#### Plugins ########
plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/plugin_config.yaml'

#### Subscribers ########
pointcloud_topics: ['/depth_camera_front/depth/color/points',
                    '/depth_camera_rear/depth/color/points',
                    '/depth_camera_left/depth/color/points',
                    '/depth_camera_right/depth/color/points',
                    '/robot_self_filter/bpearl_front/point_cloud',
                    '/robot_self_filter/bpearl_rear/point_cloud'
                    ]

#### Publishers ########
# topic_name:
#   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
#   basic_layers:         # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
#   fps:                  # Publish rate. Use smaller value than `map_acquire_fps`.
publishers:
  elevation_map_raw:
    layers: ['elevation', 'traversability']
    basic_layers: ['elevation', 'traversability']
    fps: 30.0
  elevation_map_filter:
    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound', 'median_filter', 'closing_filter'] 
    basic_layers: ['min_filter', 'median_filter', 'closing_filter']
    fps: 30.0

#### Initializer ########
initialize_method: 'nearest'                                         # Choose one from 'nearest', 'linear', 'cubic'
initialize_frame_id: ['rf_foot', 'lf_foot', 'rf_foot', 'lf_foot']   # One tf (like ['footprint'] ) initializes a square around it.
initialize_tf_offset: [-0.03, -0.03, -0.03, -0.03]                          # z direction. Should be same number as initialize_frame_id.
dilation_size_initialize: 5                                         # dilation size after the init.
initialize_tf_grid_size: 0.4                                        # This is not used if number of tf is more than 3.
use_initializer_at_start: true                                      # Use initializer when the node starts.

[itaouil]

Here are other two pictures with and without pointcloud as soon as the grid map is launched:

[mktk1117]

It might be that there are not enough flat area to perform drift compensation.

You can try tuning these parameters.
drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation.
traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation.
drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation
min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number.

For example if you put drift_compensation_alpha to 1.0. It will correct using the exact error.
In this case, the drift effect will be smaller, but the map can become a bit jumpy.

[itaouil]

Thank you.

We will try to tune these parameters.