PCL点云学习七（从距离图像重建曲面）

论文阅读

原文：Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing
1.摘要/简介

• 利用局部平面信息和临近点平滑测量点
• 将图像分割为平面区域和其他的几何单元
• 可以高速处理距离图像

2. 相关

• 为了在复杂环境下获取目标部分，常用方法如下：
  <1>: 检测水平支撑平面
  <2>: 在这些平面上提取聚类点
  <2>: 再执行辨识、分类或者分钟对应点集
• 通常存在一个问题：如何将3D数据转换为相应几何特征/局部平面
• 文章利用网格局部的邻域点和区域增长法，来近似距离图像中的平面和几何
  利用网格局部的邻域点高效计算法线（双边滤波器），并估计曲率，来平滑3D点
• 在机器视觉领域，按照原理通常可以将曲面重建分为3类：
  <1> 使用随机一致性(RANSAC):
  找到最适合前点集的简单集合模型，剔除内点，再分割区域的剩下点：
  ex1:Silva,确定联通区域，并按对应与应用RANSAC，
  ex2:Gotardo,使用预分割计算边缘映射，使用MSAC（基于M估计的随机采样一致性）来拟合模型
  ex3:Schnabel,使用无序提取简单的几何模型（平面、圆柱、球体），分解原始无序点云为八叉树，并应用到原始点云
  论文的文献提出了一种适用于TOF相机的方法，处理相机误差，加速处理与图像结构相似数据，在检测时：检测到主平面后，利用Schnabel提出的八叉树基元检测处理提取的平面，并对图像进行积分，加速分割、提取目标。该方法适用与kinect
  <2> 3D霍夫变化(Hough)
  在2D图像上查找实时上的直线和圆，扩展到3D中，通过在各个方向中找到具有最大值的平面和直方图。
  论文的文献提出过一种快速分割方法：使用相似参数空间进行霍夫变化，在发现空间上预先分割点集和区域，对于每个点集获得一个独特的平面，在通过一个特殊的POS过程（合并参数相似的区域）补偿离散化误差，但是可能导致两个不相邻区域合并
  <3> 区域增长法
  方法1：将数据变为类似与图像结构的数据。
  ex1:Hähnel 将3D激光扫描为网状结构，合并可能位于同一平面的区域，
  ex2:Poppinga;将该方法应用于机载相机上，并使用重构了算法:增长区域通过添加临近点中小于指定阈值的点，此时平面的质心和协防差也会同时更新。
  方法2：对于深度图像使用，计算局曲率，区域增长指导曲率达到指定值停止。
  ex1:Harati;计算一条边缘映射，查找连续的局部区域
  ex2:Rabbani;将局部区域点映射到平面上，让后计算每个点到平面距离
  ex3:Cupec;在深度图像上使用2.5D Delaunay 三角化获取三角化网格,在使用当前点到区域内所有网格的最大距离决定是否添加。
• 在3D激光激光常用的曲面重建算法为：先在切割平面提取多条直线，然后合并相邻线到局部平面面片中。
• RANSAC和霍夫变化缺点：当两个分割区域存在同一公共点时，会将两个区域合并，直到不相同的区域
• 霍夫变化受离散化影响
• 所以文章采用类似图像的数据（距离图像），直接推导曲面重建，使用局部环形区域估计法线和曲率，使用双边滤波器平滑深度值，支持多种区域增长方法。

3. 快速网格构造和区域重建

• 整个过程可以分为如下流程：
  1）：从图像邻域内推导网格
  2）：使用网格邻域近似估计局部曲面的法线和曲率
  3）：使用双边滤波器平滑法线和点集
  4）：使用区域增长法分割区域
• 整个过程的邻域点来自于网格结构或者通过搜索树构造的非结构数据
• 快速网格重建的拓扑结构(点检测)
  遍历深度图像$R$上的每一点$p_i=R(u,v)$,
  1）：检测点$p_i$的邻域点，$R(u,v+1),R(u+1,v+1),R(u+1,v)$的值是否有效，所有边与点$p_i$,和其三个邻域值构成向量是否存在遮挡，如果通过检测不为遮挡点，就可用于构建网格
  × 遮挡检测：如果将邻域值构成的向量与，视点与其测量值的连线共线，那么表示存在遮挡，有一个值无效，检测方法表达式如下：
  ${\xi }_{\theta }:$最大角度容忍误差
  ${\xi }_d:$最大长度允许误差
  $$valid=(|p_i\ast p_j|\le cos{\xi }_{\theta })\wedge (||p_i-p_j|{|}^2\le {\xi }_d^2)$$
• 快速网格重建的拓扑结构(网格存在4种)：
  
  方形网格：顺序连接$R(u,v),R(u,v+1),R(u+1,v+1),R(u+1,v)$
  
  左切网格：分别链接方格对角点
  
  自适应网格：沿对角线较短的方向切割网格，论文采用这种方式进行三角化，再三角化过程中，单个无效邻域点会导致值添加了一个三角形，在最后我们会剔除所有不构成多边形的三角形顶点。
  2）：快速计算平面法线和曲率
  a:原始点云
  

采用$p_i$邻域点的面法线的权重计算平面法线$n_i$，使用叉积计算差分向量得到法线，选取适当的比例（与其三角化面积相关，避免了归一化），法线$n_i$的计算公式如下:
$p_{j,a},p_{j,b},p_{j,c}:$三角$j$的三个顶点
$$n_i=\frac{{\sum }_{j=0}^{N_T}(p_{j,a}-p_{j,b})\times (p_{j,a}-p_{j,c})}{||{\sum }_{j=0}^{N_T}(p_{j,a}-p_{j,b})\times (p_{j,a}-p_{j,c})||}$$
实际中，只需要遍历所有差分向量并进行叉积，并将其法线相关的点相加，并在最后归一化所有点的法线，得到下图b：

3）：使用双边滤波器平滑法线和点集
去除噪声，对点与法相进行平滑;
$N_i:$距离为1的环形邻域，即直接连接到点$p_i$的点集
$c_I:$归一化函数
$$\begin{gathered} p_i=\frac{{\sum }_{j\in N_i}{w}_{ij}{p}_i}{{\sum }_{j\in N_i}{w}_{ij}};n_i=\frac{{\sum }_{j\in N_i}{w}_{ij}{n}_i}{{\sum }_{j\in N_i}{w}_{ij}} \\ {w}_{ij}=距离因素\ast 法线因素\ast 强度因素(可选)=e^{||p_i-p_j||}{e}^{||n_i-n_j||}{e}^{||I_i-I_j||/c_I} \end{gathered}$$
最后得到下图C:

4）：使用区域增长法分割区域
对给定点（起始点的集合和队列）进行如下操作：
1）通过迭代选择下一个起始点
2）初始化区域增长的兴趣点
3）将起始点插入一个空的处理队列
4）内循环：处理队列：当起处理队列不为空，从队列中提取点。
5）内循环：检测其与区域模型的匹配性，如果匹配添加，不匹配停止
6）内循环：添加当前点的邻域到处理队列

• 作者已经将2,4,5,6步骤封装，并实现了几种区域增长模型，并分割
  （1）：近似平面分割（Approximate Plane Segmentation）：
  使用起始点和其法线初始化区域模型的质心和法线，在进行区域匹配时，只需要检查区域法线$n_i$和点法线的夹角，为了更新模型参数，递增的更新平面质心（而不时协防差），从中导出法线，即:通过预先计算网格邻域上的曲面法线，并通过平均点法线来逼近平面法线，我们可以大大减少计算量。。
  （2）：提取局部平滑曲面（Extracting Locally Smooth Surfaces）：
  简单的几何分割和提取可以通过邻域点实现，通过检测局部曲率变化，应用一种典型的区域分割如图d:
  
• 几何模型的选取
  为了找到最适合区域的几何模型，使用RANSAC，为了提高效率，论文中只有当一个区域无法找到fully exlplain 才会使用。
  
  左边：提取到平面（黄色）、圆柱（淡蓝）、球体(紫色)
  右边：将提取的五一映射到平面
• 其他：为了增加算法的鲁棒性，假设原始数据存在等向噪声（Isotropic Noise Model），噪声模型为：$N(0,{\sigma }^2)$,作者找到适合的$\sigma$(kinect相机),$\sigma (d)=0.00263d^2-0.00518d+0.00752$
• 算法时间（SegComp Data Sets数据集）
  

PCL库

使用：
核心代码：

    pcl::search::KdTree<pcl::PointWithRange>::Ptr tree(new pcl::search::KdTree<pcl::PointWithRange>());
    tree->setInputCloud(rangeImage);

    pcl::PolygonMesh triangles;//三角化
	tri->setTrianglePixelSize(size);    //三角化网格环状邻域大小
	tri->setInputCloud(rangeImage);      //输出带你元
	tri->setSearchMethod(tree);         //搜索方法
	tri->setTriangulationType(pcl::OrganizedFastMesh::TriangulationType::TRIANGLE_ADAPTIVE_CUT);//自适应
	tri->reconstruct(triangles);  

实例：来自pcl点云从入门到精通

#include <pcl/range_image/range_image.h>
#include <pcl/range_image/range_image_planar.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/surface/organized_fast_mesh.h>
#include <pcl/console/time.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <iostream>
#include <pcl/surface/impl/organized_fast_mesh.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/common/float_image_utils.h>
#include <pcl/io/png_io.h>
#include <pcl/surface/organized_fast_mesh.h>
using namespace pcl::console;
using namespace pcl;
using namespace std;
int main(int argc, const char **argv)
{
    int width = 640;
    int height = 480;
    float cx = 320, cy = 240, fx = 525, fy = 525;
    int type = 0, size = 2;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    if (io::loadPCDFile("/home/n1/notes/pcl/CPR/1.pcd", *cloud))
    {
        cout << "读取文件成功！" << endl;
    }
    Eigen::Affine3f sensorPose = Eigen::Affine3f::Identity(); //传感器姿态
    pcl::RangeImage::CoordinateFrame coordinate_frame=pcl::RangeImage::CAMERA_FRAME;//设置坐标系
    float noiselevel=0;
    float minRange=0.0f;

    pcl::RangeImagePlanar::Ptr rangeImage(new pcl::RangeImagePlanar()); //创建深度图像
    rangeImage->createFromPointCloudWithFixedSize(*cloud,width,height,cx,cy,fx,fy,sensorPose,coordinate_frame);//创建深度图像
    float* ranges = rangeImage->getRangesArray();  //获取距离数组
    unsigned char* rgb_image=pcl::visualization::FloatImageUtils::getVisualImage(ranges, rangeImage->width, rangeImage->height);
    pcl::io::saveRgbPNGFile("/home/n1/notes/pcl/CPR/rangimage.png", rgb_image, rangeImage->width, rangeImage->height);

    pcl::OrganizedFastMesh<pcl::PointWithRange>::Ptr tri(new pcl::OrganizedFastMesh<pcl::PointWithRange>());
    pcl::search::KdTree<pcl::PointWithRange>::Ptr tree(new pcl::search::KdTree<pcl::PointWithRange>());
    tree->setInputCloud(rangeImage);
    pcl::PolygonMesh triangles;//三角化
	tri->setTrianglePixelSize(size);    //三角化网格大小
	tri->setInputCloud(rangeImage);      //输出带你元
	tri->setSearchMethod(tree);         //搜索方法
	tri->setTriangulationType(pcl::OrganizedFastMesh<pcl::PointWithRange>::TriangulationType::TRIANGLE_ADAPTIVE_CUT);
    //        TRIANGLE_RIGHT_CUT,   0
    //    TRIANGLE_LEFT_CUT,        1
    //    TRIANGLE_ADAPTIVE_CUT,    2
    //    QUAD_MESH                 3
	tri->reconstruct(triangles);        //结构化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("点云库PCL从入门到精通"));
	viewer->setBackgroundColor(0.5,0.5,0.5);
	viewer->addPolygonMesh(triangles,"tin");
	viewer->addCoordinateSystem();
	while (!viewer->wasStopped())
	{

		pcl_sleep (0.01);
		viewer->spinOnce ();

	}

    return 0;
}