Elevation mapping cupy

class elevation_mapping_cupy.elevation_mapping.GridGeometry(length_x: float, length_y: float, resolution: float, center: ndarray, orientation: ndarray)

Bases: object

Lightweight holder describing a GridMap’s geometry.

length_x: float

length_y: float

resolution: float

center: ndarray

orientation: ndarray

property bounds_x: Tuple[float, float]

property bounds_y: Tuple[float, float]

property shape: Tuple[int, int]

__init__(length_x: float, length_y: float, resolution: float, center: ndarray, orientation: ndarray) → None

class elevation_mapping_cupy.elevation_mapping.ElevationMap(param: Parameter)

Bases: object

Core elevation mapping class.

__init__(param: Parameter)

Parameters:

param (elevation_mapping_cupy.parameter.Parameter)

clear()

Reset all the layers of the elevation & the semantic map.

get_center_position(position)

Return the position of the map center.

Parameters:

position (numpy.ndarray)

move(delta_position)

Shift the map along all three axes according to the input.

Parameters:

delta_position (numpy.ndarray)

move_to(position, R)

Shift the map to an absolute position and update the rotation of the robot.

Parameters:

• position (numpy.ndarray)

• R (cupy._core.core.ndarray)

pad_value(x, shift_value, idx=None, value=0.0)

Create a padding of the map along x,y-axis according to amount that has shifted.

Parameters:

• x (cupy._core.core.ndarray)

• shift_value (cupy._core.core.ndarray)

• idx (Union[None, int, None, None])

• value (float)

shift_map_xy(delta_pixel)

Shift the map along the horizontal axes according to the input.

Parameters:

delta_pixel (cupy._core.core.ndarray) – Shift in [x, y] order (world coordinates). x corresponds to columns (axis 2), y corresponds to rows (axis 1).

Note

The map array has shape (layers, height, width) = (layers, rows, cols). In row-major convention: axis 1 = rows = Y, axis 2 = cols = X. cp.roll with axis=(1, 2) expects [row_shift, col_shift] = [y_shift, x_shift]. Since delta_pixel is [x, y], we swap to [y, x] for correct axis mapping.

shift_map_z(delta_z)

Shift the relevant layers along the vertical axis.

Parameters:

delta_z (cupy._core.core.ndarray)

compile_kernels()

Compile all kernels belonging to the elevation map.

shift_translation_to_map_center(t)

Deduct the map center to get the translation of a point w.r.t. the map center.

Parameters:

t (cupy._core.core.ndarray) – Absolute point position

update_map_with_kernel(points_all, channels, R, t, position_noise, orientation_noise, source_stamp_ns=None)

Update map with new measurement.

Parameters:

• points_all (cupy._core.core.ndarray)

• channels (List[str])

• R (cupy._core.core.ndarray)

• t (cupy._core.core.ndarray)

• position_noise (float)

• orientation_noise (float)

clear_overlap_map(t)

Clear overlapping areas around the map center.

Parameters:

t (cupy._core.core.ndarray) – Absolute point position

get_additive_mean_error()

Returns the additive mean error.

Returns:

update_variance()

Adds the time variacne to the valid cells.

update_time()

adds the time interval to the time layer.

update_upper_bound_with_valid_elevation()

Filters all invalid cell’s upper_bound and is_upper_bound layers.

input_pointcloud(raw_points: cupy._core.core.ndarray, channels: List[str], R: cupy._core.core.ndarray, t: cupy._core.core.ndarray, position_noise: float, orientation_noise: float, source_stamp_ns: int | None = None)

Input the point cloud and fuse the new measurements to update the elevation map.

Parameters:

• raw_points (cupy._core.core.ndarray)

• channels (List[str])

• R (cupy._core.core.ndarray)

• t (cupy._core.core.ndarray)

• position_noise (float)

• orientation_noise (float)

• source_stamp_ns (Optional[int]) – Raw cloud source timestamp; missing, zero or nonincreasing values grant no observation evidence.

Return type:

None

input_image(image: List[cupy._core.core.ndarray], channels: List[str], R: cupy._core.core.ndarray, t: cupy._core.core.ndarray, K: cupy._core.core.ndarray, D: cupy._core.core.ndarray, distortion_model: str, image_height: int, image_width: int)

Project image channels into the map using the camera calibration and pose.

update_normal(dilated_map)

Clear the normal map and then apply the normal kernel with dilated map as input.

Parameters:

dilated_map (cupy._core.core.ndarray)

process_map_for_publish(input_map, fill_nan=False, add_z=False, xp=cupy)

Process the input_map according to the fill_nan and add_z flags.

Parameters:

• input_map (cupy._core.core.ndarray)

• fill_nan (bool)

• add_z (bool)

• xp (module)

Return type:

cupy._core.core.ndarray

get_elevation()

Get the elevation layer.

Returns:

elevation layer

get_variance()

Get the variance layer.

Returns:

variance layer

get_traversability()

Get the traversability layer.

Returns:

traversability layer

get_time()

Get the time layer.

Returns:

time layer

get_observation_age()

Seconds since accepted raw cloud observation at the published stamp.

Round upward when converting float64 ages to GridMap float32, so numeric rounding cannot make stale terrain look more recently observed.

get_upper_bound()

Get the upper bound layer.

Returns:

upper bound layer

Return type:

upper_bound

get_is_upper_bound()

Get the is upper bound layer.

Returns:

layer

Return type:

is_upper_bound

xp_of_array(array)

Indicate which library is used for xp.

Parameters:

array (cupy._core.core.ndarray)

Returns:

either np or cp

Return type:

module

copy_to_cpu(array, data, stream=None)

Transforms the data to float32 and if on gpu loads it to cpu.

Parameters:

• array (cupy._core.core.ndarray)

• data (numpy.ndarray)

• stream (Union[None, cupy.cuda.stream.Stream, None, None, None, None, None, None, None])

trim_memory_pool()

Release cached CuPy allocator blocks that are not currently in use.

exists_layer(name)

Check if the layer exists in elevation map or in the semantic map.

Parameters:

name (str) – Layer name

Returns:

Indicates if layer exists.

Return type:

bool

get_map_with_name_ref(name, data)

Load a layer according to the name input to the data input.

Parameters:

• name (str) – Name of the layer.

• data (numpy.ndarray) – Data structure that contains layer.

get_normal_maps()

Get the normal maps.

Returns:

the three normal values for each cell

Return type:

maps

get_normal_ref(normal_x_data, normal_y_data, normal_z_data)

Get the normal maps as reference.

Parameters:

• normal_x_data

• normal_y_data

• normal_z_data

get_layer(name)

Return the layer with the name input.

Parameters:

name – The layers name.

Returns:

The rqeuested layer.

Return type:

return_map

get_polygon_traversability(polygon, result)

Check if input polygons are traversable.

Parameters:

• polygon (cupy._core.core.ndarray)

• result (numpy.ndarray)

Return type:

Union[None, int]

get_untraversable_polygon(untraversable_polygon)

Copy the untraversable polygons to input untraversable_polygons.

Parameters:

untraversable_polygon (numpy.ndarray)

initialize_map(points, method='cubic')

Initializes the map according to some points and using an approximation according to method.

Parameters:

• points (numpy.ndarray)

• method (str) – Interpolation method [‘linear’, ‘cubic’, ‘nearest’]

list_layers() → List[str]

export_layers(layer_names: List[str]) → Dict[str, ndarray]

apply_masked_replace(layer_data: Dict[str, ndarray], mask: ndarray | None, geometry: GridGeometry) → None

set_full_map(fused_layers: Dict[str, ndarray], raw_layers: Dict[str, ndarray], geometry: GridGeometry) → None

class elevation_mapping_cupy.parameter.Parameter(*args: Any, **kwargs: Any)

Bases: Serializable

This class holds the parameters for the elevation mapping algorithm.

resolution

The resolution in meters. (Default: 0.04)

Type:

float

subscriber_cfg

The configuration for the subscriber. (Default: { "front_cam": { "channels": ["rgb", "person"], "topic_name": "/elevation_mapping/pointcloud_semantic", "data_type": "pointcloud", } })

Type:

dict

additional_layers

The additional layers for the map. (Default: ["color"])

Type:

list

fusion_algorithms

The list of fusion algorithms. (Default: [ "image_color", "image_exponential", "pointcloud_average", "pointcloud_bayesian_inference", "pointcloud_class_average", "pointcloud_class_bayesian", "pointcloud_class_max", "pointcloud_color", ])

Type:

list

pointcloud_channel_fusions

The fusion for pointcloud channels. (Default: {"rgb": "color", "default": "class_average"})

Type:

dict

image_channel_fusions

The fusion for image channels. (Default: {"rgb": "color", "default": "exponential"})

Type:

dict

data_type

The data type for the map. (Default: np.float32)

Type:

str

average_weight

The weight for the average fusion. (Default: 0.5)

Type:

float

map_length

The map’s size in meters. (Default: 8.0)

Type:

float

sensor_noise_factor

The point’s noise is sensor_noise_factor*z^2 (z is distance from sensor). (Default: 0.05)

Type:

float

mahalanobis_thresh

Points outside this distance is outlier. (Default: 2.0)

Type:

float

outlier_variance

If point is outlier, add this value to the cell. (Default: 0.01)

Type:

float

drift_compensation_variance_inlier

Cells under this value is used for drift compensation. (Default: 0.1)

Type:

float

time_variance

Add this value when update_variance is called. (Default: 0.01)

Type:

float

time_interval

Time layer is updated with this interval. (Default: 0.1)

Type:

float

max_variance

The maximum variance for each cell. (Default: 1.0)

Type:

float

dilation_size

The dilation filter size before traversability filter. (Default: 2)

Type:

int

dilation_size_initialize

The dilation size after the init. (Default: 10)

Type:

int

drift_compensation_alpha

The drift compensation alpha for smoother update of drift compensation. (Default: 1.0)

Type:

float

traversability_inlier

Cells with higher traversability are used for drift compensation. (Default: 0.1)

Type:

float

wall_num_thresh

If there are more points than this value, only higher points than the current height are used to make the wall more sharp. (Default: 100)

Type:

int

min_height_drift_cnt

Drift compensation only happens if the valid cells are more than this number. (Default: 100)

Type:

int

max_ray_length

The maximum length for ray tracing. (Default: 2.0)

Type:

float

cleanup_step

Substitute this value from validity layer at visibility cleanup. (Default: 0.01)

Type:

float

cleanup_cos_thresh

Substitute this value from validity layer at visibility cleanup. (Default: 0.5)

Type:

float

min_valid_distance

Points with shorter distance will be filtered out. (Default: 0.3)

Type:

float

max_height_range

Points higher than this value from sensor will be filtered out to disable ceiling. (Default: 1.0)

Type:

float

ramped_height_range_a

If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. (Default: 0.3)

Type:

float

ramped_height_range_b

If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. (Default: 1.0)

Type:

float

ramped_height_range_c

If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. (Default: 0.2)

Type:

float

safe_thresh

If traversability is smaller, it is counted as unsafe cell. (Default: 0.5)

Type:

float

safe_min_thresh

Polygon is unsafe if there exists lower traversability than this. (Default: 0.5)

Type:

float

max_unsafe_n

If the number of cells under safe_thresh exceeds this value, polygon is unsafe. (Default: 20)

Type:

int

checker_layer

Layer used for checking safety. (Default: "traversability")

Type:

str

max_drift

The maximum drift for the compensation. (Default: 0.10)

Type:

float

overlap_clear_range_xy

XY range [m] for clearing overlapped area. This defines the valid area for overlap clearance. (used for multi floor setting) (Default: 4.0)

Type:

float

overlap_clear_range_z

Z range [m] for clearing overlapped area. Cells outside this range will be cleared. (used for multi floor setting) (Default: 2.0)

Type:

float

enable_edge_sharpen

Enable edge sharpening. (Default: True)

Type:

bool

enable_drift_compensation

Enable drift compensation. (Default: True)

Type:

bool

enable_visibility_cleanup

Enable visibility cleanup. (Default: True)

Type:

bool

enable_overlap_clearance

Enable overlap clearance. (Default: True)

Type:

bool

use_only_above_for_upper_bound

Use only above for upper bound. (Default: True)

Type:

bool

use_chainer

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. (Default: True)

Type:

bool

position_noise_thresh

If the position change is bigger than this value, the drift compensation happens. (Default: 0.1)

Type:

float

orientation_noise_thresh

If the orientation change is bigger than this value, the drift compensation happens. (Default: 0.1)

Type:

float

plugin_config_file

Configuration file for the plugin. (Default: "config/plugin_config.yaml")

Type:

str

weight_file

Weight file for traversability filter. (Default: "config/weights.dat")

Type:

str

initial_variance

Initial variance for each cell. (Default: 10.0)

Type:

float

initialized_variance

Initialized variance for each cell. (Default: 10.0)

Type:

float

w1

Weights for the first layer. (Default: np.zeros((4, 1, 3, 3)))

Type:

numpy.ndarray

w2

Weights for the second layer. (Default: np.zeros((4, 1, 3, 3)))

Type:

numpy.ndarray

w3

Weights for the third layer. (Default: np.zeros((4, 1, 3, 3)))

Type:

numpy.ndarray

w_out

Weights for the output layer. (Default: np.zeros((1, 12, 1, 1)))

Type:

numpy.ndarray

true_map_length

True length of the map. (Default: None)

Type:

float

cell_n

Number of cells in the map. (Default: None)

Type:

int

true_cell_n

True number of cells in the map. (Default: None)

Type:

int

resolution: float = 0.04

subscriber_cfg: dict

additional_layers: list

fusion_algorithms: list

pointcloud_channel_fusions: dict

image_channel_fusions: dict

data_type

alias of float32

average_weight: float = 0.5

map_length: float = 8.0

sensor_noise_factor: float = 0.05

mahalanobis_thresh: float = 2.0

outlier_variance: float = 0.01

drift_compensation_variance_inlier: float = 0.1

time_variance: float = 0.01

time_interval: float = 0.1

max_variance: float = 1.0

dilation_size: int = 2

dilation_size_initialize: int = 10

drift_compensation_alpha: float = 1.0

traversability_inlier: float = 0.1

wall_num_thresh: int = 100

min_height_drift_cnt: int = 100

max_ray_length: float = 2.0

cleanup_step: float = 0.01

cleanup_cos_thresh: float = 0.5

min_valid_distance: float = 0.3

max_height_range: float = 1.0

ramped_height_range_a: float = 0.3

ramped_height_range_b: float = 1.0

ramped_height_range_c: float = 0.2

safe_thresh: float = 0.5

safe_min_thresh: float = 0.5

max_unsafe_n: int = 20

checker_layer: str = 'traversability'

max_drift: float = 0.1

overlap_clear_range_xy: float = 4.0

overlap_clear_range_z: float = 2.0

enable_edge_sharpen: bool = True

enable_drift_compensation: bool = True

enable_visibility_cleanup: bool = True

enable_overlap_clearance: bool = True

use_only_above_for_upper_bound: bool = True

use_chainer: bool = True

position_noise_thresh: float = 0.1

orientation_noise_thresh: float = 0.1

plugin_config_file: str = 'config/plugin_config.yaml'

weight_file: str = 'config/weights.dat'

initial_variance: float = 10.0

initialized_variance: float = 10.0

w1: ndarray

w2: ndarray

w3: ndarray

w_out: ndarray

true_map_length: float = None

cell_n: int = None

true_cell_n: int = None

load_weights(filename: str)

Load weights from a file into the model’s parameters.

Parameters:

filename (str) – The path to the file containing the weights.

get_names()

Get the names of the parameters.

Returns:

A list of parameter names.

Return type:

list

get_types()

Get the types of the parameters.

Returns:

A list of parameter types.

Return type:

list

set_value(name, value)

Set the value of a parameter.

Parameters:

• name (str) – The name of the parameter.

• value (any) – The new value for the parameter.

get_value(name)

Get the value of a parameter.

Parameters:

name (str) – The name of the parameter.

Returns:

The value of the parameter.

Return type:

any

update()

Update the parameters related to the map size and resolution.

__init__(resolution: float = 0.04, subscriber_cfg: dict = <factory>, additional_layers: list = <factory>, fusion_algorithms: list = <factory>, pointcloud_channel_fusions: dict = <factory>, image_channel_fusions: dict = <factory>, data_type: str = <class 'numpy.float32'>, average_weight: float = 0.5, map_length: float = 8.0, sensor_noise_factor: float = 0.05, mahalanobis_thresh: float = 2.0, outlier_variance: float = 0.01, drift_compensation_variance_inlier: float = 0.1, time_variance: float = 0.01, time_interval: float = 0.1, max_variance: float = 1.0, dilation_size: int = 2, dilation_size_initialize: int = 10, drift_compensation_alpha: float = 1.0, traversability_inlier: float = 0.1, wall_num_thresh: int = 100, min_height_drift_cnt: int = 100, max_ray_length: float = 2.0, cleanup_step: float = 0.01, cleanup_cos_thresh: float = 0.5, min_valid_distance: float = 0.3, max_height_range: float = 1.0, ramped_height_range_a: float = 0.3, ramped_height_range_b: float = 1.0, ramped_height_range_c: float = 0.2, safe_thresh: float = 0.5, safe_min_thresh: float = 0.5, max_unsafe_n: int = 20, checker_layer: str = 'traversability', max_drift: float = 0.1, overlap_clear_range_xy: float = 4.0, overlap_clear_range_z: float = 2.0, enable_edge_sharpen: bool = True, enable_drift_compensation: bool = True, enable_visibility_cleanup: bool = True, enable_overlap_clearance: bool = True, use_only_above_for_upper_bound: bool = True, use_chainer: bool = True, position_noise_thresh: float = 0.1, orientation_noise_thresh: float = 0.1, plugin_config_file: str = 'config/plugin_config.yaml', weight_file: str = 'config/weights.dat', initial_variance: float = 10.0, initialized_variance: float = 10.0, w1: ndarray = <factory>, w2: ndarray = <factory>, w3: ndarray = <factory>, w_out: ndarray = <factory>, true_map_length: float = None, cell_n: int = None, true_cell_n: int = None) → None

class elevation_mapping_cupy.plugins.plugin_manager.PluginParams(name: str, layer_name: str, fill_nan: bool = False, is_height_layer: bool = False)

Bases: object

name: str

layer_name: str

fill_nan: bool = False

is_height_layer: bool = False

__init__(name: str, layer_name: str, fill_nan: bool = False, is_height_layer: bool = False) → None

class elevation_mapping_cupy.plugins.plugin_manager.PluginBase(*args, **kwargs)

Bases: ABC

This is a base class of Plugins

__init__(*args, **kwargs)

Parameters:

• plugin_params – PluginParams

• callback (The parameter of)

get_layer_data(elevation_map: cupy.ndarray, layer_names: List[str], plugin_layers: cupy.ndarray, plugin_layer_names: List[str], semantic_map: cupy.ndarray, semantic_layer_names: List[str], name: str) → cupy.ndarray | None

Retrieve a copy of the layer data from the elevation, plugin, or semantic maps based on the layer name.

Parameters:

• elevation_map (cp.ndarray) – The elevation map containing various layers.

• layer_names (List[str]) – A list of names for each layer in the elevation map.

• plugin_layers (cp.ndarray) – The plugin layers containing additional data.

• plugin_layer_names (List[str]) – A list of names for each layer in the plugin layers.

• semantic_map (cp.ndarray) – The semantic map containing various layers.

• semantic_layer_names (List[str]) – A list of names for each layer in the semantic map.

• name (str) – The name of the layer to retrieve.

Returns:

A copy of the requested layer as a cupy ndarray if found, otherwise None.

Return type:

Optional[cp.ndarray]

class elevation_mapping_cupy.plugins.plugin_manager.PluginManager(cell_n: int, resolution: float | None = None)

Bases: object

This manages the plugins.

__init__(cell_n: int, resolution: float | None = None)

init(plugin_params: List[PluginParams], extra_params: List[Dict])

load_plugin_settings(file_path: str)

get_layer_names()

reset_layers()

Invalidate cached plugin layers so they will be recomputed on demand.

get_plugin_names()

get_plugin_index_with_name(name: str) → int

get_layer_index_with_name(name: str) → int

update_with_name(name: str, elevation_map: cupy.ndarray, layer_names: List[str], semantic_map=None, semantic_params=None, rotation=None, elements_to_shift=None, _active_stack=None)

get_map_with_name(name: str) → cupy.ndarray

get_param_with_name(name: str) → PluginParams