MATLAB点云处理：kd树近邻搜索（K近邻 | 半径R近邻）

Posted 2021-08-11 借我十斤肉

文章目录

• • 0 Object Functions（对象函数）
  • 1 findNearestNeighbors （K近邻搜索）
  • • 1.1 重载函数1 ---- [indices,dists] = findNearestNeighbors(ptCloud,point,K)
    • 1.2 重载函数2 ---- [indices,dists] = findNearestNeighbors(ptCloud,point,K,camMatrix)
    • 1.3 重载函数3 ---- findNearestNeighbors(___,Name,Value)
  • 2 findNeighborsInRadius （半径近邻搜索）
  • • 1.1重载函数1 ---- findNeighborsInRadius(ptCloud,point,radius)
    • 2.2 重载函数2 ---- findNeighborsInRadius(ptCloud,point,radius,camMatrix)
    • 2.3 重载函数3 ---- findNeighborsInRadius(___,Name,Value)
  • 3 输入输出参数详解

0 Object Functions（对象函数）

MATLAB点云处理工具箱提供了 6 个对象函数，如下表

函数名	功能 | 作用
findNearestNeighbors	K近邻搜索
findNeighborsInRadius	半径R邻域搜索
findPointsInROI	在点云中查找感兴趣区域内的点
removeInvalidPoints	从点云中删除无效点
select	在点云中选择点
copy	复制句柄对象数组

本文主要介绍前两个对象函数

1 findNearestNeighbors （K近邻搜索）

功能： 近邻点查询

语法：

三种重载函数

[indices,dists] = findNearestNeighbors(ptCloud,point,K);
[indices,dists] = findNearestNeighbors(ptCloud,point,K,camMatrix);
[indices,dists] = findNearestNeighbors(___,Name,Value);

1.1 重载函数1 ---- [indices,dists] = findNearestNeighbors(ptCloud,point,K)

使用基于Kd树的搜索算法计算查询点的K近邻，返回输入点云中查询点的K近邻的索引和距离。ptCloud可以是无组织的或有组织的点云。

示例1： 在点云中寻找K近邻

代码：

clc;
clear;
ptCloud = pcread('bunny_red.pcd');	%创建点云对象ptCloud,读取PCD点云
queryPoint = [-0.02,0.1,0.05];	%设置查询点坐标
K = 100;						%设置近邻点个数
[indices,dists] = findNearestNeighbors(ptCloud,queryPoint,K);	%执行K近邻搜索，获取近邻点索引和距离
figure
pcshow(ptCloud)													%显示点云
hold on
plot3(queryPoint(1),queryPoint(2),queryPoint(3),'*r')			%绘制查询点
plot3(ptCloud.Location(indices,1),ptCloud.Location(indices,2),ptCloud.Location(indices,3),'*g')	%绘制近邻点
legend('Point Cloud','Query Point','Nearest Neighbors','Location','southoutside')				%添加图例
hold off

显示结果： 红色为原始点云，※ 点为查询点，绿色为近邻点

1.2 重载函数2 ---- [indices,dists] = findNearestNeighbors(ptCloud,point,K,camMatrix)

返回输入点云中查询点的K近邻。输入点云是由深度摄影机生成的有组织点云，利用快速K近邻搜索算法确定查询点的K近邻。

该函数使用摄像机投影矩阵cammartrix来确定相邻点之间的关系，从而加快最近邻搜索的速度。然而，与基于Kd树的方法相比，结果的准确性较低。

注意：

• 此重载函数仅支持由RGB-D传感器生成的有组织点云数据。
• 可以使用estimateCameraMatrix估计给定点云数据的摄影机投影矩阵。

示例2： 在有组织的点云中寻找K近邻

利用摄像机投影矩阵，在有组织的点云数据中寻找查询点的K近邻。从采样点云数据点及其对应的像点坐标计算摄像机投影矩阵。

使用Kinect深度传感器生成点云。

暂时没有找到由深度摄影机生成的有组织点云数据，下面只进行代码展示。

代码：

clc;
clear;
ptCloud = pcread('1.pcd');	%创建点云对象ptCloud,读取PCD点云
stepSize = 100;					%指定点云采样步长

%对输入点云进行采样，并将采样的三维点坐标存储为点云对象
indices = 1:stepSize:ptCloud.Count;
tempPtCloud = select(ptCloud,indices);

[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);	%从采样点云中删除无效点
worldPoints = tempPtCloud.Location;								%定义输入点云的三维世界点坐标

%找到输入点云的三维点坐标对应的二维图像坐标
[Y,X] = ind2sub([size(ptCloud.Location,1),size(ptCloud.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);		%根据图像和世界点坐标估计摄像机投影矩阵

%指定查询点和要标识的最近邻点数K
queryPoint = [0.4 0.3 0.2];
K = 20;

%利用摄像机投影矩阵求K个最近邻点的索引和距离。使用点云方法选择获取最近邻点云
[indices,dists] = findNearestNeighbors(ptCloud,queryPoint,K,camMatrix);
ptCloudB = select(ptCloud,indices);

%显示点云和查询点的最近邻居
figure
pcshow(ptCloud)
hold on
pcshow(ptCloudB.Location,'ob')
legend('Point Cloud','Nearest Neighbors','Location','southoutside','Color',[1 1 1])
hold off

1.3 重载函数3 ---- findNearestNeighbors(___,Name,Value)

除了前面语法中的输入参数外，还使用一个或多个名称值参数(Name-Value Pair Arguments)指定选项。

名称值参数(Name-Value Pair Arguments)

指定可选的名称、值参数对，用逗号分隔。Name是参数Name，Value是对应的值。名称必须出现在引号内。可以按任意顺序指定多个名称和值对参数，如Name1、Value1、…、NameN、ValueN。

2 findNeighborsInRadius （半径近邻搜索）

功能： 在半径内搜索点的近邻点

语法：

三种重载函数

[indices,dists] = findNeighborsInRadius(ptCloud,point,radius)
[indices,dists] = findNeighborsInRadius(ptCloud,point,radius,camMatrix)
[indices,dists] = findNeighborsInRadius(___,Name,Value)

1.1重载函数1 ---- findNeighborsInRadius(ptCloud,point,radius)

返回输入点云中查询点半径内的邻居索引。
ptCloud可以是无组织的或有组织的点云。
使用基于Kd树的搜索算法计算查询点半径内的邻居。

示例1： 搜索半径内的近邻点

代码：

clc;
clear;
ptCloud = pcread('bunny_red.pcd');	%创建点云对象ptCloud,读取PCD点云
queryPoint = [-0.02,0.1,0.05];	%设置查询点坐标
radius = 0.03;					%设置搜索半径
[indices,dists] = findNeighborsInRadius(ptCloud,queryPoint,radius);	%执行半径R近邻搜索，获取近邻点索引和距离
%ptCloudB = select(ptCloud,indices);	%获取R邻域点

%显示点云
figure
pcshow(ptCloud)													
hold on
plot3(queryPoint(1),queryPoint(2),queryPoint(3),'*r')			%绘制查询点
plot3(ptCloud.Location(indices,1),ptCloud.Location(indices,2),ptCloud.Location(indices,3),'*b')	%绘制近邻点
legend('Point Cloud','Query Point','Radial Neighbors','Location','southoutside')				%添加图例
hold off

显示结果：　红色为原始点云，※ 点为查询点，蓝色为近邻点

2.2 重载函数2 ---- findNeighborsInRadius(ptCloud,point,radius,camMatrix)

利用摄像机投影矩阵在有组织的点云数据中寻找查询点的径向邻域。从采样点云数据点及其对应的像点坐标计算摄像机投影矩阵。

使用Kinect深度传感器生成点云。

暂时没有找到由深度摄影机生成的有组织点云数据，下面只进行代码展示。

代码：

ld = load('object3d.mat');
ptCloud = ld.ptCloud;
stepSize = 100;	%指定采样点云数据的步长

%对输入点云进行采样，并将采样的三维点坐标存储为点云对象
indices = 1:stepSize:ptCloud.Count;
tempPtCloud = select(ptCloud,indices);

%找到输入点云的三维点坐标对应的二维图像坐标
[Y,X] = ind2sub([size(ptCloud.Location,1),size(ptCloud.Location,2)],indices);
imagePoints = [X(validIndices)' Y(validIndices)'];

camMatrix = estimateCameraMatrix(imagePoints,worldPoints);		%根据图像和世界点坐标估计摄像机投影矩阵。
[tempPtCloud,validIndices] = removeInvalidPoints(tempPtCloud);	%从采样点云中删除无效点
worldPoints = tempPtCloud.Location;								%定义输入点云的三维世界点坐标

%指定查询点和要在其中标识邻居的半径
point = [0.4 0.3 0.2];
radius = 0.05;

[indices,dists] = findNeighborsInRadius(ptCloud,point,radius);	%获取位于指定半径内的点的索引和距离
ptCloudB = select(ptCloud,indices);								%获取R邻域的点云数据

%显示点云，绘制查询点和相应的半径邻域点。
figure
pcshow(ptCloud);
hold on;
pcshow(ptCloudB.Location, 'b');
legend('Point Cloud','Radial Neighbors','Location','southoutside');
hold off;

2.3 重载函数3 ---- findNeighborsInRadius(___,Name,Value)

除了前面语法中的输入参数外，还使用一个或多个名称-值对参数指定选项。

3 输入输出参数详解

输入参数：

• queryPoint： 指定为形式为[x，y，z]的三元素向量。
• K： 最近邻数，为正整数。
• radius： 搜索半径，指定为标量。该函数用于查找输入点云中查询点周围指定半径内的近邻点。
• camMatrix： 图像矩阵，指定为将三维世界点映射到二维图像点的4x3矩阵，可以使用estimateCameraMatrix函数计算camMatrix。
• sort： 排序索引，指定为逗号分隔的“Sort”和逻辑标量对。如果将Sort设置为true，则返回的索引将根据与查询点的距离按升序排序。要关闭排序，请将Sort设置为false（默认值）。
• MaxLeafChecks： 要检查的叶节点数，指定为由“MaxLeafChecks”和整数组成的逗号分隔对。将此值设置为Inf时，将搜索整个树。当搜索整个树时，它会产生精确的搜索结果。增加要检查的叶节点数可以提高准确性，但会降低效率。

注意： 名称值参数MaxLeafChecks仅对基于Kd树的搜索方法有效。

输出参数：

• indices： 存储点的索引，作为列向量返回。向量包含存储在点云中的最近邻的K个线性索引。
• dists： 到查询点的距离，作为列向量返回。向量包含查询点与其最近邻居之间的欧氏距离。