Tango.Tango3DReconstruction._UpdateExtrinsics C# (CSharp) Method

        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;
        }