findNearestNeighbors

Find nearest neighbors of query points in point cloud

Syntax

[indices,dists] = findNearestNeighbors(ptCloud,points,K)

[indices,dists] = findNearestNeighbors(ptCloud,points,K,camMatrix)

[indices,dists] = findNearestNeighbors(___,Name,Value)

Description

[indices,dists] = findNearestNeighbors(ptCloud,points,K) returns the indices for the K-nearest neighbors of one or more query points in the input point cloud. ptCloud can be an unorganized or organized point cloud. The K-nearest neighbors of the query points are computed by using the Kd-tree based search algorithm.

[indices,dists] = findNearestNeighbors(ptCloud,points,K,camMatrix) returns the K-nearest neighbors of one or more query points in the input point cloud. The input point cloud is an organized point cloud generated by a depth camera. The K-nearest neighbors of the query points are determined using fast approximate K-nearest neighbor search algorithm.

The function uses the camera projection matrix camMatrix to know the relationship between adjacent points and hence, speeds up the nearest neighbor search. However, the results have lower accuracy as compared to the Kd-tree based approach.

Note

• This syntax only supports organized point cloud data produced by RGB-D sensors.

• You can use estimateCameraMatrix to estimate camera projection matrix for the given point cloud data.

[indices,dists] = findNearestNeighbors(___,Name,Value) specifies options using one or more name-value arguments in addition to the input arguments in the preceding syntaxes.

Examples

Find K-Nearest Neighbors in Point Cloud

Load a set of 3-D coordinate points into the workspace.

load("xyzPoints.mat");

Create a point cloud object.

ptCloud = pointCloud(xyzPoints);

Specify two query points and the number of nearest neighbors to identify.

points = [0 0 0; 0 0 3];
K = 70;

Get the indices and the distances of the K nearest neighbors for each query point.

[indices,dists] = findNearestNeighbors(ptCloud,points,K);

Display the point cloud. Plot the query points and their nearest neighbors.

figure
pcshow(ptCloud)
hold on
plot3(points(:,1),points(:,1),points(:,3),"*r")
plot3(ptCloud.Location(indices,1), ...
    ptCloud.Location(indices,2), ...
    ptCloud.Location(indices,3),"*m")
legend("Point Cloud","Query Points", ...
    "Nearest Neighbors", ...
    Location="southoutside", ...
    Color=[1 1 1])
hold off

Find K-Nearest Neighbors in Organized Point Cloud

Find the K-nearest neighbors of a query point in the organized point cloud data by using the camera projection matrix. Compute the camera projection matrix from sampled point cloud data points and their corresponding image point coordinates.

Load an organized point cloud data into the workspace. The point cloud is generated by using the Kinect depth sensor.

ld = load('object3d.mat');
ptCloud = ld.ptCloud;

Specify the step size for sampling the point cloud data.

stepSize = 100;

Sample the input point cloud and store the sampled 3-D point coordinates as a point cloud object.

indices = 1:stepSize:ptCloud.Count;
tempPtCloud = select(ptCloud,indices);

Remove invalid points from the sampled point cloud.

[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);

Define the 3-D world point coordinates of input point cloud.

worldPoints = tempPtCloud.Location;

Find the 2-D image coordinates corresponding to the 3-D point coordinates of input point cloud.

[Y,X] = ind2sub([size(ptCloud.Location,1),size(ptCloud.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

Estimate camera projection matrix from the image and the world point coordinates.

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);

Specify a query point and the number of nearest neighbors to be identified.

point = [0.4 0.3 0.2];
K = 1000;

Find the indices and distances of K nearest neighboring points by using the camera projection matrix. Use the point cloud method select to get the point cloud data of nearest neighbors.

[indices,dists] = findNearestNeighbors(ptCloud,point,K,camMatrix);
ptCloudB = select(ptCloud,indices);

Display the point cloud and the nearest neighbors of the query point.

figure
pcshow(ptCloud)
hold on
plot3(ptCloudB.Location(:,1),ptCloudB.Location(:,2),ptCloudB.Location(:,3),'*')
legend('Point Cloud','Nearest Neighbors','Location','southoutside','Color',[1 1 1])
hold off

Input Arguments

ptCloud — Point cloud

pointCloud object

Point cloud, specified as a pointCloud object.

Note

The function supports organized point cloud data generated only from RGB-D sensors.

points — Query points

1-by-3 vector | N-by-3 matrix

Query points, specified as a 1-by-3 vector or N-by-3 matrix.

• To find the nearest neighbors of a single query point, specify its coordinates as a 1-by-3 vector of the form [x y z].

• To find the nearest neighbors of multiple query points, specify them as an N-by-3 matrix. N is the number of query points. Each row of the matrix is of the form [x y z], representing the coordinates of a query point. (since R2025a)

K — Number of nearest neighbors

positive integer

Number of nearest neighbors, specified as a positive integer.

camMatrix — Camera projection matrix

4-by-3 matrix

Camera projection matrix, specified as a 4-by-3 matrix that maps 3-D world points to 2-D image points. You can compute the camMatrix by using the estimateCameraMatrix function.

Name-Value Arguments

Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose Name in quotes.

Example: findNearestNeighbors(ptCloud,point,k,'Sort',true)

Sort — Sort indices

false (default) | true

Sort indices, specified as a comma-separated pair of 'Sort' and a logical scalar. When you set Sort to true, the returned indices are sorted in the ascending order based on the distance from a query point. To turn off sorting, set Sort to false.

MaxLeafChecks — Number of leaf nodes to check

Inf (default) | integer

Number of leaf nodes to check, specified as a comma-separated pair consisting of 'MaxLeafChecks' and an integer. When you set this value to Inf, the entire tree is searched. When the entire tree is searched, it produces exact search results. Increasing the number of leaf nodes to check increases accuracy, but reduces efficiency.

Note

The name-value argument 'MaxLeafChecks' is valid only with Kd-tree based search method.

Output Arguments

indices — Indices of stored points

K-by-1 vector | K-by-N matrix

Indices of stored points, returned as a K-by-1 vector or K-by-N matrix. K is the number of nearest neighbors.

• For a single query point, the function returns linear indices of the K nearest neighbors as a K-by-1 vector.

• For multiple query points, the function returns a K-by-N matrix. K is the number of nearest neighbors and N is the number of query points. Each column of the matrix contains the linear indices of the K nearest neighbors of the query point specified by the corresponding row of the points input argument. (since R2025a)

dists — Distances between query points and their nearest neighbors

K-by-1 vector | K-by-N matrix

Distances between query points and their nearest neighbors, returned as a K-by-1 vector or K-by-N matrix. K is the number of nearest neighbors.

• For a single query point, the function returns a K-by-1 vector. The vector contains the Euclidean distances between the query point and its K nearest neighbors.

• For multiple query points, the function returns a K-by-N matrix. K is the number of nearest neighbors and N is the number of query points. Each column of the matrix contains the Euclidean distances to the K nearest neighbors of a query point. This query point is specified by the corresponding row in the points input argument. (since R2025a)

References

[1] Muja, M. and David G. Lowe. "Fast Approximate Nearest Neighbors with Automatic Algorithm Configuration". In VISAPP International Conference on Computer Vision Theory and Applications. 2009. pp. 331–340.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• For code generation in non-host platforms, the value for 'MaxLeafChecks' must be set to the default value Inf. If you specify values other than Inf, the function generates a warning and automatically assigns the default value for 'MaxLeafChecks'.

GPU Code Generation

Generate CUDA® code for NVIDIA® GPUs using GPU Coder™.

Usage notes and limitations:

• For GPU code generation, the camMatrix input argument is ignored.

• For GPU code generation, the 'MaxLeafChecks' name-value pair option is ignored.

Version History

Introduced in R2015a

R2025a: Find nearest neighbors of multiple query points in point cloud

Starting in R2025a, the points input argument value supports an N-by-3 matrix. Using an N-by-3 matrix enables you to simultaneously find the nearest neighbors of multiple query points in the input point cloud. To find the nearest neighbors of a single query point, specify the points argument as a 1-by-3 vector.