public void updatePosition() { Tango.TangoPoseData data = new Tango.TangoPoseData(); Tango.PoseProvider.GetPoseAtTime(data, 0.0, fp); Matrix4x4 T_ss_d = data.ToMatrix4x4(); //Vector3 tangoTranslation = new Vector3((float) data.translation.x, (float) data.translation.y, (float) data.translation.z); Matrix4x4 T_uw_ss = new Matrix4x4(); T_uw_ss.SetRow(0, new Vector4(1f, 0f, 0f, 0f)); T_uw_ss.SetRow(1, new Vector4(0f, 0f, 1f, 0f)); T_uw_ss.SetRow(2, new Vector4(0f, 1f, 0f, 0f)); T_uw_ss.SetRow(3, new Vector4(0f, 0f, 0f, 1f)); Matrix4x4 T_d_uc = new Matrix4x4(); T_d_uc.SetRow(0, new Vector4(1f, 0f, 0f, 0f)); T_d_uc.SetRow(1, new Vector4(0f, 1f, 0f, 0f)); T_d_uc.SetRow(2, new Vector4(0f, 0f, -1f, 0f)); T_d_uc.SetRow(3, new Vector4(0f, 0f, 0f, 1f)); Matrix4x4 T_uw_uc = (T_uw_ss * T_ss_d) * T_d_uc; currPos = T_uw_uc.GetColumn(3); position = "x: " + System.Math.Round(currPos.x, 3) + ", y: " + System.Math.Round(currPos.y, 3) + ", z: " + System.Math.Round(currPos.z, 3); }
/// <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; } // 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_colorCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_colorCamera; _UpdateColor(imageBuffer, world_T_colorCamera); }
/// <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> /// This is called each time new depth data is available. /// /// On the Tango tablet, the depth callback occurs at 5 Hz. /// </summary> /// <param name="tangoDepth">Tango depth.</param> public void OnTangoDepthMultithreadedAvailable(TangoXYZij tangoDepth) { if (!m_enabled) { 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, tangoDepth.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, tangoDepth.timestamp, pair); } if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { Debug.Log(string.Format("Time {0} has bad status code {1}", tangoDepth.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_depthCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_depthCamera; _UpdateDepth(tangoDepth, world_T_depthCamera); }
/// <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 { m_overallZ = 0; } }
/// <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> /// 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)); } } }