// Generate a point cloud from depth and RGB data
internal List<ColoredPoint3d> GeneratePointCloud(
KinectSensor kinect, short[] depth, byte[] color,
int sampling, bool withColor = false
)
{
if (depth == null || color == null)
return null;
int depWidth = 640;
// We will return a list of our ColoredPoint3d objects
List<ColoredPoint3d> res = new List<ColoredPoint3d>();
// Loop through the depth information - we process two
// bytes at a time
for (int i = 0; i < depth.Length; i += sampling)
{
// The x and y positions can be calculated using modulus
// division from the array index
int x = i % depWidth;
int y = i / depWidth;
SkeletonPoint p =
kinect.MapDepthToSkeletonPoint(
DepthImageFormat.Resolution640x480Fps30,
x, y, depth[i]
);
// A zero value for Z means there is no usable depth for
// that pixel
if (p.Z > 0)
{
// Create a ColoredPoint3d to store our XYZ and RGB info
// for a pixel
ColoredPoint3d cv = new ColoredPoint3d();
cv.X = p.X;
cv.Y = p.Z;
cv.Z = p.Y;
// Only calculate the colour when it's needed (as it's
// now more expensive, albeit more accurate)
if (withColor)
{
// Get the colour indices for that particular depth
// pixel
ColorImagePoint cip =
kinect.MapDepthToColorImagePoint(
DepthImageFormat.Resolution640x480Fps30,
x, y, depth[i],
ColorImageFormat.RgbResolution640x480Fps30
);
// Extract the RGB data from the appropriate place
// in the colour data
int colIndex = 4 * (cip.X + (cip.Y * depWidth));
if (colIndex <= color.GetUpperBound(0) - 2)
{
cv.B = (byte)(color[colIndex + 0]);
cv.G = (byte)(color[colIndex + 1]);
cv.R = (byte)(color[colIndex + 2]);
}
}
else
{
// If we don't need colour information, just set each
// pixel to white
cv.B = 255;
cv.G = 255;
cv.R = 255;
}
// Add our pixel data to the list to return
res.Add(cv);
}
}
// Apply a bounding box filter, if one is defined
if (_ext.HasValue)
{
// Use LINQ to get the points within the
// bounding box
var vecSet =
from ColoredPoint3d vec in res
where
vec.X > _ext.Value.MinPoint.X &&
vec.X < _ext.Value.MaxPoint.X &&
vec.Y > _ext.Value.MinPoint.Y &&
vec.Y < _ext.Value.MaxPoint.Y &&
vec.Z > _ext.Value.MinPoint.Z &&
vec.Z < _ext.Value.MaxPoint.Z
select vec;
// Convert our IEnumerable<> into a List<>
res = vecSet.ToList<ColoredPoint3d>();
}
return res;
}