点云拟合平面-最小二乘法

参考：https://blog.csdn.net/qq_45427038/article/details/100139330

1. 普通的最小二乘拟合平面

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import math

# 从txt文档读取点
def readTxt(textfile):
    with open(textfile, 'r') as f:
        x2, y2, z2 = [], [], []
        print(x2)
        for line in f.readlines():
            line.replace('\n', '')
            x2.append(float(line.split(' ')[0]))
            y2.append(float(line.split(' ')[1]))
            z2.append(float(line.split(' ')[2]))
    f.close()
    return x2, y2, z2
 
# read 3D points from txt file.
x2, y2, z2 = readTxt('./3D-Point-result/1/all_s1_inliers.txt')
if len(x2)==len(y2) and len(x2)==len(z2):
    point_num = len(x2)
    print('points number:', point_num)

    #创建系数矩阵A
    A=np.zeros((3,3))
    for i in range(0,point_num):
        A[0,0]=A[0,0]+x2[i]**2
        A[0,1]=A[0,1]+x2[i]*y2[i]
        A[0,2]=A[0,2]+x2[i]
        A[1,0]=A[0,1]
        A[1,1]=A[1,1]+y2[i]**2
        A[1,2]=A[1,2]+y2[i]
        A[2, 0] = A[0,2]
        A[2, 1] = A[1, 2]
        A[2, 2] = point_num
     
    #创建b
    b = np.zeros((3,1))
    for i in range(0,point_num):
        b[0,0]=b[0,0]+x2[i]*z2[i]
        b[1,0]=b[1,0]+y2[i]*z2[i]
        b[2,0]=b[2,0]+z2[i]
     
    #求解X
    A_inv=np.linalg.inv(A)
    X = np.dot(A_inv, b)
    print(X[0,0],X[1,0],X[2,0])

    print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(X[0,0],X[1,0],X[2,0]))
    # Ax+By+Cz+D=0，则其法向量为(A/√(A²+B²+C²),B/√(A²+B²+C²),C/√(A²+B²+C²))

    A = float('%.3f' % X[0,0])
    B = float('%.3f' % X[1,0])
    C = float(-1)
    # D = '%.3f' % X[2,0]
    # print(A,B,C,D)
    normal_v = (100*A/math.sqrt(A*A + B*B + C*C), 100*B/math.sqrt(A*A + B*B + C*C), 100*C/math.sqrt(A*A + B*B + C*C))   
    print('normal_v:', normal_v) 
    #计算方差
    R=0
    for i in range(0,point_num):
        R=R+(X[0, 0] * x2[i] + X[1, 0] * y2[i] + X[2, 0] - z2[i])**2
    print ('方差为：%.*f'%(3,R))

    # 展示图像
    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111, projection='3d')
    ax1.set_xlabel("x")
    ax1.set_ylabel("y")
    ax1.set_zlabel("z")
    ax1.scatter3D(x2,y2,z2,c='r',marker='.')
    x_p = np.linspace(min(x2), max(x2), 5) #从min(x2), max(x2)按等差数列生成10个数

    y_p = np.linspace(min(y2), max(y2), 5)
    # print('x_p, y_p:', x_p)
    x_p, y_p = np.meshgrid(x_p, y_p)

    print('x_p:', x_p)
    print('---------')
    print('y_p:', y_p)

    z_p = X[0, 0] * x_p + X[1, 0] * y_p + X[2, 0]
    print('---------')
    print('z_p:' ,z_p)
    ax1.plot_wireframe(x_p, y_p, z_p, rstride=1, cstride=1)
    # -12.13989258, 5.4409194, -69.69098588
    # -11.84282207, 5.4409194, -69.69098588
    # -12.13989258, 5.63315105,-69.84325345
    # -11.84282207, 5.63315105,-69.84325345
    plt.show()

3. 在一定条件下，最小二乘拟合平面。这里是确定，拟合的平面与某一已知平面垂直。即两个平面的法向量相垂直。

原本待的参数消除一个，只求2个。

代码：

if case_flag == 1: #路牌面和路面的法向量垂直
        # p1, p1向量垂直于路面
        p1 = [-11.8873, 5.5655, -70.0447]
        p2 = [-11.6893, 5.6936, -70.5120]
        l0, m0, n0= cal_v0(p1,p2) #垂直于路面的法向量
        # 设平面方程为ax+by+c=z --> 平面的法向量为（a,b,-1）

        #创建系数矩阵A
        A=np.zeros((2,2))
        for i in range(0,point_num):
            A[0,0]=A[0,0] + (x2[i] - float(l0/m0) *y2[i])**2
            A[0,1]=A[0,1] + x2[i] - float(l0/m0) *y2[i]
            A[1,0]=A[0,1] + x2[i] - float(l0/m0) *y2[i]
            A[1,1]=point_num
        #创建b
        b = np.zeros((2,1))
        for i in range(0,point_num):
            b[0,0]=b[0,0]+(z2[i]-float(n0/m0)*y2[i])*(x2[i] - float(l0/m0)*y2[i])
            b[1,0]=b[1,0]+z2[i]-float(n0/m0)*y2[i]   
        #求解X [a,c] b=b0+b1*a
        A_inv=np.linalg.inv(A)
        X = np.dot(A_inv, b)
        print(X[0,0],X[1,0])
        # print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(X[0,0],X[1,0],X[2,0]))
        # Ax+By+Cz+D=0，则其法向量为(A/√(A²+B²+C²),B/√(A²+B²+C²),C/√(A²+B²+C²))
        A = float('%.3f' % X[0,0])
        C = float('%.3f' % X[1,0])
        B = float((n0 - l0*A)/m0)

推导：