/// <summary>
/// Aligns the rgb and depth image, calculates the aligned ROI and pushes it through the pipeline.
/// </summary>
/// <param name="depth"></param>
/// <param name="depthImage"></param>
/// <param name="colorImage"></param>
private void PushAlignedRgbAndDepthImageROI(PXCMImage depth, Image <Gray, float> depthImage, Image <Rgb, byte> colorImage)
{
/* Get UV map */
var uvMapImage = Senz3DUtils.GetDepthUvMap(depth);
/* Get RgbOfDepth */
Senz3DUtils.GetRgbOfDepthPixels(depthImage, colorImage, uvMapImage, true, ref _rgbInDepthROI);
Stage(new ROI(this, "rgbInDepthROI")
{
RoiRectangle = _rgbInDepthROI
});
Push();
LogFormat("Identified rgbInDepthROI as {0}", _rgbInDepthROI);
}
private void GrabFrames()
{
while (_isRunning)
{
_alternate = !_alternate;
var confidenceThreshold = DepthConfidenceThreshold;
if (IsAdaptiveSensing)
{
confidenceThreshold = _alternate ? DepthConfidenceThresholdHigh : DepthConfidenceThreshold;
}
/* If raw depth is needed, disable smoothing */
_device.SetProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_SMOOTHING, DepthSmoothing ? 1 : 0);
_device.SetProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_CONFIDENCE_THRESHOLD, confidenceThreshold);
/* Wait until a frame is ready */
if (!_pp.AcquireFrame(true))
{
break;
}
if (_pp.IsDisconnected())
{
break;
}
/* Get RGB color image */
Stopwatch sw = Stopwatch.StartNew();
var color = _pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_COLOR);
var colorBitmap = Senz3DUtils.GetRgb32Pixels(color);
var colorImage = new Image <Rgb, byte>(colorBitmap);
ColorImageFrameTime = sw.ElapsedMilliseconds;
/* Get depth image */
sw.Restart();
var depth = _pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_DEPTH);
var depthImageAndConfidence = Senz3DUtils.GetHighPrecisionDepthImage(depth, MinDepthValue, MaxDepthValue);
// do adaptive sensing (alternating low/high confidence threshold) on depth image if enabled
if (IsAdaptiveSensing)
{
depthImageAndConfidence = PerformAdaptiveSensing(depthImageAndConfidence);
}
var depthImage = (Image <Gray, float>)depthImageAndConfidence[0];
var confidenceMapImage = (Image <Rgb, Byte>)depthImageAndConfidence[1];
DepthImageFrameTime = sw.ElapsedMilliseconds;
ConfidenceMapImageFrameTime = 0;
// alignment of rgb and depth image (depth images field of view is larger
//than rgb image field of view and therefore needs to be 'cropped' properly.
if (_rgbInDepthROI == Rectangle.Empty)
{
PushAlignedRgbAndDepthImageROI(depth, depthImage, colorImage);
}
_pp.ReleaseFrame();
if (IsRenderContent)
{
RenderImages(colorImage, depthImage, confidenceMapImage);
}
// publish images finally
PublishImages(colorImage, depthImage, confidenceMapImage);
}
_pp.Close();
_pp.Dispose();
}
private void DoRendering()
{
_isRunning = true;
/* UtilMPipeline works best for synchronous color and depth streaming */
_pp = new UtilMPipeline();
/* Set Input Source */
_pp.capture.SetFilter("DepthSense Device 325V2");
/* Set Color & Depth Resolution */
PXCMCapture.VideoStream.ProfileInfo cinfo = GetConfiguration(PXCMImage.ColorFormat.COLOR_FORMAT_RGB32);
_pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, cinfo.imageInfo.width, cinfo.imageInfo.height);
_pp.capture.SetFilter(ref cinfo); // only needed to set FPS
PXCMCapture.VideoStream.ProfileInfo dinfo2 = GetConfiguration(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH);
_pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, dinfo2.imageInfo.width, dinfo2.imageInfo.height);
_pp.capture.SetFilter(ref dinfo2); // only needed to set FPS
/* Initialization */
if (!_pp.Init())
{
LogFormat("Could not initialize Senz3D hardware");
HasErrorState = true;
return;
}
var capture = _pp.capture;
_device = capture.device;
_device.SetProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_CONFIDENCE_THRESHOLD, DepthConfidenceThreshold);
_device.QueryProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_LOW_CONFIDENCE_VALUE, out EmguExtensions.LowConfidence);
_device.QueryProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_SATURATION_VALUE, out EmguExtensions.Saturation);
while (_isRunning)
{
/* If raw depth is needed, disable smoothing */
_pp.capture.device.SetProperty(PXCMCapture.Device.Property.PROPERTY_DEPTH_SMOOTHING, DepthSmoothing ? 1 : 0);
/* Wait until a frame is ready */
if (!_pp.AcquireFrame(true))
{
break;
}
if (_pp.IsDisconnected())
{
break;
}
/* Get RGB color image */
Stopwatch sw = Stopwatch.StartNew();
var color = _pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_COLOR);
var colorBitmap = Senz3DUtils.GetRgb32Pixels(color);
var colorImage = new Image <Rgb, byte>(colorBitmap);
var colorImageCopy = colorImage.Copy();
ColorImageFrameTime = sw.ElapsedMilliseconds;
/* Get depth image */
sw.Restart();
var depth = _pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_DEPTH);
var depthImageAndConfidence = Senz3DUtils.GetHighPrecisionDepthImage(depth, MinDepthValue, MaxDepthValue);
var depthImage = (Image <Gray, float>)depthImageAndConfidence[0];
var depthImageCopy = depthImage.Copy();
var confidenceMapImage = (Image <Rgb, Byte>)depthImageAndConfidence[1];
var confidenceMapImageCopy = confidenceMapImage.Copy();
DepthImageFrameTime = sw.ElapsedMilliseconds;
ConfidenceMapImageFrameTime = 0;
bool getRgbInDepthROI = false;
/* if rgbInDepthROI is undefined get uvmap and rgbofdepth and rgbInDepthROI */
if (_rgbInDepthROI.Left == 0 && _rgbInDepthROI.Right == 0 && _rgbInDepthROI.Width == 0 &&
_rgbInDepthROI.Height == 0)
{
getRgbInDepthROI = true;
}
/* Get UV map */
Image <Rgb, float> uvMapImage, uvMapImageCopy;
if (UvMapChecked || getRgbInDepthROI)
{
sw.Restart();
uvMapImage = Senz3DUtils.GetDepthUvMap(depth);
uvMapImageCopy = uvMapImage.Copy();
UVMapImageFrameTime = sw.ElapsedMilliseconds;
}
else
{
uvMapImage = null;
uvMapImageCopy = null;
UVMapImageFrameTime = -1;
}
/* Get RgbOfDepth */
Image <Rgb, byte> rgbOfDepthImage, rgbOfDepthImageCopy;
if ((RgbOfDepthChecked && uvMapImage != null) || getRgbInDepthROI)
{
sw.Restart();
if (getRgbInDepthROI)
{
rgbOfDepthImage = Senz3DUtils.GetRgbOfDepthPixels(depthImage, colorImage, uvMapImage, true, ref _rgbInDepthROI);
Stage(new ROI(this, "rgbInDepthROI")
{
RoiRectangle = _rgbInDepthROI
});
Push();
LogFormat("Identified rgbInDepthROI as {0}", _rgbInDepthROI);
}
else
{
rgbOfDepthImage = Senz3DUtils.GetRgbOfDepthPixels(depthImage, colorImage, uvMapImage);
}
rgbOfDepthImageCopy = rgbOfDepthImage.Copy();
RgbOfDepthImageFrameTime = sw.ElapsedMilliseconds;
}
else
{
rgbOfDepthImage = null;
rgbOfDepthImageCopy = null;
RgbOfDepthImageFrameTime = -1;
}
/* Get DepthOfRGB */
Image <Gray, float> depthOfRgbImage, depthOfRgbImageCopy;
if (DepthOfRgbChecked && uvMapImage != null)
{
sw.Restart();
depthOfRgbImage = Senz3DUtils.GetDepthOfRGBPixels(depthImage, colorImage, uvMapImage);
depthOfRgbImageCopy = depthOfRgbImage.Copy();
DepthOfRgbImageFrameTime = sw.ElapsedMilliseconds;
}
else
{
depthOfRgbImage = null;
depthOfRgbImageCopy = null;
DepthOfRgbImageFrameTime = -1;
}
_pp.ReleaseFrame();
if (IsRenderContent)
{
Task.Factory.StartNew(() =>
{
var bitmap = colorImageCopy.ToBitmapSource(true);
colorImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => ColorImageSource = s.Result);
Task.Factory.StartNew(() =>
{
var bitmap = depthImageCopy.ToGradientBitmapSource(true, EmguExtensions.LowConfidence, EmguExtensions.Saturation);
depthImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => DepthImageSource = s.Result);
Task.Factory.StartNew(() =>
{
var bitmap = confidenceMapImageCopy.ToBitmapSource(true);
confidenceMapImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => ConfidenceMapImageSource = s.Result);
/* draw uvmap */
if (uvMapImage != null)
{
Task.Factory.StartNew(() =>
{
var bitmap = uvMapImageCopy.ToBitmapSource(true);
uvMapImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => UVMapImageSource = s.Result);
}
/* draw rgbofdepth */
if (rgbOfDepthImage != null)
{
Task.Factory.StartNew(() =>
{
var bitmap = rgbOfDepthImageCopy.ToBitmapSource(true);
rgbOfDepthImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => RgbOfDepthImageSource = s.Result);
}
/* draw depthofrgb */
if (depthOfRgbImage != null)
{
Task.Factory.StartNew(() =>
{
var bitmap = depthOfRgbImageCopy.ToGradientBitmapSource(true, EmguExtensions.LowConfidence, EmguExtensions.Saturation);
depthOfRgbImageCopy.Dispose();
return(bitmap);
}).ContinueWith(s => DepthOfRgbImageSource = s.Result);
}
}
var dc = new DataContainer(++_frameId, DateTime.Now)
{
new RgbImageData(this, "color", colorImage),
new GrayFloatImage(this, "depth", depthImage),
new RgbImageData(this, "confidence", confidenceMapImage),
};
if (uvMapImage != null)
{
dc.Add(new RgbFloatImage(this, "uvmap", uvMapImage));
}
if (rgbOfDepthImage != null)
{
dc.Add(new RgbImageData(this, "rgbofdepth", rgbOfDepthImage));
}
if (depthOfRgbImage != null)
{
dc.Add(new GrayFloatImage(this, "depthofrgb", depthOfRgbImage));
}
Publish(dc);
}
_pp.Close();
_pp.Dispose();
}
