@[TOC]PCL中K-Dtree与八叉树模块的学习
学习背景
参考书籍:《点云库PCL从入门到精通》以及官方代码PCL官方代码链接,PCL版本为1.8.1,CMake版本为3.10.2
学习内容
如何使用PCL中的八叉树空间变化检测器来检测新添加到点云中的点。需要注意的是,这个方法只能检测点云中新增加的点,而无法检测减少的点。
源代码及所用函数
源代码
#include<iostream>//c++标准输入输出流头文件
#include<vector>//C++标准容器
#include<ctime>//时间相关函数和头文件
#include<pcl/point_cloud.h>点云类定义头文件
#include<pcl/octree/octree_pointcloud_changedetector.h>//提供使用八叉树数据结构来检测点云变化的方法
int main()
{
srand((unsigned int)time(NULL));
float Resolution = 32.0f;//八叉树分辨率即体素大小
//初始化空间变化检测对象
/* ------------------------ 首先,将第一个点云作为输入,构建一个八叉树结构。 ------------------------ */
/* --------------------- 然后,将第二个点云作为输入,将其点添加到之前构建的八叉树中。 --------------------- */
/* -------------------- 通过比较两个点云对应的八叉树结构,识别出第二个点云中新增加的点。 -------------------- */
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> Octree(Resolution);
//创建点云实例,并用随机点填充
pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud(new pcl::PointCloud<pcl::PointXYZ>);
Cloud->width = 128;
Cloud->height = 1;
Cloud->points.resize(Cloud->width*Cloud->height);
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);
}
//添加到点云八叉树,构建八叉树
Octree.setInputCloud(Cloud);//设置输入点云
Octree.addPointsFromInputCloud();//从输入点云构建八叉树
//交换八叉树缓存,但是Cloud对应的八叉树结构仍保存在内存中
Octree.switchBuffers();
//实例化第二个点云对象
pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud2(new pcl::PointCloud<pcl::PointXYZ>);
Cloud2->width = 128;
Cloud2->height = 1;
Cloud2->points.resize(Cloud2->width*Cloud2->height);
for (size_t i = 0; i < Cloud2->points.size(); i++)
{
Cloud2->points[i].x = 1024.0f*rand()/(RAND_MAX+1.0f);
Cloud2->points[i].y = 1024.0f*rand()/(RAND_MAX+1.0f);
Cloud2->points[i].z = 1024.0f*rand()/(RAND_MAX+1.0f);
}
//添加Cloud2到八叉树
Octree.setInputCloud(Cloud2);
Octree.addPointsFromInputCloud();
//探测Cloud2新增加的点集。注:只能探测增加的探测不了减少的
std::vector<int> NewPointIdxVector;//存储新点的索引向量
Octree.getPointIndicesFromNewVoxels(NewPointIdxVector);
//输出点坐标
std::cout<<"getPointIndicesFromNewVoxels函数的输出:"<<std::endl;
for(int i = 0;i < NewPointIdxVector.size(); i++)
{
std::cout<<i<<"#索引"<<NewPointIdxVector[i]<<"点:"<<Cloud2->points[NewPointIdxVector[i]].x<<" "<<Cloud2->points[NewPointIdxVector[i]].y
<<" "<<Cloud2->points[NewPointIdxVector[i]].z<<std::endl;
}
}
CMakeLists.txt
cmake_minimum_required(VERSION 3.1 FATAL_ERROR)#指定CMake的最低版本要求为3.1
project(octree_change_detection)#设置项目名称
set(CMAKE_CXX_STANDARD 11)#设置C++编译器版本为C++11
find_package(PCL 1.7 REQUIRED)#查找PCL库,要求版本为1.7或更高。
include_directories(${PCL_INCLUDE_DIRS})#将PCL库的头文件目录添加到包含路径中
link_directories(${PCL_LIBRARY_DIRS})#将PCL库的库文件目录添加到链接器搜索路径中。
add_definitions(${PCL_DEFINITIONS})#添加PCL库的编译器定义
add_executable (octree_change_detection octree_change_detection.cpp)#需要修改
target_link_libraries (octree_change_detection ${PCL_LIBRARIES})#将PCL库链接到可执行文件目标。
运行结果
函数
头文件<pcl/octree/octree_pointcloud_changedetector.h>
头文件<pcl/octree/octree_pointcloud_changedetector.h>
提供了一个称为 OctreePointCloudChangeDetector
的类,用于检测两个点云之间的变化。
在本文中主要使用该头文件的OctreePointCloudChangeDetector
类,代码中用到了以下函数
setInputCloud():设置输入点云。
addPointsFromInputCloud():从输入点云构建八叉树。
switchBuffers():交换八叉树的缓存。
getPointIndicesFromNewVoxels():获取新增加点的索引
-
switchBuffers()
保留上一次点云数据的八叉树结构,同时准备好新的八叉树来接收新的点云数据。这样就可以在新旧两个时间步的点云数据之间进行对比和变化检测。
在代码中,先将初始的 Cloud 点云添加到八叉树中,构建了第一个"源"八叉树。然后调用switchBuffers()
函数,将第一个八叉树设置为"源",并清空"目标"八叉树,为添加新的 Cloud2 点云做准备。这样就可以在后续使用getPointIndicesFromNewVoxels()
函数来检测 Cloud2 相对于 Cloud 的变化。 -
void getPointIndicesFromNewVoxels (std::vector<int> &newPointIdxVector) //newPointIdxVector 是一个引用参数,用于存储新点的索引。
getPointIndicesFromNewVoxels()
该函数的工作流程如下:遍历"目标"八叉树中的所有节点。对于每个节点,检查它是否在"源"八叉树中存在对应的节点。如果在"源"八叉树中不存在对应节点,则说明该节点是新添加的,属于新体素。将该新体素中包含的所有点的索引添加到 newPointIdxVector 中。 -
还有一些代码中没有用到的函数:检测新增加的点:通过
getPointIndicesFromNewVoxels
函数,可以获得在新的时间步中新增加的点的索引;检测移除的点:通过getPointIndicesFromDiskVoxels
函数,可以获得在新的时间步中被移除的点的索引;检测位置改变的点:通过getPointIndicesFromAllocatedVoxels
函数,可以获得在新的时间步中位置发生改变的点的索引。