sensor_config = sensor_data['sensor_config']
registration_config = sensor_data['registration_config'].copy()
registration_config.update(config['chessboard_registration'])
# open sensor
try:
sensor_type = sensor_config['type']
sensor_config['frame'] = sensor_frame
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
logging.info('Starting sensor')
sensor.start()
ir_intrinsics = sensor.ir_intrinsics
logging.info('Sensor initialized')
# register
reg_result = CameraChessboardRegistration.register(sensor, registration_config)
T_camera_world = T_cb_world * reg_result.T_camera_cb
logging.info('Final Result for sensor %s' %(sensor_frame))
logging.info('Rotation: ')
logging.info(T_camera_world.rotation)
logging.info('Quaternion: ')
logging.info(T_camera_world.quaternion)
logging.info('Translation: ')
logging.info(T_camera_world.translation)
sensor.stop()
except Exception as e:
logging.error('Failed to register sensor {}'.format(sensor_frame))
continue
# load T_world_cam
T_world_cam_path = os.path.join(config['calib_dir'], sensor_frame,
'{0}_to_world.tf'.format(sensor_frame))
T_world_cam = RigidTransform.load(T_world_cam_path)
# open sensor
sensor_type = sensor_config['type']
sensor_config['frame'] = sensor_frame
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
logging.info('Starting sensor')
sensor.start()
ir_intrinsics = sensor.ir_intrinsics
logging.info('Sensor initialized')
# register
reg_result = CameraChessboardRegistration.register(
sensor, config['chessboard_registration'])
T_cb_cam = reg_result.T_camera_cb
T_world_obj = T_world_cam * T_cb_cam.inverse() * T_cb_obj
output_path = os.path.join(config['calib_dir'], T_world_obj.from_frame)
if not os.path.exists(output_path):
os.makedirs(output_path)
output_filename = os.path.join(
output_path, '{0}_to_world.tf'.format(T_world_obj.from_frame))
print T_world_obj
T_world_obj.save(output_filename)
if config['vis'] and VIS_SUPPORTED: