找活做的网站,湖北网站开发公司,手机英语网站,如何建设企业人力资源网站pclpy 最小二乘法拟合平面 一、算法原理二、代码三、结果1.左边原点云、右边最小二乘法拟合平面后点云投影 四、相关数据 一、算法原理
平面方程的一般表达式为#xff1a; A x B y C z D 0 ( C ≠ 0 ) Ax By Cz D 0 \quad (C\neq0) AxByCzD0(C0) 即#xff1a; … pclpy 最小二乘法拟合平面 一、算法原理二、代码三、结果1.左边原点云、右边最小二乘法拟合平面后点云投影 四、相关数据 一、算法原理
平面方程的一般表达式为 A x B y C z D 0 ( C ≠ 0 ) Ax By Cz D 0 \quad (C\neq0) AxByCzD0(C0) 即 Z − A C x − B C y − D C Z -\frac{A}{C}x-\frac{B}{C}y-\frac{D}{C} Z−CAx−CBy−CD 记 a 0 − A C , a 1 − B C , a 2 − D C a_0 -\frac{A}{C}, a_1 -\frac{B}{C}, a_2-\frac{D}{C} a0−CA,a1−CB,a2−CD 所以 Z a 0 x a 1 y a 2 Z a_0x a_1y a_2 Za0xa1ya2 要用该 n 个点拟合平面方程即使 ∑ n 1 N ( a 0 x a 1 y a 2 ) 2 − m i n \sum_{n1}^{N}{(a_0x a_1y a_2)^2}-min n1∑N(a0xa1ya2)2−min 要使S最小对a,b,c求偏导 改写成矩阵的形式为 二、代码
from pclpy import pcl
import numpy as npdef CloudShow(cloud1, cloud2):Args:在一个窗口可视化多个点云cloud1: 点云数据1cloud2: 点云数据2viewer pcl.visualization.PCLVisualizer(viewer) # 建立可刷窗口对象 窗口名 viewerv0 1 # 设置标签名0, 1标记第一个窗口viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v0) # 创建一个可视化的窗口viewer.setBackgroundColor(0.0, 0.0, 0.0, v0) # 设置窗口背景为黑色single_color pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud1, 255.0, 0, 0.0) # 将点云设置为红色viewer.addPointCloud(cloud1, # 要添加到窗口的点云数据。single_color, # 指定点云的颜色sample cloud1, # 添加的点云命名v0) # 点云添加到的视图v1 2 # 设置标签名2代表第二个窗口viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v1) # 创建一个可视化的窗口viewer.setBackgroundColor(255.0, 255.0, 255.0, v1) # 设置窗口背景为白色single_color pcl.visualization.PointCloudColorHandlerCustom.PointXYZ(cloud2, 0.0, 255.0, 0.0) # 将点云设置为绿色viewer.addPointCloud(cloud2, # 要添加到窗口的点云数据。single_color, # 指定点云的颜色sample cloud2, # 添加的点云命名v1) # 点云添加到的视图# 设置点云窗口可移除对点云可视化没有影响viewer.setPointCloudRenderingProperties(0, # 设置点云点的大小1, # 点云像素sample cloud1, # 识别特定点云v0) # 在那个窗口可视化viewer.setPointCloudRenderingProperties(0, # 设置点云点的大小1, # 点云像素sample cloud2, # 识别特定点云v1) # 在那个窗口可视化# viewer.addCoordinateSystem(1.0) # 设置坐标轴 坐标轴的长度为1.0# 窗口建立while not viewer.wasStopped():viewer.spinOnce(10)def plane(cloud, normal_vector):coeffs pcl.ModelCoefficients() # 创建了一个模型系数对象coeffs.values.append(normal_vector[0]) # a 0.0coeffs.values.append(normal_vector[1]) # b 0.0coeffs.values.append(normal_vector[2]) # c 1.0coeffs.values.append(normal_vector[3]) # d 0.0# 创建滤波器proj pcl.filters.ProjectInliers.PointXYZ() # 过滤器对象 proj用于将点云投影到一个模型上。proj.setModelType(0) # 模型类型被设为 0代表使用平面模型。proj.setInputCloud(cloud) # 将cloud点云数据进行处理proj.setModelCoefficients(coeffs) # 处理参数coeffscloud_projected pcl.PointCloud.PointXYZ() # 建立保存点云proj.filter(cloud_projected) # 将投影结果保存return cloud_projectedif __name__ __main__:cloud1 pcl.PointCloud.PointXYZ()reader pcl.io.PCDReader() # 设置读取对象reader.read(res/bunny.pcd, cloud1) # 读取点云保存在cloud中# 调用函数生成离散点x, y, z cloud1.x, cloud1.y, cloud1.zN cloud1.size()# ------------------------构建系数矩阵-----------------------------A np.array([[sum(x ** 2), sum(x * y), sum(x)],[sum(x * y), sum(y ** 2), sum(y)],[sum(x), sum(y), N]])B np.array([[sum(x * z), sum(y * z), sum(z)]])# 求解X np.linalg.solve(A, B.T)print(平面拟合结果为z %.3f * x %.3f * y %.3f % (X[0], X[1], X[2]))a, b, c, d X[0], X[1], -1, X[2]plane_cloud plane(cloud1, [a, b, c, d]) # 获得投影后的点云数据# ------------------ 可视化点云 -----------------CloudShow(cloud1, plane_cloud) 三、结果
1.左边原点云、右边最小二乘法拟合平面后点云投影 四、相关数据
最小二乘法拟合参考链接最小二乘拟合平面(python/C版) - 知乎 (zhihu.com)
