三维ICP-SVD配准

---

1. 理论

理论参考：

二维ICP配准
视觉SLAM笔记（38） 3D-3D: ICP
视觉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
Github

---

谢谢