Least squares fitting plane - direct solution method

1. Algorithm principle

  The general expression of the plane equation is:
$$Ax+By+Cz+D=0(C\ne 0)\text{\hspace{1em}(1)}$$
即：
$$z=-\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C}\text{\hspace{1em}(2)}$$
记：
$$a_0=-\frac{A}{C},a_1=-\frac{B}{C},a_2=-\frac{D}{C}\text{\hspace{1em}( 3 )}$$
Substitute formula (3) into formula (2) to get formula (4):
$$z=a_{0}x+a_{1}y+a_2\text{\hspace{1em}( 4 )}$$
  For a series$n$ points$(n\ge 3)$；$(x_i,y_i,z_i),i=0,1,...,n-1$ , to use the$n$ points fit the plane equation, even if:
$$S=\sum _{i=1}^n(a_{0}x+a_{1}y+a_2-z)\to min\text{\hspace{1em}(5)}$$

  To make $S$ is the smallest, and the two sides of formula (4) should be$a_0,a_1,a_2$求偏导，并且令偏导数为零。
即：
$$\{\begin{array}{l}2{\sum }_{i=1}^n(a_0{x}_i+a_1{y}_i+a_2-z_i)x_i=0\\ 2{\sum }_{i=1}^n(a_0{x}_i+a_1{y}_i+a_2-z_i)y_i=0\\ 2{\sum }_{i=1}^n(a_0{x}_i+a_1{y}_i+a_2-z_i)=0\end{array}\text{\hspace{1em}(6)}$$
  改写成矩阵的形式为：
$$\left[\begin{array}{ccc}{\sum }_{i=1}^n{x}_i^2& {\sum }_{i=1}^n{x}_i{y}_i& {\sum }_{i=1}^n{x}_i\\ {\sum }_{i=1}^n{x}_i{y}_i& {\sum }_{i=1}^n{y}_i^2& {\sum }_{i=1}^n{y}_i\\ {\sum }_{i=1}^n{x}_i& {\sum }_{i=1}^n{y}_i& n\end{array}\right]\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\end{array}\right]=\left[\begin{array}{c}{\sum }_{i=1}^n{x}_i{z}_i\\ {\sum }_{i=1}^n{y}_i{z}_i\\ {\sum }_{i=1}^n{z}_i\end{array}\right]\text{\hspace{1em}( 7 )}$$
Solving equations (7), you can get parameters$a_0,a_1,a_2$, substituting into formula (4) to get the plane equation.

2. Code implementation

1、python

import numpy as np
import matplotlib.pyplot as plt


# 创建函数，用于生成不同属于一个平面的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)
# ------------------------构建系数矩阵-----------------------------
A = np.array([[sum(x ** 2), sum(x * y), sum(x)],
              [sum(x * y), sum(y ** 2), sum(y)],
              [sum(x), sum(y), N]])

B = np.array([[sum(x * z), sum(y * z), sum(z)]])

# 求解
X = np.linalg.solve(A, B.T)
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (X[0], X[1], X[2]))
# -------------------------结果展示-------------------------------
fig1 = plt.figure()
ax1 = fig1.add_subplot(111, projection='3d')
ax1.set_xlabel("x")
ax1.set_ylabel("y")
ax1.set_zlabel("z")
ax1.scatter(x, y, z, c='r', marker='o')
x_p = np.linspace(-10, 10, 100)
y_p = np.linspace(-10, 10, 100)
x_p, y_p = np.meshgrid(x_p, y_p)
z_p = X[0] * x_p + X[1] * y_p + X[2]
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
plt.show()

2、matlab

clc;clear;
%% -------------------------------读取点云---------------------------------
pc = ReadPointCloud('plane1.pcd');
%% -----------------------------获取点云信息-------------------------------
n ; % 点的个数
x ; % 点的x坐标
y ; % 点的y坐标
z ; % 点的z坐标
%% -------------------------------拟合平面---------------------------------
% 矩阵M
M = [sumXX sumXY sumX;
	sumXY sumYY sumY;
	sumX sumY n];
% 矩阵N
N = [sumXZ sumYZ sumZ]';
% 求解
X = pinv(M)*N;
a = X(1);b = X(2);c = X(3);
%% ---------------------------可视化拟合结果-------------------------------
 figure
% 图形绘制
scatter3(x,y,z,'filled')
hold on;
[XFit,YFit]= meshgrid (xfit,yfit);
ZFit = a * XFit + b * YFit + c;
mesh(XFit,YFit,ZFit);
title('最小二乘拟合平面');

3. Algorithm effect