/// <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); }
public static extern int TangoSupport_getDepthAtPointNearestNeighbor( ref TangoPointCloudIntPtr point_cloud, ref DVector3 poitnCloudTranslation, ref DVector4 pointCloudOrientation, ref Vector2 uvCoordinatesInColorCamera, OrientationManager.Rotation display_rotation, ref DVector3 colorCameraTranslation, ref DVector4 colorCameraOrientation, out Vector3 outputPoint);
public static extern int TangoSupport_fitPlaneModelNearPoint( ref TangoPointCloudIntPtr pointCloud, ref DVector3 pointCloudTranslation, ref DVector4 pointCloundOrientation, ref Vector2 uvCoordinates, OrientationManager.Rotation rotation, ref DVector3 cameraTranslation, ref DVector4 cameraOrientation, out DVector3 intersectionPoint, out DVector4 planeModel);
public static int TangoSupport_getDepthAtPointNearestNeighbor( ref TangoPointCloudIntPtr point_cloud, ref DVector3 poitnCloudTranslation, ref DVector4 pointCloudOrientation, ref Vector2 uvCoordinatesInColorCamera, OrientationManager.Rotation display_rotation, ref DVector3 colorCameraTranslation, ref DVector4 colorCameraOrientation, out Vector3 outputPoint) { outputPoint = Vector3.zero; return(Common.ErrorType.TANGO_SUCCESS); }
public static int TangoSupport_fitPlaneModelNearPoint( ref TangoPointCloudIntPtr pointCloud, ref DVector3 pointCloudTranslation, ref DVector4 pointCloundOrientation, ref Vector2 uvCoordinates, OrientationManager.Rotation rotation, ref DVector3 cameraTranslation, ref DVector4 cameraOrientation, out DVector3 intersectionPoint, out DVector4 planeModel) { intersectionPoint = new DVector3(); planeModel = new DVector4(); return(Common.ErrorType.TANGO_SUCCESS); }
private static void _OnPointCloudAvailable(IntPtr callbackContext, ref TangoPointCloudIntPtr rawPointCloud) { // Fill in the data to draw the point cloud. if (m_onPointCloudMultithreadedAvailable != null) { m_onPointCloudMultithreadedAvailable(ref rawPointCloud); } lock (m_lockObject) { // copy single members m_pointCloud.m_timestamp = rawPointCloud.m_timestamp; m_pointCloud.m_numPoints = rawPointCloud.m_numPoints; if (rawPointCloud.m_numPoints > 0) { Marshal.Copy(rawPointCloud.m_points, m_pointCloud.m_points, 0, rawPointCloud.m_numPoints * 4); } m_isDirty = true; } // This must be done after the above Marshal.Copy so that it can efficiently reduce the array to just XYZ. if (m_onTangoDepthMultithreadedAvailable != null) { TangoXYZij xyzij = new TangoXYZij(); xyzij.version = rawPointCloud.m_version; xyzij.timestamp = rawPointCloud.m_timestamp; xyzij.xyz_count = rawPointCloud.m_numPoints; xyzij.ij_rows = 0; xyzij.ij_cols = 0; xyzij.ij = IntPtr.Zero; xyzij.color_image = IntPtr.Zero; for (int it = 0; it < m_pointCloud.m_numPoints; ++it) { m_xyzPoints[(it * 3) + 0] = m_pointCloud.m_points[(it * 4) + 0]; m_xyzPoints[(it * 3) + 1] = m_pointCloud.m_points[(it * 4) + 1]; m_xyzPoints[(it * 3) + 2] = m_pointCloud.m_points[(it * 4) + 2]; } GCHandle pinnedXyzPoints = GCHandle.Alloc(m_xyzPoints, GCHandleType.Pinned); xyzij.xyz = pinnedXyzPoints.AddrOfPinnedObject(); m_onTangoDepthMultithreadedAvailable(xyzij); pinnedXyzPoints.Free(); } }
/// <summary> /// Raise a Tango depth event if there is new data. /// </summary> internal static void SendIfAvailable() { if (m_onPointCloudAvailableCallback == null) { return; } #if UNITY_EDITOR lock (m_lockObject) { if (DepthProvider.m_emulationIsDirty) { DepthProvider.m_emulationIsDirty = false; if (m_onTangoDepthAvailable != null || m_onTangoDepthMultithreadedAvailable != null || m_onPointCloudAvailable != null | m_onPointCloudMultithreadedAvailable != null) { _FillEmulatedPointCloud(ref m_pointCloud); } if (m_onTangoDepthMultithreadedAvailable != null) { // Pretend to be making a call from unmanaged code. TangoUnityDepth depth = new TangoUnityDepth(m_pointCloud); GCHandle pinnedPoints = GCHandle.Alloc(depth.m_points, GCHandleType.Pinned); TangoXYZij emulatedXyzij = _GetEmulatedRawXyzijData(depth, pinnedPoints); m_onTangoDepthMultithreadedAvailable(emulatedXyzij); pinnedPoints.Free(); } if (m_onPointCloudMultithreadedAvailable != null) { // Pretend to be making a call from unmanaged code. GCHandle pinnedPoints = GCHandle.Alloc(m_pointCloud.m_points, GCHandleType.Pinned); TangoPointCloudIntPtr rawData = _GetEmulatedRawData(m_pointCloud, pinnedPoints); m_onPointCloudMultithreadedAvailable(ref rawData); pinnedPoints.Free(); } if (m_onTangoDepthAvailable != null || m_onPointCloudAvailable != null) { m_isDirty = true; } } } #endif if (m_isDirty && (m_onTangoDepthAvailable != null || m_onPointCloudAvailable != null)) { lock (m_lockObject) { _ReducePointCloudPoints(m_pointCloud, m_maxNumReducedDepthPoints); if (m_onTangoDepthAvailable != null) { m_onTangoDepthAvailable(new TangoUnityDepth(m_pointCloud)); } if (m_onPointCloudAvailable != null) { m_onPointCloudAvailable(m_pointCloud); } } m_isDirty = false; } }
/// <summary> /// Update the 3D Reconstruction with a new point cloud and pose. /// /// It is expected this will get called in from the Tango binder thread. /// </summary> /// <param name="pointCloud">Point cloud from Tango.</param> /// <param name="depthPose">Pose matrix the point cloud corresponds too.</param> private void _UpdateDepth(TangoPointCloudIntPtr pointCloud, Matrix4x4 depthPose) { if (m_context == IntPtr.Zero) { Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace); return; } APIPointCloud apiCloud; apiCloud.numPoints = pointCloud.m_numPoints; apiCloud.points = pointCloud.m_points; apiCloud.timestamp = pointCloud.m_timestamp; APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose); if (!m_sendColorToUpdate) { // No need to wait for a color image, update reconstruction immediately. IntPtr rawUpdatedIndices; Status result = (Status)API.Tango3DR_update( m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, out rawUpdatedIndices); if (result != Status.SUCCESS) { Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace); return; } _AddUpdatedIndices(rawUpdatedIndices); API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices); } else { lock (m_lockObject) { // We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud // because there are much less depth points than pixels. m_mostRecentDepth = apiCloud; m_mostRecentDepth.points = IntPtr.Zero; Marshal.Copy(pointCloud.m_points, m_mostRecentDepthPoints, 0, pointCloud.m_numPoints * 4); m_mostRecentDepthPose = apiDepthPose; m_mostRecentDepthIsValid = true; } } }
/// <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); }
private static void _OnPointCloudAvailable(IntPtr callbackContext, ref TangoPointCloudIntPtr rawPointCloud) { // Fill in the data to draw the point cloud. if (m_onPointCloudMultithreadedAvailable != null) { m_onPointCloudMultithreadedAvailable(ref rawPointCloud); } lock (m_lockObject) { // copy single members m_pointCloud.m_timestamp = rawPointCloud.m_timestamp; m_pointCloud.m_numPoints = rawPointCloud.m_numPoints; if (rawPointCloud.m_numPoints > 0) { Marshal.Copy(rawPointCloud.m_points, m_pointCloud.m_points, 0, rawPointCloud.m_numPoints * 4); } m_isDirty = true; } // This must be done after the above Marshal.Copy so that it can efficiently reduce the array to just XYZ. if (m_onTangoDepthMultithreadedAvailable != null) { TangoXYZij xyzij = new TangoXYZij(); xyzij.version = rawPointCloud.m_version; xyzij.timestamp = rawPointCloud.m_timestamp; xyzij.xyz_count = rawPointCloud.m_numPoints; xyzij.ij_rows = 0; xyzij.ij_cols = 0; xyzij.ij = IntPtr.Zero; xyzij.color_image = IntPtr.Zero; for (int it = 0; it < m_pointCloud.m_numPoints; ++it) { m_xyzPoints[(it * 3) + 0] = m_pointCloud.m_points[(it * 4) + 0]; m_xyzPoints[(it * 3) + 1] = m_pointCloud.m_points[(it * 4) + 1]; m_xyzPoints[(it * 3) + 2] = m_pointCloud.m_points[(it * 4) + 2]; } GCHandle pinnedXyzPoints = GCHandle.Alloc(m_xyzPoints, GCHandleType.Pinned); xyzij.xyz = pinnedXyzPoints.AddrOfPinnedObject(); m_onTangoDepthMultithreadedAvailable(xyzij); pinnedXyzPoints.Free(); } }