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;
}