public void CreateSmoothDepthCloud(FusionFloatImageFrame downsampledDepthFloatFrame)
{
// Smooth the depth frame
this.volume.SmoothDepthFloatFrame(downsampledDepthFloatFrame, this.DownsampledSmoothDepthFloatFrame, SmoothingKernelWidth, SmoothingDistanceThreshold);
// Calculate point cloud from the smoothed frame
FusionDepthProcessor.DepthFloatFrameToPointCloud(this.DownsampledSmoothDepthFloatFrame, DownsampledDepthPointCloudFrame);
}
/// <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>
/// Perform camera pose finding when tracking is lost using AlignPointClouds.
/// This is typically more successful than FindCameraPoseAlignDepthFloatToReconstruction.
/// </summary>
/// <returns>Returns true if a valid camera pose was found, otherwise false.</returns>
private bool FindCameraPoseAlignPointClouds()
{
if (!this.cameraPoseFinderAvailable)
{
return(false);
}
ProcessColorForCameraPoseFinder();
var matchCandidates = this.cameraPoseFinder.FindCameraPose(
this.depthFloatFrame,
this.resampledColorFrame);
if (matchCandidates == null)
{
return(false);
}
var poseCount = matchCandidates.GetPoseCount();
var minDistance = matchCandidates.CalculateMinimumDistance();
if (poseCount == 0 || minDistance >= settings.CameraPoseFinderDistanceThresholdReject)
{
return(false);
}
// Smooth the depth frame
this.volume.SmoothDepthFloatFrame(this.depthFloatFrame, this.smoothDepthFloatFrame, settings.SmoothingKernelWidth, settings.SmoothingDistanceThreshold);
// Calculate point cloud from the smoothed frame
FusionDepthProcessor.DepthFloatFrameToPointCloud(this.smoothDepthFloatFrame, this.depthPointCloudFrame);
var smallestEnergy = double.MaxValue;
var smallestEnergyNeighborIndex = -1;
var bestNeighborIndex = -1;
var bestNeighborCameraPose = Matrix4.Identity;
var bestNeighborAlignmentEnergy = settings.MaxAlignPointCloudsEnergyForSuccess;
// Run alignment with best matched poseCount (i.e. k nearest neighbors (kNN))
var maxTests = Math.Min(settings.MaxCameraPoseFinderPoseTests, poseCount);
var neighbors = matchCandidates.GetMatchPoses();
for (var n = 0; n < maxTests; n++)
{
// Run the camera tracking algorithm with the volume
// this uses the raycast frame and pose to find a valid camera pose by matching the raycast against the input point cloud
var poseProposal = neighbors[n];
// Get the saved pose view by raycasting the volume
this.volume.CalculatePointCloud(this.raycastPointCloudFrame, poseProposal);
var success = this.volume.AlignPointClouds(
this.raycastPointCloudFrame,
this.depthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
this.resampledColorFrame,
out this.alignmentEnergy,
ref poseProposal);
var relocSuccess = success && this.alignmentEnergy <bestNeighborAlignmentEnergy && this.alignmentEnergy> settings.MinAlignPointCloudsEnergyForSuccess;
if (relocSuccess)
{
bestNeighborAlignmentEnergy = this.alignmentEnergy;
bestNeighborIndex = n;
// This is after tracking succeeds, so should be a more accurate pose to store...
bestNeighborCameraPose = poseProposal;
// Update the delta image
this.resampledColorFrame.CopyPixelDataTo(this.deltaFromReferenceFramePixelsArgb);
}
// Find smallest energy neighbor independent of tracking success
if (this.alignmentEnergy < smallestEnergy)
{
smallestEnergy = this.alignmentEnergy;
smallestEnergyNeighborIndex = n;
}
}
matchCandidates.Dispose();
// Use the neighbor with the smallest residual alignment energy
// At the cost of additional processing we could also use kNN+Mean camera pose finding here
// by calculating the mean pose of the best n matched poses and also testing this to see if the
// residual alignment energy is less than with kNN.
if (bestNeighborIndex > -1)
{
this.worldToCameraTransform = bestNeighborCameraPose;
this.SetReferenceFrame(this.worldToCameraTransform);
// Tracking succeeded!
this.SetTrackingSucceeded();
return(true);
}
else
{
this.worldToCameraTransform = neighbors[smallestEnergyNeighborIndex];
this.SetReferenceFrame(this.worldToCameraTransform);
// Camera pose finding failed - return the tracking failed error code
this.SetTrackingFailed();
return(false);
}
}
public void UpdateDepthPointCloud(FusionFloatImageFrame smooth)
{
FusionDepthProcessor.DepthFloatFrameToPointCloud(smooth, DepthPointCloudFrame);
}
