def load_calibration(self, path, robot_pose=[[0, 0, 0], [0, 0, 0]]):
"""Set the camera by using the camera calibration results.
Args:
path: The data directory of the calibration results.
"""
intrinsics_path = os.path.join(path, 'IR_intrinsics.npy')
intrinsics = np.load(intrinsics_path, encoding='latin1')
translation_path = os.path.join(path, 'robot_IR_translation.npy')
translation = np.load(translation_path, encoding='latin1')
rotation_path = os.path.join(path, 'robot_IR_rotation.npy')
rotation = np.load(rotation_path, encoding='latin1')
# Convert the extrinsics from the robot frame to the world frame.
from_robot_to_world = get_transform(source=robot_pose)
robot_pose_in_camera = Pose([translation, rotation])
camera_pose_in_robot = robot_pose_in_camera.inverse()
camera_pose_in_world = from_robot_to_world.transform(
camera_pose_in_robot)
world_origin_in_camera = camera_pose_in_world.inverse()
translation = world_origin_in_camera.position
rotation = world_origin_in_camera.matrix3
return intrinsics, translation, rotation
def pose(self):
world_origin_in_camera = Pose([self._translation, self._rotation])
return world_origin_in_camera.inverse()
