网站建设合理性,中关村科技租赁,制作网页时创建超链接,全包胶衣网站0.open3d打包太大了#xff0c;所以决定网上找找代码
使用open3d拟合平面并且求平面的法向量#xff0c;open3d打包大概1个g的大小。
import open3d as o3dpcd o3d.geometry.PointCloud()pcd.points o3d.utility.Vector3dVector(points)## 使用RANSAC算法拟合平面plane_m…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.---def __init__(self):self.inliers []self.equation []def fit(self, pts, thresh0.05, minPoints100, maxIteration1000):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 AxByCyD 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.inliers2.改进代码
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.---def __init__(self):self.inliers []self.equation []def fit(self, pts, thresh0.05, minPoints100, maxIteration1000, P0.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 AxByCyD 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.inliers2.2 提升精度
经过测试发现拟合的平面的精度还是比open3d差。然后使用最小二乘法在求一次平面了
def ransac_fitplan(pts, thresh5,num_iterations1000):# # 希望的得到正确模型的概率n_points pts.shape[0]best_inliers []P 0.9999i0while True:if inum_iterations:i1# 随机在数据中红选出两个点去求解模型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
