Open3d学习计划——高级篇 2(彩色点云配准)
本教程演示了一种同时使用几何和颜色进行配准的ICP变体。它实现了这篇文章的算法 [Park2017] ,实现了颜色信息锁定与切平面的对齐(The color information locks the alignment along the tangent plane)。这个算法与之前的ICP配准速度相当,但是实现了更高的精度和鲁棒性。本教程使用的符号来自ICP配准。
可视化函数
为了掩饰不同颜色点云之间的对齐,draw_registration_result_original_color使用原本的颜色可视化源点云.
def draw_registration_result_original_color(source, target, transformation):
source_temp = copy.deepcopy(source)
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target])
注意:这里原来的教程里可视化函数都加了初始视角之类的,但是很多人反映这个会报错,并且官方函数里也没给出可接受的参数,所以在这里把初始视角的参数都去掉了
输入
这段代码从两个文件中读取源点云和目标点云.使用单位阵作为初始化的配准矩阵.
print("1. Load two point clouds and show initial pose")
source = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
target = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_116.ply")
# draw initial alignment
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target, current_transformation)
- Load two point clouds and show initial pose
Point-to-plane ICP
我们首先使用 Point-to-plane ICP 作为一个基准算法.下面的可视化结果展示了未对其的绿色三角形纹理.这是因为几何约束不能够阻止两个平面滑动.
# point to plane ICP
current_transformation = np.identity(4)
print("2. Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. Distance threshold 0.02.")
result_icp = o3d.registration.registration_icp(
source, target, 0.02, current_transformation,
o3d.registration.TransformationEstimationPointToPlane())
print(result_icp)
draw_registration_result_original_color(source, target, result_icp.transformation)
- Point-to-plane ICP registration is applied on original point
clouds to refine the alignment. Distance threshold 0.02.
registration::RegistrationResult with fitness=9.745825e-01, inlier_rmse=4.220433e-03, and correspondence_set size of 62729
Access transformation to get result.
彩色点云配准
彩色点云配准的核心函数是 registration_colored_icp .
在这篇文章中,他使用的是具有联合优化目标的ICP迭代(细节请看 Point-to-point ICP):
E
(
T
)
=
(
1
−
δ
)
E
c
(
T
)
+
δ
E
G
(
T
)
\mathbf E(T) = (1-δ)E_c(T) + δE_G(T)
E(T)=(1−δ)Ec(T)+δEG(T)
这里的
T
\mathbf T
T 是被估计旋转矩阵.
E
C
\mathbf E_C
EC 和
E
G
\mathbf E_G
EG 分别是光度项和几何项.
δ
∈
[
0
,
1
]
\mathbf δ∈[0,1]
δ∈[0,1]是通过经验决定的权重变量.
这里的几何项
E
G
\mathbf E_G
EG 和 Point-to-plane ICP 的目标是相等的.
E
G
(
T
)
=
∑
(
p
,
q
)
∈
K
(
(
p
−
T
q
)
⋅
n
p
)
2
\mathbf E_G(T) = \sum_{(p,q)∈K} ((p-T_q)\cdot n_p)^2
EG(T)=(p,q)∈K∑((p−Tq)⋅np)2
这里的
K
\mathbf K
K 是当前迭代的对应集,
n
p
\mathbf n_p
np 是对应点
p
\mathbf p
p 的法线.
颜色项
E
C
\mathbf E_C
EC 测量的是
q
\mathbf q
q 点的颜色(用
C
(
q
)
\mathbf C(q)
C(q) 表示)与其在点
p
\mathbf p
p的切平面的投影上的颜色之间的差.
E
C
(
T
)
=
∑
(
p
,
q
)
∈
K
(
C
p
(
f
(
T
q
)
)
−
C
(
q
)
)
2
\mathbf E_C(T) = \sum_{(p,q)∈K}(C_p(f(Tq))-C(q))^2
EC(T)=(p,q)∈K∑(Cp(f(Tq))−C(q))2
这里的
C
p
(
⋅
)
\mathbf C_p(\cdot)
Cp(⋅) 是在
p
p
p 的切平面上连续定义的预计算函数. 函数
f
(
⋅
)
f(\cdot)
f(⋅) 将3D点投影到切平面.更多细节请参看 [Park2017].
为了提高效率, [Park2017]提供了多尺度的配准方案,已经在以下接口中实现.
# colored pointcloud registration
# This is implementation of following paper
# J. Park, Q.-Y. Zhou, V. Koltun,
# Colored Point Cloud Registration Revisited, ICCV 2017
voxel_radius = [0.04, 0.02, 0.01]
max_iter = [50, 30, 14]
current_transformation = np.identity(4)
print("3. Colored point cloud registration")
for scale in range(3):
iter = max_iter[scale]
radius = voxel_radius[scale]
print([iter, radius, scale])
print("3-1. Downsample with a voxel size %.2f" % radius)
source_down = source.voxel_down_sample(radius)
target_down = target.voxel_down_sample(radius)
print("3-2. Estimate normal.")
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
print("3-3. Applying colored point cloud registration")
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, radius, current_transformation,
o3d.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
print(result_icp)
draw_registration_result_original_color(source, target, result_icp.transformation)
- Colored point cloud registration
[50, 0.04, 0]
3-1. Downsample with a voxel size 0.04
3-2. Estimate normal.
3-3. Applying colored point cloud registration
registration::RegistrationResult with fitness=8.763667e-01, inlier_rmse=1.457778e-02, and correspondence_set size of 2084
Access transformation to get result.
[30, 0.02, 1]
3-1. Downsample with a voxel size 0.02
3-2. Estimate normal.
3-3. Applying colored point cloud registration
registration::RegistrationResult with fitness=8.661842e-01, inlier_rmse=8.759721e-03, and correspondence_set size of 7541
Access transformation to get result.
[14, 0.01, 2]
3-1. Downsample with a voxel size 0.01
3-2. Estimate normal.
3-3. Applying colored point cloud registration
registration::RegistrationResult with fitness=8.437191e-01, inlier_rmse=4.851480e-03, and correspondence_set size of 24737
Access transformation to get result.
使用 voxel_down_sample 创造了三层多分辨率的点云.使用顶点法线估计来计算的法线.核心的配准函数 registration_colored_icp 在每一层从粗糙到精细都有调用.lambda_geometric 是 registration_colored_icp 中可选的参数,用于确定
(
1
−
δ
)
E
c
+
δ
E
G
\mathbf (1-δ)E_c + δE_G
(1−δ)Ec+δEG 中的
δ
∈
[
0
,
1
]
δ∈[0,1]
δ∈[0,1].
输出的是两组紧密对齐的点云,注意看上面的绿色三角形.