第十六课程Open3D点云数据处理:最小二乘-平芜编程栈

最小二乘直线拟合（二维）

最小二乘直线拟合代码实现

点云最小二乘直线拟合

最小二乘直线拟合（矩阵方程法）

最小二乘直线拟合代码实现

点云最小二乘直线拟合

最小二乘直线拟合（三维）

代码实现

计算拟合的评估指标

最小二乘多项式拟合

最小二乘多项式拟合（手写实现）

参考文献

最小二乘直线拟合（二维）

最小二乘直线拟合代码实现

import numpy as np import matplotlib.pyplot as plt """ @describe: 最小二乘直线拟合 @param[I]: x, x坐标 @param[I]: y, y坐标 @return: lope, 斜率 @return: intercept, 截距 """ def least_squares_fit_line(x, y): n = len(x) # 样本数 sum_x = np.sum(x) # x的总和 sum_y = np.sum(y) # y的总和 sum_x2 = np.sum(x**2) # x的平方和 sum_xy = np.sum(x*y) # xy的总和 # 使用公式计算斜率和截距 slope = (n*sum_xy - sum_x*sum_y) / (n*sum_x2 - sum_x**2) intercept = (sum_y - slope*sum_x) / n return slope, intercept if __name__ == "__main__": # 生成样本数据 x = np.array([1, 2, 3, 4, 5]) y = np.array([2, 3, 4, 5, 6]) # 调用函数计算斜率和截距 slope, intercept = least_squares_fit_line(x, y) # 使用斜率和截距绘制拟合直线 x_fit = np.linspace(0, 6, 100) # 生成x轴数据 y_fit = slope*x_fit + intercept # 计算y轴数据 plt.plot(x_fit, y_fit, '-r', label='fit line') # 绘制样本散点图 plt.scatter(x, y) # 添加标题和坐标轴标签 plt.title('Least Squares Fit Line') plt.xlabel('x') plt.ylabel('y') # 显示图例 plt.legend() # plot时要设置label属性才会显示图例 # 显示拟合方程, 前两个参数为方程左上角坐标(x,y) plt.text(0, 6, 'function: y=%.2fx + %.2f' % (slope, intercept), fontsize=12, color='r', verticalalignment="top") # 显示图形 plt.show()

点云最小二乘直线拟合

import numpy as np import matplotlib.pyplot as plt import open3d as o3d # 用于显示图形中的中文，否则中文会乱码 plt.rcParams['font.family'] = ['sans-serif'] plt.rcParams['font.sans-serif'] = ['SimHei'] """ @describe: 最小二乘直线拟合 @param[I]: x, x坐标 @param[I]: y, y坐标 @return: lope, 斜率 @return: intercept, 截距 """ def least_squares_fit_line(x, y): n = len(x) # 样本数 sum_x = np.sum(x) # x的总和 sum_y = np.sum(y) # y的总和 sum_x2 = np.sum(x**2) # x的平方和 sum_xy = np.sum(x*y) # xy的总和 # 使用公式计算斜率和截距 slope = (n*sum_xy - sum_x*sum_y) / (n*sum_x2 - sum_x**2) intercept = (sum_y - slope*sum_x) / n return slope, intercept if __name__ == "__main__": # 读取直线点云 pcd = o3d.io.read_point_cloud("data/fit/straightLine.pcd") # 从点云中提取numpy数组及xyz坐标 points = np.asarray(pcd.points) x = points[:, 0] y = points[:, 1] z = points[:, 2] # 调用函数计算斜率和截距 slope, intercept = least_squares_fit_line(x, y) # 使用斜率和截距绘制拟合直线 x_fit = x # 生成x轴数据 y_fit = slope*x_fit + intercept # 计算y轴数据 plt.plot(x_fit, y_fit, '-r', label='拟合直线') # 绘制样本散点图 plt.scatter(x, y) # 添加标题和坐标轴标签 plt.title('点云最小二乘直线拟合') plt.xlabel('x') plt.ylabel('y') # 显示图例 plt.legend() # plot时要设置label属性才会显示图例 # 显示拟合方程, 前两个参数为方程左上角坐标(x,y) plt.text(4, 12, '拟合方程: y=%.2fx + %.2f' % (slope, intercept), fontsize=12, color='r', verticalalignment="top") # 显示图形 plt.show()

最小二乘直线拟合（矩阵方程法）

最小二乘直线拟合代码实现

import numpy as np import matplotlib.pyplot as plt # 用于显示中文，否则中文会乱码 plt.rcParams['font.family'] = ['sans-serif'] plt.rcParams['font.sans-serif'] = ['SimHei'] # 生成随机数据 np.random.seed(0) # 设置随机种子，确保每次运行结果相同 x = np.linspace(0, 10, 100) # 生成100个从0到10的均匀分布的随机数 y = 2 * x + 1 + np.random.normal(size=100) # 使用线性方程y = 2x + 1加上一些噪声生成y值，size参数指定生成噪声的个数为100 # 绘制随机数据散点图 plt.scatter(x, y) # 使用最小二乘法拟合直线 A = np.vstack([x, np.ones(len(x))]).T # 构造矩阵A，A的第一列是x，第二列是1 k, b = np.linalg.lstsq(A, y, rcond=None)[0] # 使用最小二乘法求解k和b，rcond=None表示不进行奇异值分解，直接求解 # 求解的k和b即为直线的斜率和截距 # 绘制拟合直线 plt.plot(x, k * x + b, 'r', label='拟合直线') # y = kx + b # 显示图例和拟合方程 plt.legend() plt.title("最小二乘直线拟合") plt.xlabel('x') plt.ylabel('y') plt.text(1, 20, '拟合方程: y=%.2fx+%.2f' % (k, b), fontsize=12, color='r', verticalalignment="top") # 可视化拟合结果 plt.show()

点云最小二乘直线拟合

import numpy as np import matplotlib.pyplot as plt import open3d as o3d # 用于显示中文，否则中文会乱码 plt.rcParams['font.family'] = ['sans-serif'] plt.rcParams['font.sans-serif'] = ['SimHei'] # 读取直线点云 pcd = o3d.io.read_point_cloud("data/fit/straightLine.pcd") # 从点云中提取numpy数组及xyz坐标 points = np.asarray(pcd.points) x = points[:, 0] y = points[:, 1] z = points[:, 2] # 绘制散点图 plt.scatter(x, y) # 使用最小二乘法拟合直线 A = np.vstack([x, np.ones(len(x))]).T # 构造矩阵A，A的第一列是x，第二列是1 k, b = np.linalg.lstsq(A, y, rcond=None)[0] # 使用最小二乘法求解k和b，rcond=None表示不进行奇异值分解，直接求解 # 求解的k和b即为直线的斜率和截距 # 绘制拟合直线 plt.plot(x, k * x + b, 'r', label='拟合直线') # y = kx + b # 显示图例和拟合方程 plt.legend() plt.title("最小二乘直线拟合") plt.xlabel('x') plt.ylabel('y') plt.text(4, 14, '拟合方程: y=%.2fx+%.2f' % (k, b), fontsize=12, color='r', verticalalignment="top") # 可视化拟合结果 plt.show()

最小二乘直线拟合（三维）

代码实现

import numpy as np import open3d as o3d pcd = o3d.io.read_point_cloud("data/fit/straightLine.pcd") # 从点云中提取numpy数组及xyz坐标 points = np.asarray(pcd.points) x = points[:, 0] y = points[:, 1] z = points[:, 2] # 构造A矩阵 A = np.vstack((x, y, np.ones_like(x))).T # 计算最小二乘解 w = np.linalg.lstsq(A, z, rcond=None)[0] # 构造拟合直线的xyz坐标 x_fit = np.array([x.min(), x.max()]) y_fit = np.array([y.min(), y.max()]) z_fit = x_fit*w[0] + y_fit*w[1] + w[2] # 打印拟合方程 print("z = ",w[0],'x + ',w[1],"y + ",w[2]) # 可视化结果 line_set = o3d.geometry.LineSet()# 创建一条线集合 line_set.points = o3d.utility.Vector3dVector(np.column_stack((x_fit, y_fit, z_fit))) line_set.lines = o3d.utility.Vector2iVector(np.array([[0, 1]])) o3d.visualization.draw_geometries([pcd, line_set])

计算拟合的评估指标

import numpy as np import open3d as o3d # 读取待拟合点云 pcd = o3d.io.read_point_cloud("data/fit/line.pcd") # 从点云中提取numpy数组及xyz坐标 points = np.asarray(pcd.points) x = points[:, 0] y = points[:, 1] z = points[:, 2] # 构造A矩阵 A = np.vstack((x, y, np.ones_like(x))).T # 计算最小二乘解 X, RSS, rank, s = np.linalg.lstsq(A, z, rcond=None) # 构造拟合直线的xyz坐标 x_fit = np.array([x.min(), x.max()]) y_fit = np.array([y.min(), y.max()]) z_fit = x_fit*X[0] + y_fit*X[1] + X[2] # 打印拟合方程 print("z = ",X[0],'x + ',X[1],"y + ",X[2]) # 打印各类指标 print("残差平方和：{:.2f}".format(RSS[0]))#RSS是一个形如[0.1234] 的数组，故而取第一个元素。 print("均方误差：{:.2f}".format(RSS[0] / len(z))) print("均方根误差：{:.2f}".format(np.sqrt(RSS[0] / len(z)))) # 拟合后的预测值 z_pred = np.dot(A, X) # z均值 z_mean = np.mean(z) # 残差平方和 SS_res = np.sum((z-z_pred)**2) # 总平方和 SS_tot = np.sum((z-z_mean)**2) # 平均绝对误差 MAE = np.mean(np.abs(z-z_pred)) # 决定系数 R2 = 1 - SS_res/SS_tot print("残差平方和（直接计算法）：{:.2f}".format(SS_res)) print("平均绝对误差：{:.2f}".format(MAE)) print("决定系数：{:.2f}".format(R2)) # 可视化结果 line_set = o3d.geometry.LineSet()# 创建一条线集合 line_set.points = o3d.utility.Vector3dVector(np.column_stack((x_fit, y_fit, z_fit))) line_set.lines = o3d.utility.Vector2iVector(np.array([[0, 1]])) o3d.visualization.draw_geometries([pcd, line_set])

最小二乘多项式拟合

import open3d as o3d import numpy as np import matplotlib.pyplot as plt """ @describe: 最小二乘多项式拟合 @param[I]: pcd, 待拟合点云 @param[I]: degree, 多项式阶数 @return: coeffs: 拟合系数 @return: RSS: 残差平方和 @return: TSS: 总平方和 """ def least_squares_polynomial_fit(pcd, degree): # 点云转numpy数组 points = np.asarray(pcd.points) # 提取点云坐标 x = points[:, 0] y = points[:, 1] z = points[:, 2] # 计算多项式系数 coeffs = np.polyfit(x, y, degree) # 拟合的点坐标 xfit = np.linspace(np.min(x), np.max(x), len(x)) yfit = np.polyval(coeffs, xfit) # 可视化结果 plt.plot(x, y, "o", label="original points") plt.plot(xfit, yfit, "-", label="fit curve") plt.legend() plt.show() # 计算拟合的残差，残差平方和 yfit = np.polyval(coeffs, x) # 残差 residuals = y - yfit # 残差平方和RSS RSS = np.sum(residuals**2) # 总平方和TSS TSS = np.sum((y - np.mean(y))**2) return coeffs, RSS, TSS if __name__ == "__main__": # 读取点云数据 pcd = o3d.io.read_point_cloud("data/fit/curve.pcd") # 多项式拟合 coeffs, RSS, TSS = least_squares_polynomial_fit(pcd, degree=3) # 将多项式系数转换为拟合方程 fit_equation = np.poly1d(coeffs) # 打印拟合方程 print('拟合方程：y = ', fit_equation) # 计算拟合指标 n=len(pcd.points) # 均方误差 MSE = RSS/n # 均方根误差 RMSE = np.sqrt(MSE) # 平均绝对误差 MAE = np.mean(np.abs(np.sqrt(RSS)))/n # 决定系数 R2 = 1 - (RSS / TSS) print('残差平方和：{:.3f}'.format(RSS)) print('均方差：{:.3f}'.format(MSE)) print('均方根误差：{:.3f}'.format(RMSE)) print('平均绝对误差：{:.3f}'.format(MAE)) print('决定系数：{:.3f}'.format(R2))

最小二乘多项式拟合（手写实现）

import numpy as np import matplotlib.pyplot as plt import open3d as o3d """ @describe: 最小二乘多项式拟合 @param[I]: pcd, 待拟合点云 @param[I]: k, 多项式阶数 @return: coeffs: 拟合系数 @return: rss: 残差平方和 @return: mse: 均方误差 @return: rmse: 均方根误差 @return: mae: 平均绝对误差 @return: r2: 残差平方和 """ def my_least_squares_polynomial_fit(pcd, k, evaluation=False): # 点云转numpy数组 points = np.asarray(pcd.points) # 提取点云坐标 x = points[:, 0] y = points[:, 1] z = points[:, 2] # 计算回归系数 n = len(x) X = np.zeros((n, k+1)) for i in range(n): for j in range(k+1): X[i][j] = x[i]**j X_T = X.transpose() y = y.reshape((-1, 1)) alpha = np.linalg.inv(X_T.dot(X)).dot(X_T).dot(y) # 使用 squeeze() 函数去掉第一个维度（形状为1的维度） coeffs = alpha.squeeze() # 输出拟合系数 print("拟合系数:", coeffs) # 构造拟合方程,系数保留两位小数 f = [] for i in range(k+1): if i == 0: f.append(f"{round(coeffs[i],4)}") else: f.append(f"{round(coeffs[i],4)}x^{i}") print("拟合方程:y = ", " + ".join(f)) # 绘制拟合曲线 y_fit = X.dot(alpha) plt.scatter(x, y) plt.plot(x, y_fit, color='r') plt.show() # 计算拟合评估指标 if(evaluation): rss = np.sum((y_fit - y)**2) print("残差平方和:", rss) mse = rss / n print("均方误差:", mse) rmse = np.sqrt(mse) print("均方根误差:", rmse) mae = np.mean(np.abs(y_fit - y)) print("平均绝对误差:", mae) y_mean = np.mean(y) # 计算实际值的均值 ss_res = np.sum((y - y_fit)**2) # 计算残差平方和 ss_tot = np.sum((y - y_mean)**2) # 计算总平方和 r2 = 1 - (ss_res / ss_tot) # 计算R^2 print("决定系数（R^2）:", r2) return coeffs, rss, mse, rmse, mae, r2 if __name__ == "__main__": # 读取点云数据 pcd = o3d.io.read_point_cloud("data/fit/curve.pcd") # 多项式拟合 coeffs, rss, mse, rmse, mae, r2 = my_least_squares_polynomial_fit(pcd, k=3, evaluation=True)