二维ICP配准

傷城~ 2022-11-01 04:19 261阅读 0赞

### 二维ICP配准 ###

*  1. 理论
 *   *  1.1 ICP算法
     *  1.2 目标函数
     *  1.3 简化目标函数
     *  1.4 变换矩阵求解
 *  2. Python实现
 *   *  2.1 导入模块
     *  2.2. 定义相关函数
     *   *  2.2.1 求解二维点间距函数
         *  2.2.2 求解二维点距离点集最近点函数
         *  2.2.3 求解二维点集间平均距离函数
     *  2.3. 定义ICP\_2D
     *  2.4. 定义点云
     *  2.5. ICP 配准

--------------------

# 1. 理论 #

--------------------

## 1.1 ICP算法 ##

ICP算法本质上是基于最小二乘法的最优配准方法

重复进行选择对应关系点对， 计算最优刚体变换\[R, T\]  
直到满足正确配准的收敛精度要求

即目的就是要找到待配准点云数据与参考云数据之间的旋转矩阵R和平移矩阵 T  
使得两点数据之间满足某种度量准则下的最优匹配

--------------------

## 1.2 目标函数 ##

假设两帧点云A与B，将A 称为参考点云（也就是目标点云），把 B 称为源点云  
在机器人定位的时候，A就为地图上的点云，B为设备感知的点云

如何定义两个点云匹配误差呢  
一般定义所有最近点的欧式距离平方和为目标函数  
![在这里插入图片描述][20210224204530638.png_pic_center]  
平时要先去噪点，这里忽略

--------------------

## 1.3 简化目标函数 ##

上式存在两个变量，比较难处理  
这时可通过**归一化**，就可先 **不考虑平移矩阵 T**  
![在这里插入图片描述][20210224204418493.png_pic_center]  
那么目标函数则为：  
![在这里插入图片描述][20210224204700823.png_pic_center]

分解目标函数可得：  
![在这里插入图片描述][20210224205305248.png_pic_center]  
即使函数G最大即可  
![在这里插入图片描述][20210224205525219.png_pic_center]

--------------------

## 1.4 变换矩阵求解 ##

现在取旋转矩阵R和平移矩阵 T  
![在这里插入图片描述][20210224205849436.png_pic_center]  
则函数G：  
![在这里插入图片描述][20210224212131110.png_pic_center]

对函数G进行 `theta` 求极值，且令其为0，则有

![在这里插入图片描述][20210224212708346.png_pic_center]  
只要求出：  
![在这里插入图片描述][20210224212819648.png_pic_center]  
就可以求出旋转角 `theta`  
求出`theta`即可得到旋转矩阵R

再根据旋转矩阵R，求平移矩阵 T  
![在这里插入图片描述][2021022421304618.png_pic_center]  
然后不断迭代，至人为目标距离足够小即可

--------------------

# 2. Python实现 #

--------------------

## 2.1 导入模块 ##

import numpy as np
    import math
    
    
    # 不以科学计数显示
    np.set_printoptions(suppress=True)

--------------------

## 2.2. 定义相关函数 ##

--------------------

### 2.2.1 求解二维点间距函数 ###

def GetDistOfPoints2D(p1, p2):
        ''' 求出两个点的距离 '''
        return math.sqrt(math.pow(p2[0] - p1[0], 2) + math.pow(p2[1] - p1[1], 2))

--------------------

### 2.2.2 求解二维点距离点集最近点函数 ###

def GetClosestID(p, p_set):
        ''' 在p_set点集中找到距p最近点的id '''
        id = 0
        min = float("inf")  # 初始化取最大值
        for i in range(p_set.shape[1]):
            dist = GetDistOfPoints2D(p, p_set[:, i])
            if dist < min:
                id = i
                min = dist
        return id

--------------------

### 2.2.3 求解二维点集间平均距离函数 ###

def GetDistOf2DPointsSet(set1, set2):
        ''' 求两个点集之间的平均点距 '''
        loss = 0
        for i in range(set1.shape[1]):
            id = GetClosestID(set1[:, i], set2)
            dist = GetDistOfPoints2D(set1[:, i], set2[:, id])
            loss += dist
        return loss/set1.shape[1]

--------------------

## 2.3. 定义ICP\_2D ##

def ICP_2D(targetPoints, sourcePoints):
        ''' 二维 ICP 配准算法 '''
        A = targetPoints  # A是目标点云（地图值）
        B = sourcePoints  # B是源点云（感知值）
    
        # 初始化迭代参数
        iteration_times = 0  # 迭代次数
        dist_now = 1  # A,B两点集之间初始化距离
        dist_improve = 1  # A,B两点集之间初始化距离提升
        dist_before = GetDistOf2DPointsSet(A, B)  # A,B两点集之间距离
    
        # 初始化 R，T
        R = np.identity(2)
        T = np.zeros((2, 1))
    
        # 均一化点云
        A_mean = np.mean(A, axis=1).reshape((2, 1))
        A_ = A - A_mean
    
        # 迭代次数小于10并且距离大于0.01时，继续迭代
        while iteration_times < 10 and dist_now > 0.01:
    
            # 均一化点云
            B_mean = np.mean(B, axis=1).reshape((2, 1))
            B_ = B - B_mean
    
            # t_nume表示角度公式中的分子 t_deno表示分母
            t_nume, t_deno = 0, 0
    
            # 对源点云B中每个点进行循环
            for i in range(B_.shape[1]):
                j = GetClosestID(B_[:, i], A_)  # 找到距离最近的目标点云A点id
                t_nume += A_[1][j] * B_[0][i] - A_[0][j] * B_[1][i]  # 获得求和公式，分子的一项
                t_deno += A_[0][j] * B_[0][i] + A_[1][j] * B_[1][i]  # 获得求和公式，分母的一项
    
            # 计算旋转弧度θ
            theta = math.atan2(t_nume, t_deno)
    
            # 由旋转弧度θ得到旋转矩阵Ｒ
            delta_R = np.array([[math.cos(theta), -math.sin(theta)], [math.sin(theta), math.cos(theta)]])
    
            # 计算平移矩阵Ｔ
            delta_T = A_mean - np.matmul(delta_R, B_mean)
    
            # 更新最新的点云
            B = np.matmul(delta_R, B) + delta_T
    
            # 更新旋转矩阵Ｒ和平移矩阵Ｔ
            R = np.matmul(delta_R, R)
            T = np.matmul(delta_R, T) + delta_T
    
            # 更新迭代
            iteration_times += 1  # 迭代次数+1
            dist_now = GetDistOf2DPointsSet(A, B)  # 更新两个点云之间的距离
            dist_improve = dist_before - dist_now  # 计算这一次迭代两个点云之间缩短的距离
            dist_before = dist_now  # 将"现在距离"赋值给"以前距离"
    
            # 打印迭代次数、损失距离、损失提升
            print("迭代：第{}次，距离：{:.2f}，缩短：{:.2f}".format(iteration_times, dist_now, dist_improve))
    
        return R, T

--------------------

## 2.4. 定义点云 ##

# 目标点云（地图上的数据）
    a = np.array([[1, 2, 2], [1, 2, 3]])
    
    # 设置转换量
    c_theta = math.radians(30)  # 逆时针旋转30° 求解R旋转角度应为顺时针旋转30°
    c_r = np.array([[math.cos(c_theta), -math.sin(c_theta)], [math.sin(c_theta), math.cos(c_theta)]])
    c_t = np.array([[6], [-0.6]])  # 偏移（6，-0.6） 求解T应为（-6，0.6）
    
    # 源点云（假设检测到的数据）
    b = np.matmul(c_r, a + c_t)
    
    # ICP 配准
    r, t = ICP_2D(a, b)
    
    # 源点云（假设检测到的数据）
    b = np.matmul(c_r, a + c_t)

--------------------

## 2.5. ICP 配准 ##

# ICP 配准
    r, t = ICP_2D(a, b)
    
    # 显示参数
    print("A:\n", a)
    print("\nB:\n", b)
    print("\nR:\n", r)
    print("\nT:\n", t)
    print("\nNew B:\n", np.matmul(r, b) + t)
    input("\n输入任意键退出")
    
    # 迭代：第1次，距离：0.00，缩短：4.62
    # A:
    # [[1 2 2]
    # [1 2 3]]
    
    # B:
    # [[5.86217783 6.22820323 5.72820323]
    # [3.84641016 5.21243557 6.07846097]]
    
    # R:
    # [[ 0.8660254 0.5 ]
    # [-0.5 0.8660254]]
    
    # T:
    # [[-6. ]
    # [ 0.6]]
    
    # New B:
    # [[1. 2. 2.]
    # [1. 2. 3.]]
    
    # 输入任意键退出

--------------------

完整代码：  
[CSDN ][CSDN]  
[Github][]

--------------------

谢谢

[20210224204530638.png_pic_center]: /images/20221024/9ef315aa72ae4b9ab701e16112554d25.png
[20210224204418493.png_pic_center]: /images/20221024/d9d9abc9315e48e3bc8e91e0e5f101f1.png
[20210224204700823.png_pic_center]: /images/20221024/f04257f4be9d48b19454da9cc4a0243d.png
[20210224205305248.png_pic_center]: /images/20221024/ab509bb3799247c99d086b17af48ae0d.png
[20210224205525219.png_pic_center]: /images/20221024/b529433927394147991c18b4e6ef2d06.png
[20210224205849436.png_pic_center]: /images/20221024/3393362f0daf4861a64b7b398290cd54.png
[20210224212131110.png_pic_center]: /images/20221024/3ba2cb3f8aa8414987dcf89c7a8a1764.png
[20210224212708346.png_pic_center]: /images/20221024/2c39436727014c18bde37e224fd3816f.png
[20210224212819648.png_pic_center]: /images/20221024/2d178f5ecb7f477eb4b4cf687d9f1da2.png
[2021022421304618.png_pic_center]: /images/20221024/af3bd5152d2e4c81a789c33ff34cfaa9.png
[CSDN]: https://codechina.csdn.net/qq_32618327/a-simple-explanation/-/blob/main/icp_2d.py
[Github]: https://github.com/JoveH-H/A-simple-explanation/blob/master/icp_2d.py