81 lines
2.5 KiB
C#
81 lines
2.5 KiB
C#
|
using Microsoft.Azure.Kinect.Sensor;
|
||
|
|
using System;
|
||
|
|
using System.Runtime.InteropServices;
|
||
|
|
using System.Threading.Tasks;
|
||
|
|
|
||
|
|
namespace Akvfx
|
||
|
|
{
|
||
|
|
//
|
||
|
|
// Per pixel ray direction table ("XY table") used to unproject depth
|
||
|
|
// samples to the 3D camera space
|
||
|
|
//
|
||
|
|
// This class directly invokes a native method (k4a_calibration_2d_to_3d)
|
||
|
|
// contained in libk4a to avoid double symbol definition problem between
|
||
|
|
// System.Numerics and System.Numerics.Vectors.
|
||
|
|
//
|
||
|
|
sealed class XYTable
|
||
|
|
{
|
||
|
|
// Public property: Table data
|
||
|
|
public ReadOnlySpan<float> Data { get { return _data; } }
|
||
|
|
|
||
|
|
// Float vvector types
|
||
|
|
struct Float2 { public float x, y; }
|
||
|
|
struct Float3 { public float x, y, z; }
|
||
|
|
|
||
|
|
// Data storage
|
||
|
|
float [] _data;
|
||
|
|
|
||
|
|
// Constructor
|
||
|
|
public XYTable(Calibration calibration, int width, int height)
|
||
|
|
{
|
||
|
|
// Data storage allocation
|
||
|
|
var table = new float [width * height * 2];
|
||
|
|
|
||
|
|
// XY table initialization
|
||
|
|
// These loops could take a significant amount of time (e.g. 1 sec)
|
||
|
|
// even on a decent desktop PC. Luckily, this can be easily
|
||
|
|
// parallelized by using Parallel.For, so we did.
|
||
|
|
Parallel.For(0, height, y => {
|
||
|
|
Float2 v2;
|
||
|
|
Float3 v3;
|
||
|
|
bool isValid;
|
||
|
|
|
||
|
|
v2.y = y;
|
||
|
|
var offs = width * 2 * y;
|
||
|
|
|
||
|
|
for (var x = 0; x < width; x++)
|
||
|
|
{
|
||
|
|
v2.x = x;
|
||
|
|
|
||
|
|
k4a_calibration_2d_to_3d(
|
||
|
|
ref calibration,
|
||
|
|
ref v2, 1,
|
||
|
|
CalibrationDeviceType.Color,
|
||
|
|
CalibrationDeviceType.Color,
|
||
|
|
out v3,
|
||
|
|
out isValid
|
||
|
|
);
|
||
|
|
|
||
|
|
table[offs++] = v3.x;
|
||
|
|
table[offs++] = v3.y;
|
||
|
|
}
|
||
|
|
});
|
||
|
|
|
||
|
|
// Publish the table data.
|
||
|
|
_data = table;
|
||
|
|
}
|
||
|
|
|
||
|
|
// k4a_calibration_2d_to_3d native method from libk4a
|
||
|
|
[DllImport("k4a", CallingConvention = CallingConvention.Cdecl)]
|
||
|
|
static extern int k4a_calibration_2d_to_3d(
|
||
|
|
[In] ref Calibration calibration,
|
||
|
|
ref Float2 source_point2d,
|
||
|
|
float source_depth,
|
||
|
|
CalibrationDeviceType source_camera,
|
||
|
|
CalibrationDeviceType target_camera,
|
||
|
|
out Float3 target_point3d,
|
||
|
|
out bool valid
|
||
|
|
);
|
||
|
|
}
|
||
|
|
}
|