激光点云法向量估计原理及数学推导

激光点云法向量估计原理及数学推导

激光点云法向量估计原理及数学推导

简述

点云法向量估计这个问题,相信很多人在点云处理,曲面重建的过程中遇到过。表面法线是几何体面的重要属性。而点云数据集在真实物体的表面表现为一组定点样本。对点云数据集的每个点的法线估计,可以看作是对表面法线的近似推断。在开源库提供我们调用便利的同时,了解其实现原理也有利于我们对问题的深刻认识!格物要致知:)
在这里插入图片描述

原理

确定表面一点法线的问题近似于估计表面的一个相切面法线的问题,因此转换过来以后就变成一个最小二乘法平面拟合估计问题。

  • 平面方程
    c o s α ⋅ x + c o s β ⋅ y + c o s γ ⋅ z + p = 0 cos\alpha·x+cos\beta·y+cos\gamma·z+p=0 cosαx+cosβy+cosγz+p=0
    c o s α , c o s β , c o s γ cos\alpha,cos\beta,cos\gamma cosα,cosβ,cosγ为平面上点 ( x , y , z ) (x,y,z) (x,y,z)处法向量的方向余弦, ∣ p ∣ |p| p为原点到平面的距离。
    a x + b y + c z = d ( d ≥ 0 ) , a 2 + b 2 + c 2 = 1 ax+by+cz=d(d\geq0),a^{2}+b^{2}+c^{2}=1 ax+by+cz=d(d0),a2+b2+c2=1
    求平面方程即转化为求 a , b , c , d a,b,c,d a,b,c,d四个参数。

  • 求解过程

1. 待拟合平面点集 ( x i , y i , z i ) , i = 1 , 2 , . . . , n (x_i,y_i,z_i),i=1,2,...,n (xi,yi,zi),i=1,2,...,n

待拟合的平面方程:
a x + b y + c z = d ( d ≥ 0 ) , a 2 + b 2 + c 2 = 1 ax+by+cz=d(d\geq0),a^{2}+b^{2}+c^{2}=1 ax+by+cz=d(d0),a2+b2+c2=1
任意点到平面的距离:
d i = ∣ a x + b y + c z − d ∣ d_i=|ax+by+cz-d| di=ax+by+czd

2. 要获得最佳拟合平面,则需要满足:

e = ∑ i = 1 n d i 2 → m i n e=\sum^n_{i=1} d_i^2\to min e=i=1ndi2min

因此,转化为求解极值的问题,
f = ∑ i = 1 n d i 2   − λ ( a 2 + b 2 + c 2 − 1 ) f=\sum^n_{i=1} d_i^2\space-\lambda(a^{2}+b^{2}+c^{2}-1) f=i=1ndi2 λ(a2+b2+c21)

3. 分别对 d , a , b , c d,a,b,c d,a,b,c求偏导
∂ f ∂ d = − 2 ∑ i = 1 n ( a x i + b y i + c z i − d ) = 0 \frac{\partial f}{\partial d}=-2\sum^n_{i=1} (ax_i+by_i+cz_i-d)=0 df=2i=1n(axi+byi+czid)=0
d = ∑ i = 1 n x i n a + ∑ i = 1 n y i n b + ∑ i = 1 n z i n c d=\frac{\sum ^{n}_{i=1}x_i}{n}a+\frac{\sum^{n}_{i=1}y_i}{n}b+\frac{\sum^{n}_{i=1}z_i}{n}c d=ni=1nxia+ni=1nyib+ni=1nzic
d d d带入任意点到平面的距离公式:

d i = ∣ a ( x i − ∑ i = 1 n x i n ) + b ( y i − ∑ i = 1 n y i n ) + c ( z i − ∑ i = 1 n z i n ) ∣ = ∣ a ( x i − x ‾ ) + b ( y i − y ‾ ) + c ( z i − z ‾ ) ∣ \begin{equation}\begin{split} d_i&=|a(x_i-\frac{\sum ^{n}_{i=1}x_i}{n})+b(y_i-\frac{\sum ^{n}_{i=1}y_i}{n})+c(z_i-\frac{\sum ^{n}_{i=1}z_i}{n})|\\ &=|a(x_i-\overline x)+b(y_i-\overline y)+c(z_i-\overline z)|\\ \end{split}\end{equation} di=a(xini=1nxi)+b(yini=1nyi)+c(zini=1nzi)=a(xix)+b(yiy)+c(ziz)

继续求偏导
∂ f ∂ a = 2 ∑ i = 1 n ( a ( x i − x ‾ ) + b ( y i − y ‾ ) + c ( z i − z ‾ ) ) ( x i − x ‾ ) − 2 λ a = 0 \frac{\partial f}{\partial a}=2\sum^n_{i=1} (a(x_i-\overline x)+b(y_i-\overline y)+c(z_i-\overline z))(x_i-\overline x)-2\lambda a=0 af=2i=1n(a(xix)+b(yiy)+c(ziz))(xix)2λa=0
Δ x i = x i − x ‾ , Δ y i = y i − y ‾ , Δ z i = z i − z ‾ \Delta x_i=x_i-\overline x,\Delta y_i=y_i-\overline y,\Delta z_i=z_i-\overline z Δxi=xix,Δyi=yiy,Δzi=ziz
则:
∂ f ∂ a = 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ x i − 2 λ a = 0 \frac{\partial f}{\partial a}=2\sum^n_{i=1} (a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta x_i-2\lambda a=0 af=2i=1n(aΔxi+bΔyi+cΔzi)Δxi2λa=0
同理:
∂ f ∂ b = 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ y i − 2 λ b = 0 \frac{\partial f}{\partial b}=2\sum^n_{i=1} (a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta y_i-2\lambda b=0 bf=2i=1n(aΔxi+bΔyi+cΔzi)Δyi2λb=0
∂ f ∂ c = 2 ∑ i = 1 n ( a Δ x i + b Δ y i + c Δ z i ) Δ z i − 2 λ c = 0 \frac{\partial f}{\partial c}=2\sum^n_{i=1} (a\Delta x_i+b\Delta y_i+c\Delta z_i)\Delta z_i-2\lambda c=0 cf=2i=1n(aΔxi+bΔyi+cΔzi)Δzi2λc=0

将上述三式统一:
( ∑ Δ x i Δ x i ∑ Δ x i Δ y i ∑ Δ x i Δ z i ∑ Δ x i Δ y i ∑ Δ y i Δ y i ∑ Δ y i Δ z i ∑ Δ x i Δ z i ∑ Δ y i Δ z i ∑ Δ z i Δ z i ) ( a b c ) = λ ( a b c ) \begin{pmatrix}\sum \Delta x_i\Delta x_i & \sum \Delta x_i\Delta y_i &\sum \Delta x_i\Delta z_i\\ \sum \Delta x_i\Delta y_i & \sum \Delta y_i\Delta y_i &\sum \Delta y_i\Delta z_i\\\sum \Delta x_i\Delta z_i & \sum \Delta y_i\Delta z_i &\sum \Delta z_i\Delta z_i \end{pmatrix}\begin{pmatrix}a \\ b \\c \end{pmatrix}=\lambda\begin{pmatrix}a\\b\\c \end{pmatrix} ΔxiΔxiΔxiΔyiΔxiΔziΔxiΔyiΔyiΔyiΔyiΔziΔxiΔziΔyiΔziΔziΔzi abc =λ abc
易得:
A x = λ x Ax=\lambda x Ax=λx

即转化到了求解矩阵A的特征值与特征向量的问题,矩阵A即为n个点的协方差矩阵。 ( a , b , c ) T (a,b,c)^T (a,b,c)T即为该矩阵的一个特征向量。

4. 求最小特征向量
如上所示,求得的特征向量可能不止一个,那么如何来选取特征向量,使得求得法向量为最佳拟合平面的法向量呢?

a 2 + b 2 + c 2 = 1 , ⇒ ( x , x ) = 1 a^2+b^2+c^2=1,\Rightarrow(x,x)=1 a2+b2+c2=1,(x,x)=1(内积形式),
A x = λ x , ⇒ ( A x , x ) = ( λ x , x ) , ⇒ λ = ( A x , x ) , Ax=\lambda x,\Rightarrow (Ax,x)=(\lambda x ,x),\Rightarrow \lambda=(Ax,x), Ax=λx,(Ax,x)=(λx,x),λ=(Ax,x),
⇒ λ = ∑ i = 0 n ( a Δ x i + b Δ y i + c Δ z i ) 2 , \Rightarrow \lambda=\sum_{i=0}^{n}(a\Delta x_i+b\Delta y_i+c\Delta z_i)^2, λ=i=0n(aΔxi+bΔyi+cΔzi)2,
⇒ λ = ∑ i = 0 n d i 2 \Rightarrow \lambda=\sum_{i=0}^{n}d_i^2 λ=i=0ndi2

e = ∑ i = 1 n d i 2 → m i n , λ → m i n e=\sum^n_{i=1} d_i^2\to min,\lambda \to min e=i=1ndi2min,λmin

因此,最小特征值对应的特征向量即为法向量

程序应用

  • PCL中的NormalEstimation
    #include <pcl/point_types.h>
    #include <pcl/features/normal_3d.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}
  • OpenMP加速法线估计
    PCL提供了表面法线估计的加速实现,基于OpenMP使用多核/多线程来加速计算。 该类的名称是pcl :: NormalEstimationOMP,其API与单线程pcl :: NormalEstimation 100%兼容。 在具有8个内核的系统上,一般计算时间可以加快6-8倍。
include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  ... read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
  ne.setNumberOfThreads(12);  // 手动设置线程数,否则提示错误
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

作者:codehory
链接:https://www.jianshu.com/p/faa9953213dd
来源:简书
著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。

以下是一个简单的点云切片求交的 Python 代码示例: ```python import numpy as np def plane_normal(point1, point2, point3): # 计算平面向量 v1 = point2 - point1 v2 = point3 - point1 normal = np.cross(v1, v2) return normal / np.linalg.norm(normal) def point_plane_intersection(point, normal, plane_point): # 计算点与平面的交点 t = np.dot(normal, plane_point - point) / np.dot(normal, normal) intersection = point + t * normal return intersection def polygon_plane_intersection(polygon, normal, plane_point): # 计算多边形与平面的交点 intersections = [] for i in range(len(polygon)): j = (i + 1) % len(polygon) edge_start = polygon[i] edge_end = polygon[j] edge_direction = edge_end - edge_start t = np.dot(normal, plane_point - edge_start) / np.dot(normal, edge_direction) if t >= 0 and t <= 1: intersection = edge_start + t * edge_direction intersections.append(intersection) return intersections def slice_point_cloud(point_cloud, plane_point, plane_normal): # 将点云投影到切片平面上,并计算点云与平面的交点 projected_points = [] intersections = [] for point in point_cloud: projected_point = point_plane_intersection(point, plane_normal, plane_point) projected_points.append(projected_point) intersection = point_plane_intersection(point, plane_normal, plane_point) intersections.append(intersection) # 将投影点集按照顺序连成一条封闭的多边形 polygon = np.array(projected_points) # 计算多边形与平面的交点 intersections = polygon_plane_intersection(polygon, plane_normal, plane_point) return intersections ``` 该代码实现了一个简单的点云切片求交,包括计算平面向量、点与平面的交点、多边形与平面的交点,以及将点云投影到切片平面上并计算点云与平面的交点等功能。使用该代码可以计算点云与任意平面的相交情况,具体使用方可以参考以下示例: ```python # 创建一个随机点云 point_cloud = np.random.rand(100, 3) # 定义一个平面 plane_point = np.array([0, 0, 0]) plane_normal = np.array([1, 0, 0]) # 计算点云与平面的交点 intersections = slice_point_cloud(point_cloud, plane_point, plane_normal) ```
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值