2.2.4 法向量估计(Normal Estimation)算法
法向量估计的目的是计算每个点的法向量,用于后续任务如曲面重建和特征提取。常用的法向量估计(Normal Estimation)算法如下所示。
1. 最小二乘法(Least Squares)算法
最小二乘法(Least Squares)算法通过最小化点云到法向量的误差来估计法向量。这是一种数学优化技术,通常用于拟合模型到一组观测数据。其目标是通过最小化观测数据与模型预测值之间的残差平方和,来找到最优的模型参数。在LiDAR点云处理中,最小二乘法经常被用于拟合几何形状,比如拟合平面、曲线或其他表面。
2. 法线采样(Normal Sampling)算法
法线采样(Normal Sampling)算法用于估计每个点的法线方向,通过在点云中的局部区域进行采样来估计法向量。法线通常用于描述点云中表面的方向,有助于分析物体的几何结构。
请看下面的例子,功能是使用NumPy和最小二乘法来拟合平面到LiDAR点云。
实例2-4:使用RANSAC算法处理LiDAR点云(codes/2/zui.py)
实例文件zui.py的具体实现代码如下所示。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.optimize import least_squares
import matplotlib.patches as mpatches
# 生成模拟的LiDAR点云数据
np.random.seed(42)
num_points = 100
x = np.random.uniform(low=-5, high=5, size=num_points)
y = np.random.uniform(low=-5, high=5, size=num_points)
z = 0.5 * x - 0.3 * y + 2 + 0.1 * np.random.normal(size=num_points)
points = np.column_stack((x, y, z))
# 定义拟合的平面模型
def plane_model(params, x, y):
a, b, c = params
return a * x + b * y + c
# 定义残差函数
def residual(params, x, y, z):
return plane_model(params, x, y) - z
# 初始参数猜测
initial_params = [1.0, -1.0, 1.0]
# 使用最小二乘法拟合平面到LiDAR点云
result = least_squares(residual, initial_params, args=(x, y, z))
# 提取拟合结果
fit_params = result.x
# 可视化结果
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(x, y, z, c='b', marker='.', label='Original Point Cloud')
x_fit = np.linspace(-5, 5, 100)
y_fit = np.linspace(-5, 5, 100)
x_fit, y_fit = np.meshgrid(x_fit, y_fit)
z_fit = plane_model(fit_params, x_fit, y_fit)
ax.plot_surface(x_fit, y_fit, z_fit, alpha=0.5, color='r', label='Fitted Plane')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('Fitting a Plane to LiDAR Point Cloud')
plt.legend(handles=[mpatches.Patch(color='r', label='Fitted Plane')])
plt.show()
上述代码的实现流程如下:
- 首先,通过NumPy和Matplotlib库生成了一个模拟的LiDAR点云数据,包括x、y和z坐标。这些数据点模拟了一个平面,加上了一些噪声。
- 接着,定义了拟合的平面模型。这个平面模型是一个线性方程,其中a * x + b * y + c 表示平面方程。
- 然后,定义了残差函数。残差函数用于计算模型预测值与实际观测值之间的差异,即残差。
- 在拟合之前,提供了初始参数的猜测,即initial_params = [1.0, -1.0, 1.0]。
- 使用scipy.optimize.least_squares函数进行最小二乘法拟合。这个函数通过最小化残差来找到最优的参数,即能够最好地拟合LiDAR点云的平面。
- 提取了拟合结果,即拟合得到的平面的参数。
- 最后,通过Matplotlib可视化工具,创建了一个3D散点图显示原始LiDAR点云,并在同一图中绘制了拟合得到的平面,如图2-4所示。为了解决Matplotlib版本问题,使用了mpatches.Patch指定了图例的颜色。
图2-4 可视化原始点云和拟合的平面
2.2.5 曲面重建(Surface Reconstruction)算法
曲面重建的目的是基于点云数据重建物体表面的几何模型。用到的算法有:
1. 基于网格的曲面重建算法
基于网格的曲面重建算法利用点云数据生成三角网格模型,并通过网格上的插值或拟合来获得曲面模型的一种方法。一个常见的实现方式是使用Delaunay 三角剖分。
2. 基于体素的曲面重建(例如Marching Cubes算法)算法
基于网格的曲面重建(例如Poisson重建)算法通常将点云数据划分为体素(三维像素)的网格,然后在每个体素内估计曲面形状。一个常见的实现方式是使用体素网格中的点云数据进行插值或拟合。例如下面是一个简单的例子,演示了自定义实现基于体素的曲面重建算法,并在LiDAR点云数据上应用的过程。
实例2-5:使用基于体素的曲面重建算法处理LiDAR点云数据(codes/2/tisu.py)
实例文件tisu.py的具体实现代码如下所示。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.interpolate import griddata
def voxel_surface_reconstruction(points, voxel_size=0.1):
# 计算每个点所属的体素索引
voxel_indices = (points[:, :2] / voxel_size).astype(int)
# 创建体素网格
voxel_grid = {}
# 将点分配到体素中
for i, point in enumerate(points):
index = tuple(voxel_indices[i])
if index not in voxel_grid:
voxel_grid[index] = [point]
else:
voxel_grid[index].append(point)
# 对每个体素内的点进行插值或拟合
voxel_surfaces = []
for index, voxel_points in voxel_grid.items():
if len(voxel_points) > 0:
voxel_center = np.mean(voxel_points, axis=0)
voxel_surfaces.append(voxel_center)
return np.array(voxel_surfaces)
# 生成模拟的LiDAR点云数据
np.random.seed(42)
num_points = 1000
x = np.random.uniform(low=-5, high=5, size=num_points)
y = np.random.uniform(low=-5, high=5, size=num_points)
z = 0.5 * x + 0.3 * y + 2 + 0.1 * np.random.normal(size=num_points)
points = np.column_stack((x, y, z))
# 应用基于体素的曲面重建算法
voxel_surfaces = voxel_surface_reconstruction(points, voxel_size=0.1)
# 可视化结果
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(points[:, 0], points[:, 1], points[:, 2], c='b', marker='.', label='Original LiDAR Point Cloud')
ax.scatter(voxel_surfaces[:, 0], voxel_surfaces[:, 1], voxel_surfaces[:, 2], c='r', marker='.', label='Surface Reconstruction')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('LiDAR Point Cloud Surface Reconstruction using Voxel-based Method')
plt.legend()
plt.show()
上述代码的实现流程如下:
- 首先,定义了函数voxel_surface_reconstruction(),该函数接收LiDAR点云数据和体素大小作为输入。函数计算每个点所属的体素索引,然后创建体素网格并将点分配到体素中。对每个体素内的点进行插值或拟合,最后得到曲面的估计。
- 然后,生成了一个简单的模拟LiDAR点云数据,其中包括x、y和z坐标。这些数据模拟了一个平面,并且添加了一些噪声。
- 接着,调用函数voxel_surface_reconstruction(),对生成的LiDAR点云数据执行基于体素的曲面重建。
- 最后,使用Matplotlib创建了一个3D散点图,展示原始LiDAR点云和基于体素的曲面重建结果。如图2-5所示。这个可视化结果展示了基于体素的曲面重建算法的效果。
图2-5 可视化原始LiDAR点云和基于体素的曲面重建结果
2.2.6 配准(Registration)算法
配准的目的是将多个时刻或不同传感器获取的点云进行匹配,实现点云的对齐。实现配准功能的常用算法是Iterative Closest Point(ICP),ICP通过迭代最小化两点云之间的误差来实现配准。ICP的主要思想是通过迭代的方式优化两组点云之间的刚性变换,使它们最好地对齐。例如下面是一个简单的例子,功能是使用自定义实现的ICP 算法处理LiDAR 点云数据。
实例2-6:使用ICP算法处理LiDAR点云数据(codes/2/icp.py)
实例文件icp.py的具体实现代码如下所示。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def icp(source_points, target_points, max_iterations=100, tolerance=1e-4):
"""
Iterative Closest Point (ICP)算法用于点云配准。
参数:
- source_points (numpy array): 源点云(Nx3)。
- target_points (numpy array): 目标点云(Nx3)。
- max_iterations (int): 最大迭代次数。
- tolerance (float): 收敛容差。
返回:
- transformation_matrix (numpy array): 3x3 变换矩阵。
"""
transformation_matrix = np.identity(3)
for iteration in range(max_iterations):
# 寻找源点云和目标点云之间的最近邻点
distances, indices = find_nearest_neighbors(source_points, target_points)
# 提取对应的点对
source_matched = source_points[indices]
target_matched = target_points
# 使用奇异值分解(SVD)计算变换矩阵
transformation_matrix = calculate_transformation_matrix(source_matched, target_matched)
# 将变换应用于源点云
source_points = apply_transformation(source_points, transformation_matrix)
# 检查收敛
if np.abs(distances.mean()) < tolerance:
break
return transformation_matrix
def find_nearest_neighbors(source_points, target_points):
# 使用简单的最近邻搜索
distances = np.linalg.norm(source_points[:, np.newaxis, :] - target_points, axis=2)
indices = np.argmin(distances, axis=1)
return distances, indices
def calculate_transformation_matrix(source_points, target_points):
# 使用奇异值分解(SVD)计算变换矩阵
source_centered = source_points - source_points.mean(axis=0)
target_centered = target_points - target_points.mean(axis=0)
matrix = target_centered.T @ source_centered
u, _, vh = np.linalg.svd(matrix)
rotation_matrix = vh.T @ u.T
translation_vector = target_points.mean(axis=0) - source_points.mean(axis=0)
transformation_matrix = np.identity(3)
transformation_matrix[:2, :2] = rotation_matrix
transformation_matrix[:2, 2] = translation_vector[:2]
return transformation_matrix
def apply_transformation(points, transformation_matrix):
# 将变换应用于点
homogeneous_points = np.column_stack((points, np.ones(len(points))))
transformed_points = homogeneous_points @ transformation_matrix.T
return transformed_points[:, :2]
# 生成模拟的LiDAR点云数据
np.random.seed(42)
num_points = 100
theta = np.linspace(0, 2*np.pi, num_points)
source_points = np.column_stack((np.cos(theta), np.sin(theta)))
# 旋转并平移源点云生成目标点云
rotation_matrix = np.array([[np.cos(np.pi/4), -np.sin(np.pi/4)],
[np.sin(np.pi/4), np.cos(np.pi/4)]])
translation_vector = np.array([2.0, 1.0])
target_points = source_points.dot(rotation_matrix.T) + translation_vector
# 添加噪声到源和目标点云
source_points += 0.1 * np.random.randn(num_points, 2)
target_points += 0.1 * np.random.randn(num_points, 2)
# 将源点云旋转45度作为初始猜测
initial_guess_matrix = np.array([[np.cos(np.pi/4), -np.sin(np.pi/4)],
[np.sin(np.pi/4), np.cos(np.pi/4)]])
# 应用ICP算法进行点云配准
final_transformation = icp(source_points, target_points, max_iterations=100, tolerance=1e-4)
# 应用最终变换到源点云
registered_source = apply_transformation(source_points, final_transformation)
# 可视化结果
plt.figure(figsize=(8, 8))
plt.scatter(target_points[:, 0], target_points[:, 1], c='b', label='Target Point Cloud', alpha=0.7)
plt.scatter(registered_source[:, 0], registered_source[:, 1], c='r', label='Registered Source Point Cloud', alpha=0.7)
plt.title('LiDAR Point Cloud Registration using ICP')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.show()
上述代码的实现流程如下:
- 首先,自定义创建了函数icp(),将接收源点云和目标点云作为输入,并通过迭代优化的方式计算点云之间的刚性变换。这个算法包括查找最近邻点、计算变换矩阵和应用变换等步骤。
- 然后,生成了一个简单的模拟LiDAR点云数据,其中 source_points 是一个环形的点云,target_points 是通过旋转和平移对 source_points 进行变换生成的目标点云。
- 接着,应用了ICP算法进行点云配准。在这个例子中,使用源点云旋转45度作为初始猜测,并进行最多100次迭代。
- 最后,使用 Matplotlib创建了一个散点图,展示目标点云和通过ICP算法配准的源点云。如图2-6所示。
图2-6 可视化点云