SVD计算旋转，平移矩阵

问题描述

最近在做手眼标定，3D点云相机+ABB机器人，因相机与机器人在流水线同侧，无法通过传统的eye-to-hand标定板固定在机械臂末端进行手眼标定。

相机与机械臂的位置如下图所示，C是相机，B是机械臂base。

已知：物体在P1点时在C坐标系下的位置坐标，求物体运动到P2点时在B坐标系下的位置，其中P1->P2的运动是固定的。

问题建模

问题简化为由C下点集和B下的点集，求C->B的变换矩阵Hcb = Hcp1*Hp1p2*Hp2b.

其中物体在C下的坐标通过点云上获得，物体在B下的坐标通过机械臂末端指向获得。

奇异值分解

假设有两个点集A（同上文点集C）和B，且这两个点集合的元素数目相同且一一对应。为了寻找这两个点集之间的旋转矩阵R RR和转移矩阵t tt。可以将这个问题建模成如下的公式：

 为了寻找R 和t ，通常需要一下三个步骤：

（1）计算点集合的中心点

（2）将点集合所有点减去中心点（点集合移动到原点），计算最优旋转矩阵R

（3）计算平移矩阵t

（1）计算中心点

（2）点集移动到中心点计算最优旋转矩阵R

为了计算旋转矩阵R ，需要消除转移矩阵t的影响，所以我们首先需要将点集重新中心化，生成新点集合A ′  和B ′ 

 计算点集之间的协方差矩阵H

 通过SVD方法获得矩阵的U 、S 和V ,可以计算点集之间的旋转矩阵R 

 

 最后，通过R 可以获得转移矩阵t

 python代码实现

import numpy as np
import open3d as o3d

# Input: expects Nx3 matrix of points
# Returns R,t
# R = 3x3 rotation matrix
# t = 3x1 column vector

def rigid_transform_3D(A, B):
    assert len(A) == len(B)

    N = A.shape[0]  # total points
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)

    # centre the points
    AA = A - np.tile(centroid_A, (N, 1))
    BB = B - np.tile(centroid_B, (N, 1))

    H = np.matmul(np.transpose(AA),BB)
    U, S, Vt = np.linalg.svd(H)
    R = np.matmul(Vt.T, U.T)

    # special reflection case
    if np.linalg.det(R) < 0:
        print("Reflection detected")
        Vt[2, :] *= -1
        R = np.matmul(Vt.T,U.T)

    t = -np.matmul(R, centroid_A) + centroid_B
    # err = B - np.matmul(A,R.T) - t.reshape([1, 3])
    return R, t


if __name__=='__main__':
    pcdnamec = r'H:\rebar-lash\svd-calibrate-test\cloud_c1.pcd'
    savenameb = r'H:\rebar-lash\svd-calibrate-test\cloud_c12b1.pcd'
    a = np.array([[-193.584,-176.756,1060.589],
                  [-4.692,-174.317,1042.293],
                  [196.692,-172.488,1020.904],
                  [-199.756,5.549,1060.600],
                  [-7.829,8.059,1040.581],
                  [192.549,11.980,1018.920],
                  [-204.643,199.184,1057.278],
                  [-10.170,208.742,1034.667],
                  [188.412,216.853,1012.393],
                  [-191.635,-229.113,1068.749],
                  [-3.304,-224.797,1030.896],
                  [197.989,-220.181,1028.344],
                  [-373.171,-180.676,1077.685],
                  [-372.920,2.533,1076.298],
                  [-373.844,191.841,1080.242]])
    b = np.array([[1906.781,-117.738,1160.026],
                  [1902.407,-308.042,1165.0017],
                  [1897.769,-511.202,1172.318],
                  [1931.002,-122.888,976.885],
                  [1905.272,-313.118,983.545],
                  [1921.167,-516.281,987.524],
                  [1909.811,-123.847,781.676],
                  [1900.205,-317.529,782.726],
                  [1894.174,-518.614,782.249],
                  [1914.024,-117.893,1211.583],
                  [1941.923,-309.307,1215.946],
                  [1905.388,-510.974,1218.967],
                  [1910.622,60.445,1157.234],
                  [1911.670,52.165,974.569],
                  [1920.808,45.444,783.796]])


    c = np.reshape(a[-2:], (2, 3))
    test_a1 = np.reshape(c[0],(1,3))
    test_a2 = np.reshape(c[1],(1,3))

    c=np.reshape(b[-2:], (2, 3))
    test_b1 = np.reshape(c[0],(1,3))
    test_b2 = np.reshape(c[1],(1,3))

    a = a[:-6]
    b = b[:-6]
    r, t = rigid_transform_3D(a, b)
    print('r:',r)
    print('t:',t)

    bb = np.matmul(a, r.T) + t.reshape([1, 3])
    print('b-bb:', b - bb)

    c = np.matmul(test_a1, r.T) + t.reshape([1, 3])
    print('c-test_b1:', c - test_b1)

    c = np.matmul(test_a2, r.T) + t.reshape([1, 3])
    print('c-test_b2:', c - test_b2)

    pcd  = o3d.io.read_point_cloud(pcdnamec)
    point_arr =np.asarray(pcd.points)* 1000
    color_arr =np.asarray(pcd.colors)
    point_arrb = np.matmul(point_arr, r.T) + t.reshape([1, 3])
    pcd_new = o3d.geometry.PointCloud()
    pcd_new.points = o3d.utility.Vector3dVector(point_arrb)
    pcd_new.colors = o3d.utility.Vector3dVector(color_arr)
    o3d.io.write_point_cloud(savenameb, pcd_new)