コード例 #1
0
def Process2DCSV(xyt, xyt_target):
    m1 = np.identity(4)
    m1[0:2, 3] = xyt[0:2]
    m1[0:3, 0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, xyt[2]], degrees=True))

    m2 = np.identity(4)
    m2[0:2, 3] = xyt_target[0:2]
    m2[0:3,
       0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, xyt_target[2]],
                                    degrees=True))

    return pd.distance_RT(m1, m2)
コード例 #2
0
def eulerChangeToWorldRot(current_Rmat, d_rot):
    initial_rot = Rot.from_euler(np.array([0, np.pi, 0]), 'XYZ')
    init_mat = Rot.as_dcm(initial_rot)
    cameraCoordNewRot = Rot.from_euler('XYZ', d_rot).as_dcm()
    # old orientation of cam frame in world coords
    curCamFrameR = np.matmul(current_Rmat, init_mat)
    # new orientation of cam frame in world coords
    worldNewRotCamFrame = np.matmul(cameraCoordNewRot, curCamFrameR)
    # rotate back to pre cam frame orientation
    worldNewRotCamFrame = np.matmul(np.transpose(init_mat),
                                    worldNewRotCamFrame)
    return Rot.from_dcm(worldNewRotCamFrame).as_euler('XYZ')
コード例 #3
0
def rotation_matrix(angle, rotation_axis):
    """ Takes an angle (radians) and axis of rotation (3-D vector) and outputs a rotation matrix"""
    normed_axis = rotation_axis / np.linalg.norm(rotation_axis)
    rot_vec = R.from_rotvec(normed_axis * angle)
    return R.as_dcm(rot_vec)
コード例 #4
0
def Convert6dofPoseToMatrix(pose):
    m = np.identity(4)
    m[0:3, 3] = pose[0:3]
    m[0:3, 0:3] = R.as_dcm(R.from_quat(pose[3:7]))
    return m
コード例 #5
0
    print(np.transpose(m @ pt_zero))
    for i in range(0, 3):
        vec_zero = np.zeros([4, 1])
        vec_zero[i] = 1
        print(np.transpose(m @ vec_zero))

    print('\n')


if __name__ == '__main__':
    print('Checking Pose Distance Metric code\n')

    m1 = np.identity(4)
    d_ang = np.pi / 3
    m1[0:3, 3] = [1, 1, 1]
    m1[0:3, 0:3] = R.as_dcm(R.from_euler('XYZ', [0, 0, d_ang]))

    m2 = np.identity(4)
    m2[0:3, 3] = [2, 2, 2]

    pd = PoseDistance()

    dist_RT = pd.distance_RT(m1, m2)
    dist_RT_seq = pd.distance_RT_seq(pd.get_sequence_RT(m1, m2, n=10))
    print(
        'Distance RT, should be {0:0.2f}, is {1:0.2f}, integrated {2:0.2f}\n'.
        format(np.sqrt(3) + d_ang, dist_RT, dist_RT_seq))

    mat_seq2 = pd.get_sequence_RT(m1, m2, 2)

    print('M1:')