/// <summary>
/// Performs a ray/mesh intersection with the depth map
/// </summary>
/// <param name="kinectCalibration">Defines the calibration (extrinsics and intrinsics) for the Kinect</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">Should undistortion be applied to the point?</param>
/// <returns>Returns point of intersection</returns>
internal static Point3D?IntersectLineWithDepthMesh(IKinectCalibration kinectCalibration, 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(kinectCalibration, 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>
/// 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.Calibration.Do((kc) => this.calibration = kc.DeepClone());
pipeline.Run(TimeSpan.FromSeconds(10));
}
}
private static float GetMeshDepthAtPoint(IKinectCalibration kinectCalibration, Image depthImage, Point3D point, bool undistort)
{
Point2D depthSpacePoint = kinectCalibration.DepthIntrinsics.ToPixelSpace(point, undistort);
// depthSpacePoint = new Point2D(depthSpacePoint.X, kinectCalibration.DepthIntrinsics.ImageHeight - depthSpacePoint.Y);
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.Calibration.Do((kc) => this.calibration = 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.calibration != null)
{
var mesh = Test.Psi.Kinect.Mesh.MeshFromDepthMap(this.lastImage, this.lastColor, this.calibration);
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.Now.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.Run(TimeSpan.FromSeconds(10));
}
}
private void CoordinateMapper_CoordinateMappingChanged(object sender, CoordinateMappingChangedEventArgs e)
{
if (this.DepthFrameToCameraSpaceTable.HasSubscribers)
{
this.DepthFrameToCameraSpaceTable.Post(this.kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable(), this.pipeline.GetCurrentTime());
}
if (this.configuration.OutputCalibration)
{
if (!this.calibrationPosted)
{
// Extract and created old style calibration
var kinectInternalCalibration = new KinectInternalCalibration();
kinectInternalCalibration.RecoverCalibrationFromSensor(this.kinectSensor);
Matrix <double> colorCameraMatrix = Matrix <double> .Build.Dense(3, 3);
Matrix <double> depthCameraMatrix = Matrix <double> .Build.Dense(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
colorCameraMatrix[i, j] = kinectInternalCalibration.colorCameraMatrix[i, j];
depthCameraMatrix[i, j] = kinectInternalCalibration.depthCameraMatrix[i, j];
}
}
Vector <double> colorLensDistortion = Vector <double> .Build.Dense(5);
Vector <double> depthLensDistortion = Vector <double> .Build.Dense(5);
for (int i = 0; i < 5; i++)
{
colorLensDistortion[i] = kinectInternalCalibration.colorLensDistortion[i];
depthLensDistortion[i] = kinectInternalCalibration.depthLensDistortion[i];
}
Matrix <double> depthToColorTransform = Matrix <double> .Build.Dense(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
depthToColorTransform[i, j] = kinectInternalCalibration.depthToColorTransform[i, j];
}
}
// Extract and create new style calibration
this.kinectCalibration = new KinectCalibration(
this.kinectSensor.ColorFrameSource.FrameDescription.Width,
this.kinectSensor.ColorFrameSource.FrameDescription.Height,
colorCameraMatrix,
kinectInternalCalibration.colorLensDistortion[0],
kinectInternalCalibration.colorLensDistortion[1],
0.0,
0.0,
depthToColorTransform,
this.kinectSensor.DepthFrameSource.FrameDescription.Width,
this.kinectSensor.DepthFrameSource.FrameDescription.Height,
depthCameraMatrix,
kinectInternalCalibration.depthLensDistortion[0],
kinectInternalCalibration.depthLensDistortion[1],
0.0,
0.0);
/* Warning about comments being preceded by blank line */
#pragma warning disable SA1515
// KinectCalibrationOld's original ToColorSpace() method flips the Y axis (height-Y). To account
// for this we adjust our Transform.
// Matrix<double> flipY = Matrix<double>.Build.DenseIdentity(3, 3);
// flipY[1, 1] = -1.0;
// flipY[1, 2] = this.kinectSensor.ColorFrameSource.FrameDescription.Height;
// this.kinectCalibration.ColorIntrinsics.Transform = flipY * this.kinectCalibration.ColorIntrinsics.Transform;
#pragma warning restore SA1515
this.Calibration.Post(this.kinectCalibration, this.pipeline.GetCurrentTime());
this.calibrationPosted = true;
}
}
}
public static Mesh MeshFromDepthMap(Shared <Image> depthMap, Shared <Image> colorData, IKinectCalibration calib)
{
Mesh mesh = new Mesh();
int width = depthMap.Resource.Width;
int height = depthMap.Resource.Height;
mesh.Vertices = new Vertex[width * height];
bool[] vertexValid = new bool[width * height];
mesh.Faces = new Face[2 * (width - 1) * (height - 1)];
byte[] depthData = depthMap.Resource.ReadBytes(depthMap.Resource.Size);
byte[] pixelData = colorData.Resource.ReadBytes(colorData.Resource.Size);
int count = 0;
unsafe
{
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
ushort *src = (ushort *)((byte *)depthMap.Resource.ImageData.ToPointer() + (i * depthMap.Resource.Stride)) + j;
ushort depth = *src;
Point2D pt = new Point2D(j, i);
vertexValid[count] = (depth == 0) ? false : true;
mesh.Vertices[count].Pos = new Point3D(0.0, 0.0, 0.0);
mesh.Vertices[count].Color = new Point3D(0.0, 0.0, 0.0);
if (depth != 0)
{
DepthSpacePoint depthPt;
depthPt.X = j;
depthPt.Y = i;
Point2D pixelCoord;
// Determine vertex position+color via new calibration
Point2D newpt = new Point2D(pt.X, calib.DepthIntrinsics.ImageHeight - pt.Y);
Point3D p = calib.DepthIntrinsics.ToCameraSpace(newpt, depth, true);
mesh.Vertices[count].Pos = new Point3D(p.X / 1000.0, p.Y / 1000.0, p.Z / 1000.0);
Vector <double> pos = Vector <double> .Build.Dense(4);
pos[0] = mesh.Vertices[count].Pos.X;
pos[1] = mesh.Vertices[count].Pos.Y;
pos[2] = mesh.Vertices[count].Pos.Z;
pos[3] = 1.0;
pos = calib.ColorExtrinsics * pos;
Point3D clrPt = new Point3D(pos[0], pos[1], pos[2]);
pixelCoord = calib.ColorIntrinsics.ToPixelSpace(clrPt, true);
if (pixelCoord.X >= 0 && pixelCoord.X < colorData.Resource.Width &&
pixelCoord.Y >= 0 && pixelCoord.Y < colorData.Resource.Height)
{
byte *pixel = ((byte *)colorData.Resource.ImageData.ToPointer() + ((int)pixelCoord.Y * colorData.Resource.Stride)) + 4 * (int)pixelCoord.X;
mesh.Vertices[count].Color = new Point3D((double)(int)*(pixel + 2), (double)(int)*(pixel + 1), (double)(int)*pixel);
}
}
count++;
}
}
}
count = 0;
// Create our edge list
//
// There are 6 edges per quad along with the edges
// around the outside of the entire image (2*(width+height-2) edges)
// <------ <------
// X ------> X ------> X
// |^ 0 //|| 6 //||
// || // || // ||
// ||2 1// ||8 7// ||
// || // || // ||
// || //3 4|| //9 10||
// || // || // ||
// v| // 5 v| // 11 v|
// X <------ X <------ X
// ------> ------>
// |^ 12 //|| 18 //||
// || // || // ||
// ||14 13// ||20 19// ||
// || // 16|| // 22||
// || //15 || //21 ||
// || // || // ||
// v| // 17 v| // 23 v|
// X <------ X <------ X
// ------> ------>
int edgeOffset = (width - 1) * (height - 1) * 6;
int numEdges = edgeOffset + 2 * (width - 1) + 2 * (height - 1);
mesh.Edges = new HalfEdge[numEdges];
for (int i = 0; i < numEdges; i++)
{
mesh.Edges[i] = default(HalfEdge);
}
int faceIndex = 0;
int edgeIndex = 0;
// Create our edge list
for (int j = 0; j < height - 1; j++)
{
for (int i = 0; i < width - 1; i++)
{
mesh.Faces[faceIndex] = default(Face);
mesh.Faces[faceIndex].Valid =
vertexValid[j * width + i + 1] &&
vertexValid[(j + 1) * width + i] &&
vertexValid[j * width + i];
mesh.Edges[edgeIndex].Ccw = edgeIndex + 2;
mesh.Edges[edgeIndex].Cw = edgeIndex + 1;
mesh.Edges[edgeIndex].Face = faceIndex;
mesh.Edges[edgeIndex].Head = j * width + i + 1;
if (j == 0)
{
mesh.Edges[edgeIndex].Opp = edgeOffset + i;
}
else
{
mesh.Edges[edgeIndex].Opp = edgeIndex - width * 6 + 5;
}
mesh.Edges[edgeIndex + 1].Ccw = edgeIndex;
mesh.Edges[edgeIndex + 1].Cw = edgeIndex + 2;
mesh.Edges[edgeIndex + 1].Face = faceIndex;
mesh.Edges[edgeIndex + 1].Head = (j + 1) * width + i;
mesh.Edges[edgeIndex + 1].Opp = edgeIndex + 3;
mesh.Edges[edgeIndex + 2].Ccw = edgeIndex + 1;
mesh.Edges[edgeIndex + 2].Cw = edgeIndex;
mesh.Edges[edgeIndex + 2].Face = faceIndex;
mesh.Edges[edgeIndex + 2].Head = j * width + i;
if (i == 0)
{
mesh.Edges[edgeIndex].Opp = edgeOffset + (width - 1) + j;
}
else
{
mesh.Edges[edgeIndex].Opp = edgeIndex - 4;
}
mesh.Faces[faceIndex].Edge = edgeIndex;
edgeIndex += 3;
faceIndex++;
mesh.Faces[faceIndex] = default(Face);
mesh.Faces[faceIndex].Valid =
vertexValid[j * width + i + 1] &&
vertexValid[(j + 1) * width + i + 1] &&
vertexValid[(j + 1) * width + i];
mesh.Edges[edgeIndex].Ccw = edgeIndex + 2;
mesh.Edges[edgeIndex].Cw = edgeIndex + 1;
mesh.Edges[edgeIndex].Face = faceIndex;
mesh.Edges[edgeIndex].Head = j * width + i + 1;
mesh.Edges[edgeIndex].Opp = edgeIndex - 2;
mesh.Edges[edgeIndex + 1].Ccw = edgeIndex;
mesh.Edges[edgeIndex + 1].Cw = edgeIndex + 2;
mesh.Edges[edgeIndex + 1].Face = faceIndex;
mesh.Edges[edgeIndex + 1].Head = (j + 1) * width + i + 1;
if (i == width - 1)
{
mesh.Edges[edgeIndex].Opp = edgeOffset + (width - 1) + (height - 1) + j;
}
else
{
mesh.Edges[edgeIndex].Opp = edgeIndex + 4;
}
mesh.Edges[edgeIndex + 2].Ccw = edgeIndex + 1;
mesh.Edges[edgeIndex + 2].Cw = edgeIndex;
mesh.Edges[edgeIndex + 2].Face = faceIndex;
mesh.Edges[edgeIndex + 2].Head = (j + 1) * width + i;
if (j == height - 1)
{
mesh.Edges[edgeIndex].Opp = edgeOffset + (width - 1) + 2 * (height - 1) + i;
}
else
{
mesh.Edges[edgeIndex].Opp = edgeIndex + (width - 1) * 6 - 5;
}
mesh.Faces[faceIndex].Edge = edgeIndex;
edgeIndex += 3;
faceIndex++;
}
}
// Link up outer edges... first top edges
int prevEdge = edgeOffset + width;
int edge = edgeOffset;
for (int i = 0; i < width - 1; i++)
{
mesh.Edges[edge].Cw = prevEdge;
mesh.Edges[edge].Ccw = edge + 1;
mesh.Edges[edge].Opp = i * 6;
mesh.Edges[edge].Face = -1;
mesh.Edges[edge].Head = i;
prevEdge = edge;
edge++;
}
// next the left edges
prevEdge = edgeOffset;
for (int i = 0; i < height - 1; i++)
{
mesh.Edges[edge].Cw = edge + 1;
mesh.Edges[edge].Ccw = prevEdge;
mesh.Edges[edge].Opp = i * (width - 1) * 6 + 2;
mesh.Edges[edge].Face = -1;
mesh.Edges[edge].Head = width * (i + 1);
prevEdge = edge;
}
// next the right edges
prevEdge = edgeOffset + (width - 1);
for (int i = 0; i < height - 1; i++)
{
mesh.Edges[edge].Ccw = edge + 1;
mesh.Edges[edge].Cw = prevEdge;
mesh.Edges[edge].Opp = i * (width - 1) * 6 - 2;
mesh.Edges[edge].Face = -1;
mesh.Edges[edge].Head = i * width - 1;
prevEdge = edge;
}
// finally the bottom edges
prevEdge = edgeOffset + (width - 1) + (height - 1);
for (int i = 0; i < width - 1; i++)
{
mesh.Edges[edge].Cw = edge + 1;
mesh.Edges[edge].Ccw = prevEdge;
mesh.Edges[edge].Opp = (height - 2) * (width - 1) * 6 + i * 6 + 5;
mesh.Edges[edge].Face = -1;
mesh.Edges[edge].Head = (height - 1) * width + i;
prevEdge = edge;
}
return(mesh);
}
