ransac拟合平面，代替open3d的segment_plane

0.open3d打包太大了，所以决定网上找找代码

使用open3d拟合平面并且求平面的法向量，open3d打包大概1个g的大小。

import open3d as o3dpcd = o3d.geometry.PointCloud()pcd.points = o3d.utility.Vector3dVector(points)## 使用RANSAC算法拟合平面plane_model, inliers = pcd.segment_plane(distance_threshold, ransac_n, num_iterations, probability)plane_normal = np.array(plane_model[:3])plane_normal /= np.linalg.norm(plane_normal)X_normal = [1, 0, 0]Y_normal = [0, 1, 0]Z_normal = [0, 0, 1]# 计算夹角（单位为弧度）angle = np.arccos(np.dot(plane_normal, X_normal))# 将夹角转换为角度X_angel = degrees(angle)# 计算夹角（单位为弧度）angle = np.arccos(np.dot(plane_normal, Y_normal))# 将夹角转换为角度Y_angel = degrees(angle)# 计算夹角（单位为弧度）angle = np.arccos(np.dot(plane_normal, Z_normal))# 将夹角转换为角度Z_angel = degrees(angle)

1.找了一个git上的代码

https://github.com/leomariga/pyRANSAC-3D/blob/master/pyransac3d/plane.py

import randomimport numpy as npclass Plane:"""Implementation of planar RANSAC.Class for Plane object, which finds the equation of a infinite plane using RANSAC algorithim.Call `fit(.)` to randomly take 3 points of pointcloud to verify inliers based on a threshold.![Plane](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/plano.gif "Plane")---"""def __init__(self):self.inliers = []self.equation = []def fit(self, pts, thresh=0.05, minPoints=100, maxIteration=1000):"""Find the best equation for a plane.:param pts: 3D point cloud as a `np.array (N,3)`.:param thresh: Threshold distance from the plane which is considered inlier.:param maxIteration: Number of maximum iteration which RANSAC will loop over.:returns:- `self.equation`:  Parameters of the plane using Ax+By+Cy+D `np.array (1, 4)`- `self.inliers`: points from the dataset considered inliers---"""n_points = pts.shape[0]best_eq = []best_inliers = []for it in range(maxIteration):# Samples 3 random pointsid_samples = random.sample(range(0, n_points), 3)pt_samples = pts[id_samples]# We have to find the plane equation described by those 3 points# We find first 2 vectors that are part of this plane# A = pt2 - pt1# B = pt3 - pt1vecA = pt_samples[1, :] - pt_samples[0, :]vecB = pt_samples[2, :] - pt_samples[0, :]# Now we compute the cross product of vecA and vecB to get vecC which is normal to the planevecC = np.cross(vecA, vecB)# The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k# We have to use a point to find kvecC = vecC / np.linalg.norm(vecC)k = -np.sum(np.multiply(vecC, pt_samples[1, :]))plane_eq = [vecC[0], vecC[1], vecC[2], k]# Distance from a point to a plane# https://mathworld.wolfram.com/Point-PlaneDistance.htmlpt_id_inliers = []  # list of inliers idsdist_pt = (plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] + plane_eq[3]) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)# Select indexes where distance is biggers than the thresholdpt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]if len(pt_id_inliers) > len(best_inliers):best_eq = plane_eqbest_inliers = pt_id_inliersself.inliers = best_inliersself.equation = best_eqreturn self.equation, self.inliers

2.改进代码

2.1 提速

用的时候发现代码的速度比open3d的慢了50ms左右。找了一圈找到方法了
https://zhuanlan.zhihu.com/p/62238520
就是替换循环次数

import randomimport numpy as npclass Plane:"""Implementation of planar RANSAC.Class for Plane object, which finds the equation of a infinite plane using RANSAC algorithim.Call `fit(.)` to randomly take 3 points of pointcloud to verify inliers based on a threshold.![Plane](https://raw.githubusercontent.com/leomariga/pyRANSAC-3D/master/doc/plano.gif "Plane")---"""def __init__(self):self.inliers = []self.equation = []def fit(self, pts, thresh=0.05, minPoints=100, maxIteration=1000, P=0.99):"""Find the best equation for a plane.:param pts: 3D point cloud as a `np.array (N,3)`.:param thresh: Threshold distance from the plane which is considered inlier.:param maxIteration: Number of maximum iteration which RANSAC will loop over.:param P: desired probability that we get a good sample:returns:- `self.equation`:  Parameters of the plane using Ax+By+Cy+D `np.array (1, 4)`- `self.inliers`: points from the dataset considered inliers---"""n_points = pts.shape[0]best_eq = []best_inliers = []i = 0while True:if i < maxIteration:i += 1# Samples 3 random pointsid_samples = random.sample(range(0, n_points), 3)pt_samples = pts[id_samples]# We have to find the plane equation described by those 3 points# We find first 2 vectors that are part of this plane# A = pt2 - pt1# B = pt3 - pt1vecA = pt_samples[1, :] - pt_samples[0, :]vecB = pt_samples[2, :] - pt_samples[0, :]# Now we compute the cross product of vecA and vecB to get vecC which is normal to the planevecC = np.cross(vecA, vecB)# The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k# We have to use a point to find kvecC = vecC / np.linalg.norm(vecC)k = -np.sum(np.multiply(vecC, pt_samples[1, :]))plane_eq = [vecC[0], vecC[1], vecC[2], k]# Distance from a point to a plane# https://mathworld.wolfram.com/Point-PlaneDistance.htmlpt_id_inliers = []  # list of inliers idsdist_pt = (plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] +plane_eq[3]) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)# Select indexes where distance is biggers than the thresholdpt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]#https://www.cse.psu.edu/~rtc12/CSE486/lecture15.pdf#speed upif len(pt_id_inliers) > len(best_inliers):maxIteration = math.log(1 - P) / math.log(1 - pow(len(pt_id_inliers) / n_points, 3))best_eq = plane_eqbest_inliers = pt_id_inliersself.inliers = best_inliersself.equation = best_eqif len(pt_id_inliers) > minPoints:breakreturn self.equation, self.inliers

2.2 提升精度

经过测试发现，拟合的平面的精度还是比open3d差。然后使用最小二乘法在求一次平面了

def ransac_fitplan(pts, thresh=5,num_iterations=1000):# # 希望的得到正确模型的概率n_points = pts.shape[0]best_inliers = []P = 0.9999i=0while True:if i<num_iterations:i+=1# 随机在数据中红选出两个点去求解模型id_samples = random.sample(range(0, n_points), 3)pt_samples = pts[id_samples]vecA = pt_samples[1, :] - pt_samples[0, :]vecB = pt_samples[2, :] - pt_samples[0, :]# Now we compute the cross product of vecA and vecB to get vecC which is normal to the planevecC = np.cross(vecA, vecB)# The plane equation will be vecC[0]*x + vecC[1]*y + vecC[0]*z = -k# We have to use a point to find kvecC = vecC / np.linalg.norm(vecC)k = -np.sum(np.multiply(vecC, pt_samples[1, :]))plane_eq = [vecC[0], vecC[1], vecC[2], k]pt_id_inliers = []  # list of inliers idsdist_pt = (plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + plane_eq[2] * pts[:, 2] + plane_eq[3]) / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2)# Select indexes where distance is biggers than the thresholdpt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]if len(pt_id_inliers) > len(best_inliers):num_iterations = math.log(1 - P) / math.log(1 - pow(len(pt_id_inliers) / n_points, 3))best_inliers = pt_id_inliers# 判断是否当前模型已经符合超过一半的点if len(pt_id_inliers) > 0.5*n_points:breakelse:break# 最小二乘法拟合平面X = np.column_stack((pts[:, :2], np.ones(pts.shape[0])))coefficients, _, _, _ = lstsq(X[best_inliers, :], pts[best_inliers, 2])return coefficients,best_inliers