使用积分图计算点云法向量

目前网上能找到的所有关于“使用积分图计算点云法相”的资料都是来自PCL的，只有code，没有原理
这篇笔记来自论文 Adaptive neighborhood selection for real-time surface normal estimation from organized point cloud data using integral images

积分图定义

将一张图片中的每一个像素点的取值替换为其（在原始图片中）左上角区域的所有像素值之和，就是积分图。公式如下：
$${\mathcal{I}}_{\mathcal{O}}(m,n)=\sum _{i=0}^m\sum _{j=0}^n\mathcal{O}(i,j)$$

也可通过迭代的方式计算，这样只用从左上角开始遍历一次图片就可以获得它的积分图：
$$\begin{array}{rl}{\mathcal{I}}_{\mathcal{O}}(m,n)=& \mathcal{O}(m,n)+{\mathcal{I}}_{\mathcal{O}}(m-1,n)+{\mathcal{I}}_{\mathcal{O}}(m,n-1)-{\mathcal{I}}_{\mathcal{O}}(m-1,n-1)\end{array}$$
计算一个方形区域的平均值可以用如下公式：
$$\begin{array}{rl}S\left({\mathcal{I}}_{\mathcal{O}},m,n,r\right)=\frac{1}{4r^2}\cdot (& {\mathcal{I}}_{\mathcal{O}}(m+r,n+r)\\ & -{\mathcal{I}}_{\mathcal{O}}(m-r,n+r)\\ & -{\mathcal{I}}_{\mathcal{O}}(m+r,n-r)\\ & +{\mathcal{I}}_{\mathcal{O}}(m-r,n-r))\end{array}$$
其中$(m,n{)}^{\top }$表示方形区域的中心坐标，$r$为边长的一半。这里计算方形区域是为了后面用的，当然也可以计算任意矩形区域，将$r$换为$a,b$做相应的改变就好啦

参考资料：积分图

平滑处理（预处理）

由于RGB-D相机采集的数据（即深度图）会有很多噪声，常用平滑处理来降噪。
平滑处理的一个很重要的参数是平滑窗口的大小。
在深度图中，不能使用固定大小的平滑窗口，因为1）不同深度的地方，信噪比不一样；2）边缘区域不能用大的平滑窗口，否则边缘信息都被抹掉了。
所以，需要考虑上述两个因素，分别计算每一个地方的平滑窗口大小，计算细节如下：

• 首先，用公式表示深度值$d$与最小深度变化(minimum depth change)的关系：$$f_{DC}(d)=\alpha \cdot d^2$$也就是下图中的红线
  实验结果表明，$\alpha =0.0028$
• 计算Depth-Dependent Smoothing Area Map：$$\mathcal{B}(m,n)=\beta \cdot f_{DC}(\mathcal{D}(m,n))$$其中，$\beta$由用户设定，用来控制平滑窗口的大小，$\mathcal{D}(m,n)$为深度图上点$(m,n{)}^{\top }$处的值（即深度）
• 计算Depth Change Indication Map：$$\mathcal{C}(m,n)=\{\begin{array}{l}1\{\begin{array}{l}\text{ if }\Vert \delta {\mathcal{D}}_x(m,n)\Vert \ge t_{DC}(\mathcal{D}(m,n))\\ \text{ or }\Vert \delta {\mathcal{D}}_y(m,n)\Vert \ge t_{DC}(\mathcal{D}(m,n))\end{array}\\ 0\text{ otherwise. }\end{array}$$其中$\delta {\mathcal{D}}_x(m,n)=\mathcal{D}(m+1,n)-\mathcal{D}(m,n),\delta {\mathcal{D}}_y(m,n)=\mathcal{D}(m,n+1)-\mathcal{D}(m,n)$分别表示在x、y正方向的深度变化，$t_{DC}(d)=\gamma \cdot f_{DC}(d)$表示深度变化检测的阈值，通过$\gamma$来控制用户想要的检测灵敏度
• 计算Final Smoothing Area Map：也就是每个位置的滑动窗口大小$$\mathcal{R}(m,n)=\min \left(\mathcal{B}(m,n),\frac{\mathcal{T}(m,n)}{\sqrt{2}}\right)$$其中，$\mathcal{T}(m,n)$为距离变换算法(Distance Transformations)计算获得的值，表示点$(m,n{)}^{\top }$的到下一个depth change的2D距离；$\sqrt{2}$是因为这里用的是方形区域，而非圆。

方法1：只使用一张积分图

最简单的计算法向量的方法是将上下左右的点（红色）连线叉乘，Kinect中用的就是这种方法。公式为：$${\vec{n}}_p={\vec{v}}_{p,h}\times {\vec{v}}_{p,v}\text{\hspace{1em}(1)}$$

但是这样计算太粗糙了，很容易受到噪声影响。可以使用单张积分图来计算公式(1)中的两个向量
$$\begin{array}{rl}{\vec{v}}_{p,h,x}& =\frac{{\mathcal{P}}_x(m+r,n)-{\mathcal{P}}_x(m-r,n)}{2}\\ {\vec{v}}_{p,h,y}& =\frac{{\mathcal{P}}_y(m+r,n)-{\mathcal{P}}_y(m-r,n)}{2}\\ {\vec{v}}_{p,h,z}& =\frac{S\left({\mathcal{I}}_{{\mathcal{P}}_z},m+1,n,r-1\right)}{2}-\frac{S\left({\mathcal{I}}_{{\mathcal{P}}_z},m-1,n,r-1\right)}{2}\\ {\vec{v}}_{p,v,x}& =\frac{{\mathcal{P}}_x(m,n+r)-{\mathcal{P}}_x(m,n-r)}{2}\\ {\vec{v}}_{p,v,y}& =\frac{{\mathcal{P}}_y(m,n+r)-{\mathcal{P}}_y(m,n-r)}{2}\\ {\vec{v}}_{p,v,z}& =\frac{S\left({\mathcal{I}}_{{\mathcal{P}}_z},m,n+1,r-1\right)}{2}-\frac{S\left({\mathcal{I}}_{{\mathcal{P}}_z},m,n-1,r-1\right)}{2}\end{array}$$其中${\mathcal{P}}_x,{\mathcal{P}}_y,{\mathcal{P}}_z$分别表示图像上的二维点经内参矩阵变换到三维点的每一个通道(x,y,z)组成的“图片”；${\mathcal{I}}_{{\mathcal{P}}_z}$表示z对应的“图片”的积分图；$r=\mathcal{R}(m,n)$。
然后就可以用公式(1)计算法向量了。

方法2：使用多张积分图

另一种计算法向量的思路是计算领域的协方差，然后求得特征值最小的特征向量作为法向量。领域的选择可以为最近领域搜索or一定半径内的范围，但是太费计算量了。可以使用九张积分图来计算协方差然后获得法向量：

• 先计算三张积分图： ${\mathcal{I}}_{{\mathcal{P}}_x},{\mathcal{I}}_{{\mathcal{P}}_y},{\mathcal{I}}_{{\mathcal{P}}_z}$，与方法1中倒数第二段中的一样，分别为图像上的二维点经内参矩阵变换到三维点的每一个通道(x,y,z)组成的“图片”的积分图
• 然后计算六张积分图：${\mathcal{I}}_{{\mathcal{P}}_{xx}},{\mathcal{I}}_{{\mathcal{P}}_{xy}},{\mathcal{I}}_{{\mathcal{P}}_{xz}},{\mathcal{I}}_{{\mathcal{P}}_{yy}},{\mathcal{I}}_{{\mathcal{P}}_{yz}},{\mathcal{I}}_{{\mathcal{P}}_{zz}}$。其中${\mathcal{I}}_{{\mathcal{P}}_{ab}}$表示${\mathcal{I}}_{{\mathcal{P}}_a},{\mathcal{I}}_{{\mathcal{P}}_b}$的点积，即对应位置相乘（类似向量的点乘）
• 点$(m,n{)}^{\top }$处的协方差公式为$${\mathcal{C}}_p=\left(\begin{array}{ccc}{c}_{xx}& c_{xy}& c_{xz}\\ c_{yx}& c_{yy}& c_{yz}\\ c_{zx}& c_{zy}& c_{zz}\end{array}\right)-\left(\begin{array}{c}{c}_x\\ c_y\\ c_z\end{array}\right){\left(\begin{array}{c}{c}_x\\ c_y\\ c_z\end{array}\right)}^{\top }$$其中$$\begin{array}{rl}{c}_{xx}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{xx}},m,n,\mathcal{R}(m,n)\right)\\ c_{xy}=c_{yx}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{xy}},m,n,\mathcal{R}(m,n)\right)\\ c_{xz}=c_{zx}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{xz}},m,n,\mathcal{R}(m,n)\right)\\ c_{yy}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{yy}},m,n,\mathcal{R}(m,n)\right)\\ c_{yz}=c_{zy}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{yz}},m,n,\mathcal{R}(m,n)\right)\\ c_{zz}& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_{zz}},m,n,\mathcal{R}(m,n)\right)\\ c_x& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_x},m,n,\mathcal{R}(m,n)\right)\\ c_y& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_y},m,n,\mathcal{R}(m,n)\right)\\ c_z& =\mathcal{S}\left({\mathcal{I}}_{{\mathcal{P}}_z},m,n,\mathcal{R}(m,n)\right)\end{array}$$
• 最后，计算${\mathcal{C}}_p$的最小特征值对应的特征向量就可以作为法向量

虽然多个积分图的计算量大于单张积分图的，但是它可以直接获得领域的平面性 (planarity)，可用于平面拟合、边缘/角点检测

实验

• 实验平台：2.26 GHz Intel® Core™2 Quad CPU, 4 GB of RAM
• 速度：方法1(SDC) VS 方法2(CM) VS KNN（也就是将方法2中领域用KNN找，再计算协方差矩阵，最后求得特征向量）
  
  可以看到SDC的时间最少，而且CM和SDC的复杂度（几乎）为O(1)
• 误差：
  
  

可以看到SDC的误差最少

PCL中的积分图计算法向量方法

• COVARIANCE_MATRIX：从邻域的协方差矩阵中创建9个积分图去计算一个点的法线
• AVERAGE_3D_GRADIENT：创建了6个积分图去计算3D梯度里面垂直和水平方向的光滑部分，同时利用两个梯度的叉积来计算法线。
• AVERAGE_DEPTH_CHANGE：只创建了一个单一的积分图，从平均深度的变化中计算法线。
• SIMPLE_3D_GRADIENT：无任何介绍

参考资料：
PCL官方文档中IntegralImageNormalEstimation模板类里的NormalEstimationMethod枚举