closestPoint

Find closest point on reference path to global point

collapse all in page

Syntax

pathPoints = closestPoint(refPath,points)
[pathPoints,inWindow] = closestPoint(refPath,points)
[_] = closestPoint(refPath,points,searchWindow)

Description

pathPoints = closestPoint(refPath,points) finds the closest point on the reference path to each of the specified (x,y)-positions points.

example

[pathPoints,inWindow] = closestPoint(refPath,points) optionally returns a logical vector inWindow, specifying whether each point for the corresponding xy coordinate in points is projected within the search window..

[_] = closestPoint(refPath,points,searchWindow) optionally accepts a nondecreasing row vector searchWindow, which defines the interval of the path to use to find the closest points.

example

Examples

collapse all

Open Live Script

Generate a reference path from a set of waypoints.

waypoints = [0 0; 50 20; 100 0; 150 10];
refPath = referencePathFrenet(waypoints);

Create a trajectoryGeneratorFrenet object from the reference path.

connector = trajectoryGeneratorFrenet(refPath);

Generate a five-second trajectory between the path origin and a point 30 meters down the path as Frenet states.

initCartState = refPath.SegmentParameters(1,:);
initFrenetState = global2frenet(refPath,initCartState);
termFrenetState = initFrenetState + [30 zeros(1,5)];
frenetTraj = connect(connector,initFrenetState,termFrenetState,5);

Convert the trajectory to the global states.

globalTraj = frenet2global(refPath,frenetTraj.Trajectory);

Display the reference path and the trajectory.

show(refPath);
axis equal
hold on
plot(globalTraj(:,1),globalTraj(:,2),'b')

Specify global points and find the closest points on reference path.

globalPoints = waypoints(2:end,:) + [20 -50];
nearestPathPoint = closestPoint(refPath,globalPoints);

Display the global points and the closest points on reference path.

plot(globalPoints(:,1),globalPoints(:,2),'r*','MarkerSize',10)
plot(nearestPathPoint(:,1),nearestPathPoint(:,2),'b*','MarkerSize',10)

Interpolate between the arc lengths of the first two closest points along the reference path.

arclengths = linspace(nearestPathPoint(1,6),nearestPathPoint(2,6),10);
pathStates = interpolate(refPath,arclengths);

Display the interpolated path points.

plot(pathStates(:,1),pathStates(:,2),'g')
legend(["Waypoints","Reference Path","Trajectory to 30m",...
        "Global Points","Closest Points","Interpolated Path Points"])

Figure contains an axes object. The axes object contains 6 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, Reference Path, Trajectory to 30m, Global Points, Closest Points, Interpolated Path Points.

Open Live Script

Create a self-intersecting reference path.

refPath = referencePathFrenet([0 100 -pi/4; ...
                               50 50 -pi/4;...
                               75 50  pi/2; ...
                               50 50 -3*pi/4; ...
                               0  0  -3*pi/4]);

Show the reference path.

figure
show(refPath); 
title("Closest Points Around Intersection")
xlim([0 125])
ylim([0 125])
hold on

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 2 objects of type line. One or more of the lines displays its values using only markers

Find the arclength at which the intersection occurs.

sIntersection = refPath.SegmentParameters(2,end);

Generate the Frenet states lying just before and after the intersection.

f0 = [sIntersection-20 5 0 10 0 0]; % [S dS ddS L Lp Lpp]
f1 = [sIntersection+20 5 0 -5 0 0]; % [S dS ddS L Lp Lpp]

Calculate the time to travel with constant longitudinal velocity.

T = (f1(1)-f0(1))/f1(2);

Create trajectory generator using the reference path.

generator = trajectoryGeneratorFrenet(refPath);

Generate a trajectory between the points.

[fTraj,gTraj] = connect(generator,f0,f1,T);
pts = gTraj.Trajectory;

Define the plot helper functions.

mergeFcn = @(v1,v2)reshape([v1 v2 nan(size(v1,1),size(v2,2))]',[],1);
plotFcn = @(L1,L2,linespec)plot(mergeFcn(L1(:,1),L2(:,1:min(1,size(L2,2)))),mergeFcn(L1(:,2),L2(:,2:min(2,size(L2,2)))),linespec{:});
plotInterval = @(bounds,nPt,linespec)plotFcn(interpolate(refPath,linspace(bounds(1),bounds(2),nPt)'),[],linespec);

Plot the trajectory.

plotFcn(pts,[],{"k.-"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 3 objects of type line. One or more of the lines displays its values using only markers

Calculate closest point on path to each global state.

closestPts = closestPoint(refPath,pts);

Plot the closest point vectors.

plotFcn(pts,closestPts,{"b"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 4 objects of type line. One or more of the lines displays its values using only markers

Define a window in which to search for closest points.

buffWindow = [f0(1)-5 f1(1)+5];
plotInterval(buffWindow,100,{"Color",[.5 .5 .5],"LineWidth",5});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 5 objects of type line. One or more of the lines displays its values using only markers

Find closest points within window.

closestPtsInWindow = closestPoint(refPath,pts,buffWindow);

Display windowed results.

plotFcn(pts,closestPtsInWindow,{"g","LineWidth",3});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 6 objects of type line. One or more of the lines displays its values using only markers

Find closest points using a window that is too small.

smallWindow = [f0(1)+5 f1(1)-5];
[closestPtsSmall,inWindow] = closestPoint(refPath,pts,smallWindow);

Overlay small window results.

plotInterval(smallWindow,100,{"m","LineWidth",3});
plotFcn(pts(inWindow,:),closestPtsSmall(inWindow,:),{"Color",[.5 1 .5]});
plotFcn(pts(~inWindow,:),closestPtsSmall(~inWindow,:),{"r"});
legend({"Waypoints","ReferencePath","Trajectory","ClosestPoints",...
    "BuffWindow","ClosestInsideBuffWindow","SmallWindow",...
    "ClosestInsideSmallWindow","ClosestOutsideSmallWindow"});

Figure contains an axes object. The axes object with title Closest Points Around Intersection contains 9 objects of type line. One or more of the lines displays its values using only markers These objects represent Waypoints, ReferencePath, Trajectory, ClosestPoints, BuffWindow, ClosestInsideBuffWindow, SmallWindow, ClosestInsideSmallWindow, ClosestOutsideSmallWindow.

Input Arguments

collapse all

Reference path, specified as a referencePathFrenet object.

Global points, specified as a P-by-2 numeric matrix with rows of the form [x y]. P is the number of points. Positions are in meters.

Search window on path to determine closest points, specified as an two-element row vector of arclengths.

Output Arguments

collapse all

Closest points on the reference path , returned as an N-by-6 numeric matrix with rows of form [x y theta kappa dkappa s], where:

  • x y and theta — SE(2) state expressed in global coordinates, with x and y in meters and theta in radians

  • kappa — Curvature, or inverse of the radius, in m-1

  • dkappa — Derivative of curvature with respect to arc length in m-2

  • s — Arc length, or distance along path from path origin, in meters

N is the number of points sampled along the reference path.

Indication whether each point nearest to the corresponding xy coordinate in points, is projected within the search window, returned as an N-element logical column vector, where N is the number of points in points. Points being projected within the search window are true, or false if they lie at the end of a window.

Extended Capabilities

expand all

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

Version History

Introduced in R2020b

See Also

Objects

Functions

Topics