python如何拟合三维平面（拟合Intel Realsense D435深度数据点）

参考文章：【MQ笔记】超简单的最小二乘法拟合平面（Python）

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
 
# 创建函数，用于生成不同属于一个平面的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
a = 0
A = np.ones((100, 3))
for i in range(0, 100):
    A[i, 0] = x[a]
    A[i, 1] = y[a]
    a = a + 1
#print(A)
 
#创建矩阵b
b = np.zeros((100, 1))
a = 0
for i in range(0, 100):
    b[i, 0] = z[a]
    a = a + 1
#print(b)
 
#通过X=(AT*A)-1*AT*b直接求解
A_T = A.T
A1 = np.dot(A_T,A)
A2 = np.linalg.inv(A1)
A3 = np.dot(A2,A_T)
X= np.dot(A3, b)
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(X[0,0],X[1,0],X[2,0]))
 
#计算方差
R=0
for i in range(0,100):
    R=R+(X[0, 0] * x[i] + X[1, 0] * y[i] + X[2, 0] - z[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.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, 0] * x_p + X[1, 0] * y_p + X[2, 0]
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
 
# 创建函数，用于生成不同属于一个平面的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
 
 
# 调用函数，生成离散点
x2, y2, z2 = not_all_in_plane(2, 5, 6)
 
#创建系数矩阵A
A=np.zeros((3,3))
for i in range(0,100):
    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] = 100
#print(A)
 
#创建b
b = np.zeros((3,1))
for i in range(0,100):
    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]
#print(b)
 
#求解X
A_inv=np.linalg.inv(A)
X = np.dot(A_inv, b)
print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f'%(X[0,0],X[1,0],X[2,0]))
 
#计算方差
R=0
for i in range(0,100):
    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.scatter(x2,y2,z2,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, 0] * x_p + X[1, 0] * y_p + X[2, 0]
ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
plt.show()

拟合Intel Realsense D435深度数据点

# -*- coding: utf-8 -*-
"""
@File    : 摄像头精度测试.py
@Time    : 2020/9/7 10:49
@Author  : Dontla
@Email   : [email protected]
@Software: PyCharm
"""
import datetime
import time
from datetime import date

import pyrealsense2 as rs
import cv2 as cv
import numpy as np

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

from numba import jit


def fit_flat(x, y, z):
    # 取样点数量
    point_num = len(x)
    print(point_num)
    # 创建系数矩阵A
    a = 0
    A = np.ones((point_num, 3))
    for i in range(0, point_num):
        A[i, 0] = x[a]
        A[i, 1] = y[a]
        a = a + 1
    # print(A)

    # 创建矩阵b
    b = np.zeros((point_num, 1))
    a = 0
    for i in range(0, point_num):
        b[i, 0] = z[a]
        a = a + 1
    # print(b)

    # 通过X=(AT*A)-1*AT*b直接求解
    A_T = A.T
    A1 = np.dot(A_T, A)
    A2 = np.linalg.inv(A1)
    A3 = np.dot(A2, A_T)
    X = np.dot(A3, b)
    print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (X[0, 0], X[1, 0], X[2, 0]))

    # 计算方差
    R = 0
    for i in range(0, point_num):
        R = R + (X[0, 0] * x[i] + X[1, 0] * y[i] + X[2, 0] - z[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.scatter(x, y, z, c='r', marker='o')
    x_p = np.linspace(0, 1500, 150)
    y_p = np.linspace(0, 1500, 150)
    x_p, y_p = np.meshgrid(x_p, y_p)
    z_p = X[0, 0] * x_p + X[1, 0] * y_p + X[2, 0]
    ax1.plot_wireframe(x_p, y_p, z_p, rstride=10, cstride=10)
    plt.show()


def run_cam():
    ctx = rs.context()
    pipeline = rs.pipeline(ctx)
    cfg = rs.config()
    cfg.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
    cfg.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
    profile = pipeline.start(cfg)
    try:
        count = 0
        while True:
            fs = pipeline.wait_for_frames()

            color_frame = fs.get_color_frame()
            depth_frame = fs.get_depth_frame()
            # print(type(depth_frame))    # 

            if not depth_frame or not color_frame:
                continue

            # color_image = np.asanyarray(color_frame.get_data())
            depth_image = np.asanyarray(depth_frame.get_data())
            # print(type(depth_image))    # 

            x, y, z = [], [], []
            for i in range(1280):
                for j in range(720):
                    # if depth_image[j, i] == 0:
                    # 发现绘制的点中有很多深度几万到六万多不等的，把它们过滤掉
                    point_depth = np.asanyarray(depth_frame.get_data())[j, i]
                    if point_depth == 0 or point_depth >= 2000:
                        continue
                    else:
                        x.append(i)
                        y.append(j)
                        z.append(point_depth)
            # 将数据写入numpy文件
            count += 1
            np.savez('{}_{}'.format(count, time.time()), x=x, y=y, z=z)
            fit_flat(x, y, z)

    finally:
        pipeline.stop()


# print('hh')
if __name__ == '__main__':
    print('hh')
    run_cam()

结果：