/// <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>
/// Create a visible color shaded image of a point cloud and its normals. All image
/// frames must have the same width and height.
/// </summary>
/// <param name="pointCloudFrame">The point cloud frame to be shaded.</param>
/// <param name="worldToCameraTransform">
/// The world to camera transform (camera pose) where the raycast was performed from.
/// Pass identity if the point cloud did not originate from a raycast and is in the
/// camera local coordinate system.
/// </param>
/// <param name="shadedSurfaceFrame">
/// Optionally, a pre-allocated color image frame, to be filled with the color L.N shaded
/// surface image. Pass null to skip this image.
/// </param>
/// <param name="shadedSurfaceNormalsFrame">
/// Optionally, a pre-allocated color image frame, to be filled with the color shaded
/// normals image with color indicating orientation. Pass null to skip this image.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="pointCloudFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="pointCloudFrame"/> or <paramref name="shadedSurfaceFrame"/>
/// or <paramref name="shadedSurfaceNormalsFrame"/> 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 ShadePointCloud(
FusionPointCloudImageFrame pointCloudFrame,
Matrix4 worldToCameraTransform,
FusionColorImageFrame shadedSurfaceFrame,
FusionColorImageFrame shadedSurfaceNormalsFrame)
{
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionShadePointCloud2(
FusionImageFrame.ToHandleRef(pointCloudFrame),
ref worldToCameraTransform,
IntPtr.Zero,
FusionImageFrame.ToHandleRef(shadedSurfaceFrame),
FusionImageFrame.ToHandleRef(shadedSurfaceNormalsFrame)));
}
/// <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>
/// The AlignPointClouds function uses an iterative algorithm to align two sets of oriented
/// point clouds and calculate the camera's relative pose. This is a generic function which
/// can be used independently of a Reconstruction Volume with sets of overlapping point clouds.
/// To find the frame to frame relative transformation between two sets of point clouds in
/// the camera local frame of reference (created by DepthFloatFrameToPointCloud),
/// set the <paramref name="observedToReferenceTransform"/> to the identity.
/// To calculate the pose transformation between new depth frames and an existing
/// Reconstruction volume, pass in previous frames point cloud from RenderReconstruction as
/// the reference frame, and the current frame point cloud (from DepthFloatFrameToPointCloud)
/// as the observed frame. Set the <paramref name="observedToReferenceTransform"/> to the
/// previous frames calculated camera pose.
/// Note that here the current frame point cloud will be in the camera local frame of
/// reference, whereas the synthetic points and normals will be in the global/world volume
/// coordinate system. By passing the <paramref name="observedToReferenceTransform"/> you
/// make the algorithm aware of the transformation between them.
/// The <paramref name="observedToReferenceTransform"/> pose supplied can also take into
/// account information you may have from other sensors or sensing mechanisms to aid the
/// tracking. To do this multiply the relative frame to frame delta transformation from
/// the other sensing system with the previous frame's pose before passing to this function.
/// Note that any delta transform used should be in the same coordinate system as that
/// returned by the DepthFloatFrameToPointCloud calculation.
/// </summary>
/// <param name="referencePointCloudFrame">
/// The point cloud frame of the reference camera, or the previous Kinect point cloud frame.
/// </param>
/// <param name="observedPointCloudFrame">
/// The point cloud frame of the observed camera, or the current Kinect frame.
/// </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 color image frame, to be filled with color-coded data
/// from the camera tracking. Values vary depending on whether the pixel was a valid pixel
/// used in tracking (green) or failed in different tests. Pass null if not required.
/// </param>
/// <param name="observedToReferenceTransform">
/// A pre-allocated transformation matrix. At entry to the function this should be filled
/// with the best guess for the observed to reference transform (usually the last frame's
/// calculated pose). At exit this is filled with he calculated pose or identity if the
/// calculation failed.
/// </param>
/// <returns>
/// Returns true if successful; returns false if the algorithm encountered a problem aligning
/// the input point clouds and could not calculate a valid transformation, and
/// the <paramref name="observedToReferenceTransform"/> parameter is set to identity.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="referencePointCloudFrame"/> or the
/// <paramref name="observedPointCloudFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="referencePointCloudFrame"/> or <paramref name="observedPointCloudFrame"/>
/// or <paramref name="deltaFromReferenceFrame"/> parameters are different image sizes.
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1.
/// </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 bool AlignPointClouds(
FusionPointCloudImageFrame referencePointCloudFrame,
FusionPointCloudImageFrame observedPointCloudFrame,
int maxAlignIterationCount,
FusionColorImageFrame deltaFromReferenceFrame,
ref Matrix4 observedToReferenceTransform)
{
if (null == referencePointCloudFrame)
{
throw new ArgumentNullException("referencePointCloudFrame");
}
if (null == observedPointCloudFrame)
{
throw new ArgumentNullException("observedPointCloudFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
HRESULT hr = NativeMethods.NuiFusionAlignPointClouds(
FusionImageFrame.ToHandleRef(referencePointCloudFrame),
FusionImageFrame.ToHandleRef(observedPointCloudFrame),
maxIterations,
FusionImageFrame.ToHandleRef(deltaFromReferenceFrame),
ref observedToReferenceTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return(false);
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return(true);
}
/// <summary>
/// A high-level function to process a depth frame through the Kinect Fusion pipeline.
/// Also integrates color, further using a parameter to constrain the integration to
/// integrate color over a given angle relative to the surface normal (recommended use
/// is for thin structure scanning).
/// 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="colorFrame">The color 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="maxColorIntegrationAngle">An angle parameter in degrees to specify the angle
/// with respect to the surface normal over which color will be integrated.This can be used so
/// only when the camera sensor is near parallel with the surface (i.e. the camera direction of
/// view is perpendicular to the surface), or +/- an angle from the surface normal direction that
/// color is integrated.
/// Pass FusionDepthProcessor.DefaultColorIntegrationOfAllAngles to ignore and accept color from
/// all angles (default, fastest processing).
/// This angle relative to this normal direction vector describe the acceptance half angle, for
/// example, a +/- 90 degree acceptance angle in all directions (i.e. a 180 degree hemisphere)
/// relative to the normal would integrate color in any orientation of the sensor towards the
/// front of the surface, even when parallel to the surface, whereas a 0 acceptance angle would
/// only integrate color directly along a single ray exactly perpendicular to the surface.
/// In reality, the useful range of values is actually between 0 and 90 exclusively
/// (e.g. setting +/- 60 degrees = 120 degrees total acceptance angle).
/// Note that there is a trade-off here, as setting this has a runtime cost, however, conversely,
/// ignoring this will integrate color from any angle over all voxels along camera rays around the
/// zero crossing surface region in the volume, which can cause thin structures to have the same
/// color on both sides.</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"/> or <paramref name="colorFrame"/>
/// parameter is null. </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="depthFloatFrame"/> or <paramref name="colorFrame"/>
/// 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.
/// Thrown when the <paramref name="maxColorIntegrationAngle"/> parameter value is not
/// FusionDepthProcessor.DefaultColorIntegrationOfAllAngles or between 0 and 90 degrees,
/// exclusively.
/// </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,
FusionColorImageFrame colorFrame,
int maxAlignIterationCount,
int maxIntegrationWeight,
float maxColorIntegrationAngle,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
ushort maxWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
HRESULT hr = volume.ProcessFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
maxIterations,
maxWeight,
maxColorIntegrationAngle,
ref worldToCameraTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return false;
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return true;
}
/// <summary>
/// Integrates depth float data and color data into the reconstruction volume from the
/// passed camera pose. Here the angle parameter constrains the integration to
/// integrate color over a given angle relative to the surface normal (recommended use
/// is for thin structure scanning).
/// </summary>
/// <param name="depthFloatFrame">The depth float frame to be integrated.</param>
/// <param name="colorFrame">The color 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="maxColorIntegrationAngle">An angle parameter in degrees to specify the angle
/// with respect to the surface normal over which color will be integrated. This can be used so
/// only when the camera sensor is near parallel with the surface (i.e. the camera direction of
/// view is perpendicular to the surface), or +/- an angle from the surface normal direction that
/// color is integrated.
/// Pass FusionDepthProcessor.DefaultColorIntegrationOfAllAngles to ignore and accept color from
/// all angles (default, fastest processing).
/// This angle relative to this normal direction vector describe the acceptance half angle, for
/// example, a +/- 90 degree acceptance angle in all directions (i.e. a 180 degree hemisphere)
/// relative to the normal would integrate color in any orientation of the sensor towards the
/// front of the surface, even when parallel to the surface, whereas a 0 acceptance angle would
/// only integrate color directly along a single ray exactly perpendicular to the surface.
/// In reality, the useful range of values is actually between 0 and 90 exclusively
/// (e.g. setting +/- 60 degrees = 120 degrees total acceptance angle).
/// Note that there is a trade-off here, as setting this has a runtime cost, however, conversely,
/// ignoring this will integrate color from any angle over all voxels along camera rays around the
/// zero crossing surface region in the volume, which can cause thin structures to have the same
/// color on both sides</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"/> or <paramref name="colorFrame"/>
/// 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, or the
/// Thrown when the <paramref name="maxColorIntegrationAngle"/> parameter value is not
/// FusionDepthProcessor.DefaultColorIntegrationOfAllAngles or between 0 and 90 degrees,
/// exclusively.
/// </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,
FusionColorImageFrame colorFrame,
int maxIntegrationWeight,
float maxColorIntegrationAngle,
Matrix4 worldToCameraTransform)
{
if (null == depthFloatFrame)
{
throw new ArgumentNullException("depthFloatFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
ushort integrationWeight = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxIntegrationWeight);
ExceptionHelper.ThrowIfFailed(volume.IntegrateFrame(
FusionImageFrame.ToHandleRef(depthFloatFrame),
FusionImageFrame.ToHandleRef(colorFrame),
integrationWeight,
maxColorIntegrationAngle,
ref worldToCameraTransform));
}
/// <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 bool TrackIntegrate(DepthImagePixel[] depthPixels, byte[] colorPixels, KinectFormat workFormat)
{
var depthSize = FormatHelper.GetDepthSize(workFormat.DepthImageFormat);
var colorSize = FormatHelper.GetColorSize(workFormat.ColorImageFormat);
// Convert the depth image frame to depth float image frame
FusionDepthProcessor.DepthToDepthFloatFrame(
depthPixels,
(int)depthSize.Width,
(int)depthSize.Height,
this.depthFloatBuffer,
FusionDepthProcessor.DefaultMinimumDepth,
FusionDepthProcessor.DefaultMaximumDepth,
false);
bool trackingSucceeded = this.volume.AlignDepthFloatToReconstruction(
depthFloatBuffer,
FusionDepthProcessor.DefaultAlignIterationCount,
residualFloatBuffer,
out _alignmentEnergy,
volume.GetCurrentWorldToCameraTransform());
//if (trackingSucceeded && _alignmentEnergy == 0.0)
// trackingSucceeded = false;
// ProcessFrame will first calculate the camera pose and then integrate
// if tracking is successful
//bool trackingSucceeded = this.volume.ProcessFrame(
// this.depthFloatBuffer,
// FusionDepthProcessor.DefaultAlignIterationCount,
// IntegrationWeight,
// this.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)
{
this.trackingErrorCount++;
// Show tracking error on status bar
FusionStatusMessage = Properties.Resources.CameraTrackingFailed;
_audioManager.State = AudioState.Error;
}
else
{
ProcessResidualImage();
this.worldToCameraTransform = volume.GetCurrentWorldToCameraTransform();
if (!IsIntegrationPaused)
{
if (IntegratingColor)
{
FusionColorImageFrame frame = new FusionColorImageFrame((int)colorSize.Width, (int)colorSize.Height);
Single colorIntegrationAngle = 10.0f;
int[] intColorPixels = new int[colorPixels.Length / 4];
Buffer.BlockCopy(colorPixels, 0, intColorPixels, 0, colorPixels.Length);
frame.CopyPixelDataFrom(intColorPixels);
this.volume.IntegrateFrame(depthFloatBuffer, frame, FusionDepthProcessor.DefaultIntegrationWeight, colorIntegrationAngle, this.worldToCameraTransform);
}
else
{
this.volume.IntegrateFrame(depthFloatBuffer, IntegrationWeight, this.worldToCameraTransform);
}
}
this.trackingErrorCount = 0;
}
if (AutoResetReconstructionWhenLost && !trackingSucceeded && this.trackingErrorCount == MaxTrackingErrors)
{
// Auto Reset due to bad tracking
FusionStatusMessage = Properties.Resources.ResetVolume;
// Automatically Clear Volume and reset tracking if tracking fails
this.ResetReconstruction(_currentVolumeCenter);
}
return trackingSucceeded;
}
/// <summary>
/// The AlignPointClouds function uses an iterative algorithm to align two sets of oriented
/// point clouds and calculate the camera's relative pose. This is a generic function which
/// can be used independently of a Reconstruction Volume with sets of overlapping point clouds.
/// All images must be the same size and have the same camera parameters.
/// To find the frame-to-frame relative transformation between two sets of point clouds in
/// the camera local frame of reference (created by DepthFloatFrameToPointCloud),
/// set the <paramref name="observedToReferenceTransform"/> to the identity.
/// To calculate the frame-to-model pose transformation between point clouds calculated from
/// new depth frames with DepthFloatFrameToPointCloud and point clouds calculated from an
/// existing Reconstruction volume with CalculatePointCloud (e.g. from the previous frame),
/// pass the CalculatePointCloud image as the reference frame, and the current depth frame
/// point cloud from DepthFloatFrameToPointCloud as the observed frame. Set the
/// <paramref name="observedToReferenceTransform"/> to the previous frames calculated camera
/// pose that was used in the CalculatePointCloud call.
/// Note that here the current frame point cloud will be in the camera local frame of
/// reference, whereas the raycast points and normals will be in the global/world coordinate
/// system. By passing the <paramref name="observedToReferenceTransform"/> you make the
/// algorithm aware of the transformation between the two coordinate systems.
/// The <paramref name="observedToReferenceTransform"/> pose supplied can also take into
/// account information you may have from other sensors or sensing mechanisms to aid the
/// tracking. To do this multiply the relative frame to frame delta transformation from
/// the other sensing system with the previous frame's pose before passing to this function.
/// Note that any delta transform used should be in the same coordinate system as that
/// returned by the DepthFloatFrameToPointCloud calculation.
/// </summary>
/// <param name="referencePointCloudFrame">
/// The point cloud frame of the reference camera, or the previous Kinect point cloud frame.
/// </param>
/// <param name="observedPointCloudFrame">
/// The point cloud frame of the observed camera, or the current Kinect frame.
/// </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 color image frame, to be filled with color-coded data
/// from the camera tracking. This may be used as input to additional vision algorithms such as
/// object segmentation. Values vary depending on whether the pixel was a valid pixel used in
/// tracking (inlier) or failed in different tests (outlier). 0xff000000 indicates an invalid
/// input vertex (e.g. from 0 input depth), or one where no correspondences occur between point
/// cloud images. Outlier vertices rejected due to too large a distance between vertices are
/// coded as 0xff008000. Outlier vertices rejected due to to large a difference in normal angle
/// between point clouds are coded as 0xff800000. Inliers are color shaded depending on the
/// residual energy at that point, with more saturated colors indicating more discrepancy
/// between vertices and less saturated colors (i.e. more white) representing less discrepancy,
/// or less information at that pixel. Pass null if this image is not required.
/// </param>
/// <param name="observedToReferenceTransform">
/// A pre-allocated transformation matrix. At entry to the function this should be filled
/// with the best guess for the observed to reference transform (usually the last frame's
/// calculated pose). At exit this is filled with he calculated pose or identity if the
/// calculation failed.
/// </param>
/// <returns>
/// Returns true if successful; returns false if the algorithm encountered a problem aligning
/// the input point clouds and could not calculate a valid transformation, and
/// the <paramref name="observedToReferenceTransform"/> parameter is set to identity.
/// </returns>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="referencePointCloudFrame"/> or the
/// <paramref name="observedPointCloudFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="referencePointCloudFrame"/> or <paramref name="observedPointCloudFrame"/>
/// or <paramref name="deltaFromReferenceFrame"/> parameters are different image sizes.
/// Thrown when the <paramref name="referencePointCloudFrame"/> or <paramref name="observedPointCloudFrame"/>
/// or <paramref name="deltaFromReferenceFrame"/> parameters have different camera parameters.
/// Thrown when the <paramref name="maxAlignIterationCount"/> parameter is less than 1.
/// </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 bool AlignPointClouds(
FusionPointCloudImageFrame referencePointCloudFrame,
FusionPointCloudImageFrame observedPointCloudFrame,
int maxAlignIterationCount,
FusionColorImageFrame deltaFromReferenceFrame,
ref Matrix4 observedToReferenceTransform)
{
if (null == referencePointCloudFrame)
{
throw new ArgumentNullException("referencePointCloudFrame");
}
if (null == observedPointCloudFrame)
{
throw new ArgumentNullException("observedPointCloudFrame");
}
ushort maxIterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
HRESULT hr = NativeMethods.NuiFusionAlignPointClouds(
FusionImageFrame.ToHandleRef(referencePointCloudFrame),
FusionImageFrame.ToHandleRef(observedPointCloudFrame),
maxIterations,
FusionImageFrame.ToHandleRef(deltaFromReferenceFrame),
ref observedToReferenceTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return false;
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return true;
}
// 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;
}
}
/// <summary>
/// Allocate the frame buffers used for rendering virtualCamera
/// </summary>
private void AllocateFrames()
{
// Allocate point cloud frame
if (null == this.PointCloudFrame || this.depthWidth != this.PointCloudFrame.Width
|| this.depthHeight != this.PointCloudFrame.Height)
{
this.PointCloudFrame = new FusionPointCloudImageFrame(this.depthWidth, this.depthHeight);
}
// Allocate shaded surface frame
if (null == this.ShadedSurfaceFrame || this.depthWidth != this.ShadedSurfaceFrame.Width
|| this.depthHeight != this.ShadedSurfaceFrame.Height)
{
this.ShadedSurfaceFrame = new FusionColorImageFrame(this.depthWidth, this.depthHeight);
}
// Allocate shaded surface normals frame
if (null == this.ShadedSurfaceNormalsFrame || this.depthWidth != this.ShadedSurfaceNormalsFrame.Width
|| this.depthHeight != this.ShadedSurfaceNormalsFrame.Height)
{
this.ShadedSurfaceNormalsFrame = new FusionColorImageFrame(this.depthWidth, this.depthHeight);
}
}
/// <summary>
/// Render Fusion color frame to UI
/// </summary>
/// <param name="colorFrame">Fusion color frame</param>
/// <param name="colorPixels">Pixel buffer for fusion color frame</param>
/// <param name="bitmap">Bitmap contains color frame data for rendering</param>
/// <param name="image">UI image component to render the color frame</param>
private static void RenderColorImage(
FusionColorImageFrame colorFrame, ref int[] colorPixels, ref WriteableBitmap bitmap, System.Windows.Controls.Image image)
{
if (null == colorFrame)
{
return;
}
if (null == colorPixels || colorFrame.PixelDataLength != colorPixels.Length)
{
// Create pixel array of correct format
colorPixels = new int[colorFrame.PixelDataLength];
}
if (null == bitmap || colorFrame.Width != bitmap.Width || colorFrame.Height != bitmap.Height)
{
// Create bitmap of correct format
bitmap = new WriteableBitmap(colorFrame.Width, colorFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
// Set bitmap as source to UI image object
image.Source = bitmap;
}
// Copy pixel data to pixel buffer
colorFrame.CopyPixelDataTo(colorPixels);
// Write pixels to bitmap
bitmap.WritePixels(new Int32Rect(0, 0, colorFrame.Width, colorFrame.Height), colorPixels, bitmap.PixelWidth * sizeof(int), 0);
}
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(); }
}
}
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();
}
/// <summary>
/// Calculate a point cloud by raycasting into the reconstruction volume, returning 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, and optionally the color visualization image.
/// This point cloud can 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 <paramref name="pointCloudFrame"/> can be an arbitrary image size, for example, enabling
/// you to calculate point clouds at the size of your window and then create a visible image by
/// calling FusionDepthProcessor.ShadePointCloud and render this image, however, be aware that
/// large images will be expensive to calculate.
/// </summary>
/// <param name="pointCloudFrame">
/// The pre-allocated 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="colorFrame">Optionally, the color frame to fill. Pass null to ignore.</param>
/// <param name="worldToCameraTransform">
/// The world to camera transform (camera pose) to raycast from.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="pointCloudFrame"/> parameter is null. </exception>
/// <exception cref="InvalidOperationException">
/// Thrown when the call failed for an unknown reason.
/// </exception>
public void CalculatePointCloud(
FusionPointCloudImageFrame pointCloudFrame,
FusionColorImageFrame colorFrame,
Matrix4 worldToCameraTransform)
{
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
if (null == colorFrame)
{
throw new ArgumentNullException("colorFrame");
}
ExceptionHelper.ThrowIfFailed(volume.CalculatePointCloud(
FusionImageFrame.ToHandleRef(pointCloudFrame),
FusionImageFrame.ToHandleRef(colorFrame),
ref worldToCameraTransform));
}
/// <summary>
/// Create a visible color shaded image of a point cloud and its normals with simple
/// grayscale L.N surface shading. All image frames must have the same width and height.
/// </summary>
/// <param name="pointCloudFrame">The point cloud frame to be shaded.</param>
/// <param name="worldToCameraTransform">
/// The world to camera transform (camera pose) where the raycast was performed from.
/// Pass identity if the point cloud did not originate from a raycast and is in the
/// camera local coordinate system.
/// </param>
/// <param name="shadedSurfaceFrame">
/// Optionally, a pre-allocated color image frame, to be filled with the grayscale L.N
/// shaded surface image. Pass null to skip this image.
/// </param>
/// <param name="shadedSurfaceNormalsFrame">
/// Optionally, a pre-allocated color image frame, to be filled with the color shaded
/// normals image with color indicating orientation. Pass null to skip this image.
/// </param>
/// <exception cref="ArgumentNullException">
/// Thrown when the <paramref name="pointCloudFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="pointCloudFrame"/> or <paramref name="shadedSurfaceFrame"/>
/// or <paramref name="shadedSurfaceNormalsFrame"/> parameters are different image sizes.
/// Thrown when the <paramref name="pointCloudFrame"/> or <paramref name="shadedSurfaceFrame"/>
/// or <paramref name="shadedSurfaceNormalsFrame"/> parameters have different camera parameters.
/// </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 ShadePointCloud(
FusionPointCloudImageFrame pointCloudFrame,
Matrix4 worldToCameraTransform,
FusionColorImageFrame shadedSurfaceFrame,
FusionColorImageFrame shadedSurfaceNormalsFrame)
{
if (null == pointCloudFrame)
{
throw new ArgumentNullException("pointCloudFrame");
}
ExceptionHelper.ThrowIfFailed(NativeMethods.NuiFusionShadePointCloud2(
FusionImageFrame.ToHandleRef(pointCloudFrame),
ref worldToCameraTransform,
IntPtr.Zero,
FusionImageFrame.ToHandleRef(shadedSurfaceFrame),
FusionImageFrame.ToHandleRef(shadedSurfaceNormalsFrame)));
}
/// <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();
}
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;
}
}
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>
/// The AlignPointClouds function uses an on GPU iterative algorithm to align two sets of
/// overlapping oriented point clouds and calculate the camera's relative pose.
/// All images must be the same size and have the same camera parameters.
/// </summary>
/// <param name="referencePointCloudFrame">A reference point cloud frame.</param>
/// <param name="observedPointCloudFrame">An observerd point cloud frame.</param>
/// <param name="maxAlignIterationCount">The number of iterations to run.</param>
/// <param name="deltaFromReferenceFrame">
/// Optionally, a pre-allocated color image frame, to be filled with color-coded data
/// from the camera tracking. This may be used as input to additional vision algorithms such as
/// object segmentation. Values vary depending on whether the pixel was a valid pixel used in
/// tracking (inlier) or failed in different tests (outlier). 0xff000000 indicates an invalid
/// input vertex (e.g. from 0 input depth), or one where no correspondences occur between point
/// cloud images. Outlier vertices rejected due to too large a distance between vertices are
/// coded as 0xff008000. Outlier vertices rejected due to to large a difference in normal angle
/// between point clouds are coded as 0xff800000. Inliers are color shaded depending on the
/// residual energy at that point, with more saturated colors indicating more discrepancy
/// between vertices and less saturated colors (i.e. more white) representing less discrepancy,
/// or less information at that pixel. Pass null if this image is not required.
/// </param>
/// <param name="alignmentEnergy">A value describing
/// how well the observed frame aligns to the model with the calculated pose (mean distance between
/// matching points in the point clouds). 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.
/// Pass NULL to ignore this parameter.</param>
/// <param name="referenceToObservedTransform">The initial guess at the transform. This is
/// updated on tracking success, or returned as identity on failure.</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="referencePointCloudFrame"/> or
/// <paramref name="observedPointCloudFrame"/> parameter is null.
/// </exception>
/// <exception cref="ArgumentException">
/// Thrown when the <paramref name="referencePointCloudFrame"/> or
/// <paramref name="observedPointCloudFrame"/> or <paramref name="deltaFromReferenceFrame"/>
/// parameter is an incorrect image size, or the iterations parameter is not greater than 0.
/// </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 AlignPointClouds(
FusionPointCloudImageFrame referencePointCloudFrame,
FusionPointCloudImageFrame observedPointCloudFrame,
int maxAlignIterationCount,
FusionColorImageFrame deltaFromReferenceFrame,
out float alignmentEnergy,
ref Matrix4 referenceToObservedTransform)
{
if (null == referencePointCloudFrame)
{
throw new ArgumentNullException("referencePointCloudFrame");
}
if (null == observedPointCloudFrame)
{
throw new ArgumentNullException("observedPointCloudFrame");
}
ushort iterations = ExceptionHelper.CastAndThrowIfOutOfUshortRange(maxAlignIterationCount);
HRESULT hr = volume.AlignPointClouds(
FusionImageFrame.ToHandleRef(referencePointCloudFrame),
FusionImageFrame.ToHandleRef(observedPointCloudFrame),
iterations,
FusionImageFrame.ToHandleRef(deltaFromReferenceFrame),
out alignmentEnergy,
ref referenceToObservedTransform);
if (hr == HRESULT.E_NUI_FUSION_TRACKING_ERROR)
{
return false;
}
else
{
ExceptionHelper.ThrowIfFailed(hr);
}
return true;
}
/// <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);
}
}
