PythonRobotics机器人算法-SLAM算法之迭代最近点匹配(Iterative closest point matching,ICPM)


1.概述

P. J. Besl 和 N. D. McKay 描述了一种精确的、高效率的二维/三维形状配准方法,并且这种方法是通用的、与表示无关的,可以处理六自由度配准问题,我们称之为迭代最近点算法(Iterative closest point,ICP)。迭代最近点算法被广泛应用于多视点云拼接和图像配准中。由于该算法不断被学者改进并且广泛应用,为了与改进之后的迭代最近点算法相区分,我们在本文中称 P. J. Besl 和 N. D. McKay 提出的迭代最近点算法为基本迭代最近点算法,也即是基本 ICP 算法。这种三维形状配准方法可以用于多种不同的几何数据形式,例如点集、线段集(折线)、隐式曲线、参数曲线、三角集(用小平面表示的曲面)、隐式曲面、参数曲面。

2.迭代最近点匹配实例

下面展示一下基于python实现的迭代最近点匹配。

(1)定义初始参数

1
2
3
4
5
6
7
8
9
import math
import numpy as np
import matplotlib.pyplot as plt

#ICP parameters
EPS = 0.0001
MAXITER = 100

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
def ICP_matching(ppoints, cpoints):
    """
    Iterative Closest Point matching
    - input
    ppoints: 2D points in the previous frame
    cpoints: 2D points in the current frame
    - output
    R: Rotation matrix
    T: Translation vector
    """

    H = None  # homogeneraous transformation matrix
    dError = 1000.0
    preError = 1000.0
    count = 0
    while dError >= EPS:
        count += 1
        if show_animation:
            plt.cla()
            plt.plot(ppoints[0, :], ppoints[1, :], ".r")
            plt.plot(cpoints[0, :], cpoints[1, :], ".b")
            plt.plot(0.0, 0.0, "xr")
            plt.axis("equal")
            plt.pause(1.0)
        inds, error = nearest_neighbor_assosiation(ppoints, cpoints)
        Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints)
        # update current points
        cpoints = (Rt * cpoints) + Tt
        H = update_homogenerous_matrix(H, Rt, Tt)
        dError = abs(preError - error)
        preError = error
        print("Residual:", error)
        if dError <= EPS:
            print("Converge", error, dError, count)
            break
        elif MAXITER <= count:
            print("Not Converge...", error, dError, count)
            break
    R = np.matrix(H[0:2, 0:2])
    T = np.matrix(H[0:2, 2])
    return R, T

def update_homogenerous_matrix(Hin, R, T):
    H = np.matrix(np.zeros((3, 3)))
    H[0, 0] = R[0, 0]
    H[1, 0] = R[1, 0]
    H[0, 1] = R[0, 1]
    H[1, 1] = R[1, 1]
    H[2, 2] = 1.0
    H[0, 2] = T[0, 0]
    H[1, 2] = T[1, 0]
    if Hin is None:
        return H
    else:
        return Hin * H

def nearest_neighbor_assosiation(ppoints, cpoints):
    # calc the sum of residual errors
    dcpoints = ppoints - cpoints
    d = np.linalg.norm(dcpoints, axis=0)
    error = sum(d)
    # calc index with nearest neighbor assosiation
    inds = []
    for i in range(cpoints.shape[1]):
        minid = -1
        mind = float("inf")
        for ii in range(ppoints.shape[1]):
            d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i])
            if mind >= d:
                mind = d
                minid = ii
        inds.append(minid)
    return inds, error

def SVD_motion_estimation(ppoints, cpoints):
    pm = np.matrix(np.mean(ppoints, axis=1))
    cm = np.matrix(np.mean(cpoints, axis=1))
    pshift = np.matrix(ppoints - pm)
    cshift = np.matrix(cpoints - cm)
    W = cshift * pshift.T
    u, s, vh = np.linalg.svd(W)
    R = (u * vh).T
    t = pm - R * cm
    return R, t

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
def main():
    print(_ _file__ + " start!!")
    # simulation parameters
    nPoint = 10
    fieldLength = 50.0
    motion = [0.5, 2.0, math.radians(-10.0)]  # movement [x[m],y[m],yaw[deg]]
    nsim = 3  # number of simulation
    for _ in range(nsim):
        # previous points
        px = (np.random.rand(nPoint) - 0.5) * fieldLength
        py = (np.random.rand(nPoint) - 0.5) * fieldLength
        ppoints = np.matrix(np.vstack((px, py)))
        # current points
        cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0]
              for (x, y) in zip(px, py)]
        cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1]
              for (x, y) in zip(px, py)]
        cpoints = np.matrix(np.vstack((cx, cy)))
        R, T = ICP_matching(ppoints, cpoints)

if _ _ name__ == '_ _main__':
    main()

执行结果如下。下面为基于奇异值分解的二维迭代最近点匹配的例子。该算法能计算从一些点到另一些点的旋转矩阵和平移矩阵。

参考文献

  1. Introduction to Mobile Robotics: Iterative Closest Point Algorithm
  2. 基于鲁棒统计学方法的迭代最近点算法研究
  3. Besl P J, McKay N D. Method for registration of 3-D shapes[C]//Sensor Fusion IV: Control Paradigms and Data Structures. International Society for Optics and Photonics, 1992, 1611: 586-607.
  4. https://pythonrobotics.readthedocs.io/en/latest/
  5. https://github.com/AtsushiSakai/PythonRobotics/
  6. PythonRobotics: a Python code collection of robotics algorithms

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-slam-icpm/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。