平面几何参数表示

平面几何

ref：

• slam中线和面特征的参数化表示方法https://blog.csdn.net/qq_35102059/article/details/122437847

• SLAM中面特征的参数化https://zhuanlan.zhihu.com/p/71924149

• 计算机世界中的多视图几何

平面方程描述

面的参数化表示

Hesse形式

3D空间中，平面方程为
$$ax+by+cz+d=0$$
平面方程系数只有比例是存在意义的$a:b:c:d$，因此虽然平面方程有四个系数，但是在3D空间中只有3个自由度。

平面法向量$n_\pi $的齐次坐标为
$$(\frac{a}{d},\frac{b}{d},\frac{c}{d},1{)}^T$$
空间平面上一点$P_f$的齐次坐标为
$$(x,y,z,1{)}^T$$
则平面方程转化为
$$n_{\pi }^T\cdot P_f=0$$
如果用非齐次坐标,法向量$n_{\pi }=(a,b,c{)}^T,P_f=(x,y,z{)}^T$，则方程转化为Hesse形式
$$n_{\pi }^T\cdot P_f+d=0$$
$\frac{d}{||n||}$为原点到平面的距离，此种表示形式左乘一个变换矩阵也可以进行坐标的变换。

注意：

• 此种表示形式有4个参数，但平面只有3个自由度，存在过参数化问题。

• 在使用高斯牛顿优化时，计算出的Hessian矩阵会不满秩，也就不能求逆。所以只能使用列文伯格-马夸尔特算法(L-M)提供的正则化(regularization)。

• 也会使得代价函数在某些方向上为平面时的优化收敛速度有二阶收敛变为线性收敛。

• 还不能使用信赖区域法。

平面性质：

点到平面的距离

$$D=\frac{a{x}_1+b{y}_1+c{z}_1+d}{\sqrt{a^2+b^2+c^2}}$$

三点确定一个平面

设三个点$X_i$都在平面$\pi$上，则每个点都满足平面方程，得
[ X 1 T X 2 T X 3 T ] 3 × 4 ⋅ π = 0 \begin{bmatrix} X_1^T\\X_2^T\\X_3^T \end{bmatrix}_{3\times 4} \cdot \pi=0 ​X1T​X2T​X3T​​ ​3×4​⋅π=0
如果三个点$X_1,X_2,X_3$线性无关，所以由它们作为行组成的矩阵的秩为3，则平面$\pi$作为它的一维零空间唯一确定(差一个常数因子)，如果秩为2，则零空间为2维，那么这些点是共线的，得到以共线点组成的直线为轴的一个平面束。

那么平面参数$(a,b,c,d)$就可以构建一个$n\times 4$的系数矩阵，构建线性方程$A_{n\times 4}x=0$，用SVD即可求解。

#include 
#include 

Eigen::Vector4d PlaneFitting(const std::vector<Eigen::Vector3d> & plane_pts) {
    Eigen::Vector3d center = Eigen::Vector3d::Zero();
    // 计算平面上所有点的质心
    for (const auto & pt : plane_pts) center += pt;
    	center /= plane_pts.size();
	//构建质心坐标系下的矩阵A，用于计算平面拟合
    Eigen::MatrixXd A(plane_pts.size(), 3);
    for (int i = 0; i < plane_pts.size(); i++) {
        A(i, 0) = plane_pts[i][0] - center[0];
        A(i, 1) = plane_pts[i][1] - center[1];
        A(i, 2) = plane_pts[i][2] - center[2];
    }
	//利用SVD分解拟合平面参数
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);
    const float a = svd.matrixV()(0, 2);
    const float b = svd.matrixV()(1, 2);
    const float c = svd.matrixV()(2, 2);
    const float d = -(a * center[0] + b * center[1] + c * center[2]);
    return Eigen::Vector4d(a, b, c, d);
}