【点云学习PCL 】一

点云学习

说明：仅做学习使用，侵删

参考网址1

一、点云基础 0 概述

PCL（Point Cloud ）是用于 2D/3D 图像和点云处理的大型开源跨平台的 C++ 编程库，PCL 框架实现了大量点云相关的通用算法和高效的数据结构，PCL 是 BSD 授权方式，可以免费进行商业和学术应用。相比图像数据，点云数据多了一个维度，因此能够更全面的刻画三维场景中的对象

支持多种操作系统，可以在 、Linux、MacOS X、、部分嵌入式实时系统上运行。

应用领域：机器人、CAD/CAM、逆向工程、遥感测量、VR/AR、人机交互等

PCL 内容涉及了点云的获取、滤波、分割、配准、检索、特征提取、特征估计，表面重建、识别、模型拟合、追踪、曲面重建、可视化等。

PCL 中的所有模块和算法都是通过 Boost 共享指针来传送数据，因而避免了多次复制系统中已存在的数据

1.1 点云及其可视化

基本显示的代码逻辑：

拓展：

输出结果为：

1.2 点云数据

根据激光测量原理得到的点云，包含三维坐标信息和激光反射强度信息，激光反射强度与仪器的激光发射能量、波长，目标的表面材质、粗糙程度、入射角相关。根据摄影测量原理得到的点云，包括三维坐标（xyz）和颜色信息。结合两个原理的多传感器融合技术（多见于手持式三维扫描仪），能够同时得到这三种信息。

也即：三维坐标信息 ( x , y , z ) (x,y,z) (x,y,z) 强度信息 I I I（）,颜色信息（RGB）

基本数据类型

PCL的基本数据类型是，一个是一个C++的模板类，它包含了以下字段：

(int)

：制定点云数据集的高度

(std::

)：包含所有类型的点的数据列表

1.2.1 内置数据类型

类型主要有：

1.2.2 生成PCD文件分析

以输出的点云为例

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity normal_x normal_y normal_z curvature
SIZE 4 4 4 4 4 4 4 4
TYPE F F F F F F F F
COUNT 1 1 1 1 1 1 1 1
WIDTH 27211527
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 27211527
DATA binary
̆I��E�@���     

# .PCD v.7 - Point Cloud Data file format
VERSION .7			# 版本号
FIELDS x y z rgb		#  指定一个点可以有的每一个维度和字段的名字
SIZE 4 4 4 4			# 用字节数指定每一个维度的大小。例如：
TYPE F FFF			# 用一个字符指定每一个维度的类型
COUNT 1 1 1 1			# 指定每一个维度包含的元素数目，默认情况下，如果没有COUNT，所有维度的数目被设置成1
WIDTH 640    			# 像图像一样的有序结构，有640行和480列，
HEIGHT 480    			# 这样该数据集中共有640*480=307200个点
VIEWPOINT 0 0 0 1 0 0 0		# 指定数据集中点云的获取视点 视点信息被指定为平移（tx ty tz）+ 四元数（qw qx qy qz）
POINTS 307200			# 指定点云中点的总数。从0.7版本开始，该字段就有点多余了
DATA ascii			# 指定存储点云数据的数据类型。支持两种数据类型：ascii和二进制
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06

注解：

TYPE：用一个字符指定每一个维度的类型

：指定一个点可以有的每一个维度和字段的名字

PCD 文件的文件头部分必须按上面的顺序精确指定

1.2.3 点云序列化常用

其他文件格式

1.3 点云的基本处理方法

1.3.1文件简介

1.3.2 点云pcl和ROS之间相互转换

/*
ConstPtr：const pointer（常量指针），可理解为传递常量数据的共享指针1. 为什么要用指针而不直接传数据，因为数据太大了，值传递需要进行对数据的复制而指针引用不需要，因为涉及到右值传参和类型推断，不加引用的话类型可能发生变化2. ::constPtr 可以理解为指向常量的共享指针，::Ptr 可以理解为指向一个（可修改的）共享指针3. ::constPtr& 可以理解为对一个常量消息的共享指针的引用，::Ptr& 可以理解为对一个（可修改的）消息的共享指针的引用4. 使用共享指针，它是一种智能指针，可以自动管理他指向的内存的存储时间，不必手动释放内存
*/
void elevatedCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &input_cloud) {pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud());pcl::fromROSMsg(*input_cloud, *output_cloud);//pcl::toROSMsg(pcl::PointCloud, sensor_msgs::PointCloud2); // 逆过程
}

1.4 点云可视化工具 1.4.1 工具 1.4.2 程序的内部对象方法

//
// Created by pf on 2023/10/25.
//
#include 
#include 
#include 
#include int count = 0;void viewerPsycho(pcl::visualization::PCLVisualizer &viewer);void viewerOneOff(pcl::visualization::PCLVisualizer &viewer);int main() {pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);pcl::io::loadPCDFile("./data/scans.pcd", *cloud);pcl::visualization::CloudViewer viewer("Cloud Viewer");
//    viewer.showCloud(cloud);viewer.runOnVisualizationThreadOnce(viewerOneOff);viewer.runOnVisualizationThread(viewerPsycho);while (!viewer.wasStopped()) {count++;}return 0;
}void viewerPsycho(pcl::visualization::PCLVisualizer &viewer) {static unsigned cnt = 0;std::stringstream ss;ss << "Once per viewer loop: " << cnt++;// 每次刷新时，移除text，添加新的textviewer.removeShape("text", 0);viewer.addText(ss.str(), 200, 300, "text", 0);//FIXME: possible race condition here:count++;
}void viewerOneOff(pcl::visualization::PCLVisualizer &viewer) {// 设置背景色为粉红色viewer.setBackgroundColor(1.0, 0.5, 1.0);pcl::PointXYZ o;o.x = 1.0;o.y = 0.0;o.z = 0.0;//    添加一个圆心为o，半径为0.25m的球体viewer.addSphere(o, 0.25, "sphere", 0);std::cout << "i only run once" << std::endl;
}
对应的结果

雷达对象方法

方法类似，不作演示，没有点云文件。

//
// Created by pf on 2023/10/25.
//
#include 
#include 
#include int main(int argc, char **argv) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile("./data/bunny.pcd", *cloud);pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_milk(new pcl::PointCloud<pcl::PointXYZRGB>);pcl::io::loadPCDFile("./data/milk_color.pcd", *cloud_milk);// 创建PCLVisualizerboost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));// 设置背景色为灰色（非必须）viewer->setBackgroundColor(0.05, 0.05, 0.05, 0);// 添加一个普通点云 (可以设置指定颜色，也可以去掉single_color参数不设置)pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");// 再添加一个彩色点云及配置pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_milk);viewer->addPointCloud<pcl::PointXYZRGB>(cloud_milk, rgb, "sample cloud milk");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud milk");// 添加一个0.5倍缩放的坐标系（非必须）viewer->addCoordinateSystem(0.5);// 直到窗口关闭才结束循环while (!viewer->wasStopped()) {// 每次循环调用内部的重绘函数viewer->spinOnce();}return 0;
}

1.4.3 矩阵变换

将一个点云经过 z z z轴旋转45°，然后经过平移变换。

//
// Created by pf on 2023/10/25.
//
#include #include 
#include 
#include 
#include 
#include int main(int argc, char **argv) {const std::string file = "./data/bunny.pcd";pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());if (pcl::io::loadPCDFile(file, *source_cloud) < 0) {std::cout << "Error loading point cloud " << argv[1] << std::endl << std::endl;return -1;}Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();// 定义旋转矩阵auto m_data = float(M_PI_4);transform_1 << cos(m_data), -sin(m_data), 0, 2.5,sin(m_data), cos(m_data), 0, 0,0, 0, 0, 0,0, 0, 0, 0;// 使用仿射变换Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();transform_2.translation() << 0.8, 0.0, 0.0;transform_2.rotate(Eigen::AngleAxisf(m_data, Eigen::Vector3f::UnitZ()));pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);pcl::visualization::PCLVisualizer viewer("Matrix transformation example");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>source_cloud_color_handler(source_cloud, 255, 255, 255);// whiteviewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red\
viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");// 设置坐标系系统viewer.addCoordinateSystem(0.5, "cloud", 0);// 设置背景色viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey// 设置渲染属性（点大小）viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");// 设置渲染属性（点大小）viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");//viewer.setPosition(800, 400); // Setting visualiser window positionwhile (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressedviewer.spinOnce();}return 0;
}

二、点云的拓扑结构 3.1 点云存储结构，K-D树

通过3D相机（雷达、激光扫描、立体相机）获取到的点云，一般数据量较大，分布不均匀，数据主要表征了目标物表面的大量点的集合，这些离散的点如果希望实现基于邻域关系的快速查找比对功能，就必须对这些离散的点之间建立拓扑关系。常见的空间索引一般是自上而下逐级划分空间的各种索引结构，包括BSP树，k-d tree、KDB tree、R tree、CELL tree、八叉树等。有了这些关系，我们就可以实现点云的降采样，计算特征向量，点云匹配，点云拆分等功能。

原理概述

k-d tree（ k- tree）是计算机科学中用于在k维空间中一些点建立关系的数据结构。它是一个包含特定约束的二叉搜索树。k-d tree对于范围搜索和最近邻居搜索非常有用。我们通常只处理三维空间的点云，因此我们所有的k-d树都是三维空间的。

k-d树的每个级别都使用垂直于相应轴的超平面沿特定维度拆分所有子级。在树的根部，所有子项都将根据第一维进行拆分（即，如果第一维坐标小于根，则它将位于左子树中，如果大于根，则显然位于右边的子树）。树中向下的每个级别都在下一个维度上划分，其他所有元素都用尽后，将返回到第一个维度。他们构建k-d树的最有效方法是使用一种分区方法，例如快速排序所用的一种方法，将中值点放置在根上，所有具有较小一维值的东西都放置在根部，而右侧则更大。然后，在左右两个子树上都重复此过程，直到要分区的最后一棵树仅由一个元素组成。

KD树的原理查资料即可。

手写kd树

//
// Created by pf on 2023/10/26.
//
#pragma once
#include 
#include 
#include 
#include 
#include 
#include 
#include 
struct KDNode
{int index;int axis;KDNode *left;KDNode *right;KDNode(int index, int axis, KDNode *left = nullptr, KDNode *right = nullptr){this->index = index;this->axis = axis;this->left = left;this->right = right;}
};template <class T>
class KDTree
{
private:int dimension;KDNode *root;KDNode *build(std::vector<T> &);std::set<int> visited;std::stack<KDNode *> queueNode;std::vector<T> m_data;void release(KDNode *);void printNode(KDNode *);int chooseAxis(std::vector<T> &);void dfs(KDNode *, T);// 点与点之间的距离inline double distanceT(KDNode *, T);inline double distanceT(int, T);// 点与超平面的距离inline double distanceP(KDNode *, T);// 检查父节点超平面是否在超球体中inline bool checkParent(KDNode *, T, double);public:KDTree(std::vector<T> &, int);~KDTree();void Print();int findNearestPoint(T);
};template <class T>
KDTree<T>::KDTree(std::vector<T> &data, int dim)
{dimension = dim;m_data = data;root = build(data);
}template <class T>
KDTree<T>::~KDTree()
{release(root);
}template <class T>
void KDTree<T>::Print()
{printNode(root);
}template <class T>
void KDTree<T>::release(KDNode *node)
{if (node){if (node->left)release(node->left);if (node->right)release(node->right);delete node;node = nullptr;}
}template <class T>
KDNode *KDTree<T>::build(std::vector<T> &data)
{if (data.empty())return nullptr;std::vector<T> temp = data;int mid_index = static_cast<int>(data.size() / 2);int axis = data.size() > 1 ? chooseAxis(temp) : -1;std::sort(temp.begin(), temp.end(), [axis](T a, T b){ return a[axis] < b[axis]; });std::vector<T> leftData, rightData;leftData.assign(temp.begin(), temp.begin() + mid_index);rightData.assign(temp.begin() + mid_index + 1, temp.end());KDNode *leftNode = build(leftData);KDNode *rightNode = build(rightData);KDNode *rootNode;rootNode = new KDNode(temp[mid_index].index, axis, leftNode, rightNode);return rootNode;}template <class T>
void KDTree<T>::printNode(KDNode *node)
{if (node){std::cout << "Index: " << node->index << "\tAxis: " << node->axis << std::endl;printNode(node->left);printNode(node->right);}
}template <class T>
int KDTree<T>::chooseAxis(std::vector<T> &data)
{int axis = -1;double maxVar = -1.0;size_t size = data.size();for (int i = 0; i < dimension; i++)// x数据方向的方差最大进行设置axis，也就是这个方向是最离散的{double mean = 0;double Var = 0;for (int j = 0; j < size; j++){mean += static_cast<double>(data[j][i]);}mean = mean / static_cast<double>(size);for (int j = 0; j < size; j++){Var += (static_cast<double>(data[j][i]) - mean) * (static_cast<double>(data[j][i]) - mean);}Var = Var / static_cast<double>(size);// 找到最大的这个方差对应的轴if (Var > maxVar){axis = i;// 这个就是这个轴对应最大方差的轴maxVar = Var;}}return axis;
}template <class T>
int KDTree<T>::findNearestPoint(T pt)
{visited.clear();while (!queueNode.empty())queueNode.pop();double min_dist = DBL_MAX;int resNodeIdx = -1;dfs(root, pt);while (!queueNode.empty()){KDNode *curNode = queueNode.top();queueNode.pop();double dist = distanceT(curNode, pt);if (dist < min_dist){min_dist = dist;resNodeIdx = curNode->index;}if (!queueNode.empty()){KDNode *parentNode = queueNode.top();int parentAxis = parentNode->axis;int parentIndex = parentNode->index;if (checkParent(parentNode, pt, min_dist)){if (m_data[curNode->index][parentNode->axis] < m_data[parentNode->index][parentNode->axis])dfs(parentNode->right, pt);elsedfs(parentNode->left, pt);}}}return resNodeIdx;
}template <class T>
void KDTree<T>::dfs(KDNode *node, T pt)
{if (node){if (visited.find(node->index) != visited.end())return;queueNode.push(node);visited.insert(node->index);if (pt[node->axis] <= m_data[node->index][node->axis] && node->left)dfs(node->left, pt);else if (pt[node->axis] >= m_data[node->index][node->axis] && node->right)dfs(node->right, pt);else if ((node->left == nullptr) ^ (node->right == nullptr)){dfs(node->left, pt);dfs(node->right, pt);}}
}template <class T>
double KDTree<T>::distanceT(KDNode *node, T pt)
{double dist = 0;for (int i = 0; i < dimension; i++){dist += (m_data[node->index][i] - pt[i]) * (m_data[node->index][i] - pt[i]);}return sqrt(dist);
}template <class T>
double KDTree<T>::distanceT(int index, T pt)
{double dist = 0;for (int i = 0; i < dimension; i++){dist += (m_data[index][i] - pt[i]) * (m_data[index][i] - pt[i]);}return sqrt(dist);
}template <class T>
double KDTree<T>::distanceP(KDNode *node, T pt)
{int axis = node->axis;double dist = static_cast<double>(pt[axis] - m_data[node->index][axis]);return dist >= 0 ? dist : -dist;
}template <class T>
bool KDTree<T>::checkParent(KDNode *node, T pt, double distT)
{double distP = distanceP(node, pt);return distP <= distT;
}