Exemple #1
0
        public IDictionary <TrackingMarker, Point3D> Convert2DTrackingPointsTo3DTrackingPoints(
            IDictionary <TrackingMarker, Point2D> trackingPoints2D,
            DepthFormat dFormat, ICollection <short> depthShorts, ColourFormat cFormat)
        {
            var kdFormat = FormatConvertor.ConvertToKinect(dFormat);
            var kcFormat = FormatConvertor.ConvertToKinect(cFormat);

            var dShorts = depthShorts.ToArray();

            // HACK!!!!!!!!!!!!!!!!
            return(_ConvertColourPointsToWorldPoints(trackingPoints2D, kdFormat, dShorts, kcFormat));
        }
Exemple #2
0
        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);
        }
Exemple #3
0
        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);
        }
 public SensorManager(DepthFormat dFormat, ColourFormat cFormat)
 {
     this.desiredDepthFormat         = FormatConvertor.ConvertToKinect(dFormat);
     this.desiredColourFormat        = FormatConvertor.ConvertToKinect(cFormat);
     this.frameReadyEventsRegistered = false;
 }