标签：组织 方程组 三维 位置 定义 方向 多点 向量 array
点云法向量是最基本的点云特征，在诸多点云处理算法中起着至关重要的作用。本文记录了两种常用的点云法向量估计方法，分别针对一般点云和深度图两种点云组织形式。
估计一般点云法向量的思路是根据邻域内的点拟合一个平面，则平面的法线方向即为点的法向量。记点的邻域为：
\[\mathcal{N}=\{\mathcal{p}_i(x_i, y_i, z_i)|i=1,2,..,n\}\]
需拟合的平面的一般形式是：
\begin{align}
Ax+By+Cz+D &= 0 \notag \newline
A^2 + B^2 + C^2 &= 1 \notag
\end{align}
采用最小二乘法进行平面的拟合,即求解如下的最小值问题：
\[\min_{A,B,C,D}\sum_{i=1}^n{(Ax_i +By_i+Cz_i+D)}^2 \quad\mathcal{s}.\mathcal{t}.\quad A^2 + B^2 + C^2 = 1 \]
通过求导并令导数为\(0\)，且将方程组中的\(D\)消去，可以得到如下线性方程组：
\[
M\left[ \begin{array}{c}A\\B\\C\end{array} \right ] =
\left[ \begin{array}{ccc}\bar{x^2}-\bar{x}^2& \bar{xy}-\bar{x}\bar{y} & \bar{xz}-\bar{x}\bar{z} \ \bar{xy}-\bar{x}\bar{y}& \bar{y^2}-\bar{y}^2 & \bar{yz}-\bar{y}\bar{z} \ \bar{xz}-\bar{x}\bar{z}& \bar{yz}-\bar{y}\bar{z} & \bar{z^2}-\bar{z}^2
\end{array} \right ]\left[ \begin{array}{c}A\\B\\C\end{array} \right ] = 0 \quad\mathcal{s}.\mathcal{t}.\quad A^2 + B^2 + C^2 = 1
\]
其中\(M\)是协方差矩阵，\(\bar{x}=\frac{1}{n}\sum_{i=1}^nx_i\), \(\bar{xy}=\frac{1}{n}\sum_(i=1)^nx_iy_i\)，依此类推。
这是典型的在单位圆上求解寻找方程组的解。一般情况下，协方差矩阵是非奇异的，因此上述方程没有精确解，只能寻找近似解，令\(n=\left[A,B,C\right]^T\)满足：
\[\min_{n}\Vert{Mn}\Vert \quad\mathcal{s}.\mathcal{t}.\Vert n \Vert = 1 \]
利用拉格朗日乘子法，构造新的优化函数：
\[f(n,\lambda)={(Mn)}^TMn + \lambda(1-n^Tn)\]
从而能够推导出上式的解是协方差矩阵\(M\)的最小特征值所对应的归一化特征向量
从而可以得到点云的法向量是:\[\vec{n}=[n_x,n_y,n_z]^T=\pm[A,B,C]^T\]。
对于结构化的点云——深度图而言，可以更加简便的估计点云的法向量。思路是利用深度梯度来近似估计点云法向量。记深度是\(d\)，已知深度图，与图像梯度的计算方式相同（梯度方向定义为函数上升的方向），可以直接求出\(x\)和\(y\)方向上的深度梯度，分别记作\(\nabla{d_x}\)和\(\nabla{d_y}\)。
根据针孔相机模型，可以将图像坐标转化为三维空间坐标：
\[{(x,y,z)}^T={(\frac{u-c_x}{f}d, \frac{u-c_x}{f}d, d)}^T\]
其中\(f,c_x,c_y\)是相机内参，\(u,v\)是像素坐标，以及将深度梯度转化为\(z\)关于\(x,y\)的梯度：
\[{(\nabla{z_x},\nabla{z_y})}^T=\frac{f}{d}{(\nabla{d_x}, \nabla{d_y})}^T\]
则点云的法向量可以近似估计为：
\[\vec{n}=(n_x,n_y,n_z)^T=\pm\mathcal{normalize}{(\nabla{z_x},\nabla{z_y},-1)}^T\]
前述方法计算得到的法向量方向具有不确定性，有必要进行统一，通常做法是根据视线方向进行统一，确保法向量方向的一致性。一般将原点作为视点位置，则视线为：\[\vec{v}={(-x,-y,-z)}^T\]
对点云的法向量进行调整，使得：
\[\vec{v}\cdot\vec{n}>0\]
标签：组织 方程组 三维 位置 定义 方向 多点 向量 array
原文地址：https://www.cnblogs.com/lijian-tech-blog/p/11623400.html
