Added debug output from trajectory estimator and added new extrinsics calibration reader to transforms