DepthCamera Data Access and Visualization

Unity Editor version : 2022.3.12f1
ML2 OS version : 1.5.0
Unity SDK version : 1.12.1
Host OS : Windows

I am developing a Unity application that captures the depth data from the DepthCamera and displays it as a texture on a Quad GameObject. I found the following code snippet on one of the earlier posts in the forum but it is not working. All I get is a blank (black) texture that never changes or updates.

    private IEnumerator Start()
    {
        yield return ProcessDepthData();
    }

    private void InitializeMagicLeapInputs()
    {
        magicLeapInputs = new MagicLeapInputs();
        magicLeapInputs.Enable();
        controllerActions = new MagicLeapInputs.ControllerActions(magicLeapInputs);
        controllerActions.Enable();
    }

    /// <summary>
    /// Checks for depth camera permissions and requests them if not already granted.
    /// Starts the depth camera after granting the permission.
    /// </summary>
    private void CheckAndRequestDepthCameraPermission()
    {
        if (MLPermissions.CheckPermission(MLPermission.DepthCamera).IsOk)
        {
            StartDepthCamera();
        }
        else
        {
            MLPermissions.Callbacks callbacks = new MLPermissions.Callbacks();
            callbacks.OnPermissionGranted += _ => StartDepthCamera();
            MLPermissions.RequestPermission(MLPermission.DepthCamera, callbacks);
        }
    }

    /// <summary>
    /// Starts the depth camera if it's not already running by setting its configuration and connecting to it.
    /// </summary>
    private void StartDepthCamera()
    {
        if (isDepthCameraRunning)
        {
            Debug.LogWarning("DepthCamera: Already running.");
            return;
        }

        MLDepthCamera.Settings settings = ConfigureDepthCameraSettings();
        MLDepthCamera.SetSettings(settings);

        Debug.Log("DepthCamera: StartDepthCamera() - Settings set");

        MLResult result = MLDepthCamera.Connect();

        if (result.IsOk)
        {
            isDepthCameraRunning = true;
            Debug.Log("DepthCamera: Connected.");
        }
        else
        {
            Debug.LogError($"DepthCamera: Connection failed with error: {result}.");
        }
    }

    /// <summary>
    /// Sets the settings for the depth camera to preset values, including stream configuration and exposure settings.
    /// </summary>
    /// <returns>Returns the configured settings to be applied when starting the depth camera</returns>
    private MLDepthCamera.Settings ConfigureDepthCameraSettings()
    {
        uint flags = (uint)(MLDepthCamera.CaptureFlags.DepthImage | MLDepthCamera.CaptureFlags.Confidence);
        MLDepthCamera.StreamConfig longConfig = new MLDepthCamera.StreamConfig
        {
            FrameRateConfig = MLDepthCamera.FrameRate.FPS_5,
            Flags = flags,
            Exposure = 1600
        };

        MLDepthCamera.StreamConfig shortConfig = new MLDepthCamera.StreamConfig
        {
            FrameRateConfig = MLDepthCamera.FrameRate.FPS_30,
            Flags = flags,
            Exposure = 375
        };

        return new MLDepthCamera.Settings
        {
            Streams = depthStream,
            StreamConfig = new[] { longConfig, shortConfig }
        };
    }

    /// <summary>
    /// Stops the depth camera if it is running.
    /// </summary>
    private void StopDepthCamera()
    {
        if (!isDepthCameraRunning)
        {
            Debug.LogWarning("DepthCamera: Not running.");
            return;
        }

        Debug.Log($"DepthCamera: StopDepthCamera() - Stopping depthTexture camera");

        MLResult result = MLDepthCamera.Disconnect();

        if (result.IsOk)
        {
            isDepthCameraRunning = false;
            Debug.Log("DepthCamera: Disconnected.");
        }
        else
        {
            Debug.LogError($"DepthCamera: Disconnection failed with error: {result}.");
        }
    }

    /// <summary>
    /// Coroutine for processing depth data continuously while the depth camera is running. 
    /// It waits for the depth camera to start, then enters a loop to fetch and process the latest depth data
    /// at regular intervals, updating textures and calculating point clouds based on the depth and confidence data.
    /// </summary>
    private IEnumerator ProcessDepthData()
    {
        // Wait until the depth camera has started before proceeding.
        yield return new WaitUntil(() => isDepthCameraRunning);

        // Continue processing depth data as long as the depth camera is running.
        while (isDepthCameraRunning)
        {
            MLDepthCamera.Data data;

            // Loop until valid depth data is received, including both depth image and confidence values.
            while (!MLDepthCamera.GetLatestDepthData(1, out data).IsOk || !data.DepthImage.HasValue ||
                   !data.ConfidenceBuffer.HasValue)
            {
                // Wait until the next frame before trying again.
                yield return null;
            }

            // Prepare and update the textures for both depth and confidence data using the latest valid data received.
            depthTexture = CreateOrUpdateTexture(depthTexture, data.DepthImage.Value);
            confidenceTexture = CreateOrUpdateTexture(confidenceTexture, data.ConfidenceBuffer.Value);

            // Check if the controller's bumper is pressed to trigger point cloud calculation.
            if (controllerActions.Bumper.IsPressed())
            {
                Debug.Log("DepthCloud: Calculate Point Cloud - Started");

                // Create a transformation matrix from the depth camera's position and rotation to world space.
                Matrix4x4 cameraToWorldMatrix = new Matrix4x4();
                cameraToWorldMatrix.SetTRS(data.Position, data.Rotation, Vector3.one);

                // Calculate the point cloud based on the current depth data and camera position.
                yield return CalculatePointCloud(data.Intrinsics, cameraToWorldMatrix);

            }

            yield return null;
        }
    }

    /// <summary>
    /// Calculates the point cloud from depth and confidence textures using camera intrinsics and a transformation matrix.
    /// </summary>
    /// <param name="intrinsics">The depth camera's intrinsic parameters./param>
    /// <param name="cameraToWorldMatrix">A transform matrix based on the depth camera's position and rotation.</param>
    private IEnumerator CalculatePointCloud(MLDepthCamera.Intrinsics intrinsics, Matrix4x4 cameraToWorldMatrix)
    {
        var depthData = depthTexture.GetRawTextureData<float>();
        var confidenceData = confidenceTexture.GetRawTextureData<float>();
        Vector2Int resolution = new Vector2Int((int)intrinsics.Width, (int)intrinsics.Height);

        Task t = Task.Run(() =>
        {
            // Ensure the projection table is calculated and cached to avoid recomputation.
            if (cachedProjectionTable == null)
            {
                cachedProjectionTable = CreateProjectionTable(intrinsics);
                Debug.Log("DepthCloud: Projection Table Created");
            }

            // Process depth points to populate the cachedDepthPoints array with world positions.
            ProcessDepthPoints(ref cachedDepthPoints, depthData, confidenceData, resolution, cameraToWorldMatrix);

        });

        yield return new WaitUntil(() => t.IsCompleted);

        Debug.Log("DepthCloud: Updating Renderer");
        // Update the point cloud renderer with the newly calculated points.
        // pointCloudRenderer.UpdatePointCloud(cachedDepthPoints);
    }

    /// <summary>
    /// Processes range data from a sensor to generate a point cloud. This function transforms sensor range data,
    /// which measures the distance to each point in the sensor's view, into 3D world coordinates. It takes into account
    /// the resolution of the range data, a confidence map for filtering purposes, and the camera's position and orientation
    /// in the world to accurately map each point from sensor space to world space.
    /// (Range Image = distance to point, Depth Image = distance to plane)
    /// </summary>
    /// <param name="depthPoints">Reference to an array of Vector3 points that will be populated with the world coordinates of each depth point. This array is directly modified to contain the results.</param>
    /// <param name="depthData">A NativeArray of float values representing the range data from the depth sensor. Each float value is the distance from the sensor to the point in the scene.</param>
    /// <param name="confidenceData">A NativeArray of float values representing the confidence for each point in the depthData array. This is used to filter out unreliable data points based on a confidence threshold.</param>
    /// <param name="resolution">The resolution of the depth sensor's output, given as a Vector2Int where x is the width and y is the height of the depth data array.</param>
    /// <param name="cameraToWorldMatrix">A Matrix4x4 representing the transformation from camera space to world space. This is used to translate each point's coordinates into the global coordinate system.</param>
    private void ProcessDepthPoints(ref Vector3[] depthPoints, NativeArray<float> depthData, NativeArray<float> confidenceData, Vector2Int resolution, Matrix4x4 cameraToWorldMatrix)
    {
        // Initialize or resize the depthPoints array based on the current resolution, if necessary.
        if (depthPoints == null || depthPoints.Length != depthData.Length)
        {
            depthPoints = new Vector3[resolution.x * resolution.y];
            Debug.Log("DepthCloud: Initializing New Depth Array");
        }

        Debug.Log($"DepthCloud: Processing Depth. Resolution : {resolution.x} x {resolution.y}");

        // Iterate through each pixel in the depth data.
        for (int y = 0; y < resolution.y; ++y)
        {
            for (int x = 0; x < resolution.x; ++x)
            {
                // Calculate the linear index based on x, y coordinates.
                int index = x + (resolution.y - y - 1) * resolution.x;
                float depth = depthData[index];

                // Skip processing if depth is out of range or confidence is too low (if filter is enabled).
                // Confidence comes directly from the sensor pipeline and is represented as a float ranging from
                // [-1.0, 0.0] for long range and [-0.1, 0.0] for short range, where 0 is highest confidence. 
                if (depth < minDepth || depth > maxDepth || (useConfidenceFilter && confidenceData[index] < -0.1f))
                {
                    //Set the invalid points to be positioned at 0,0,0
                    depthPoints[index] = Vector3.zero;
                    continue;
                }

                // Use the cached projection table to find the UV coordinates for the current point.
                Vector2 uv = cachedProjectionTable[y, x];
                // Transform the UV coordinates into a camera space point.
                Vector3 cameraPoint = new Vector3(uv.x, uv.y, 1).normalized * depth;
                // Convert the camera space point into a world space point.
                Vector3 worldPoint = cameraToWorldMatrix.MultiplyPoint3x4(cameraPoint);

                // Store the world space point in the depthPoints array.
                depthPoints[index] = worldPoint;
            }
        }
    }

    /// <summary>
    /// Creates a new Texture2D or updates an existing one using data from a depth camera's frame buffer.
    /// </summary>
    /// <param name="texture">The current texture to update. If this is null, a new texture will be created.</param>
    /// <param name="frameBuffer">The frame buffer from the MLDepthCamera containing the raw depth data.</param>
    /// <returns>The updated or newly created Texture2D populated with the frame buffer's depth data.</returns>
    private Texture2D CreateOrUpdateTexture(Texture2D texture, MLDepthCamera.FrameBuffer frameBuffer)
    {
        if (texture == null)
        {
            texture = new Texture2D((int)frameBuffer.Width, (int)frameBuffer.Height, TextureFormat.RFloat, false);
        }

        texture.LoadRawTextureData(frameBuffer.Data);

        DepthFrameQuadRenderer.material.mainTexture = texture;

        byte[] _bytes = texture.EncodeToPNG();
        string fileName = dirPath + DateTime.Now.ToString("HHmmss") + ".png";
        System.IO.File.WriteAllBytes(fileName, _bytes);

        texture.Apply();
        return texture;
    }

    /// <summary>
    /// Creates a projection table mapping 2D pixel coordinates to normalized device coordinates (NDC),
    /// accounting for lens distortion.
    /// </summary>
    /// <param name="intrinsics">The intrinsic parameters of the depth camera, including resolution,
    /// focal length, principal point, and distortion coefficients.</param>
    private Vector2[,] CreateProjectionTable(MLDepthCamera.Intrinsics intrinsics)
    {
        // Convert the camera's resolution from intrinsics to a Vector2Int for easier manipulation.
        Vector2Int resolution = new Vector2Int((int)intrinsics.Width, (int)intrinsics.Height);
        // Initialize the projection table with the same dimensions as the camera's resolution.
        Vector2[,] projectionTable = new Vector2[resolution.y, resolution.x];

        // Iterate over each pixel in the resolution.
        for (int y = 0; y < resolution.y; ++y)
        {
            for (int x = 0; x < resolution.x; ++x)
            {
                // Normalize the current pixel coordinates to a range of [0, 1] by dividing
                // by the resolution. This converts pixel coordinates to UV coordinates.
                Vector2 uv = new Vector2(x, y) / new Vector2(resolution.x, resolution.y);

                // Apply distortion correction to the UV coordinates. This step compensates
                // for the lens distortion inherent in the depth camera's optics.
                Vector2 correctedUV = Undistort(uv, intrinsics.Distortion);

                // Convert the corrected UV coordinates back to pixel space, then shift
                // them based on the principal point and scale by the focal length to
                // achieve normalized device coordinates (NDC). These coordinates are
                // useful for mapping 2D image points to 3D space.
                projectionTable[y, x] = ((correctedUV * new Vector2(resolution.x, resolution.y)) - intrinsics.PrincipalPoint) / intrinsics.FocalLength;
            }
        }
        //Return the created projection Table
        return projectionTable;
    }

    /// <summary>
    /// Applies distortion correction to a UV coordinate based on given distortion parameters.
    /// </summary>
    /// <param name="uv">The original UV coordinate to undistort.</param>
    /// <param name="distortionParameters">Distortion parameters containing radial and tangential distortion coefficients.</param>
    /// <returns>The undistorted UV coordinate.</returns>
    private Vector2 Undistort(Vector2 uv, MLDepthCamera.DistortionCoefficients distortionParameters)
    {
        // Calculate the offset from the center of the image.
        Vector2 offsetFromCenter = uv - Half2;

        // Compute radial distance squared (r^2), its fourth power (r^4), and its sixth power (r^6) for radial distortion correction.
        float rSquared = Vector2.Dot(offsetFromCenter, offsetFromCenter);
        float rSquaredSquared = rSquared * rSquared;
        float rSquaredCubed = rSquaredSquared * rSquared;

        // Apply radial distortion correction based on the distortion coefficients.
        Vector2 radialDistortionCorrection = offsetFromCenter * (float)(1 + distortionParameters.K1 * rSquared + distortionParameters.K2 * rSquaredSquared + distortionParameters.K3 * rSquaredCubed);

        // Compute tangential distortion correction.
        float tangentialDistortionCorrectionX = (float)((2 * distortionParameters.P1 * offsetFromCenter.x * offsetFromCenter.y) + (distortionParameters.P2 * (rSquared + 2 * offsetFromCenter.x * offsetFromCenter.x)));
        float tangentialDistortionCorrectionY = (float)((2 * distortionParameters.P2 * offsetFromCenter.x * offsetFromCenter.y) + (distortionParameters.P1 * (rSquared + 2 * offsetFromCenter.y * offsetFromCenter.y)));
        Vector2 tangentialDistortionCorrection = new Vector2(tangentialDistortionCorrectionX, tangentialDistortionCorrectionY);

        // Combine the radial and tangential distortion corrections and adjust back to original image coordinates.
        return (radialDistortionCorrection + tangentialDistortionCorrection) + Half2;
    }

Is the com.magicleap.permission.DEPTH_CAMERA permission set in Project Settings > Magic Leap > Permissions?

Yes, they are! It failed to asked for permissions earlier but I manually added the permissions

Are you receiving any errors in the console upon running this script?

Well, I managed to visualize the depth data using that script. But I'm having two issues:

  1. The rendered streaming point clouds are not properly co-registered with the real objects, but the geometry looks correct at least.

  2. The program crashes in ~20 seconds every time due to possible memory leak problem.

Looking forward to ML's new SDK updates and documentation.