protected override DateTime GenerateNext(DateTime currentTime)
{
Shared <DepthImage> imgDepth = DepthImagePool.GetOrCreate(sicsDepth.width, sicsDepth.height);
Shared <Image> imgDepthColor = ImagePool.GetOrCreate(sicsDepth.width, sicsDepth.height, PixelFormat.BGR_24bpp);
Shared <Image> imgRBG = ImagePool.GetOrCreate(sicsRBG.width, sicsRBG.height, PixelFormat.BGR_24bpp);
try
{
if (qDepth.PollForFrame(out Frame frDepth))
{
imgDepth.Resource.CopyFrom(frDepth.Data);
imgDepthColor.Resource.CopyFrom(imgDepth.Resource.PseudoColorize((0, 2048)));
OutDepthImage.Post(imgDepth, msPipe.GetCurrentTime());
OutDepthImageColorized.Post(imgDepthColor, msPipe.GetCurrentTime());
}
if (qRBG.PollForFrame(out Frame frRBG))
{
imgRBG.Resource.CopyFrom(frRBG.Data);
OutRBGImage.Post(imgRBG, msPipe.GetCurrentTime());
}
}
catch (Exception e)
{
Console.WriteLine(e.Message);
}
return(msPipe.GetCurrentTime());
}
private static Shared <DepthImage> Adapter(Shared <EncodedDepthImage> sharedEncodedDepthImage, Envelope envelope)
{
Shared <DepthImage> sharedDepthImage = null;
if ((sharedEncodedDepthImage != null) && (sharedEncodedDepthImage.Resource != null))
{
sharedDepthImage = DepthImagePool.GetOrCreate(sharedEncodedDepthImage.Resource.Width, sharedEncodedDepthImage.Resource.Height);
var decoder = new DepthImageFromStreamDecoder();
decoder.DecodeFromStream(sharedEncodedDepthImage.Resource.ToStream(), sharedDepthImage.Resource);
}
return(sharedDepthImage);
}
/// <summary>
/// Decode a shared encoded depth image to a shared depth image.
/// </summary>
/// <param name="sharedEncodedDepthImage">The shared encoded depth image to decode.</param>
/// <returns>The decoded shared depth image.</returns>
internal static Shared <DepthImage> Decode(this Shared <EncodedDepthImage> sharedEncodedDepthImage)
{
Shared <DepthImage> sharedDepthImage = null;
if (sharedEncodedDepthImage.Resource != null)
{
sharedDepthImage = DepthImagePool.GetOrCreate(sharedEncodedDepthImage.Resource.Width, sharedEncodedDepthImage.Resource.Height);
var decoder = new DepthImageFromStreamDecoder();
decoder.DecodeFromStream(sharedEncodedDepthImage.Resource.ToStream(), sharedDepthImage.Resource);
}
return(sharedDepthImage);
}
/// <inheritdoc/>
public override Shared <DepthImage> GetAdaptedValue(Shared <EncodedDepthImage> source, Envelope envelope)
{
Shared <DepthImage> sharedDepthImage = null;
if (source != null && source.Resource != null)
{
sharedDepthImage = DepthImagePool.GetOrCreate(source.Resource.Width, source.Resource.Height);
var decoder = new DepthImageFromStreamDecoder();
decoder.DecodeFromStream(source.Resource.ToStream(), sharedDepthImage.Resource);
}
return(sharedDepthImage);
}
private void ThreadProc()
{
Imaging.PixelFormat pixelFormat = PixelFormat.BGR_24bpp;
switch (this.device.GetColorBpp())
{
case 24:
pixelFormat = PixelFormat.BGR_24bpp;
break;
case 32:
pixelFormat = PixelFormat.BGRX_32bpp;
break;
default:
throw new NotSupportedException("Expected 24bpp or 32bpp image.");
}
var colorImage = ImagePool.GetOrCreate((int)this.device.GetColorWidth(), (int)this.device.GetColorHeight(), pixelFormat);
uint colorImageSize = this.device.GetColorHeight() * this.device.GetColorStride();
switch (this.device.GetDepthBpp())
{
case 16:
pixelFormat = PixelFormat.Gray_16bpp;
break;
case 8:
pixelFormat = PixelFormat.Gray_8bpp;
break;
default:
throw new NotSupportedException("Expected 8bpp or 16bpp image.");
}
var depthImage = DepthImagePool.GetOrCreate(
(int)this.device.GetDepthWidth(),
(int)this.device.GetDepthHeight(),
DepthValueSemantics.DistanceToPlane,
0.001);
uint depthImageSize = this.device.GetDepthHeight() * this.device.GetDepthStride();
while (!this.shutdown)
{
this.device.ReadFrame(colorImage.Resource.ImageData, colorImageSize, depthImage.Resource.ImageData, depthImageSize);
DateTime t = DateTime.UtcNow;
this.ColorImage.Post(colorImage, t);
this.DepthImage.Post(depthImage, t);
}
}
private static Shared <Image> Adapter(Shared <EncodedDepthImage> sharedEncodedDepthImage, Envelope envelope)
{
Shared <Image> sharedImage = null;
if ((sharedEncodedDepthImage != null) && (sharedEncodedDepthImage.Resource != null))
{
var sharedDepthImage = DepthImagePool.GetOrCreate(sharedEncodedDepthImage.Resource.Width, sharedEncodedDepthImage.Resource.Height);
sharedImage = ImagePool.GetOrCreate(sharedEncodedDepthImage.Resource.Width, sharedEncodedDepthImage.Resource.Height, PixelFormat.Gray_16bpp);
var decoder = new DepthImageFromStreamDecoder();
decoder.DecodeFromStream(sharedEncodedDepthImage.Resource.ToStream(), sharedDepthImage.Resource);
sharedDepthImage.Resource.CopyTo(sharedImage.Resource);
}
return(sharedImage);
}
/// <inheritdoc/>
public override Shared <Image> GetAdaptedValue(Shared <EncodedDepthImage> source, Envelope envelope)
{
Shared <Image> sharedImage = null;
if ((source != null) && (source.Resource != null))
{
using var sharedDepthImage = DepthImagePool.GetOrCreate(source.Resource.Width, source.Resource.Height);
sharedImage = ImagePool.GetOrCreate(source.Resource.Width, source.Resource.Height, PixelFormat.Gray_16bpp);
var decoder = new DepthImageFromStreamDecoder();
decoder.DecodeFromStream(source.Resource.ToStream(), sharedDepthImage.Resource);
sharedDepthImage.Resource.CopyTo(sharedImage.Resource);
}
return(sharedImage);
}
/// <summary>
/// Decodes an encoded depth image image rectangle using a specified depth image decoder.
/// </summary>
/// <param name="source">The source stream of encoded depth image rectangles.</param>
/// <param name="decoder">The depth image decoder to use.</param>
/// <param name="deliveryPolicy">An optional delivery policy.</param>
/// <param name="name">An optional name for the stream operator.</param>
/// <returns>A stream of decoded depth image rectangles.</returns>
public static IProducer <DepthImageRectangle3D> Decode(
this IProducer <EncodedDepthImageRectangle3D> source,
IDepthImageFromStreamDecoder decoder,
DeliveryPolicy <EncodedDepthImageRectangle3D> deliveryPolicy = null,
string name = nameof(Decode))
{
return(source.Process <EncodedDepthImageRectangle3D, DepthImageRectangle3D>(
(encodedDepthImageRectangle3D, envelope, emitter) =>
{
var encodedDepthImage = encodedDepthImageRectangle3D.Image.Resource;
using var depthImage = DepthImagePool.GetOrCreate(
encodedDepthImage.Width,
encodedDepthImage.Height,
encodedDepthImage.DepthValueSemantics,
encodedDepthImage.DepthValueToMetersScaleFactor);
depthImage.Resource.DecodeFrom(encodedDepthImage, decoder);
emitter.Post(new DepthImageRectangle3D(encodedDepthImageRectangle3D.Rectangle3D, depthImage), envelope.OriginatingTime);
},
deliveryPolicy,
name));
}
/// <summary>
/// Decodes an encoded depth image camera view using a specified image decoder.
/// </summary>
/// <param name="source">The source stream of encoded depth image camera views.</param>
/// <param name="decoder">The depth image decoder to use.</param>
/// <param name="deliveryPolicy">An optional delivery policy.</param>
/// <param name="name">An optional name for the stream operator.</param>
/// <returns>A stream of decoded depth image camera views.</returns>
public static IProducer <DepthImageCameraView> Decode(
this IProducer <EncodedDepthImageCameraView> source,
IDepthImageFromStreamDecoder decoder,
DeliveryPolicy <EncodedDepthImageCameraView> deliveryPolicy = null,
string name = nameof(Decode))
{
return(source.Process <EncodedDepthImageCameraView, DepthImageCameraView>(
(encodedDepthImageCameraView, envelope, emitter) =>
{
var encodedDepthImage = encodedDepthImageCameraView.ViewedObject.Resource;
using var depthImage = DepthImagePool.GetOrCreate(
encodedDepthImage.Width,
encodedDepthImage.Height,
encodedDepthImage.DepthValueSemantics,
encodedDepthImage.DepthValueToMetersScaleFactor);
depthImage.Resource.DecodeFrom(encodedDepthImage, decoder);
using var depthImageCameraView = new DepthImageCameraView(depthImage, encodedDepthImageCameraView.CameraIntrinsics, encodedDepthImageCameraView.CameraPose);
emitter.Post(depthImageCameraView, envelope.OriginatingTime);
},
deliveryPolicy,
name));
}
/// <inheritdoc/>
protected override void ProcessSensorFrame(IResearchModeSensorFrame sensorFrame, ResearchModeSensorResolution resolution, ulong frameTicks, DateTime originatingTime)
{
var shouldOutputDepthImage = this.Configuration.OutputDepthImage &&
(originatingTime - this.DepthImage.LastEnvelope.OriginatingTime) > this.Configuration.OutputMinInterval;
var shouldOutputDepthImageCameraView = this.Configuration.OutputDepthImageCameraView &&
(originatingTime - this.DepthImageCameraView.LastEnvelope.OriginatingTime) > this.Configuration.OutputMinInterval;
var shouldOutputInfraredImage = this.Configuration.OutputInfraredImage &&
(originatingTime - this.InfraredImage.LastEnvelope.OriginatingTime) > this.Configuration.OutputMinInterval;
var shouldOutputInfraredImageCameraView = this.Configuration.OutputInfraredImageCameraView &&
(originatingTime - this.InfraredImageCameraView.LastEnvelope.OriginatingTime) > this.Configuration.OutputMinInterval;
if (shouldOutputDepthImage ||
shouldOutputDepthImageCameraView ||
shouldOutputInfraredImage ||
shouldOutputInfraredImageCameraView)
{
var depthFrame = sensorFrame as ResearchModeSensorDepthFrame;
int depthImageWidth = (int)resolution.Width;
int depthImageHeight = (int)resolution.Height;
// Process and post the depth image if need be
if (shouldOutputDepthImage || shouldOutputDepthImageCameraView)
{
byte[] sigmaBuffer = null;
var depthBuffer = depthFrame.GetBuffer();
if (this.isLongThrow)
{
sigmaBuffer = depthFrame.GetSigmaBuffer(); // Long-throw only
Debug.Assert(depthBuffer.Length == sigmaBuffer.Length, "Depth and sigma buffers should be of equal size!");
}
using var depthImage = DepthImagePool.GetOrCreate(
depthImageWidth,
depthImageHeight,
DepthValueSemantics.DistanceToPoint,
0.001);
Debug.Assert(depthImage.Resource.Size == depthBuffer.Length * sizeof(ushort), "DepthImage size does not match raw depth buffer size!");
unsafe
{
ushort *depthData = (ushort *)depthImage.Resource.ImageData.ToPointer();
for (int i = 0; i < depthBuffer.Length; ++i)
{
bool invalid = this.isLongThrow ?
((sigmaBuffer[i] & InvalidMask) > 0) :
(depthBuffer[i] >= InvalidAhatValue);
*depthData++ = invalid ? (ushort)0 : depthBuffer[i];
}
}
if (shouldOutputDepthImage)
{
this.DepthImage.Post(depthImage, originatingTime);
}
if (shouldOutputDepthImageCameraView)
{
using var depthImageCameraView = new DepthImageCameraView(depthImage, this.GetCameraIntrinsics(), this.GetCameraPose());
this.DepthImageCameraView.Post(depthImageCameraView, originatingTime);
}
}
// Process and post the infrared image if need be
if (shouldOutputInfraredImage || shouldOutputInfraredImageCameraView)
{
var infraredBuffer = depthFrame.GetAbDepthBuffer();
using var infraredImage = ImagePool.GetOrCreate(depthImageWidth, depthImageHeight, PixelFormat.Gray_16bpp);
Debug.Assert(infraredImage.Resource.Size == infraredBuffer.Length * sizeof(ushort), "InfraredImage size does not match raw infrared buffer size!");
unsafe
{
fixed(ushort *p = infraredBuffer)
{
infraredImage.Resource.CopyFrom((IntPtr)p);
}
}
if (shouldOutputInfraredImage)
{
this.InfraredImage.Post(infraredImage, originatingTime);
}
if (shouldOutputInfraredImageCameraView)
{
using var infraredImageCameraView = new ImageCameraView(infraredImage, this.GetCameraIntrinsics(), this.GetCameraPose());
this.InfraredImageCameraView.Post(infraredImageCameraView, originatingTime);
}
}
}
}
