Point classification using polyshape

Hello everyone,

I have code that runs a random route of a robot every time (using the RTT algorithm). The start point and end point are random.

I have a function that converts the continuous track to a discrete track of points.

Here I need your help:

Besides the points of the track, I have a grid of points that represent where the robot can step with its 2 legs.

I want to classify the points into 2, where the line of the continuous robot path will separate them.

Uploading sample code that someone here on the forum helped me in the past.

The problem is that when I change values for the start and end points it doesn't work well. I would be happy to help on this matter. Even if you have advice for a different approach to the solution, I would really love to hear it!! :)

Thank you

CODE:

function [class1_points, class2_points] = MatlabForumHelp(disPath)
figure()
% from matlab forum help:
roadPoints=disPath;
[X,Y]=ndgrid(0:1:25);
p1=polyshape([roadPoints;min(roadPoints(:,1)), max(roadPoints(:,2))],'Simplify',true);
p2=polyshape([roadPoints;max(roadPoints(:,1)), min(roadPoints(:,2))],'Simplify',true);
% if the start point isnt the lowest point in the graph, like statr(4,22)
% goal(22,23)
% p3=polyshape([roadPoints;min(roadPoints(:,1)), max(roadPoints(:,2))],'Simplify',true);
% p4=polyshape([roadPoints;max(roadPoints(:,1)), min(roadPoints(:,2));min(roadPoints(:,1)), min(roadPoints(:,2)) ],'Simplify',true);
class1=isinterior(p1,X(:),Y(:));
class2=isinterior(p2,X(:),Y(:));
noclass=~(class1|class2);
hold on
scatter(X(class1), Y(class1),'g','filled','MarkerEdgeColor','none');
scatter(X(class2), Y(class2),'c','filled','MarkerEdgeColor','none');
scatter(X(noclass),Y(noclass),'MarkerFaceColor',[0.5,0.5,0.5],'MarkerEdgeColor','none')
plot(roadPoints(:,1), roadPoints(:,2),'r','LineWidth',5); 
legend('Class 1 (Left leg)', 'Class 2 (Right leg)','UnClassified','Trajectory','Location','NorthOutside')
hold off
% return the points by clases:
class1_X = X(class1);
class1_Y = Y(class1);
class2_X = X(class2);
class2_Y = Y(class2);
noclass_X = X(noclass);
noclass_Y = Y(noclass);
class1_points = [class1_X,class1_Y];
class2_points = [class2_X,class2_Y];
end

[Yovel]

Someone ?

[Yovel]

Please help

[Benjamin Kraus]

Some tips that may help you get more assistance:

• Format your code using the code block to make it much easier for others to read your code. See this answer for help with formatting your question: https://www.mathworks.com/matlabcentral/answers/7885-tutorial-how-to-format-your-question
• Clarify what you mean by "doesn't work well". Do you get an error message? Does it return invalid results? How are the results invalid?
• Provide runnable code, perhaps by providing an example value for disPath that works well, and another example value that doesn't work well. You can attach MAT files to questions if that helps.

Answer 1

[Vinayak]

Hi Yovel

I understand that you have a distance path generated using RRT (Rapidly-exploring Random Tree) for a robot and you wish to classify the point grid as the left or right side of the robot.

The points I could see in the example figure looks like they are open shapes. I would recommend not to use “polyshape” in such cases as the shape boundaries are defined to be closed.

I created a sample code with custom data (100 points interpolated with 5 key maxima/minima).

I have used basic comparison to classify instead of any fitting.

numPoints = 100;

% Generate a smaller set of random key points
numKeyPoints = 5;
keyX = linspace(0, 25, numKeyPoints); 
keyY = rand(1, numKeyPoints) * 25;  

% Interpolate the key points to create a smooth yPath
xPath = linspace(0, 25, numPoints);
yPath = interp1(keyX, keyY, xPath, 'spline');

% Combine x and y coordinates into a single Nx2 matrix
disPath = [xPath', yPath'];

MatlabForumHelp(disPath);

function [class1_points, class2_points] = MatlabForumHelp(disPath)
[X, Y] = ndgrid(0:1:25);
X = X(:);
Y = Y(:);

% Initialize classification arrays
class1 = false(size(X));
class2 = false(size(X));

% Loop through each segment in the path
for i = 1:(size(disPath, 1) - 1)
    % Calculate the slope and intercept of the segment
    dy = disPath(i+1,2) - disPath(i,2);
    dx = disPath(i+1,1) - disPath(i,1);
    
    % Handle vertical line segments
    if dx == 0
        isRight = X > disPath(i,1);
    else
        % Non-vertical line segment
        slope = dy / dx;
        intercept = disPath(i,2) - slope * disPath(i,1);
        % For each grid point, check if it is above or below the line segment
        isRight = Y > slope * X + intercept;
    end
    
    % Determine the range of X for the current segment
    inRange = (X >= min(disPath(i:i+1,1)) & X <= max(disPath(i:i+1,1)));
    
    % Update classification arrays
    class1 = class1 | (isRight & inRange);
    class2 = class2 | (~isRight & inRange);
end

% Plotting
figure;
hold on;
grid on;
scatter(X(class1), Y(class1), 'g', 'filled');
scatter(X(class2), Y(class2), 'c', 'filled');
plot(disPath(:,1), disPath(:,2), 'r', 'LineWidth', 2);
legend('Class 1 (Above trajectory)', 'Class 2 (Below trajectory)', 'Trajectory', 'Location', 'best');
hold off;
% return the points by clases:
class1_points = [X(class1), Y(class1)];
class2_points = [X(class2), Y(class2)];
end

You may like to fit using a “polyfit”, refer to the following to incorporate the same:

• https://www.mathworks.com/help/matlab/ref/polyfit.html

I hope this helps. Please feel free to reach out for further clarifications.

[Yovel]

Hey!

First of all, thank you very much for the help! Your solution works for me in many cases, and it looks really good :)

I tried to run a random track (a bit complicated) but it doesn't work well..

Do you have an idea how to improve the function?

I was thinking of a possibility that if the start and end points are not on the frame of the plot (right at the edges) then add another start/end point that is vertical to the same start/end point and is on the edges, and that's how you actually get a kind of shape with the plot frame

For example:

The question is if there is any function that knows how to classify the points when the route line crosses the plot into 2 parts.

Thank you so much! I really appreciate your help :)))

[Vinayak]

Hi Yovel,

I am not aware of any built-in function designed for classifying the points as required. However, we can modify the existing code to improve the results. You could adjust the "isRight" assignment based on slope of the segment. Here's a sample modification:

if slope > 0
    isRight = Y > slope * X + intercept;
elseif slope < 0
    isRight = Y < slope * X + intercept;
end

This adjustment takes into account the direction of the slope, but keep in mind that it's not a complete solution. For a more accurate classification, you may need to break down the path into individual segments and classify the points relative to each segment. Additionally, to avoid reclassifying points, you could maintain a grid that tracks whether a point has already been classified.

I hope this helps you make some progress. I'll continue to look into this and will update you if I come across a more robust solution.