private void TrackCamera_AlignPointClouds(ref Matrix4 calculatedCameraPose)
{
DownsampleDepthFrameNearestNeighbor(downsampledDepthFloatFrame, DownsampleFactor);
volume.SmoothDepthFloatFrame(downsampledDepthFloatFrame, downsampledSmoothDepthFloatFrame, 1, 0.04f); //TODO: Magic numbers
FusionDepthProcessor.DepthFloatFrameToPointCloud(downsampledSmoothDepthFloatFrame, downsampledDepthPointCloudFrame);
volume.CalculatePointCloud(downsampledRaycastPointCloudFrame, calculatedCameraPose);
Matrix4 initialPose = calculatedCameraPose;
FusionDepthProcessor.AlignPointClouds(downsampledRaycastPointCloudFrame, downsampledDepthPointCloudFrame, FusionDepthProcessor.DefaultAlignIterationCount, downsampledDeltaFromReferenceFrameColorFrame, ref calculatedCameraPose);
UpsampleColorDeltasFrameNearestNeighbor(DownsampleFactor);
//UpdateAlignDeltas(); //TODO: Necessary?
}
void result()
{
reconstruction.CalculatePointCloud(pointCloudImageFrame, surfaceImageFrame, worldToCameraTransform);
}
/// <summary>
/// processes the depth data package into the kinect fusion volume
/// </summary>
/// <param name="pKdp">the data package</param>
void processDepthData(KinectDataPackage pKdp, System.Threading.CancellationToken pCancelToken)
{
lock (canWorkLock)
{
Log.LogManager.updateAlgorithmStatus("Kinect Fusion integration");
this.volume.ResetReconstruction(Matrix4.Identity);
int picturesIntegrated = 0;
foreach (ushort[] pDepth in pKdp.rawDepthData)
{
pCancelToken.ThrowIfCancellationRequested();
WriteableBitmap bitmap = new WriteableBitmap(this.depthFloatFrame.Width, this.depthFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
FusionFloatImageFrame depthFloatBuffer = new FusionFloatImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
FusionPointCloudImageFrame pointCloudBuffer = new FusionPointCloudImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
FusionColorImageFrame shadedSurfaceColorFrame = new FusionColorImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
int[] voxelPixels = new int[this.depthFloatFrame.Width * this.depthFloatFrame.Height];
this.volume.DepthToDepthFloatFrame(
pDepth,
depthFloatBuffer,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.minDepthClip,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.maxDepthClip,
false);
float alignmentValue;
bool trackingSucceeded = this.volume.ProcessFrame(depthFloatBuffer,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.iterationCount,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.integrationWeight,
out alignmentValue,
volume.GetCurrentWorldToCameraTransform());
// If camera tracking failed, no data integration or raycast for reference
// point cloud will have taken place, and the internal camera pose
// will be unchanged.
if (!trackingSucceeded)
{
trackingErrorCount++;
}
else
{
Matrix4 calculatedCameraPose = volume.GetCurrentWorldToCameraTransform();
// Set the camera pose and reset tracking errors
worldToCameraTransform = calculatedCameraPose;
trackingErrorCount = 0;
}
// Calculate the point cloud
volume.CalculatePointCloud(pointCloudBuffer, worldToCameraTransform);
// Shade point cloud and render
FusionDepthProcessor.ShadePointCloud(
pointCloudBuffer,
worldToCameraTransform,
null,
shadedSurfaceColorFrame
);
shadedSurfaceColorFrame.CopyPixelDataTo(voxelPixels);
bitmap.WritePixels(
new Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight),
voxelPixels,
bitmap.PixelWidth * sizeof(int),
0);
bitmap.Freeze();
OnNewFusionPictureEvent.BeginInvoke(pKdp.usedConfig.ID, bitmap, null, null);
picturesIntegrated++;
Log.LogManager.writeLogDebug("[DataIntegration:Reconstruction] " + picturesIntegrated + " of " + pKdp.rawDepthData.Count + " Pictures integrated");
}
//if request was calibration request, export meshes
if (pKdp.usedConfig.clientRequestObject.requestType == ClientConfigObject.RequestType.calibration)
{
exportMesh(volume, pKdp, false);
Log.LogManager.writeLog("[DataIntegration:Reconstruction] Mesh of " + pKdp.usedConfig.name + " exported.");
return;
}
//broadcast new point cloud
PointCloud p = new PointCloud(volume);
p.ConfigObject = pKdp.usedConfig;
OnNewPointCloudEvent.BeginInvoke(p, null, null);
Log.LogManager.writeLog("[DataIntegration:Reconstruction] All pictures of " + pKdp.usedConfig.name + " integrated");
Log.LogManager.updateAlgorithmStatus("Done");
}
}