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