beluga_tools.conversion_utils module

class beluga_tools.conversion_utils.DiscreteCell2D(x: int, y: int)

Bases: object

x: int
y: int
class beluga_tools.conversion_utils.DiscreteCell3D(x: int, y: int, z: int)

Bases: object

x: int
y: int
z: int
class beluga_tools.conversion_utils.NDTMap2D(resolution: float)

Bases: object

add_distribution(cell: DiscreteCell2D, ndt: NormalDistribution)

Add a new cell with its distribution.

classmethod from_hdf5(hdf5_file: Path)

Create an NDTMap instance from a path to a hdf5 file.

See ‘to_hdf5’ docstring for details on the hdf5 format.

is_close(other: NDTMap2D, abs_tol: float = 1e-08) bool

Equality with tolerance between two NDT maps.

plot(show: bool = False) matplotlib.pyplot.figure

Plot the map using contour plots into the current figure and returns it.

Optionally show the plot based on the ‘show’ parameter.

to_hdf5(output_file_path: Path)

Serialize the NDT map into an HDF5 format.

See https://docs.hdfgroup.org/hdf5/develop/_intro_h_d_f5.html for details on the format. It’ll create 4 datasets:

  • “resolution”: () resolution for the discrete grid (cells are resolution x resolution m^2 squares).

  • “cells”: (NUM_CELLS, 2) that contains the cell coordinates.

  • “means”: (NUM_CELLS, 2) that contains the 2d mean of the normal distribution of the cell.

  • “covariances”: (NUM_CELLS, 2, 2) Contains the covariance for each cell.

class beluga_tools.conversion_utils.NDTMap3D(resolution: float)

Bases: object

add_distribution(cell: DiscreteCell3D, ndt: NormalDistribution)

Add a new cell with its distribution.

classmethod from_hdf5(hdf5_file: Path)

Create an NDTMap3D instance from a path to a hdf5 file.

See ‘to_hdf5’ docstring for details on the hdf5 format.

is_close(other: NDTMap3D, abs_tol: float = 1e-08) bool

Equality with tolerance between two NDT maps.

to_hdf5(output_file_path: Path)

Serialize the NDT map into an HDF5 format.

See https://docs.hdfgroup.org/hdf5/develop/_intro_h_d_f5.html for details on the format. It’ll create 4 datasets:

  • “resolution”: () resolution for the discrete grid (cells are resolution x resolution x resolution m^3 cubes).

  • “cells”: (NUM_CELLS, 3) that contains the cell coordinates.

  • “means”: (NUM_CELLS, 3) that contains the 3d mean of the normal distribution of the cell.

  • “covariances”: (NUM_CELLS, 3, 3) Contains the covariance for each cell.

class beluga_tools.conversion_utils.NormalDistribution(mean: numpy.ndarray, covariance: numpy.ndarray)

Bases: object

Wrapper around scipy’s multivariate normal distribution implementation.

Allows for equality checks and typing hints.

covariance: numpy.ndarray
is_close(other: NormalDistribution, abs_tol: float = 1e-08) bool

Element-wise equality with tolerance.

mean: numpy.ndarray
to_scipy()

Get its scipy representation.

class beluga_tools.conversion_utils.OccupancyGrid(resolution: float, origin: numpy.ndarray, grid: numpy.ndarray)

Bases: object

grid: numpy.ndarray
static load_from_file(yaml_path: Path) OccupancyGrid

Create an grid from a yaml file describing a ROS style occupancy grid.

origin: numpy.ndarray
resolution: float
beluga_tools.conversion_utils.fit_normal_distribution(points: numpy.ndarray, min_variance: float = 0.005) NormalDistribution | None

Fit a normal distribution to a set of 2D or 3D points.

A minimum variance in each dimension will be enforced to avoid singularities.

beluga_tools.conversion_utils.grid_to_point_cloud(occupancy_grid: OccupancyGrid) numpy.ndarray

Convert an OccupancyGrid’s occupied cells to a 2D point cloud.

Uses the center of the cell for the conversion to reduce max error.

beluga_tools.conversion_utils.point_cloud_to_ndt_2d(pc: numpy.ndarray, cell_size=1.0) NDTMap2D

Convert a 2D point cloud into a NDT map representation.

Does so by clustering points in 2D cells of {cell_size} * {cell_size} meters^2 and fitting a normal distribution when applicable.

beluga_tools.conversion_utils.point_cloud_to_ndt_3d(pc: numpy.ndarray, cell_size=1.0) NDTMap3D

Convert a 3D point cloud into a NDT map representation.

Does so by clustering points in 3D cells of cell_size**3 meters^3 and fitting a normal distribution when applicable.