/// <summary>
/// Computes the depth values of a given floor and ceiling in pixel space
/// </summary>
/// <param name="cameraPose">The camera pose</param>
/// <param name="floorThreshold">The floor threshold</param>
/// <param name="ceilingThreshold">The ceiling threshold</param>
/// <param name="invProjectionMatrix">The inverse projection matrix</param>
/// <param name="floorCeilingMinDepths">The computed min depths of floor and ceiling</param>
/// <param name="floorCeilingMaxDepths">The computed max depths of floor and ceiling</param>
/// <param name="width">Width of the image</param>
/// <param name="height">Height of the image</param>
/// <param name="depthRange">Depth range of the camera</param>
public static void ComputeCeilingAndFloorDepthsInPixelSpace(
Pose cameraPose,
float floorThreshold,
float ceilingThreshold,
Microsoft.Robotics.PhysicalModel.Matrix invProjectionMatrix,
ref short[] floorCeilingMinDepths,
ref short[] floorCeilingMaxDepths,
int width,
int height,
float depthRange)
{
floorCeilingMinDepths = new short[width * height];
floorCeilingMaxDepths = new short[width * height];
ComputeCeilingAndFloorDepthsInPixelSpaceInternal(
ref cameraPose,
floorThreshold,
ceilingThreshold,
ref invProjectionMatrix,
floorCeilingMinDepths,
floorCeilingMaxDepths,
width,
height,
depthRange);
}
/// <summary>
/// Computes projection matrix from width, height, fov and caches inverse
/// </summary>
private void UpdateProjectionMatrix()
{
if (this.Width != 0 && this.Height != 0 && this.HorizontalFieldOfView != 0)
{
this.ProjectionMatrix = MathUtilities.ComputeProjectionMatrix(
(float)this.HorizontalFieldOfView,
this.Width,
this.Height,
10);
this.InverseProjectionMatrix = MathUtilities.Invert(this.ProjectionMatrix);
}
}
/// <summary>
/// Converts a coordinate in screen space to a vector in view space in millimeters
/// Allows for working in the native resolution of the depth camera with less conversions
/// </summary>
/// <param name="x">X-coordinate with range [0, screenWidth-1]</param>
/// <param name="y">Y-coordinate with range [0, screenHeight-1]</param>
/// <param name="z">Z-coordinate with range [0, maxValidDepth] in mm</param>
/// <param name="invProjectionMatrix">Inverse projection matrix of the camera</param>
/// <param name="screenWidth">Width of the camera view plane in pixel space</param>
/// <param name="screenHeight">Height of the camera view plane in pixel space</param>
/// <returns>Viewspace vector with depth in millimeters</returns>
public static Vector3 ConvertPixelSpaceToViewSpaceInMillimeters(
int x,
int y,
int z,
Microsoft.Robotics.PhysicalModel.Matrix invProjectionMatrix,
int screenWidth,
int screenHeight)
{
var positionInMeters = ConvertPixelSpaceToViewSpace(x, y, z, invProjectionMatrix, screenWidth, screenHeight);
// multiply by 1000 to convert to millimeters
return(Vector3.Scale(positionInMeters, 1000.0f));
}
/// <summary>
/// Converts a vector in view space to a vector in screen space
/// </summary>
/// <param name="viewSpaceVector">View space vector</param>
/// <param name="projectionMatrix">Projection matrix</param>
/// <param name="screenWidth">Screen width</param>
/// <param name="screenHeight">Screen height</param>
/// <returns>Pixel space vector</returns>
public static Vector3 ConvertViewSpaceToPixelSpaceWithDepthInMillimeters(
Vector3 viewSpaceVector,
Microsoft.Robotics.PhysicalModel.Matrix projectionMatrix,
int screenWidth,
int screenHeight)
{
var screenSpacePoint = ConvertViewSpaceToPixelSpace(ref viewSpaceVector, ref projectionMatrix, screenWidth, screenHeight);
// we return 'z' in millimeters as that is the unit z is returned from the camera
return(new Vector3(
screenSpacePoint.X,
screenSpacePoint.Y,
viewSpaceVector.Z * 1000.0f));
}
/// <summary>
/// Converts a coordinate in screen space to a vector in view space
/// </summary>
/// <param name="x">X-coordinate with range [0, screenWidth-1]</param>
/// <param name="y">Y-coordinate with range [0, screenHeight-1]</param>
/// <param name="z">Z-coordinate with range [0, maxValidDepth] in mm</param>
/// <param name="invProjectionMatrix">Inverse projection matrix of the camera</param>
/// <param name="screenWidth">Width of the camera view plane in pixel space</param>
/// <param name="screenHeight">Height of the camera view plane in pixel space</param>
/// <returns>Viewspace vector</returns>
public static Vector3 ConvertPixelSpaceToViewSpace(
int x,
int y,
int z,
Microsoft.Robotics.PhysicalModel.Matrix invProjectionMatrix,
int screenWidth,
int screenHeight)
{
float matrixAx, matrixBx, matrixAy, matrixBy;
ComputeTranslationScaleTransform(screenWidth, screenHeight, out matrixAx, out matrixBx, out matrixAy, out matrixBy);
return(ConvertPixelSpaceToViewSpace(x, y, z, invProjectionMatrix, screenWidth, matrixAx, matrixBx, matrixAy, matrixBy));
}
/// <summary>
/// Converts a vector in view space to a vector in screen space
/// </summary>
/// <param name="viewSpaceVector">View space vector</param>
/// <param name="projectionMatrix">Projection matrix</param>
/// <param name="screenWidth">Screen width</param>
/// <param name="screenHeight">Screen height</param>
/// <returns>Pixel space vector</returns>
private static Vector3 ConvertViewSpaceToPixelSpace(
ref Vector3 viewSpaceVector,
ref Microsoft.Robotics.PhysicalModel.Matrix projectionMatrix,
int screenWidth,
int screenHeight)
{
Vector4 ptW = TransformVector(
new Vector4(viewSpaceVector.X, viewSpaceVector.Y, viewSpaceVector.Z, 1.0f),
projectionMatrix);
// project to screen space
var screenSpacePoint = new Vector3(ptW.X / ptW.W, ptW.Y / ptW.W, ptW.Z / ptW.W);
float w = (float)screenWidth, h = (float)screenHeight;
float matrixAx = w * 0.5f, matrixBx = (w * 0.5f) - 0.5f;
float matrixAy = -h * 0.5f, matrixBy = (h * 0.5f) - 0.5f;
screenSpacePoint.X = (matrixAx * screenSpacePoint.X) + matrixBx + 0.5f;
screenSpacePoint.Y = (matrixAy * screenSpacePoint.Y) + matrixBy + 0.5f;
return(screenSpacePoint);
}
/// <summary>
/// Inverts matrix
/// </summary>
/// <param name="matrix">Matrix to invert</param>
/// <returns>Inverted matrix</returns>
private static Microsoft.Robotics.PhysicalModel.Matrix InvertInternal(ref pm.Matrix matrix)
{
pm.Matrix matrix2;
float num5 = matrix.M11;
float num4 = matrix.M12;
float num3 = matrix.M13;
float num2 = matrix.M14;
float num9 = matrix.M21;
float num8 = matrix.M22;
float num7 = matrix.M23;
float num6 = matrix.M24;
float num17 = matrix.M31;
float num16 = matrix.M32;
float num15 = matrix.M33;
float num14 = matrix.M34;
float num13 = matrix.M41;
float num12 = matrix.M42;
float num11 = matrix.M43;
float num10 = matrix.M44;
float num23 = (num15 * num10) - (num14 * num11);
float num22 = (num16 * num10) - (num14 * num12);
float num21 = (num16 * num11) - (num15 * num12);
float num20 = (num17 * num10) - (num14 * num13);
float num19 = (num17 * num11) - (num15 * num13);
float num18 = (num17 * num12) - (num16 * num13);
float num39 = ((num8 * num23) - (num7 * num22)) + (num6 * num21);
float num38 = -(((num9 * num23) - (num7 * num20)) + (num6 * num19));
float num37 = ((num9 * num22) - (num8 * num20)) + (num6 * num18);
float num36 = -(((num9 * num21) - (num8 * num19)) + (num7 * num18));
float num = 1f / ((((num5 * num39) + (num4 * num38)) + (num3 * num37)) + (num2 * num36));
matrix2.M11 = num39 * num;
matrix2.M21 = num38 * num;
matrix2.M31 = num37 * num;
matrix2.M41 = num36 * num;
matrix2.M12 = -(((num4 * num23) - (num3 * num22)) + (num2 * num21)) * num;
matrix2.M22 = (((num5 * num23) - (num3 * num20)) + (num2 * num19)) * num;
matrix2.M32 = -(((num5 * num22) - (num4 * num20)) + (num2 * num18)) * num;
matrix2.M42 = (((num5 * num21) - (num4 * num19)) + (num3 * num18)) * num;
float num35 = (num7 * num10) - (num6 * num11);
float num34 = (num8 * num10) - (num6 * num12);
float num33 = (num8 * num11) - (num7 * num12);
float num32 = (num9 * num10) - (num6 * num13);
float num31 = (num9 * num11) - (num7 * num13);
float num30 = (num9 * num12) - (num8 * num13);
matrix2.M13 = (((num4 * num35) - (num3 * num34)) + (num2 * num33)) * num;
matrix2.M23 = -(((num5 * num35) - (num3 * num32)) + (num2 * num31)) * num;
matrix2.M33 = (((num5 * num34) - (num4 * num32)) + (num2 * num30)) * num;
matrix2.M43 = -(((num5 * num33) - (num4 * num31)) + (num3 * num30)) * num;
float num29 = (num7 * num14) - (num6 * num15);
float num28 = (num8 * num14) - (num6 * num16);
float num27 = (num8 * num15) - (num7 * num16);
float num26 = (num9 * num14) - (num6 * num17);
float num25 = (num9 * num15) - (num7 * num17);
float num24 = (num9 * num16) - (num8 * num17);
matrix2.M14 = -(((num4 * num29) - (num3 * num28)) + (num2 * num27)) * num;
matrix2.M24 = (((num5 * num29) - (num3 * num26)) + (num2 * num25)) * num;
matrix2.M34 = -(((num5 * num28) - (num4 * num26)) + (num2 * num24)) * num;
matrix2.M44 = (((num5 * num27) - (num4 * num25)) + (num3 * num24)) * num;
return(matrix2);
}
/// <summary>
/// Matrix inversion from reflector since I don't feel like rewriting myself at the moment
/// </summary>
/// <param name="matrix">Matrix to invert</param>
/// <returns>Inverted matrix</returns>
public static pm.Matrix Invert(pm.Matrix matrix)
{
return(InvertInternal(ref matrix));
}