Find intersection points of rays and occupied map cells
returns intersection points of rays and occupied cells in the specified
intersectionPts = rayIntersection(
map. Rays emanate from the specified
points are returned in the world coordinate frame. If there is no intersection
up to the specified
is returned. By default, the
OccupiedThreshold property is
used to determine occupied cells.
Get Ray Intersection Points on Occupancy Map
Create an occupancy grid map. Add obstacles and inflate them. A lower resolution map is used to illustrate the importance of using grid cells. Show the map.
map = occupancyMap(10,10,2); obstacles = [4 10; 3 5; 7 7]; setOccupancy(map,obstacles,ones(length(obstacles),1)) inflate(map,0.25) show(map)
Find the intersection points of occupied cells and rays that emit from the given vehicle pose. Specify the max range and angles for these rays. The last ray does not intersect with an obstacle within the max range, so it has no collision point.
maxrange = 6; angles = [pi/4,-pi/4,0,-pi/8]; vehiclePose = [4,4,pi/2]; intsectionPts = rayIntersection(map,vehiclePose,angles,maxrange,0.7)
intsectionPts = 4×2 3.5000 4.5000 6.0000 6.0000 4.0000 9.0000 NaN NaN
Plot the intersection points and plot rays from the pose to the intersection points.
hold on plot(intsectionPts(:,1),intsectionPts(:,2),'*r') % Intersection points plot(vehiclePose(1),vehiclePose(2),'ob') % Vehicle pose for i = 1:3 plot([vehiclePose(1),intsectionPts(i,1)],... [vehiclePose(2),intsectionPts(i,2)],'-b') % Plot intersecting rays end plot([vehiclePose(1),vehiclePose(1)-6*sin(angles(4))],... [vehiclePose(2),vehiclePose(2)+6*cos(angles(4))],'-b') % No intersection ray legend('Collision Points','Vehicle Position','Rays','Location','SouthEast')
map — Map representation
Map representation, specified as a
occupancyMap object. This object
represents the environment of the sensor. The object contains a matrix grid
with values representing the probability of the occupancy of that cell.
Values close to 1 represent a high probability that the cell contains an
obstacle. Values close to 0 represent a high probability that the cell is
not occupied and obstacle free.
pose — Position and orientation of sensor
Position and orientation of sensor, specified as an
theta] vector. The sensor pose is an
x and y position with angular
orientation theta (in radians) measured from the
angles — Ray angles emanating from sensor
Ray angles emanating from the sensor, specified as a vector with elements
in radians. These angles are relative to the specified sensor
maxrange — Maximum range of sensor
Maximum range of laser range sensor, specified as a scalar in meters.
Range values greater than or equal to
considered free along the whole length of the ray, up to
threshold — Threshold for occupied cells
scalar from 0 to 1
Threshold for occupied cells, specified as a scalar from 0 to 1. Occupancy values greater than or equal to the threshold are treated as occupied cells to trigger intersections.
intersectionPts — Intersection points
Intersection points, returned as n-by-2 matrix of
y] pairs in the world frame, where
n is the length of
Introduced in R2019b
binaryOccupancyMap (Robotics System Toolbox)