ME4932 科学研究与创新实践

基于3D点云的特征识别算法研究

前言

课题背景与研究目标

轨道车辆转向架在工作过程中承受着安装部件的工作载荷、制动、牵引和惯性力，对行车安全起着至关重要的作用。转向架构架大部分结构都有钢板焊接而成，而焊缝表面的几何形状会影响焊缝周围的应力及焊缝的疲劳强度；因此，转向架之间的焊缝质量直接影响转向架的性能。

焊缝打磨主要有人工打磨与机器打磨两种方式。当前，此类焊缝打磨作业主要依靠人工手持角磨机以试凑方式进行打磨，效率非常低，生产环境恶劣，且难以保证打磨后焊缝外形的一致性及修磨质量，因此迫切需要实现焊缝的自动打磨。工业机器人特有的柔顺性、可编译性、适应能力强、价格低等特点使其非常适合此类打磨作业，选用工业机器人实现自动化打磨加工已经成为当前打磨作业的一个重要发展方向。借助现有的机器自动打磨技术，可以解决转向架横梁、侧梁组成平焊缝的粗磨问题。

本课题将通过线激光扫描仪获取转向架的点云数据，提取出转向架焊缝的位置、厚度、宽度等信息，再通过刀具规划软件Mastercam以及机器人离线编程软件Robomaster得到打磨轨迹点，制定机器人自动打磨焊缝的加工方案，代替工人手持角磨机进行打磨作业，采用力控去除和定尺结构相结合的方法有效保证焊缝打磨后的高度大小与一致性问题。最后通过实验验证和打磨结果检测，证明本方案的可行性和适用性，为焊缝自动打磨提供一定的参考依据。

主要研究内容与计划

首先，进行转向架点云数据的获取。研究预计采用线激光扫描仪获取转向架的点云数据，由于转向架体积较大，单次扫描过程并不能提取出所有需要的三维信息，因此需设计多次扫描过程，设计原则是既要保证扫描的完整性，也要花费尽量少的时间，以满足生产节拍的要求。

其次，根据手眼标定矩阵与扫描过程对应的旋转平移矩阵，将不同扫描过程的图像坐标系下的多个点云变换到机器人基坐标系下。由于最初的点云数据存在噪声点、离群点以及点分布不均匀等问题，需根据焊缝特征识别算法需求在进行点云特征识别之前对点云数据进行预处理。

最后，提取出转向架点云数据的特征信息。根据打磨需求，工控机需要输入焊缝的位置特征、厚度与宽度等信息，然后控制机器人带着打磨头运动到指定位置对焊缝进行打磨。因此，提取特征信息需根据焊缝特征来定。转向架焊缝由六条直线焊缝与四条转角焊缝组成————

六条直线焊缝均分布在两个平面相交形成的夹角上，故可以根据两个平面的位置信息确定直线焊缝的位置，再通过焊缝的形状特征得到焊缝的宽度与厚度；
四条转角焊缝分布于斜圆柱面与平面相交形成的夹角上，很难直接得到焊缝的特征信息，因此采用点云配准的方法。该方法首先利用刀具规划软件Mastercam以及机器人离线编程软件Robomaster得到在某一点云下转角打磨轨迹点，再将待打磨工件的点云数据与该点云进行配准，进而得到待打磨工件四条转角焊缝的打磨轨迹点。

环境准备

Linux系统安装

Linux相比其他操作系统，具有代码开源、内核精简、安全稳定高效、多用户多任务多线程等诸多优点，因而非常适用于编程和开发工作。
我的笔记本上已经提前装好了Ubuntu 20.04 LTS发行版，和Windows组成双系统进行日常使用。具体安装过程此处不再赘述。

C++编程环境配置

安装C++整套编译工具

Ubuntu在缺省情况下，并不提供C/C++的编译环境，因此需要手动安装。Ubuntu提供了一个build-essential软件包。查看该软件包的依赖关系并安装：

查看编译器、调试器的版本信息：

使用g++编译工具对C++程序进行编译，并运行生成的可执行文件。代码示例：

#include <iostream>
using namespace std;

int main(int argc,char** argv)
{
  cout<<"Hello World!"<<endl;
  return 0;
}

编译并运行程序~~~

C++编译工具至此配置完毕！

安装编译配置工具—CMake

一个大型C++项目通常含有十几个类，各类间还存在复杂的依赖关系。其中一部分要编译成可执行文件，另一部分编译成库文件。若仅靠g++命令，需输入大量的编译指令，整个编译过程会变得异常繁琐。因此，对于C++项目，使用一些工程管理工具会更高效。

CMake是一个跨平台的、开源的编译配置工具。CMake允许开发者编写与平台无关的CMakeLists.txt文件定制整个编译流程，然后用户通过cmake命令生成本地化Makefile文件，最后用make命令编译整个工程。本课题将使用CMake工具对C++项目进行管理。

附：CMake工具官网：https://cmake.org/
CMakeLists快速入门教程：https://github.com/ganleiboy/CMakeTutorial

安装CMake工具并查询版本：

CMake是一种跨平台的编译工具，比make更为高级，使用起来也更方便。CMake主要是编写CMakeLists.txt文件，编译步骤可分为两步——用cmake命令将CMakeLists.txt文件转化为make所需要的makefile文件，最后用make命令编译源码生成可执行程序或共享库。

CMakeLists.txt的结构组成：

# 该模板适用于源文件都在文件夹/src中，头文件都在文件夹/include中，
# 且调用第三方库的情况。

# 设置运行本配置文件所需的CMake最低版本
cmake_minimum_required(VERSION 2.8)

# 设置工程名称为"Demo"（可随意设置）
project(Demo)

# 设定编译参数
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE "Release")  # 也可以是Debug

# 设定源码列表，查找指定目录(./src)中的所有源文件，将名称全保存在DIR_SRC变量中
aux_source_directory(./src DIR_SRC)

# 设定头文件路径(./include)
include_directories(./include)

# 查找并添加OpenCV的头文件目录
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

# 将源文件编译成可执行文件，文件名为main（可随便取名，保存在当前目录下）
add_executable(main ${DIR_SRC})
target_link_libraries(main ${OpenCV_LIBS})  # 可执行文件链接OpenCV库

cmake指向CMakeLists.txt所在目录的上一级目录，而cmake后会生成很多编译的中间文件以及makefile文件，所以一般建议新建一个目录，专门用来编译。编译一个cmake工程的基本步骤如下：

mkdir build  # 创建一个用于存放中间文件的文件夹
cd build    # 进入build目录
cmake ..    # 根据CMakeLists.txt对工程进行分析，生成中间文件和makefile文件
make       # 根据生成的makefile文件编译程序，生成可执行文件和库文件]

C/C++集成开发环境—CLion

我平日编程用得最多的IDE是Visual Studio Code，但在学长推荐下决定还是尝试一下CLion. 它是Jetbrains公司旗下的一款专为开发C/C++设计的跨平台IDE，以IntelliJ为基础进行设计，非常适合开发。

附：CLion官网：https://www.jetbrains.com/clion/

该软件经官方授权，可通过学创软件授权中心下载和使用。命令行安装步骤如下：

1
2
3

tar -zxvf jetbrains-toolbox-1.21.9712.tar.gz
cd jetbrains-toolbox-1.21.9712
./clion.sh

通过Clion开发、编译简单的CMake项目~~~

点云PCL库入门

PCL库基本介绍

PCL(Point Cloud Library)是在点云相关基础研究上建立起的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，可以实现点云的获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等操作。PCL在3D信息获取与处理上的地位，已然与OpenCV在2D上的地位等同。
附：PCL官网：https://pointclouds.org

PCL库的发展历史

PCL起初是ROS(Robot Operating System)框架的一部分，由Radu Bogdan Rusu在德国慕尼黑工业大学读博期间开发和维护的开源项目，于2009年底由Willow Garage公司进一步开发完成。PCL主要应用于移动机器人的场景感知等研究领域。随着各算法模块的积累，于2011年独立出来，全球知名大学、研究机构加入项目，使其获得迅猛的发展，目前已发布到1.12.0版本。

PCL及依赖库的安装

安装PCL依赖的第三方库

PCL库的安装依赖许多第三方库，如Boost、Eigen、FLANN、VTK、CUDA、OpenNI、Qhull等。安装过程极其繁琐，涉及许多库的版本问题，我在这一阶段也花了较长的时间。

命令行安装各种依赖（Ubuntu 20.04）~~~

sudo apt-get update
sudo apt-get install git build-essential linux-libc-dev
sudo apt-get install cmake cmake-gui
sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev
sudo apt-get install mpi-default-dev openmpi-bin openmpi-common
sudo apt-get install libflann1.9 libflann-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libboost-all-dev
sudo apt-get install libqhull* libgtest-dev
sudo apt-get install freeglut3-dev pkg-config
sudo apt-get install libxmu-dev libxi-dev
sudo apt-get install mono-complete
sudo apt-get install libopenni2-dev

源码安装VTK库

VTK库若用apt安装会出现.so文件缺失的情况，因此必须下载源码安装。先安装VTK库的依赖项X11和OpenGL~~~

1
2
3

sudo apt-get install libx11-dev libxext-dev libxtst-dev libxrender-dev libxmu-dev libxmuu-dev
sudo apt-get install libgl1-mesa-dev libglu1-mesa-dev

然后到VTK官网下载源码（当前版本为9.2.2）并解压缩。在安装目录下打开命令行，输入cmake-gui打开cmake图形界面——配置“where is the source code”的路径为vtk-9.2.2所在的目录；在vtk-9.2.2目录新建build文件夹，配置“where to build the binaries”为build文件夹；勾选Grouped和Advanced，点击Configure。配置完成后提示configure done，点击generate按钮，会在build文件夹下生成工程文件。

切换目录到vtk-9.2.2文件夹下的build文件夹，然后打开命令行，依次输入~~~

1 2	sudo make sudo make install

至此VTK-9.2.2安装成功。

安装PCL库

最后命令行安装pcl-1.12.0

# 从github上克隆PCL源码
git clone https://github.com/PointCloudLibrary/pcl.git
cd pcl
mkdir build
cd build
# 用cmake分析整个源代码
cmake -DCMAKE_BUILD_TYPE=None -DBUILD_GPU=ON -DBUILD_apps=ON -DBUILD_examples=ON ..
sudo make
sudo make install

编译过程中出现"AUTOGEN: No valid Qt version found for target"报错，需要安装qt5库以满足依赖关系。

sudo apt-get install build-essential  # 安装Qt5的组件
sudo apt-get install qtbase5-dev qtchooser qt5-qmake qtbase5-dev-tools  # 安装Qt的开发工具
sudo apt-get install qtcreator  # 安装qtcreator
sudo apt-get install qt5*  # 安装qt

运行完上述所有命令后执行"locate pcl-1.12"，发现安装目录在/usr/local/include下，即可确认安装成功。

测试PCL库

测试代码pcl_test.cpp内容如下：

#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>

int main(int argc, char **argv) {
    std::cout << "Test PCL !!!" << std::endl;
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
    uint8_t r(255), g(15), b(15);
    for (float z(-1.0); z <= 1.0; z += 0.05)
    {
      for (float angle(0.0); angle <= 360.0; angle += 5.0)
      {
	pcl::PointXYZRGB point;
	point.x = 0.5 * cosf (pcl::deg2rad(angle));
	point.y = sinf (pcl::deg2rad(angle));
	point.z = z;
	uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
		static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
	point.rgb = *reinterpret_cast<float*>(&rgb);
	point_cloud_ptr->points.push_back (point);
      }
      if (z < 0.0) { r -= 12; g += 12; }
      else  { g -= 12; b += 12; }
    }
    point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
    point_cloud_ptr->height = 1;
    
    pcl::visualization::CloudViewer viewer ("test");
    viewer.showCloud(point_cloud_ptr);
    while (!viewer.wasStopped()){ };
    return 0;
}

CMakeLists.txt文件内容如下：

cmake_minimum_required(VERSION 2.6)
project(pcl_test)

# 在find_package命令前需提供PCL的cmake文件所在的目录，或者添加至环境变量
set(PCL_DIR /usr/local/include/pcl-1.12/share/pcl-1.12)
find_package(PCL 1.12 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_test pcl_test.cpp)

target_link_libraries (pcl_test ${PCL_LIBRARIES})

将pcl_test.cpp和CMakeLists.txt放在一个目录下，执行如下命令：
（或者直接在Clion下构建并运行）

mkdir build
cd build
cmake ..
make
./pcl_test

若可以看到一个漂亮的3D模型，则pcl及其依赖库安装成功⬇

转向架点云数据的获取和配准

柯学长布置的任务清单如下：

项目的总体任务：提取出焊缝的特征数据
注：需提前思考焊缝打磨的特征数据是什么，根据自己的思考提取相应的特征数据。

现阶段的任务（10-12月）：

点云数据的读取；

点云数据的可视化；

将三个点云数据通过旋转平移矩阵变换到一个坐标系下。

文件清单说明：
data1, data2, data3是扫描的点云数据。
transform.txt文件里的数据分别是data1，data2，data3的相机坐标系与世界坐标系之间的旋转平移变换矩阵。

点云数据的读取

txt点云数据格式说明：每个点的坐标数据单独占据一行，每一行的三维坐标之间用空格间隔（如下图所示）。

// CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(pcl_visualize)

find_package(PCL 1.12 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_visualize pcl_visualize.cpp)

target_link_libraries (pcl_visualize ${PCL_LIBRARIES})

点云主要通过激光扫描或双（多）目相机摄影获得，其主要信息包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）。根据不同应用场景，PCL库提供了诸如pcl::PointXYZ、pcl::PointXYZI、pcl::PointXYZRGB等点云数据类型。PCL的点云类为pcl::PointCloud，其中PointT为类模板。

下面这段代码通过输入文件流ifstream和字符串流stringstream，实现了从txt文本文件中读取数百万个点的三维坐标，并存储在点云实例的过程。

// pcl_visualize.cpp

void CreateCloudFromTxt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // 从txt文件中读取三维点云数据，并存储在PCL点云实例中
    std::ifstream data(file_path);
    std::string line;
    pcl::PointXYZ point;
    while (getline(data, line, '\n')) {
        std::stringstream ss(line);
        ss >> point.x;
        ss >> point.y;
        ss >> point.z;
        cloud->push_back(point);
    }
}

点云数据的可视化

为了能够方便地展现点云原型，或查看点云算法的运行效果，PCL提供了pcl::visualization库,可以通过简短的几行代码，查看点云的三维特征。下面这段代码使用了CloudViewer子类，完成点云数据的可视化。

// pcl_visualize.cpp

# include <iostream>
# include <pcl/point_types.h>
# include <pcl/visualization/cloud_viewer.h>

int main(int argc, char** argv)
{
    //读取点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    CreateCloudFromTxt("../data1.txt", cloud);
    
    //可视化点云
    pcl::visualization::CloudViewer viewer("cloud");
    viewer.showCloud(cloud);
    while(!viewer.wasStopped()) {  };
    
    return 0;
}

可视化效果展示如下：

点云图	data1	data2	data3
俯视图
侧视图

点云数据的配准

点云的配准，即通过两点云间的旋转平移矩阵，将源点云变换到与目标点云相同的坐标系下。设原始坐标为${P_0(x_0,y_0,z_0)}$，变换后的坐标为${P(x,y,z)}$，则点的平移描述为${x=x_0+x_t}$， ${y=y_0+y_t}$， ${z=z_0+z_t}$. 令${T(x_t,y_t,z_t)}$为平移向量，则可得到${P=P_0+T}$.
点云的旋转可通过欧拉角、旋转矩阵、旋转向量、四元数等方式来表达。欧拉角定义点云分别绕XYZ三个轴进行旋转，旋转角度分别为${\alpha、\beta、\gamma}$。每做一次旋转可得一个旋转矩阵，分别对应${R_x(\alpha)、R_y(\beta)、R_z(\gamma)}$。总的旋转矩阵R等于这三个矩阵相乘。即

旋转矩阵直接定义一个三维矩阵R，相当于欧拉角乘积后的表现形式。将旋转矩阵直接左乘点云坐标即可完成变换。其中R必须是正交矩阵，且模长为1.
Eigen库有多种子类可以实现点云的变换，其中较常用到的是Matrix4f类和Affine3f类。两个类初始化后可相互转换。Matrix4f适用于已给出旋转矩阵的情况，需手动输入3x3旋转矩阵和1x3平移向量的值；Affine3f适用于仅给出欧拉角的情况，其多出了translation()和rotate()两个类函数，分别进行点云的平移和旋转运算。
以下是将三个焊缝点云数据变换至一个坐标系下的代码示例：

// pcl_visualize.cpp
# include <pcl/common/transforms.h>

int main(int argc, char** argv)
{
    //读取点云
    ...
    //可视化点云
    ...
    //通过Matrix4f矩阵变换点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();  //转换矩阵初始化为单位矩阵
    float tfmatrix[4][4] = {-1,0,0,1650,0,1,0,-455,0,0,1,1365,0,0,0,1};
    for(int i=0; i<4; ++i)
        for (int j=0; j<4; ++j)
            transform(i,j) = tfmatrix[i][j];  //为转换矩阵赋值
            
    pcl::transformPointCloud(*cloud, *transformed_cloud, transform);
    
    //合并原始点云和转换后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    *all_cloud = *cloud + *transformed_cloud;
    
    //可视化全部点云
    pcl::visualization::CloudViewer viewer2("all_cloud");
    viewer2.showCloud(all_cloud);
    while(!viewer2.wasStopped()) {  };
    
    return 0;
}

将原始点云分别和变换后点云合并后，得到的点云图可视化效果如下：

data1	data2	data3

（注：所有变换后点云位于原始点云的右下方）

将三个变换后点云合并，即可得到同坐标系下的转向架点云图：

俯视图	侧视图

通过点云滤波和分割提取平面特征

在上一节中，已经将三个点云数据通过旋转平移变换到机器人基坐标系下，得到完整的转向架点云图。而本项目的目标是提取转向架焊缝的位置、厚度与宽度等信息。从三维空间中提取直线焊缝特征的方法有两种——1. 找到经过焊缝的两个平面，两平面的交线即为焊缝；2. 找到转向架上和焊缝平行的边缘线，再求出从边缘线到焊缝的平移向量。

这里采用第一个思路，找出转向架的4个大平面，通过平面的交线得到直线焊缝的位置信息。平面特征的获取，可分为以下三个步骤进行——数据预处理、平面模型分割、索引滤波。

点云数据的预处理

初始点云存在大量噪声点、离群点，若不予以消除，会导致平面拟合模型误差增大；此外初始点云数据量较大（超过300万个点），会增加计算机内存和计算开销，降低求解效率。综合上述原因，点云数据必须进行预处理。

点云的降采样

PCL点云库提供了VoxelGrid类进行降采样处理，VoxelGrid类在输入点云数据上创建一个三维体素网络，网络由空间中一组微小的三维长方体构成。在每个体素中，所有存在的点将近似成质心，从而实现数据量的减少。

点云降采样的代码示例如下：

// planar_segmentation.cpp

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>

int main(int argc, char** argv)
{
    // 从硬盘加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("../original_cloud.pcd", *cloud);
    std::cout << "->共加载了" << cloud->size() << "个数据点" << std::endl;

    // 点云降采样
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> sor1;  // 创建一个立体栅格对象
    sor1.setInputCloud (cloud);  // 导入待降采样的点云
    sor1.setLeafSize (1.0f, 1.0f, 1.0f);  // 设置滤波体素的栅格为1cm3的立方体
    sor1.filter (*filtered_cloud);  // 执行降采样操作
    std::cout << "->下采样后剩余" << filtered_cloud->size() << "个数据点" << std::endl;
    
    return 0;
}

经过上述代码，点云数据量由初始的3068795个下降至313218个，约为原来的1/10，使得平面拟合的计算量大幅减少。

点云的离群点去除

激光扫描通常生成不同点密度的点云数据集，且测量误差会导致稀疏的异常值，为局部点云特性的估计带来干扰。PCL点云库提供了StatisticalOutlierRemoval类来去除噪声点。其主要原理是：对每个点的邻域进行统计分析，计算每个点到所有相邻点的平均距离；假设得到的分布为高斯分布，可计算出均值μ和标准差σ；则所有与邻域距离大于 μ + std_mul*σ的点可视为离群点。（其中std_mul是标准差倍数的阈值，需根据数据集特点手动指定）

去除点云离群点的代码示例如下：

// planar_segmentation.cpp

#include <pcl/filters/statistical_outlier_removal.h>

int main(int argc, char** argv)
{
    // 从硬盘加载点云数据
    ...
    // 点云降采样
    ...
    // 去除离群点（去噪）
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor2;
    sor2.setInputCloud(filtered_cloud);  // 导入待去噪的点云
    sor2.setMeanK(50);  // 设置统计平均距离的邻域点个数为50
    sor2.setStddevMulThresh(2);  // 设置判断是否为离群点的阀值
    sor2.filter(*filtered_cloud);
    std::cout << "->去噪后剩余" << filtered_cloud->size() << "个数据点" << std::endl;
    
    return 0;
}

经过上述代码，消除了约四千个因测量误差导致的噪声点，提高了点云表面的平滑性，为平面拟合做好了数据准备。

点云的平面模型分割

对于二维平面的直线拟合，我们常常使用最小二乘算法求解；然而这一算法并不适用空间复杂点云的平面拟合，主要原因是无法预知哪些点集合归属于该平面。
空间点云普遍使用的是随机采样一致性算法（RANSAC）.与最小二乘法不同的是，该算法并非用所有初始数据去获取一初始解，然后剔除无效数据；而是使用满足可行条件的尽量少的初始数据，并使用一致性数据集扩大它，这是一种寻找模型去拟合数据的思想，在计算机视觉领域中有广泛的应用。
RANSAC平面拟合的过程如下：

在初始点云中随机选取三个点，计算其对应的平面方程${Ax+By+Cz+D=0}$.
计算所有点至该平面的代数距离${d_i=|Ax_i+By_i+Cz_i+D|}$，选取阈值${d_{threshold}}$，若${d_i <= d_{threshold}}$，则该点可被认为是模型内样本点(inliers)，否则为模型外样本点(outliers)，记录当前内点的个数。
重复上述步骤，选取最佳拟合参数，即内点数量最多的平面对应的模型参数；每次迭代末尾都根据期望的误差率、最佳内点个数、总样本个数等计算迭代结束评判因子，从而决定是否停止迭代。

PCL中的sample_consensus模块提供了RANSAC平面拟合模型。我们可以实例化一个分割对象SACSegmentation，并设置方法类型为RANSAC. 根据数据集特点设置最大迭代次数和距离阈值，即可进行训练。最后通过ModelCoefficients类导出估计的平面参数。

// planar_segmentation.cpp

#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main(int argc, char** argv)
{
    // 从硬盘加载点云数据
    ...
    // 点云降采样
    ...
    // 去除离群点（去噪）
    ...
    // 平面模型分割
    pcl::SACSegmentation<pcl::PointXYZ> seg;  // 创建点云分割对象
    pcl::PointIndices::Ptr inlier_indices(new pcl::PointIndices);  // 创建存储平面内点索引的对象
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);  // 创建存储模型参数的对象

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);  // 设置分割模型类别为平面
    seg.setMethodType(pcl::SAC_RANSAC);  // 使用随机采样一致性算法（RANSAC）
    seg.setMaxIterations(1000);  // 设置最大迭代次数
    seg.setDistanceThreshold(1.5);  // 设置距离阈值，与平面距离小于1.5的点作为内点

    int i = 1, nr_points = (int)filtered_cloud->points.size();
    while (filtered_cloud->points.size() > 0.45 * nr_points)  // 当还有至少45%的原始点云数据时
    {
        seg.setInputCloud(filtered_cloud);  // 导入待分割点云
        seg.segment(*inlier_indices, *coefficients);  // 执行分割操作

        if (inlier_indices->indices.size () == 0) {
            std::cout << "无法从指定数据集中拟合平面模型！" << std::endl;
            break;
        }

        std::cout << "第" << i << "个平面方程为：" << coefficients->values[0] << "x + "
                  << coefficients->values[1] << "y + " << coefficients->values[2] << "z +  "
                  << coefficients->values[3] << " = 0" << std::endl;
                  ...
    }
    
    return 0;
}

（注：上述代码的输出放在了小节总结里）

点云的索引滤波

在前一小节中，通过segment分割操作已经获得内点对应的索引inlier_indices，接下来可借助PCL库中的extract_indices滤波器完成对索引的滤波。
由于有多个平面需要拟合，此处的“分割+索引滤波”操作也可分多步进行——通过设置extract.setNegative的值决定选取内点或外点，每次迭代前将估计出的外点作为转向架点云进行训练，直至剩余点数低于初始点数的45%为止。

// planar_segmentation.cpp

#include <pcl/filters/extract_indices.h>

int main(int argc, char** argv)
{
    // 从硬盘加载点云数据
    ...
    // 点云降采样
    ...
    // 去除离群点（去噪）
    ...
    // 平面模型分割
    ...
    while() {
    	...
      // 索引滤波器滤波
      // 分离内点
      pcl::ExtractIndices<pcl::PointXYZ> extract;  // 创建索引滤波器对象
      pcl::PointCloud<pcl::PointXYZ>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);

      extract.setInputCloud(filtered_cloud);
      extract.setIndices(inlier_indices);
      extract.setNegative(false);  // false表示选取内点
      extract.filter(*inlier_cloud);  // 执行滤波操作
      std::cout << "内点总数为: " << inlier_cloud->width * inlier_cloud->height << std::endl;

      // 提取外点
      pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);
      extract.setNegative(true);  // true表示选取外点
      extract.filter(*outlier_cloud);  // 滤波操作
      std::cout << "外点总数为: " << outlier_cloud->width * outlier_cloud->height << std::endl;

      // 将分割得到的平面点云保存至硬盘
      std::stringstream ss1, ss2;
      ss1 << "inliercloud_" << i << ".pcd";
      ss2 << "outliercloud_" << i << ".pcd";
      pcl::io::savePCDFileASCII(ss1.str(), *inlier_cloud);
      pcl::io::savePCDFileASCII(ss2.str(), *outlier_cloud);

      // 将外点作为新点云迭代得到多个平面
      filtered_cloud.swap(outlier_cloud);
      i++;
    }
    return 0;  
}

小节总结

本小节使用的所有源代码整理如下：

// CMakeLists.txt
cmake_minimum_required(VERSION 2.8)

project(planar_segmentation)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (planar_segmentation planar_segmentation.cpp)
target_link_libraries (planar_segmentation ${PCL_LIBRARIES})

// planar_segmentation.cpp

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main(int argc, char** argv)
{
    // 从硬盘加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("../original_cloud.pcd", *cloud);
    std::cout << "->共加载了" << cloud->size() << "个数据点" << std::endl;

    // 点云降采样
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> sor1;  // 创建一个立体栅格对象
    sor1.setInputCloud(cloud);  // 导入待降采样的点云
    sor1.setLeafSize(1.0f, 1.0f, 1.0f);  // 设置滤波体素的栅格为1cm3的立方体
    sor1.filter(*filtered_cloud);  // 执行降采样操作
    std::cout << "->下采样后剩余" << filtered_cloud->size() << "个数据点" << std::endl;

    // 去除离群点（去噪）
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor2;
    sor2.setInputCloud(filtered_cloud);  // 导入待去噪的点云
    sor2.setMeanK(50);  // 设置统计平均距离的邻域点个数为50
    sor2.setStddevMulThresh(2);  // 设置判断是否为离群点的阀值
    sor2.filter(*filtered_cloud);
    std::cout << "->去噪后剩余" << filtered_cloud->size() << "个数据点" << std::endl;

    // 平面模型分割
    pcl::SACSegmentation<pcl::PointXYZ> seg;  // 创建点云分割对象
    pcl::PointIndices::Ptr inlier_indices(new pcl::PointIndices);  // 创建存储平面内点索引的对象
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);  // 创建存储模型参数的对象

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);  // 设置分割模型类别为平面
    seg.setMethodType(pcl::SAC_RANSAC);  // 使用随机采样一致性算法（RANSAC）
    seg.setMaxIterations(1000);  // 设置最大迭代次数
    seg.setDistanceThreshold(1.5);  // 设置距离阈值，与平面距离小于1.5的点作为内点

    int i = 1, nr_points = (int)filtered_cloud->points.size();
    while (filtered_cloud->points.size() > 0.45 * nr_points)  // 当还有至少45%的原始点云数据时
    {
        seg.setInputCloud(filtered_cloud);  // 导入待分割点云
        seg.segment(*inlier_indices, *coefficients);  // 执行分割操作

        if (inlier_indices->indices.size () == 0) {
            std::cout << "无法从指定数据集中拟合平面模型！" << std::endl;
            break;
        }

        std::cout << "第" << i << "个平面方程为：" << coefficients->values[0] << "x + "
                  << coefficients->values[1] << "y + " << coefficients->values[2] << "z +  "
                  << coefficients->values[3] << " = 0" << std::endl;

        // 索引滤波器滤波
        // 分离内点
        pcl::ExtractIndices<pcl::PointXYZ> extract;  // 创建索引滤波器对象
        pcl::PointCloud<pcl::PointXYZ>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);

        extract.setInputCloud(filtered_cloud);
        extract.setIndices(inlier_indices);
        extract.setNegative(false);  // false表示选取内点
        extract.filter(*inlier_cloud);  // 执行滤波操作
        std::cout << "内点总数为: " << inlier_cloud->width * inlier_cloud->height << std::endl;

        // 提取外点
        pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        extract.setNegative(true);  // true表示选取外点
        extract.filter(*outlier_cloud);// 滤波操作
        std::cout << "外点总数为: " << outlier_cloud->width * outlier_cloud->height << std::endl;

        // 将分割得到的平面点云保存至硬盘
        std::stringstream ss1, ss2;
        ss1 << "inliercloud_" << i << ".pcd";
        ss2 << "outliercloud_" << i << ".pcd";
        pcl::io::savePCDFileASCII(ss1.str(), *inlier_cloud);
        pcl::io::savePCDFileASCII(ss2.str(), *outlier_cloud);

        // 将外点作为新点云迭代得到多个平面
        filtered_cloud.swap(outlier_cloud);
        i++;
    }
    
    return 0;
}

源代码所有输出结果如下：

->共加载了3068795个数据点
->下采样后剩余313218个数据点
->去噪后剩余309175个数据点
第1个平面方程为：-0.359203x + 0.00140967y + 0.933258z +  -641.473 = 0
内点总数为: 81926
外点总数为: 227249
第2个平面方程为：-0.00400156x + -0.964687y + -0.26337z +  89.7737 = 0
内点总数为: 30474
外点总数为: 196775
第3个平面方程为：0.998895x + 0.00199732y + -0.0469618z +  -1451.53 = 0
内点总数为: 34455
外点总数为: 162320
第4个平面方程为：0.00321649x + -0.957514y + 0.28837z +  -954.657 = 0
内点总数为: 25908
外点总数为: 136412

将求解得到的四个平面做上色处理，再与转向架其余点云合并，得到如下效果图：

俯视图	侧视图

前视图	后视图

通过平面法线估计和边缘检测提取焊缝特征

在前一节中，我们成功拟合并提取出四个关键平面的点云集合。由于焊缝多数位于上述平面的边缘，因而可以通过法线估计+边缘检测提取所有的平面边缘，再通过设置合理的坐标阈值滤去不是焊缝的点。

计算点云的平均点间距

平面法线估计的一个重要参数是邻域半径的选择，邻域半径设置得过小，法线参数易受噪声点的影响而出错；邻域半径设置得过大，会使估计速度变慢，且不利于平面边界的判断。
在计算平面法线之前，有必要统计出点云的平均点距离，以便确定邻域半径的大致范围。PCL库提供了k维树结构，用于三维点云的距离和最近邻搜索。

// MeanDistance.cpp

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("../inliercloud_1.pcd", *cloud);

    pcl::search::KdTree<pcl::PointXYZ> tree;  // 实例化一个Kd树对象
    tree.setInputCloud(cloud);  // 设置输入点云

    // 计算平均点间距
    double meanDistance = 0;
    std::vector<int> indices(2);
    std::vector<float> sqr_distances(2);
    for (int i = 0; i < cloud->size(); i++)
        if (tree.nearestKSearch(cloud->points[i], 2, indices, sqr_distances) > 1)
            // K近邻查找的成员函数，将目标点的最近邻索引和平方距离存储在容器中，返回找到的邻域数量
            meanDistance += sqrt(sqr_distances[1]);
    meanDistance /= cloud->size();

    std::cout << "平均点间距: " << meanDistance << " cm" << std::endl;
    return 0;
}

计算得到的平均点间距为0.652724cm，邻域半径设置为分辨率的10倍左右时，法线估计效果较好，故取邻域半径为5.

基于法线估计实现的PCL边界检测与提取

法线估计

表面法线是几何体表面的重要属性，在计算机视觉领域有广泛应用（例如，通过阴影和其他视觉影响推断光源的位置）。法线估计的思路大致有以下两种：

从已获取的点云数据集中得到潜在表面，并用表面网格化技术，来计算网格的表面法线。
使用近似值直接从点云数据集中推断表面法线。

PCL采用了较简单的第二种思路，即将点的法线估计问题近似为估计与曲面相切的平面法线问题，进而转化为最小二乘平面拟合估计问题。因此，表面法线估计问题最终可简化为，对从一个协方差矩阵的特征向量和特征值的分析（也称PCA主成分分析）。这个协方差矩阵是由查询点的最近邻产生的。对于每个点Pi,我们假设协方差矩阵C如下：

$${C=\frac{1}{k}\sum_{i=1}{k\left(p_i-\bar{p}\right)\bullet\left(p_i-\bar{p}\right)T},C\bullet\vec{v_j}=\lambda_j\bullet\vec{v_j},j\in0,1,2}$$

其中K指的是离${p_i}$点最近的K个点，${\bar{p}}$是最近邻的中心，${\lambda_j}$是第j个特征值，${\vec{v_j}}$是第j个特征向量。

接下来我们需解决法线方向的问题。以球面为例，法线可以指向球心，也可以背向球心。对于图像中的物体表面，只有法线全部朝向表面一侧才有助于表面特征的分析。因而我们引入视角向量这一概念，只要法线和视角与点的连线的夹角是锐角，即计算向量点积。若点积小于0，将向量转向即可。

PCL库提供了NormalEstimation类来估计点的法向向量。该类的成员函数先得到点p的N个最近邻，再计算p的表面法线${\vec{n}}$，最后检查检查${\vec{n}}$的方向是否一致指向视点，如果不是则翻转。

边界估计

法线估计为平面边缘检测任务提供了充分的判定依据。对于一个二维平面，边缘点的法线向量往往与其他点差异巨大。通过设置合理的角度阈值（如本项目将阈值设为45°），可以轻松标记出平面的边缘轮廓。
PCL库提供了BoundaryEstimation类以执行边界估计操作。上两个小节的代码示例如下：

// boundEst.cpp

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundaries(new pcl::PointCloud<pcl::PointXYZ>);

    for(int i=1; i<=3; ++i)
    {
        // 从硬盘中依次加载三个平面点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        std::stringstream ss;
        ss << "../inliercloud_" << i << ".pcd";
        pcl::io::loadPCDFile(ss.str(), *cloud);

        // 法线估计
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normEst;  // 定义一个法线估计的对象
        pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);  // 保存法线估计的结果
        normEst.setInputCloud(cloud);  // 设置输入点云
        normEst.setRadiusSearch(5);  // 设置领域搜索球半径为5cm
        normEst.compute(*normals);  // 执行法线估计

        // 边界估计
        pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> boundEst;
        pcl::PointCloud<pcl::Boundary> boundaries;
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
        boundEst.setInputCloud(cloud);  // 设置输入点云
        boundEst.setInputNormals(normals);  // 设置边界估计依赖的法线
        boundEst.setKSearch(50);  // 设置一次边界估计用到的邻域点数为50
        boundEst.setAngleThreshold(M_PI/4); // 设置边界估计的角度阈值
        boundEst.setSearchMethod(tree);  // 设置搜索方式为KdTree
        boundEst.compute(boundaries);  // 执行边界估计

        // 将边界结果保存为pcl::PointXYZ类型
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);
        for(int i=0; i<cloud->size(); i++)
            if(boundaries[i].boundary_point > 0)
                cloud_boundary->push_back(cloud->points[i]);

        *cloud_boundaries += *cloud_boundary;
    }

    pcl::io::savePCDFileASCII("cloud_boundaries.pcd", *cloud_boundaries);
    pcl::visualization::CloudViewer viewer("cloud");
    viewer.showCloud(cloud_boundaries);
    while(!viewer.wasStopped()) { };
    
    return 0;
}

转向架的边界轮廓提取效果如下：

边界检测效果图	提取边界后的点云图

直通滤波保留符合焊缝特征的边缘

转向架圆角焊缝的提取

圆角焊缝的提取相较于直线焊缝困难不少，原因在于圆角过渡区的几何特征未知，无法通过模型拟合的方式进行估算。但是利用转向架各个曲面的几何约束关系，我们不妨利用排除法得到圆角焊缝的点云集合。这里作两点假设：1. 圆角过渡区为光滑的外凸曲面；2. 圆角焊缝和直角焊缝关于地面的投影为一直线。

获取圆角区域的位置信息

若以上假设成立，我们通过前文得到的直角焊缝的位置信息，即可确定圆角焊缝的位置信息。以下程序showPointXYZ.cpp允许用户与PCLVisualizer的可视化界面进行交互，当用户选中一个点时，该点的坐标会被打印到控制台，并且在可视化界面中以红色高亮显示。

// showPointXYZ.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

using PointT = pcl::PointXYZRGB;
using PointCloudT = pcl::PointCloud<PointT>;

// 用于将参数传递给回调函数的结构体
struct CallbackArgs {
    PointCloudT::Ptr clicked_points_3d;
    pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

void pickPointCallback(const pcl::visualization::PointPickingEvent &event, void *args) {
    CallbackArgs *data = (CallbackArgs *) args;
    if (event.getPointIndex() == -1)
        return;
    PointT current_point;
    event.getPoint(current_point.x, current_point.y, current_point.z);
    data->clicked_points_3d->points.push_back(current_point);
    // 绘制红色点
    pcl::visualization::PointCloudColorHandlerCustom<PointT> red(data->clicked_points_3d, 255, 0, 0);
    data->viewerPtr->removePointCloud("clicked_points");
    data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
    data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10,
                                                      "clicked_points");
    std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
}

int main() {
    std::string file_name("../colored_cloud.pcd");
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
    // 加载点云
    if (pcl::io::loadPCDFile(file_name, *cloud) == -1) {
        std::cerr << "could not load file: " << file_name << std::endl;
    }
    std::cout << cloud->points.size() << std::endl;
    // 显示点云
    viewer->addPointCloud(cloud, "cloud");
    viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
    // 添加点拾取回调函数
    CallbackArgs  cb_args;
    PointCloudT::Ptr clicked_points_3d(new PointCloudT);
    cb_args.clicked_points_3d = clicked_points_3d;
    cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
    viewer->registerPointPickingCallback(pickPointCallback, (void*)&cb_args);
    std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

    viewer->spin();
    std::cout << "done." << std::endl;

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }
    return 0;
}

下面是拾取焊缝边缘点的结果

侧架1边缘点扫描	侧架2边缘点扫描

// pcl_passthrough.cpp

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>

int main(int argc, char** argv) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("../all_cloud.pcd", *cloud);

    pcl::PassThrough<pcl::PointXYZ> pass_x, pass_y, pass_z; //创建滤波器对象
    pass_x.setInputCloud(cloud);			                //设置待滤波的点云
    pass_x.setFilterFieldName("x");		        //设置在X轴方向上进行滤波
    pass_x.setFilterLimits(1535.0, 1740.0);		//设置滤波范围为0~1,在范围之外的点会被剪除
    //pass.setFilterLimitsNegative(true);                   //是否反向过滤，默认为false
    pass_x.filter(*cloud_filtered);		                //开始过滤

    pass_y.setInputCloud(cloud_filtered);			    //设置待滤波的点云
    pass_y.setFilterFieldName("y");		        //设置在Y轴方向上进行滤波
    pass_y.setFilterLimits(-320.0, -250.0);		//设置滤波范围为0~1,在范围之外的点会被剪除
    //pass.setFilterLimitsNegative(true);                   //是否反向过滤，默认为false
    pass_y.filter(*cloud_filtered);		                //开始过滤

    pass_z.setInputCloud(cloud_filtered);			    //设置待滤波的点云
    pass_z.setFilterFieldName("z");		        //设置在Z轴方向上进行滤波
    pass_z.setFilterLimits(1232.0, 1360.0);		//设置滤波范围为0~1,在范围之外的点会被剪除
    //pass.setFilterLimitsNegative(true);                   //是否反向过滤，默认为false
    pass_z.filter(*cloud_filtered);		                //开始过滤

    pcl::visualization::CloudViewer viewer("filtered_cloud");
    viewer.showCloud(cloud_filtered);
    while(!viewer.wasStopped())  { };

    return 0;
}