Plot points at a specific height from an existing 3D plot/data set

Question

I have a robot leg (3 joints) and I've plotted the maximum range of the end of the leg in a 3D plot using convhull. Now, I want to be able to specify a particular height within that entire workspace and create a 2D plot with X and Y coordinates of all the possible points within the workspace at that height (3D plot works just as well but might be more difficult).

EDIT: Forgot to mention that the data is stored in a 3 by 1088 matrix with coordinates for each row. Also, since the Z coordinate might not match exactly the value I'm looking for, the next closest point works just as well.

Thank you.

"next closest" - as in the closest Z value for each X and Y, or Z values within some threshold of Z height? — nkjt

rayryeng rayryeng · Accepted Answer · 2015-01-05T19:10:42

If I am interpreting your question correctly, you wish to isolate out points in your matrix that match a particular z coordinate. Failing an exact match, you wish to find the closest z coordinate to your desired query. Also, since your data is stored in a 3 x 1088 matrix, you probably meant to say that each column is a coordinate, not each row.

I'm going to assume that the first, second and third rows denote the x, y and z coordinates of the movement of your robot. The first step would simply be to find the minimum distance between your desired z coordinate with all of those found in the matrix. Once you find that matching coordinate, we simply need to find those z coordinates in your matrix that match, isolate those out and plot only the x and y coordinates. Therefore, assuming your matrix of points is stored in data, and your query z coordinate is stored in queryZ, do something like this:

queryZ = 2.0; %// 1
zPoints = data(3,:); %// 2
[~,loc] = min(abs(queryZ - zPoints)); %// 3
minZ = zPoints(loc); %// 4
ind = data(3,:) == minZ; %// 5
xPoints = data(1,ind); %// 6
yPoints = data(2,ind); %// 7
plot(xPoints, yPoints, 'b.'); %// 8
title(['Points found for ' num2str(minZ)]); %// 9

The first line of code declares a desired z coordinate for you to search for. The next two lines extract out the z coordinates for your data, and then uses min and searches through the z coordinates and finds the location that is closest to your desired z coordinate. We use this location to extract out what the closest z coordinate is (line 4), then find those locations in your data matrix that share this same z coordinate (line 5).

Lastly, these locations are used to filter out the x and y coordinates of your data matrix (lines 6 and 7) and we then plot these points in blue and with dot markers (line 8). As a bonus, we place a title on the plot that shows you what the actual z coordinate was that matched to your query (line 9).

Edit

Given your inquiry in your comments, you would like to find multiple values of z within a particular tolerance for each value. The easiest way would be to do this in a for loop. There are certainly other ways to do this vectorized, but I won't invest the time into doing so. As such, you would have to slightly modify the above formulation and perform the following steps:

For each query point queryZ:
- Find the closest point in your data
- Search for all z points within a tolerance tol of this point
- Add these points to a list
Repeat Step #1 for all query points desired
Plot all of these points for display

As such, the code would look something like this:

%// Step #1
queryZ = [2.0 1.0 -1.0 -2.0]; %// Define desired z points
tol = 0.001; %// Define tolerance here
zPoints = data(3,:); %// Extract out z points

%// Step #2
loc = false(numel(zPoints));
for idx = 1 : numel(queryZ)
    z = queryZ(idx); %// Get query point
    [~,minInd] = min(abs(z - zPoints)); %// Find closest point to query
    minZ = zPoints(minInd);
    loc(abs(minZ - zPoints) < tol) = true; %// Find indices within tolerance wrt closest point
                                           %// Set to true
end

%// Step #3
xPoints = data(1,ind); %// 6
yPoints = data(2,ind); %// 7
plot(xPoints, yPoints, 'b.'); %// 8

The first step is self-explanatory. We first define a bunch of z coordinates that you are seeking, define a tolerance for similarity and extract out the z coordinates of your data. Next, for each point in our query set, we find the closest z coordinate to your data, and then with respect to this closest coordinate, we search for points that are within a specified tolerance of this matched point. We use these locations (not the coordinates) to mark into a logical array where true means that this point has met the criteria of being matched and false otherwise. In the end, any locations in the logical array that are set to true means that the corresponding point located at this index has met the criteria to be matched for at least one of the points in your query.

Finally, we use this logical array to index into our data, grab all of the valid points and plot them.

Hope this helps!

Plot points at a specific height from an existing 3D plot/data set

1 Answers

Edit