点云滤波（一）——MLS（移动最小二乘）/PSS介绍

一、前言

笔者最近在做的一个项目是要从点云中提取特征（边缘、角点）。点云的来源是双目结构光相机，尽管工业级的双目结构光相机已经有了比较高的精度（名义z轴精度为0.5mm），但实际测量误差有时甚至能到2mm，为了能获得更鲁棒和准确的结果还需要尽可能的减小噪声，因此对点云滤波方法进行学习。

采集的平面点云，CloudCompare中的最小包围盒高度达到了2mm 三维重建中为了能显示更加好看的模型，通常会把点云滤波方法作为前置步骤，效果如下： 左图为噪声数据+泊松重建的结果，右图为滤波之后再泊松重建的结果

这里有一篇点云滤波综述[1]，我将选出一些经典的方法进行学习，并将笔记分享出来，欢迎交流。

参考的资料是Survey of Methods in Computer Graphics (600.658)课件以及最初论文

这次的笔记记录的是MLS，论文原文[2]。实话说我没有搞清楚这个命名到底是怎么回事，这篇论文的缩写是PSS（Point Set Surfaces），论文里用到的数学工具是MLS。所以我看网上讨论这种方法时候，用这两个名称的都有。

二、前置知识

2.1 小引理

• 极值点的判断条件：
  如果给定一个方程$F:{\mathbb{R}}^3\to \mathbb{R}$，$p\in {\mathbb{R}}^3$是$F$的极值点当且仅当 $F$ 在 $p$ 点的梯度为0，即$\nabla F|p=0$。
• 几种内积的梯度（后边会用到）
  $F:{\mathbb{R}}^3\to \mathbb{R}$ 满足如下形式，$F(p)=⟨p,q⟩$，$\nabla F{|}_p=q$
  $F:{\mathbb{R}}^3\to \mathbb{R}$ 满足如下形式，$F(p)=⟨p,q{⟩}^n$，$\nabla F{|}_p=n⟨p,q{⟩}^{n-1}q$
  $F:{\mathbb{R}}^3\to \mathbb{R}$ 满足如下形式，$F(p)=⟨p,p⟩$，$\nabla F{|}_p=2p$
  $F:{\mathbb{R}}^3\to \mathbb{R}$ 满足如下形式，$F(p)=⟨p,Mp⟩$，$\nabla F{|}_p=(M^T+M)p$
  但其实雅可比矩阵有行形式跟列形式，不同形式之间的结果差了个转置。
• 二元二次多项式的一般形式
  $$F(x,y)=c_{00}+c_{10}x+c_{01}y+c_{11}xy+c_{20}{x}^2+c_{02}{y}^2$$
  可以将其改写成内积形式：
  $$\begin{gathered} F(x,y)=⟨v,Q(x,y)⟩ \\ v=(c_{00},c_{10},c_{01},c_{11},c_{20},c_{02}{)}^T \\ Q(x,y)=(1,x,y,xy,x^2,y^2{)}^T \end{gathered}$$
• 拉格朗日乘数定理（用来求解有约束条件下的极值问题）
  满足 $G(p)=c$约束条件下 $F$的极值点 $p$，一定满足$\nabla F{|}_p=\lambda \nabla G{|}_p$，可以简单记作有约束目标函数的极值点在约束表面上的变化率为零（这地方没理解透彻啊）
• Lagrangians and Symmetric Matrices
  如果$M$是一个对称矩阵，$F(p)=⟨p,Mp⟩s.t.||p|{|}^2=1$，那么$p$是$F(p)$的极值点当且仅当$p$是$M$的特征向量
  $$\begin{gathered} \nabla F{|}_p=M^{T}p+Mp=2Mp=\lambda G{|}_p=\lambda 2p \\ \therefore Mp=\lambda p \end{gathered}$$

2.2 线性最小二乘法

通常用于在给定数据的情况下，找到一个最佳的线性模型来拟合数据。

2.2.1 $Ax=b$ 形式

假设有这样一些数据点$\{(x_1,y_1),(x_2,y_2),\dots ,(x_n,y_n)\}$，希望能找到 $y=kx+b$ 这样的模型来拟合这些数据。改写成矩阵形式:
$$\left(\begin{array}{cc}{x}_1& 1\\ x_2& 1\\ ⋮& ⋮\\ x_n& 1\end{array}\right)\left(\begin{array}{c}k\\ b\end{array}\right)=\left(\begin{array}{c}{y}_1\\ y_2\\ ⋮\\ y_n\end{array}\right)$$
使用$Ax=b$来代替上边的三个矩阵，由于有噪声的存在，线性方程组的系数矩阵的秩应该小于增广矩阵的秩？（可以这样说么），可能没有精确解，所以使用最小二乘法来求解近似解。

一个最直观的想法是使得残差平方和最小：

$$\begin{array}{rl}min\epsilon & ={\Vert \mathbf{Ax}-\mathbf{b}\Vert }_2^2\\ & =(Ax-b{)}^T(Ax-b)\\ & =x^T{A}^{T}Ax-2x^T{A}^{T}b+b^{T}b\end{array}$$
为求出最小值，令 $\epsilon$对$x$的导数为零
$$\frac{\partial \epsilon }{\partial x}=2A^{T}Ax-2A^{T}b=0⟹x=(A^{T}A{)}^{-1}{A}^{T}b$$

还有一种是通过投影来理解最小二乘，感兴趣可以查一下，更加直观

2.2.2 $Ax=0$形式

超定方程的求解、最小二乘解、Ax=0、Ax=b的解，求解齐次方程组，求解非齐次方程组（推导十分详细）

三、MLS（移动最小二乘）

3.1 原理

LS直接生成一个全局模型来拟合所有数据点，因此拟合结果容易被离群值拉偏。
而MLS会对每个预测点的局部区域拟合出一个模型，局部区域中的点对于模型的贡献取决于其与预测点之间的距离。移动二字也体现在这里，即通过一个移动窗口来逐点拟合。

在每一个点$p$，找到一个函数${\varphi }_p$使得
$$E({\varphi }_p)={\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)({\varphi }_p(p_i)-{\varphi }_i{)}^2$$
$i$ 是点 $p$ 邻域的序号，$\Theta$是一个权重函数，它的函数值随自变量的减小而增大，通常使用指数函数来表示，下面举两个例子。

• 当${\varphi }_p()$表示常值函数时，可以发现MLS的结果是以距离为权重的加权平均和
  $$E({\varphi }_p)={\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)({\varphi }_p-{\varphi }_i{)}^2$$
  求梯度并令其等于零，可以求得使得$E$最小的${\varphi }_p$
  $$\begin{gathered} \frac{\partial E}{\partial {\varphi }_p}={\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)2({\varphi }_p-{\varphi }_i)=0 \\ ⟹{\varphi }_p=\frac{{\Sigma }_i\Theta (\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert ){\varphi }_i}{{\Sigma }_i\Theta (\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert )} \end{gathered}$$
  这个结果看着非常亲切对吧，其实就是在做高斯滤波
• 当${\varphi }_p()$为二次多项式时候
  $$E({\varphi }_p)={\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)(⟨{\varphi }_p,Q(p_i)⟩-{\varphi }_i{)}^2$$
  对于$p\in {\mathbb{R}}^2$的情况，$Q(p_i)=(1,x_i,y_i,x_i{y}_i,x_i^2,y_i^2{)}^T$，${\varphi }_p$为${\varphi }_p()$各项的系数$(c_{00},c_{10},c_{01},c_{11},c_{20},c_{02}{)}^T$
  $$\begin{gathered} \frac{\partial E}{\partial {\varphi }_p}={\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)2(⟨{\varphi }_p,Q(p_i)⟩-{\varphi }_i)Q(p_i)=0 \\ ⟹{\varphi }_p=({\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2)Q(p_i)Q(p_i{)}^T{)}^{-1}({\Sigma }_i\Theta ({\Vert {\mathbf{p}}_{\mathbf{i}}-\mathbf{p}\Vert }_2){\varphi }_{i}Q(p_i)) \end{gathered}$$
• 这个方法也可以拓展到任意维度的方程以及任意次数的多项式。

3.2 回到论文

那么面对实际的点云数据，要怎么使用MLS进行点云滤波呢。下面用几幅图来简单描述下MLS的想法（图片截自上交应忍冬老师的课件）

1. 对查询点的邻域拟合一条多项式曲面（在图中是条曲线）
   
2. 移动查询点到曲面上，实现滤波
   但这时候问题来了，以什么方向移动到曲面上是合理的
   很显然这样是不合理的。
   所以论文的做法是在额外拟合一个平面，用平面的法线作为点的移动方向
   
   接着我们应用上边提到的理论。以查询点在平面上的投影点作为坐标系原点，那邻域点在平面上的投影就构成了一个${\mathbb{R}}^2$的点集，邻域点$p_i$到平面的距离是${\varphi }_i$，要求解的平面和曲面也可以从上边提过的两个例子中获得启发。这样一说，MLS做滤波的思路是不是就非常直观了呢。

我这里讲的比较简洁，如果想看论文详解，可以移步下面这位博主的内容PCL MLS論文Computing and Rendering Point Set Surfaces研讀筆記

3.3 PCL测试

//核心头文件
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/surface/mls.h>
#include <pcl/io/ply_io.h>

int main(){
	// 加载点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPLYFile("D://welding_project//Project1//ply_source//three_edges_new.ply", *cloud);
    // 创建搜索树对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    // 创建 MLS 对象，并设置参数
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> mls;
    mls.setComputeNormals(true);
    mls.setInputCloud(cloud);
    mls.setPolynomialOrder(2);
    mls.setSearchMethod(tree);
    mls.setSearchRadius(5);

    // 存储 MLS 平滑后的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr mls_points(new pcl::PointCloud<pcl::PointXYZ>);
    mls.process(*mls_points);
    pcl::io::savePLYFile("D://welding_project//Project1//ply_source//MLS.ply", *mls_points);
}

下面是用实际采集到的数据测试的结果，采集的物体是两个接近垂直的平板，下面展示的是侧视图

其中白色的点云是原始点云，红色的点云是经过MLS滤波之后的点云，可以很明显的看到噪声被减小了。
但是注意两平面交界的地方，数据明显变圆了，说明MLS对于锐利边缘的保持效果较差，因此又有研究者提出了RMLS点云滤波（二）——RMLS（鲁棒移动最小二乘）介绍。

四. 参考文献

[1]Han X F, Jin J S, Wang M J, et al. A review of algorithms for filtering the 3D point cloud[J]. Signal Processing: Image Communication, 2017, 57: 103-112.

[2]Alexa M, Behr J, Cohen-Or D, et al. Point set surfaces[C]//Proceedings Visualization, 2001. VIS’01. IEEE, 2001: 21-29, 537.