快速估计一组点形成的平面并判断点云数据的一致性

 补充一下关于点和平面的知识：

        一个平面通常可以表达为：ax + by + cz + d = 0             

        其中，向量（a,b,c）就是该平面的法向量，如果一个点在平面上应该符合平面方程。

        那么如果在空间中存在一点P(x_0, y_0, z_0)，该点到平面的距离为：

$$
D=\frac{\left| ax_0+by_0+cz_0+d \right|}{\sqrt{a^2+b^2+c^2}}
$$

        将平面归一化：ax + by + cz + 1 = 0 

        那么如果存在多个点，应当都符合 ax + by + cz + 1 = 0，我们写成矩阵形式：

        $$
\left[ \begin{array}{l}
    x_0&        y_0&        z_0\\
    \cdot&        \cdot&        \cdot\\
    x_n&        y_n&        z_n\\
\end{array} \right] \left[ \begin{array}{c}
    a\\
    b\\
    c\\
\end{array} \right] +\left[ \begin{array}{c}
    1\\
    \cdot\\
    1\\
\end{array} \right] =\left[ \begin{array}{c}
    0\\
    \cdot\\
    0\\
\end{array} \right] 
$$

        记：

$$
A=\left[ \begin{array}{l}
    x_0&        y_0&        z_0\\
    \cdot&        \cdot&        \cdot\\
    x_n&        y_n&        z_n\\
\end{array} \right] \,\,   b=\left[ \begin{array}{c}
    -1\\
    \cdot\\
    -1\\
\end{array} \right] \,\, normvec=\left[ \begin{array}{c}
    a\\
    b\\
    c\\
\end{array} \right] 
$$

        则有方程：A * normvec = b

        通过解这个方程，就可以算出a,b,c。

但是在实际应用时，

        提供的点往往多余3个，那么A * normvec = b就是一个超定线性方程，解决方法通常是找到一个最优解，使得 A * normvec = b 的范数最小，相当于求解问题：

$$
\min _{normvec}||A*normvec+1||
$$

        这个问题的直观理解是：在空间中有一组点，寻找一个平面使得所有点到该平面的距离之和最小。

        求解这个超定线性方程，可以采用

下面是相关代码，摘自Robust Real-time LiDAR-inertial Initialization: