C-平面拟合原理和最小法实现示例
C++ 平面拟合原理和最小法实现示例
平面拟合算法的核心目标是从三维空间中的一组离散点中找到最优拟合平面，使得这些点到该平面的垂直距离之和最小。以下是平面拟合的详细原理及实现方法：
1. 平面方程表示
三维平面的一般方程为：
[ A x + B y + C z + D
0 ] [ Ax + By + Cz + D = 0 ]
[
A
x
B
y
C
z
D
=
0
]
其中：
法向量 ：
( n
( A , B , C ) ) ( \mathbf{n} = (A, B, C) )
(
n
=
(
A
,
B
,
C
)) ，表示平面的朝向（通常归一化为单位向量）。
截距 ：
( D ) ( D )
(
D
) 为平面到原点的平移量。
为简化计算，常将方程标准化为：
[ A x + B y + C z
1 或 n ⋅ p + D
0 ] [ Ax + By + Cz = 1 \quad \text{或} \quad \mathbf{n} \cdot \mathbf{p} + D = 0 ]
[
A
x
B
y
C
z
=
1
或
n
⋅
p
D
=
0
]
其中
( p
( x , y , z ) ) ( \mathbf{p} = (x, y, z) )
(
p
=
(
x
,
y
,
z
)) 为点坐标。
2. 最小二乘法平面拟合
目标函数
最小二乘法通过最小化所有点到平面的 垂直距离平方和 来求解平面参数。点到平面的距离公式为：
[ d i
∣ A x i + B y i + C z i + D ∣ A 2 + B 2 + C 2 ] [ d_i = \frac{|Ax_i + By_i + Cz_i + D|}{\sqrt{A^2 + B^2 + C^2}} ]
[
d
i
=
A
2
B
2
C
2
∣
A
x
i
B
y
i
C
z
i
D
∣
]
为简化优化，忽略分母（即假设法向量归一化），目标函数简化为：
[ min A , B , C , D ∑ i
1 N ( A x i + B y i + C z i + D ) 2 ] [ \min_{A,B,C,D} \sum_{i=1}^N (Ax_i + By_i + Cz_i + D)^2 ]
[
A
,
B
,
C
,
D
min
i
=
1
∑
N
(
A
x
i
B
y
i
C
z
i
D
)
2
]
求解步骤
中心化数据 ：
计算点云质心
( p ˉ
( x ˉ , y ˉ , z ˉ ) ) ( \bar{\mathbf{p}} = (\bar{x}, \bar{y}, \bar{z}) )
(
p
ˉ
=
(
x
ˉ
,
y
ˉ
,
z
ˉ
)) ，并将所有点平移至质心坐标系：
[ x i ′
x i − x ˉ , y i ′
y i − y ˉ , z i ′
z i − z ˉ ] [ x_i’ = x_i - \bar{x}, \quad y_i’ = y_i - \bar{y}, \quad z_i’ = z_i - \bar{z} ]
[
x
i
′
=
x
i
−
x
ˉ
,
y
i
′
=
y
i
−
y
ˉ
,
z
i
′
=
z
i
−
z
ˉ
]
此时平面方程变为
( A x ′ + B y ′ + C z ′
0 ) ( A x’ + B y’ + C z’ = 0 )
(
A
x
′
B
y
′
C
z
′
=
0
) ，消去截距 ( D )。 2. 构建矩阵方程 ：
将每个点的坐标代入平面方程，形成超定方程组：
[ [ x 1 ′ y 1 ′ z 1 ′ x 2 ′ y 2 ′ z 2 ′ ⋮ ⋮ ⋮ x N ′ y N ′ z N ′ ] [ A B C ]
0 ] [ \begin{bmatrix} x_1’ & y_1’ & z_1’ \ x_2’ & y_2’ & z_2’ \ \vdots & \vdots & \vdots \ x_N’ & y_N’ & z_N’ \end{bmatrix} \begin{bmatrix} A \ B \ C \end{bmatrix} = \mathbf{0} ]
[
x
1
′
x
2
′
⋮
x
N
′
y
1
′
y
2
′
⋮
y
N
′
z
1
′
z
2
′
⋮
z
N
′
A
B
C
=
0
]
记为
( M v
0 ) ( \mathbf{M} \mathbf{v} = \mathbf{0} )
(
Mv
=
0
) ，其中
( v
( A , B , C ) T ) ( \mathbf{v} = (A, B, C)^T )
(
v
=
(
A
,
B
,
C
)
T
) 。 3. 奇异值分解（SVD）求解 ：
对矩阵
( M ) ( \mathbf{M} )
(
M
) 进行奇异值分解：
[ M
U Σ V T ] [ \mathbf{M} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T ]
[
M
=
UΣ
V
T
]
最小奇异值对应的右奇异向量即为法向量 ( \mathbf{v} )。 4. 恢复截距 ：
根据质心坐标计算
( D ) ( D )
(
D
) ：
[ D
− ( A x ˉ + B y ˉ + C z ˉ ) ] [ D = - (A \bar{x} + B \bar{y} + C \bar{z}) ]
[
D
=
−
(
A
x
ˉ
B
y
ˉ
C
z
ˉ
)]
3. 主成分分析（PCA）法
PCA方法通过分析点云的协方差矩阵，找到方差最小的方向（即法向量方向）。
步骤
计算协方差矩阵 ：
[ C
1 N ∑ i
1 N ( p i − p ˉ ) ( p i − p ˉ ) T ] [ \mathbf{C} = \frac{1}{N} \sum_{i=1}^N (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{p}_i - \bar{\mathbf{p}})^T ]
[
C
=
N
1
i
=
1
∑
N
(
p
i
−
p
ˉ
)
(
p
i
−
p
ˉ
)
T
]
特征值分解 ：
对
( C ) ( \mathbf{C} )
(
C
) 进行特征分解，得到特征值
( λ 1 ≥ λ 2 ≥ λ 3 ) ( \lambda_1 \geq \lambda_2 \geq \lambda_3 )
(
λ
1
≥
λ
2
≥
λ
3
) 和对应的特征向量
( v 1 , v 2 , v 3 ) ( \mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3 )
(
v
1
,
v
2
,
v
3
) 。
确定法向量 ：
最小特征值
( λ 3 ) ( \lambda_3 )
(
λ
3
) 对应的特征向量
( v 3 ) ( \mathbf{v}_3 )
(
v
3
) 即为平面法向量
( n ) ( \mathbf{n} )
(
n
) 。
计算截距 ：
利用质心坐标计算
( D ) ( D )
(
D
) ：
[ D
− n ⋅ p ˉ ] [ D = -\mathbf{n} \cdot \bar{\mathbf{p}} ]
[
D
=
−
n
⋅
p
ˉ
]
4. 鲁棒平面拟合（RANSAC）
当数据中存在噪声或离群点时，RANSAC（随机采样一致性）算法可提高拟合鲁棒性。
步骤
随机采样 ：
随机选择3个点计算初始平面参数。
内点检测 ：
计算所有点到该平面的距离，设定阈值
( ϵ ) ( \epsilon )
(
ϵ
) ，统计距离小于
( ϵ ) ( \epsilon )
(
ϵ
) 的内点数量。
迭代优化 ：
重复上述过程，保留内点最多的平面模型。
精确拟合 ：
使用所有内点通过最小二乘法重新拟合最终平面。
5. 算法对比
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 最小二乘法 | 计算简单，闭式解 | 对噪声敏感，离群点影响大 | 数据干净，无显著离群点 |
| PCA法 | 无需迭代，计算高效 | 假设数据服从高斯分布，对噪声敏感 | 点云分布均匀，主方向明显 |
| RANSAC | 鲁棒性强，抗离群点 | 计算开销大，依赖随机采样次数 | 数据含噪声或大量离群点 |
6. 代码实现示例（最小二乘法）
#include <Eigen/Dense>
using namespace Eigen;
void fitPlane(const std::vector<Vector3d>& points, Vector3d& normal, double& d) {
// 计算质心
Vector3d centroid(0, 0, 0);
for (const auto& p : points) centroid += p;
centroid /= points.size();
// 构建矩阵 M
MatrixXd M(points.size(), 3);
for (size_t i = 0; i < points.size(); ++i) {
M.row(i) = points[i] - centroid;
}
// SVD分解
JacobiSVD<MatrixXd> svd(M, ComputeThinV);
normal = svd.matrixV().col(2); // 最小奇异值对应的列
d = -normal.dot(centroid);
}7. 应用场景
- 三维重建 ：从点云中提取平面结构（如墙面、地面）。
- 机器人导航 ：识别地面平面用于定位。
- 工业检测 ：检测工件表面的平整度。
通过上述方法，可根据数据特性选择合适算法，平衡精度、速度和鲁棒性。