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;
// Divide by 1e3 to store points position per each 1 millimeter of z-distance.
var point = new Vector3(pointCloudBuffer[k], pointCloudBuffer[k + 1], pointCloudBuffer[k + 2]) / 1000;
pointCloudCache[v, u] = point;
}
}
}
}
}
public void Init()
{
//check if the scene is a game scene
Game game = FindObjectOfType <Game>();
if (game != null)
{
game.OnGameFinished += Close;
}
bodyDisplay = BodyDisplay.Instance;
int count = Device.GetInstalledCount();
if (count == 0)
{
Debug.Log("No k4a devices attached!\n");
return;
}
// Open the first plugged in Kinect device
device = Device.Open();
if (device == null)
{
Debug.Log("Failed to open k4a device!\n");
return;
}
string serial = device.SerialNum;
Debug.Log("Opened device:" + serial);
var config = new DeviceConfiguration
{
ColorResolution = colorResolution,
ColorFormat = imageFormat,
DepthMode = depthMode,
CameraFPS = fps,
SynchronizedImagesOnly = syncronizedImagesOnly
};
device.StartCameras(config);
device.StartImu();
calibration = device.GetCalibration(depthMode, colorResolution);
transformation = calibration.CreateTransformation();
colourWidth = calibration.ColorCameraCalibration.ResolutionWidth;
colourHeight = calibration.ColorCameraCalibration.ResolutionHeight;
Debug.Log("Kinect started successfully");
//Task.Run(() => ImuCapture());
AppManager.applicationRunning = true;
}
public bool OpenSensor(int deviceIndex = 0)
{
if (_KinectSensor != null)
{
Debug.Log("AzureKinect [" + _KinectSensor.SerialNum + "] has already been opened.");
return(false);
}
_KinectSensor = Device.Open(deviceIndex);
if (_KinectSensor == null)
{
Debug.Log("AzureKinect cannot be opened.");
return(false);
}
DeviceConfiguration kinectConfig = new DeviceConfiguration();
kinectConfig.ColorFormat = _ColorImageFormat;
kinectConfig.ColorResolution = (ColorResolution)_ColorCameraMode;
kinectConfig.DepthMode = (DepthMode)_DepthCameraMode;
if (_ColorCameraMode != ColorCameraMode._4096_x_3072_15fps &&
_DepthCameraMode != DepthCameraMode._1024x1024_15fps)
{
kinectConfig.CameraFPS = FPS.FPS30;
}
else
{
kinectConfig.CameraFPS = FPS.FPS15;
}
_KinectSensor.StartCameras(kinectConfig);
_IsCameraStarted = true;
_KinectSensor.StartImu();
_DeviceCalibration = _KinectSensor.GetCalibration();
_Transformation = _DeviceCalibration.CreateTransformation();
CameraCalibration colorCamera = _DeviceCalibration.ColorCameraCalibration;
_ColorImageWidth = colorCamera.ResolutionWidth;
_ColorImageHeight = colorCamera.ResolutionHeight;
CameraCalibration depthCamera = _DeviceCalibration.DepthCameraCalibration;
_DepthImageWidth = depthCamera.ResolutionWidth;
_DepthImageHeight = depthCamera.ResolutionHeight;
return(true);
}
public void OpenSensor(int deviceIndex = 0)
{
_KinectSensor = Device.Open(deviceIndex);
if (_KinectSensor == null)
{
Debug.LogError("AzureKinect cannot be opened.");
return;
}
DeviceConfiguration kinectConfig = new DeviceConfiguration();
kinectConfig.ColorFormat = ColorImageFormat;
kinectConfig.ColorResolution = (ColorResolution)ColorCameraMode;
kinectConfig.DepthMode = (DepthMode)DepthCameraMode;
if (ColorCameraMode != ColorCameraMode._4096_x_3072_15fps &&
DepthCameraMode != DepthCameraMode._1024x1024_15fps)
{
kinectConfig.CameraFPS = FPS.FPS30;
}
else
{
kinectConfig.CameraFPS = FPS.FPS15;
}
_KinectSensor.StartCameras(kinectConfig);
_IsCameraStarted = true;
_DeviceCalibration = _KinectSensor.GetCalibration();
_Transformation = _DeviceCalibration.CreateTransformation();
CameraCalibration colorCamera = _DeviceCalibration.ColorCameraCalibration;
_ColorImageWidth = colorCamera.ResolutionWidth;
_ColorImageHeight = colorCamera.ResolutionHeight;
CameraCalibration depthCamera = _DeviceCalibration.DepthCameraCalibration;
_DepthImageWidth = depthCamera.ResolutionWidth;
_DepthImageHeight = depthCamera.ResolutionHeight;
}
// processes the camera frame
private void ProcessCameraFrame(KinectInterop.SensorData sensorData, Capture capture)
{
// check for color & depth sync
if (isSyncDepthAndColor && (capture.Color == null || capture.Depth == null))
{
return;
}
try
{
// color frame
if (capture.Color != null && rawColorImage != null)
{
if (kinectPlayback != null)
{
WaitForPlaybackTimestamp("color", capture.Color.DeviceTimestamp.Ticks);
}
lock (colorFrameLock)
{
capture.Color.CopyBytesTo(rawColorImage, 0, 0, rawColorImage.Length);
rawColorTimestamp = (ulong)capture.Color.DeviceTimestamp.Ticks;
colorFrameNumber = currentFrameNumber;
//Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
}
}
// depth frame
if (capture.Depth != null && rawDepthImage != null)
{
if (kinectPlayback != null)
{
WaitForPlaybackTimestamp("depth", capture.Depth.DeviceTimestamp.Ticks);
}
lock (depthFrameLock)
{
capture.Depth.CopyTo(rawDepthImage, 0, 0, rawDepthImage.Length);
rawDepthTimestamp = (ulong)capture.Depth.DeviceTimestamp.Ticks;
depthFrameNumber = currentFrameNumber;
//Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
}
}
// infrared frame
if (capture.IR != null && rawInfraredImage != null)
{
if (kinectPlayback != null)
{
WaitForPlaybackTimestamp("ir", capture.IR.DeviceTimestamp.Ticks);
}
lock (infraredFrameLock)
{
capture.IR.CopyTo(rawInfraredImage, 0, 0, rawInfraredImage.Length);
rawInfraredTimestamp = (ulong)capture.IR.DeviceTimestamp.Ticks;
infraredFrameNumber = currentFrameNumber;
//Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
}
}
// transformation data frames
if ((depth2ColorDataFrame != null || color2DepthDataFrame != null) && capture.Color != null && capture.Depth != null)
{
if (coordMapperTransform == null)
{
coordMapperTransform = coordMapper.CreateTransformation();
}
if (depth2ColorDataFrame != null)
{
lock (depth2ColorFrameLock)
{
using (Image d2cColorData = coordMapperTransform.ColorImageToDepthCamera(capture))
{
d2cColorData.CopyTo <byte>(depth2ColorDataFrame, 0, 0, depth2ColorDataFrame.Length);
lastDepth2ColorFrameTime = (ulong)capture.Depth.DeviceTimestamp.Ticks;
}
}
}
if (color2DepthDataFrame != null)
{
lock (color2DepthFrameLock)
{
using (Image c2dDepthData = coordMapperTransform.DepthImageToColorCamera(capture))
{
c2dDepthData.CopyTo <ushort>(color2DepthDataFrame, 0, 0, color2DepthDataFrame.Length);
lastColor2DepthFrameTime = (ulong)capture.Color.DeviceTimestamp.Ticks;
}
}
}
}
}
catch (System.Exception ex)
{
Debug.LogException(ex);
}
}
