/// <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); }