三维ICP-SVD配准

àì夳堔傛蜴生んèń 2022-11-03 00:45 326阅读 0赞

### 三维ICP-SVD配准 ###

*  1. 理论
 *  2. Python实现
 *   *  2.1 导入模块
     *  2.2. 定义ICP\_3D\_SVD
     *  2.3. 定义点云
     *  2.4. ICP\_3D\_SVD 配准

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

# 1. 理论 #

理论参考：

[二维ICP配准][ICP]  
[视觉SLAM笔记（38） 3D-3D: ICP][SLAM_38_ 3D-3D_ ICP]  
[视觉SLAM笔记（65） 简约总结][SLAM_65_]

利用奇异值分解SVD的前提是源点云和目标点云是互相配对好的  
源点云的第一点配对目标点云的第一点，源点云的第二点配对目标点云的第二点…

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

# 2. Python实现 #

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

## 2.1 导入模块 ##

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

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

## 2.2. 定义ICP\_3D\_SVD ##

def ICP_3D_SVD(targetPoints, sourcePoints):
        ''' ICP_3D_SVD ICP 配准算法 '''
        A = targetPoints  # A是目标点云（地图值）
        B = sourcePoints  # B是源点云（感知值）
    
        # 均一化点云
        A_mean = np.mean(A, axis=1).reshape((3, 1))
        B_mean = np.mean(B, axis=1).reshape((3, 1))
        A_ = A - A_mean
        B_ = B - B_mean
    
        # 奇异值SVD分解
        H = np.matmul(B_, A_.T)
        U, _, Vh = np.linalg.svd(H)
    
        # 显示SVD参数
        print("\nU:\n", U)
        print("\nV.T:\n", Vh)
    
        # 计算旋转矩阵Ｒ 和 平移矩阵Ｔ A > B
        R_a2b = np.matmul(U, Vh)
        T_a2b = B_mean - np.matmul(R_a2b, A_mean)
    
        # 逆变换
        R = R_a2b.T
        T = - np.matmul(R_a2b.T, T_a2b)
    
        return R, T

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

## 2.3. 定义点云 ##

# 目标点云（地图上的数据）
    a = np.array([[1, 2, 2, 4, 5], [4, 3, 3, 2, 5], [2, 5, 3, 1, 4]])
    
    # 设置转换量 B > A
    r_b2a = np.array([[0.99791909, -0.05138589, 0.03894846],
                      [0.05033962, 0.99835666, 0.02738435],
                      [-0.04029162, -0.02536671, 0.99886591]])
    t_b2a = np.array([[-0.1], [-0.2], [0.5]])
    
    # 源点云（假设检测到的数据）
    b = np.matmul(r_b2a.T, a - t_b2a)
    
    # 显示已配对好的点云参数（一一对应）
    print("A:\n", a)
    print("\nB:\n", b)
    
    # A:
    # [[1 2 2 4 5]
    # [4 3 3 2 5]
    # [2 5 3 1 4]]
    
    # B:
    # [[1.24869997 2.07540458 2.15598782 4.18206962 5.21013271]
    # [4.09852343 2.97268075 3.02341417 1.97301915 4.84060311]
    # [1.65615644 4.66431828 2.66658646 0.71936721 3.83706645]]

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

## 2.4. ICP\_3D\_SVD 配准 ##

# ICP 3D SVD 配准
    r, t = ICP_3D_SVD(a, b)
    
    # 显示变换参数
    print("\nR:\n", r)
    print("\nT:\n", t)
    print("\nNew B:\n", np.matmul(r, b) + t)
    input("输入任意键退出")
    
    # U:
    # [[-0.56266267 0.81931016 0.11018888]
    # [-0.38607908 -0.14256887 -0.91138195]
    # [-0.73099498 -0.55534222 0.3965367 ]]
    
    # V.T:
    # [[-0.57012392 -0.43378667 -0.69770182]
    # [ 0.80330155 -0.11629851 -0.58410725]
    # [ 0.17223626 -0.89347847 0.41476607]]
    
    # R:
    # [[ 0.99791909 -0.05138589 0.03894846]
    # [ 0.05033962 0.99835666 0.02738435]
    # [-0.04029162 -0.02536671 0.99886591]]
    
    # T:
    # [[-0.10000001]
    # [-0.2 ]
    # [ 0.49999999]]
    
    # New B:
    # [[1. 2. 2. 4. 5. ]
    # [4. 3. 3. 2. 5. ]
    # [2. 4.99999999 3. 1. 4. ]]

可以看到求解的\[R, T\]与预设转换的\[R, T\]一样，转换后的B与A一样，配对成功

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

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

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

谢谢

[ICP]: https://joveh-h.blog.csdn.net/article/details/113997681
[SLAM_38_ 3D-3D_ ICP]: https://joveh-h.blog.csdn.net/article/details/102615428
[SLAM_65_]: https://joveh-h.blog.csdn.net/article/details/112761286
[CSDN]: https://codechina.csdn.net/qq_32618327/a-simple-explanation/-/blob/main/icp_3d_svd.py
[Github]: https://github.com/JoveH-H/A-simple-explanation/blob/master/icp_3d_svd.py