457 lines
19 KiB
C++
457 lines
19 KiB
C++
//------------------------------------------------------------------------------
|
|
// <copyright file="KinectFusionHelper.h" company="Microsoft">
|
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
|
// </copyright>
|
|
//------------------------------------------------------------------------------
|
|
|
|
#pragma once
|
|
|
|
#include "NuiKinectFusionApi.h"
|
|
#include <vector>
|
|
#include <string>
|
|
|
|
/// <summary>
|
|
/// Set Identity in a Matrix4
|
|
/// </summary>
|
|
/// <param name="mat">The matrix to set to identity</param>
|
|
void SetIdentityMatrix(Matrix4 &mat);
|
|
|
|
/// <summary>
|
|
/// Extract translation values from the 4x4 Matrix4 transformation in M41,M42,M43
|
|
/// </summary>
|
|
/// <param name="transform">The transform matrix.</param>
|
|
/// <param name="translation">Array of 3 floating point values for translation.</param>
|
|
void ExtractVector3Translation(const Matrix4 &transform, _Out_cap_c_(3) float *translation);
|
|
|
|
/// <summary>
|
|
/// Extract translation Vector3 from the 4x4 Matrix transformation in M41,M42,M43
|
|
/// </summary>
|
|
/// <param name="transform">The transform matrix.</param>
|
|
/// <returns>Returns a Vector3 containing the translation.</returns>
|
|
Vector3 ExtractVector3Translation(const Matrix4 &transform);
|
|
|
|
/// <summary>
|
|
/// Extract 3x3 rotation from the 4x4 Matrix and return in new Matrix4
|
|
/// </summary>
|
|
/// <param name="transform">The transform matrix.</param>
|
|
/// <returns>Returns a Matrix4 containing the rotation.</returns>
|
|
Matrix4 Extract3x3Rotation(const Matrix4 &transform);
|
|
|
|
/// <summary>
|
|
/// Extract 3x3 rotation matrix from the Matrix4 4x4 transformation:
|
|
/// Then convert to Euler angles.
|
|
/// </summary>
|
|
/// <param name="transform">The transform matrix.</param>
|
|
/// <param name="rotation">Array of 3 floating point values for euler angles.</param>
|
|
void ExtractRot2Euler(const Matrix4 &transform, _Out_cap_c_(3) float *rotation);
|
|
|
|
/// <summary>
|
|
/// Test whether the camera moved too far between sequential frames by looking at starting and end transformation matrix.
|
|
/// We assume that if the camera moves or rotates beyond a reasonable threshold, that we have lost track.
|
|
/// Note that on lower end machines, if the processing frame rate decreases below 30Hz, this limit will potentially have
|
|
/// to be increased as frames will be dropped and hence there will be a greater motion between successive frames.
|
|
/// </summary>
|
|
/// <param name="T_initial">The transform matrix from the previous frame.</param>
|
|
/// <param name="T_final">The transform matrix from the current frame.</param>
|
|
/// <param name="maxTrans">The maximum translation in meters we expect per x,y,z component between frames under normal motion.</param>
|
|
/// <param name="maxRotDegrees">The maximum rotation in degrees we expect about the x,y,z axes between frames under normal motion.</param>
|
|
/// <returns>true if camera transformation is greater than the threshold, otherwise false</returns>
|
|
bool CameraTransformFailed(const Matrix4 &T_initial, const Matrix4 &T_final, float maxTrans, float maxRotDegrees);
|
|
|
|
/// <summary>
|
|
/// Invert the 3x3 Rotation Matrix Component of a 4x4 matrix
|
|
/// </summary>
|
|
/// <param name="rot">The rotation matrix to invert.</param>
|
|
void InvertRotation(Matrix4 &rot);
|
|
|
|
/// <summary>
|
|
/// Negate the 3x3 Rotation Matrix Component of a 4x4 matrix
|
|
/// </summary>
|
|
/// <param name="rot">The rotation matrix to negate.</param>
|
|
void NegateRotation(Matrix4 &rot);
|
|
|
|
/// <summary>
|
|
/// Rotate a vector with the 3x3 Rotation Matrix Component of a 4x4 matrix
|
|
/// </summary>
|
|
/// <param name="vec">The Vector3 to rotate.</param>
|
|
/// <param name="rot">Rotation matrix.</param>
|
|
Vector3 RotateVector(const Vector3 &vec, const Matrix4 &rot);
|
|
|
|
/// <summary>
|
|
/// Invert Matrix4 Pose either from WorldToCameraTransform (view) matrix to CameraToWorldTransform pose matrix (world/SE3) or vice versa
|
|
/// </summary>
|
|
/// <param name="transform">The camera pose transform matrix.</param>
|
|
/// <returns>Returns a Matrix4 containing the inverted camera pose.</returns>
|
|
Matrix4 InvertMatrix4Pose(const Matrix4 &transform);
|
|
|
|
/// <summary>
|
|
/// Write Binary .STL mesh file
|
|
/// see http://en.wikipedia.org/wiki/STL_(file_format) for STL format
|
|
/// </summary>
|
|
/// <param name="mesh">The Kinect Fusion mesh object.</param>
|
|
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
|
|
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
|
|
/// <returns>indicates success or failure</returns>
|
|
HRESULT WriteBinarySTLMeshFile(INuiFusionColorMesh *mesh, LPOLESTR lpOleFileName, bool flipYZ = true);
|
|
|
|
/// <summary>
|
|
/// Write ASCII Wavefront .OBJ mesh file
|
|
/// See http://en.wikipedia.org/wiki/Wavefront_.obj_file for .OBJ format
|
|
/// </summary>
|
|
/// <param name="mesh">The Kinect Fusion mesh object.</param>
|
|
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
|
|
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
|
|
/// <returns>indicates success or failure</returns>
|
|
HRESULT WriteAsciiObjMeshFile(INuiFusionColorMesh *mesh, LPOLESTR lpOleFileName, bool flipYZ = true);
|
|
|
|
/// <summary>
|
|
/// Write ASCII .PLY file
|
|
/// See http://paulbourke.net/dataformats/ply/ for .PLY format
|
|
/// </summary>
|
|
/// <param name="mesh">The Kinect Fusion mesh object.</param>
|
|
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
|
|
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
|
|
/// <param name="outputColor">Set this true to write out the surface color to the file when it has been captured.</param>
|
|
/// <returns>indicates success or failure</returns>
|
|
HRESULT WriteAsciiPlyMeshFile(INuiFusionColorMesh *mesh, LPOLESTR lpOleFileName, bool flipYZ = true, bool outputColor = false);
|
|
|
|
/// <summary>
|
|
/// Write ASCII Wavefront .OBJ file with bitmap texture and material file
|
|
/// See http://en.wikipedia.org/wiki/Wavefront_.obj_file for .OBJ format
|
|
/// </summary>
|
|
/// <param name="mesh">The Kinect Fusion mesh object.</param>
|
|
/// <param name="lpOleFileName">The full path and filename of the file to save.</param>
|
|
/// <param name="flipYZ">Flag to determine whether the Y and Z values are flipped on save.</param>
|
|
/// <param name="pTexture">The Kinect Fusion color texture image.</param>
|
|
/// <param name="texcoords">Three Vector3 texture coordinates per mesh triangle, normalized by the image size.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT WriteTexturedeAsciiObjMeshFile(INuiFusionColorMesh *mesh, LPOLESTR lpOleFileName, bool flipYZ, NUI_FUSION_IMAGE_FRAME *pTexture, const std::vector<Vector3> &texcoords);
|
|
|
|
/// <summary>
|
|
/// Returns whether this is running as a 32 or 64bit application.
|
|
/// </summary>
|
|
/// <returns>TRUE indicates a 64bit app.</returns>
|
|
BOOL Is64BitApp();
|
|
|
|
/// <summary>
|
|
/// Write 32bit BMP image file
|
|
/// </summary>
|
|
/// <param name="pszFile">The full path and filename of the file to save.</param>
|
|
/// <param name="imageBytes">A pointer to the image bytes to save.</param>
|
|
/// <param name="width">The width of the image to save.</param>
|
|
/// <param name="height">The width of the image to save.</param>
|
|
/// <returns>indicates success or failure</returns>
|
|
HRESULT SaveBMPFile(LPCWSTR pszFile, const byte *pImageBytes, unsigned int width, unsigned int height);
|
|
|
|
/// <summary>
|
|
/// Copy an image with identical sizes and parameters.
|
|
/// </summary>
|
|
/// <param name="pSrc">A pointer to the source image.</param>
|
|
/// <param name="pDest">A pointer to the destination image.</param>
|
|
/// <returns>indicates success or failure</returns>
|
|
HRESULT CopyImageFrame(const NUI_FUSION_IMAGE_FRAME *pSrc, const NUI_FUSION_IMAGE_FRAME *pDest);
|
|
|
|
/// <summary>
|
|
/// Horizontally mirror a 32bit (color/float) image in-place.
|
|
/// </summary>
|
|
/// <param name="pImage">A pointer to the image to mirror.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT HorizontalMirror32bitImageInPlace(const NUI_FUSION_IMAGE_FRAME *pImage);
|
|
|
|
/// <summary>
|
|
/// Horizontally mirror a 32bit (color/float) image.
|
|
/// </summary>
|
|
/// <param name="pSrcImage">A pointer to the image to mirror.</param>
|
|
/// <param name="pDestImage">A pointer to the destination mirrored image.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT HorizontalMirror32bitImage(const NUI_FUSION_IMAGE_FRAME *pSrcImage, const NUI_FUSION_IMAGE_FRAME *pDestImage);
|
|
|
|
/// <summary>
|
|
/// Color the residual/delta image from the AlignDepthFloatToReconstruction call
|
|
/// </summary>
|
|
/// <param name="pFloatDeltaFromReference">A pointer to the source pFloatDeltaFromReference image.</param>
|
|
/// <param name="pShadedDeltaFromReference">A pointer to the destination ShadedDeltaFromReference image.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT ColorResiduals(const NUI_FUSION_IMAGE_FRAME *pFloatDeltaFromReference, const NUI_FUSION_IMAGE_FRAME *pShadedDeltaFromReference);
|
|
|
|
/// <summary>
|
|
/// Statistics calculated for a FloatDeltaFromReference Image after the
|
|
/// AlignDepthFloatToReconstruction and CalculateResidualStatistics calls.
|
|
/// </summary>
|
|
struct DeltaFromReferenceImageStatistics
|
|
{
|
|
unsigned int totalPixels;
|
|
unsigned int zeroPixels;
|
|
unsigned int validPixels;
|
|
unsigned int invalidDepthOutsideVolumePixels;
|
|
float totalValidPixelsDistance;
|
|
};
|
|
|
|
/// <summary>
|
|
/// Calculate statistics on the residual/delta image from the AlignDepthFloatToReconstruction call.
|
|
/// </summary>
|
|
/// <param name="pFloatDeltaFromReference">A pointer to the source FloatDeltaFromReference image.</param>
|
|
/// <param name="stats">A pointer to a DeltaFromReferenceImageStatistics struct to fill with the statistics.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT CalculateResidualStatistics(const NUI_FUSION_IMAGE_FRAME *pFloatDeltaFromReference, DeltaFromReferenceImageStatistics *stats);
|
|
|
|
/// <summary>
|
|
/// Down sample color, depth float or point cloud frame with nearest neighbor
|
|
/// </summary>
|
|
/// <param name="src">The source color, depth float or pointcloud image.</param>
|
|
/// <param name="dest">The destination down sampled color, depth float or pointcloud image.</param>
|
|
/// <param name="factor">The down sample factor (1=just copy, 2=x/2,y/2, 4=x/4,y/4).</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT DownsampleFrameNearestNeighbor(NUI_FUSION_IMAGE_FRAME *src, NUI_FUSION_IMAGE_FRAME *dest, unsigned int factor);
|
|
|
|
/// <summary>
|
|
/// Up sample color or depth float (32 bits/pixel) frame with nearest neighbor - replicates pixels
|
|
/// </summary>
|
|
/// <param name="src">The source color image.</param>
|
|
/// <param name="dest">The destination up-sampled color image.</param>
|
|
/// <param name="factor">The up sample factor (1=just copy, 2=x*2,y*2, 4=x*4,y*4).</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT UpsampleFrameNearestNeighbor(NUI_FUSION_IMAGE_FRAME *src, NUI_FUSION_IMAGE_FRAME *dest, unsigned int factor);
|
|
|
|
/// <summary>
|
|
/// Down sample color frame with nearest neighbor to the depth frame resolution
|
|
/// </summary>
|
|
/// <param name="src">The source color image.</param>
|
|
/// <param name="dest">The destination down sampled image.</param>
|
|
/// <returns>S_OK on success, otherwise failure code</returns>
|
|
HRESULT DownsampleColorFrameToDepthResolution(NUI_FUSION_IMAGE_FRAME *src, NUI_FUSION_IMAGE_FRAME *dest);
|
|
|
|
/// <summary>
|
|
/// Convert int to string
|
|
/// </summary>
|
|
/// <param name="theValue">The int value to convert.</param>
|
|
/// <returns>Returns a string containing the int value.</returns>
|
|
inline std::string to_string(int theValue)
|
|
{
|
|
char buffer[65];
|
|
|
|
errno_t err = _itoa_s(theValue, buffer, ARRAYSIZE(buffer), 10);
|
|
|
|
if (0 != err)
|
|
{
|
|
return std::string("");
|
|
}
|
|
|
|
return std::string(buffer);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert float to string
|
|
/// </summary>
|
|
/// <param name="theValue">The float value to convert.</param>
|
|
/// <returns>Returns a string containing the float value.</returns>
|
|
inline std::string to_string(float theValue)
|
|
{
|
|
char buffer[_CVTBUFSIZE];
|
|
|
|
errno_t err = _gcvt_s(buffer, _CVTBUFSIZE, theValue, 6);
|
|
|
|
if (0 != err)
|
|
{
|
|
return std::string("");
|
|
}
|
|
|
|
return std::string(buffer);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clamp a value if outside two given thresholds
|
|
/// </summary>
|
|
/// <param name="x">The value to clamp.</param>
|
|
/// <param name="a">The minimum inclusive threshold.</param>
|
|
/// <param name="b">The maximum inclusive threshold.</param>
|
|
/// <returns>Returns the clamped value.</returns>
|
|
template <typename T>
|
|
inline T clamp(const T& x, const T& a, const T& b)
|
|
{
|
|
if (x < a)
|
|
return a;
|
|
else if (x > b)
|
|
return b;
|
|
else
|
|
return x;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Load an 24bit RGB color from a packed int pixel image and return as float values
|
|
/// </summary>
|
|
/// <param name="colorImage">The int image array.</param>
|
|
/// <param name="x">The x coordinate of the pixel to return.</param>
|
|
/// <param name="y">The y coordinate of the pixel to return.</param>
|
|
/// <param name="imageWidth">The width of the image in pixels.</param>
|
|
/// <returns>Returns a Vector3 containing the rgb color components.</returns>
|
|
inline Vector3 load_color(const unsigned int *colorImage, int x, int y, int imageWidth)
|
|
{
|
|
Vector3 rgb;
|
|
|
|
unsigned int packedValue = colorImage[(y * imageWidth) + x];
|
|
|
|
rgb.x = static_cast<float>(packedValue & 255); // r
|
|
rgb.y = static_cast<float>((packedValue >> 8) & 255); // g
|
|
rgb.z = static_cast<float>((packedValue >> 16) & 255); // b
|
|
|
|
return rgb;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Linearly interpolate (Lerp) between two values.
|
|
/// </summary>
|
|
/// <param name="z">The first value.</param>
|
|
/// <param name="b">The second value.</param>
|
|
/// <param name="f">The amount to interpolate between the values.</param>
|
|
/// <returns>Returns the interpolated value.</returns>
|
|
template <typename T, typename Tf>
|
|
inline auto lerp(const T& a, const T& b, Tf f) -> decltype(a + f * (b - a))
|
|
{
|
|
return a + f * (b - a);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Linearly interpolate (Lerp) between two Vector3-based RGB color pixels.
|
|
/// </summary>
|
|
/// <param name="z">The first color pixel.</param>
|
|
/// <param name="b">The second color pixel.</param>
|
|
/// <param name="f">The amount to interpolate between the values.</param>
|
|
/// <returns>Returns the interpolated value.</returns>
|
|
inline Vector3 lerp_color(const Vector3 &a, const Vector3 &b, float f)
|
|
{
|
|
Vector3 rgb;
|
|
|
|
rgb.x = lerp(a.x, b.x, f); // r
|
|
rgb.y = lerp(a.y, b.y, f); // g
|
|
rgb.z = lerp(a.z, b.z, f); // b
|
|
|
|
return rgb;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Bilinear sample an RGB int image
|
|
/// </summary>
|
|
/// <param name="colorImage">The int image array.</param>
|
|
/// <param name="x">The x coordinate of the pixel to return.</param>
|
|
/// <param name="y">The y coordinate of the pixel to return.</param>
|
|
/// <param name="imageWidth">The width of the image in pixels.</param>
|
|
/// <param name="imageHeight">The height of the image in pixels.</param>
|
|
/// <returns>Returns a packed int containing the rgb color components.</returns>
|
|
inline unsigned int bilinear_sample(const unsigned int *colorImage, float x, float y, int imageWidth, int imageHeight)
|
|
{
|
|
const float half = 0.5f;
|
|
|
|
const unsigned int xi = static_cast<unsigned int>(x - half);
|
|
const unsigned int yi = static_cast<unsigned int>(y - half);
|
|
|
|
const float xf = x - half - static_cast<float>(xi);
|
|
const float yf = y - half - static_cast<float>(yi);
|
|
|
|
const unsigned int posax = clamp<unsigned int>(xi, 0, imageWidth - 1);
|
|
const unsigned int posay = clamp<unsigned int>(yi, 0, imageHeight - 1);
|
|
|
|
const unsigned int posbx = clamp<unsigned int>(xi+1, 0, imageWidth - 1);
|
|
const unsigned int posby = clamp<unsigned int>(yi+1, 0, imageHeight - 1);
|
|
|
|
// Load the corners
|
|
Vector3 d00 = load_color(colorImage, posax, posay, imageWidth);
|
|
Vector3 d01 = load_color(colorImage, posax, posby, imageWidth);
|
|
Vector3 d10 = load_color(colorImage, posbx, posay, imageWidth);
|
|
Vector3 d11 = load_color(colorImage, posbx, posby, imageWidth);
|
|
|
|
// Interpolate over x
|
|
auto dx0 = lerp_color(d00, d10, xf);
|
|
auto dx1 = lerp_color(d01, d11, xf);
|
|
|
|
// Interpolate over y
|
|
auto dxy = lerp_color(dx0, dx1, yf);
|
|
|
|
return (255 << 24 | static_cast<unsigned int>(dxy.z) << 16 | static_cast<unsigned int>(dxy.y) << 8 | static_cast<unsigned int>(dxy.x) ); // always full alpha
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the squared difference between two Vector3 vertices
|
|
/// </summary>
|
|
/// <param name="v1">The first vertex.</param>
|
|
/// <param name="v2">The second vertex.</param>
|
|
/// <returns>Returns the squared difference.</returns>
|
|
inline float squared_difference(const Vector3 &v1, const Vector3 &v2)
|
|
{
|
|
float dx = v1.x-v2.x;
|
|
float dy = v1.y-v2.y;
|
|
float dz = v1.z-v2.z;
|
|
return (dx*dx) + (dy*dy) + (dz*dz);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the distance between two Vector3 vertices
|
|
/// </summary>
|
|
/// <param name="v1">The first vertex.</param>
|
|
/// <param name="v2">The second vertex.</param>
|
|
/// <returns>Returns the distance.</returns>
|
|
inline float distance(const Vector3 &v1, const Vector3 &v2)
|
|
{
|
|
return sqrtf(squared_difference(v1, v2));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the normalized dot product between two vectors.
|
|
/// Must be normalized input Vector3s.
|
|
/// Output: 1 if parallel, same dir, 0 if 90 degrees, -1 if parallel, looking opposite direction
|
|
/// </summary>
|
|
/// <param name="v1">The first vector.</param>
|
|
/// <param name="v2">The second vector.</param>
|
|
/// <returns>Returns the dot product.</returns>
|
|
inline float dot_normalized(const Vector3 &v1, const Vector3 &v2)
|
|
{
|
|
return (v1.x*v2.x) + (v1.y*v2.y) + (v1.z*v2.z);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform a vertex in the world coordinate system by the worldToCamera pose,
|
|
/// into the camera coordinate system.
|
|
/// </summary>
|
|
/// <param name="v1">The vertex to transform.</param>
|
|
/// <param name="worldToCamera">The worldToCamera pose.</param>
|
|
/// <returns>Returns the transformed vertex.</returns>
|
|
inline Vector3 transform(const Vector3 &v1, const Matrix4 &worldToCamera)
|
|
{
|
|
// Transform the point from the global frame into the local camera frame.
|
|
Vector3 R;
|
|
|
|
R.x = worldToCamera.M41 + (worldToCamera.M11 * v1.x) + (worldToCamera.M21 * v1.y) + (worldToCamera.M31 * v1.z);
|
|
R.y = worldToCamera.M42 + (worldToCamera.M12 * v1.x) + (worldToCamera.M22 * v1.y) + (worldToCamera.M32 * v1.z);
|
|
R.z = worldToCamera.M43 + (worldToCamera.M13 * v1.x) + (worldToCamera.M23 * v1.y) + (worldToCamera.M33 * v1.z);
|
|
|
|
return R;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Project a 3D vertex in the world coordinate system into a 2D camera image,
|
|
/// given its known intrinsic parameters and camera pose.
|
|
/// </summary>
|
|
/// <param name="v1">The vertex to transform.</param>
|
|
/// <param name="flx">The focal length in the x axis, in pixels.</param>
|
|
/// <param name="fly">The focal length in the y axis, in pixels.</param>
|
|
/// <param name="ppx">The principal point of the image in the x axis, in pixels.</param>
|
|
/// <param name="ppy">The principal point of the image in the y axis, in pixels.</param>
|
|
/// <param name="worldToCamera">The worldToCamera pose.</param>
|
|
/// <returns>Returns the 3D vertex transformed into a pixel in the 2D camera image.</returns>
|
|
inline Vector3 fast_project(const Vector3 &v1, float flx, float fly, float ppx, float ppy, const Matrix4 &worldToCamera)
|
|
{
|
|
// Transform from world to camera coordinate system
|
|
Vector3 R = transform(v1, worldToCamera);
|
|
|
|
Vector3 uv;
|
|
uv.x = R.x / R.z;
|
|
uv.y = R.y / R.z;
|
|
|
|
// Project from camera plane in world to image
|
|
uv.x = ppx + flx * uv.x;
|
|
uv.y = ppy + fly * uv.y;
|
|
|
|
uv.z = R.z;
|
|
return uv;
|
|
}
|