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

open3d点云配准函数registration_icp

news 2025/7/19 0:29:14

文章目录

- 基本原理
- open3d调用
- 绘图

基本原理

ICP, 即Iterative Closest Point, 迭代点算法。

ICP算法有多种形式，其中最简单的思路就是比较点与点之间的距离，对于点云 $P=\{p_i\}, Q=\{q_i\}$ 而言，如果二者是同一目标，通过旋转、平移等操作可以实现重合的话，那么只需要固定 $Q$ 而不断地旋转或平移 $P$ ，最终二者一定能最完美地重合。

设旋转 $P$ 的矩阵为 $R$ ，平移矩阵为 $t$ ，在完美匹配的情况下，必有 $q_i = Rp_i + t$ 。

又因三维点云不具备栅格特征，故而很难保证 $q_i$ 和 $p_i$ 是同一点，所以要使得目标函数最小化

$min⁡R,t12∑i=1n∥qi−Rpi−t∥2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}\Vert q_i-Rp_i-t\Vert^2$

1992年Chen和Medioni对此方案进行了改进，提出了点对面的预估方法，其目标函数为

$min⁡R,t12∑i=1n[(qi−Rpi)⋅np]2\argmin_{R,t}\frac{1}{2}\sum^n_{i=1}[(q_i-Rp_i)\cdot n_p]^2$

其中 $n_p$ 是点 $p$ 的法线，这种方案显然效率更高。

open3d调用

open3d中实现了ICP算法，参数如下

registration_icp(source, target, max_correspondence_distance, init, estimation_method, criteria)

source为点云 $P$ ，target为目标点云 $Q$ ，max_correspondence_distance为匹配点在未匹配时的最大距离，init为初始变化矩阵，默认为单位矩阵；criteria为精度。

estimation_method可以理解为上面提到的两种方案，下面选择点对点ICP方法进行计算

import numpy as np
import open3d as o3dpipreg = o3d.pipelines.registrationpcd = o3d.data.DemoICPPointClouds()
src = o3d.io.read_point_cloud(pcd.paths[0])
tar = o3d.io.read_point_cloud(pcd.paths[1])
th = 0.02
trans_init = np.array([[0.862, 0.011, -0.507, 0.5], [-0.139, 0.967, -0.215, 0.7],[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])reg = pipreg.registration_icp(src, tar, th, trans_init,pipreg.TransformationEstimationPointToPoint())print(reg.transformation)
''' 变换矩阵
[[ 0.83924644  0.01006041 -0.54390867  0.64639961][-0.15102344  0.96521988 -0.21491604  0.75166079][ 0.52191123  0.2616952   0.81146378 -1.50303533][ 0.          0.          0.          1.        ]]
'''
print(reg)

print(reg)的返回信息如下，表示点云配准的拟合程度

RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056 Access transformation to get result.

绘图

为了对比配准前后的区别，对src和tar放在图中对比

import copy
srcDraw = copy.deepcopy(src)
tarDraw = copy.deepcopy(tar)
srcDraw.paint_uniform_color([1, 1, 0])
tarDraw.paint_uniform_color([0, 1, 1])
srcDraw.transform(tf)
o3d.visualization.draw_geometries([srcDraw, tarDraw])

此为原图，可以看到两组点云完全是错位的

在这里插入图片描述

srcDraw = copy.deepcopy(src)
tarDraw.paint_uniform_color([0, 1, 1])
srcDraw.transform(reg.transformation)
o3d.visualization.draw_geometries([srcDraw, tarDraw])