occupancyMap3D
Create 3-D occupancy map
Description
The occupancyMap3D class stores a 3-D map and map information.
The map is stored as probabilistic values in an octree data
structure. The class handles arbitrary environments and expands its size
dynamically based on observation inputs. You can add observations as point clouds or as
specific xyz locations. These observations update the probability
values. Probabilistic values represent the occupancy of locations. The octree data
structure trims data appropriately to remain efficient both in memory and on
disk.
Creation
Description
omap = occupancyMap3D
omap = occupancyMap3D(res)Resolution property.
omap = occupancyMap3D(res,Name,Value)Name,Value pair arguments. For example,
'FreeThreshold',0.25 sets the threshold to consider
cells obstacle-free as a probability value of 0.25. Enclose each property
name in quotes.
Properties
Object Functions
checkOccupancy | Check if locations are free or occupied |
getOccupancy | Get occupancy probability of locations |
inflate | Inflate map |
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 | Show 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 achieving a desired map resolution (voxel size) is achieved. 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. Using this representation, locations easily recover from dynamic observations and numerical errors due to small probabilities are reduced. 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 class internally handles the organization of this data structure, including
the pruning of branches. Specify all observations as spatial coordinates when using
functions such as setOccupancy, getOccupancy, or insertPointCloud. Insertions into the tree, and navigation through
the tree, is determined 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, Vol. 34, No. 3, 2013, pp. 189–206.. doi:10.1007/s10514-012-9321-0.