最小二乘拟合平面——拉格朗日乘子法

目录

  • 一、算法原理
  • 二、代码实现
    • 1、python
    • 2、matlab
  • 三、算法效果

一、算法原理

  设拟合出的平面方程为:
a x + b y + c z + d = 0 (1) ax+by+cz+d=0\tag{1} ax+by+cz+d=0(1)
约束条件为: a 2 + b 2 + c 2 = 1 (2) a^2+b^2+c^2=1\tag{2} a2+b2+c2=1(2)
  可以得到平面参数 a 、 b 、 c 、 d a、b、c、d abcd。此时,要使获得的拟合平面是最佳的,就是使点到该平面的距离的平方和最小,即满足:
e = ∑ i = 1 n d i 2 → m i n (3) e=\sum_{i=1}^nd_i^2\rightarrow min\tag{3} e=i=1ndi2min(3)
  式中, d i = ∣ a x i + b y i + c z i + d ∣ d_i=|ax_i+by_i+cz_i+d| di=axi+byi+czi+d,是点云数据中的任一点 p i ( x i , y i , z i ) p_i(x_i,y_i,z_i) pi(xi,yi,zi)到这个平面的距离。要使 e → m i n e\rightarrow min emin,可以利用拉格朗日乘子法求解极值,得到函数:
f = e − λ ( a 2 + b 2 + c 2 − 1 ) = ∑ i = 1 n d i 2 − λ ( a 2 + b 2 + c 2 − 1 ) (4) f = e - λ(a^2 + b^2 + c^2 - 1) =\sum_{i=1}^nd_i^2 - λ(a^2 + b^2 + c^2 - 1) \tag{4} f=eλ(a2+b2+c21)=i=1ndi2λ(a2+b2+c21)(4)

  先将式(2-4)两边对 d d d求偏导,并且令偏导数为零,得到:
d = 1 n ( a ∑ i = 1 n x i 2 + b ∑ i = 1 n y i 2 + c ∑ i = 1 n z i 2 ) (5) d=\frac{1}{n}(a\sum_{i=1}^nx_i^2+b\sum_{i=1}^ny_i^2+c\sum_{i=1}^nz_i^2)\tag{5} d=n1(ai=1nxi2+bi=1nyi2+ci=1nzi2)(5)
x ˉ = ∑ i = 1 n x i n \bar{x}=\sum_{i=1}^n\frac{x_i}{n} xˉ=i=1nnxi y ˉ = ∑ i = 1 n y i n \bar{y}=\sum_{i=1}^n\frac{y_i}{n} yˉ=i=1nnyi z ˉ = ∑ i = 1 n z i n \bar{z}=\sum_{i=1}^n\frac{z_i}{n} zˉ=i=1nnzi,质心为 P ˉ = ( x ˉ , y ˉ , z ˉ ) \bar{P}=(\bar{x},\bar{y},\bar{z}) Pˉ=(xˉ,yˉ,zˉ) Δ x i = x i − x ˉ \Delta x_i=x_i-\bar{x} Δxi=xixˉ Δ y i = y i − y ˉ \Delta y_i=y_i-\bar{y} Δyi=yiyˉ Δ z i = z i − z ˉ \Delta z_i=z_i-\bar{z} Δzi=zizˉ则:
d i = ∣ a Δ x i + b Δ y i + c Δ z i ∣ (6) d_i=|a\Delta x_i+b\Delta y_i+c\Delta z_i|\tag{6} di=aΔxi+bΔyi+cΔzi(6)
  再对式(2-4)两边求对 a 、 b 、 c a 、b 、c abc 的偏导数,得
{ 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ x i − 2 λ a = 0 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ y i − 2 λ b = 0 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ z i − 2 λ c = 0 \begin{cases} 2\sum_{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta x_i-2λa=0\\ 2\sum_{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta y_i-2λb=0\\ 2\sum_{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta z_i-2λc=0 \end{cases} 2i=1n(aΔxi+bΔyi+cΔzi)Δxi2λa=02i=1n(aΔxi+bΔyi+cΔzi)Δyi2λb=02i=1n(aΔxi+bΔyi+cΔzi)Δzi2λc=0

  将上述方程组构成特征值方程:
A x = λ x (7) Ax = λx \tag{7} Ax=λx(7)
式中,
A = [ ∑ i = 1 n Δ x i 2 ∑ i = 1 n Δ x i Δ y i ∑ i = 1 n Δ x i Δ z i ∑ i = 1 n Δ x i Δ y i ∑ i = 1 n Δ y i 2 ∑ i = 1 n Δ y i Δ z i ∑ i = 1 n Δ x i Δ z i ∑ i = 1 n Δ y i Δ z i ∑ i = 1 n Δ z i 2 ] A= \left[ \begin{matrix} \sum_{i=1}^n\Delta x_{i}^{2}&\sum_{i=1}^n\Delta x_{i}\Delta y_{i}&\sum_{i=1}^n\Delta x_{i}\Delta z_{i} \\ \sum_{i=1}^n\Delta x_{i}\Delta y_{i}&\sum_{i=1}^n\Delta y_{i}^{2}&\sum_{i=1}^n\Delta y_{i}\Delta z_{i} \\ \sum_{i=1}^n\Delta x_{i}\Delta z_{i} &\sum_{i=1}^n\Delta y_{i}\Delta z_{i} &\sum_{i=1}^n\Delta z_{i}^{2}\\ \end{matrix} \right] A= i=1nΔxi2i=1nΔxiΔyii=1nΔxiΔzii=1nΔxiΔyii=1nΔyi2i=1nΔyiΔzii=1nΔxiΔzii=1nΔyiΔzii=1nΔzi2
x = [ a b c ] x= \left[ \begin{matrix} a\\ b\\ c\\ \end{matrix} \right] x= abc
  那么,求解平面参数 a 、 b 、 c a 、b 、c abc ,就是求解矩阵的特征值和特征向量。又因为 A A A 是3 阶实对称矩阵,其特征值求解公式为:
λ = ( A x ) T x x T x , x ≠ 0 (8) λ=\frac{(Ax)^Tx}{x^Tx},x\neq0\tag{8} λ=xTx(Ax)Tx,x=0(8)
  在约束条件 a 2 + b 2 + c 2 = 1 a^2 + b^2 + c^2 = 1 a2+b2+c2=1 下,得 λ = ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) 2 = ∑ i = 1 n d i 2 = e λ=\sum_{i=1}^n(a\Delta x_i+b\Delta y_i+c\Delta z_i)^2=\sum_{i=1}^nd_i^2=e λ=i=1n(aΔxi+bΔyi+cΔzi)2=i=1ndi2=e。 所以, e e e最小值就是矩阵 A A A的最小特征值,对应特征向量为平面参数 a 、 b 、 c a 、b 、c abc ,利用质心可求得 d d d

二、代码实现

1、python

import numpy as np


# 创建函数,用于生成不同属于一个平面的100个离散点
def not_all_in_plane(a, b, c):
    x = np.random.uniform(-10, 10, size=100)
    y = np.random.uniform(-10, 10, size=100)
    z = (a * x + b * y + c) + np.random.normal(-1, 1, size=100)
    return x, y, z


# 调用函数,生成离散点
x, y, z = not_all_in_plane(2, 5, 6)
# 计算质心
x0 = np.mean(x)
y0 = np.mean(y)
z0 = np.mean(z)
# 去质心
x = x - x0
y = y - y0
z = z - z0
# ------------------------构建系数矩阵-----------------------------
A = np.array([[sum(x * x), sum(x * y), sum(x * z)],
              [sum(x * y), sum(y * y), sum(y * z)],
              [sum(x * z), sum(y * z), sum(z * z)]])
[D, X] = np.linalg.eig(A)

print('平面拟合结果为:z = %.3f * x + %.3f * y + %.3f' % (X[0, 2], X[1, 2], X[2, 2]))

2、matlab

clc;clear;
%% -------------------------------读取点云---------------------------------
pc = ReadPointCloud('plane1.pcd');
%% -----------------------------获取点云信息-------------------------------
n ; % 点的个数
x ; % 点的x坐标
y ; % 点的y坐标
z ; % 点的z坐标
% 1、计算点云质心
centroid = pcmean(pc);
% 2、去质心化
deMean=pcdemean(pc.Location,centroid);
%% -------------------------------拟合平面---------------------------------
% 3、矩阵A
A = [sumXX sumXY sumXZ;
	sumXY sumYY sumYZ;
	sumXZ sumYZ sumZZ];
% 4、矩阵A分解求特征值特征向量
[V,D]=eig(A);
a = V(1,1);b = V(2,1);c = V(3,1);
% 5、计算原点到拟合平面的距离
d = -dot([a b c],centroid);
%% ---------------------------可视化拟合结果-------------------------------
 figure
% 图形绘制
scatter3(x,y,z,'filled')
hold on;
[XFit,YFit]= meshgrid (xfit,yfit);
mesh(XFit,YFit,ZFit);
title('拉格朗日乘子法拟合平面');


三、算法效果

最小二乘拟合平面——拉格朗日乘子法_第1张图片

你可能感兴趣的:(CloudCompare,平面,开发语言,算法,计算机视觉)