/// <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> /// Fits a plane to a point cloud near a user-specified location. This /// occurs in two passes. First, all points in cloud within /// certain pixel distance 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> /// <c>true</c>, if plane 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="intersectionPoint"> /// The output point in depth camera coordinates that the user selected. /// </param> /// <param name="planeModel"> /// The four parameters a, b, c, d for the general plane /// equation ax + by + cz + d = 0 of the plane fit. The first three /// components are a unit vector. The output is in the coordinate system of /// the requested output frame. Cannot be NULL. /// </param> public static bool FitPlaneModelNearClick( TangoPointCloudData pointCloud, double colorCameraTimestamp, Vector2 uvCoordinates, out Vector3 intersectionPoint, out DVector4 planeModel) { 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); intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f); planeModel = new DVector4(); 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); DVector3 doubleIntersectionPoint = new DVector3(); DVector4 pointCloudRotation = DVector4.IdentityQuaternion; DVector3 pointCloudTranslation = DVector3.Zero; returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPoint( ref tangoPointCloudIntPtr, ref pointCloudTranslation, ref pointCloudRotation, ref uvCoordinatesTango, AndroidHelper.GetDisplayRotation(), ref depth_T_colorCameraPose.translation, ref depth_T_colorCameraPose.orientation, out doubleIntersectionPoint, out planeModel); pointCloudHandle.Free(); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.LogError("TangoSupport_fitPlaneModelNearPoint error. " + Environment.StackTrace); intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f); planeModel = new DVector4(); return(false); } else { intersectionPoint = doubleIntersectionPoint.ToVector3(); } return(true); }