PCL点云学习（2）点云拓扑结构
一、 k-d Tree点云分解
1.1 kd树的创建
KdTreeFLANN kdtree; pcl::KdTreeFLANN
1.2 setInputCloud函数
kdtree.setInputCloud(cloud);1、setInputCloud用于设置算法要处理的输入点云数据，使其能够基于这些数据构建空间索引结构，它有的函数定义：void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr());参数1表示需要处理的点云指针，参数2为可选参数用于设置允许指定只使用点云中的部分点来构建 KD 树，参数二为plc中的IndicesPtr类，在构造时通过vector容器传入指定索引值。
IndicesPtr indices(new std::vector{0, 2, 4, 6, 8});
kdtree.setInputCloud(cloud, indices); // 只使用指定索引的点 2、内部工作原理：当调用setInputCloud时，KD 树内部保存指向输入点云的指针，不复制数据，检查点云是否有效（非空、无NaN值等）。
构建后修改：设置点云后修改点云内容会导致不一致
3、注意：
1）多个线程同时调用setInputCloud需要同步
#pragma omp critical
{
kdtree.setInputCloud(cloud);
}2）确保在搜索前调用setInputCloud
pcl::KdTreeFLANN kdtree;
kdtree.nearestKSearch(searchPoint, 5, indices, distances); // 崩溃！ 3）当点云包含点云包含NaN值时，需要预处理移除NaN值。
pcl::removeNaNFromPointCloud(*cloud, *filteredCloud);
kdtree.setInputCloud(filteredCloud);1.3 最近邻搜索
1.3.1 初始化查询点
srand(time(NULL));
PointXYZ searchPoint;
searchPoint.x = 0.5*rand() / (RAND_MAX + 1.0f);
searchPoint.y = 0.5*rand() / (RAND_MAX + 1.0f);
searchPoint.z = 0.5*rand() / (RAND_MAX + 1.0f);
通过rand()函数产生[0,0.5)的随机数作为查询点，搜索距离查询点最近的10个点云,K表示要搜索10个临近点，两个vector容器来保证搜索到的临界点的索引值和查询点到每个临近点的距离平方。
// K = 10 表示搜索10个临近点
// pointIdxNKNSearch 保存搜索到的临近点的索引
// pointNKNSquaredDistance 保存对应临近点的距离的平方
int K = 10;
vector pointIdxNKNSearch(K);
vector pointNKNSquaredDistance(K); 1.3.2 nearestKSearch函数
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
cout << "方法1搜索到的点：" << endl;
for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
{
cout << rabbitCloud->points[pointIdxNKNSearch[i]].x << " "<<
rabbitCloud->points[pointIdxNKNSearch[i]].y << " "<<
rabbitCloud->points[pointIdxNKNSearch[i]].z << " "<<
"距离平方为："<points[pointIdxNKNSearch[i]].x << endl;
}
} nearestKSearch函数用于查找距离给定查询点最近的 K 个邻居点。
1、函数原型
int nearestKSearch(
const PointT &point,
int k,
std::vector &k_indices,
std::vector &k_squared_distances
) const; 2、 参数详解
| 参数 | 类型 | 描述 |
| point | const PointT & | 查询点（要查找邻居的点） |
| k | int | 要查找的最近邻数量 |
| k_indices | std::vector |
输出参数，存储找到的最近邻点的索引 |
| k_squared_distances | std::vector |
输出参数，存储到最近邻点的平方距离 |
nearestKSearch函数返回实际找到的邻居数量和距离度量（可能小于请求的K值）， 返回0表示没有找到任何邻居，默认使用的是欧几里得距离平方，默认按距离从小到大排序，通过函数 setSortedResults(false) 设置禁用排序，setEpsilon()设置近似因子，epsilon=0：精确搜索， epsilon>0：允许近似结果，加入下列代码并行运行程序。
#pragma omp parallel for1.4 半径搜索
1.4.1 初始设置
//初始化一个随机的点，作为第二中查询方式的查询点
PointXYZ searchPoint2;
searchPoint2.x = 0.1*rand() / (RAND_MAX + 1.0f);
searchPoint2.y = 0.1*rand() / (RAND_MAX + 1.0f);
searchPoint2.z = 0.1*rand() / (RAND_MAX + 1.0f);
//方式二：通过指定半径搜索
vector pointIdRadiusSearch;
vector pointRadiusSquaredDistance;
//创建随机[0,0.9)半径
float radius = 0.09f * rand() / (RAND_MAX + 1.0f); 1.4.2 radiusSearch函数
radiusSearch用于查找距离给定查询点一定半径范围内的所有邻居点。
1、 函数原型
int radiusSearch(
const PointT &point,
double radius,
std::vector &indices,
std::vector &squared_distances,
unsigned int max_nn = 0
) const; 2、参数详解
| 参数 | 类型 | 描述 |
| point | const PointT & | 查询点（要查找邻居的点） |
| radius | double | 搜索半径 |
| indices | std::vector |
输出参数，存储找到的邻居点的索引 |
| squared_distances | std::vector |
输出参数，存储到邻居点的平方距离 |
| max_nn | unsigned int | 可选，返回的最大邻居数量限制（0表示无限制） |
radiusSearch函数返回实际找到的邻居数量和距离，返回0表示没有找到任何邻居，返回结果默认不排序（与nearestKSearch不同）。
1.4.3 addLine函数
addLine函数是使用 PCL Visualizer类中的成员函数用于添加线段，
1、函数原型
template
bool addLine(const PointT &pt1, const PointT &pt2, const std::string &id, int viewport = 0); 2、参数详解
| 参数 | 类型 | 描述 |
| pt1 | const PointT & | 线段的起点坐标 |
| pt2 | const PointT & | 线段的终点坐标 |
| id | const std::string & | 线段的唯一标识符 |
| viewport | int | 可选，指定视口编号 |
通过函数setShapeRenderingProperties可以设置线段的颜色和宽度。
// 可选：设置线段颜色 (r,g,b)
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
1.0, 0.0, 0.0, "line1"); // 红色
// 可选：设置线段宽度
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,
3, "line1");
二、octree八叉树
八叉树将3D空间递归地划分为8个等大小的立方体(称为八分体或体素)，若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。体素描述了一个预设的最小单位的正方体，通过pcl的octree库可以对点数据集进行空间分区，下采样和搜索操作。每个八叉树节点有八个子节点或没有子节点，提供了有效的最近邻居搜索API，例如“ 体素邻居搜索”，“ K最近邻居搜索”和“半径搜索邻居”。叶子节点类提供空间“占用率”和“每个体素的点密度”检查功能。
2.1 构建八叉树
2.1.1 OctreePointCloudSearch
// 设置分辨率为128
float resolution = 128.0f;
// resolution该参数描述了octree叶子leaf节点的最小体素尺寸。
pcl::octree::OctreePointCloudSearch octree(resolution); pcl::octree::OctreePointCloudSearch
2.1.2 setInputCloud函数
// 设置输入点云
octree.setInputCloud(cloud);
// 通过点云构建octree
octree.addPointsFromInputCloud();setInputCloud(cloud)用于为八叉树结构设置输入点云数据。它主要用来将点云指针传递给八叉树结构，为后续调用addPointsFromInputCloud()做准备，仅设置引用，不会立即构建八叉树结构，必须addPointsFromInputCloud使用，必须配合addPointsFromInputCloud()才能完成八叉树的构建。
2.2 体素近邻搜索和半径搜索
// Neighbors within voxel search
// 方式一：“体素近邻搜索”,它把查询点所在的体素中其他点的索引作为查询结果返回,
// 结果以点索引向量的形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数
std::vector pointIdxVec;
if (octree.voxelSearch(searchPoint, pointIdxVec)) {
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
for (size_t i = 0; i < pointIdxVec.size(); ++i)
std::cout << " " << cloud->points[pointIdxVec[i]].x
<< " " << cloud->points[pointIdxVec[i]].y
<< " " << cloud->points[pointIdxVec[i]].z << std::endl;
} voxelSearch函数用于查找位于同一个体素内的所有点。这种搜索方式非常高效，因为它直接利用了八叉树的空间划分结构，它直接访问体素内的点，只返回与查询点位于同一体素内的点，结果受八叉树分辨率影响，它有两个参数，searchPoint为查询点，pointIdxVec查询到的点的索引值，存储同一体素内所有点的索引。
2.3 K近邻搜索和半径搜索
// K nearest neighbor search
// 方式二：K 近邻搜索,本例中K被设置成10, "K 近邻搜索”方法把搜索结果写到两个分开的向量中,
// 第一个pointIdxNKNSearch 包含搜索结果〈结果点的索引的向量〉
// 第二个pointNKNSquaredDistance 保存相应的搜索点和近邻之间的距离平方。
int K = 10;
std::vector pointIdxNKNSearch;
std::vector pointNKNSquaredDistance;
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
std::cout << " " << cloud->points[pointIdxNKNSearch[i]].x
<< " " << cloud->points[pointIdxNKNSearch[i]].y
<< " " << cloud->points[pointIdxNKNSearch[i]].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
// 方式三：半径内近邻搜索
// “半径内近邻搜索”原理和“K 近邻搜索”类似,它的搜索结果被写入两个分开的向量中,
// 这两个向量分别存储结果点的索引和对应的距离平方
std::vector pointIdxRadiusSearch;
std::vector pointRadiusSquaredDistance;
float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
std::cout << " " << cloud->points[pointIdxRadiusSearch[i]].x
<< " " << cloud->points[pointIdxRadiusSearch[i]].y
<< " " << cloud->points[pointIdxRadiusSearch[i]].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
} 八叉树的K近邻搜索及半径搜索与KD树的搜索类似，只是创建的搜索树不同。
#include
#include
#include
#include
#include
#include
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
using namespace std;
using namespace pcl;
int main(int argc, char** argv)
{
PointCloud::Ptr cloud(new PointCloud);
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
srand(time(NULL));
for (size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
float resolution = 152.0f;
octree::OctreePointCloudSearch octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);
vector PointIdxVec;
if (octree.voxelSearch(searchPoint, PointIdxVec))
{
cout << "体素搜索结果：" << endl;
for (size_t i = 0; ipoints[PointIdxVec[i]].x << " " <<
cloud->points[PointIdxVec[i]].y << " " <<
cloud->points[PointIdxVec[i]].z << endl;
}
}
int K = 10;
vector PointIdxNKNSearch(10);
vector PointNKNSquaredDistance(10);
if (octree.nearestKSearch(searchPoint, K, PointIdxNKNSearch, PointNKNSquaredDistance) > 0)
{
cout << "最近邻搜索：" << endl;
for (size_t i = 0; ipoints[PointIdxNKNSearch[i]].x << " " <<
cloud->points[PointIdxNKNSearch[i]].y << " " <<
cloud->points[PointIdxNKNSearch[i]].z << " " <<
"距离平方：" << PointNKNSquaredDistance[i] << endl;
}
}
vector PointIdxRadiusSearch;
vector PointRadiusSquaredDistance;
float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
if (octree.radiusSearch(searchPoint, radius, PointIdxRadiusSearch, PointRadiusSquaredDistance, 10) > 0)
{
cout << "半径搜索：" << endl;
for (size_t i = 0; ipoints[PointIdxRadiusSearch[i]].x << " " <<
cloud->points[PointIdxRadiusSearch[i]].y << " " <<
cloud->points[PointIdxRadiusSearch[i]].z << " " <<
"距离平方：" << PointRadiusSquaredDistance[i] << endl;
}
}
visualization::PCLVisualizer viewer("PCL viewer");
viewer.setBackgroundColor(0.7, 0.6, 0.4);
visualization::PointCloudColorHandlerCustom cloudColor(cloud, 50, 230, 50);
viewer.addPointCloud(cloud, "cloud", 0);
PointXYZ origin;
origin.x = 0.0;
origin.y = 0.0;
origin.z = 0.0;
viewer.addLine(origin, searchPoint, "line", 0);
viewer.addSphere(searchPoint,radius,"sphere",0);
viewer.addCoordinateSystem(200);
viewer.spin();
return -1;
}