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占据的。
实现八叉树的原理
(1). 设定最大递归深度。
(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体。
(3).依序将单位元元素丢入能被包含且没有子节点的立方体。
(4).若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体。
(5).若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。
(6). 重复3，直到达到最大递归深度。
/*
* @Description: 点云搜索
*/
#include <pcl/point_cloud.h> //点云头文件
#include <pcl/octree/octree.h> //八叉树头文件
#include <iostream>
#include <vector>
#include <ctime>
int main(int argc, char **argv)
{
srand((unsigned int)time(NULL)); //用系统时间初始化随机种子与 srand (time (NULL))的区别
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);
cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
/****************************************************************************
创建一个octree实例，用设置分辨率进行初始化，该octree用它的页节点存放点索引向量，
分辨率参数描述最低一级octree的最小体素的尺寸，因此octree的深度是分辨率和点云空间维度
的函数，如果知道点云的边界框，应该用defineBoundingbox方法把它分配给octree然后通过
点云指针把所有点增加到ctree中。
*****************************************************************************/
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution); //初始化Octree
octree.setInputCloud(cloud); //设置输入点云 这两句是最关键的建立PointCloud和octree之间的联系
octree.addPointsFromInputCloud(); //构建octree
pcl::PointXYZ searchPoint; //设置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);
/*************************************************************************************
一旦PointCloud和octree联系一起，就能进行搜索操作，这里使用的是“体素近邻搜索”，把查询点所在体素中
其他点的索引作为查询结果返回，结果以点索引向量的形式保存，因此搜索点和搜索结果之间的距离取决于octree的分辨率参数
*****************************************************************************************/
std::vector<int> pointIdxVec; //存储体素近邻搜索结果向量
if (octree.voxelSearch(searchPoint, pointIdxVec)) //执行搜索，返回结果到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;
}
/**********************************************************************************
K 被设置为10 ，K近邻搜索 方法把搜索结果写到两个分开的向量，第一个pointIdxNKNSearch包含搜索结果
（结果点的索引的向量） 第二个向量pointNKNSquaredDistance存储搜索点与近邻之间的距离的平方。
*************************************************************************************/
//K 近邻搜索
int K = 10;
std::vector<int> pointIdxNKNSearch; //结果点的索引的向量
std::vector<float> 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;
}
// 半径内近邻搜索
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;
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;
}
}
3.3 采样一致性算法RANSAC
RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点 ( inliers），或称内部点、局内点或内点，否则为模型外样本点（outliers），或称外部点、局外点或外点，记录下当前的 inliers 的个数，然后重复这一过程。每一次重复都记录当前最佳的模型参数，所谓最佳即是inliers的个数最多 ，此时对应的inliers个数为 best_ninliers 。 每次迭代的末尾都会根据期望的误差率、 best_ninliers、总样本个数、当前迭代次数，计算一 个迭代结束评判因子，据此决定是否迭代结束。迭代结束后，最佳模型参数就是最终的模型参数估计值 。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
1.有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
2.用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
3.如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
4.然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
5.最后，通过估计局内点与模型的错误率来评估模型。
/*
* @Description: 1随机采样一致性算法
*/
#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
//viewer->addCoordinateSystem (1.0, "global");
viewer->initCameraParameters ();
return (viewer);
}
/******************************************************************************************************************
对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算
而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的局外点。
*****************************************************************************************************************/
int
main(int argc, char** argv)
{
// 初始化点云对象
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); //存储源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); //存储提取的局内点
// 填充点云数据
cloud->width = 500; //填充点云数目
cloud->height = 1; //无序点云
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
{
//根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); //此处对应的点为局外点
else if(i % 2 == 0)
cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{ //用x+y+z=1设置一部分点云数据，此时地拿云组成的菱形平面作为内点
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if( i % 2 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); //对应的局外点
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
std::vector<int> inliers; //存储局内点集合的点的索引的向量
//创建随机采样一致性对象
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud)); //针对球模型的对象
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud)); //针对平面模型的对象
if(pcl::console::find_argument (argc, argv, "-f") >= 0)
{ //根据命令行参数，来随机估算对应平面模型，并存储估计的局内点
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01); //与平面距离小于0.01 的点称为局内点考虑，注意局内点设置参数
ransac.computeModel(); //执行随机参数估计
ransac.getInliers(inliers); //存储估计所得的局内点
}
else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
{
//根据命令行参数 来随机估算对应的圆球模型，存储估计的内点
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// 复制估算模型的所有的局内点到final中
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// 创建可视化对象并加入原始点云或者所有的局内点
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
viewer = simpleVis(cloud);
while (!viewer->wasStopped ())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return 0;
}
3.4 filter滤波
PCL中总结了几种需要进行点云滤波处理情况，这几种情况分别如下：
- （1） 点云数据密度不规则需要平滑
- （2） 因为遮挡等问题造成离群点需要去除
- （3） 大量数据需要下采样
- （4） 噪声数据需要去除
对应的方案如下：
（1）按照给定的规则限制过滤去除点
（2） 通过常用滤波算法修改点的部分属性
（3）对数据进行下采样
PCL点云格式分为有序点云和无序点云
针对有序点云提供了双边滤波、高斯滤波、中值滤波等
针对无序点云提供了体素栅格、随机采样等
是根据传感器的采集数据上来说
一般深度相机采集到的点云的数据是有序点云
而我们激光雷达采集的点云的数据是无序的点云
/*
* @Description: 2VoxelGrid滤波器对点云进行下采样
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
int
main (int argc, char** argv)
{
pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
//点云对象的读取
pcl::PCDReader reader;
reader.read ("../table_scene_lms400.pcd", *cloud); //读取点云到cloud中
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ").";
/******************************************************************************
创建一个voxel叶大小为1cm的pcl::VoxelGrid滤波器，
**********************************************************************************/
pcl::VoxelGrid<pcl::PCLPointCloud2> sor; //创建滤波对象
sor.setInputCloud (cloud); //设置需要过滤的点云给滤波对象
sor.setLeafSize (0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
sor.filter (*cloud_filtered); //执行滤波处理，存储输出
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";
pcl::PCDWriter writer;
writer.write ("../table_scene_lms400_downsampled.pcd", *cloud_filtered,
Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
return (0);
}
原始点云与滤波后的点云可视化结果，明显的可以看出来，点的密度大小与整齐程度不同，虽然处理后的数据量大大减小，但是很明显所含有的形状特征和空间结构信息与原始点云差不多。
3.5 三角化
本部分是利用贪婪投影三角化算法来处理来自一个或者多个扫描仪扫描到得到并且有多个连接处的散乱点云。另外它更适用于采样点云来自表面连续光滑的曲面且点云的密度变化比较均匀的情况。具体方法是：
- （1）先将有向点云投影到某一局部二维坐标平面内
- （2）在坐标平面内进行平面内的三角化
- （3）根据平面内三位点的拓扑连接关系获得一个三角网格曲面模型.
/*
* @Description: 无序点云的快速三角化
*/
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h> //贪婪投影三角化算法
int
main (int argc, char** argv)
{
// 将一个XYZ点类型的PCD文件打开并存储到对象中
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud_blob;
pcl::io::loadPCDFile ("../bun0.pcd", cloud_blob);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
//* the data should be available in cloud
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n; //法线估计对象
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); //存储估计的法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); //定义kd树指针
tree->setInputCloud (cloud); //用cloud构建tree对象
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.compute (*normals); //估计法线存储到其中
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); //连接字段
//* cloud_with_normals = cloud + normals
//定义搜索树对象
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals); //点云构建搜索树
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3; //定义三角化对象
pcl::PolygonMesh triangles; //存储最终三角化的网络模型
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (0.025); //设置连接点之间的最大距离，（即是三角形最大边长）
// 设置各参数值
gp3.setMu (2.5); //设置被样本点搜索其近邻点的最远距离为2.5，为了使用点云密度的变化
gp3.setMaximumNearestNeighbors (100); //设置样本点可搜索的邻域个数
gp3.setMaximumSurfaceAngle(M_PI/4); // 设置某点法线方向偏离样本点法线的最大角度45
gp3.setMinimumAngle(M_PI/18); // 设置三角化后得到的三角形内角的最小的角度为10
gp3.setMaximumAngle(2*M_PI/3); // 设置三角化后得到的三角形内角的最大角度为120
gp3.setNormalConsistency(false); //设置该参数保证法线朝向一致
// Get result
gp3.setInputCloud (cloud_with_normals); //设置输入点云为有向点云
gp3.setSearchMethod (tree2); //设置搜索方式
gp3.reconstruct (triangles); //重建提取三角化
// 附加顶点信息
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
// Finish
return (0);
}
3.6 分割
点云分割是根据空间，几何和纹理等特征对点云进行划分，使得同一划分内的点云拥有相似的特征，点云的有效分割往往是许多应用的前提，例如逆向工作，CAD领域对零件的不同扫描表面进行分割，然后才能更好的进行空洞修复曲面重建，特征描述和提取，进而进行基于3D内容的检索，组合重用等。
/*
* @Description: PCL中实现欧式聚类提取。对三维点云组成的场景进行分割。
*/
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
/******************************************************************************
打开点云数据，并对点云进行滤波重采样预处理，然后采用平面分割模型对点云进行分割处理
提取出点云中所有在平面上的点集，并将其存盘
******************************************************************************/
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("../table_scene_lms400.pcd", *cloud); // 点云文件
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg; // VoxelGrid类在输入点云数据上创建3D体素网格（将体素网格视为一组空间中的微小3D框
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud); //输入
vg.setLeafSize (0.01f, 0.01f, 0.01f); // setLeafSize (float lx, float ly, float lz)
vg.filter (*cloud_filtered); //输出
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*滤波后
//创建平面模型分割的对象并设置参数
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE); //分割模型
seg.setMethodType (pcl::SAC_RANSAC); //随机参数估计方法
seg.setMaxIterations (100); //最大的迭代的次数
seg.setDistanceThreshold (0.02); //设置阀值
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points) // 滤波停止条件
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered); // 输入
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);// [平面
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// // 移去平面局内点，提取剩余点云
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; //欧式聚类对象
ec.setClusterTolerance (0.02); // 设置近邻搜索的搜索半径为2cm
ec.setMinClusterSize (100); //设置一个聚类需要的最少的点数目为100
ec.setMaxClusterSize (25000); //设置一个聚类需要的最大点数目为25000
ec.setSearchMethod (tree); //设置点云的搜索机制
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices); //从点云中提取聚类，并将点云索引保存在cluster_indices中
//迭代访问点云索引cluster_indices,直到分割出所有聚类
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "../cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); // 保存文件
j++;
}
return (0);
}
3.7 NDT配准
为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换 ，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作，这就是点云数据的配准。
粗配准
粗配准是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。常见粗配准算法：
基于特征匹配(PFH)的配准算法
基于穷举搜索的配准算法
精配准
精配准是指在粗配准的基础上，让点云之间的空间位置差异最小化，得到一个更加精准的旋转平移变换矩阵。该算法的运行速度以及向全局最优化的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。
ICP 迭代最近点算法（Iterative Cloest Point）
NDT 正态分布变换算法（Normal Distributions Transform）
这里我们选用了NDT来作为例子
/*
* @Description: 使用正态分布变换进行配准
*/
/*
使用正态分布变换进行配准的实验 。其中room_scan1.pcd room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h> //NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h> //滤波类头文件 （使用体素网格过滤器处理的效果比较好）
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
int
main (int argc, char** argv)
{
// 加载房间的第一次扫描点云数据作为目标
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
// 加载从新视角得到的第二次扫描点云数据作为源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
//以上的代码加载了两个PCD文件得到共享指针，后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵
// 将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度，只对源点云进行滤波，减少其数据量，而目标点云不需要滤波处理
//因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
// 初始化正态分布(NDT)对象
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// 根据输入数据的尺度设置NDT相关参数
ndt.setTransformationEpsilon (0.01); //为终止条件设置最小转换差异
ndt.setStepSize (0.1); //为more-thuente线搜索设置最大步长
ndt.setResolution (1.0); //设置NDT网格网格结构的分辨率（voxelgridcovariance）
//以上参数在使用房间尺寸比例下运算比较好，但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体，需要对参数进行很大程度的缩小
//设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
//添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
ndt.setMaximumIterations (35);
ndt.setInputSource (filtered_cloud); //源点云
// Setting point cloud to be aligned to.
ndt.setInputTarget (target_cloud); //目标点云
// 设置使用机器人测距法得到的粗略初始变换矩阵结果
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// 计算需要的刚体变换以便将输入的源点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
//这个地方的output_cloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// 使用创建的变换对为过滤的输入点云进行变换
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
// 保存转换后的源点云作为最终的变换输出
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// 初始化点云可视化对象
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// 对转换后的源点云着色 (green)可视化.
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// 启动可视化
viewer_final->addCoordinateSystem (1.0); //显示XYZ指示轴
viewer_final->initCameraParameters (); //初始化摄像头参数
// 等待直到可视化窗口关闭
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
以上这些基本可以覆盖所有的LOAM系列的操作，并能够良好的完成PCL从入门到应用的部分操作
4. 参考链接
https://www.yuque.com/huangzhongqing/pcl/aghhcg
https://blog.csdn.net/sinat_23853639/article/details/80305327
https://blog.csdn.net/m0_37957160/article/details/103822168
评论（0）
您还未登录，请登录后发表或查看评论