python 最小二乘平面拟合

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 = [], [], []
        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

def cal_v0(p3, p4):
    l0 = p4[0]-p3[0]
    m0 = p4[1]-p3[1]
    n0 = p4[2]-p3[2]
    return l0, m0, n0

def cal_center(xlist, ylist, zlist):
    xmin = min(xlist)
    xmax = max(xlist)
    ymin = min(ylist)
    ymax = max(ylist)
    zmin = min(zlist)
    zmax = max(zlist)

    dx = xmax - xmin
    dy = ymax - ymin
    dz = zmax - zmin

    print('xmin:', xmin)
    print('dx/2:', dx/2)
    cx = xmin + dx/2
    cy = ymin + dy/2
    cz = zmin + dz/2
    center_p = [cx, cy, cz]
    return center_p

def plane_fitting(x2, y2, z2, p1, p2):

    if len(x2)==len(y2) and len(x2)==len(z2):
        point_num = len(x2)
        # print('points number:', point_num)
        # 点云外接bounding box中心
        center_p = cal_center(x2, y2, z2) #[-11.991357326507568, 5.537035226821899, -69.77203750610352]

        #路牌面和路面的法向量垂直
        # p1, p1向量垂直于路面 
        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)

        #计算方差
        print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(A,B,C))
        R=0
        for i in range(0,point_num):
            R=R+(A * x2[i] + B * y2[i] + C - z2[i])**2
        fitting_variance = R
        print ('方差为：%.*f'%(3,R))

    return A, B, C, R

def show_plane(x2, y2, z2, A,B,C):                    
    # 展示图像
    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)
    x_p, y_p = np.meshgrid(x_p, y_p)
    z_p = A * x_p + B* y_p + C
    ax1.plot_wireframe(x_p, y_p, z_p, rstride=1, cstride=1)

    print('validation...')
    plt.show()

def displayPoint(data):
    #解决中文显示问题
    # plt.rcParams['font.sans-serif']=['SimHei']
    # plt.rcParams['axes.unicode_minus'] = False
 
    #散点图参数设置
    fig=plt.figure() 
    ax=Axes3D(fig) 
    ax.set_title(' ') 
    ax.scatter3D(data[0], data[1],data[2], c = 'r', marker = '.') 
    ax.set_xlabel('x') 
    ax.set_ylabel('y') 
    ax.set_zlabel('z') 
    plt.show()

if __name__ == '__main__':
    # read 3D points from txt file.
    data = []
    x2, y2, z2 = readTxt('./3D-Point-result/1/group_all_3Dposition1_inliers.txt')
    data.append(x2)
    data.append(y2)
    data.append(z2)
    displayPoint(data)

    #路牌面和路面的法向量垂直
    # p1, p1向量垂直于路面 
    p1 = [10.0002, -16.6615, 53.4859]
    p2 = [10.0384, -16.5292, 53.6389]
    #拟合平面z=Ax+By+C, 拟合结果方差R
    A,B,C,R = plane_fitting(x2, y2, z2, p1, p2) 
    show_plane(x2, y2, z2, A, B, C)

    # show_plane_bbox(x2, y2, z2, A, B, C, xmin, ymin, zmin, dx, dy, dz, color='green')