/// <summary>
/// Track camera pose using AlignPointClouds
/// </summary>
/// <param name="calculateDeltaFrame">A flag to indicate it is time to calculate the delta frame.</param>
/// <param name="calculatedCameraPose">The calculated camera pose.</param>
/// <returns>Returns true if tracking succeeded, false otherwise.</returns>
private bool TrackCameraAlignPointClouds(ref bool calculateDeltaFrame, ref Matrix4 calculatedCameraPose)
{
var trackingSucceeded = false;
DownsampleDepthFrameNearestNeighbor();
// Smooth the depth frame
this.volume.SmoothDepthFloatFrame(this.downsampledDepthFloatFrame, this.downsampledSmoothDepthFloatFrame, settings.SmoothingKernelWidth, settings.SmoothingDistanceThreshold);
// Calculate point cloud from the smoothed frame
FusionDepthProcessor.DepthFloatFrameToPointCloud(this.downsampledSmoothDepthFloatFrame, this.downsampledDepthPointCloudFrame);
// Get the saved pose view by raycasting the volume from the current camera pose
this.volume.CalculatePointCloud(this.downsampledRaycastPointCloudFrame, calculatedCameraPose);
var initialPose = calculatedCameraPose;
// Note that here we only calculate the deltaFromReferenceFrame every
// DeltaFrameCalculationInterval frames to reduce computation time
if (calculateDeltaFrame)
{
trackingSucceeded = FusionDepthProcessor.AlignPointClouds(
this.downsampledRaycastPointCloudFrame,
this.downsampledDepthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
this.downsampledDeltaFromReferenceFrameColorFrame,
ref calculatedCameraPose);
UpsampleColorDeltasFrameNearestNeighbor();
// Set calculateDeltaFrame to false as we are rendering it here
calculateDeltaFrame = false;
}
else
{
// Don't bother getting the residual delta from reference frame to cut computation time
trackingSucceeded = FusionDepthProcessor.AlignPointClouds(
this.downsampledRaycastPointCloudFrame,
this.downsampledDepthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
null,
ref calculatedCameraPose);
}
if (trackingSucceeded)
{
trackingSucceeded = MathUtils.CheckTransformChange(
initialPose,
calculatedCameraPose,
settings.MaxTranslationDeltaAlignPointClouds,
settings.MaxRotationDeltaAlignPointClouds);
}
return(trackingSucceeded);
}
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?
}
/// <summary>
/// Track camera pose using AlignPointClouds
/// </summary>
/// <param name="calculateDeltaFrame">A flag to indicate it is time to calculate the delta frame.</param>
/// <param name="calculatedCameraPose">The calculated camera pose.</param>
/// <returns>Returns true if tracking succeeded, false otherwise.</returns>
private bool TrackCameraAlignPointClouds(ushort[] depthPixels, ref bool calculateDeltaFrame, ref Matrix4 calculatedCameraPose)
{
bool trackingSucceeded = false;
//Resample at lower resolution
var downsampledDepthFloatFrame = engine.Resampler.GetDefaultFloatFrame();
engine.Resampler.DownsampleDepthFrameNearestNeighbor(downsampledDepthFloatFrame, depthPixels, engine.FusionVolume.MirrorDepth);
//Smooth
engine.PointCloudCalculator.CreateSmoothDepthCloud(downsampledDepthFloatFrame);
//get point cloud for tracking
engine.PointCloudCalculator.RaycastPointCloud(calculatedCameraPose);
Matrix4 initialPose = calculatedCameraPose;
// Note that here we only calculate the deltaFromReferenceFrame every
// DeltaFrameCalculationInterval frames to reduce computation time
if (calculateDeltaFrame)
{
trackingSucceeded = engine.DeltaCalculator.CalculateDeltaFrame(calculatedCameraPose);
// Set calculateDeltaFrame to false as we are rendering it here
calculateDeltaFrame = false;
}
else
{
// Don't bother getting the residual delta from reference frame to cut computation time
trackingSucceeded = FusionDepthProcessor.AlignPointClouds(
engine.PointCloudCalculator.DownsampledRaycastPointCloudFrame,
engine.PointCloudCalculator.DownsampledDepthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
null,
ref calculatedCameraPose);
}
if (trackingSucceeded)
{
bool failed = KinectFusionHelper.CameraTransformFailed(
initialPose,
calculatedCameraPose,
Constants.MaxTranslationDeltaAlignPointClouds,
Constants.MaxRotationDeltaAlignPointClouds);
if (failed)
{
trackingSucceeded = false;
}
}
return(trackingSucceeded);
}
/// <summary>
/// Calculates delta frame and returns if tracking succeeded
/// </summary>
/// <returns></returns>
public bool CalculateDeltaFrame(Matrix4 calculatedCameraPose)
{
var pc = engine.PointCloudCalculator;
var trackingSucceeded = FusionDepthProcessor.AlignPointClouds(
engine.PointCloudCalculator.DownsampledRaycastPointCloudFrame,
engine.PointCloudCalculator.DownsampledDepthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
DownsampledDeltaFromReferenceFrameColorFrame,
ref calculatedCameraPose);
UpsampleColorDeltasFrameNearestNeighbor();
OnColorDeltaPixelReady(this.DeltaFromReferenceFramePixelsArgb);
return(trackingSucceeded);
}