/// <summary>
/// Performs a ray/mesh intersection with the depth map.
/// </summary>
/// <param name="calibration">Defines the calibration (extrinsics and intrinsics) for the depth camera.</param>
/// <param name="line">Ray to intersect against depth map.</param>
/// <param name="depthImage">Depth map to ray cast against.</param>
/// <param name="skipFactor">Distance to march on each step along ray.</param>
/// <param name="undistort">Whether undistortion should be applied to the point.</param>
/// <returns>Returns point of intersection.</returns>
internal static Point3D?IntersectLineWithDepthMesh(IDepthDeviceCalibrationInfo calibration, Line3D line, Image depthImage, double skipFactor, bool undistort = true)
{
// max distance to check for intersection with the scene
double totalDistance = 5;
var delta = skipFactor * (line.EndPoint - line.StartPoint).Normalize();
// size of increment along the ray
int maxSteps = (int)(totalDistance / delta.Length);
var hypothesisPoint = line.StartPoint;
for (int i = 0; i < maxSteps; i++)
{
hypothesisPoint += delta;
// get the mesh distance at the extended point
float meshDistance = DepthExtensions.GetMeshDepthAtPoint(calibration, depthImage, hypothesisPoint, undistort);
// if the mesh distance is less than the distance to the point we've hit the mesh
if (!float.IsNaN(meshDistance) && (meshDistance < hypothesisPoint.Z))
{
return(hypothesisPoint);
}
}
return(null);
}
/// <summary>
/// Method for projecting a point in pixel coordinate from the color camera into the depth camera's coordinates by determining the corresponding depth pixel.
/// </summary>
/// <param name="depthDeviceCalibrationInfo">Defines the calibration information (extrinsics and intrinsics) for the depth device.</param>
/// <param name="point2D">Pixel coordinates in the color camera.</param>
/// <param name="depthImage">Depth map.</param>
/// <returns>Point in camera coordinates.</returns>
public static Point3D?ProjectToCameraSpace(IDepthDeviceCalibrationInfo depthDeviceCalibrationInfo, Point2D point2D, Shared <DepthImage> depthImage)
{
var colorExtrinsicsInverse = depthDeviceCalibrationInfo.ColorPose;
var pointInCameraSpace = depthDeviceCalibrationInfo.ColorIntrinsics.ToCameraSpace(point2D, 1.0, true);
double x = pointInCameraSpace.X * colorExtrinsicsInverse[0, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[0, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[0, 2] + colorExtrinsicsInverse[0, 3];
double y = pointInCameraSpace.X * colorExtrinsicsInverse[1, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[1, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[1, 2] + colorExtrinsicsInverse[1, 3];
double z = pointInCameraSpace.X * colorExtrinsicsInverse[2, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[2, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[2, 2] + colorExtrinsicsInverse[2, 3];
Point3D pointInWorldSpace = new Point3D(x, y, z);
Point3D cameraOriginInWorldSpace = new Point3D(colorExtrinsicsInverse[0, 3], colorExtrinsicsInverse[1, 3], colorExtrinsicsInverse[2, 3]);
Line3D rgbLine = new Line3D(cameraOriginInWorldSpace, pointInWorldSpace);
return(IntersectLineWithDepthMesh(depthDeviceCalibrationInfo, rgbLine, depthImage.Resource, 0.05));
}
/// <summary>
/// Method for projecting a point in pixel coordinate from the color camera into the depth camera's coordinates by determining the corresponding depth pixel.
/// </summary>
/// <param name="depthDeviceCalibrationInfo">Defines the calibration information (extrinsics and intrinsics) for the depth device.</param>
/// <param name="point2D">Pixel coordinates in the color camera.</param>
/// <param name="depthImage">Depth map.</param>
/// <returns>Point in 3D depth camera coordinates, assuming MathNet basis (Forward=X, Left=Y, Up=Z).</returns>
public static Point3D?ProjectToCameraSpace(IDepthDeviceCalibrationInfo depthDeviceCalibrationInfo, Point2D point2D, Shared <DepthImage> depthImage)
{
var colorExtrinsicsInverse = depthDeviceCalibrationInfo.ColorPose;
var pointInCameraSpace = depthDeviceCalibrationInfo.ColorIntrinsics.GetCameraSpacePosition(point2D, 1.0, depthImage.Resource.DepthValueSemantics, true);
double x = pointInCameraSpace.X * colorExtrinsicsInverse[0, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[0, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[0, 2] + colorExtrinsicsInverse[0, 3];
double y = pointInCameraSpace.X * colorExtrinsicsInverse[1, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[1, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[1, 2] + colorExtrinsicsInverse[1, 3];
double z = pointInCameraSpace.X * colorExtrinsicsInverse[2, 0] + pointInCameraSpace.Y * colorExtrinsicsInverse[2, 1] + pointInCameraSpace.Z * colorExtrinsicsInverse[2, 2] + colorExtrinsicsInverse[2, 3];
var pointInDepthCameraSpace = new Point3D(x, y, z);
var colorCameraOriginInDepthCameraSpace = new Point3D(colorExtrinsicsInverse[0, 3], colorExtrinsicsInverse[1, 3], colorExtrinsicsInverse[2, 3]);
var searchLine = new Line3D(colorCameraOriginInDepthCameraSpace, pointInDepthCameraSpace);
return(IntersectLineWithDepthMesh(depthDeviceCalibrationInfo.DepthIntrinsics, searchLine, depthImage.Resource));
}
/// <summary>
/// Setup Kinect.
/// </summary>
public void SetupKinect()
{
using (var pipeline = Pipeline.Create())
{
this.sensor = new KinectSensor(pipeline, new KinectSensorConfiguration()
{
OutputCalibration = true, OutputBodies = true, OutputColor = true
});
var calibration = this.sensor.DepthDeviceCalibrationInfo.Do((kc) => this.depthDeviceCalibrationInfo = kc.DeepClone());
pipeline.RunAsync();
pipeline.WaitAll(TimeSpan.FromSeconds(10));
}
}
private static float GetMeshDepthAtPoint(IDepthDeviceCalibrationInfo calibration, Image depthImage, Point3D point, bool undistort)
{
Point2D depthSpacePoint = calibration.DepthIntrinsics.ToPixelSpace(point, undistort);
int x = (int)Math.Round(depthSpacePoint.X);
int y = (int)Math.Round(depthSpacePoint.Y);
if ((x < 0) || (x >= depthImage.Width) || (y < 0) || (y >= depthImage.Height))
{
return(float.NaN);
}
int byteOffset = (int)((y * depthImage.Stride) + (x * 2));
var depth = BitConverter.ToUInt16(depthImage.ReadBytes(2, byteOffset), 0);
if (depth == 0)
{
return(float.NaN);
}
return((float)depth / 1000);
}
public void GenerateTexturedMappedMesh()
{
// Setup Kinect
using (var pipeline = Pipeline.Create())
{
this.sensor = new KinectSensor(pipeline, new KinectSensorConfiguration()
{
OutputCalibration = true, OutputBodies = true, OutputColor = true, OutputDepth = true
});
var calibration = this.sensor.DepthDeviceCalibrationInfo.Do((kc) => this.depthDeviceCalibrationInfo = kc.DeepClone());
this.sensor.ColorImage.Do((image) =>
{
if (this.lastColor == null)
{
this.lastColor = image.AddRef();
}
});
var c = this.sensor.DepthImage.Do((image) =>
{
if (this.lastImage == null)
{
this.lastImage = image.AddRef();
}
if (this.lastImage != null && this.lastColor != null && this.depthDeviceCalibrationInfo != null)
{
var mesh = Test.Psi.Kinect.Mesh.MeshFromDepthMap(this.lastImage, this.lastColor, this.depthDeviceCalibrationInfo);
int faceCount = 0;
foreach (var face in mesh.Faces)
{
if (face.Valid)
{
faceCount++;
}
}
bool writePLY = false;
if (writePLY)
{
string temppath = System.IO.Path.GetTempPath();
string fn = temppath + @"\Mesh-New-" + DateTime.UtcNow.ToString("MM-dd-yy.HH.mm.ss") + ".ply";
using (System.IO.StreamWriter file = new System.IO.StreamWriter(fn))
{
file.WriteLine("ply");
file.WriteLine("format ascii 1.0");
file.WriteLine("element vertex " + mesh.NumberVertices.ToString());
file.WriteLine("property float x");
file.WriteLine("property float y");
file.WriteLine("property float z");
file.WriteLine("property uchar red");
file.WriteLine("property uchar green");
file.WriteLine("property uchar blue");
file.WriteLine("element face " + faceCount.ToString());
file.WriteLine("property list uchar int vertex_indices");
file.WriteLine("end_header");
for (int i = 0; i < mesh.NumberVertices; i++)
{
file.WriteLine(
string.Format(
"{0:f2} {1:f2} {2:f2} {3:d} {4:d} {5:d}",
mesh.Vertices[i].Pos.X,
mesh.Vertices[i].Pos.Y,
mesh.Vertices[i].Pos.Z,
(int)mesh.Vertices[i].Color.X,
(int)mesh.Vertices[i].Color.Y,
(int)mesh.Vertices[i].Color.Z));
}
for (int i = 0; i < mesh.NumberFaces; i++)
{
if (mesh.Faces[i].Valid)
{
file.Write("3 ");
int edgeIndex = mesh.Faces[i].Edge;
file.Write(mesh.Edges[edgeIndex].Head.ToString() + " ");
edgeIndex = mesh.Edges[edgeIndex].Cw;
file.Write(mesh.Edges[edgeIndex].Head.ToString() + " ");
edgeIndex = mesh.Edges[edgeIndex].Cw;
file.WriteLine(mesh.Edges[edgeIndex].Head.ToString());
}
}
}
}
}
});
pipeline.RunAsync();
pipeline.WaitAll(TimeSpan.FromSeconds(10));
}
}
