/// <summary> /// Reduces depth points down to below a fixed number of points. /// /// TODO: Do this sort of thing in C code before before passing to Unity instead. /// </summary> /// <param name="pointCloud">Tango depth data to reduce.</param> /// <param name="maxNumPoints">Max points to reduce down to.</param> private static void _ReducePointCloudPoints(TangoPointCloudData pointCloud, int maxNumPoints) { if (maxNumPoints > 0 && pointCloud.m_numPoints > maxNumPoints) { // Here (maxNumPoints - 1) rather than maxPoints is just a quick and // dirty way to avoid any possibile edge-case accumulated FP error // in the sketchy code below. float keepFraction = (maxNumPoints - 1) / (float)pointCloud.m_numPoints; int keptPoints = 0; float keepCounter = 0; for (int i = 0; i < pointCloud.m_numPoints; i++) { keepCounter += keepFraction; if (keepCounter > 1) { pointCloud.m_points[keptPoints] = pointCloud.m_points[i]; keepCounter--; keptPoints++; } } pointCloud.m_numPoints = keptPoints; } }
/// <summary> /// Calculates the depth in the color camera space at a user-specified /// location using nearest-neighbor interpolation. /// </summary> /// <returns> /// <c>true</c>, if a point is found is found successfully, <c>false</c> otherwise. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least three points. /// </param> /// <param name="colorCameraTimestamp"> /// Color camera's timestamp when UV is captured. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// in Unity viewport space. The bottom-left of the camera is (0,0); /// the top-right is (1,1). /// </param> /// <param name="colorCameraPoint"> /// The point (x, y, z), where (x, y) is the back-projection of the UV /// coordinates to the color camera space and z is the z coordinate of /// the point in the point cloud nearest to the user selection after /// projection onto the image plane. If there is not a point cloud point /// close to the user selection after projection onto the image plane, /// then the point will be set to (0.0, 0.0, 0.0) and isValidPoint will /// be set to false. /// </param> public static bool ScreenCoordinateToWorldNearestNeighbor( TangoPointCloudData pointCloud, double colorCameraTimestamp, Vector2 uvCoordinates, out Vector3 colorCameraPoint) { TangoPoseData depth_T_colorCameraPose = new TangoPoseData(); int returnValue = TangoSupportAPI.TangoSupport_calculateRelativePose( pointCloud.m_timestamp, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH, colorCameraTimestamp, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR, depth_T_colorCameraPose); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.LogError("TangoSupport_calculateRelativePose error. " + Environment.StackTrace); colorCameraPoint = Vector3.zero; return(false); } GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud.m_points, GCHandleType.Pinned); TangoPointCloudIntPtr tangoPointCloudIntPtr = new TangoPointCloudIntPtr(); tangoPointCloudIntPtr.m_points = pointCloudHandle.AddrOfPinnedObject(); tangoPointCloudIntPtr.m_timestamp = pointCloud.m_timestamp; tangoPointCloudIntPtr.m_numPoints = pointCloud.m_numPoints; // Unity viewport space is: the bottom-left of the camera is (0,0); // the top-right is (1,1). // Tango (Android) defined UV space is: the top-left of the camera is (0,0); // the bottom-right is (1,1). Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y); DVector4 pointCloudRotation = DVector4.IdentityQuaternion; DVector3 pointCloudTranslation = DVector3.Zero; returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointNearestNeighbor( ref tangoPointCloudIntPtr, ref pointCloudTranslation, ref pointCloudRotation, ref uvCoordinatesTango, AndroidHelper.GetDisplayRotation(), ref depth_T_colorCameraPose.translation, ref depth_T_colorCameraPose.orientation, out colorCameraPoint); pointCloudHandle.Free(); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.LogError("TangoSupport_getDepthAtPointNearestNeighbor error. " + Environment.StackTrace); colorCameraPoint = Vector3.zero; return(false); } return(true); }
/// <summary> /// Register to get Tango depth callbacks. /// /// NOTE: Tango depth callbacks happen on a different thread than the main /// Unity thread. /// </summary> internal void SetCallback() { m_pointCloud = new TangoPointCloudData(); m_pointCloud.m_points = new float[Common.MAX_NUM_POINTS * 4]; m_xyzPoints = new float[Common.MAX_NUM_POINTS * 3]; m_onPointCloudAvailableCallback = new DepthProvider.APIOnPointCloudAvailable(_OnPointCloudAvailable); Tango.DepthProvider.SetCallback(m_onPointCloudAvailableCallback); }
/// <summary> /// It's backwards, but fill an emulated TangoPointCloudIntPtr instance from an emulated TangoPointCloudData /// instance. It is the responsibility of the caller to GC pin/free the pointCloudData's m_points. /// </summary> /// <returns>Emulated TangoPointCloudIntPtr instance.</returns> /// <param name="pointCloud">Emulated point cloud data.</param>> /// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param> private static TangoPointCloudIntPtr _GetEmulatedRawData(TangoPointCloudData pointCloud, GCHandle pinnedPoints) { TangoPointCloudIntPtr raw; raw.m_version = 0; raw.m_timestamp = pointCloud.m_timestamp; raw.m_numPoints = pointCloud.m_numPoints; raw.m_points = pinnedPoints.AddrOfPinnedObject(); return(raw); }
/// <summary> /// Fill out <c>pointCloudData</c> with emulated values from Tango. /// </summary> /// <param name="pointCloudData">The point cloud data to fill out.</param> private static void _FillEmulatedPointCloud(ref TangoPointCloudData pointCloud) { List <Vector3> emulated = DepthProvider.GetTangoEmulation(out pointCloud.m_timestamp); pointCloud.m_numPoints = emulated.Count; for (int it = 0; it < emulated.Count; ++it) { pointCloud.m_points[(it * 4) + 0] = emulated[it].x; pointCloud.m_points[(it * 4) + 1] = emulated[it].y; pointCloud.m_points[(it * 4) + 2] = emulated[it].z; pointCloud.m_points[(it * 4) + 3] = 1; } }
/// <summary> /// Stop getting Tango depth callbacks, clear all listeners. /// </summary> internal static void Reset() { // Avoid calling into tango_client_api before the correct library is loaded. if (m_onPointCloudAvailableCallback != null) { Tango.DepthProvider.ClearCallback(); } m_onPointCloudAvailableCallback = null; m_isDirty = false; m_pointCloud = new TangoPointCloudData(); m_pointCloud.m_points = new float[Common.MAX_NUM_POINTS * 4]; m_xyzPoints = new float[Common.MAX_NUM_POINTS * 3]; m_maxNumReducedDepthPoints = 0; m_onTangoDepthAvailable = null; m_onTangoDepthMultithreadedAvailable = null; m_onPointCloudAvailable = null; m_onPointCloudMultithreadedAvailable = null; }
/// <summary> /// Initializes a new instance of the <see cref="Tango.TangoUnityDepth"/> class from a /// <see cref="PointCloud"/> instance. /// </summary> /// <param name="pointCloud">Point cloud.</param> public TangoUnityDepth(TangoPointCloudData pointCloud) { m_points = new float[MAX_POINTS_ARRAY_SIZE]; m_ij = new int[MAX_IJ_ARRAY_SIZE]; m_timestamp = pointCloud.m_timestamp; m_pointCount = pointCloud.m_numPoints; for (int it = 0; it < pointCloud.m_points.Length / 4; ++it) { m_points[(it * 3) + 0] = pointCloud.m_points[(it * 4) + 0]; m_points[(it * 3) + 1] = pointCloud.m_points[(it * 4) + 1]; m_points[(it * 3) + 2] = pointCloud.m_points[(it * 4) + 2]; } m_ijRows = m_ijColumns = 0; for (int it = 0; it < m_ij.Length; ++it) { m_ij[it] = -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 { m_overallZ = 0; } }
/// <summary> /// It's backwards, but fill an emulated TangoPointCloudIntPtr instance from an emulated TangoPointCloudData /// instance. It is the responsibility of the caller to GC pin/free the pointCloudData's m_points. /// </summary> /// <returns>Emulated TangoPointCloudIntPtr instance.</returns> /// <param name="pointCloud">Emulated point cloud data.</param>> /// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param> private static TangoPointCloudIntPtr _GetEmulatedRawData(TangoPointCloudData pointCloud, GCHandle pinnedPoints) { TangoPointCloudIntPtr raw; raw.m_version = 0; raw.m_timestamp = pointCloud.m_timestamp; raw.m_numPoints = pointCloud.m_numPoints; raw.m_points = pinnedPoints.AddrOfPinnedObject(); return raw; }
/// <summary> /// Fill out <c>pointCloudData</c> with emulated values from Tango. /// </summary> /// <param name="pointCloudData">The point cloud data to fill out.</param> private static void _FillEmulatedPointCloud(ref TangoPointCloudData pointCloud) { List<Vector3> emulated = DepthProvider.GetTangoEmulation(out pointCloud.m_timestamp); pointCloud.m_numPoints = emulated.Count; for (int it = 0; it < emulated.Count; ++it) { pointCloud.m_points[(it * 4) + 0] = emulated[it].x; pointCloud.m_points[(it * 4) + 1] = emulated[it].y; pointCloud.m_points[(it * 4) + 2] = emulated[it].z; pointCloud.m_points[(it * 4) + 3] = 1; } }