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

謝謝

氫鍵H-H

CSDN認證博客專家機器人軟體運動控制深度學習

一位永遠相信美好的事情即將發生，從事自動化機器人軟體開發，不忘初心，牢記使命，為實作中華民族偉大復興而奮斗的社會主義接班人，

轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/264163.html

標籤：python

上一篇：Python建構式與解構式（__init__()和__del__()）

下一篇：2021-02-25 Python人工智能（自學之路）分享（一）