76 lines
3.1 KiB
C#
76 lines
3.1 KiB
C#
using Microsoft.Azure.Kinect.Sensor;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
using System.Runtime.InteropServices;
|
|
|
|
namespace Csharp_3d_viewer
|
|
{
|
|
public static class PointCloud
|
|
{
|
|
private static Vector3[,] pointCloudCache;
|
|
|
|
public static void ComputePointCloudCache(Calibration calibration)
|
|
{
|
|
using (var transformation = calibration.CreateTransformation())
|
|
using (var fakeDepth = new Image(ImageFormat.Depth16, calibration.DepthCameraCalibration.ResolutionWidth, calibration.DepthCameraCalibration.ResolutionHeight))
|
|
{
|
|
// compute 3D points at z = 1000mm distance from the camera.
|
|
MemoryMarshal.Cast<byte, ushort>(fakeDepth.Memory.Span).Fill(1000);
|
|
using (var pointCloudImage = transformation.DepthImageToPointCloud(fakeDepth))
|
|
{
|
|
var pointCloudBuffer = MemoryMarshal.Cast<byte, short>(pointCloudImage.Memory.Span);
|
|
|
|
pointCloudCache = new Vector3[calibration.DepthCameraCalibration.ResolutionHeight, calibration.DepthCameraCalibration.ResolutionWidth];
|
|
for (int k = 0, v = 0; v < calibration.DepthCameraCalibration.ResolutionHeight; ++v)
|
|
{
|
|
for (int u = 0; u < calibration.DepthCameraCalibration.ResolutionWidth; ++u, k += 3)
|
|
{
|
|
// Divide by 1e6 to store points position per each 1 millimeter of z-distance.
|
|
var point = new Vector3(pointCloudBuffer[k], pointCloudBuffer[k + 1], pointCloudBuffer[k + 2]) / 1000000;
|
|
pointCloudCache[v, u] = point;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public static void ComputePointCloud(Image depth, ref List<Vertex> pointCloud)
|
|
{
|
|
if (pointCloudCache == null)
|
|
{
|
|
throw new Exception("Must compute the point cloud cache before calling ComputePointCloud.");
|
|
}
|
|
|
|
if (pointCloud == null)
|
|
{
|
|
pointCloud = new List<Vertex>(depth.HeightPixels * depth.WidthPixels);
|
|
}
|
|
|
|
pointCloud.Clear();
|
|
|
|
var depthPixels = depth.GetPixels<ushort>().ToArray();
|
|
|
|
for (int v = 0, pixelIndex = 0; v < depth.HeightPixels; ++v)
|
|
{
|
|
for (int u = 0; u < depth.WidthPixels; ++u, ++pixelIndex)
|
|
{
|
|
var depthInMillimeters = depthPixels[pixelIndex];
|
|
|
|
var positionPerMillimeterDepth = pointCloudCache[v, u];
|
|
if (depthInMillimeters > 0)
|
|
{
|
|
pointCloud.Add(new Vertex
|
|
{
|
|
Position = positionPerMillimeterDepth * depthInMillimeters,
|
|
|
|
// Computing normal on the CPU is expensive. Keeping it simple in this sample instead of making another Shader.
|
|
Normal = Vector3.UnitZ,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|