12. RANSAC点云多平面拟合分割

        本文看了博客RANSAC点云多平面拟合分割-CSDN博客的文章，该博客将多平面拟合分割讲的很详细了，这里只是作为笔记用。

1.RANSAC算法原理

三维平面拟合（最小二乘法）

具体实现见下面代码：

def SVD(points):# 二维，三维均适用# 二维直线，三维平面pts = points.copy()# 奇异值分解c = np.mean(pts, axis=0)A = pts - c # shift the pointsA = A.T #3*nu, s, vh = np.linalg.svd(A, full_matrices=False, compute_uv=True) # A=u*s*vhnormal = u[:,-1]# 法向量归一化nlen = np.sqrt(np.dot(normal,normal))normal = normal / nlen# normal 是主方向的方向向量 与PCA最小特征值对应的特征向量是垂直关系# u 每一列是一个方向# s 是对应的特征值# c >>> 点的中心# normal >>> 拟合的方向向量return u,s,c,normalclass plane_model(object):def __init__(self):平面模型参数（平面上任意一点 cx, cy, cx） + （平面法向量 nx, ny, nz）self.parameters = Nonedef calc_inliers(self,points,dst_threshold):# 根据点到平面的距离计算内点和外点c = self.parameters[0:3]n = self.parameters[3:6]dst = abs(np.dot(points-c,n))ind = dst<dst_thresholdreturn inddef estimate_parameters(self,pts):# 最小二乘法估算平面模型# 只有三个点时，可以直接计算num = pts.shape[0]if num == 3:c = np.mean(pts,axis=0)l1 = pts[1]-pts[0]l2 = pts[2]-pts[0]n = np.cross(l1,l2)scale = [n[i]**2 for i in range(n.shape[0])]#print(scale)n = n/np.sqrt(np.sum(scale))else:_,_,c,n = SVD(pts)params = np.hstack((c.reshape(1,-1),n.reshape(1,-1)))[0,:]self.parameters = paramsreturn paramsdef set_parameters(self,parameters):self.parameters = parameters

RANSAC算法

参考：​​​​​​RANSAC介绍（Matlab版直线拟合+平面拟合）_sylvester的博客-CSDN博客_matlab ransac

RANSAC是一种算法，一种思想，不仅仅可以用于拟合平面，实际上还有很多用处。

这里的算法如下算法

具体实现见下面代码：

# 注意这里并没有根据内点比例和模型可靠的概率动态调整最大迭代次数
def ransac_planefit(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None):# RANSAC 平面拟合pts = points.copy()num = pts.shape[0]cc = np.mean(pts,axis=0)iter_max = max_trialsbest_inliers_ratio = 0 #符合拟合模型的数据的比例best_plane_params = Nonebest_inliers = Nonebest_remains = Nonefor i in range(iter_max):sample_index = random.sample(range(num),ransac_n)sample_points = pts[sample_index,:]plane = plane_model()plane_params = plane.estimate_parameters(sample_points)#  计算内点 外点index = plane.calc_inliers(points,max_dst)inliers_ratio = pts[index].shape[0]/numif inliers_ratio > best_inliers_ratio:best_inliers_ratio = inliers_ratiobest_plane_params = plane_paramsbset_inliers = pts[index]bset_remains = pts[index==False]if best_inliers_ratio > stop_inliers_ratio:# 检查是否达到最大的比例print("iter: %d\n" % i)print("best_inliers_ratio: %f\n" % best_inliers_ratio)breakreturn best_plane_params,bset_inliers,bset_remains

多平面拟合

针对一个三维点云，进行多平面拟合的基本思想是：首先使用RANSAC平面拟合算法从点云中提取出一个平面，对于剩下的外点，继续循环RANSAC平面拟合算法提取平面，直到提取出所有的平面，循环时注意循环停止条件的设置

具体实现见下面代码：

def ransac_plane_detection(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None, out_layer_inliers_threshold=230, out_layer_remains_threshold=230):# out_layer_inliers_threshold --> 每一次RANSAC提取出来的平面最少含有的内点数量# out_layer_remains_threshold --> 每一次RANSAC提取平面后剩余外点的最少数量，少于这个值，则停止循环inliers_num = out_layer_inliers_threshold + 1remains_num = out_layer_remains_threshold + 1plane_set = []plane_inliers_set = []plane_inliers_num_set = []data_remains = np.copy(points)i = 0while inliers_num>out_layer_inliers_threshold and remains_num>out_layer_remains_threshold:# robustly fit line only using inlier data with RANSAC algorithmbest_plane_params,pts_inliers,pts_outliers = ransac_planefit(data_remains, ransac_n, max_dst, max_trials=max_trials, stop_inliers_ratio=stop_inliers_ratio)inliers_num = pts_inliers.shape[0]remains_num = pts_outliers.shape[0]if inliers_num>out_layer_inliers_threshold:plane_set.append(best_plane_params)plane_inliers_set.append(pts_inliers)plane_inliers_num_set.append(inliers_num)i = i+1print('------------> %d <--------------' % i)print(best_plane_params)data_remains = pts_outliers# sortingplane_set = [x for _, x in sorted(zip(plane_inliers_num_set,plane_set), key=lambda s: s[0], reverse=True)]plane_inliers_set = [x for _, x in sorted(zip(plane_inliers_num_set,plane_inliers_set), key=lambda s: s[0], reverse=True)]return plane_set, plane_inliers_set, data_remains

案例

点云

这里给出一个立方体的三维点云，如下图所示，资源可以从下面下载:

链接: https://pan.baidu.com/s/1G2g6sYp46-FWnDX20uphvg?pwd=dzbn

提取码: dzbn

拟合结果

6个平面

红色边框是6个平面的参数，即 （cx,cy,cz,nx,ny,nz)

绘图显示

不同颜色代表不同的平面

2.三维点云的全部代码

包含一些读取txt和绘制三维点的代码，如下：

import os
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3Dimport math
import randomdef strlist2num(dl):#将字符串列表转化为浮点型列表data = []for i in range(len(dl)):if dl[i]=='nan'or dl[i]=='NaN':raise ValueError('data is nan')data.append(float(dl[i]))return np.array(data)def read_txt(path,row_skip=0,split_char=',',num_range=None, verbose=False):"""read txt file into a np.ndarray.Input：------path: file pathrow_skip: skip the first rows to read datasplit_char: spliting characternum_range: data range of each numberOutput：------data: data read. data is np.array([]) when reading error happeneddata is np.array([]) when nan or NaN appearsdata is np.array([]) when any number is out of range"""try:f = open(path,'r',encoding='utf-8')line_list = f.readlines()read_lines_num = len(line_list)for i in range(read_lines_num):line_list[i] = line_list[i].rstrip()i = row_skip # 从第三行开始读取data = []while i <= read_lines_num-1:data_str = line_list[i].split(split_char)data.append(strlist2num(data_str))i = i + 1f.close()except:if verbose:print("type data of [{}] is wrong...".format(path))data = np.array([])f.close()data = np.array(data)if num_range is not None:if np.any(data<num_range[0]) or np.any(data>num_range[1]):data = np.array([])if verbose:print("data of [{}] is out of range...".format(path))return datadef SVD(points):# 二维，三维均适用# 二维直线，三维平面pts = points.copy()# 奇异值分解c = np.mean(pts, axis=0)A = pts - c # shift the pointsA = A.T #3*nu, s, vh = np.linalg.svd(A, full_matrices=False, compute_uv=True) # A=u*s*vhnormal = u[:,-1]# 法向量归一化nlen = np.sqrt(np.dot(normal,normal))normal = normal / nlen# normal 是主方向的方向向量 与PCA最小特征值对应的特征向量是垂直关系# u 每一列是一个方向# s 是对应的特征值# c >>> 点的中心# normal >>> 拟合的方向向量return u,s,c,normalclass plane_model(object):def __init__(self):self.parameters = Nonedef calc_inliers(self,points,dst_threshold):c = self.parameters[0:3]n = self.parameters[3:6]dst = abs(np.dot(points-c,n))ind = dst<dst_thresholdreturn inddef estimate_parameters(self,pts):num = pts.shape[0]if num == 3:c = np.mean(pts,axis=0)l1 = pts[1]-pts[0]l2 = pts[2]-pts[0]n = np.cross(l1,l2)scale = [n[i]**2 for i in range(n.shape[0])]#print(scale)n = n/np.sqrt(np.sum(scale))else:_,_,c,n = SVD(pts)params = np.hstack((c.reshape(1,-1),n.reshape(1,-1)))[0,:]self.parameters = paramsreturn paramsdef set_parameters(self,parameters):self.parameters = parametersdef ransac_planefit(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None):# RANSAC 平面拟合pts = points.copy()num = pts.shape[0]cc = np.mean(pts,axis=0)iter_max = max_trialsbest_inliers_ratio = 0 #符合拟合模型的数据的比例best_plane_params = Nonebest_inliers = Nonebest_remains = Nonefor i in range(iter_max):sample_index = random.sample(range(num),ransac_n)sample_points = pts[sample_index,:]plane = plane_model()plane_params = plane.estimate_parameters(sample_points)#  计算内点 外点index = plane.calc_inliers(points,max_dst)inliers_ratio = pts[index].shape[0]/numif inliers_ratio > best_inliers_ratio:best_inliers_ratio = inliers_ratiobest_plane_params = plane_paramsbset_inliers = pts[index]bset_remains = pts[index==False]if best_inliers_ratio > stop_inliers_ratio:# 检查是否达到最大的比例print("iter: %d\n" % i)print("best_inliers_ratio: %f\n" % best_inliers_ratio)breakreturn best_plane_params,bset_inliers,bset_remainsdef ransac_plane_detection(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None, out_layer_inliers_threshold=230, out_layer_remains_threshold=230):inliers_num = out_layer_inliers_threshold + 1remains_num = out_layer_remains_threshold + 1plane_set = []plane_inliers_set = []plane_inliers_num_set = []data_remains = np.copy(points)i = 0while inliers_num>out_layer_inliers_threshold and remains_num>out_layer_remains_threshold:# robustly fit line only using inlier data with RANSAC algorithmbest_plane_params,pts_inliers,pts_outliers = ransac_planefit(data_remains, ransac_n, max_dst, max_trials=max_trials, stop_inliers_ratio=stop_inliers_ratio)inliers_num = pts_inliers.shape[0]remains_num = pts_outliers.shape[0]if inliers_num>out_layer_inliers_threshold:plane_set.append(best_plane_params)plane_inliers_set.append(pts_inliers)plane_inliers_num_set.append(inliers_num)i = i+1print('------------> %d <--------------' % i)print(best_plane_params)data_remains = pts_outliers# sortingplane_set = [x for _, x in sorted(zip(plane_inliers_num_set,plane_set), key=lambda s: s[0], reverse=True)]plane_inliers_set = [x for _, x in sorted(zip(plane_inliers_num_set,plane_inliers_set), key=lambda s: s[0], reverse=True)]return plane_set, plane_inliers_set, data_remainsdef show_3dpoints(pointcluster,s=None,colors=None,quiver=None,q_length=10,tri_face_index=None):# pointcluster should be a list of numpy ndarray# This functions would show a list of pint cloud in different colorsn = len(pointcluster)if colors is None:colors = ['r','g','b','c','m','y','k','tomato','gold']if n < 10:colors = np.array(colors[0:n])else: colors = np.random.rand(n,3)fig = plt.figure()ax = fig.add_subplot(projection='3d')if s is None:s = np.ones(n)*2for i in range(n):ax.scatter(pointcluster[i][:,0],pointcluster[i][:,1],pointcluster[i][:,2],s=s[i],c=[colors[i]],alpha=0.6)if not (quiver is None):c1 = [random.random() for _ in range(len(quiver))]c2 = [random.random() for _ in range(len(quiver))]c3 = [random.random() for _ in range(len(quiver))]c = []for i in range(len(quiver)):c.append((c1[i],c2[i],c3[i]))cp = []for i in range(len(quiver)):cp.append(c[i])cp.append(c[i])c = c + cpax.quiver(quiver[:,0],quiver[:,1],quiver[:,2],quiver[:,3],quiver[:,4],quiver[:,5],length=q_length,arrow_length_ratio=.2,pivot='tail',normalize=False,color=c)if not (tri_face_index is None):for i in range(len(tri_face_index)):for j in range(tri_face_index[i].shape[0]):index = tri_face_index[i][j].tolist()index = index + [index[0]]ax.plot(*zip(*pointcluster[i][index]))ax.set_xlabel('x')ax.set_ylabel('y')ax.set_zlabel('z')#ax.set_ylim([-20,60])plt.show()return 0if __name__ == "__main__":path = "./files/multiplane_detection/cubic15.txt"pcd = read_txt(path, row_skip=1, split_char=' ')pcd = pcd[:,:3]plane_set, plane_inliers_set, data_remains = ransac_plane_detection(pcd, 3, 5, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None,out_layer_inliers_threshold=230, out_layer_remains_threshold=230)plane_set = np.array(plane_set)print("================= 平面参数 ====================")print(plane_set)# 绘图show_3dpoints(plane_inliers_set)print("over!!!")

3.二维点的数据拟合直线的全部代码

代码如下：

import os
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3Dimport math
import random# 1.获取二维数据
def ply_txt(files):outs = []with open(files, 'r') as file:for line in file:ss = line.strip().split(" ")outs.append([float(ss[2]),float(ss[0])])return outs#2. svd求法向量
def SVD(points):# 二维，三维均适用# 二维直线，三维平面pts = points.copy()# 奇异值分解c = np.mean(pts, axis=0)A = pts - c  # shift the pointsA = A.T  # 3*nu, s, vh = np.linalg.svd(A, full_matrices=False, compute_uv=True)  # A=u*s*vhnormal = u[:, -1]# 法向量归一化nlen = np.sqrt(np.dot(normal, normal))normal = normal / nlen# normal 是主方向的方向向量 与PCA最小特征值对应的特征向量是垂直关系# u 每一列是一个方向# s 是对应的特征值# c >>> 点的中心# normal >>> 拟合的方向向量return u, s, c, normalclass plane_model(object):def __init__(self):self.parameters = Nonedef calc_inliers(self, points, dst_threshold):c = self.parameters[0:2]n = self.parameters[2:4]dst = abs(np.dot(points - c, n))ind = dst < dst_thresholdreturn inddef estimate_parameters(self, pts):num = pts.shape[0]# print("sssssssssss:   ",pts)if num == 3:c = np.mean(pts, axis=0)l1 = pts[1] - pts[0]l2 = pts[2] - pts[0]n = np.cross(l1, l2)scale = [n[i] ** 2 for i in range(n.shape[0])]# print(scale)n = n / np.sqrt(np.sum(scale))else:_, _, c, n = SVD(pts)params = np.hstack((c.reshape(1, -1), n.reshape(1, -1)))[0, :]self.parameters = paramsreturn paramsdef set_parameters(self, parameters):self.parameters = parametersdef ransac_planefit(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None):# RANSAC 平面拟合pts = points.copy()num = pts.shape[0]cc = np.mean(pts, axis=0)iter_max = max_trialsbest_inliers_ratio = 0  # 符合拟合模型的数据的比例best_plane_params = Nonebest_inliers = Nonebest_remains = Nonefor i in range(iter_max):sample_index = random.sample(range(num), ransac_n)sample_points = pts[sample_index, :]plane = plane_model()plane_params = plane.estimate_parameters(sample_points)#  计算内点 外点index = plane.calc_inliers(points, max_dst)inliers_ratio = pts[index].shape[0] / numif inliers_ratio > best_inliers_ratio:best_inliers_ratio = inliers_ratiobest_plane_params = plane_paramsbset_inliers = pts[index]bset_remains = pts[index == False]if best_inliers_ratio > stop_inliers_ratio:# 检查是否达到最大的比例print("iter: %d\n" % i)print("best_inliers_ratio: %f\n" % best_inliers_ratio)breakreturn best_plane_params, bset_inliers, bset_remainsdef ransac_plane_detection(points, ransac_n, max_dst, max_trials=1000, stop_inliers_ratio=1.0, initial_inliers=None,out_layer_inliers_threshold=230, out_layer_remains_threshold=230):inliers_num = out_layer_inliers_threshold + 1remains_num = out_layer_remains_threshold + 1plane_set = []plane_inliers_set = []plane_inliers_num_set = []data_remains = np.copy(points)i = 0while inliers_num > out_layer_inliers_threshold and remains_num > out_layer_remains_threshold:# robustly fit line only using inlier data with RANSAC algorithmbest_plane_params, pts_inliers, pts_outliers = ransac_planefit(data_remains, ransac_n, max_dst,max_trials=max_trials,stop_inliers_ratio=stop_inliers_ratio)inliers_num = pts_inliers.shape[0]remains_num = pts_outliers.shape[0]if inliers_num > out_layer_inliers_threshold:plane_set.append(best_plane_params)plane_inliers_set.append(pts_inliers)plane_inliers_num_set.append(inliers_num)i = i + 1print('------------> %d <--------------' % i)print(best_plane_params)data_remains = pts_outliers# sortingplane_set = [x for _, x in sorted(zip(plane_inliers_num_set, plane_set), key=lambda s: s[0], reverse=True)]plane_inliers_set = [x for _, x in sorted(zip(plane_inliers_num_set, plane_inliers_set), key=lambda s: s[0], reverse=True)]return plane_set, plane_inliers_set, data_remainsif __name__ == "__main__":# 1.获取水平距离的值files = "1.txt"points = ply_txt(files)# u, s, c, normal = SVD(points)# print("1.u: ", u)# print("2.s: ", s)# print("3.c: ", c)# print("4.normal: ", normal)plane_set, plane_inliers_set, data_remains = ransac_plane_detection(points, 2, 0.3, max_trials=1000,stop_inliers_ratio=1.0, initial_inliers=None,out_layer_inliers_threshold=15,out_layer_remains_threshold=15)plane_set = np.array(plane_set)# 画散点图colors = ["blue","red","green", "yellow"]x = []y = []for i in range(len(plane_inliers_set)):print(plane_inliers_set[i].shape)for item in plane_inliers_set[i]:plt.scatter(item[0], item[1], label=i, color=colors[i], linewidth=3.0)plt.show()print(len(plane_inliers_set))

        运行数据如下（注意：该数据是三维点云数据，在读取数据时将第二列数据去掉了，只保留了第一列和第三列数据，这样就成了二维数据）：

1317.8586 3.2500 161.5373
1316.9133 3.2500 161.3521
1315.9966 3.2500 161.1666
1315.4659 3.2500 160.9775
1314.1918 3.2500 160.7953
1313.3609 3.2500 160.6090
1312.5728 3.2500 160.4223
1312.2994 3.2500 160.2309
1311.1111 3.2500 160.0479
1310.6090 3.2500 159.8585
1310.3498 3.2500 159.6669
1309.9192 3.2500 159.4769
1309.5314 3.2500 159.2865
1309.3009 3.2500 159.0947
1309.1418 3.2500 158.9021
1308.8541 3.2500 158.7108
1308.2948 3.2500 158.5220
1307.9785 3.2500 158.3309
1308.0053 3.2500 158.1366
1307.9463 3.2500 157.9432
1307.8730 3.2500 157.7499
1307.7569 3.2500 157.5569
1307.4549 3.2500 157.3657
1307.3959 3.2500 157.1723
1307.3655 3.2500 156.9785
1307.3208 3.2500 156.7849
1307.0188 3.2500 156.5937
1306.6024 3.2500 156.4036
1306.2575 3.2500 156.2128
1306.0413 3.2500 156.0208
1305.9252 3.2500 155.8279
1305.9091 3.2500 155.6340
1306.1360 3.2500 155.4379
1306.1056 3.2500 155.2442
1306.0323 3.2500 155.0508
1305.8161 3.2500 154.8588
1305.6427 3.2500 154.6664
1305.5409 3.2500 154.4734
1305.5391 3.2500 154.2794
1305.4515 3.2500 154.0862
1305.2781 3.2500 153.8938
1305.4050 3.2500 153.6986
1305.3174 3.2500 153.5054
1305.2299 3.2500 153.3122
1304.9850 3.2500 153.1205
1304.7259 3.2500 152.9289
1304.5383 3.2500 152.7366
1304.5651 3.2500 152.5423
1304.7205 3.2500 152.3469
1304.9188 3.2500 152.1510
1304.7169 3.2500 151.9589
1304.4006 3.2500 151.7678
1304.2559 3.2500 151.5752
1304.1969 3.2500 151.3817
1304.1951 3.2500 151.1877
1304.2790 3.2500 150.9929
1304.2058 3.2500 150.7996
1304.0467 3.2500 150.6071
1303.9448 3.2500 150.4140
1303.8144 3.2500 150.2212
1303.8841 3.2500 150.0266
1304.0395 3.2500 149.8311
1304.1664 3.2500 149.6359
1304.3075 3.2500 149.4406
1304.1485 3.2500 149.2481
1304.1753 3.2500 149.0538
1304.1020 3.2500 148.8605
1304.0001 3.2500 148.6674
1303.9268 3.2500 148.4741
1304.0823 3.2500 148.2786
1304.1663 3.2500 148.0839
1303.9643 3.2500 147.8917
1303.8910 3.2500 147.6984
1303.8178 3.2500 147.5051
1303.5443 3.2500 147.3136
1303.4711 3.2500 147.1203
1303.1834 3.2500 146.9289
1303.1101 3.2500 146.7356
1302.9082 3.2500 146.5435
1302.7634 3.2500 146.3508
1302.7902 3.2500 146.1566
1303.5889 3.2500 145.9551
1303.8873 3.2500 145.7583
1304.0142 3.2500 145.5632
1303.9981 3.2500 145.3693
1303.7961 3.2500 145.1772
1303.6800 3.2500 144.9842
1303.2922 3.2500 144.7938
1303.1189 3.2500 144.6014
1303.0742 3.2500 144.4078
1303.3869 3.2500 144.2109
1303.5852 3.2500 144.0151
1303.4690 3.2500 143.8221
1303.1670 3.2500 143.6309
1302.9365 3.2500 143.4391
1302.8346 3.2500 143.2460
1302.9472 3.2500 143.0510
1302.7453 3.2500 142.8588
1302.5862 3.2500 142.6663
1302.4701 3.2500 142.4734
1302.5111 3.2500 142.2790
1303.0669 3.2500 142.0798
1303.4653 3.2500 141.8821
1303.5493 3.2500 141.6873
1303.4045 3.2500 141.4946
1303.5028 3.2500 141.2997
1303.5439 3.2500 141.1053
1303.3848 3.2500 140.9128
1303.1829 3.2500 140.7207
1303.0810 3.2500 140.5276
1303.0363 3.2500 140.3340
1303.0203 3.2500 140.1402
1302.8612 3.2500 139.9476
1302.7879 3.2500 139.7543
1302.9720 3.2500 139.5586
1302.9416 3.2500 139.3649
1302.7539 3.2500 139.1726
1302.7521 3.2500 138.9786
1303.5080 3.2500 138.7776
1303.5491 3.2500 138.5832
1303.4043 3.2500 138.3905
1303.1595 3.2500 138.1988
1302.6288 3.2500 138.0097
1301.9551 3.2500 137.8219
1302.0962 3.2500 137.6266
1302.3089 3.2500 137.4306
1302.8217 3.2500 137.2319
1302.9628 3.2500 137.0366
1303.0611 3.2500 136.8416
1303.0879 3.2500 136.6474
1303.2433 3.2500 136.4519
1303.3559 3.2500 136.2569
1303.2111 3.2500 136.0642
1302.9377 3.2500 135.8727
1302.8359 3.2500 135.6797
1302.3337 3.2500 135.4903
1302.0746 3.2500 135.2987
1301.7869 3.2500 135.1074
1301.7279 3.2500 134.9139
1301.7261 3.2500 134.7199
1302.0102 3.2500 134.5233
1302.1657 3.2500 134.3278
1302.2354 3.2500 134.1332
1302.7911 3.2500 133.9340
1302.7321 3.2500 133.7406
1302.8018 3.2500 133.5459
1302.7142 3.2500 133.3527
1302.6409 3.2500 133.1594
1302.5819 3.2500 132.9659
1302.6373 3.2500 132.7714

运行结果如下：

        显然，该数据拟合成了3条直线，3条直线的点分别是蓝色、绿色和红色。

3.1 将均值改为中值对比

将以上代码中的SVD函数中的c = np.mean(pts, axis=0)修改为c = np.median(pts, axis=0)，即将均值修改为中值，得到的结果如下：

        显然，两个图有差异，是因为中值不受异常点的影响，而均值会受到异常点的影响，所以，在这里（二维和三维数据），建议大家都用中值替代均值。

最后再给一个基于open3d的实现，以供参考

Multiple_Planes_Detection/utils.py at 7c660104ab75ca6ab1871bb9ee374f925d21b181 · yuecideng/Multiple_Planes_Detection (github.com)
                        
原文链接：https://blog.csdn.net/Subtlechange/article/details/123004329