private void _UpdateExtrinsics()
{
TangoCoordinateFramePair pair;
TangoPoseData imu_T_devicePose = new TangoPoseData();
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(imu_T_devicePose, 0, pair);
TangoPoseData imu_T_depthCameraPose = new TangoPoseData();
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
PoseProvider.GetPoseAtTime(imu_T_depthCameraPose, 0, pair);
TangoPoseData imu_T_colorCameraPose = new TangoPoseData();
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR;
PoseProvider.GetPoseAtTime(imu_T_colorCameraPose, 0, pair);
// Convert into matrix form to combine the poses.
Matrix4x4 device_T_imu = Matrix4x4.Inverse(imu_T_devicePose.ToMatrix4x4());
m_device_T_depthCamera = device_T_imu * imu_T_depthCameraPose.ToMatrix4x4();
m_device_T_colorCamera = device_T_imu * imu_T_colorCameraPose.ToMatrix4x4();
m_unityWorld_T_startService.SetColumn(0, new Vector4(1, 0, 0, 0));
m_unityWorld_T_startService.SetColumn(1, new Vector4(0, 0, 1, 0));
m_unityWorld_T_startService.SetColumn(2, new Vector4(0, 1, 0, 0));
m_unityWorld_T_startService.SetColumn(3, new Vector4(0, 0, 0, 1));
// Update the camera intrinsics too.
TangoCameraIntrinsics colorCameraIntrinsics = new TangoCameraIntrinsics();
VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR, colorCameraIntrinsics);
m_colorCameraIntrinsics.calibration_type = (int)colorCameraIntrinsics.calibration_type;
m_colorCameraIntrinsics.width = colorCameraIntrinsics.width;
m_colorCameraIntrinsics.height = colorCameraIntrinsics.height;
m_colorCameraIntrinsics.cx = colorCameraIntrinsics.cx;
m_colorCameraIntrinsics.cy = colorCameraIntrinsics.cy;
m_colorCameraIntrinsics.fx = colorCameraIntrinsics.fx;
m_colorCameraIntrinsics.fy = colorCameraIntrinsics.fy;
m_colorCameraIntrinsics.distortion0 = colorCameraIntrinsics.distortion0;
m_colorCameraIntrinsics.distortion1 = colorCameraIntrinsics.distortion1;
m_colorCameraIntrinsics.distortion2 = colorCameraIntrinsics.distortion2;
m_colorCameraIntrinsics.distortion3 = colorCameraIntrinsics.distortion3;
m_colorCameraIntrinsics.distortion4 = colorCameraIntrinsics.distortion4;
}