/// <summary>
/// Aligns a depth float image to the Reconstruction volume to calculate the new camera pose.
/// This camera tracking method requires a Reconstruction volume, and updates the internal
/// camera pose if successful. The maximum image resolution supported in this function is 640x480.
/// Note that this function is designed primarily for tracking either with static scenes when
/// performing environment reconstruction, or objects which move rigidly when performing object
/// reconstruction from a static camera. Consider using the standalone function
/// NuiFusionAlignPointClouds instead if tracking failures occur due to parts of a scene which
/// move non-rigidly or should be considered as outliers, although in practice, such issues are
/// best avoided by carefully designing or constraining usage scenarios wherever possible.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="maxAlignIterationCount">
/// The maximum number of iterations of the algorithm to run.
/// The minimum value is 1. Using only a small number of iterations will have a faster runtime,
/// however, the algorithm may not converge to the correct transformation.
/// </param>
/// <param name="deltaFromReferenceFrame">
/// Optionally, a pre-allocated float image frame, to be filled with information about how
/// well each observed pixel aligns with the passed in reference frame. This maybe processed
/// to create a color rendering, or may be used as input to additional vision algorithms such
/// as object segmentation. Pass null if not required.
/// </param>
/// <param name="alignmentEnergy">
/// A float to receive a value describing how well the observed frame aligns to the model with
/// the calculated pose. A larger magnitude value represent more discrepancy, and a lower value
/// represent less discrepancy. Note that it is unlikely an exact 0 (perfect alignment) value will
/// ever be returned as every frame from the sensor will contain some sensor noise.
/// </param>
/// <param name="worldToCameraTransform">
/// The best guess of the camera pose (usually the camera pose result from the last
/// AlignPointClouds or AlignDepthFloatToReconstruction).
/// </param>
/// <returns>
/// Returns true if successful; return false if the algorithm encountered a problem aligning
/// the input depth image and could not calculate a valid transformation.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
public bool AlignDepthFloatToReconstruction(
FusionFloatImageFrame depthFloatFrame,
int maxAlignIterationCount,
FusionFloatImageFrame deltaFromReferenceFrame,
out float alignmentEnergy,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
HRESULT hr = volume.AlignDepthFloatToReconstruction(
FusionImageFrame.ToHandleRef(depthFloatFrame),
maxIterations,
FusionImageFrame.ToHandleRef(deltaFromReferenceFrame),
out alignmentEnergy,
ref worldToCameraTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return(false);
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return(true);
}
/// <summary>
/// Test input camera frames against the camera pose finder database, adding frames to the
/// database if dis-similar enough to existing frames. Both input depth and color frames
/// must be identical sizes, a minimum size of 80x60, with valid camera parameters, and
/// captured at the same time.
/// Note that once the database reaches its maximum initialized size, it will overwrite old
/// pose information. Check the <pararmref name="pHistoryTrimmed"/> flag or the number of
/// poses in the database to determine whether the old poses are being overwritten.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="colorFrame">The color frame to be processed.</param>
/// <param name="worldToCameraTransform"> The current camera pose (usually the camera pose
/// result from the last AlignPointClouds or AlignDepthFloatToReconstruction).</param>
/// <param name="minimumDistanceThreshold">A float distance threshold between 0 and 1.0f which
/// regulates how close together poses are stored in the database. Input frames
/// which have a minimum distance equal to or above this threshold when compared against the
/// database will be stored, as it indicates the input has become dis-similar to the existing
/// stored poses. Set to 0.0f to ignore and always add a pose when this function is called,
/// however in this case, unless there is an external test of distance, there is a risk this
/// can lead to many duplicated poses.
/// </param>
/// <param name="addedPose">
/// Set true when the input frame was added to the camera pose finder database.
/// </param>
/// <param name="trimmedHistory">
/// Set true if the maxPoseHistoryCount was reached when the input frame is stored, so the
/// oldest pose was overwritten in the camera pose finder database to store the latest pose.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="colorFrame"/>
/// parameter is null. </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> and <paramref name="colorFrame"/>
/// parameter is an incorrect or different image size, or their <c>CameraParameters</c>
/// member is null or has incorrectly sized focal lengths, or the
/// <paramref name="minimumDistanceThreshold"/> parameter is less than 0 or greater
/// than 1.0f.</exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// The camera pose finder works by accumulating whether the values at each sample location pixel
/// in a saved pose frame are less than or greater than a threshold which is randomly chosen
/// between minimum and maximum boundaries (e.g. for color this is 0-255). Given enough samples
/// this represents a unique key frame signature that we can match against, as different poses
/// will have different values for surfaces which are closer or further away, or different
/// colors.
/// Note that unlike depth, the robustness of finding a valid camera pose can have issues with
/// ambient illumination levels in the color image. For best matching results, both the Kinect
/// camera and also the environment should have exactly the same configuration as when the
/// database key frame images were captured i.e. if you had a fixed exposure and custom white
/// balance, this should again be set when testing the database later, otherwise the matching
/// accuracy will be reduced.
/// To improve accuracy, it is possible to not just provide a red, green, blue input in the
/// color image, but instead provide a different 3 channels of match data scaled 0-255. For
/// example, to be more illumination independent, you could calculate hue and saturation, or
/// convert RGB to to LAB and use the AB channels. Other measures such as texture response
/// or corner response could additionally be computed and used in one or more of the channels.
/// </remarks>
public void ProcessFrame(
FusionFloatImageFrame depthFloatFrame,
FusionColorImageFrame colorFrame,
Matrix4 worldToCameraTransform,
float minimumDistanceThreshold,
out bool addedPose,
out bool trimmedHistory)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
HRESULT hr = cameraPoseFinder.ProcessFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
ref worldToCameraTransform,
minimumDistanceThreshold,
out addedPose,
out trimmedHistory);
ExceptionHelper.ThrowIfFailed(hr);
}
/// <summary>
/// Converts Kinect depth frames in unsigned short format to depth frames in float format
/// representing distance from the camera in meters (parallel to the optical center axis).
/// </summary>
/// <param name="depthImageData">
/// An array which stores the extended-depth texture of a depth image from the Kinect camera.
/// </param>
/// <param name="depthImageDataWidth">Width of the depth image data.</param>
/// <param name="depthImageDataHeight">Height of the depth image data.</param>
/// <param name="depthFloatFrame">
/// A pre-allocated depth float type image frame, to be filled with the floating point depth values.
/// </param>
/// <param name="minDepthClip">
/// Minimum depth distance threshold in meters. Depth pixels below this value will be
/// returned as invalid (0). Min depth must be positive or 0.
/// </param>
/// <param name="maxDepthClip">
/// Maximum depth distance threshold in meters. Depth pixels above this value will be
/// returned as invalid (0). Max depth must be greater than 0.
/// </param>
/// <param name="mirrorDepth">
/// A boolean parameter specifying whether to horizontally mirror the input depth image.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthImageData"/> or the <paramref name="depthFloatFrame"/>
/// parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthImageDataWidth"/> parameter and depthFloatFrame's
/// <c>width</c> is not equal, or the <paramref name="depthImageDataHeight"/> parameter and
/// depthFloatFrame's <c>height</c> member is not equal.
/// Thrown when the <paramref name="minDepthClip"/> parameter or the
/// <paramref name="maxDepthClip"/> is less than zero.
/// </exception>
/// <exception cref="OutOfMemoryException">
/// Thrown if a CPU memory allocation failed.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// a GPU memory allocation failed or the call failed for an unknown reason.
/// </exception>
public static void DepthToDepthFloatFrame(
DepthImagePixel[] depthImageData,
int depthImageDataWidth,
int depthImageDataHeight,
FusionFloatImageFrame depthFloatFrame,
float minDepthClip,
float maxDepthClip,
bool mirrorDepth)
{
if (null == depthImageData)
{
throw new ArgumentNullException("depthImageData");
}
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionDepthToDepthFloatFrame(
depthImageData,
(uint)depthImageDataWidth,
(uint)depthImageDataHeight,
FusionImageFrame.ToHandleRef(depthFloatFrame),
minDepthClip,
maxDepthClip,
mirrorDepth));
}
/// <summary>
/// A high-level function to process a depth frame through the Kinect Fusion pipeline.
/// Specifically, this performs on-GPU processing equivalent to the following functions
/// for each frame:
/// <para>
/// 1) AlignDepthFloatToReconstruction
/// 2) IntegrateFrame
/// </para>
/// Users may also optionally call the low-level functions individually, instead of calling this
/// function, for more control. However, this function call will be faster due to the integrated
/// nature of the calls. After this call completes, if a visible output image of the reconstruction
/// is required, the user can call CalculatePointCloud and then ShadePointCloud.
/// The maximum image resolution supported in this function is 640x480.
/// <para/>
/// If there is a tracking error in the AlignDepthFloatToReconstruction stage, no depth data
/// integration will be performed, and the camera pose will remain unchanged.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="maxAlignIterationCount">
/// The maximum number of iterations of the align camera tracking algorithm to run.
/// The minimum value is 1. Using only a small number of iterations will have a faster
/// runtime, however, the algorithm may not converge to the correct transformation.
/// </param>
/// <param name="maxIntegrationWeight">
/// A parameter to control the temporal smoothing of depth integration. Lower values have
/// more noisy representations, but objects that move appear and disappear faster, so are
/// suitable for more dynamic environments. Higher values integrate objects more slowly,
/// but provides finer detail with less noise.
/// </param>
/// <param name="worldToCameraTransform">
/// The best guess of the latest camera pose (usually the camera pose result from the last
/// process call).
/// </param>
/// <returns>
/// Returns true if successful; return false if the algorithm encountered a problem aligning
/// the input depth image and could not calculate a valid transformation.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// or the call failed for an unknown reason.
/// </exception>
public bool ProcessFrame(
FusionFloatImageFrame depthFloatFrame,
int maxAlignIterationCount,
int maxIntegrationWeight,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
ushort maxWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
HRESULT hr = volume.ProcessFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
maxIterations,
maxWeight,
ref worldToCameraTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return(false);
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return(true);
}
/// <summary>
/// Integrates depth float data into the reconstruction volume from the
/// worldToCameraTransform camera pose.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be integrated.</param>
/// <param name="maxIntegrationWeight">
/// A parameter to control the temporal smoothing of depth integration. Minimum value is 1.
/// Lower values have more noisy representations, but objects that move integrate and
/// disintegrate faster, so are suitable for more dynamic environments. Higher values
/// integrate objects more slowly, but provides finer detail with less noise.</param>
/// <param name="worldToCameraTransform">
/// The camera pose (usually the camera pose result from the last AlignPointClouds or
/// AlignDepthFloatToReconstruction).
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="maxIntegrationWeight"/> parameter is less than 1
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
public void IntegrateFrame(
FusionFloatImageFrame depthFloatFrame,
int maxIntegrationWeight,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort integrationWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
ExceptionHelper.ThrowIfFailed(volume.IntegrateFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
integrationWeight,
ref worldToCameraTransform));
}
/// <summary>
/// Construct an oriented point cloud in the local camera frame of reference from a depth float
/// image frame. Here we calculate the 3D position of each depth float pixel with the optical
/// center of the camera as the origin. Both images must be the same size and have the same camera
/// parameters. We use a right-hand coordinate system, and (in common with bitmap images with top left
/// origin) +X is to the right, +Y down, and +Z is now forward from the Kinect camera into the scene,
/// as though looking into the scene from behind the kinect.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be converted.</param>
/// <param name="pointCloudFrame">
/// A pre-allocated point cloud frame, to be filled with 3D points and normals.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or the <paramref name="pointCloudFrame"/>
/// parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="pointCloudFrame"/>
/// parameters are different image sizes.
/// </exception>
/// <exception cref="OutOfMemoryException">
/// Thrown if a CPU memory allocation failed.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// a GPU memory allocation failed or the call failed for an unknown reason.
/// </exception>
public static void DepthFloatFrameToPointCloud(
FusionFloatImageFrame depthFloatFrame,
FusionPointCloudImageFrame pointCloudFrame)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionDepthFloatFrameToPointCloud(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(pointCloudFrame)));
}
/// <summary>
/// Find the most similar camera poses to the current camera input by comparing against the
/// camera pose finder database, and returning a set of similar camera poses. These poses
/// and similarity measurements are ordered in terms of decreasing similarity (i.e. the most
/// similar is first). Both input depth and color frames must be identical sizes, with valid
/// camera parameters and captured at the same time.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="colorFrame">The color frame to be processed.</param>
/// <returns>Returns the matched frames object created by the camera pose finder.</returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="colorFrame"/>
/// parameter is null. </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> and <paramref name="colorFrame"/>
/// parameter is an incorrect or different image size, or their <c>CameraParameters</c>
/// member is null or has incorrectly sized focal lengths.</exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed,
/// or the call failed for an unknown reason.
/// </exception>
/// <returns>Returns a set of matched frames/poses.</returns>
public MatchCandidates FindCameraPose(
FusionFloatImageFrame depthFloatFrame,
FusionColorImageFrame colorFrame)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
INuiFusionMatchCandidates matchCandidates = null;
ExceptionHelper.ThrowIfFailed(cameraPoseFinder.FindCameraPose(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
out matchCandidates));
return(new MatchCandidates(matchCandidates));
}
/// <summary>
/// Allocate the frame buffers used in the process
/// </summary>
private void AllocateFrames()
{
// Allocate depth float frame
if (null == this.depthFloatFrame || this.width != this.depthFloatFrame.Width || this.height != this.depthFloatFrame.Height)
{
this.depthFloatFrame = new FusionFloatImageFrame(this.width, this.height);
}
// Allocate delta from reference frame
if (null == this.deltaFromReferenceFrame || this.width != this.deltaFromReferenceFrame.Width || this.height != this.deltaFromReferenceFrame.Height)
{
this.deltaFromReferenceFrame = new FusionFloatImageFrame(this.width, this.height);
}
// Allocate point cloud frame
if (null == this.pointCloudFrame || this.width != this.pointCloudFrame.Width || this.height != this.pointCloudFrame.Height)
{
this.pointCloudFrame = new FusionPointCloudImageFrame(this.width, this.height);
}
// Allocate shaded surface frame
if (null == this.shadedSurfaceFrame || this.width != this.shadedSurfaceFrame.Width || this.height != this.shadedSurfaceFrame.Height)
{
this.shadedSurfaceFrame = new FusionColorImageFrame(this.width, this.height);
}
// Allocate shaded surface normals frame
if (null == this.shadedSurfaceNormalsFrame || this.width != this.shadedSurfaceNormalsFrame.Width || this.height != this.shadedSurfaceNormalsFrame.Height)
{
this.shadedSurfaceNormalsFrame = new FusionColorImageFrame(this.width, this.height);
}
}
/// <summary>
/// Construct an oriented point cloud in the local camera frame of reference from a depth float
/// image frame. Here we calculate the 3D position of each depth float pixel with the optical
/// center of the camera as the origin. We use a right-hand coordinate system, and (in common
/// with bitmap images with top left origin) +X is to the right, +Y down, and +Z is now forward
/// from the Kinect camera into the scene, as though looking into the scene from behind the
/// Kinect camera. Both images must be the same size and have the same camera parameters.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be converted.</param>
/// <param name="pointCloudFrame">
/// A pre-allocated point cloud frame, to be filled with 3D points and normals.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or the <paramref name="pointCloudFrame"/>
/// parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="pointCloudFrame"/>
/// parameters are different image sizes.
/// </exception>
/// <exception cref="OutOfMemoryException">
/// Thrown if a CPU memory allocation failed.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// a GPU memory allocation failed or the call failed for an unknown reason.
/// </exception>
public static void DepthFloatFrameToPointCloud(
FusionFloatImageFrame depthFloatFrame,
FusionPointCloudImageFrame pointCloudFrame)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionDepthFloatFrameToPointCloud(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(pointCloudFrame)));
}
/// <summary>
/// Converts Kinect depth frames in unsigned short format to depth frames in float format
/// representing distance from the camera in meters (parallel to the optical center axis).
/// Note: <paramref name="depthImageData"/> and <paramref name="depthFloatFrame"/> must
/// be the same pixel resolution and equal to <paramref name="depthImageDataWidth"/> by
/// <paramref name="depthImageDataHeight"/>.
/// The min and max depth clip values enable clipping of the input data, for example, to help
/// isolate particular objects or surfaces to be reconstructed. Note that the thresholds return
/// different values when a depth pixel is outside the threshold - pixels inside minDepthClip will
/// will be returned as 0 and ignored in processing, whereas pixels beyond maxDepthClip will be set
/// to 1000 to signify a valid depth ray with depth beyond the set threshold. Setting this far-
/// distance flag is important for reconstruction integration in situations where the camera is
/// static or does not move significantly, as it enables any voxels closer to the camera
/// along this ray to be culled instead of persisting (as would happen if the pixels were simply
/// set to 0 and ignored in processing). Note that when reconstructing large real-world size volumes,
/// be sure to set large maxDepthClip distances, as when the camera moves around, any voxels in view
/// which go beyond this threshold distance from the camera will be removed.
/// </summary>
/// <param name="depthImageData">
/// An array which stores the extended-depth texture of a depth image from the Kinect camera.
/// </param>
/// <param name="depthImageDataWidth">Width of the depth image data.</param>
/// <param name="depthImageDataHeight">Height of the depth image data.</param>
/// <param name="depthFloatFrame">
/// A pre-allocated depth float type image frame, to be filled with the floating point depth values.
/// </param>
/// <param name="minDepthClip">
/// Minimum depth distance threshold in meters. Depth pixels below this value will be
/// returned as invalid (0). Min depth must be positive or 0.
/// </param>
/// <param name="maxDepthClip">
/// Maximum depth distance threshold in meters. Depth pixels above this value will be
/// returned as invalid (1000). Max depth must be greater than 0.
/// </param>
/// <param name="mirrorDepth">
/// A boolean parameter specifying whether to horizontally mirror the input depth image.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthImageData"/> or the <paramref name="depthFloatFrame"/>
/// parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthImageDataWidth"/> parameter and depthFloatFrame's
/// <c>width</c> is not equal, or the <paramref name="depthImageDataHeight"/> parameter and
/// depthFloatFrame's <c>height</c> member is not equal.
/// Thrown when the <paramref name="minDepthClip"/> parameter or the
/// <paramref name="maxDepthClip"/> is less than zero.
/// </exception>
/// <exception cref="OutOfMemoryException">
/// Thrown if a CPU memory allocation failed.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// a GPU memory allocation failed or the call failed for an unknown reason.
/// </exception>
public static void DepthToDepthFloatFrame(
DepthImagePixel[] depthImageData,
int depthImageDataWidth,
int depthImageDataHeight,
FusionFloatImageFrame depthFloatFrame,
float minDepthClip,
float maxDepthClip,
bool mirrorDepth)
{
if (null == depthImageData)
{
throw new ArgumentNullException("depthImageData");
}
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionDepthToDepthFloatFrame(
depthImageData,
(uint)depthImageDataWidth,
(uint)depthImageDataHeight,
FusionImageFrame.ToHandleRef(depthFloatFrame),
minDepthClip,
maxDepthClip,
mirrorDepth));
}
/// <summary>
/// Find the most similar camera poses to the current camera input by comparing against the
/// camera pose finder database, and returning a set of similar camera poses. These poses
/// and similarity measurements are ordered in terms of decreasing similarity (i.e. the most
/// similar is first). Both input depth and color frames must be identical sizes, with valid
/// camera parameters and captured at the same time.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="colorFrame">The color frame to be processed.</param>
/// <returns>Returns the matched frames object created by the camera pose finder.</returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="colorFrame"/>
/// parameter is null. </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> and <paramref name="colorFrame"/>
/// parameter is an incorrect or different image size, or their <c>CameraParameters</c>
/// member is null or has incorrectly sized focal lengths.</exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed,
/// or the call failed for an unknown reason.
/// </exception>
/// <returns>Returns a set of matched frames/poses.</returns>
public MatchCandidates FindCameraPose(
FusionFloatImageFrame depthFloatFrame,
FusionColorImageFrame colorFrame)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
INuiFusionMatchCandidates matchCandidates = null;
ExceptionHelper.ThrowIfFailed(cameraPoseFinder.FindCameraPose(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
out matchCandidates));
return new MatchCandidates(matchCandidates);
}
/// <summary>
/// Test input camera frames against the camera pose finder database, adding frames to the
/// database if dis-similar enough to existing frames. Both input depth and color frames
/// must be identical sizes, a minimum size of 80x60, with valid camera parameters, and
/// captured at the same time.
/// Note that once the database reaches its maximum initialized size, it will overwrite old
/// pose information. Check the <pararmref name="pHistoryTrimmed"/> flag or the number of
/// poses in the database to determine whether the old poses are being overwritten.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="colorFrame">The color frame to be processed.</param>
/// <param name="worldToCameraTransform"> The current camera pose (usually the camera pose
/// result from the last AlignPointClouds or AlignDepthFloatToReconstruction).</param>
/// <param name="minimumDistanceThreshold">A float distance threshold between 0 and 1.0f which
/// regulates how close together poses are stored in the database. Input frames
/// which have a minimum distance equal to or above this threshold when compared against the
/// database will be stored, as it indicates the input has become dis-similar to the existing
/// stored poses. Set to 0.0f to ignore and always add a pose when this function is called,
/// however in this case, unless there is an external test of distance, there is a risk this
/// can lead to many duplicated poses.
/// </param>
/// <param name="addedPose">
/// Set true when the input frame was added to the camera pose finder database.
/// </param>
/// <param name="trimmedHistory">
/// Set true if the maxPoseHistoryCount was reached when the input frame is stored, so the
/// oldest pose was overwritten in the camera pose finder database to store the latest pose.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="colorFrame"/>
/// parameter is null. </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> and <paramref name="colorFrame"/>
/// parameter is an incorrect or different image size, or their <c>CameraParameters</c>
/// member is null or has incorrectly sized focal lengths, or the
/// <paramref name="minimumDistanceThreshold"/> parameter is less than 0 or greater
/// than 1.0f.</exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// The camera pose finder works by accumulating whether the values at each sample location pixel
/// in a saved pose frame are less than or greater than a threshold which is randomly chosen
/// between minimum and maximum boundaries (e.g. for color this is 0-255). Given enough samples
/// this represents a unique key frame signature that we can match against, as different poses
/// will have different values for surfaces which are closer or further away, or different
/// colors.
/// Note that unlike depth, the robustness of finding a valid camera pose can have issues with
/// ambient illumination levels in the color image. For best matching results, both the Kinect
/// camera and also the environment should have exactly the same configuration as when the
/// database key frame images were captured i.e. if you had a fixed exposure and custom white
/// balance, this should again be set when testing the database later, otherwise the matching
/// accuracy will be reduced.
/// To improve accuracy, it is possible to not just provide a red, green, blue input in the
/// color image, but instead provide a different 3 channels of match data scaled 0-255. For
/// example, to be more illumination independent, you could calculate hue and saturation, or
/// convert RGB to to LAB and use the AB channels. Other measures such as texture response
/// or corner response could additionally be computed and used in one or more of the channels.
/// </remarks>
public void ProcessFrame(
FusionFloatImageFrame depthFloatFrame,
FusionColorImageFrame colorFrame,
Matrix4 worldToCameraTransform,
float minimumDistanceThreshold,
out bool addedPose,
out bool trimmedHistory)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
HRESULT hr = cameraPoseFinder.ProcessFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
ref worldToCameraTransform,
minimumDistanceThreshold,
out addedPose,
out trimmedHistory);
ExceptionHelper.ThrowIfFailed(hr);
}
private void InitializeKinectFusion()
{
// KinecFusionの初期化
var volParam = new ReconstructionParameters( VoxelsPerMeter, VoxelResolutionX, VoxelResolutionY, VoxelResolutionZ );
volume = Reconstruction.FusionCreateReconstruction( volParam, ReconstructionProcessor.Amp, -1, Matrix4.Identity );
// 変換バッファの作成
depthFloatBuffer = new FusionFloatImageFrame( DepthWidth, DepthHeight );
pointCloudBuffer = new FusionPointCloudImageFrame( DepthWidth, DepthHeight );
shadedSurfaceColorFrame = new FusionColorImageFrame( DepthWidth, DepthHeight );
// リセット
volume.ResetReconstruction( Matrix4.Identity );
}
/// <summary>
/// Render Fusion depth float frame to UI
/// </summary>
/// <param name="depthFloatFrame">Fusion depth float frame</param>
/// <param name="bitmap">Bitmap contains depth float frame data for rendering</param>
/// <param name="image">UI image component to render depth float frame to</param>
private void RenderDepthFloatImage(
FusionFloatImageFrame depthFloatFrame, ref WriteableBitmap bitmap, System.Windows.Controls.Image image)
{
if (null == depthFloatFrame)
{
return;
}
// PixelDataLength is the number of pixels, not bytes
if (null == this.depthFloatFramePixelsArgb || depthFloatFrame.PixelDataLength != this.depthFloatFramePixelsArgb.Length)
{
// Create colored pixel array of correct format
this.depthFloatFramePixelsArgb = new int[depthFloatFrame.PixelDataLength];
}
if (null == this.depthFloatFrameDepthPixels || depthFloatFrame.PixelDataLength != this.depthFloatFrameDepthPixels.Length)
{
// Create colored pixel array of correct format
this.depthFloatFrameDepthPixels = new float[depthFloatFrame.PixelDataLength];
}
if (null == bitmap || depthFloatFrame.Width != bitmap.Width || depthFloatFrame.Height != bitmap.Height)
{
// Create bitmap of correct format
bitmap = new WriteableBitmap(depthFloatFrame.Width, depthFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
// Set bitmap as source to UI image object
image.Source = bitmap;
}
depthFloatFrame.CopyPixelDataTo(this.depthFloatFrameDepthPixels);
// Calculate color of pixels based on depth of each pixel
float range = 4.0f;
float oneOverRange = 1.0f / range;
float minRange = 0.0f;
Parallel.For(
0,
depthFloatFrame.Height,
y =>
{
int index = y * depthFloatFrame.Width;
for (int x = 0; x < depthFloatFrame.Width; ++x, ++index)
{
float depth = this.depthFloatFrameDepthPixels[index];
int intensity = (depth >= minRange) ? ((int)(((depth - minRange) * oneOverRange) * 256.0f) % 256) : 0;
this.depthFloatFramePixelsArgb[index] = (intensity << 16) | (intensity << 8) | intensity; // argb
}
});
// Copy colored pixels to bitmap
bitmap.WritePixels(
new Int32Rect(0, 0, depthFloatFrame.Width, depthFloatFrame.Height),
this.depthFloatFramePixelsArgb,
bitmap.PixelWidth * sizeof(int),
0);
}
/// <summary>
/// Set a reference depth frame to be used internally to help with tracking when calling
/// AlignDepthFloatToReconstruction to calculate a new camera pose. This function should
/// only be called when not using the default tracking behavior of Kinect Fusion.
/// </summary>
/// <param name="referenceDepthFloatFrame">A previous depth float frame where align was
/// successful (and hence same functionality as AlignDepthFloatToReconstruction),
/// or a ray-casted model depth.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="referenceDepthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="referenceDepthFloatFrame"/> parameter is an incorrect
/// image size.</exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// AlignDepthFloatToReconstruction internally saves the last depth frame it was passed and
/// uses this image to help it track when called the next time. For example, this can be used
/// if you are reconstructing and lose track, then want to re-start tracking from a different
/// (known) location without resetting the volume. To enable the tracking to succeed you
/// could perform a raycast from the new location to get a depth image (by calling
/// CalculatePointCloudAndDepth) then call this set function with the depth image, before
/// calling AlignDepthFloatToReconstruction.
/// </remarks>
public void SetAlignDepthFloatToReconstructionReferenceFrame(
FusionFloatImageFrame referenceDepthFloatFrame)
{
if (null == referenceDepthFloatFrame)
{
throw new ArgumentNullException("referenceDepthFloatFrame");
}
ExceptionHelper.ThrowIfFailed(volume.SetAlignDepthFloatToReconstructionReferenceFrame(
FusionImageFrame.ToHandleRef(referenceDepthFloatFrame)));
}
/// <summary>
/// Converts Kinect depth frames in unsigned short format to depth frames in float format
/// representing distance from the camera in meters (parallel to the optical center axis).
/// Note: <paramref name="depthImageData"/> and <paramref name="depthFloatFrame"/> must
/// be the same pixel resolution. This version of the function runs on the GPU.
/// </summary>
/// <param name="depthImageData">The source depth data.</param>
/// <param name="depthFloatFrame">A depth float frame, to be filled with depth.</param>
/// <param name="minDepthClip">The minimum depth threshold. Values below this will be set to 0.</param>
/// <param name="maxDepthClip">The maximum depth threshold. Values above this will be set to 1000.</param>
/// <param name="mirrorDepth">Set true to mirror depth, false so the image appears correct if viewing
/// the Kinect camera from behind.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthImageData"/> or
/// <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthImageData"/> or
/// <paramref name="depthFloatFrame"/> parameter is an incorrect image size, or the
/// kernelWidth is an incorrect size.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// The min and max depth clip values enable clipping of the input data, for example, to help
/// isolate particular objects or surfaces to be reconstructed. Note that the thresholds return
/// different values when a depth pixel is outside the threshold - pixels inside minDepthClip will
/// will be returned as 0 and ignored in processing, whereas pixels beyond maxDepthClip will be set
/// to 1000 to signify a valid depth ray with depth beyond the set threshold. Setting this far-
/// distance flag is important for reconstruction integration in situations where the camera is
/// static or does not move significantly, as it enables any voxels closer to the camera
/// along this ray to be culled instead of persisting (as would happen if the pixels were simply
/// set to 0 and ignored in processing). Note that when reconstructing large real-world size volumes,
/// be sure to set large maxDepthClip distances, as when the camera moves around, any voxels in view
/// which go beyond this threshold distance from the camera will be removed.
/// </remarks>
public void DepthToDepthFloatFrame(
DepthImagePixel[] depthImageData,
FusionFloatImageFrame depthFloatFrame,
float minDepthClip,
float maxDepthClip,
bool mirrorDepth)
{
if (null == depthImageData)
{
throw new ArgumentNullException("depthImageData");
}
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ExceptionHelper.ThrowIfFailed(volume.DepthToDepthFloatFrame(
depthImageData,
(uint)depthImageData.Length * sizeof(int), // DepthImagePixel is 2 ushort per pixel
FusionImageFrame.ToHandleRef(depthFloatFrame),
minDepthClip,
maxDepthClip,
mirrorDepth));
}
/// <summary>
/// Integrates depth float data into the reconstruction volume from the passed
/// camera pose.
/// Note: this function will also set the internal camera pose.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be integrated.</param>
/// <param name="maxIntegrationWeight">
/// A parameter to control the temporal smoothing of depth integration. Minimum value is 1.
/// Lower values have more noisy representations, but objects that move integrate and
/// disintegrate faster, so are suitable for more dynamic environments. Higher values
/// integrate objects more slowly, but provides finer detail with less noise.</param>
/// <param name="worldToCameraTransform">
/// The camera pose (usually the camera pose result from the last
/// FusionDepthProcessor.AlignPointClouds or AlignDepthFloatToReconstruction).
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="maxIntegrationWeight"/> parameter is less than 1 or
/// greater than the maximum unsigned short value.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
public void IntegrateFrame(
FusionFloatImageFrame depthFloatFrame,
int maxIntegrationWeight,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort integrationWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
ExceptionHelper.ThrowIfFailed(volume.IntegrateFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
integrationWeight,
ref worldToCameraTransform));
}
private void InitFusion()
{
if (_isFusionInitialized)
return;
_currentFormat = new KinectFormat();
_currentFormat.DepthImageFormat = DepthImageFormat.Undefined;
_currentFormat.ColorImageFormat = ColorImageFormat.Undefined;
_isFusionInitialized = true;
var depthFormat = KinectSensor.DepthStream.Format;
var colorFormat = KinectSensor.ColorStream.Format;
var kinectFormat = new KinectFormat();
kinectFormat.DepthImageFormat = depthFormat;
kinectFormat.ColorImageFormat = colorFormat;
var depthSize = FormatHelper.GetDepthSize(depthFormat);
_fusionWorkItemPool = new Pool<FusionWorkItem, KinectFormat>(5, kinectFormat, FusionWorkItem.Create);
_fusionWorkQueue = new WorkQueue<FusionWorkItem>(ProcessFusionFrameBackground)
{
CanceledCallback = ReturnFusionWorkItem,
MaxQueueLength = 2
};
this.frameDataLength = KinectSensor.DepthStream.FramePixelDataLength;
// Allocate space to put the color pixels we'll create
this.colorPixels = new int[(int)(depthSize.Width * 2 * depthSize.Height * 2)];
// This is the bitmap we'll display on-screen
this.colorFusionBitmap = new WriteableBitmap(
(int)depthSize.Width * 2,
(int)depthSize.Height * 2,
96.0,
96.0,
PixelFormats.Bgr32,
null);
FusionOutputImage = colorFusionBitmap;
var volParam = new ReconstructionParameters(VoxelsPerMeter, VoxelResolutionX, VoxelResolutionY, VoxelResolutionZ);
// Set the world-view transform to identity, so the world origin is the initial camera location.
this.worldToCameraTransform = Matrix4.Identity;
try
{
// This creates a volume cube with the Kinect at center of near plane, and volume directly
// in front of Kinect.
this.volume = ColorReconstruction.FusionCreateReconstruction(volParam, ProcessorType, DeviceToUse, this.worldToCameraTransform);
this.defaultWorldToVolumeTransform = this.volume.GetCurrentWorldToVolumeTransform();
if (this.translateResetPose)
{
this.ResetReconstruction(_currentVolumeCenter);
}
}
catch (ArgumentException)
{
FusionStatusMessage = "ArgumentException - DX11 GPU not found?";
return;
}
catch (InvalidOperationException ex)
{
FusionStatusMessage = ex.Message;
return;
}
catch (DllNotFoundException)
{
FusionStatusMessage = Properties.Resources.MissingPrerequisite;
return;
}
// Depth frames generated from the depth input
this.depthFloatBuffer = new FusionFloatImageFrame((int)depthSize.Width, (int)depthSize.Height);
this.residualFloatBuffer = new FusionFloatImageFrame((int)depthSize.Width, (int)depthSize.Height);
_residualData = new float[(int)(depthSize.Width * depthSize.Height)];
// Point cloud frames generated from the depth float input
this.pointCloudBuffer = new FusionPointCloudImageFrame((int)depthSize.Width * 2, (int)depthSize.Height * 2);
// Create images to raycast the Reconstruction Volume
this.shadedSurfaceColorFrame = new FusionColorImageFrame((int)depthSize.Width * 2, (int)depthSize.Height * 2);
// Reset the reconstruction
this.ResetReconstruction(_currentVolumeCenter);
IntegratingColor = false;
_audioManager.Start();
}
// Not used
/// <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(FusionFloatImageFrame floatFrame, FusionColorImageFrame imageFrame)
{
MatchCandidates matchCandidates = poseFinder.FindCameraPose(floatFrame, imageFrame);
if (null == matchCandidates) {
return false;
}
int poseCount = matchCandidates.GetPoseCount();
float minDistance = matchCandidates.CalculateMinimumDistance();
if (0 == poseCount || minDistance >= 1) {
return false;
}
// Smooth the depth frame
this.volume.SmoothDepthFloatFrame(floatFrame, smoothDepthFloatFrameCamera, 1, .04f);
// Calculate point cloud from the smoothed frame
FusionDepthProcessor.DepthFloatFrameToPointCloud(smoothDepthFloatFrameCamera, depthPointCloudFrame);
double smallestEnergy = double.MaxValue;
int smallestEnergyNeighborIndex = -1;
int bestNeighborIndex = -1;
Matrix4 bestNeighborCameraPose = Matrix4.Identity;
double bestNeighborAlignmentEnergy = 0.006f;
// Run alignment with best matched poseCount (i.e. k nearest neighbors (kNN))
int maxTests = Math.Min(5, poseCount);
var neighbors = matchCandidates.GetMatchPoses();
for (int 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
Matrix4 poseProposal = neighbors[n];
// Get the saved pose view by raycasting the volume
this.volume.CalculatePointCloud(raycastPointCloudFrame, poseProposal);
bool success = this.volume.AlignPointClouds(
raycastPointCloudFrame,
depthPointCloudFrame,
FusionDepthProcessor.DefaultAlignIterationCount,
imageFrame,
out alignmentEnergy,
ref poseProposal);
bool relocSuccess = success && alignmentEnergy < bestNeighborAlignmentEnergy && alignmentEnergy > 0;
if (relocSuccess) {
bestNeighborAlignmentEnergy = alignmentEnergy;
bestNeighborIndex = n;
// This is after tracking succeeds, so should be a more accurate pose to store...
bestNeighborCameraPose = poseProposal;
// Update the delta image
imageFrame.CopyPixelDataTo(this.deltaFromReferenceFramePixelsArgb);
}
// Find smallest energy neighbor independent of tracking success
if (alignmentEnergy < smallestEnergy) {
smallestEnergy = 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;
return true;
} else {
this.worldToCameraTransform = neighbors[smallestEnergyNeighborIndex];
return false;
}
}
// When both color and depth frames are ready
private void onFrameReady(object sender, AllFramesReadyEventArgs e)
{
// Only get the frames when we're done processing the previous one, to prevent
// frame callbacks from piling up
if (frameReady) {
Monitor.Enter(frameLock);
// Enter a context with both the depth and color frames
using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
using (ColorImageFrame colorFrame = e.OpenColorImageFrame()) {
// Lock resources and prevent further frame callbacks
frameReady = false;
// Init
DepthImagePixel[] depth = new DepthImagePixel[depthFrame.PixelDataLength];
// Obtain raw data from frames
depthFrame.CopyDepthImagePixelDataTo(depth);
colorFrame.CopyPixelDataTo(color);
// Construct into a fusionfloatimageframe
FusionFloatImageFrame frame = new FusionFloatImageFrame(640, 480);
FusionFloatImageFrame smoothDepthFloatFrame = new FusionFloatImageFrame(640, 480);
FusionDepthProcessor.DepthToDepthFloatFrame(depth, 640, 480, frame, FusionDepthProcessor.DefaultMinimumDepth, 2.5f, false);
this.volume.SmoothDepthFloatFrame(frame, smoothDepthFloatFrame, 1, .04f);
/*
byte[] pixelData = new byte[4 * 640 * 480];
colorFrame.CopyPixelDataTo(pixelData);
int[] intPixelData = new int[640 * 480];
Buffer.BlockCopy(pixelData, 0, intPixelData, 0, 640 * 480 * sizeof(int));
FusionColorImageFrame imageFrame = new FusionColorImageFrame(640, 480);
imageFrame.CopyPixelDataFrom(intPixelData);
bool success = FindCameraPoseAlignPointClouds(frame, imageFrame);
Matrix4 t = worldToCameraTransform;
Console.WriteLine("{0} {1} {2} {3}", t.M11, t.M12, t.M13, t.M14);
Console.WriteLine("{0} {1} {2} {3}", t.M21, t.M22, t.M23, t.M24);
Console.WriteLine("{0} {1} {2} {3}", t.M31, t.M32, t.M33, t.M34);
Console.WriteLine("{0} {1} {2} {3}", t.M41, t.M42, t.M43, t.M44);
if (success) {
currentMatrix = t;
}*/
// Calculate point cloud
FusionPointCloudImageFrame observedPointCloud = new FusionPointCloudImageFrame(640, 480);
FusionDepthProcessor.DepthFloatFrameToPointCloud(smoothDepthFloatFrame, observedPointCloud);
FusionPointCloudImageFrame imgFrame = new FusionPointCloudImageFrame(640, 480);
volume.CalculatePointCloud(imgFrame, currentMatrix);
Matrix4 t = currentMatrix;
float alignmentEnergy = 0;
bool success = volume.AlignPointClouds(imgFrame, observedPointCloud, 100, null, out alignmentEnergy, ref t);
Console.WriteLine(success);
Console.WriteLine("{0}", alignmentEnergy);
Console.WriteLine("{0} {1} {2} {3}", t.M11, t.M12, t.M13, t.M14);
Console.WriteLine("{0} {1} {2} {3}", t.M21, t.M22, t.M23, t.M24);
Console.WriteLine("{0} {1} {2} {3}", t.M31, t.M32, t.M33, t.M34);
Console.WriteLine("{0} {1} {2} {3}", t.M41, t.M42, t.M43, t.M44);
if (success) {
currentMatrix = t;
}
frame.Dispose();
smoothDepthFloatFrame.Dispose();
observedPointCloud.Dispose();
imgFrame.Dispose();
// Release resources, now ready for next callback
frameReady = true;
}
Monitor.Exit(frameLock);
}
}
/// <summary>
/// Render Fusion depth float frame to UI
/// </summary>
/// <param name="depthFloatFrame">Fusion depth float frame</param>
/// <param name="depthPixels">Pixel buffer for depth float frame with pixel in depth</param>
/// <param name="colorPixels">Pixel buffer for depth float frame with pixel in colors</param>
/// <param name="bitmap">Bitmap contains depth float frame data for rendering</param>
/// <param name="image">UI image component to render depth float frame to</param>
private static void RenderDepthFloatImage(FusionFloatImageFrame depthFloatFrame, ref float[] depthPixels, ref int[] colorPixels, ref WriteableBitmap bitmap, System.Windows.Controls.Image image)
{
if (null == depthFloatFrame)
{
return;
}
if (null == depthPixels || depthFloatFrame.PixelDataLength != depthPixels.Length)
{
// Create depth pixel array of correct format
depthPixels = new float[depthFloatFrame.PixelDataLength];
}
if (null == colorPixels || depthFloatFrame.PixelDataLength != colorPixels.Length)
{
// Create colored pixel array of correct format
colorPixels = new int[depthFloatFrame.PixelDataLength];
}
if (null == bitmap || depthFloatFrame.Width != bitmap.Width || depthFloatFrame.Height != bitmap.Height)
{
// Create bitmap of correct format
bitmap = new WriteableBitmap(depthFloatFrame.Width, depthFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
// Set bitmap as source to UI image object
image.Source = bitmap;
}
depthFloatFrame.CopyPixelDataTo(depthPixels);
// Calculate color pixels based on depth of each pixel
float range = 4.0f;
float minRange = 0.0f;
for (int i = 0; i < depthPixels.Length; i++)
{
float depth = depthPixels[i];
int intensity = (depth >= minRange) ? ((int)(((depth - minRange) / range) * 256.0f) % 256) : 0;
colorPixels[i] = 0;
colorPixels[i] += intensity; // blue
intensity *= 256;
colorPixels[i] += intensity; // green
intensity *= 256;
colorPixels[i] += intensity; // red
}
// Copy colored pixels to bitmap
bitmap.WritePixels(
new Int32Rect(0, 0, depthFloatFrame.Width, depthFloatFrame.Height),
colorPixels,
bitmap.PixelWidth * sizeof(int),
0);
}
/// <summary>
/// Aligns a depth float image to the Reconstruction volume to calculate the new camera pose.
/// This camera tracking method requires a Reconstruction volume, and updates the internal
/// camera pose if successful. The maximum image resolution supported in this function is 640x480.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="maxAlignIterationCount">
/// The maximum number of iterations of the algorithm to run.
/// The minimum value is 1. Using only a small number of iterations will have a faster runtime,
/// however, the algorithm may not converge to the correct transformation.
/// </param>
/// <param name="deltaFromReferenceFrame">
/// Optionally, a pre-allocated float image frame, to be filled with information about how
/// well each observed pixel aligns with the passed in reference frame. This may be processed
/// to create a color rendering, or may be used as input to additional vision algorithms such
/// as object segmentation. These residual values are normalized -1 to 1 and represent the
/// alignment cost/energy for each pixel. Larger magnitude values (either positive or negative)
/// represent more discrepancy, and lower values represent less discrepancy or less information
/// at that pixel.
/// Note that if valid depth exists, but no reconstruction model exists behind the depth
/// pixels, 0 values indicating perfect alignment will be returned for that area. In contrast,
/// where no valid depth occurs 1 values will always be returned. Pass null if not required.
/// </param>
/// <param name="alignmentEnergy">
/// A float to receive a value describing how well the observed frame aligns to the model with
/// the calculated pose. A larger magnitude value represent more discrepancy, and a lower value
/// represent less discrepancy. Note that it is unlikely an exact 0 (perfect alignment) value
/// will ever be returned as every frame from the sensor will contain some sensor noise.
/// </param>
/// <param name="worldToCameraTransform">
/// The best guess of the camera pose (usually the camera pose result from the last
/// AlignPointClouds or AlignDepthFloatToReconstruction).
/// </param>
/// <returns>
/// Returns true if successful; return false if the algorithm encountered a problem aligning
/// the input depth image and could not calculate a valid transformation.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is an incorrect image size.
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1 or
/// an incorrect value.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// Note that this function is designed primarily for tracking either with static scenes when
/// performing environment reconstruction, or objects which move rigidly when performing object
/// reconstruction from a static camera. Consider using the function AlignPointClouds instead
/// if tracking failures occur due to parts of a scene which move non-rigidly or should be
/// considered as outliers, although in practice, such issues are best avoided by carefully
/// designing or constraining usage scenarios wherever possible.
/// </remarks>
public bool AlignDepthFloatToReconstruction(
FusionFloatImageFrame depthFloatFrame,
int maxAlignIterationCount,
FusionFloatImageFrame deltaFromReferenceFrame,
out float alignmentEnergy,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
HRESULT hr = volume.AlignDepthFloatToReconstruction(
FusionImageFrame.ToHandleRef(depthFloatFrame),
maxIterations,
FusionImageFrame.ToHandleRef(deltaFromReferenceFrame),
out alignmentEnergy,
ref worldToCameraTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return false;
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return true;
}
public void Evaluate(int SpreadMax)
{
this.VoxelResolutionX = this.FInVX[0];
this.VoxelResolutionY = this.FInVY[0];
this.VoxelResolutionZ = this.FInVZ[0];
this.VoxelsPerMeter = this.FInVPM[0];
if (this.FTextureOutput[0] == null) { this.FTextureOutput[0] = new DX11Resource<DX11DynamicTexture2D>(); }
if (this.FPCOut[0] == null) { this.FPCOut[0] = new DX11Resource<IDX11ReadableStructureBuffer>(); }
if (this.FGeomOut[0] == null) { this.FGeomOut[0] = new DX11Resource<DX11IndexedGeometry>(); }
if (this.FOutVoxels[0] == null) { this.FOutVoxels[0] = new DX11Resource<IDX11ReadableStructureBuffer>(); }
if (this.FInExport[0]) { this.FGeomOut[0].Dispose(); this.FGeomOut[0] = new DX11Resource<DX11IndexedGeometry>(); }
if (this.FInvalidateConnect)
{
this.FInvalidateConnect = false;
if (this.FInRuntime.PluginIO.IsConnected)
{
this.runtime = this.FInRuntime[0];
this.runtime.DepthFrameReady += this.runtime_DepthFrameReady;
var volParam = new ReconstructionParameters(VoxelsPerMeter, VoxelResolutionX, VoxelResolutionY, VoxelResolutionZ);
this.worldToCameraTransform = Matrix4.Identity;
//this.volume = Reconstruction.FusionCreateReconstruction(volParam, ProcessorType, 0, this.worldToCameraTransform);
this.colorVolume = ColorReconstruction.FusionCreateReconstruction(volParam, ProcessorType, 0, this.worldToCameraTransform);
//this.volume.
/*FusionPointCloudImageFrame pc;
pc.*/
this.defaultWorldToVolumeTransform = this.colorVolume.GetCurrentWorldToVolumeTransform();
// Depth frames generated from the depth input
this.depthFloatBuffer = new FusionFloatImageFrame(width, height);
// Point cloud frames generated from the depth float input
this.pointCloudBuffer = new FusionPointCloudImageFrame(width, height);
// Create images to raycast the Reconstruction Volume
this.shadedSurfaceColorFrame = new FusionColorImageFrame(width, height);
this.ResetReconstruction();
}
}
if (this.runtime != null)
{
bool needreset = this.FInReset[0];
if (needreset) { this.ResetReconstruction(); }
}
}
/// <summary>
/// A high-level function to process a depth frame through the Kinect Fusion pipeline.
/// Specifically, this performs processing equivalent to the following functions for each frame:
/// <para>
/// 1) AlignDepthFloatToReconstruction
/// 2) IntegrateFrame
/// </para>
/// If there is a tracking error in the AlignDepthFloatToReconstruction stage, no depth data
/// integration will be performed, and the camera pose will remain unchanged.
/// The maximum image resolution supported in this function is 640x480.
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be processed.</param>
/// <param name="maxAlignIterationCount">
/// The maximum number of iterations of the align camera tracking algorithm to run.
/// The minimum value is 1. Using only a small number of iterations will have a faster
/// runtime, however, the algorithm may not converge to the correct transformation.
/// </param>
/// <param name="maxIntegrationWeight">
/// A parameter to control the temporal smoothing of depth integration. Lower values have
/// more noisy representations, but objects that move appear and disappear faster, so are
/// suitable for more dynamic environments. Higher values integrate objects more slowly,
/// but provides finer detail with less noise.
/// </param>
/// <param name="worldToCameraTransform">
/// The best guess of the latest camera pose (usually the camera pose result from the last
/// process call).
/// </param>
/// <returns>
/// Returns true if successful; return false if the algorithm encountered a problem aligning
/// the input depth image and could not calculate a valid transformation.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> parameter is an incorrect image size.
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1 or
/// greater than the maximum unsigned short value.
/// Thrown when the <paramref name="maxIntegrationWeight"/> parameter is less than 1 or
/// greater than the maximum unsigned short value.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected,
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// Users may also optionally call the low-level functions individually, instead of calling this
/// function, for more control. However, this function call will be faster due to the integrated
/// nature of the calls. After this call completes, if a visible output image of the reconstruction
/// is required, the user can call CalculatePointCloud and then FusionDepthProcessor.ShadePointCloud.
/// </remarks>
public bool ProcessFrame(
FusionFloatImageFrame depthFloatFrame,
int maxAlignIterationCount,
int maxIntegrationWeight,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
ushort maxWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
HRESULT hr = volume.ProcessFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
maxIterations,
maxWeight,
ref worldToCameraTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return false;
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return true;
}
/// <summary>
/// Execute startup tasks
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void WindowLoaded(object sender, RoutedEventArgs e)
{
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break;
}
}
if (null == this.sensor)
{
this.statusBarText.Text = Properties.Resources.NoKinectReady;
return;
}
// Turn on the depth stream to receive depth frames
this.sensor.DepthStream.Enable(DepthImageResolution);
this.sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
this.frameDataLength = this.sensor.DepthStream.FramePixelDataLength;
// Allocate space to put the color pixels we'll create
this.colorPixels = new int[this.frameDataLength];
// This is the bitmap we'll display on-screen
this.colorBitmap = new WriteableBitmap(
(int)ImageSize.Width,
(int)ImageSize.Height,
96.0,
96.0,
PixelFormats.Bgr32,
null);
// Set the image we display to point to the bitmap where we'll put the image data
this.Image.Source = this.colorBitmap;
// Add an event handler to be called whenever there is new depth frame data
this.sensor.DepthFrameReady += this.SensorDepthFrameReady;
this.sensor.ColorFrameReady += this.kinect_colorframe_ready;
var volParam = new ReconstructionParameters(VoxelsPerMeter, VoxelResolutionX, VoxelResolutionY, VoxelResolutionZ);
// Set the world-view transform to identity, so the world origin is the initial camera location.
this.worldToCameraTransform = Matrix4.Identity;
try
{
// This creates a volume cube with the Kinect at center of near plane, and volume directly
// in front of Kinect.
this.volume = Reconstruction.FusionCreateReconstruction(volParam, ProcessorType, DeviceToUse, this.worldToCameraTransform);
this.defaultWorldToVolumeTransform = this.volume.GetCurrentWorldToVolumeTransform();
if (this.translateResetPoseByMinDepthThreshold)
{
this.ResetReconstruction();
}
}
catch (InvalidOperationException ex)
{
this.statusBarText.Text = ex.Message;
return;
}
catch (DllNotFoundException)
{
this.statusBarText.Text = this.statusBarText.Text = Properties.Resources.MissingPrerequisite;
return;
}
// Depth frames generated from the depth input
this.depthFloatBuffer = new FusionFloatImageFrame((int)ImageSize.Width, (int)ImageSize.Height);
// Point cloud frames generated from the depth float input
this.pointCloudBuffer = new FusionPointCloudImageFrame((int)ImageSize.Width, (int)ImageSize.Height);
// Create images to raycast the Reconstruction Volume
this.shadedSurfaceColorFrame = new FusionColorImageFrame((int)ImageSize.Width, (int)ImageSize.Height);
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException ex)
{
// Device is in use
this.sensor = null;
this.statusBarText.Text = ex.Message;
return;
}
catch (InvalidOperationException ex)
{
// Device is not valid, not supported or hardware feature unavailable
this.sensor = null;
this.statusBarText.Text = ex.Message;
return;
}
// Set Near Mode by default
try
{
this.sensor.DepthStream.Range = DepthRange.Near;
checkBoxNearMode.IsChecked = true;
}
catch
{
// device not near mode capable
}
// Initialize and start the FPS timer
this.fpsTimer = new DispatcherTimer();
this.fpsTimer.Tick += new EventHandler(this.FpsTimerTick);
this.fpsTimer.Interval = new TimeSpan(0, 0, FpsInterval);
this.fpsTimer.Start();
// Reset the reconstruction
this.ResetReconstruction();
}
/// <summary>
/// Spatially smooth a depth float image frame using edge-preserving filtering on GPU.
/// </summary>
/// <param name="depthFloatFrame">A source depth float frame.</param>
/// <param name="smoothDepthFloatFrame">A depth float frame, to be filled with smoothed
/// depth.</param>
/// <param name="kernelWidth">Smoothing Kernel Width. Valid values are 1,2,3
/// (for 3x3,5x5,7x7 smoothing kernel block size respectively).</param>
/// <param name="distanceThreshold">A distance difference range that smoothing occurs in.
/// Pixels with neighboring pixels outside this distance range will not be smoothed
/// (larger values indicate discontinuity/edge). Must be greater than 0.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="smoothDepthFloatFrame"/> or
/// <paramref name="depthFloatFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="smoothDepthFloatFrame"/> or
/// <paramref name="depthFloatFrame"/> parameter is an incorrect image size, or the
/// kernelWidth is an incorrect size.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
public void SmoothDepthFloatFrame(
FusionFloatImageFrame depthFloatFrame,
FusionFloatImageFrame smoothDepthFloatFrame,
int kernelWidth,
float distanceThreshold)
{
if (null == smoothDepthFloatFrame)
{
throw new ArgumentNullException("smoothDepthFloatFrame");
}
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
uint kW = ExceptionHelper.CastAndThrowIfOutOfUintRange(kernelWidth);
ExceptionHelper.ThrowIfFailed(volume.SmoothDepthFloatFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(smoothDepthFloatFrame),
kW,
distanceThreshold));
}
protected virtual void Dispose( bool disposing )
{
if ( !disposed ) {
if ( depthFloatBuffer != null ) {
depthFloatBuffer.Dispose();
depthFloatBuffer = null;
}
if ( pointCloudBuffer != null ) {
pointCloudBuffer.Dispose();
pointCloudBuffer = null;
}
if ( shadedSurfaceColorFrame != null ) {
shadedSurfaceColorFrame.Dispose();
shadedSurfaceColorFrame = null;
}
if ( volume != null ) {
volume.Dispose();
volume = null;
}
disposed = true;
}
}
/// <summary>
/// Calculate a point cloud and depth image by raycasting into the reconstruction volume.
/// This returns the point cloud containing 3D points and normals of the zero-crossing dense
/// surface at every visible pixel in the image from the given camera pose, the depth
/// to the surface.
/// </summary>
/// <param name="pointCloudFrame">A point cloud frame, to be filled by raycasting into the
/// reconstruction volume. Typically used as the reference frame with the
/// FusionDepthProcessor.AlignPointClouds function or for visualization by calling
/// FusionDepthProcessor.ShadePointCloud.</param>
/// <param name="depthFloatFrame">A floating point depth frame, to be filled with floating point
/// depth in meters to the raycast surface. This image must be identical
/// in size, and camera parameters to the <paramref name="pointCloudFrame"/> parameter.</param>
/// <param name="worldToCameraTransform">The world-to-camera transform (camera pose) to raycast
/// from.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="pointCloudFrame"/> or <paramref name="depthFloatFrame"/>
/// parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="pointCloudFrame"/> or<paramref name="depthFloatFrame"/>
/// parameter is an incorrect image size.
/// </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the Kinect Runtime could not be accessed, the device is not connected
/// or the call failed for an unknown reason.
/// </exception>
/// <remarks>
/// This point cloud can then be used as a reference frame in the next call to
/// FusionDepthProcessor.AlignPointClouds, or passed to FusionDepthProcessor.ShadePointCloud
/// to produce a visible image output.The depth image can be used as a reference frame for
/// AlignDepthFloatToReconstruction by calling SetAlignDepthFloatToReconstructionReferenceFrame
/// to enable a greater range of tracking. The <paramref name="pointCloudFrame"/> and
/// <paramref name="depthFloatFrame"/> parameters can be an arbitrary image size, for example,
/// enabling you to calculate point clouds at the size of your UI window and then create a visible
/// image by calling FusionDepthProcessor.ShadePointCloud and render this image, however, be aware
/// that the calculation of high resolution images will be expensive in terms of runtime.
/// </remarks>
public void CalculatePointCloudAndDepth(
FusionPointCloudImageFrame pointCloudFrame,
FusionFloatImageFrame depthFloatFrame,
Matrix4 worldToCameraTransform)
{
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
ExceptionHelper.ThrowIfFailed(volume.CalculatePointCloudAndDepth(
FusionImageFrame.ToHandleRef(pointCloudFrame),
FusionImageFrame.ToHandleRef(depthFloatFrame),
ref worldToCameraTransform));
}
/// <summary>
/// Render Fusion AlignDepthFloatToReconstruction float deltas frame to UI
/// </summary>
/// <param name="alignDeltasFloatFrame">Fusion depth float frame</param>
/// <param name="bitmap">Bitmap contains float frame data for rendering</param>
/// <param name="image">UI image component to render float frame to</param>
private void RenderAlignDeltasFloatImage(FusionFloatImageFrame alignDeltasFloatFrame, ref WriteableBitmap bitmap, System.Windows.Controls.Image image)
{
if (null == alignDeltasFloatFrame)
{
return;
}
if (null == this.deltaFromReferenceFrameFloatPixels || alignDeltasFloatFrame.PixelDataLength != this.deltaFromReferenceFrameFloatPixels.Length)
{
// Create depth pixel array of correct format
this.deltaFromReferenceFrameFloatPixels = new float[alignDeltasFloatFrame.PixelDataLength];
}
if (null == this.deltaFromReferenceFramePixels || alignDeltasFloatFrame.PixelDataLength != this.deltaFromReferenceFramePixels.Length)
{
// Create colored pixel array of correct format
this.deltaFromReferenceFramePixels = new int[alignDeltasFloatFrame.PixelDataLength];
}
if (null == bitmap || alignDeltasFloatFrame.Width != bitmap.Width || alignDeltasFloatFrame.Height != bitmap.Height)
{
// Create bitmap of correct format
bitmap = new WriteableBitmap(alignDeltasFloatFrame.Width, alignDeltasFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
// Set bitmap as source to UI image object
image.Source = bitmap;
}
alignDeltasFloatFrame.CopyPixelDataTo(this.deltaFromReferenceFrameFloatPixels);
Parallel.For(
0,
alignDeltasFloatFrame.Height,
y =>
{
int index = y * alignDeltasFloatFrame.Width;
for (int x = 0; x < alignDeltasFloatFrame.Width; ++x, ++index)
{
float residue = this.deltaFromReferenceFrameFloatPixels[index];
if (residue < 1.0f)
{
this.deltaFromReferenceFramePixels[index] = (byte)(255.0f * ClampFloat(1.0f - residue, 0.0f, 1.0f)); // blue
this.deltaFromReferenceFramePixels[index] |= ((byte)(255.0f * ClampFloat(1.0f - Math.Abs(residue), 0.0f, 1.0f))) << 8; // green
this.deltaFromReferenceFramePixels[index] |= ((byte)(255.0f * ClampFloat(1.0f + residue, 0.0f, 1.0f))) << 16; // red
}
else
{
this.deltaFromReferenceFramePixels[index] = 0;
}
}
});
// Copy colored pixels to bitmap
bitmap.WritePixels(
new Int32Rect(0, 0, alignDeltasFloatFrame.Width, alignDeltasFloatFrame.Height),
this.deltaFromReferenceFramePixels,
bitmap.PixelWidth * sizeof(int),
0);
}