public static void Save(Blkd blkd, string fileName, byte version = LATEST_VERSION)
{
using (FileStream stream = new FileStream(fileName, FileMode.Create))
{
Blkd.Save(blkd, stream, version);
stream.Close();
}
}
public static Blkd Load(string fileName)
{
if (!File.Exists(fileName))
{
return(null);
}
Blkd blkd = new Blkd();
using (FileStream stream = new FileStream(fileName, FileMode.Open))
using (BinaryReader reader = new BinaryReader(stream))
{
// 1. Magic number
reader.ReadChars(4);
// 2. Version
blkd.Version = reader.ReadByte();
// 3. Dimensions
blkd.Width = reader.ReadUInt16();
blkd.Height = reader.ReadUInt16();
// 4. Bytes per pixel
if (blkd.Version == 1)
{
blkd.BytesPerPixel = 2; // one short per pixel
}
else if (blkd.Version == 2)
{
blkd.BytesPerPixel = reader.ReadByte();
}
else
{
throw new InvalidDataException(string.Format("Unsupported version {0} -- we support 1 or 2", blkd.Version));
}
// 5. Depth data
using (MemoryStream memoryStream = new MemoryStream())
{
reader.BaseStream.CopyTo(memoryStream);
long expectedLength = blkd.Width * blkd.Height * blkd.BytesPerPixel;
if (memoryStream.Length != expectedLength)
{
throw new InvalidDataException(string.Format("Data is {0} bytes, expected {1}", memoryStream.Length, expectedLength));
}
blkd.Data = memoryStream.ToArray();
}
}
return(blkd);
}
public static void Save(Blkd blkd, Stream stream, byte version = LATEST_VERSION)
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
// 1. Magic number
writer.Write((char)'B');
writer.Write((char)'L');
writer.Write((char)'K');
writer.Write((char)'D');
// 2. Version
blkd.Version = version;
writer.Write(version);
// 3. Dimensions
writer.Write(blkd.Width);
writer.Write(blkd.Height);
// 4. Bytes per pixel
if (version == 1)
{
if (blkd.BytesPerPixel != 2)
{
throw new InvalidDataException(string.Format("BLKD version 1 supports exactly 2 bytes per pixel, not {0}", blkd.BytesPerPixel));
}
}
else if (version == 2)
{
writer.Write(version);
}
else
{
throw new InvalidDataException(string.Format("Unsupported version {0} -- we support 1 or 2", version));
}
// 4. Depth data
long expectedLength = blkd.Width * blkd.Height * blkd.BytesPerPixel;
if (blkd.Data.LongLength < expectedLength)
{
throw new InvalidDataException(string.Format("Data is {0} bytes, expected at least {1}", blkd.Data.LongLength, expectedLength));
}
// Write only the desired number of bytes. This provides more flexibility, since
// it allows reusing the buffer for data with different dimensions.
if (expectedLength > int.MaxValue)
{
throw new ArgumentException(string.Format("FIXME, This doesn't handle data buffers longer than {0}", int.MaxValue));
}
writer.Write(blkd.Data, 0, (int)expectedLength);
writer.Close();
}
}
/// <summary>
/// Densely store depth to color mapping as BLKD.
///
/// Returns the number of shorts written to buffer.
/// </summary>
/// <param name="frame">KinectScanner.Reading.Frame</param>
/// <param name="filename">filename to store the mapping</param>
/// <returns></returns>
public Tuple<Blkd, TimeSpan> CaptureMappedFrame(LiveFrame frame, byte[] buffer)
{
DepthFrame depthFrame = frame.NativeDepthFrame;
CoordinateMapper mapper = frame.NativeCoordinateMapper;
if (buffer.Length != Frame.DEPTH_INFRARED_PIXELS * DEPTH_MAPPING_BYTES_PER_PIXEL)
throw new ArgumentException(string.Format("Buffer length is {0} but {1} is needed", buffer.LongLength, Frame.DEPTH_INFRARED_PIXELS * DEPTH_MAPPING_BYTES_PER_PIXEL));
depthFrame.CopyFrameDataToArray(_depthData);
mapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);
mapper.MapDepthFrameToCameraSpace(_depthData, _cameraSpacePoints);
Array.Clear(buffer, 0, buffer.Length);
int count = 0;
for (int i = 0; i < Frame.DEPTH_INFRARED_PIXELS; ++i)
{
ColorSpacePoint colorPoint = _colorPoints[i];
CameraSpacePoint cameraPoint = _cameraSpacePoints[i];
// make sure the depth pixel maps to a valid point in color space
short colorX = (short)Math.Floor(colorPoint.X + 0.5);
short colorY = (short)Math.Floor(colorPoint.Y + 0.5);
if (colorX < 0 || colorX >= Frame.COLOR_WIDTH || colorY < 0 || colorY >= Frame.COLOR_HEIGHT)
{
colorX = -1;
colorY = -1;
}
// Little endian === lowest order bytes at lower addresses
buffer[count++] = (byte)(colorX >> 0);
buffer[count++] = (byte)(colorX >> 8);
buffer[count++] = (byte)(colorY >> 0);
buffer[count++] = (byte)(colorY >> 8);
float[] cameraPointValues = new float[] { cameraPoint.X, cameraPoint.Y, cameraPoint.Z };
System.Buffer.BlockCopy(cameraPointValues, 0, buffer, count, 12);
count += 12;
}
Blkd result = new Blkd
{
Width = (UInt16)Frame.DEPTH_INFRARED_WIDTH,
Height = (UInt16)Frame.DEPTH_INFRARED_HEIGHT,
BytesPerPixel = DEPTH_MAPPING_BYTES_PER_PIXEL,
Version = 2,
Data = buffer
};
return new Tuple<Blkd, TimeSpan>(result, depthFrame.RelativeTime);
}
public static Blkd Load(string fileName)
{
if (!File.Exists(fileName)) return null;
Blkd blkd = new Blkd();
using (FileStream stream = new FileStream(fileName, FileMode.Open))
using (BinaryReader reader = new BinaryReader(stream))
{
// 1. Magic number
reader.ReadChars(4);
// 2. Version
blkd.Version = reader.ReadByte();
// 3. Dimensions
blkd.Width = reader.ReadUInt16();
blkd.Height = reader.ReadUInt16();
// 4. Bytes per pixel
if (blkd.Version == 1)
blkd.BytesPerPixel = 2; // one short per pixel
else if (blkd.Version == 2)
blkd.BytesPerPixel = reader.ReadByte();
else
throw new InvalidDataException(string.Format("Unsupported version {0} -- we support 1 or 2", blkd.Version));
// 5. Depth data
using (MemoryStream memoryStream = new MemoryStream())
{
reader.BaseStream.CopyTo(memoryStream);
long expectedLength = blkd.Width * blkd.Height * blkd.BytesPerPixel;
if (memoryStream.Length != expectedLength)
throw new InvalidDataException(string.Format("Data is {0} bytes, expected {1}", memoryStream.Length, expectedLength));
blkd.Data = memoryStream.ToArray();
}
}
return blkd;
}
public static void Save(Blkd blkd, string fileName, byte version = LATEST_VERSION)
{
using (FileStream stream = new FileStream(fileName, FileMode.Create))
{
Blkd.Save(blkd, stream, version);
stream.Close();
}
}
public static void Save(Blkd blkd, Stream stream, byte version = LATEST_VERSION)
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
// 1. Magic number
writer.Write((char)'B');
writer.Write((char)'L');
writer.Write((char)'K');
writer.Write((char)'D');
// 2. Version
blkd.Version = version;
writer.Write(version);
// 3. Dimensions
writer.Write(blkd.Width);
writer.Write(blkd.Height);
// 4. Bytes per pixel
if (version == 1)
{
if (blkd.BytesPerPixel != 2)
throw new InvalidDataException(string.Format("BLKD version 1 supports exactly 2 bytes per pixel, not {0}", blkd.BytesPerPixel));
}
else if (version == 2)
{
writer.Write(version);
}
else
{
throw new InvalidDataException(string.Format("Unsupported version {0} -- we support 1 or 2", version));
}
// 4. Depth data
long expectedLength = blkd.Width * blkd.Height * blkd.BytesPerPixel;
if (blkd.Data.LongLength < expectedLength)
throw new InvalidDataException(string.Format("Data is {0} bytes, expected at least {1}", blkd.Data.LongLength, expectedLength));
// Write only the desired number of bytes. This provides more flexibility, since
// it allows reusing the buffer for data with different dimensions.
if (expectedLength > int.MaxValue)
throw new ArgumentException(string.Format("FIXME, This doesn't handle data buffers longer than {0}", int.MaxValue));
writer.Write(blkd.Data, 0, (int)expectedLength);
writer.Close();
}
}
public static Blkd CreateDepthToCameraBlkd(PXCMPoint3DF32[] _depthToCameraMapping, int width, int height)
{
var BytesPerPixel = 3 * sizeof(float);
byte[] buffer = new byte[width * height * BytesPerPixel];
for (int i = 0; i < _depthToCameraMapping.Length; i++)
{
var point = _depthToCameraMapping[i];
var smallfloatBuffer = new float[] { point.x, point.y, point.z };
Buffer.BlockCopy(smallfloatBuffer, 0, buffer, i * BytesPerPixel, BytesPerPixel);
}
Blkd result = new Blkd
{
Width = (UInt16)width,
Height = (UInt16)height,
BytesPerPixel = (byte)BytesPerPixel,
Version = 2,
Data = buffer
};
return result;
}
