private IDictionary <TrackingMarker, Point3D> _ConvertColourPointsToWorldPoints( IDictionary <TrackingMarker, Point2D> trackingPoints2D, DepthImageFormat kdFormat, short[] dShorts, ColorImageFormat kcFormat) { var dWidth = FormatConvertor.PixelWidth(kdFormat); var dSize = FormatConvertor.PixelDataLength(kdFormat); var dips = new DepthImagePoint[dSize]; var cips = new ColorImagePoint[dSize]; var trackingPoints3D = new Dictionary <TrackingMarker, Point3D>(); for (int i = 0; i < dSize; i++) { var depth = (short)(dShorts[i] >> 3); if (depth < 0) { continue; } dips[i] = (new DepthImagePoint() { Depth = depth, X = i % dWidth, Y = i / dWidth }); cips[i] = (this.mapper.MapDepthPointToColorPoint(kdFormat, dips[i], kcFormat)); } int dIndex = 0; for (; dIndex < cips.Length; dIndex++) { foreach (var qPair in trackingPoints2D) { var q = qPair.Value; if (cips[dIndex].X == q.X && cips[dIndex].Y == q.Y) { var sk = this.mapper.MapDepthPointToSkeletonPoint(kdFormat, dips[dIndex]); var p = new Point3D(sk.X, sk.Y, sk.Z); trackingPoints3D.Add(qPair.Key, p); } } if (trackingPoints3D.Count == trackingPoints2D.Count) { break; } } return(trackingPoints3D); }
public ICollection <Point3D> GeneratePointCloud(DepthFormat dFormat, ICollection <short> depthShorts, ColourFormat cFormat, ICollection <byte> colourPixels) { var kdFormat = FormatConvertor.ConvertToKinect(dFormat); var kcFormat = FormatConvertor.ConvertToKinect(cFormat); var dWidth = FormatConvertor.PixelWidth(kdFormat); var cWidth = FormatConvertor.PixelWidth(kcFormat); var points = new List <Point3D>(); var d = depthShorts.ToArray(); var c = colourPixels.ToArray(); for (int i = 0; i < d.Length; i++) { var depth = (short)(d[i] >> 3); if (depth < 0) { continue; } var dip = new DepthImagePoint() { Depth = depth, X = i % dWidth, Y = i / dWidth }; var skel = this.mapper.MapDepthPointToSkeletonPoint(kdFormat, dip); var cip = this.mapper.MapDepthPointToColorPoint(kdFormat, dip, kcFormat); var cIndex = 4 * (cip.X + cip.Y * cWidth); if (cIndex > c.Length || cIndex < 0) { continue; } var r = c[cIndex + 2]; var g = c[cIndex + 1]; var b = c[cIndex]; points.Add(new Point3D(skel.X, skel.Y, skel.Z, r, g, b)); } return(points); }
/// <summary> /// Perform calibration of the scene using user input and saves parameters to a file. /// </summary> /// <param name="path">Calibration parameters file to save.</param> /// <param name="markerSearchSpace">Space to search for tracking cubes within the colour image.</param> /// <param name="markers">Collection of active tracking cubes.</param> private Calibration PerformCalibration(AreaInt markerSearchSpace, ICollection <TrackingMarker> markers) { var calibration = new Calibration(); // set search space calibration.MarkerSearchSpace = markerSearchSpace; // Capture 1 frame DepthImagePixel[] depthCalFrame = new DepthImagePixel[FormatConvertor.PixelDataLength(this.desiredDepthFormat)]; using (var f = this.sensor.DepthStream.OpenNextFrame(100)) f.CopyDepthImagePixelDataTo(depthCalFrame); // Convert colour frame to depth frame DepthImagePoint[] dips = new DepthImagePoint[FormatConvertor.PixelDataLength(this.desiredDepthFormat)]; this.sensor.CoordinateMapper.MapColorFrameToDepthFrame(this.desiredColourFormat, this.desiredDepthFormat, depthCalFrame, dips); // Convert colour box centre, L & R to depth point // Get points at edge of 2D search area (for use in 3D TT search clipping) var cPixelWidth = FormatConvertor.PixelWidth(desiredColourFormat); int colourCentreIndex = markerSearchSpace.Centre.X + markerSearchSpace.Centre.Y * cPixelWidth; int leftColourIndex = markerSearchSpace.RangeX.Low + markerSearchSpace.RangeY.High * cPixelWidth; int rightColourIndex = markerSearchSpace.RangeX.High + markerSearchSpace.RangeY.High * cPixelWidth; var centreDepthPoint = dips[colourCentreIndex]; var leftDepthPoint = dips[leftColourIndex]; var rightDepthPoint = dips[rightColourIndex]; // get 3d point in camera basis of centre, left & right of user-drawn box var centre3DRaw = this.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint( this.desiredDepthFormat, centreDepthPoint); var left3DRaw = this.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint( this.desiredDepthFormat, leftDepthPoint); var right3DRaw = this.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint( this.desiredDepthFormat, rightDepthPoint); // get y-value of turntable var tilt = this.sensor.ElevationAngle * Math.PI / 180; var finalCameraY = (float)(centre3DRaw.Y * Math.Cos(-tilt) - centre3DRaw.Z * Math.Sin(-tilt)); // get minX and maxX to search-clip in var minXClip = left3DRaw.X; var maxXClip = right3DRaw.X; // get point cloud of captured scene var calSceneRaw = new SkeletonPoint[FormatConvertor.PixelDataLength(this.desiredDepthFormat)]; this.sensor.CoordinateMapper.MapDepthFrameToSkeletonFrame(this.desiredDepthFormat, depthCalFrame, calSceneRaw); PointCloud calScene = new PointCloud(); foreach (var p in calSceneRaw) { var myP = new Point3D(p.X, p.Y, p.Z); // vector-style point myP.RotateAboutAxis(Axis.X, -tilt); // rotate to null if (myP.X < minXClip) { continue; // too far left } if (myP.X > maxXClip) { continue; // too far right } if (myP.Y + 0.01f < finalCameraY) { continue; // too low down } if (myP.Y > finalCameraY + Kinect.CalibrationSettings.Default.MaxY) { continue; // too high up } if (myP.Z < Kinect.CalibrationSettings.Default.MinZ) { continue; // too close } if (myP.Z > Kinect.CalibrationSettings.Default.MaxZ) { continue; // too far back } calScene.Points.Add(myP); // got here, so point is ok } // calculate centre point from max-min ranges in X,Z float maxX = calScene.Points[0].X, maxZ = calScene.Points[0].Z; float minX = calScene.Points[0].X, minZ = calScene.Points[0].Z; for (int i = 1; i < calScene.Points.Count; i++) { if (calScene.Points[i].X > maxX) { maxX = calScene.Points[i].X; } if (calScene.Points[i].Z > maxZ) { maxZ = calScene.Points[i].Z; } if (calScene.Points[i].X < minX) { minX = calScene.Points[i].X; } if (calScene.Points[i].Z < minZ) { minZ = calScene.Points[i].Z; } } var finalCameraX = (minX + maxX) / 2; var finalCameraZ = (minZ + maxZ) / 2; // set turntable radius (from average of X and Z ranges) var radius = (float)((maxX - minX) + (maxZ - minZ)) / 4; calibration.TurntableRadius = radius; // set final camera location calibration.Camera = new CameraLocation( new Point3D(-finalCameraX, -finalCameraY, -finalCameraZ), tilt); // use chosen samples calibration.Markers = markers; if (DebugSettings.Default.DebugMode) { // quick calibration tt cloud output KaptureLibrary.IO.PointCloudWriter.WritePLY(DebugSettings.Default.LogRoot + @"\calibration.ply", calScene); // log output StreamWriter log = new StreamWriter(DebugSettings.Default.LogRoot + @"\calibration.log"); log.WriteLine("CAMERA = " + calibration.Camera.Origin.ToString()); log.WriteLine("TILT = " + calibration.Camera.ElevationAngle.ToString()); log.WriteLine("SEARCHSPACE2D = " + calibration.MarkerSearchSpace.ToString()); log.WriteLine("TTRADIUS = " + calibration.TurntableRadius); log.WriteLine("MARKERS := "); foreach (var cube in calibration.Markers) { log.Write(" (*) " + cube.Hue.ToString() + " HUE:" + cube.Hue.ToString()); log.Write(" +- " + cube.HueTolerance.ToString()); log.WriteLine(" ROTOFFSET: " + cube.RotationalOffset.ToString()); } log.Close(); } return(calibration); }