ransac拟合平面，代替open3d的segment_plane
0.open3d打包太大了，所以决定网上找找代码
使用open3d拟合平面并且求平面的法向量，open3d打包大概1个g的大小。
import open3d as o3d
pcd = 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 random
import numpy as np
class 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, 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 points
id_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 - pt1
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 plane
vecC = 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 k
vecC = 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.html
pt_id_inliers = [] # list of inliers ids
dist_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 threshold
pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]
if len(pt_id_inliers) > len(best_inliers):
best_eq = plane_eq
best_inliers = pt_id_inliers
self.inliers = best_inliers
self.equation = best_eq
return self.equation, self.inliers
2.改进代码
2.1 提速
用的时候发现代码的速度比open3d的慢了50ms左右。找了一圈找到方法了
https://zhuanlan.zhihu.com/p/62238520
就是替换循环次数
import random
import numpy as np
class 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, 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 = 0
while True:
if i < maxIteration:
i += 1
# Samples 3 random points
id_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 - pt1
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 plane
vecC = 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 k
vecC = 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.html
pt_id_inliers = [] # list of inliers ids
dist_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 threshold
pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0]
#https://www.cse.psu.edu/~rtc12/CSE486/lecture15.pdf
#speed up
if 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_eq
best_inliers = pt_id_inliers
self.inliers = best_inliers
self.equation = best_eq
if len(pt_id_inliers) > minPoints:
break
return 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.9999
i=0
while 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 plane
vecC = 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 k
vecC = 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 ids
dist_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 threshold
pt_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:
break
else:
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