1. 概述

点云是某个坐标系下的点的数据集。点包含了丰富的信息，包括三维坐标 X，Y，Z、颜色、分类值、强度值、时间等等。点云在组成特点上分为两种，一种是有序点云，一种是无序点云：

有序点云：一般由深度图还原的点云，有序点云按照图方阵一行一行的，从左上角到右下角排列，当然其中有一些无效点因为。有序点云按顺序排列，可以很容易的找到它的相邻点信息。有序点云在某些处理的时候还是很便利的，但是很多情况下是无法获取有序点云的
无序点云：无序点云就是其中的点的集合，点排列之间没有任何顺序，点的顺序交换后没有任何影响。是比较普遍的点云形式，有序点云也可看做无序点云来处理

点云不是通过普通的相机拍摄得到的，一般是通过三维成像传感器获得，比如双目相机、三维扫描仪、RGB-D 相机等。目前主流的 RGB-D 相机有微软的 Kinect 系列、Intel 的 realsense 系列、structure sensor（需结合 iPad 使用）等。点云可通过扫描的 RGB-D 图像，以及扫描相机的内在参数创建点云，方法是通过相机校准，使用相机内在参数计算真实世界的点（x，y）。因此，RGB-D 图像是网格对齐的图像，而点云则是更稀疏的结构。此外，获得点云的较好方法还包括 LiDAR 激光探测与测量，主要通过星载、机载和地面三种方式获取。

点云的属性包括：

空间分辨率、点位精度、表面法向量等
点云可以表达物体的空间轮廓和具体位置，我们能看到街道、房屋的形状，物体距离摄像机的距离也是可知的；其次，点云本身和视角无关，可以任意旋转，从不同角度和方向观察一个点云，而且不同的点云只要在同一个坐标系下就可以直接融合

存储格式：

PTS点云文件格式是最简便的点云格式，直接按XYZ顺序存储点云数据，可以是整型或者浮点型
LAS是激光雷达数据（LiDAR），存储格式比 pts 复杂，旨在提供一种开放的格式标准，允许不同的硬件和软件提供商输出可互操作的统一格式。LAS 格式点云截图，其中 C：class(所属类)，F：flight(航线号)，T：time(GPS 时间)，I：intensity(回波强度)，R：return(第几次回波)，N：number of return(回波次数)，A：scan angle(扫描角)，RGB：red green blue(RGB 颜色值)
PCD存储格式，现有的文件结构因本身组成的原因不支持由 PCL 库（后文会进行介绍）引进 n 维点类型机制处理过程中的某些扩展，而 PCD 文件格式能够很好地补足这一点。PCD 格式具有文件头，用于描绘点云的整体信息：定义数字的可读头、尺寸、点云的维数和数据类型；一种数据段，可以是 ASCII 码或二进制码。数据本体部分由点的笛卡尔坐标构成，文本模式下以空格做分隔符
XYZ一种文本格式，前面 3 个数字表示点坐标，后面 3 个数字是点的法向量，数字间以空格分隔
PCAB是一种通用的数据流格式，现在流行的Velodyne公司出品的激光雷达默认采集数据文件格式，是一种二进制文件
OBJ是一种文本文件，通常用以“#”开头的注释行作为文件头，数据部分每一行的开头关键字代表该行数据所表示的几何和模型元素，以空格做数据分隔符

基础算法库:

PCL（Point Cloud Library）库支持跨平台存储，可以在Windows、Linux、macOS、iOS、Android上部署。可应用于计算资源有限或者内存有限的应用场景，是一个大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，其基于以下第三方库：Boost、Eigen、FLANN、VTK、CUDA、OpenNI、Qhull，实现点云相关的获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等操作，非常方便移动端开发
VCG 库（Visulization and Computer Graphics Libary）是专门为处理三角网格而设计的，该库很大，且提供了许多先进的处理网格的功能，以及比较少的点云处理功能
CGAL（Computational Geometry Algorithms Library）计算几何算法库，设计目标是以C++库的形式，提供方便、高效、可靠的几何算法，其实现了很多处理点云以及处
Open3D是一个可以支持3D数据处理软件快速开发的开源库。支持快速开发处理3D数据的软件。Open3D前端在C++和Python中公开了一组精心选择的数据结构和算法。后端经过高度优化，并设置为并行化。Open3D是从一开始就开发出来的，带有很少的、经过仔细考虑的依赖项。它可以在不同的平台上设置，并且可以从源代码进行最小的编译。代码干净，样式一致，并通过清晰的代码审查机制进行维护。在点云、网格、RGBD数据上都有支持

点云法向量：

法向量是点云中各点的重要属性之一。众多点云算法的实施都基于精确的法向量估计，例如许多表面重建算法、点云分割算法、点云去噪算法以及特征描述算法等
由空间变换可知，点云中每一点的法向量夹角及曲率值均不随物体的运动而改变，具有刚体运动不变性
点云法向量求解需要其邻域内点支持，而邻域的大小一般由邻域半径值或临近点个数来表示。现实中需要根据点分别率、物体细节精细程度和用途等因素来取值。过大的邻域会抹平三维结构细节使得法向量过于粗糙，而过小的邻域由于包含了太少的点受噪声干扰程度较强
点云法向量计算方法
- 选取一个点
- 找到其邻居点
- PCA
- 计算法向量
- 曲率
  
  如下图所示：

2. 算法详解

2.1 滤波

点云滤波处理：

点云数据密度不规则需要平滑
因为遮挡等问题造成离群点需要去除
大量数据需要下采样
噪声数据需要去除

2.1.1 Radius Outlier Removal算法

在点云数据中用户可以定义每个点的一定半径圆的范围内要有足够多近邻。例如当指定圆领域内至少要有一个邻近点时，黄色点将被删除；如果指定要有两个点时，黄色和绿色点都将被删除而黑色点会被保留（黑色点近邻圆内有4个点），如下图所示：

2.1.2 Statistical Outlier Removal算法

稀疏离群点移除方法，通过对查询点与邻域点集之间的距离统计判断来过滤离群点：对每个点，我们计算它到它的所有临近点的平均距离。假设得到的结果是一个高斯分布，其形状由均值和标准差决定，平均距离在标准范围（由全局距离平均值和方差定义〉之外的点，可被定义为离群点并可从数据集中去除掉。

2.1.3 体素滤波

体素的概念类似于像素，使用AABB包围盒将点云数据体素化，一般体素越密集的地方信息越多，噪音点及离群点可通过体素网格去除。另一方面如果使用高分辨率相机等设备对点云进行采集，往往点云会较为密集。过多的点云数量会对后续分割工作带来困难。体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能。

2.1.4 Bilateral Filter算法

双边滤波是一种非线性滤波器，它可以达到保持边缘、降噪平滑的效果。和其他滤波原理一样，双边滤波也是采用加权平均的方法，用周边像素亮度值的加权平均代表某个像素的强度，所用的加权平均基于高斯分布[1]。最重要的是，双边滤波的权重不仅考虑了像素的欧氏距离（如普通的高斯低通滤波，只考虑了位置对中心像素的影响），还考虑了像素范围域中的辐射差异（例如卷积核中像素与中心像素之间相似程度、颜色强度，深度距离等），在计算中心像素的时候同时考虑这两个权重。

2.2 降采样

2.2.1 Farthest Point Sampling (FPS)算法

主要思想：

不断迭代地选择距离已有采样点集合的最远点

算法步骤：

输入点云有N个点，从点云中选取一个点P0作为起始点，得到采样点集合S={P0}
计算所有点到P0的距离，构成N维数组L，从中选择最大值对应的点作为P1，更新采样点集合S={P0，P1}
计算所有点到P1的距离，对于每一个点Pi，其距离P1的距离如果小于L[i]，则更新L[i] = d(Pi, P1)，因此，数组L中存储的一直是每一个点到采样点集合S的最近距离
选取L中最大值对应的点作为P2，更新采样点集合S={P0，P1，P2}
重复2-4步，一直采样到N’个目标采样点为止

有两个问题，一个是初始点选择，一个是采用的距离度量。

初始点选择
- 随机选择一个点，每次结果不同
- 选择距离点云重心的最远点，每次结果相同，一般位于局部极值点，具有刻画能力
距离度量
- 欧氏距离：主要对于点云，在3D体空间均匀采样
- 测地距离：主要对于三角网格，在三角网格面上进行均匀采样

2.2.2 Normal Space Sampling (NSS)算法

法向量上面的降采样，为了保持其局部的特征，针对于点云的对齐而采用。

主要思想：

先在法向量的空间里建一堆容器
依据平面法向量将所有点放入容器中
在每个容器中都挑选出同等数量的点。

3. 代码实现

3.1Radius Outlier Removal算法

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>

int main(int argc, char** argv) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr RawCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud_Filtered(new pcl::PointCloud<pcl::PointXYZ>);
    RawCloud->width = 10;
    RawCloud->height = 1;
    RawCloud->points.resize(RawCloud->width * RawCloud->height);
    for (rsize_t i = 0; i < RawCloud->points.size(); ++i) {
        RawCloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
        RawCloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        RawCloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }
     pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(RawCloud);
    outrem.setRadiusSearch(0.8);
    outrem.setMinNeighborsInRadius(2);
    outrem.filter (*Cloud_Filtered);
    std::cerr << "Cloud before filtering: " << std::endl;
    for (size_t i = 0; i < RawCloud->points.size(); ++i)
        std::cerr << "    " << RawCloud->points[i].x << " "
        << RawCloud->points[i].y << " "
        << RawCloud->points[i].z << std::endl;
    std::cerr << "Cloud after filtering: " << std::endl;
    for (size_t i = 0; i < Cloud_Filtered->points.size(); ++i)
        std::cerr << "    " << Cloud_Filtered->points[i].x << " "
        << Cloud_Filtered->points[i].y << " "
        << Cloud_Filtered->points[i].z << std::endl;
    return (0);
}

3.2 Statistical Outlier Removal算法

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.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::PCDReader reader;
  reader.read<pcl::PointXYZ> ("..\\source\\table_scene_lms400.pcd", *cloud);
  std::cerr << "Cloud before filtering: " << std::endl;
  std::cerr << *cloud << std::endl;
  
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);
  sor.setStddevMulThresh (1.0);
  sor.filter (*cloud_filtered);
  
  std::cerr << "Cloud after filtering: " << std::endl;
  std::cerr << *cloud_filtered << std::endl;
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);
  
  sor.setNegative (true);
  sor.filter (*cloud_filtered);
  writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);
  
  return (0);
}

3.3 体素滤波

#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);
    std::cerr << "Point cloud before filtering: " << cloud->width * cloud->height 
    << " data points (" << pcl::getFieldsList (*cloud) << ").";

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloud);
    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.filter (*cloud_filtered);

    std::cerr << "Point cloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
    << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";

    pcl::PCDWriter writer;
    writer.write ("2f.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

    return (0);
}

3.4 Farthest Point Sampling (FPS)算法

from __future__ import print_function
import torch
from torch.autograd import Variable

def farthest_point_sample(xyz, npoint): 
    """
    Input:
        xyz: pointcloud data, [B, N, 3]
        npoint: number of samples
    Return:
        centroids: sampled pointcloud index, [B, npoint]
    """
    xyz = xyz.transpose(2,1)
    device = xyz.device
    B, N, C = xyz.shape
    
    centroids = torch.zeros(B, npoint, dtype=torch.long).to(device)
    distance = torch.ones(B, N).to(device) * 1e10

    batch_indices = torch.arange(B, dtype=torch.long).to(device)
    barycenter = torch.sum((xyz), 1) 
    barycenter = barycenter/xyz.shape[1]
    barycenter = barycenter.view(B, 1, 3)

    dist = torch.sum((xyz - barycenter) ** 2, -1)
    farthest = torch.max(dist,1)[1]

    for i in range(npoint):
        print("-------------------------------------------------------")
        print("The %d farthest pts %s " % (i, farthest))
        centroids[:, i] = farthest
        centroid = xyz[batch_indices, farthest, :].view(B, 1, 3)
        dist = torch.sum((xyz - centroid) ** 2, -1)
        print("dist    : ", dist)
        mask = dist < distance
        print("mask %i : %s" % (i,mask))
        distance[mask] = dist[mask]
        print("distance: ", distance)

        farthest = torch.max(distance, -1)[1]
 
    return centroids

if __name__ == '__main__':
    sim_data = Variable(torch.rand(1,3,8))
    print(sim_data)

    centroids = farthest_point_sample(sim_data, 4)
    
    print("Sampled pts: ", centroids)

3.5 Normal Space Sampling (NSS)算法

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/normal_space.h>//法线空间采样
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <boost/thread/thread.hpp>

using namespace std;

void estimate_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::Normal>::Ptr normals) {
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloud_in);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    n.setSearchMethod(tree);
    n.setNumberOfThreads(8);
    n.setKSearch(30);
    n.compute(*normals);
}

void visualize_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& filter_cloud) {
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("显示点云"));

    int v1(0), v2(0);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->addText("point clouds", 10, 10, "v1_text", v1);
    viewer->createViewPort(0.5, 0.0, 1, 1.0, v2);
    viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
    viewer->addText("filtered point clouds", 10, 10, "v2_text", v2);
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud, "z");
    viewer->addPointCloud<pcl::PointXYZ>(cloud, fildColor, "sample cloud", v1);

    viewer->addPointCloud<pcl::PointXYZ>(filter_cloud, "cloud_filtered", v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloud_filtered", v2);
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

int main() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("飞机.pcd", *cloud) == -1) {
        PCL_ERROR("读取源标点云失败 \n");
        return (-1);
    }
    cout << "从点云中读取 " << cloud->size() << " 个点" << endl;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    estimate_normals(cloud,normals);
    pcl::NormalSpaceSampling<pcl::PointXYZ, pcl::Normal> nss;
    nss.setInputCloud(cloud);
    nss.setNormals(normals);
    nss.setBins(2, 2, 2);
    nss.setSeed(0);
    nss.setSample(1000);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_nss(new pcl::PointCloud<pcl::PointXYZ>);
    nss.filter(*cloud_nss);
    pcl::io::savePCDFileASCII ("nss.pcd", *cloud_nss);
    cout << "法线空间采样后的点云个数为：" << cloud_nss->points.size() << endl;

    visualize_cloud(cloud, cloud_nss);

    return 0;
}

3.6 Bilateral Filter算法

void bilateralfiter(cv::Mat& src, cv::Mat& dst, cv::Size wsize, double spaceSigma, double colorSigma) {
	cv::Mat spaceMask;
	std::vector<double> colorMask;
	cv::Mat Mask0 = cv::Mat::zeros(wsize, CV_64F);
	cv::Mat Mask1 = cv::Mat::zeros(wsize, CV_64F);
	cv::Mat Mask2 = cv::Mat::zeros(wsize, CV_64F);
 
	getGausssianMask(spaceMask, wsize, spaceSigma);
	getColorMask(colorMask, colorSigma);
	int hh = (wsize.height - 1) / 2;
	int ww = (wsize.width - 1) / 2;
	dst.create(src.size(), src.type());
	cv::Mat Newsrc;
	cv::copyMakeBorder(src, Newsrc, hh, hh, ww, ww, cv::BORDER_REPLICATE);
 
	for (int i = hh; i < src.rows + hh; ++i){
		for (int j = ww; j < src.cols + ww; ++j){
			double sum[3] = { 0 };
			int graydiff[3] = { 0 };
			double space_color_sum[3] = { 0.0 };
 
			for (int r = -hh; r <= hh; ++r){
				for (int c = -ww; c <= ww; ++c){
					if (src.channels() == 1){
						int centerPix = Newsrc.at<uchar>(i, j);
						int pix = Newsrc.at<uchar>(i + r, j + c);
						graydiff[0] = abs(pix - centerPix);
						double colorWeight = colorMask[graydiff[0]];
						Mask0.at<double>(r + hh, c + ww) = colorWeight * spaceMask.at<double>(r + hh, c + ww);
						space_color_sum[0] = space_color_sum[0] + Mask0.at<double>(r + hh, c + ww);
 
					}
					else if (src.channels() == 3){
						cv::Vec3b centerPix = Newsrc.at<cv::Vec3b>(i, j);
						cv::Vec3b bgr = Newsrc.at<cv::Vec3b>(i + r, j + c);
						graydiff[0] = abs(bgr[0] - centerPix[0]); graydiff[1] = abs(bgr[1] - centerPix[1]); graydiff[2] = abs(bgr[2] - centerPix[2]);
						double colorWeight0 = colorMask[graydiff[0]];
						double colorWeight1 = colorMask[graydiff[1]];
						double colorWeight2 = colorMask[graydiff[2]];
						Mask0.at<double>(r + hh, c + ww) = colorWeight0 * spaceMask.at<double>(r + hh, c + ww);
						Mask1.at<double>(r + hh, c + ww) = colorWeight1 * spaceMask.at<double>(r + hh, c + ww);
						Mask2.at<double>(r + hh, c + ww) = colorWeight2 * spaceMask.at<double>(r + hh, c + ww);
						space_color_sum[0] = space_color_sum[0] + Mask0.at<double>(r + hh, c + ww);
						space_color_sum[1] = space_color_sum[1] + Mask1.at<double>(r + hh, c + ww);
						space_color_sum[2] = space_color_sum[2] + Mask2.at<double>(r + hh, c + ww);
 
					}
				}
			}
 
			if(src.channels() == 1)
			Mask0 = Mask0 / space_color_sum[0]; 
			else{
				Mask0 = Mask0 / space_color_sum[0];
				Mask1 = Mask1 / space_color_sum[1];
				Mask2 = Mask2 / space_color_sum[2];
			}
				
 
			for (int r = -hh; r <= hh; ++r){
				for (int c = -ww; c <= ww; ++c){
 
					if (src.channels() == 1){
						sum[0] = sum[0] + Newsrc.at<uchar>(i + r, j + c) * Mask0.at<double>(r + hh, c + ww);
					}
					else if(src.channels() == 3){
						cv::Vec3b bgr = Newsrc.at<cv::Vec3b>(i + r, j + c);
						sum[0] = sum[0] + bgr[0] * Mask0.at<double>(r + hh, c + ww);
						sum[1] = sum[1] + bgr[1] * Mask1.at<double>(r + hh, c + ww);
						sum[2] = sum[2] + bgr[2] * Mask2.at<double>(r + hh, c + ww);
					}
				}
			}
 
			for (int k = 0; k < src.channels(); ++k){
				if (sum[k] < 0)
					sum[k] = 0;
				else if (sum[k]>255)
					sum[k] = 255;
			}
			if (src.channels() == 1)
			{
				dst.at<uchar>(i - hh, j - ww) = static_cast<uchar>(sum[0]);
			}
			else if (src.channels() == 3)
			{
				cv::Vec3b bgr = { static_cast<uchar>(sum[0]), static_cast<uchar>(sum[1]), static_cast<uchar>(sum[2]) };
				dst.at<cv::Vec3b>(i - hh, j - ww) = bgr;
			}
 
		}
}

4. 总结和讨论

5. 参考

双边滤波(bilateralfiter)原理及C++实现