RANSAC点云多平面拟合分割

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

假设 P P P 为一个点集， p i p_i pi 是点集中的任意一点， p i ∈ P p_i\in P pi∈P， p c p_c pc是 P P P 的中心，即 p c = 1 n ∑ p i ∈ P p i p_c = \frac{1}{n}\sum\limits_{p_i\in P}p_i pc=n1pi∈P∑pi，假设拟合的目标平面经过 p c p_c pc，且其法向量为 n ⃗ \vec{n} n ，根据最小二乘法，所有点到平面的距离即 ∑ i = 1 n ( p i − p c ) ⋅ n ⃗ \sum\limits_{i=1}^n(p_i-p_c)\cdot \vec{n} i=1∑n(pi−pc)⋅n 应该被最小化。这个目标函数可以根据奇异值分解（SVD）得到（参考：https://www.ltu.se/cms_fs/1.51590!/svd-fitting.pdf）。

定义一个矩阵 A A A
A = [ p 1 − p c , p 2 − p c , … , p n − p c ] A = [p_1-p_c, p_2-p_c, \dots , p_n-p_c ] A=[p1−pc,p2−pc,…,pn−pc]
则

其中， y = U T n ˙ y=U^T\dot n y=UTn˙, S = d i a g ( σ 1 , σ 2 , σ 3 ) S=diag(\sigma_1,\sigma_2,\sigma_3) S=diag(σ1,σ2,σ3)，并且 σ 1 ≥ σ 2 ≥ σ 3 \sigma_1 \geq \sigma_2 \geq \sigma_3 σ1≥σ2≥σ3。最后，拟合的平面法向量就是 U U U的第3列，即
n ⃗ = U ( : , 3 ) \vec{n} = U(:,3) n =U(:,3)

具体实现见下面代码：

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是一种算法，一种思想，不仅仅可以用于拟合平面，实际上还有很多用处。

这里的算法如下算法

随机挑选3个点，并计算3点形成的采样平面、
根据采样平面计算所有点到平面的距离，并根据参数（距离的阈值）将点分为内点和外点
统计内点，外点数量，更新最大迭代次数（ k = l o g ( 1 − p ) l o g ( 1 − w n ) k = \frac{log(1-p)}{log(1-w^n)} k=log(1−wn)log(1−p)）
重复以上过程直到达到停止条件（最大迭代次数，内点比例等）

具体实现见下面代码：

# 注意这里并没有根据内点比例和模型可靠的概率动态调整最大迭代次数
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)

绘图显示

不同颜色代表不同的平面

全部代码

包含一些读取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!!!")

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

Multiple_Planes_Detection/utils.py at 7c660104ab75ca6ab1871bb9ee374f925d21b181 · yuecideng/Multiple_Planes_Detection (github.com)