rayIntersection

Find intersection points of rays and occupied map cells

collapse all in page

Syntax

intersectionPts = rayIntersection(map,pose,angles,maxrange)
intersectionPts = rayIntersection(map,pose,angles,maxrange,threshold)

Description

intersectionPts = rayIntersection(map,pose,angles,maxrange) returns intersection points of rays and occupied cells in the specified map. Rays emanate from the specified pose and angles. Intersection points are returned in the world coordinate frame. If there is no intersection up to the specified maxrange, [NaN NaN] is returned. By default, the OccupiedThreshold property is used to determine occupied cells.

intersectionPts = rayIntersection(map,pose,angles,maxrange,threshold) returns intersection points based on the specified threshold for the occupancy values. Values greater than or equal to the threshold are considered occupied.

example

Examples

collapse all

Open Live Script

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)

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains an object of type image.

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')

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 7 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Collision Points, Vehicle Position, Rays.

Input Arguments

collapse all

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.

Position and orientation of sensor, specified as an [x y theta] vector. The sensor pose is an x and y position with angular orientation theta (in radians) measured from the x-axis.

Ray angles emanating from the sensor, specified as a vector with elements in radians. These angles are relative to the specified sensor pose.

Maximum range of laser range sensor, specified as a scalar in meters. Range values greater than or equal to maxrange are considered free along the whole length of the ray, up to maxrange.

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.

Output Arguments

collapse all

Intersection points, returned as n-by-2 matrix of [x y] pairs in the world frame, where n is the length of angles.

Extended Capabilities

expand all

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019b

See Also

| | | (Robotics System Toolbox)

Topics