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