C# TangoPoseData.ToMatrix4x4方法代码示例

        /// <summary>
        /// Convert a TangoPoseData into the Unity coordinate system. This only
        /// works on TangoPoseData that describes the device with respect to the
        /// start of service or area description. The result position and
        /// rotation can be used to set Unity's Transform.position and
        /// Transform.rotation.
        /// </summary>
        /// <param name="poseData">
        /// The input pose data that is going to be converted, please note that
        /// the pose data has to be in the start of service with respect to
        /// device frame.
        /// </param>
        /// <param name="position">The result position data.</param>
        /// <param name="rotation">The result rotation data.</param>
        public static void TangoPoseToWorldTransform(TangoPoseData poseData,
                                                     out Vector3 position,
                                                     out Quaternion rotation)
        {
            if (poseData == null)
            {
                Debug.Log("Invalid poseData.\n" + Environment.StackTrace);
                position = Vector3.zero;
                rotation = Quaternion.identity;
                return;
            }

            if (poseData.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
            {
                Debug.Log("Invalid target frame of the poseData.\n" + Environment.StackTrace);
                position = Vector3.zero;
                rotation = Quaternion.identity;
                return;
            }

            if (poseData.framePair.baseFrame !=
                TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
                poseData.framePair.baseFrame !=
                TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)
            {
                Debug.Log("Invalid base frame of the poseData.\n" + Environment.StackTrace);
                position = Vector3.zero;
                rotation = Quaternion.identity;
                return;
            }

            Matrix4x4 startServiceTDevice = poseData.ToMatrix4x4();
            Matrix4x4 unityWorldTUnityCamera = UNITY_WORLD_T_START_SERVICE *
                                               startServiceTDevice *
                                               DEVICE_T_UNITY_CAMERA *
                                               m_devicePoseRotation;

            // Extract final position, rotation.
            position = unityWorldTUnityCamera.GetColumn(3);
            rotation = Quaternion.LookRotation(unityWorldTUnityCamera.GetColumn(2),
                                               unityWorldTUnityCamera.GetColumn(1));
        }

    /// <summary>
    /// Callback that gets called when depth is available from the Tango Service.
    /// </summary>
    /// <param name="pointCloud">Depth information from Tango.</param>
    public void OnTangoPointCloudAvailable(TangoPointCloudData pointCloud)
    {
        // Calculate the time since the last successful depth data
        // collection.
        if (m_depthTimestamp != 0.0)
        {
            m_depthDeltaTime = (float)((pointCloud.m_timestamp - m_depthTimestamp) * 1000.0);
        }
        
        // Fill in the data to draw the point cloud.
        m_pointsCount = pointCloud.m_numPoints;
        if (m_pointsCount > 0)
        {
            _SetUpCameraData();
            TangoCoordinateFramePair pair;
            TangoPoseData poseData = new TangoPoseData();

            // Query pose to transform point cloud to world coordinates, here we are using the timestamp
            // that we get from depth.
            if (m_useAreaDescriptionPose)
            {
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }
            else
            {
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }

            PoseProvider.GetPoseAtTime(poseData, pointCloud.m_timestamp, pair);
            if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
            {
                return;
            }

            Matrix4x4 startServiceTDevice = poseData.ToMatrix4x4();

            // The transformation matrix that represents the pointcloud's pose. 
            // Explanation: 
            // The pointcloud which is in Depth camera's frame, is put in unity world's 
            // coordinate system(wrt unity world).
            // Then we are extracting the position and rotation from uwTuc matrix and applying it to 
            // the PointCloud's transform.
            Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera;
            transform.position = Vector3.zero;
            transform.rotation = Quaternion.identity;

            // Add offset to the pointcloud depending on the offset from TangoDeltaPoseController
            Matrix4x4 unityWorldOffsetTDepthCamera;
            if (m_tangoDeltaPoseController != null)
            {
                unityWorldOffsetTDepthCamera = m_tangoDeltaPoseController.UnityWorldOffset * unityWorldTDepthCamera;
            }
            else
            {
                unityWorldOffsetTDepthCamera = unityWorldTDepthCamera;
            }

            // Converting points array to world space.
            m_overallZ = 0;
            for (int i = 0; i < m_pointsCount; ++i)
            {
                Vector3 point = pointCloud[i];
                m_points[i] = unityWorldOffsetTDepthCamera.MultiplyPoint3x4(point);
                m_overallZ += point.z;
            }

            m_overallZ = m_overallZ / m_pointsCount;
            m_depthTimestamp = pointCloud.m_timestamp;

            if (m_updatePointsMesh)
            {
                // Need to update indicies too!
                int[] indices = new int[m_pointsCount];
                for (int i = 0; i < m_pointsCount; ++i)
                {
                    indices[i] = i;
                }

                m_mesh.Clear();
                m_mesh.vertices = m_points;
                m_mesh.SetIndices(indices, MeshTopology.Points, 0);
            }

            // The color should be pose relative, we need to store enough info to go back to pose values.
            m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldOffsetTDepthCamera.inverse);

            // Try to find the floor using this set of depth points if requested.
            if (m_findFloorWithDepth)
            {
                _FindFloorWithDepth();
            }
        }
        else
        {
//.........这里部分代码省略.........

    /// <summary>
    /// Sets up extrinsic matrixes and camera intrinsics for this hardware.
    /// </summary>
    private void _SetUpCameraData()
    {
        if (m_cameraDataSetUp)
        {
            return;
        }

        double timestamp = 0.0;
        TangoCoordinateFramePair pair;
        TangoPoseData poseData = new TangoPoseData();

        // Query the extrinsics between IMU and device frame.
        pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
        pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
        m_imuTDevice = poseData.ToMatrix4x4();

        // Query the extrinsics between IMU and depth camera frame.
        pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
        pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH;
        PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
        m_imuTDepthCamera = poseData.ToMatrix4x4();

        // Also get the camera intrinsics
        m_colorCameraIntrinsics = new TangoCameraIntrinsics();
        VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR, m_colorCameraIntrinsics);

        m_cameraDataSetUp = true;
    }

    /// @endcond
    /// <summary>
    /// Updates the transformation to the pose for that timestamp.
    /// </summary>
    /// <param name="timestamp">Time in seconds to update the transformation to.</param>
    private void _UpdateTransformation(double timestamp)
    {
        TangoPoseData pose = new TangoPoseData();
        TangoCoordinateFramePair pair;
        if (m_useAreaDescriptionPose)
        {
            pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        }
        else
        {
            pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        }

        PoseProvider.GetPoseAtTime(pose, timestamp, pair);

        // The callback pose is for device with respect to start of service pose.
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Construct matrix for the start of service with respect to device from the pose.
            Matrix4x4 ssTd = pose.ToMatrix4x4();
            
            // Calculate matrix for the camera in the Unity world, taking into account offsets.
            Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc * TangoSupport.m_colorCameraPoseRotation;
            
            // Extract final position, rotation.
            m_tangoPosition = uwTuc.GetColumn(3);
            m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
            
            // Other pose data -- Pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus)
            {
                m_poseCount = 0;
            }

            m_poseCount++;
            
            // Other pose data -- Pose time.
            m_poseTimestamp = timestamp;
        }

        m_poseStatus = pose.status_code;
        
        // Apply final position and rotation.
        transform.position = m_tangoPosition;
        transform.rotation = m_tangoRotation;
    }

    /// <summary>
    /// Gets device and camera extrinsics necessary for the transformations done
    /// by this controller. Extrinsics queries use GetPoseAtTime() with a
    /// specific frame pair, and can only be done after the Tango service is
    /// connected.
    ///
    /// The transform for the device with respect to the color camera frame is
    /// not directly queryable from API, so we use the IMU frame to get the
    /// transformation between the two.
    /// </summary>
    private void _SetCameraExtrinsics()
    {
        double timestamp = 0.0;
        TangoCoordinateFramePair pair;
        TangoPoseData poseData = new TangoPoseData();
        
        // Get the transformation of device frame with respect to IMU frame.
        pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
        pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
        Matrix4x4 imuTd = poseData.ToMatrix4x4();
        
        // Get the transformation of IMU frame with respect to color camera frame.
        pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
        pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR;
        PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
        Matrix4x4 imuTc = poseData.ToMatrix4x4();

        // Get the transform of the Unity Camera frame with respect to the Color Camera frame.
        Matrix4x4 cTuc = new Matrix4x4();
        cTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
        cTuc.SetColumn(1, new Vector4(0.0f, -1.0f, 0.0f, 0.0f));
        cTuc.SetColumn(2, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
        cTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));

        m_dTuc = Matrix4x4.Inverse(imuTd) * imuTc * cTuc;
    }

        /// <summary>
        /// Calculate the camera extrinsics for this device.
        /// </summary>
        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;
        }

        /// <summary>
        /// This will be called when a new frame is available from the camera.
        /// 
        /// The first scan-line of the color image is reserved for metadata instead of image pixels.
        /// </summary>
        /// <param name="cameraId">Camera identifier.</param>
        /// <param name="imageBuffer">Image buffer.</param>
        public void OnTangoImageMultithreadedAvailable(TangoEnums.TangoCameraId cameraId, TangoImageBuffer imageBuffer)
        {
            if (!m_enabled)
            {
                return;
            }

            if (cameraId != TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR)
            {
                return;
            }

            // Build World T depth camera
            TangoPoseData world_T_devicePose = new TangoPoseData();
            if (m_useAreaDescriptionPose)
            {
                TangoCoordinateFramePair pair;
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(world_T_devicePose, imageBuffer.timestamp, pair);
            }
            else
            {
                TangoCoordinateFramePair pair;
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(world_T_devicePose, imageBuffer.timestamp, pair);
            }

            if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
            {
                Debug.Log(string.Format("Time {0} has bad status code {1}",
                                        imageBuffer.timestamp, world_T_devicePose.status_code)
                          + Environment.StackTrace);
                return;
            }

            // NOTE: The 3D Reconstruction library does not handle left handed matrices correctly.  For now, transform
            // into the Unity world space after extraction.
            Matrix4x4 world_T_colorCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_colorCamera;

            _UpdateColor(imageBuffer, world_T_colorCamera);
        }

        /// <summary>
        /// This is called each time new depth data is available.
        /// 
        /// On the Tango tablet, the depth callback occurs at 5 Hz.
        /// </summary>
        /// <param name="pointCloud">Tango depth.</param>
        public void OnTangoPointCloudMultithreadedAvailable(ref TangoPointCloudIntPtr pointCloud)
        {
            if (!m_enabled || pointCloud.m_points == IntPtr.Zero)
            {
                return;
            }

            // Build World T depth camera
            TangoPoseData world_T_devicePose = new TangoPoseData();
            if (m_useAreaDescriptionPose)
            {
                TangoCoordinateFramePair pair;
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(world_T_devicePose, pointCloud.m_timestamp, pair);
            }
            else
            {
                TangoCoordinateFramePair pair;
                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(world_T_devicePose, pointCloud.m_timestamp, pair);
            }

            if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
            {
                Debug.LogFormat("Time {0} has bad status code {1}{2}", 
                                pointCloud.m_timestamp, world_T_devicePose.status_code, Environment.StackTrace);
                return;
            }

            // The 3D Reconstruction library can not handle a left handed transformation during update.  Instead,
            // transform into the Unity world space via the external_T_tango config.
            Matrix4x4 world_T_depthCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_depthCamera;

            _UpdateDepth(pointCloud, world_T_depthCamera);
        }

        /// <summary>
        /// This is called when successfully connected to the Tango service.
        /// </summary>
        public void OnTangoServiceConnected()
        {
            // Calculate the camera extrinsics.
            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();

            // Update the camera intrinsics.
            TangoCameraIntrinsics intrinsics = new TangoCameraIntrinsics();
            Status status;

            APICameraCalibration colorCameraCalibration;
            VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR, intrinsics);
            colorCameraCalibration.calibration_type = (int)intrinsics.calibration_type;
            colorCameraCalibration.width = intrinsics.width;
            colorCameraCalibration.height = intrinsics.height;
            colorCameraCalibration.cx = intrinsics.cx;
            colorCameraCalibration.cy = intrinsics.cy;
            colorCameraCalibration.fx = intrinsics.fx;
            colorCameraCalibration.fy = intrinsics.fy;
            colorCameraCalibration.distortion0 = intrinsics.distortion0;
            colorCameraCalibration.distortion1 = intrinsics.distortion1;
            colorCameraCalibration.distortion2 = intrinsics.distortion2;
            colorCameraCalibration.distortion3 = intrinsics.distortion3;
            colorCameraCalibration.distortion4 = intrinsics.distortion4;
            status = (Status)API.Tango3DR_setColorCalibration(m_context, ref colorCameraCalibration);
            if (status != Status.SUCCESS)
            {
                Debug.Log("Unable to set color calibration." + Environment.StackTrace);
            }

            APICameraCalibration depthCameraCalibration;
            VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_DEPTH, intrinsics);
            depthCameraCalibration.calibration_type = (int)intrinsics.calibration_type;
            depthCameraCalibration.width = intrinsics.width;
            depthCameraCalibration.height = intrinsics.height;
            depthCameraCalibration.cx = intrinsics.cx;
            depthCameraCalibration.cy = intrinsics.cy;
            depthCameraCalibration.fx = intrinsics.fx;
            depthCameraCalibration.fy = intrinsics.fy;
            depthCameraCalibration.distortion0 = intrinsics.distortion0;
            depthCameraCalibration.distortion1 = intrinsics.distortion1;
            depthCameraCalibration.distortion2 = intrinsics.distortion2;
            depthCameraCalibration.distortion3 = intrinsics.distortion3;
            depthCameraCalibration.distortion4 = intrinsics.distortion4;
            status = (Status)API.Tango3DR_setDepthCalibration(m_context, ref depthCameraCalibration);
            if (status != Status.SUCCESS)
            {
                Debug.Log("Unable to set depth calibration." + Environment.StackTrace);
            }
        }

    /// <summary>
    /// Correct all saved marks when loop closure happens.
    /// 
    /// When Tango Service is in learning mode, the drift will accumulate overtime, but when the system sees a
    /// pre-exsiting area, it will do a operation to correct all previously saved poses
    /// (the pose you can query with GetPoseAtTime). This operation is called loop closure. When loop closure happens,
    /// we will need to re-query all previously saved marker position in order to achieve the best result.
    /// This function is doing the querying job based on timestamp.
    /// </summary>
    private void _UpdateMarkersForLoopClosures()
    {
        // Adjust mark's position each time we have a loop closure detected.
        foreach (GameObject obj in m_markerList)
        {
            ARMarker tempMarker = obj.GetComponent<ARMarker>();
            if (tempMarker.m_timestamp != -1.0f)
            {
                TangoCoordinateFramePair pair;
                TangoPoseData relocalizedPose = new TangoPoseData();

                pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(relocalizedPose, tempMarker.m_timestamp, pair);

                Matrix4x4 uwTDevice = m_poseController.m_uwTss
                                      * relocalizedPose.ToMatrix4x4()
                                      * m_poseController.m_dTuc;

                Matrix4x4 uwTMarker = uwTDevice * tempMarker.m_deviceTMarker;

                obj.transform.position = uwTMarker.GetColumn(3);
                obj.transform.rotation = Quaternion.LookRotation(uwTMarker.GetColumn(2), uwTMarker.GetColumn(1));
            }
        }
    }