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)

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)

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)

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_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])

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