当前位置: 首页 > news >正文

Python实现RANSAC进行点云直线、平面、曲面、圆、球体和圆柱拟合

        本节我们分享使用RANSAC算法进行点云的拟合。RANSAC算法是什么?不知道的同学们前排罚站!(前面有)

        总的来说,RANSAC(Random Sample Consensus)是一种通用的迭代鲁棒估计框架,无论拟合何种几何模型,其思想完全一致:  1. 随机采样 → 2. 最小样本拟合 → 3. 内点判定 → 4. 模型评估 → 5. 迭代收敛 → 6. 输出最优模型。  
下面按“直线、平面、一般二次曲面、二维圆、三维球面、圆柱面”六种情形,给出统一的实现步骤与关键公式,便于对照查阅。

--------------------------------
1. 拟合 3D 空间直线
--------------------------------
随机采样  
- 每次随机抽取 2 个点 p₁、p₂(两点确定一条直线)。

模型估计  
- 方向向量 v = (p₂ − p₁) / ‖p₂ − p₁‖  
- 直线方程:L(t) = p₁ + t v,t∈ℝ

内点判定  
- 点 p 到直线的距离  
d = ‖(p − p₁) × v‖  
若 d < d_th,则为内点。

迭代终止  
- 最大迭代次数 N 或内点比例达到要求即可停止。

--------------------------------
2. 拟合 3D 平面
--------------------------------
随机采样  
- 每次随机抽取 3 个不共线的点 p₁,p₂,p₃。

模型估计  
- 平面法向量 n = (p₂ − p₁) × (p₃ − p₁) 并归一化  
- 平面方程:n·(x − p₁)=0,即 ax+by+cz+d=0

内点判定  
- 点 p 到平面距离  
d = |a·x + b·y + c·z + d| / √(a²+b²+c²)  
若 d < d_th,则为内点。

--------------------------------
3. 拟合一般二次曲面
--------------------------------
随机采样  
- 二次曲面一般式 Ax²+By²+Cz²+Dxy+Eyz+Fzx+Gx+Hy+Iz+J=0 共 10 个参数,因此最少需要 9 个点(秩缺 1,需额外约束 J=1 或 ‖参数‖=1)。

模型估计  
- 构造设计矩阵 A ∈ ℝ^{m×9} 和观测向量 b ∈ ℝ^m,用最小二乘解参数向量 θ=[A,B,…,I]^T。  
- 得到隐式曲面方程 f(x,y,z)=0。

内点判定  
- 点 p 到隐式曲面的代数距离 |f(p)| / ‖∇f(p)‖ 与阈值比较。

--------------------------------
4. 拟合 2D 圆
--------------------------------
随机采样  
- 每次随机抽取 3 个非共线的 2D 点 (x₁,y₁),(x₂,y₂),(x₃,y₃)。

模型估计  
- 圆心 (a,b) 和半径 r 的闭合公式  
a = [ (x₂²+y₂²−x₁²−y₁¹)(y₃−y₁) − (x₃²+y₃²−x₁²−y₁²)(y₂−y₁) ] / D  
b = [ (x₃²+y₃²−x₁²−y₁²)(x₂−x₁) − (x₂²+y₂²−x₁²−y₁²)(x₃−x₁) ] / D  
D = 2[(x₂−x₁)(y₃−y₁) − (x₃−x₁)(y₂−y₁)]  
r = √[(x₁−a)²+(y₁−b)²]

内点判定  
- 点 (x,y) 到圆的距离  
|√[(x−a)²+(y−b)²] − r| < d_th。

--------------------------------
5. 拟合 3D 球面
--------------------------------
随机采样  
- 每次随机抽取 4 个非共面 3D 点 p₁…p₄。

模型估计  
- 球心 c 和半径 R 的线性方程组  
‖p_i − c‖² = R², i=1…4 → 4 线性方程 → 解 c、R。

内点判定  
- 点 p 到球面距离  
|‖p − c‖ − R| < d_th。

--------------------------------
6. 拟合 3D 圆柱面
--------------------------------
随机采样  
- 每次随机抽取 5 个 3D 点(圆柱面有 7 个自由度,但 5 点可解唯一参数,见下)。

模型估计  
- 将圆柱面参数化为:  
中心轴:直线 L(t)=c + t d(方向向量 d,单位化)  
半径 r  
- 5 点约束:任意点 p_i 到轴的距离等于 r  
‖(p_i − c) × d‖ = r, i=1…5  
可通过非线性最小二乘或几何代数法一次性求出 d、c、r。

内点判定  
- 点 p 到圆柱面距离  
|‖(p − c) × d‖ − r| < d_th。

--------------------------------
下面书写一段RANSAC的统一处理流程(伪代码)
--------------------------------
输入:点云 Q,几何模型类型 T,阈值 d_th,最大迭代 N  
for k = 1…N  
S ← RandomSample(Q, m)           # m 由模型决定(2,3,4,5…)  
M ← FitModel(S, T)               # 见上各小节  
Inliers ← {p ∈ Q | Distance(p,M) < d_th}  
if |Inliers| > best_inliers  
best_model ← M  
best_inliers ← Inliers  
输出:best_model, best_inliers

可视化  
- 用 Open3D / Matplotlib / PCL 等库将原始点云和拟合几何体(直线、平面、网格化曲面、圆环、球体、圆柱网格)同时渲染。

        至此,六种常见几何模型的 RANSAC 实现步骤全部给出,可直接对照代码模板进行落地。当然,本次的数据猪脚依然是我们的老朋友——兔砸!

一、RANSAC各种拟合程序

import open3d as o3d
import numpy as np
import pyransac3d as pyrsc
import randomFILE = "E:/CSDN/规则点云/bunny.pcd"      # 改成自己的文件# ---------------- 通用工具 -----------------
def load_pcd():pcd = o3d.io.read_point_cloud(FILE)if not pcd.has_points():raise RuntimeError("找不到 {}".format(FILE))return pcd, np.asarray(pcd.points)def show(title, in_pcd, out_pcd, geom):"""统一可视化"""o3d.visualization.draw_geometries([in_pcd, out_pcd, geom],window_name=title,width=800, height=600)# AXIS = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1)# ---------------- 1. 直线 -----------------
def fit_line(pcd, pts):def ransac_3d(points, thresh=0.05, max_iter=1000):best_in, best_model = [], Nonefor _ in range(max_iter):idx = random.sample(range(len(points)), 2)p1, p2 = points[idx]vec = p2 - p1if np.linalg.norm(vec) < 1e-6:continuevec = vec / np.linalg.norm(vec)dists = np.linalg.norm(np.cross(points - p1, vec), axis=1)inliers = np.where(dists < thresh)[0]if len(inliers) > len(best_in):best_in = inliersbest_model = (p1, vec)return best_model, best_inmodel, idx = ransac_3d(pts, 0.05, 1000)if model is None:print("直线拟合失败")returnp1, vec = modelin_pcd  = pcd.select_by_index(idx).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(idx, invert=True).paint_uniform_color([0,1,0])length = np.linalg.norm(pcd.get_max_bound() - pcd.get_min_bound())start, end = p1 - vec * length, p1 + vec * lengthls = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector([start, end]),lines=o3d.utility.Vector2iVector([[0,1]]))ls.colors = o3d.utility.Vector3dVector([[0,0,1]])show("1. 直线拟合", in_pcd, out_pcd, ls)# ---------------- 2. 平面 -----------------
def fit_plane(pcd, pts):plane = pyrsc.Plane()eq, idx = plane.fit(pts, thresh=0.01, maxIteration=1000)in_pcd  = pcd.select_by_index(idx).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(idx, invert=True).paint_uniform_color([0,1,0])# 平面网格aabb = in_pcd.get_axis_aligned_bounding_box()size = max(aabb.get_extent()) * 1.5mesh = o3d.geometry.TriangleMesh.create_box(size, size, 0.001)mesh.translate([-size/2, -size/2, 0])mesh.rotate(in_pcd.get_rotation_matrix_from_xyz((0,0,0)), center=(0,0,0))normal = np.array(eq[:3])z = np.array([0,0,1])if np.linalg.norm(np.cross(z, normal)) > 1e-6:rot = o3d.geometry.get_rotation_matrix_from_axis_angle(np.cross(z, normal))mesh.rotate(rot, center=(0,0,0))mesh.translate(in_pcd.get_center())mesh.paint_uniform_color([0,0,1])mesh.compute_vertex_normals()show("2. 平面拟合", in_pcd, out_pcd, mesh)# ---------------- 3. 二次曲面 -----------------
def fit_quadric(pcd, pts):"""拟合二次曲面 z = a x² + b y² + c xy + d x + e y + f用 RANSAC 选 6 个内点,然后用最小二乘解 6 参数。"""def quadric_model(pts_sample):A = np.c_[pts_sample[:,0]**2, pts_sample[:,1]**2,pts_sample[:,0]*pts_sample[:,1],pts_sample[:,0], pts_sample[:,1],np.ones(len(pts_sample))]b = pts_sample[:,2]coeffs, *_ = np.linalg.lstsq(A, b, rcond=None)return coeffsdef residuals(pts, coeffs):a,b,c,d,e,f = coeffsreturn np.abs(pts[:,2] - (a*pts[:,0]**2 + b*pts[:,1]**2 + c*pts[:,0]*pts[:,1] + d*pts[:,0] + e*pts[:,1] + f))best_in, best_coeff = [], Nonefor _ in range(1000):idx = random.sample(range(len(pts)), 6)sample = pts[idx]try:coeff = quadric_model(sample)except np.linalg.LinAlgError:continuedists = residuals(pts, coeff)inliers = np.where(dists < 0.05)[0]if len(inliers) > len(best_in):best_in, best_coeff = inliers, coeffif best_coeff is None:print("二次曲面拟合失败")returnin_pcd  = pcd.select_by_index(best_in).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(best_in, invert=True).paint_uniform_color([0,1,0])# 网格可视化aabb = in_pcd.get_axis_aligned_bounding_box()xx, yy = np.meshgrid(np.linspace(aabb.min_bound[0], aabb.max_bound[0], 50),np.linspace(aabb.min_bound[1], aabb.max_bound[1], 50))a,b,c,d,e,f = best_coeffzz = a*xx**2 + b*yy**2 + c*xx*yy + d*xx + e*yy + fvertices = np.stack([xx.ravel(), yy.ravel(), zz.ravel()], axis=1)mesh = o3d.geometry.TriangleMesh()mesh.vertices = o3d.utility.Vector3dVector(vertices)idx = np.arange(50*50).reshape(50,50)triangles = []for i in range(49):for j in range(49):triangles.append([idx[i,j], idx[i+1,j], idx[i,j+1]])triangles.append([idx[i+1,j], idx[i+1,j+1], idx[i,j+1]])mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles))mesh.paint_uniform_color([0,0,1])mesh.compute_vertex_normals()show("3. 曲面拟合", in_pcd, out_pcd, mesh)# ---------------- 4. 圆 -----------------
def fit_circle(pcd, pts):pts2d = pts[:, :2]   # 假设圆在 XY 平面def ransac_circle(pts2d, thresh=0.05, max_iter=1000):best_in, best_model = [], Nonefor _ in range(max_iter):idx = random.sample(range(len(pts2d)), 3)A,B,C = pts2d[idx]D = 2*((B[0]-A[0])*(C[1]-A[1]) - (B[1]-A[1])*(C[0]-A[0]))if abs(D) < 1e-6: continuecx = ((C[1]-A[1])*(B[0]**2+B[1]**2-A[0]**2-A[1]**2) - (B[1]-A[1])*(C[0]**2+C[1]**2-A[0]**2-A[1]**2)) / Dcy = ((B[0]-A[0])*(C[0]**2+C[1]**2-A[0]**2-A[1]**2) - (C[0]-A[0])*(B[0]**2+B[1]**2-A[0]**2-A[1]**2)) / Dr  = np.linalg.norm([A[0]-cx, A[1]-cy])dists = np.abs(np.linalg.norm(pts2d - [cx,cy], axis=1) - r)inliers = np.where(dists < thresh)[0]if len(inliers) > len(best_in):best_in, best_model = inliers, (cx, cy, r)return best_model, best_inmodel, idx = ransac_circle(pts2d, 0.05, 1000)if model is None:print("圆拟合失败")returncx,cy,r = modelin_pcd  = pcd.select_by_index(idx).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(idx, invert=True).paint_uniform_color([0,1,0])theta = np.linspace(0, 2*np.pi, 100)x = cx + r*np.cos(theta)y = cy + r*np.sin(theta)z = np.zeros_like(x)circle_pts = np.vstack([x,y,z]).Tls = o3d.geometry.LineSet(points=o3d.utility.Vector3dVector(circle_pts),lines=o3d.utility.Vector2iVector([[i,(i+1)%100] for i in range(100)]))ls.colors = o3d.utility.Vector3dVector([[0,0,1] for _ in range(100)])show("4. 圆拟合", in_pcd, out_pcd, ls)# ---------------- 5. 球 -----------------
def fit_sphere(pcd, pts):sph = pyrsc.Sphere()center, radius, idx = sph.fit(pts, thresh=0.05, maxIteration=1000)in_pcd  = pcd.select_by_index(idx).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(idx, invert=True).paint_uniform_color([0,1,0])mesh = o3d.geometry.TriangleMesh.create_sphere(radius=radius)mesh.translate(center)mesh.paint_uniform_color([0,0,1])mesh.compute_vertex_normals()show("5. 球拟合", in_pcd, out_pcd, mesh)# ---------------- 6. 圆柱 -----------------
def fit_cylinder(pcd, pts):cyl = pyrsc.Cylinder()axis, center, radius, idx = cyl.fit(pts, thresh=0.05, maxIteration=1000)in_pcd  = pcd.select_by_index(idx).paint_uniform_color([1,0,0])out_pcd = pcd.select_by_index(idx, invert=True).paint_uniform_color([0,1,0])height = np.linalg.norm(pcd.get_max_bound() - pcd.get_min_bound()) * 1.2mesh = o3d.geometry.TriangleMesh.create_cylinder(radius=radius, height=height, resolution=50)mesh.compute_vertex_normals()mesh.paint_uniform_color([0,0,1])z = np.array([0,0,1])axis = axis / np.linalg.norm(axis)if np.linalg.norm(np.cross(z, axis)) > 1e-6:rot = o3d.geometry.get_rotation_matrix_from_axis_angle(np.cross(z, axis))mesh.rotate(rot, center=(0,0,0))mesh.translate(center)show("6. 圆柱拟合", in_pcd, out_pcd, mesh)# ---------------- 主菜单 -----------------
def main():pcd, pts = load_pcd()menu = """
1 直线
2 平面
3 二次曲面
4 圆
5 球
6 圆柱
q 退出
请选择(1-6):"""while True:choice = input(menu).strip()if choice == 'q': breakif choice not in {'1','2','3','4','5','6'}:print("输入无效")continueif choice == '1': fit_line(pcd, pts)if choice == '2': fit_plane(pcd, pts)if choice == '3': fit_quadric(pcd, pts)if choice == '4': fit_circle(pcd, pts)if choice == '5': fit_sphere(pcd, pts)if choice == '6': fit_cylinder(pcd, pts)if __name__ == "__main__":main()

二、RANSAC各种拟合结果

        本次的程序添加了选择按钮,1-6分别是RANSAC拟合直线、平面、曲面、圆、球面、圆柱面。除了圆柱面拟合过于离谱(主要兔砸长得不像柱子),其他的都挺好。

就酱,下次见^-^

http://www.dtcms.com/a/352120.html

相关文章:

  • 负载均衡之平滑加权轮询(Smooth Weighted Round Robin)详解与实现
  • 前沿技术趋势与应用:探索数字世界的下一个十年
  • 本地通过阿里云ECS建立SSH隧道连接阿里云RDS MYSQL数据库
  • 【P2P】RELAY服务2:cmake+ c实现及ubuntu运行
  • 淘宝/天猫商品详情API数据解析【附代码】
  • 软件检测报告:XML外部实体(XXE)注入漏洞原因和影响
  • 【Erdas实验教程】031:遥感图像频率域增强(傅立叶变换)
  • BCI良好棉花认证标准及申请条件(2025年最新版)
  • 加密狗与U盘的核心区别,U盘能否替代加密狗?
  • 电力工程大模型驱动AI工程计算:从“算错挨骂”到“一键精准”
  • 开发指南135-CSS中定义参数
  • 技术干货丨基于SimSolid的塑胶模具温度场瞬态分析
  • 【贪心算法】day3
  • win11在安装com0com软件后,在设备管理器中虚拟串口黄色感叹号得解决方法
  • 什么是Webpack的热更新(Hot Module Replacement)?原理是什么?
  • 2 梯度下降算法
  • 面试 总结(1)
  • 博士招生 | 南洋理工大学 PINE Lab 招收全奖博士
  • 一文看懂@Bean注解的原理
  • Markdown 编辑器 语法
  • 18、移动应用系统分析与设计
  • 字帖生成器怎么用?电脑手机双端操作指南
  • halcon的默认图像坐标系是怎么样的?
  • Agent实战教程:LangGraph关于智能体的架构模式与核心概念
  • MySQL表的管理
  • Matplotlib渲染性能提升10倍:底层原理与实战技巧
  • 具身智能常用【数据集】汇总篇
  • 三菱FX5U PLC访问字变量的某一位
  • 信长之野望 新生 威力加强版 送修改器 免安装中文版
  • 基于 MediaPipe + Three.js 的实时姿态可视化前端