点云库PCL的使用

相关头文件：

#include 
#include 

声明和定义点云对象：

pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud);
pcl::PointCloud& cloud = *cloud_ptr;

// 更简洁写法
using MyPoint = pcl::PointXYZ;
using MyCloud = pcl::PointCloud;
using MyCloudPtr = CLOUD::Ptr;

初始化点云数据PCD文件头：

cloud.width = 30720;
cloud.height = 1;
cloud.is_dense = false;
cloud.points.resize(cloud.width * cloud.height);
pcl::PointXYZRGB color_point;
for (size_t i = 0; i < cloud.points.size(); ++i) {
    cloud.points[i].x = color_point.x;
    cloud.points[i].y = color_point.y;
    cloud.points[i].z = color_point.z;
    cloud.points[i].rgb = color_point.rgb;
}

cloud.height用来判断是否为有序点云，=1则是无序点云也可以使用如下函数代替：if (!cloud.isOrganized ())，对于无序点云来说：width就是指点云中点的个数，对于有结构点云来说：width是指点云数据集一行上点的个数，立体相机或者TOF相机获得的点云数据就属于这一类。对于有结构点云的一大好处就是能知道点云中点的相邻关系，最近邻操作效率就非常高，可以大大提高PCL中相应算法的效率。但是结构点云经过某些人为操作后，会变为无结构点云，比如滤波操作等。
例如：有结构点云

cloud.width = 640; // Image-like organized structure, with 640 rows and 480 columns,

cloud.height = 480; // thus 640*480=307200 points total in the dataset

无结构点云

cloud.width = 307200;

cloud.height = 1; // unorganized point cloud dataset with 307200 points

cloud.is_dense (bool)  判断points中的数据是否是有限的（有限为true）或者说是判断点云中的点是否包含 Inf/NaN这种值（包含为false）。

points存储了数据类型为PointT的一个动态数组，例如，对于一个包含了XYZ数据的点云，points是包含了元素为pcl::PointXYZ一个vector。 相当于vector

可视化pcd文件：

sudo apt install pcl-tools
pcl_viewer file_name.pcd

pcl_viewer窗口操作的快捷键 

p,P: switch to a point-based representation（以点为基准展示） 

w,W: switch to a wireframe-based representation (where available)（以线框为基准展示）

s,S: switch to a surface-based representation (where available)（以平面为基准展示）

j,J: take a .PNG snapshot of the current window view（将当前窗口截图为png格式，保存在bin目录下的Debug或者Release目录下）

c,C: display current camera/window parameters（显示当前相机参数）

+/-: increment/decrement overall point size（放大或缩小当前所有点的尺寸）

g,G: display scale grid (on/off)（开启标尺）

u,U: display lookup table (on/off)（开启colorbar）

r,R: [+ ALT]: reset camera [to viewpoint = {0, 0, 0} -> center_{x, y, z}]（将相机平移到某个位置）

ALT + s,S: turn stereo mode on/off（打开立体模式）

ALT + f,F: switch between maximized window mode and original size（改变当前窗口的尺寸)
ALT + 0..9 [+ CTRL]: switch between different geometric handlers (where available)
      0..9 [+ CTRL]: switch between different color handlers (where available)

颜色显示：
１：随机颜色
２：从左到右(x)
３：从上到下(y)
４：按高度(z)
５：按强度（intensity）

---

读取点云：

#include 

std::string pcd_path = "/tmp/src_cloud.pcd"
pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
// pcl::PCDReader reader;
// reader.read(pcd_path, *cloud);
// reader.read (pcd_path, *cloud);
if (pcl::io::loadPCDFile(pcd_path, *cloud) == -1) {
  // pcl::io::loadPCDFile(pcd_path, *cloud);
  std::cerr << "can't load point cloud file" << std::endl;
  return -1;
}
std::cout << "loaded " << cloud->width * cloud->height << " points" << std::endl;
std::cout << "field: " << pcl::getFieldsList(*cloud) << std::endl;

保存点云：

// pcl::io::savePCDFileASCII("file_name.pcd", cloud);
pcl::io::savePCDFileBinary("file_name.pcd", cloud);

// pcl::PCDWriter writer;
// writer.write("/tmp/dst_cloud.pcd", *cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
// writer.write ("/tmp/dst_cloud.pcd", *cloud, false);

if (pcl::io::savePCDFile(path, *cloud) == -1) {
  std::cerr << "can't save point cloud file" << std::endl;
  return -1;
}

显示点云：

#include 

pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
pcl::visualization::CloudViewer viewer("Cloud Viewer");
viewer.showCloud(cloud);
while (!viewer.wasStopped()) {
}

计算点云最大/最小值

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);

pcl::PointXYZI min; // 用于存放三个轴的最小值
pcl::PointXYZI max; // 用于存放三个轴的最大值
pcl::getMinMax3D(*src_cloud, min, max);
std::cout << "min: " << min.x << " " << min.y << " " << min.z << std::endl;
std::cout << "max: " << max.x << " " << max.y << " " << max.z << std::endl;

计算两点之间距离

#include 

Eigen::Vector3f p1(0, 0, 0);
Eigen::Vector3f p2(1, 1, 1);
double dist = pcl::geometry::squaredDistance(p1, p2);

计算点云质心

#include 

pcl::PointCloud::Ptr cloud(new pcl::PointCloud);

Eigen::Vector4f centroid; // 齐次坐标(c0,c1,c2,1)
pcl::compute3DCentroid(*cloud, centroid);
std::cout << "centroid: " << centroid[0] << ", " << centroid[1] << ", " << centroid[2] << std::endl;

 网格滤波：

使用体素化网格方法实现下采样，即减少点的数量 减少点云数据，并同时保存点云的形状特征，在提高配准，曲面重建，形状识别等算法速度中非常实用，PCL是实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格，容纳后每个体素内用体素中所有点的重心来近似显示体素中其他点，这样该体素内所有点都用一个重心点最终表示，对于所有体素处理后得到的过滤后的点云，这种方法比用体素中心（注意中心和重心）逼近的方法更慢，但是对于采样点对应曲面的表示更为准确。 

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
pcl::VoxelGrid sor;
sor.setInputCloud(src_cloud);
// sor.setMinimumPointsNumberPerVoxel(1);
sor.setLeafSize(0.1, 0.1, 0.1);
sor.filter(*dst_cloud);

统计滤波：

**问题描述：**激光扫描通常会产生密度不均匀的点云数据集，另外测量中的误差也会产生稀疏的离群点，此时，估计局部点云特征（例如采样点处法向量或曲率变化率）时运算复杂，这会导致错误的数值，反过来就会导致点云配准等后期的处理失败。

**解决办法：**对每个点的邻域进行一个统计分析，并修剪掉一些不符合标准的点。具体方法为在输入数据中对点到临近点的距离分布的计算，对每一个点，计算它到所有临近点的平均距离（假设得到的结果是一个高斯分布，其形状是由均值和标准差决定），那么平均距离在标准范围之外的点，可以被定义为离群点并从数据中去除。

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
pcl::StatisticalOutlierRemoval sor;
sor.setInputCloud(src_cloud); // 设置待滤波的点云
sor.setMeanK(50); // 设置在进行统计时考虑查询点临近点数
sor.setStddevMulThresh(1.0); // 设置判断是否为离群点的阀值
// sor.setNegative(true);
sor.filter(*dst_cloud);

RadiusOutlinerRemoval

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
pcl::RadiusOutlierRemoval outrem; // 创建滤波器
outrem.setInputCloud(src_cloud); // 设置输入点云
outrem.setRadiusSearch(0.8); // 设置半径为0.8的范围内找临近点
outrem.setMinNeighborsInRadius(2); // 设置查询点的邻域点集数小于2的删除
outrem.filter(*dst_cloud); // 执行条件滤波,在半径为0.8 在此半径内必须要有两个邻居点，此点才会保存

 直通滤波：（一维）

最简单的例子：比如说高程筛选 

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
pcl::PassThrough pass;
pass.setInputCloud(src_cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(-1.2, 1); 
// pass.setFilterLimitsNegative (true); // 设置保留范围内还是过滤掉范围内
pass.filter(*dst_filtered);

裁剪：（三维）

#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
pcl::CropBox box;
box.setInputCloud(src_cloud);
box.setMin(Eigen::Vector4f(-50, -50, -100, 1.0));
box.setMax(Eigen::Vector4f(80, 50, 100, 1.0));
box.filter(*dst_cloud);

 点云变换：

#include 
#include 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
Eigen::Matrix3d r = Eigen::Matrix3d::Identity();
Eigen::Vector3d t = {1, -1, 0};
Eigen::Matrix4d T;
T << r, t, 0, 0, 0, 1;
pcl::transformPointCloud(*src_cloud, *dst_cloud, T);

根据索引拷贝点云 

pcl::PointCloud::Ptr src_cloud(new pcl::PointCloud);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);

pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// std::vector inliers;

pcl::copyPointCloud(*src_cloud, *inliers, *dst_cloud);

不同类型点云转换：

pcl::PointCloud::Ptr cloud_xyzi;
pcl::PointCloud::Ptr cloud_xyz(new pcl::PointCloud);
pcl::copyPointCloud(*cloud_xyzi, *cloud_xyz);

pcl::PCLPointCloud2::Ptr src_cloud(new pcl::PCLPointCloud2);
pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud)
pcl::fromPCLPointCloud2(*src_cloud, *dst_cloud); // 转换为模板点云

  pcl点云与ros点云转换：

#include "pcl/common/transforms.h"  // transformPointCloud
#include "pcl_conversions/pcl_conversions.h"  // fromROSMsg, toROSMsg

sensor_msgs::PointCloud2ConstPtr cloud_msg =
  message.instantiate();
pcl::PointCloud pointcloud
pcl::fromROSMsg(*cloud_msg, pointcloud);

Eigen::Matrix4d transform;
pcl::transformPointCloud(src_cloud, det_cloud, transform);

1.ROS转PCL数据格式
(1)sensor_msgs::PointCloud2转pcl::PCLPointCloud2
pcl_conversions::toPCL(sensor_msgs::PointCloud2, pcl::PCLPointCloud2)
(2)sensor_msgs::PointCloud2转pcl::PointCloud
pcl::fromROSMsg (sensor_msgs::PointCloud2, pcl::PointCloud);

2.PCL转ROS数据
(1)pcl::PCLPointCloud2转sensor_msgs::PointCloud2
pcl_conversions::fromPCL(pcl::PCLPointCloud2, sensor_msgs::PointCloud2);
(2)pcl::PointCloud转sensor_msgs::PointCloud2
pcl::toROSMsg (pcl::PointCloud,sensor_msgs::PointCloud2);

3.PCL中数据互转
(1)pcl::PCLPointCloud2转pcl::PointCloud 
pcl::fromPCLPointCloud2(pcl::PCLPointCloud2,pcl::PointCloud);
(2)pcl::PointCloud转pcl::PCLPointCloud2
pcl::toPCLPointCloud2(pcl::PointCloud, pcl::PCLPointCloud2);

kdtree搜索

#include 
#include 

pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 
pcl::KdTreeFLANN::Ptr kdtree(new pcl::KdTreeFLANN);
kdtree->setInputCloud(cloud);

pcl::PointXYZI search_point;
search_point.x = 1;
search_point.y = 1;
search_point.z = 1;

// 半径搜索
std::vector neighbor_indices;
std::vector neighbor_square_distances;
int search_radius = 1;
if (kdtree->radiusSearch(search_point, search_radius, neighbor_indices, neighbor_square_distances) == 0) {
  std::cerr << "error !" << std::endl;
  return -1;
}

// 最近邻搜索
int K = 10;
std::vector neighbor_indices(K);
std::vector neighbor_square_distances(K);
if (kdtree->nearestKSearch(searchPoint, K, neighbor_indices, neighbor_square_distances) == 0) {
  std::cerr << "error !" << std::endl;
  return -1;
}

pcl::PointCloud::Ptr select_cloud(new pcl::PointCloud);
for (const auto& idx : neighbor_indices) {
  pcl::PointXYZI point = cloud->points.at(idx);
  select_cloud->push_back(point);
}

其他：

#include 
std::cout << "rad: " << pcl::deg2rad(180.0) << std::endl;

pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

CMakeList:

cmake_minimum_required(VERSION 2.8.3)
project(test)

set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

find_package(Eigen3)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})

find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})

add_executable(test main.cc)
target_link_libraries(test
${Eigen_LIBS}
${PCL_LIBRARIES})