geom.py 文件源码

def __init__(self, transform=None, inverse_transform=None, rotation=None, translation_vector=None):
        if translation_vector is not None:
            if type(translation_vector) != np.ndarray:
                translation_vector = np.array(translation_vector)
            if translation_vector.shape != (3, 1):
                translation_vector = translation_vector.reshape(3, 1)
        if rotation is not None:
            if type(rotation) != np.ndarray:
                rotation = np.array(rotation)
            if rotation.size == 9:
                rotation_vector = cv2.Rodrigues(rotation)[0]
                rotation_matrix = rotation
            elif rotation.size == 3:
                rotation_matrix = cv2.Rodrigues(rotation)[0]
                rotation_vector = rotation
            else:
                raise ValueError(
                    "Wrong rotation size: {:d}. Expecting a 3-length vector or 3x3 matrix.".format(rotation.size))
        if transform is None:
            if translation_vector is None or rotation is None:
                raise (ValueError("Expecting either the transform matrix or both the rotation & translation vector"))
            self.T = np.vstack((np.append(rotation_matrix, translation_vector, axis=1), [0, 0, 0, 1]))
        else:
            self.T = transform
            if translation_vector is None:
                translation_vector = transform[0:3, 3].reshape(3, 1)
            if rotation is None:
                rotation_matrix = transform[0:3, 0:3]
                rotation_vector = cv2.Rodrigues(rotation_matrix)[0]
        if inverse_transform is None:
            rot_mat_inv = rotation_matrix.T
            inverse_translation = -rot_mat_inv.dot(translation_vector)
            inverse_transform = np.vstack((np.append(rot_mat_inv, inverse_translation, 1), [0, 0, 0, 1]))

        self.rmat = rotation_matrix
        self.tvec = translation_vector
        self.rvec = rotation_vector
        self.T_inv = inverse_transform