insertPointCloud

Insert 3-D points or point cloud observation into map

Syntax

insertPointCloud(map3D,pose,points,maxrange)

insertPointCloud(map3D,pose,ptcloud,maxrange)

insertPointCloud(___,invModel)

Description

insertPointCloud(map3D,pose,points,maxrange) inserts one or more sensor observations at the given points in the occupancy map, map3D. Occupied points are updated with an observation of 0.7. All other points between the sensor pose and points are treated as obstacle-free and updated with an observation of 0.4. Points outside maxrange are not updated. NaN values are ignored.

insertPointCloud(map3D,pose,ptcloud,maxrange) inserts a ptcloud object into the map.

insertPointCloud(___,invModel) inserts a point cloud with updated probabilities invModel that correspond to obstacle-free and occupied observations. Use any of the previous syntaxes to input the point cloud.

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)

Input Arguments

map3D — 3-D occupancy map

occupancyMap3D object

3-D occupancy map, specified as a occupancyMap3D object.

points — Points of point cloud

n-by-3 matrix

Points of point cloud in sensor coordinates, specified as an n-by-3 matrix of [x y z] points, where n is the number of points in the point cloud.

ptcloud — Point cloud reading

pointCloud object

Point cloud reading, specified as a pointCloud object.

Note

Using pointCloud objects requires Computer Vision Toolbox™.

pose — Position and orientation of vehicle

[x y z qw qx qy qz] vector

Position and orientation of vehicle, specified as an [x y z qw qx qy qz] vector. The vehicle pose is an xyz-position vector with a quaternion orientation vector specified as [qw qx qy qz].

maxrange — Maximum range of sensor

scalar

Maximum range of point cloud sensor, specified as a scalar. Points outside this range are ignored.

invModel — Inverse sensor model values

two-element vector

Inverse sensor model values, specified as a two-element vector corresponding to the obstacle-free and occupied probabilities. Point cloud points are updated according to the inverse sensor model and the specified range readings. NaN range values are ignored. Range values greater than maxrange are not updated. See Inverse Sensor Model.

More About

Inverse Sensor Model

The inverse sensor model determines how sensor readings and obstacle values are set in a point cloud. You can customize this model by specifying different probabilities for free and occupied locations in the invModel argument. NaN range values are ignored. Range values greater than maxrange are not updated.

Grid locations that contain range readings are updated with the occupied probability. Locations before the reading are updated with the free probability. All locations after the reading are not updated.

Extended Capabilities

C/C++ Code Generation

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

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

Version History

Introduced in R2019b

R2024a: Specify inverse sensor model values

You can now specify the inverse sensor model values for the corresponding obstacle-free and occupied probabilities. For more information about specifying the inverse sensor model values, see the invModel argument.