目录
Open3D点云算法汇总及实战案例汇总的目录地址:
Open3D点云算法与点云深度学习案例汇总(长期更新)-CSDN博客
一、概述
GICP(Generalized ICP)是一种用于点云配准的改进ICP算法。它综合了点对点ICP和点对面ICP的优点,通过引入协方差矩阵,来更准确地描述点的局部几何特性,从而提高配准精度。GICP主要应用于机器人定位、三维重建和计算机视觉等领域。
GICP算法将每个点云中的点视为带有协方差的高斯分布,这样可以更准确地描述点的局部几何形状。配准的目标是通过迭代优化,使得源点云与目标点云之间的距离最小化。
1.1实现步骤
- 初始化:选择一个初始变换(通常是单位变换)将源点云与目标点云进行初步对齐。
- 协方差矩阵计算:对于源点云和目标点云中的每个点,计算其邻域点的协方差矩阵。协方差矩阵描述了该点的局部几何形状。
- 最近点配对:对于源点云中的每个点,找到目标点云中的最近点作为对应点。
- 误差计算:使用基于协方差的马氏距离(Mahalanobis Distance)来计算源点与对应点之间的误差。
- 最小化误差:通过优化方法最小化误差,获得新的刚体变换(旋转和平移)。
- 应用变换:对源点云应用计算得到的刚体变换。
- 迭代:重复步骤3到步骤6,直到误差收敛或达到最大迭代次数。
1.2数学描述
二、代码实现
import open3d as o3d
import numpy as np
import copy
def draw_registration_result(src, tar, transformation):
source_temp = copy.deepcopy(src) # 由于函数transformand paint_uniform_color会更改点云,
target_temp = copy.deepcopy(tar) # 因此调用copy.deepcoy进行复制并保护原始点云。
source_temp.paint_uniform_color([1, 0, 0]) # 点云着色
target_temp.paint_uniform_color([0, 1, 0])
source_temp.transform(transformation)
# o3d.io.write_point_cloud("trans_of_source.pcd", source_temp) # 保存点云
o3d.visualization.draw_geometries([source_temp, target_temp],
window_name="GICP配准结果", width=800, height=600)
if __name__ == "__main__":
# --------------------读取点云数据-------------------
source = o3d.io.read_point_cloud("hand.pcd")
target = o3d.io.read_point_cloud("hand_trans.pcd")
# ------------------可视化点云初始位置---------------
o3d.visualization.draw_geometries([source, target], window_name="原始点云的位置", width=800, height=600)
threshold = 1 # 距离阈值
trans_init = np.asarray([[0.98194534, -0.18295687, -0.04806395, 0.65088957],
[0.11626176, 0.78413388, -0.60960419, 4.19087836],
[0.14921985, 0.59300999, 0.79124749, 0.42555584],
[0, 0, 0, 1]]) # 初始变换矩阵,一般由粗配准提供
# -------------------------------------------------
# 计算两个重要指标,fitness计算重叠区域(内点对应关系/目标点数)。越高越好。
# inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
print("Initial alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)
print(evaluation) # 这里输出的是初始位置的 fitness和RMSE
print("Apply GeneralizedICP")
generalized_icp = o3d.pipelines.registration.registration_generalized_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=35)) # 设置最大迭代次数
print(generalized_icp) # 输出GeneralizedICP相关信息
print("Transformation is:")
print(generalized_icp.transformation) # 输出变换矩阵
# -----------------可视化配准结果--------------------
draw_registration_result(source, target, generalized_icp.transformation)
三、实现效果
3.1原始点云
3.2配准后点云
3.3计算数据
Initial alignment
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.838722e-01, and correspondence_set size of 327323
Access transformation to get result.
Apply GeneralizedICP
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.661125e-07, and correspondence_set size of 327323
Access transformation to get result.
Transformation is:
[[ 1.00000001e+00 1.60936690e-09 6.03521144e-09 -3.66212183e-08]
[ 8.59615544e-10 7.07106783e-01 -7.07106785e-01 4.99999999e+00]
[-1.99964480e-09 7.07106784e-01 7.07106779e-01 1.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]