Utility to compute the transformation matrix between two frames given 4 corresponding points from the two frames. Can be used to get the T matrix defined from the robot base to the camera frame (or vice versa).

base_to_cam_transform.py

import numpy as np

def get_transformation(points1, points2):
    """
    Get the transformation matrix that would define
    frame 2 wrt frame 1, given 4 points from coordinate
    frame 1 and corresponding 4 points from coordinate
    frame 2. Can be used to find the camera frame wrt 
    robot base frame when the end-effector positions are
    collected from the robot as well as the camera
    frame (with marker).

    Assumes: :math:`p2 = T.p1`

    :param points1: 4 points in homogeneous form, reshaped to 4x4 (row: x,y,z,1, col: points)
    :type points1: np.array
    :param points2: 4 points in homogeneous form, reshaped to 4x4 (row: x,y,z,1, col: points)
    :type points2: np.array
    """
    return points2.dot(np.linalg.inv(points1))


if __name__ == "__main__":
        
    # random points from frame 1
    a = np.asarray([[1,1,2,3],
                    [1,2,3,3],
                    [1,3,2,3],
                    [1,1,1,1]])

    # the transformation matrix to be found
    theta = np.pi/4
    T = np.asarray([[np.cos(theta), -np.sin(theta), 0, 20],
                    [np.sin(theta), np.cos(theta), 0, 60],
                    [0,0,1,10],
                    [0,0,0,1]])

    # T = np.random.rand(4,4)*10

    # corresponding points in frame 2
    b = T.dot(a)

    T_c = get_transformation(a, b)
    print (T_c)
    print (T)
    print (np.isclose(T_c, T))
    print ("Success" if np.allclose(T_c, T) else "Not close enough")