文章目录
- ICP算法
- 鲁棒核
- ICP测试
ICP算法
ICP, 即Iterative Closest Point, 迭代点算法。
ICP算法有多种形式，其中最简单的思路就是比较点与点之间的距离，对于点云 P = { p i } , Q = { q i } P=\{p_i\}, Q=\{q_i\} P={pi},Q={qi}而言，如果二者是同一目标，通过旋转、平移等操作可以实现重合的话，那么只需要固定 Q Q Q而不断地旋转或平移 P P P，最终二者一定能最完美地重合。
设 R , t R, t R,t分别为旋转矩阵和平移矩阵，在完美匹配的情况下，必有 q i = R p i + t q_i = Rp_i + t qi=Rpi+t。但三维点云不具备栅格特征，故而很难保证 q i q_i qi和 p i p_i pi一一对应，所以求解问题变为优化问题，目标函数为
arg min R , t 1 2 ∑ i = 1 n ∥ q i − R p i − t ∥ 2 \argmin_{R,t}\frac{1}{2}\sum^n_{i=1}\Vert q_i-Rp_i-t\Vert^2 R,targmin21i=1∑n∥qi−Rpi−t∥2
1992年Chen和Medioni对此方案进行了改进，提出了点对面的预估方法，其目标函数为
arg min R , t 1 2 ∑ i = 1 n [ ( q i − R p i ) ⋅ n p ] 2 \argmin_{R,t}\frac{1}{2}\sum^n_{i=1}[(q_i-Rp_i)\cdot n_p]^2 R,targmin21i=1∑n[(qi−Rpi)⋅np]2
其中 n p n_p np是点 p p p的法线，这种方案显然效率更高。
在使用ICP算法前后，两个点云的叠加图像变化如下
基于Open3d实现的代码如下
import numpy as np
import open3d as o3d
pipreg = o3d.pipelines.registration
pcd = 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) # 变换矩阵
print(reg) # 表示点云配准的拟合程度
'''
fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056 Access transformation to get result.
'''
【registration_icp】即为open3d实现的点云配准函数，在示例中，输入的5个参数分别为点云 P P P、目标点云 Q Q Q、同名点未匹配时的最大距离、初始变化矩阵、变换方法。
【TransformationEstimationPointToPoint】即点对点的目标函数。
匹配完成后，打印的变换矩阵为
[ 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. ] ] \begin{bmatrix} 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. ]\\ \end{bmatrix} 0.83924644−0.151023440.521911230.0.010060410.965219880.26169520.−0.54390867−0.214916040.811463780.0.646399610.75166079−1.503035331.]
绘图代码如下
from copy import deepcopy
srcDraw = deepcopy(src)
tarDraw = deepcopy(tar)
srcDraw.paint_uniform_color([1, 1, 0])
tarDraw.paint_uniform_color([0, 1, 1])
srcDrawICP = deepcopy(src)
tarDrawICP = deepcopy(tarDraw)
srcDrawICP.transform(reg.transformation)
geo = [srcDraw, tarDraw,
srcDrawICP.translate((4.5, 0, 0)),
tarDrawICP.translate((4.5, 0, 0))]
o3d.visualization.draw_geometries(geo)
鲁棒核
现有点云 P , Q P,Q P,Q，若二者存在一一对应的点列 p i ∈ P , q i ∈ Q p_i\in P, q_i\in Q pi∈P,qi∈Q，通过对 Q Q Q进行矩阵变换 T T T，以期二者完全配准，那么对于第 i i i个点而言，记 r i ( T ) = ( p i − T q i ) ⋅ n p i r_i(T)=(p_i-Tq_i)\cdot n_{p_i} ri(T)=(pi−Tqi)⋅npi， n p i n_{p_i} npi为点 p i p_i pi的法向量，即 q i q_i qi在经过矩阵变换后与 p i p_i pi在其法向量方向的残差。
则点对面ICP的目的，就是让下面的函数取值最小
E ( T ) = ∑ i = 1 N r i ( T ) 2 E(T)=\sum_{i=1}^Nr_i(T)^2 E(T)=i=1∑Nri(T)2
在这个优化问题中， r i r_i ri的作用举足轻重，任何对 r i r_i ri的形式上的更改，都会直接影响优化结果，例如将这个优化问题改写成重复加权最小二乘法的形式
E ( T ) = ∑ i = 1 N w i r i ( T ) 2 E(T)=\sum_{i=1}^Nw_ir_i(T)^2 E(T)=i=1∑Nwiri(T)2
让 r i r_i ri乘上一个权重，那么在具体匹配的过程中，就可以降低某些特殊值的影响。如果这个权重本身就是 r i r_i ri的函数，那么加权过程可以写成更加抽象的形式
E ( T ) = ∑ i = 1 N ρ [ r i ( T ) ] E(T)=\sum_{i=1}^N\rho[r_i(T)] E(T)=i=1∑Nρ[ri(T)]
ρ \rho ρ可称为Robust核函数，open3d中封装了如下和函数
| 核函数 | 封装 | |
|---|---|---|
| L1损失 | L1Loss | ω ( r ) = 1 ∣ r ∣ → ρ ( r ) = ∣ r ∣ \omega(r)=\frac1{\vert r\vert }\to\rho(r)=\vert r\vert ω(r)=∣r∣1→ρ(r)=∣r∣ |
| L2损失 | L2Loss | ω ( r ) = 1 → ρ ( r ) = r 2 2 \omega(r)=1\to\rho(r)=\frac{r^2}2 ω(r)=1→ρ(r)=2r2 |
| 柯西核 | CauchyLoss | ω ( r ) = 1 1 + ( r k ) 2 → ρ ( r ) = k 2 2 log ( 1 + ( r k ) 2 ) \omega(r)=\frac1{1+(\frac r k)^2}\to \rho(r)=\frac{k^2}{2}\log(1+(\frac r k)^2) ω(r)=1+(kr)21→ρ(r)=2k2log(1+(kr)2) |
| GM核 | GMLoss | ω ( r ) = k ( k + r 2 ) 2 → ρ ( r ) = r 2 / 2 k + r 2 \omega(r)=\frac{k}{(k+r^2)^2}\to\rho(r)=\frac{r^2/2}{k+r^2} ω(r)=(k+r2)2k→ρ(r)=k+r2r2/2 |
| Huber核 | HuberLoss | |
| Tukey核 | TukeyLoss |
Huber核以及Tukey核相对复杂，表示如下
- Huber核
ω ( r ) = { 1 ∣ r ∣ ⩽ k k ∣ r ∣ otherwise → ρ ( r ) = { r 2 2 ∣ r ∣ < k k ∣ r ∣ − k 2 2 otherwise \omega(r)=\begin{cases} 1&|r|\leqslant k\\ \frac{k}{|r|}& \operatorname{otherwise} \end{cases}\to \rho(r)=\begin{cases} \frac{r^2}{2} & |r|<k \\ k|r|-\frac{k^2}{2} & \operatorname{otherwise} \end{cases} ω(r)={1∣r∣k∣r∣⩽kotherwise→ρ(r)={2r2k∣r∣−2k2∣r∣<kotherwise
- Tukey核
ω ( r ) = { [ 1 − ( r k ) 2 ] 2 ∣ r ∣ ⩽ k 0 otherwise → ρ ( r ) = { k 2 { 1 − [ 1 − ( e k ) 2 ] 3 } 2 ∣ r ∣ < k r 2 2 otherwise \omega(r)=\begin{cases} [1-(\frac r k)^2]^2 &|r|\leqslant k\\ 0 & \operatorname{otherwise} \end{cases}\to \rho(r)=\begin{cases} \frac{k^2\big\{1-[1-(\frac e k)^2]^3\big\}}{2}& |r|<k \\ \frac{r^2}{2} & \operatorname{otherwise} \end{cases} ω(r)={[1−(kr)2]20∣r∣⩽kotherwise→ρ(r)=⎩ ⎨ ⎧2k2{1−[1−(ke)2]3}2r2∣r∣<kotherwise
除了L1Loss和L2Loss之外，其他损失函数均有参数 k k k，当 k k k设为0.5时，这四个和函数的图像为
绘图代码如下
import numpy as np
import matplotlib.pyplot as plt
import open3d as o3d
pipreg = o3d.pipelines.registration
kerrDct = {
"cuchy" : pipreg.CauchyLoss,
"GM" : pipreg.GMLoss,
"huber" : pipreg.HuberLoss,
"tukey" : pipreg.TukeyLoss
}
fig = plt.figure()
xs = np.linspace(-2,2,1000)
for i,key in enumerate(kerrDct):
kerr = kerrDct[key](0.5)
ys = [kerr.weight(x) for x in xs]
ax = fig.add_subplot(221+i)
ax.plot(xs, ys)
plt.title(key)
plt.show()
ICP测试
下面对鲁棒核的效果进行测试，首先打开测试点云，并添加一点噪声
demo = o3d.data.DemoICPPointClouds()
src = o3d.io.read_point_cloud(demo.paths[0])
tar = o3d.io.read_point_cloud(demo.paths[1])
pts = np.array(src.points)
# 添加正态分布的噪声
pts += np.random.normal(0, 0.1, size=pts.shape)
srcNoise = o3d.geometry.PointCloud()
srcNoise.points = o3d.utility.Vector3dVector(pts)
srcDraw = deepcopy(src)
o3d.visualization.draw_geometries([srcDraw.translate((-4.5,0,0)),
srcNoise])
加完噪声前后的点云图如下，几乎连轮廓都看不出来了
所以问题就是，右边那一坨五颜六色的东西真的可以和左边的图匹配到一起去吗？如果仍然使用传统的ICP算法，结果如下，可见完全没有配准
代码为
p2L = pipreg.TransformationEstimationPointToPlane()
regP2L = pipreg.registration_icp(srcNoise, tar, 0.5, trans_init, p2L)
srcP2L = deepcopy(src)
srcP2L.transform(regP2L.transformation)
srcP2L.paint_uniform_color([0, 1, 1])
o3d.visualization.draw_geometries([srcP2L, tar])
接下来，则是见证奇迹的时刻，采用Turkey核来重新配准一下，考虑到核函数的权重会随着距离增大而减小，最后缩减至0，所以阈值在icp函数中显得就不那么重要了，下面随便选一个非常不靠谱的大数10，然后看一下配准结果
loss = o3d.pipelines.registration.TukeyLoss(k=0.1)
turkey = pipreg.TransformationEstimationPointToPlane(loss)
regTurkey = pipreg.registration_icp(srcNoise, tar, 10, trans_init, turkey)
srcTurkey = deepcopy(src)
srcTurkey.transform(regTurkey.transformation)
srcTurkey.paint_uniform_color([0, 1, 1])
o3d.visualization.draw_geometries([srcTurkey, tar])
结果如下，可以说相当靠谱了。
