/// <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> /// 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); }
/// <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> /// Detect one or more markers in the input image. /// </summary> /// <param name="imageBuffer"> /// The input image buffer. /// </param> /// <param name="cameraId"> /// Camera that is used for detecting markers, can be TangoEnums.TangoCameraId.TANGO_CAMERA_FISHEYE or /// TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR. /// </param> /// <param name="markerType"> /// Target marker's type. Current support marker types are QR marker and Alvar marker. /// </param> /// <param name="markerSize"> /// Physical size of marker's length. /// </param> /// <param name="markers"> /// The returned marker list. /// </param> /// <returns> /// Common.ErrorType.TANGO_SUCCESS on success, Common.ErrorType.TANGO_INVALID on invalid input, and /// Common.ErrorType.TANGO_ERROR on failure. /// </returns> public static bool DetectMarkers(TangoUnityImageData imageBuffer, TangoEnums.TangoCameraId cameraId, MarkerType markerType, double markerSize, List <Marker> markers) { if (markers == null) { Debug.Log("markers is null. " + Environment.StackTrace); return(false); } // Clear any existing marker markers.Clear(); // Detect marker. TangoImageBuffer buffer = new TangoImageBuffer(); GCHandle gchandle = GCHandle.Alloc(imageBuffer.data, GCHandleType.Pinned); IntPtr ptr = gchandle.AddrOfPinnedObject(); buffer.data = ptr; buffer.format = imageBuffer.format; buffer.frame_number = imageBuffer.frame_number; buffer.height = imageBuffer.height; buffer.stride = imageBuffer.stride; buffer.timestamp = imageBuffer.timestamp; buffer.width = imageBuffer.width; // Get Pose. TangoPoseData poseData = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR; PoseProvider.GetPoseAtTime(poseData, buffer.timestamp, pair); APIMarkerList rawAPIMarkerList = new APIMarkerList(); APIMarkerParam rawMarkerParam = new APIMarkerParam(markerType, markerSize); int ret = TangoSupportAPI.TangoSupport_detectMarkers(ref buffer, cameraId, ref poseData.translation, ref poseData.orientation, ref rawMarkerParam, ref rawAPIMarkerList); gchandle.Free(); if (ret != Common.ErrorType.TANGO_SUCCESS) { return(false); } if (rawAPIMarkerList.markerCount != 0) { List <APIMarker> apiMarkers = new List <APIMarker>(); MarshallingHelper.MarshalUnmanagedStructArrayToList <TangoSupport.APIMarker>( rawAPIMarkerList.markers, rawAPIMarkerList.markerCount, apiMarkers); for (int i = 0; i < apiMarkers.Count; ++i) { APIMarker apiMarker = apiMarkers[i]; Marker marker = new Marker(); marker.m_type = apiMarker.m_type; marker.m_timestamp = apiMarker.m_timestamp; marker.m_content = apiMarker.m_content; // Covert 2D corner points from pixel space to UV space. marker.m_corner2DP0.x = apiMarker.m_corner2DP0.x / buffer.width; marker.m_corner2DP0.y = apiMarker.m_corner2DP0.y / buffer.height; marker.m_corner2DP1.x = apiMarker.m_corner2DP1.x / buffer.width; marker.m_corner2DP1.y = apiMarker.m_corner2DP1.y / buffer.height; marker.m_corner2DP2.x = apiMarker.m_corner2DP2.x / buffer.width; marker.m_corner2DP2.y = apiMarker.m_corner2DP2.y / buffer.height; marker.m_corner2DP3.x = apiMarker.m_corner2DP3.x / buffer.width; marker.m_corner2DP3.y = apiMarker.m_corner2DP3.y / buffer.height; // Convert 3D corner points from Start of Service space to Unity World space. marker.m_corner3DP0 = GetMarkerInUnitySpace(apiMarker.m_corner3DP0); marker.m_corner3DP1 = GetMarkerInUnitySpace(apiMarker.m_corner3DP1); marker.m_corner3DP2 = GetMarkerInUnitySpace(apiMarker.m_corner3DP2); marker.m_corner3DP3 = GetMarkerInUnitySpace(apiMarker.m_corner3DP3); // Convert pose from Start of Service to Unity World space. Vector3 translation = new Vector3( (float)apiMarker.m_translation.x, (float)apiMarker.m_translation.y, (float)apiMarker.m_translation.z); Quaternion orientation = new Quaternion( (float)apiMarker.m_rotation.x, (float)apiMarker.m_rotation.y, (float)apiMarker.m_rotation.z, (float)apiMarker.m_rotation.w); Matrix4x4 ss_T_marker = Matrix4x4.TRS(translation, orientation, Vector3.one); // Note that UNITY_WORLD_T_START_SERVICE is involutory matrix. The actually transform // we wanted to multiply on the right hand side is START_SERVICE_T_UNITY_WORLD. Matrix4x4 uw_T_u_marker = TangoSupport.UNITY_WORLD_T_START_SERVICE * ss_T_marker * TangoSupport.UNITY_WORLD_T_START_SERVICE; marker.m_translation = uw_T_u_marker.GetColumn(3); marker.m_orientation = Quaternion.LookRotation(uw_T_u_marker.GetColumn(2), uw_T_u_marker.GetColumn(1)); // Add the marker to the output list markers.Add(marker); } } TangoSupportAPI.TangoSupport_freeMarkerList(ref rawAPIMarkerList); return(false); }
/// <summary> /// Initialize the support library with function pointers required by /// the library. Either this version or @c TangoSupport_initializeLibrary /// should be called during application initialization, but not both. This /// version requires each of the initialization parameters and should only be /// used if specialized parameters are necessary. /// NOTE: This function must be called after the Android service has been /// bound. /// </summary> public static void Initialize() { TangoSupportAPI.TangoUnity_initializeTangoSupportLibrary(); }