private void ProcessFrame(byte bodyIndex)
{
try
{
RemoveNonBodyPixels(bodyIndex);
reconstruction.DepthToDepthFloatFrame(rawDepthData, floatDepthFrame,
MIN_DEPTH, MAX_DEPTH,
false);
var aligned = reconstruction.ProcessFrame(
floatDepthFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
FusionDepthProcessor.DefaultIntegrationWeight,
out alignmentEnergy,
worldToCameraTransform);
if (aligned)
{
syncContext.Post(() => FrameAligned?.Invoke(this, EventArgs.Empty));
worldToCameraTransform = reconstruction.GetCurrentWorldToCameraTransform();
}
}
catch (InvalidOperationException)
{
}
try
{
reconstruction.CalculatePointCloud(pointCloudFrame, worldToCameraTransform);
FusionDepthProcessor.ShadePointCloud(pointCloudFrame, worldToCameraTransform, surfaceFrame, null);
SurfaceBitmap.Access(data => surfaceFrame.CopyPixelDataTo(data));
syncContext.Post(() => SurfaceBitmapUpdated?.Invoke(this, EventArgs.Empty));
}
catch (InvalidOperationException)
{
}
}
private void ProcessDepthData(DepthImagePixel[] depthPixels)
{
try {
if (processingSaveFile)
{
return;
}
// DepthImagePixel から DepthFloatFrame に変換する
FusionDepthProcessor.DepthToDepthFloatFrame(
depthPixels,
DepthWidth,
DepthHeight,
depthFloatBuffer,
FusionDepthProcessor.DefaultMinimumDepth,
FusionDepthProcessor.DefaultMaximumDepth,
false);
// フレームを処理する
bool trackingSucceeded = volume.ProcessFrame(
depthFloatBuffer,
FusionDepthProcessor.DefaultAlignIterationCount,
FusionDepthProcessor.DefaultIntegrationWeight,
volume.GetCurrentWorldToCameraTransform());
if (!trackingSucceeded)
{
// 一定数エラーになったらリセット
// Kinectまたは対象を素早く動かしすぎ などの場合
trackingErrorCount++;
if (trackingErrorCount >= 100)
{
Trace.WriteLine(@"tracking error.");
trackingErrorCount = 0;
volume.ResetReconstruction(Matrix4.Identity);
}
return;
}
// PointCloudを取得する
volume.CalculatePointCloud(
pointCloudBuffer,
volume.GetCurrentWorldToCameraTransform());
// PointCloudを2次元のデータに描画する
FusionDepthProcessor.ShadePointCloud(
pointCloudBuffer,
volume.GetCurrentWorldToCameraTransform(),
shadedSurfaceColorFrame,
null);
// 2次元のデータをBitmapに書きだす
var colorPixels = new int[depthPixels.Length];
shadedSurfaceColorFrame.CopyPixelDataTo(colorPixels);
ImageKinectFusion.Source = BitmapSource.Create(DepthWidth, DepthHeight, 96, 96,
PixelFormats.Bgr32, null, colorPixels, DepthWidth * 4);
}
catch (Exception ex) {
Trace.WriteLine(ex.Message);
}
finally {
processingFrame = false;
}
}