SLAM本质剖析-PCL
0. 前言
在深入剖析了Ceres、Eigen、Sophus、G2O后，以V-SLAM为代表的计算方式基本已经全部讲完，但是就L-SLAM而言我们还差一块，那就是PCL、GTSAM点云计算部分我们还没有详细的去写，正好就这个时间，我想把这块坑给填完，来形成一整个系列，方便自己回顾以及后面的人一起学习。
1. PCL系统认知
PCL（Point Cloud Library） 是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。如果说OpenCV是2D信息获取与处理的结晶，那么PCL就在3D信息获取与处理上具有同等地位，PCL是BSD授权方式，可以免费进行商业和学术应用。
如图PCL架构图所示，对于3D点云处理来说，PCL完全是一个的模块化的现代C++模板库。其基于以下第三方库：Boost、Eigen、FLANN、VTK、CUDA、OpenNI、Qhull，实现点云相关的获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。
而作为PCL的输入数据格式，一般我们默认使用.pcd的格式输出
pcd文件数据举例
VERSION .7 # 版本号
FIELDS x y z rgb # 指定一个点可以有的每一个维度和字段的名字
SIZE 4 4 4 4 # 用字节数指定每一个维度的大小。例如：
TYPE F FFF # 用一个字符指定每一个维度的类型 int uint float
COUNT 1 1 1 1 # 指定每一个维度包含的元素数目
WIDTH 640 # 像图像一样的有序结构，有640行和480列，
HEIGHT 480 # 这样该数据集中共有640*480=307200个点
VIEWPOINT 0 0 0 1 0 0 0 # 指定数据集中点云的获取视点 视点信息被指定为平移（txtytz）+四元数（qwqxqyqz）
POINTS 307200 # 指定点云中点的总数。从0.7版本开始，该字段就有点多余了
DATA ascii # 指定存储点云数据的数据类型。支持两种数据类型：ascii和二进制
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06
文件头格式
每一个PCD文件包含一个文件头，它确定和声明文件中存储的点云数据的某种特性。PCD文件头必须用ASCII码来编码。PCD文件中指定的每一个文件头字段以及ascii点数据都用一个新行（\n）分开了，从0.7版本开始，PCD文件头包含下面的字段：
-
VERSION –指定PCD文件版本
-
FIELDS –指定一个点可以有的每一个维度和字段的名字。例如：
FIELDS x y z # XYZ data
FIELDS x y z rgb # XYZ + colors
FIELDS x y z normal_xnormal_y normal_z # XYZ + surface normals
FIELDS j1 j2 j3 # moment invariants
… -
SIZE –用字节数指定每一个维度的大小。例如：
unsigned char/char has 1 byte
unsigned short/short has 2 bytes
unsignedint/int/float has 4 bytes
double has 8 bytes -
TYPE –用一个字符指定每一个维度的类型。现在被接受的类型有：
I – 表示有符号类型int8（char）、int16（short）和int32（int）；
U – 表示无符号类型uint8（unsigned char）、uint16（unsigned short）和uint32（unsigned int）；
F – 表示浮点类型。 -
COUNT –指定每一个维度包含的元素数目。例如，x这个数据通常有一个元素，但是像VFH这样的特征描述子就有308个。实际上这是在给每一点引入n维直方图描述符的方法，把它们当做单个的连续存储块。默认情况下，如果没有COUNT，所有维度的数目被设置成1。
-
WIDTH –用点的数量表示点云数据集的宽度。根据是有序点云还是无序点云，WIDTH有两层解释：
1)它能确定无序数据集的点云中点的个数（和下面的POINTS一样）；
2)它能确定有序点云数据集的宽度（一行中点的数目）。注意：有序点云数据集，意味着点云是类似于图像（或者矩阵）的结构，数据分为行和列。这种点云的实例包括立体摄像机和时间飞行摄像机生成的数据。有序数据集的优势在于，预先了解相邻点（和像素点类似）的关系，邻域操作更加高效，这样就加速了计算并降低了PCL中某些算法的成本。
例如：
WIDTH 640 # 每行有640个点 -
HEIGHT –用点的数目表示点云数据集的高度。类似于WIDTH ，HEIGHT也有两层解释：
1)它表示有序点云数据集的高度（行的总数）；
2)对于无序数据集它被设置成1（被用来检查一个数据集是有序还是无序）。有序点云例子： WIDTH 640 # 像图像一样的有序结构，有640行和480列， HEIGHT 480 #
这样该数据集中共有640*480=307200个点 无序点云例子： WIDTH 307200 HEIGHT 1 #
有307200个点的无序点云数据集 -
VIEWPOINT–指定数据集中点云的获取视点。VIEWPOINT有可能在不同坐标系之间转换的时候应用，在辅助获取其他特征时也比较有用，例如曲面法线，在判断方向一致性时，需要知道视点的方位，
视点信息被指定为平移（txtytz）+四元数（qwqxqyqz）。默认值是：
VIEWPOINT 0 0 0 1 0 0 0 -
POINTS–指定点云中点的总数。从0.7版本开始，该字段就有点多余了，因此有可能在将来的版本中将它移除。
例子：
POINTS 307200 #点云中点的总数为307200 -
DATA –指定存储点云数据的数据类型。从0.7版本开始，支持两种数据类型：ascii和二进制。查看下一节可以获得更多细节。
注意：文件头最后一行（DATA）的下一个字节就被看成是点云的数据部分了，它会被解释为点云数据。
2. 消息转换
ros 消息转换为 pcl 点云
这部分是在激光SLAM中常用的输入处理方式，我们选取了Lego-LOAM中对velodyne的lidar做了区分处理的代码部分，以16线雷达为例，激光雷达通过16根雷达光束进行旋转扫描，从而得到一周360°的点云。velodyne的lidar对点云属于16线中的哪一线做了标记，这个标记被称为Ring index，其中的Ring表示环的意思。如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束。因此保存点云的时候分别保存为"laserCloudIn"和"laserCloudInRing"中。
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. 读取ROS点云转换为PCL点云
cloudHeader = laserCloudMsg->header;
// cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// 2. 去除点云中的Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// 3. 如果点云有"ring"通过，则保存为laserCloudInRing
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}
下面是核心代码sensor_msgs::PointCloud2转到 pcl::PointCloud<pcl::PointXYZ>，从ros到pcl类型的转换，用到fromROSMsg方法。
//输入的ros pointcloud2 类型
sensor_msgs::PointCloud2 t_cloud;
//输出的pcl 类型
pcl::PointCloud<pcl::PointXYZ> t_pclCloud;
pcl::fromROSMsg(t_cloud,t_pclCloud);
pcl 点云转换为ros 消息
而对于在PCL处理后需要通过ros类型后可以publish出去，然后在rviz中查看，这就需要pcl::PointCloud<pcl::PointXYZ>转到sensor_msgs::PointCloud这是pcl 到ros类型转换，用到toROSMsg方法。
//输入的pcl 类型
pcl::PointCloud<pcl::PointXYZ> t_pclCloud;
//输出的ros 类型
sensor_msgs::PointCloud2 t_rosCloud;
pcl::toROSMsg(t_pclCloud,t_rosCloud);
//发布出去
//t_scanPub.publish(t_rosCloud);
2d激光ros消息扩展到3d激光消息
此处是两个ros类型的相互转换，这里主要用到projectLaser这个方法。这样可以有效地将大部分单线激光雷达输出转化为更加通用的多线激光雷达输出格式。sensor_msgs::LaserScan转到sensor_msgs::PointCloud2
//输入的 ros LaserScan 类型
sensor_msgs::LaserScan::ConstPtr& msg;
laser_geometry::LaserProjection t_projectoir;
//输出的ros pointcloud2 类型
sensor_msgs::PointCloud2 t_cloud;
//转换到 ros pointcloud2 类型
t_projectoir.projectLaser(*msg,t_cloud);
PCL数据与指针之间的相互转换
下面我们将以pcl::PointCloud::Ptr和pcl::PointCloud的两个类相互转换，因为在复杂的PCL点云处理时一般常用pcl::PointCloud::Ptr。kdtree和octree类中的setInputCloud()函数只支持pcl::PointCloud::Ptr类型
//方法1
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud = *cloudPtr;//由Ptr转变为另一种类型
cloudPtr = cloud.makeShared();//转变为Ptr类型
PCL不同数据格式之间的转化
这里我们以xyzrgb格式转换为xyz格式的点云来作为例子进行介绍
#include <pcl/io/pcd_io.h>
#include <ctime>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
using namespace std;
typedef pcl::PointXYZ point;
typedef pcl::PointXYZRGBA pointcolor;
int main(int argc,char **argv)
{
pcl::PointCloud<pointcolor>::Ptr input (new pcl::PointCloud<pointcolor>);
pcl::io::loadPCDFile(argv[1],*input);
pcl::PointCloud<point>::Ptr output (new pcl::PointCloud<point>);
int M = input->points.size();
cout<<"input size is:"<<M<<endl;
for (int i = 0;i <M;i++)
{
point p;
p.x = input->points[i].x;
p.y = input->points[i].y;
p.z = input->points[i].z;
output->points.push_back(p);
}
output->width = 1;
output->height = M;
cout<< "size is"<<output->size()<<endl;
pcl::io::savePCDFile("output.pcd",*output);
}
PCL赋值方式
第一种，是一种vector的赋值方式，将point数据push_back到pcl::PointXYZ类型的模板中。
pcl::PointCloud<pcl::PointXYZ> pointCloud;
// Generate the data
for (float y=-0.5f; y<=0.5f; y+=0.01f) {
for (float z=-0.5f; z<=0.5f; z+=0.01f) {
pcl::PointXYZ point;
point.x = 2.0f - y;
point.y = y;
point.z = z;
pointCloud.points.push_back(point);
}
}
pointCloud.width = (uint32_t) pointCloud.points.size();
pointCloud.height = 1;
第二种，指针型类模板，采用“->points[i]”方式赋值。
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = 5;
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 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
std::cerr << "Cloud before filtering: " << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
文件到数据的转换
这里我们想列出两种pcd数据读取方式：第一种读取方式使用reader，而第二种使用loadPCDFile。
第一种：
#include <pcl/point_types.h>//PCL对各种格式的点的支持头文件
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZI PointT;
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>); // Generate pointcloud data，新建指针cloud存放点云
pcl::PCDReader reader;
reader.read<pcl::PointXYZI>("1.pcd", *cloud);//读取1.pcd文件，用指针传递给cloud。
std::cout << "Loaded "
<< cloud->width * cloud->height
<< " data points from test_pcd.pcd with the following fields: "
<< std::endl;
for (std::size_t i = 0; i < cloud->points.size (); ++i)
{
std::cout << " " << cloud->points[i].x
<< " " << cloud->points[i].y
<< " " << cloud->points[i].z << std::endl;
}
return (0);
}
第二种：
#include <pcl/point_types.h>//PCL对各种格式的点的支持头文件
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZI PointT;
//pcl:Pointcloud<pcl::PointXYZ>cloud //1111111111111111111111
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>); // Generate pointcloud data，新建指针cloud存放点云
if (pcl::io::loadPCDFile<pcl::PointXYZI>("1.pcd", *cloud) == -1)//*打开点云文件。
{
PCL_ERROR("Couldn't read that pcd file\n");
return(-1);//如果没找到该文件，返回-1，跳出整个main函数
}
//写入方法一：
pcl::PCDWriter writer;
writer.write<pcl::PointXYZI>("name_cluster.pcd", *INcloud, false);//将点云保存到PCD文件中
//*INcloud该参数带不带*号，取决于你自己定义的cloud类型，如上使用的是Ptr,智能指针所以下边传参也要带*号-------按照111111111111方式定义点云的话，传参不带*号
//写入方法二：
pcl::io::savePCDFileASCII("name_cluster.pcd", *INcloud); //将点云保存到PCD文件中
此外还有读出部分的操作
#include <iostream> //标准C++库中的输入输出的头文件
#include <pcl/io/pcd_io.h> //PCD读写类相关的头文件
#include <pcl/point_types.h> //PCL中支持的点类型的头文件
int main(int argc, char **argv)
{
//创建一个PointCloud<pcl::PointXYZ> boost共享指针并进行实例化
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//打开点云文件
if (pcl::io::loadPCDFile<pcl::PointXYZ>("../test_pcd.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
//默认就是而二进制块读取转换为模块化的PointCLoud格式里pcl::PointXYZ作为点类型 然后打印出来
std::cout << "Loaded "
<< cloud->width * cloud->height // 宽*高
<< " data points from test_pcd.pcd with the following fields: "
<< std::endl;
for (size_t i = 0; i < cloud->points.size(); ++i)
std::cout << " " << cloud->points[i].x
<< " " << cloud->points[i].y
<< " " << cloud->points[i].z << std::endl;
return (0);
}
3. PCL方法详解
由于我们的ROS在点云数据显示以及通信方面更占优势，所以我们在实际的激光SLAM处理时，是优先将PCL点云图像通过toROSMsg转为ROS的信息图像，然后在订阅后再转为PCL点云图像进一步处理，这时候就需要我们通过PCL自带的一些函数来实现激光点云的处理了。下面是LVI-SAM的激光雷达订阅处理代码
void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& laser_msg)
{
static int lidar_count = -1;
//相当于降采样
if (++lidar_count % (LIDAR_SKIP+1) != 0)
return;
// 0. listen to transform, 接收位姿变换
static tf::TransformListener listener;
static tf::StampedTransform transform;
try{
listener.waitForTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, ros::Duration(0.01));
listener.lookupTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, transform);
}
catch (tf::TransformException ex){
// ROS_ERROR("lidar no tf");
return;
}
// 当前位姿的表示
double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform.getOrigin().x();
yCur = transform.getOrigin().y();
zCur = transform.getOrigin().z();
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);
// 1. convert laser cloud message to pcl 将点云转换成PCL格式
pcl::PointCloud<PointType>::Ptr laser_cloud_in(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(*laser_msg, *laser_cloud_in);
// 2. downsample new cloud (save memory) 调用PCL的降采样算法
pcl::PointCloud<PointType>::Ptr laser_cloud_in_ds(new pcl::PointCloud<PointType>());
static pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.setInputCloud(laser_cloud_in);
downSizeFilter.filter(*laser_cloud_in_ds);
*laser_cloud_in = *laser_cloud_in_ds;
// 3. filter lidar points (only keep points in camera view) 保证当前点云的点在当前相机视角FOV内
pcl::PointCloud<PointType>::Ptr laser_cloud_in_filter(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)laser_cloud_in->size(); ++i)
{
PointType p = laser_cloud_in->points[i];
if (p.x >= 0 && abs(p.y / p.x) <= 10 && abs(p.z / p.x) <= 10)
laser_cloud_in_filter->push_back(p);
}
*laser_cloud_in = *laser_cloud_in_filter;
// TODO: transform to IMU body frame
// 4. offset T_lidar -> T_camera 将点云从激光雷达坐标系变成相机坐标系(pcl居然有这么一个函数，作者确实厉害)
pcl::PointCloud<PointType>::Ptr laser_clou d_offset(new pcl::PointCloud<PointType>());
Eigen::Affine3f transOffset = pcl::getTransformation(L_C_TX, L_C_TY, L_C_TZ, L_C_RX, L_C_RY, L_C_RZ);
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_offset, transOffset);
*laser_cloud_in = *laser_cloud_offset;
// 5. transform new cloud into global odom frame 再把点云变换到世界坐标系
pcl::PointCloud<PointType>::Ptr laser_cloud_global(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_global, transNow);
// 6. save new cloud 最后把变换完成的点云存储到待处理队列
double timeScanCur = laser_msg->header.stamp.toSec();
cloudQueue.push_back(*laser_cloud_global);
timeQueue.push_back(timeScanCur);
// 7. pop old cloud 保持队列里的点云的时长不超过一定的时间
while (!timeQueue.empty())
{
if (timeScanCur - timeQueue.front() > 5.0)
{
cloudQueue.pop_front();
timeQueue.pop_front();
} else {
break;
}
}
std::lock_guard<std::mutex> lock(mtx_lidar);
// 8. fuse global cloud 将队列里的点云输入作为总体的待处理深度图
depthCloud->clear();
for (int i = 0; i < (int)cloudQueue.size(); ++i)
*depthCloud += cloudQueue[i];
// 9. downsample global cloud 深度图降采样
pcl::PointCloud<PointType>::Ptr depthCloudDS(new pcl::PointCloud<PointType>());
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.setInputCloud(depthCloud);
downSizeFilter.filter(*depthCloudDS);
*depthCloud = *depthCloudDS;
}
我们可以看到上述的代码中包含了很多对点云的操作，例如滤波，坐标系变化等，下面我们将一一讲述常用的pcl操作
3.1 KDTree
k-d树（k-dimensional树的简称），是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）
对于KdTree，其内部主要有以下成员：
nearestKSearch()
该函数有3个重载。
版本一接受点云，查询点在点云中的index，k，输出k近邻在点云中的indices，与这些点的平方距离。
版本二接受一个点（不一定在点云中已有）
版本三和一相同（使用默认指定点云）
如果去看代码会发现版本一和版本三不过是对版本二的包装，而版本二仅仅是一个纯虚函数而已。
nearestKSearchT()
允许接受一个类别T不同的PointTDiff，然后传给nearestKSearch的版本二。
radiusSearch()
搜索半径而不是K近邻。和K近邻搜索相同，最终会进入一个纯虚的radiusSearch()函数，只不过指定的参数从k变为radius。
里面多一个参数max_nn，默认0指定函数返回值为搜索到的点的个数。
radiusSearchT()
和上面那个差不多。
/*
* @Description: K-D 树搜索
*/
#include <pcl/point_cloud.h> //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
srand (time (NULL)); //用系统时间初始化随机种子
//创建一个PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// 随机点云生成
cloud->width = 1000; //此处点云数量
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); // // 产生数值为0-1024的浮点数
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
//创建KdTreeFLANN对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
//设置搜索空间
kdtree.setInputCloud (cloud);
//设置查询点并赋随机值
pcl::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);
// K 临近搜索
//创建一个整数（设置为10）和两个向量来存储搜索到的K近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方
int K = 10;
std::vector<int> pointIdxNKNSearch(K); //存储查询点近邻索引
std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方
//打印相关信息
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) //执行K近邻搜索
{
//打印所有近邻坐标
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;
}
/**********************************************************************************
下面的代码展示查找到给定的searchPoint的某一半径（随机产生）内所有近邻，重新定义两个向量
pointIdxRadiusSearch pointRadiusSquaredDistance来存储关于近邻的信息
********************************************************************************/
// 半径 R内近邻搜索方法
std::vector<int> pointIdxRadiusSearch; //存储近邻索引
std::vector<float> 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;
// 假设我们的kdtree返回了大于0个近邻。那么它将打印出在我们"searchPoint"附近的10个最近的邻居并把它们存到先前创立的向量中。
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) //执行半径R内近邻搜索方法
{
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;
}
return 0;
}
3.2 Octree
八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。
- 一般中心点作为节点的分叉中心。
- 八叉树若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。
- 分割一直要进行到节点所对应的立方体或是完全空白，或是完全为V占据，或是其大小已是预先定义的体素大小，并且对它与V之交作一定的“舍入”，使体素或认为是空白的，或认为是V占据的。
…详情请参照古月居
<!--
- 点赞
- 收藏
- 关注作者
评论（0）