/// <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="depth">Point cloud from Tango.</param> /// <param name="depthPose">Pose matrix the point cloud corresponds too.</param> private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose) { if (m_context == IntPtr.Zero) { Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace); return; } APIPointCloud apiCloud; apiCloud.numPoints = depth.xyz_count; apiCloud.points = IntPtr.Zero; apiCloud.timestamp = depth.timestamp; // This copy is required until TangoXYZij stores its depth as XYZC. long xyzPointerVal = depth.xyz.ToInt64(); for (int it = 0; it < depth.xyz_count; ++it) { int xyzIndex = it * 3; int depthPointsIndex = it * 4; Marshal.Copy(new IntPtr(xyzPointerVal + (xyzIndex * 4)), m_mostRecentDepthPoints, depthPointsIndex, 3); m_mostRecentDepthPoints[depthPointsIndex + 3] = 1; } APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose); if (!m_sendColorToUpdate) { GCHandle mostRecentDepthPointsHandle = GCHandle.Alloc(m_mostRecentDepthPoints, GCHandleType.Pinned); apiCloud.points = Marshal.UnsafeAddrOfPinnedArrayElement(m_mostRecentDepthPoints, 0); // 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, 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); mostRecentDepthPointsHandle.Free(); } 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_mostRecentDepthPose = apiDepthPose; m_mostRecentDepthIsValid = true; } } }
/// <summary> /// Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij) { // Fill in the data to draw the point cloud. if (xyzij != null) { lock (m_lockObject) { // copy single members m_tangoDepth.m_version = xyzij.version; m_tangoDepth.m_timestamp = xyzij.timestamp; m_tangoDepth.m_ijColumns = xyzij.ij_cols; m_tangoDepth.m_ijRows = xyzij.ij_rows; m_tangoDepth.m_pointCount = xyzij.xyz_count; // deep copy arrays // Fill in the data to draw the point cloud. if (xyzij != null) { int numberOfActivePoints = xyzij.xyz_count * 3; // copy new points if (numberOfActivePoints > 0) { Marshal.Copy(xyzij.xyz[0], m_tangoDepth.m_points, 0, numberOfActivePoints); m_isDirty = true; } } } } }
/// <summary> /// Calculates the depth in the color camera space at a user-specified /// location using bilateral filtering weighted by both spatial distance /// from the user coordinate and by intensity similarity. /// </summary> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success, /// Common.ErrorType.TANGO_INVALID on invalid input, and /// Common.ErrorType.TANGO_ERROR on failure. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least one point. /// </param> /// <param name="pointCount"> /// The number of points to read from the point cloud. /// </param> /// <param name="timestamp">The timestamp of the depth points.</param> /// <param name="cameraIntrinsics"> /// The camera intrinsics for the color camera. Cannot be null. /// </param> /// <param name="colorImage"> /// The color image buffer. Cannot be null. /// </param> /// <param name="matrix"> /// Transformation matrix of the color camera with respect to the Unity /// World frame. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// between (0.0, 0.0) and (1.0, 1.0). /// </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> /// <param name="isValidPoint"> /// A flag valued true if there is a point cloud point close to the user /// selection after projection onto the image plane and valued false /// otherwise. /// </param> public static int ScreenCoordinateToWorldBilateral( Vector3[] pointCloud, int pointCount, double timestamp, TangoCameraIntrinsics cameraIntrinsics, TangoImageBuffer colorImage, ref Matrix4x4 matrix, Vector2 uvCoordinates, out Vector3 colorCameraPoint, out bool isValidPoint) { GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned); TangoXYZij pointCloudXyzIj = new TangoXYZij(); pointCloudXyzIj.timestamp = timestamp; pointCloudXyzIj.xyz_count = pointCount; pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject(); DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix); // Unity has Y pointing screen up; Tango camera has Y pointing // screen down. Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y); int isValidPointInteger; int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointBilateralCameraIntrinsicsMatrixTransform( pointCloudXyzIj, cameraIntrinsics, colorImage, ref doubleMatrix, ref uvCoordinatesTango, out colorCameraPoint, out isValidPointInteger); isValidPoint = isValidPointInteger != 0; pointCloudHandle.Free(); return(returnValue); }
/// <summary> /// Calculates the depth in the color camera space at a user-specified /// location using nearest-neighbor interpolation. /// </summary> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success and /// Common.ErrorType.TANGO_INVALID on invalid input. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least one point. /// </param> /// <param name="pointCount"> /// The number of points to read from the point cloud. /// </param> /// <param name="timestamp">The timestamp of the depth points.</param> /// <param name="cameraIntrinsics"> /// The camera intrinsics for the color camera. Cannot be null. /// </param> /// <param name="matrix"> /// Transformation matrix of the color camera with respect to the Unity /// World frame. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// between (0.0, 0.0) and (1.0, 1.0). /// </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 0. /// </param> /// <param name="isValidPoint"> /// A flag valued 1 if there is a point cloud point close to the user /// selection after projection onto the image plane and valued 0 /// otherwise. /// </param> public static int GetDepthAtPointNearestNeighbor( Vector3[] pointCloud, int pointCount, double timestamp, TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix, Vector2 uvCoordinates, out Vector3 colorCameraPoint, out bool isValidPoint) { GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned); TangoXYZij pointCloudXyzIj = new TangoXYZij(); pointCloudXyzIj.timestamp = timestamp; pointCloudXyzIj.xyz_count = pointCount; pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject(); DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix); // Unity has Y pointing screen up; Tango camera has Y pointing // screen down. float[] uvCoordinatesArray = new float[2]; uvCoordinatesArray[0] = uvCoordinates.x; uvCoordinatesArray[1] = 1.0f - uvCoordinates.y; int isValidPointInteger; int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointNearestNeighborMatrixTransform( pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix, uvCoordinatesArray, out colorCameraPoint, out isValidPointInteger); isValidPoint = isValidPointInteger != 0; pointCloudHandle.Free(); return(returnValue); }
public static int TangoSupport_fitPlaneModelNearPointMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out DVector3 intersectionPoint, double[] planeModel) { intersectionPoint = new DVector3(); return(Common.ErrorType.TANGO_SUCCESS); }
public static int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out Vector3 colorCameraPoint, out int isValidPoint) { colorCameraPoint = Vector3.zero; isValidPoint = 1; return(Common.ErrorType.TANGO_SUCCESS); }
/// <summary> /// It's backwards, but fill emulated raw xyzij data with emulated TangoUnityDepth data. /// It is the responsibility of the caller to GC pin/free the pointCloudData's m_points. /// </summary> /// <returns>Emulated raw xyzij data.</returns> /// <param name="pointCouldData">Emulated point could data.</param>> /// <param name="pinnedPoints">Pinned array of pointCouldData.m_points.</param> private TangoXYZij _GetEmulatedRawXyzijData(TangoUnityDepth pointCouldData, GCHandle pinnedPoints) { TangoXYZij data = new TangoXYZij(); data.xyz = pinnedPoints.AddrOfPinnedObject(); data.xyz_count = pointCouldData.m_pointCount; data.ij_cols = 0; data.ij_rows = 0; data.ij = IntPtr.Zero; data.timestamp = pointCouldData.m_timestamp; return(data); }
/// <summary> /// Fits a plane to a point cloud near a user-specified location. This /// occurs in two passes. First, all points in cloud within /// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a /// plane is fit to the subset cloud using RANSAC. After the initial fit /// all inliers from the original cloud are used to refine the plane /// model. /// </summary> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success, /// Common.ErrorType.TANGO_INVALID on invalid input, and /// Common.ErrorType.TANGO_ERROR on failure. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least three points. /// </param> /// <param name="pointCount"> /// The number of points to read from the point cloud. /// </param> /// <param name="timestamp">The timestamp of the point cloud.</param> /// <param name="cameraIntrinsics"> /// The camera intrinsics for the color camera. Cannot be null. /// </param> /// <param name="matrix"> /// Transformation matrix of the color camera with respect to the Unity /// World frame. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// between (0.0, 0.0) and (1.0, 1.0). /// </param> /// <param name="intersectionPoint"> /// The output point in depth camera coordinates that the user selected. /// </param> /// <param name="plane">The plane fit.</param> public static int FitPlaneModelNearClick( Vector3[] pointCloud, int pointCount, double timestamp, TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix, Vector2 uvCoordinates, out Vector3 intersectionPoint, out Plane plane) { GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned); TangoXYZij pointCloudXyzIj = new TangoXYZij(); pointCloudXyzIj.timestamp = timestamp; pointCloudXyzIj.xyz_count = pointCount; pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject(); DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix); // Unity has Y pointing screen up; Tango camera has Y pointing // screen down. Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y); DVector3 doubleIntersectionPoint = new DVector3(); double[] planeArray = new double[4]; int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform( pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix, ref uvCoordinatesTango, out doubleIntersectionPoint, planeArray); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f); plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f); } else { intersectionPoint = doubleIntersectionPoint.ToVector3(); Vector3 normal = new Vector3((float)planeArray[0], (float)planeArray[1], (float)planeArray[2]); float distance = (float)planeArray[3] / normal.magnitude; plane = new Plane(normal, distance); } pointCloudHandle.Free(); return(returnValue); }
/// <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="depth">Point cloud from Tango.</param> /// <param name="depthPose">Pose matrix the point cloud corresponds too.</param> private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose) { if (m_context == IntPtr.Zero) { Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace); return; } APIPointCloud apiCloud; apiCloud.numPoints = depth.xyz_count; apiCloud.points = depth.xyz; apiCloud.timestamp = depth.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, 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; m_mostRecentDepthPose = apiDepthPose; Marshal.Copy(apiCloud.points, m_mostRecentDepthPoints, 0, apiCloud.numPoints * 3); m_mostRecentDepthIsValid = true; } } }
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> /// Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij) { // Fill in the data to draw the point cloud. if (xyzij != null) { // copy single members m_tangoDepth.m_version = xyzij.version; m_tangoDepth.m_timestamp = xyzij.timestamp; m_tangoDepth.m_ijColumns = xyzij.ij_cols; m_tangoDepth.m_ijRows = xyzij.ij_rows; m_tangoDepth.m_pointCount = xyzij.xyz_count; // deep copy arrays // Fill in the data to draw the point cloud. if (xyzij != null) { int numberOfActiveVertices = xyzij.xyz_count; if (numberOfActiveVertices > 0) { float[] allPositions = new float[numberOfActiveVertices * 3]; Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length); for (int i = 0; i < numberOfActiveVertices; ++i) { if (i < m_tangoDepth.m_pointCount) { m_tangoDepth.m_vertices[i].x = allPositions[i * 3]; m_tangoDepth.m_vertices[i].y = allPositions[(i * 3) + 1]; m_tangoDepth.m_vertices[i].z = allPositions[(i * 3) + 2]; } else { m_tangoDepth.m_vertices[i].x = m_tangoDepth.m_vertices[i].y = m_tangoDepth.m_vertices[i].z = 0.0f; } } m_isDirty = true; } } } }
/// <summary> /// Raise a Tango depth event if there is new data. /// </summary> internal void SendDepthIfAvailable() { #if UNITY_EDITOR lock (m_lockObject) { if (DepthProvider.m_emulationIsDirty) { DepthProvider.m_emulationIsDirty = false; if (m_onTangoDepthAvailable != null || m_onTangoDepthMultithreadedAvailable != null) { _FillEmulatedPointCloudData(m_tangoDepth); } if (m_onTangoDepthMultithreadedAvailable != null) { // Pretend to be making a call from unmanaged code. GCHandle pinnedPoints = GCHandle.Alloc(m_tangoDepth.m_points, GCHandleType.Pinned); TangoXYZij emulatedXyzij = _GetEmulatedRawXyzijData(m_tangoDepth, pinnedPoints); m_onTangoDepthMultithreadedAvailable(emulatedXyzij); pinnedPoints.Free(); } if (m_onTangoDepthAvailable != null) { m_isDirty = true; } } } #endif if (m_isDirty && m_onTangoDepthAvailable != null) { lock (m_lockObject) { m_onTangoDepthAvailable(m_tangoDepth); } m_isDirty = false; } }
/// <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; } // 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(tangoDepth, world_T_depthCamera); }
/// <summary> /// Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected abstract void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij);
public static int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out Vector3 colorCameraPoint, out int isValidPoint) { colorCameraPoint = Vector3.zero; isValidPoint = 1; return Common.ErrorType.TANGO_SUCCESS; }
public static int TangoSupport_fitPlaneModelNearClickMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics intrinsics, ref Matrix4x4 colorCameraTUnityWorld, float[] uvCoordinates, double[] intersectionPoint, double[] planeModel) { return(Common.ErrorType.TANGO_SUCCESS); }
/// <summary> /// Callback that gets called when depth is available /// from the Tango Service. /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION! /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected override void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij) { // Calculate the time since the last successful depth data // collection. if (m_previousDepthDeltaTime == 0.0) { m_previousDepthDeltaTime = xyzij.timestamp; } else { m_numberOfDepthSamples++; m_timeSinceLastDepthFrame = xyzij.timestamp - m_previousDepthDeltaTime; m_previousDepthDeltaTime = xyzij.timestamp; } // Fill in the data to draw the point cloud. if (xyzij != null && m_vertices != null) { int numberOfActiveVertices = xyzij.xyz_count; m_pointsCount = numberOfActiveVertices; if(numberOfActiveVertices > 0) { float[] allPositions = new float[numberOfActiveVertices * 3]; Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length); for(int i = 0; i < m_vertices.Length; ++i) { if( i < xyzij.xyz_count ) { m_vertices[i].x = allPositions[i * 3]; m_vertices[i].y = allPositions[(i * 3) + 1]; m_vertices[i].z = allPositions[(i * 3) + 2]; } else { m_vertices[i].x = m_vertices[i].y = m_vertices[i].z = 0.0f; } } m_isDirty = true; } } }
public static extern int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out Vector3 colorCameraPoint, [Out, MarshalAs(UnmanagedType.I4)] out int isValidPoint);
/// <summary> /// Calculates the depth in the color camera space at a user-specified /// location using bilateral filtering weighted by both spatial distance /// from the user coordinate and by intensity similarity. /// </summary> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success, /// Common.ErrorType.TANGO_INVALID on invalid input, and /// Common.ErrorType.TANGO_ERROR on failure. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least one point. /// </param> /// <param name="pointCount"> /// The number of points to read from the point cloud. /// </param> /// <param name="timestamp">The timestamp of the depth points.</param> /// <param name="cameraIntrinsics"> /// The camera intrinsics for the color camera. Cannot be null. /// </param> /// <param name="colorImage"> /// The color image buffer. Cannot be null. /// </param> /// <param name="matrix"> /// Transformation matrix of the color camera with respect to the Unity /// World frame. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// between (0.0, 0.0) and (1.0, 1.0). /// </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> /// <param name="isValidPoint"> /// A flag valued true if there is a point cloud point close to the user /// selection after projection onto the image plane and valued false /// otherwise. /// </param> public static int ScreenCoordinateToWorldBilateral( Vector3[] pointCloud, int pointCount, double timestamp, TangoCameraIntrinsics cameraIntrinsics, TangoImageBuffer colorImage, ref Matrix4x4 matrix, Vector2 uvCoordinates, out Vector3 colorCameraPoint, out bool isValidPoint) { GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned); TangoXYZij pointCloudXyzIj = new TangoXYZij(); pointCloudXyzIj.timestamp = timestamp; pointCloudXyzIj.xyz_count = pointCount; pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject(); DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix); // Unity has Y pointing screen up; Tango camera has Y pointing // screen down. Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y); int isValidPointInteger; int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointBilateralCameraIntrinsicsMatrixTransform( pointCloudXyzIj, cameraIntrinsics, colorImage, ref doubleMatrix, ref uvCoordinatesTango, out colorCameraPoint, out isValidPointInteger); isValidPoint = isValidPointInteger != 0; pointCloudHandle.Free(); return returnValue; }
/// <summary> /// Fits a plane to a point cloud near a user-specified location. This /// occurs in two passes. First, all points in cloud within /// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a /// plane is fit to the subset cloud using RANSAC. After the initial fit /// all inliers from the original cloud are used to refine the plane /// model. /// </summary> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success, /// Common.ErrorType.TANGO_INVALID on invalid input, and /// Common.ErrorType.TANGO_ERROR on failure. /// </returns> /// <param name="pointCloud"> /// The point cloud. Cannot be null and must have at least three points. /// </param> /// <param name="pointCount"> /// The number of points to read from the point cloud. /// </param> /// <param name="timestamp">The timestamp of the point cloud.</param> /// <param name="cameraIntrinsics"> /// The camera intrinsics for the color camera. Cannot be null. /// </param> /// <param name="matrix"> /// Transformation matrix of the color camera with respect to the Unity /// World frame. /// </param> /// <param name="uvCoordinates"> /// The UV coordinates for the user selection. This is expected to be /// between (0.0, 0.0) and (1.0, 1.0). /// </param> /// <param name="intersectionPoint"> /// The output point in depth camera coordinates that the user selected. /// </param> /// <param name="plane">The plane fit.</param> public static int FitPlaneModelNearClick( Vector3[] pointCloud, int pointCount, double timestamp, TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix, Vector2 uvCoordinates, out Vector3 intersectionPoint, out Plane plane) { GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned); TangoXYZij pointCloudXyzIj = new TangoXYZij(); pointCloudXyzIj.timestamp = timestamp; pointCloudXyzIj.xyz_count = pointCount; pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject(); DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix); // Unity has Y pointing screen up; Tango camera has Y pointing // screen down. Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y); DVector3 doubleIntersectionPoint = new DVector3(); double[] planeArray = new double[4]; int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform( pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix, ref uvCoordinatesTango, out doubleIntersectionPoint, planeArray); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f); plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f); } else { intersectionPoint = doubleIntersectionPoint.ToVector3(); Vector3 normal = new Vector3((float)planeArray[0], (float)planeArray[1], (float)planeArray[2]); float distance = (float)planeArray[3] / normal.magnitude; plane = new Plane(normal, distance); } pointCloudHandle.Free(); return returnValue; }
/// <summary> /// It's backwards, but fill an emulated TangoXYZij 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 raw xyzij data.</returns> /// <param name="depth">Emulated point cloud data.</param>> /// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param> private static TangoXYZij _GetEmulatedRawXyzijData(TangoUnityDepth depth, GCHandle pinnedPoints) { TangoXYZij data = new TangoXYZij(); data.xyz = pinnedPoints.AddrOfPinnedObject(); data.xyz_count = depth.m_pointCount; data.ij_cols = 0; data.ij_rows = 0; data.ij = IntPtr.Zero; data.timestamp = depth.m_timestamp; return data; }
/// <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; } }
public static extern int TangoSupport_fitPlaneModelNearClickMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics intrinsics, ref Matrix4x4 matrix, [In, MarshalAs(UnmanagedType.LPArray, SizeConst = 2)] float[] uvCoordinates, [Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 3)] double[] intersectionPoint, [Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
/// <summary> /// DEPRECATED: Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij) { // Fill in the data to draw the point cloud. if (xyzij != null) { lock (m_lockObject) { // copy single members m_tangoDepth.m_version = xyzij.version; m_tangoDepth.m_timestamp = xyzij.timestamp; m_tangoDepth.m_ijColumns = xyzij.ij_cols; m_tangoDepth.m_ijRows = xyzij.ij_rows; m_tangoDepth.m_pointCount = xyzij.xyz_count; // deep copy arrays // Fill in the data to draw the point cloud. if (xyzij != null) { int numberOfActivePoints = xyzij.xyz_count * 3; // copy new points if (numberOfActivePoints > 0) { Marshal.Copy(xyzij.xyz[0], m_tangoDepth.m_points, 0, numberOfActivePoints); m_isDirty = true; } } } } }
public static extern int TangoSupport_fitPlaneModelNearPointMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out DVector3 intersectionPoint, [Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
/// <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); }
public static int TangoSupport_fitPlaneModelNearPointMatrixTransform( TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics, ref DMatrix4x4 matrix, ref Vector2 uvCoordinates, out DVector3 intersectionPoint, double[] planeModel) { intersectionPoint = new DVector3(); return Common.ErrorType.TANGO_SUCCESS; }
/// <summary> /// Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij) { // Fill in the data to draw the point cloud. if (xyzij != null) { // copy single members m_tangoDepth.m_version = xyzij.version; m_tangoDepth.m_timestamp = xyzij.timestamp; m_tangoDepth.m_ijColumns = xyzij.ij_cols; m_tangoDepth.m_ijRows = xyzij.ij_rows; m_tangoDepth.m_pointCount = xyzij.xyz_count; // deep copy arrays // Fill in the data to draw the point cloud. if (xyzij != null) { int numberOfActiveVertices = xyzij.xyz_count; if(numberOfActiveVertices > 0) { float[] allPositions = new float[numberOfActiveVertices * 3]; Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length); for(int i = 0; i < numberOfActiveVertices; ++i) { if( i < m_tangoDepth.m_pointCount ) { m_tangoDepth.m_vertices[i].x = allPositions[i * 3]; m_tangoDepth.m_vertices[i].y = allPositions[(i * 3) + 1]; m_tangoDepth.m_vertices[i].z = allPositions[(i * 3) + 2]; } else { m_tangoDepth.m_vertices[i].x = m_tangoDepth.m_vertices[i].y = m_tangoDepth.m_vertices[i].z = 0.0f; } } m_isDirty = true; } } } }