public unsafe MainWindow()
{
InitializeComponent();
InitializeServer();
this.Closing += new CancelEventHandler(OnWindowClosing);
System.Windows.Threading.Dispatcher uiDispatcher = this.Dispatcher;
UtilMPipeline pp = new UtilMPipeline();
BackgroundWorker worker = new BackgroundWorker();
worker.DoWork += delegate(object s, DoWorkEventArgs args)
{
pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH);
pp.Init();
for (; ; )
{
if (!pp.AcquireFrame(true)) break;
{
PXCMImage image = pp.QueryImage(PXCMImage.ImageType.IMAGE_TYPE_DEPTH);
PXCMImage.ImageData ddata;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR) break;
short* depth = (short*)ddata.buffer.planes[0];
short minZ = 32001;
int minX = 0;
int minY = 0;
short maxX = 320;
for (int y = 0; y < 240; y++)
{
for (int x = 0; x < 320; x++)
{
if (depth[(y * 320) + x] < minZ)
{
minZ = depth[(y * 320) + x];
minX = x;
minY = y;
}
}
}
/////////////////////
//normalize extremes//
if (minZ < DistanceFromScreenEdge) minZ = DistanceFromScreenEdge;
//match origin//
minX -= 160;
minY -= 120;
/////////////////////
maxX = (short)(Math.Tan(FOV / 2) * minZ);
short discardedX = (short)(Math.Tan(FOV / 2) * (minZ - DistanceFromScreenEdge));
short screenMaxX = (short)(maxX - discardedX);
float xRatio = minX / screenMaxX;
UpdateGridDelegate update = new UpdateGridDelegate(UpdateGrid);
uiDispatcher.BeginInvoke(update, minX, minY, minZ, maxX, discardedX, screenMaxX, xRatio);
image.ReleaseAccess(ref ddata);
pp.ReleaseFrame();
}
}
pp.Close();
pp.Dispose();
};
worker.RunWorkerAsync();
}
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();
}
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();
}