occupancyMap3D

Create 3-D occupancy map

Description

The occupancyMap3D object represents a 3-D environment using an octree data structure, where each node stores a probabilistic value indicating the likelihood of occupancy. This probabilistic octree enables efficient modeling of arbitrary environments, dynamically expanding its size as you add new observations. You can update the map using point clouds or specific xyz-locations, and the octree structure automatically manages memory and disk usage by trimming data as needed.

Creation

Syntax

omap = occupancyMap3D

omap = occupancyMap3D(res)

omap = occupancyMap3D(res,Name,Value)

Description

omap = occupancyMap3D creates an empty 3-D occupancy map with no observations and default property values.

omap = occupancyMap3D(res) specifies a map resolution in cells/meter and sets the Resolution property.

omap = occupancyMap3D(res,Name,Value) specifies properties using one or more name-value arguments. For example, "FreeThreshold",0.25 sets the threshold to consider cells obstacle-free as a probability value of 0.25.

Properties

Resolution — Grid resolution

1 (default) | positive scalar

Grid resolution in cells per meter, specified as a scalar. Specify resolution on construction. Inserting observations with precisions higher than this value are rounded down and applied at this resolution.

FreeThreshold — Threshold to consider cells as obstacle-free

0.2 (default) | positive scalar

Threshold to consider cells as obstacle-free, specified as a positive scalar. Probability values below this threshold are considered obstacle-free.

OccupiedThreshold — Threshold to consider cells as occupied

0.65 (default) | positive scalar

Threshold to consider cells as occupied, specified as a positive scalar. Probability values above this threshold are considered occupied.

ProbabilitySaturation — Saturation limits on probability values

[0.001 0.999] (default) | [min max] vector

Saturation limits on probability values, specified as a [min max] vector. Values above or below these saturation values are set to the min or max values. This property reduces oversaturating of cells when incorporating multiple observations.

Object Functions

checkOccupancy	Check if locations are free or occupied
getOccupancy	Get occupancy probability of locations
inflate	Inflate each occupied location
insertPointCloud	Insert 3-D points or point cloud observation into map
rayIntersection	Find intersection points of rays and occupied map cells
setOccupancy	Set occupancy probability of locations
show	Display 3-D occupancy map
updateOccupancy	Update occupancy probability at locations

Examples

Create 3-D Occupancy Map and Inflate Points

The occupancyMap3D object stores obstacles in 3-D space, using sensor observations to map an environment. Create a map and add points from a point cloud to identify obstacles. Then inflate the obstacles in the map to ensure safe operating space around obstacles.

Create an occupancyMap3D object with a map resolution of 10 cells/meter.

map3D = occupancyMap3D(10);

Define a set of 3-D points as an observation from a pose [x y z qw qx qy qz]. This pose is for the sensor that observes these points and is centered on the origin. Define two sets of points to insert multiple observations.

pose = [ 0 0 0 1 0 0 0];

points = repmat((0:0.25:2)', 1, 3);
points2 = [(0:0.25:2)' (2:-0.25:0)' (0:0.25:2)'];
maxRange = 5;

Insert the first set of points using insertPointCloud. The function uses the sensor pose and the given points to insert observations into the map. The colors displayed correlate to the height of the point only for illustrative purposes.

insertPointCloud(map3D,pose,points,maxRange)
show(map3D)

Insert the second set of points. The ray between the sensor pose (origin) and these points overlap points from the previous insertion. Therefore, the free space between the sensor and the new points are updated and marked as free space.

insertPointCloud(map3D,pose,points2,maxRange)
show(map3D)

Inflate the map to add a buffer zone for safe operation around obstacles. Define the vehicle radius and safety distance and use the sum of these values to define the inflation radius for the map.

vehicleRadius = 0.2;
safetyRadius = 0.3;
inflationRadius = vehicleRadius + safetyRadius;
inflate(map3D, inflationRadius);

show(map3D)

Algorithms

Octree Data Structure

The octree data structure is a hierarchical structure used for subdivision of an environment into cubic volumes called voxels. For a given map volume, the space is recursively subdivided into eight voxels until it achieves a desired map resolution (voxel size). This subdivision can be represented as a tree, which stores probability values for locations in the map.

The probability values in the tree have a log-odds representation. This representation enables locations to easily recover from dynamic observations, and reduces numerical errors due to small probabilities. To remain efficient in memory, lower branches of the tree are pruned in the structure if they share the same occupancy values using this log-odds representation.

The object handles the organization of this data structure, including the pruning of branches, internally. When using functions such as setOccupancy, getOccupancy, or insertPointCloud, specify all observations as spatial coordinates. The object determines insertions into the tree, and navigation through the tree, based on the spatial coordinates and the resolution of the map.

References

[1] Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, and Wolfram Burgard. "OctoMap: an efficient probabilistic 3D mapping framework based on octrees." Autonomous Robots 34, no. 3 (April 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

occupancyMap3D does not support code generation using the packNGo (MATLAB Coder) function if you set the packType argument to "flat".

Version History

Introduced in R2019b