private void UpdateColorImage(PXCMImage colorFrame)
{
if (colorFrame != null)
{
PXCMImage.ImageData data = null;
var ret = colorFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24,
out data);
if (ret >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
var info = colorFrame.QueryInfo();
var length = data.pitches[0] * info.height;
var buffer = data.ToByteArray(0, length);
this.ColorImageElement = BitmapSource.Create(
info.width,
info.height,
96,
96,
PixelFormats.Bgr24,
null,
buffer,
data.pitches[0]);
colorFrame.ReleaseAccess(data);
}
}
}
// Depth(距離)データを更新する
private void UpdateDepthData(PXCMImage depthFrame)
{
if (depthFrame == null)
{
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = depthFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Depth画像の取得に失敗");
}
// Depthデータを取得する
var info = depthFrame.QueryInfo();
depthBuffer = data.ToShortArray(0, info.width * info.height);
// データを解放する
depthFrame.ReleaseAccess(data);
}
/// <summary> カラーイメージが更新された時の処理 </summary>
private void UpdateColorImage(PXCMImage colorFrame)
{
if (colorFrame == null)
{
return;
}
//データの取得
PXCMImage.ImageData data;
//アクセス権の取得
pxcmStatus ret = colorFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("カラー画像の取得に失敗");
}
//ビットマップに変換する
//画像の幅と高さ，フォーマットを取得
var info = colorFrame.QueryInfo();
//1ライン当たりのバイト数を取得し(pitches[0]) 高さをかける (1pxel 3byte)
var length = data.pitches[0] * info.height;
//画素の色データの取得
//ToByteArrayでは色データのバイト列を取得する．
var buffer = data.ToByteArray(0, length);
//バイト列をビットマップに変換
ColorImage.Source = BitmapSource.Create(info.width, info.height, 96, 96, PixelFormats.Bgr32, null, buffer, data.pitches[0]);
//データを解放する
colorFrame.ReleaseAccess(data);
}
// Depth画像を更新する
private void UpdateDepthImage(PXCMImage depthFrame)
{
if (depthFrame == null)
{
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = depthFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new Exception("Depth画像の取得に失敗");
}
// Bitmapに変換する
var info = depthFrame.QueryInfo();
var length = data.pitches[0] * info.height;
var buffer = data.ToByteArray(0, length);
ImageHand.Source = BitmapSource.Create(info.width, info.height, 96, 96,
PixelFormats.Bgr32, null, buffer, data.pitches[0]);
// データを解放する
depthFrame.ReleaseAccess(data);
}
void ProcessImage(PXCMImage depth)
{
if (depth == null)
{
return;
}
/* Returns the image properties. */
info = depth.QueryInfo();
/* Set the pixel format*/
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
/* Perform blob extraction on the specified depth image.*/
m_blob.ProcessImage(depth);
/* Create an instance of the PXC[M]Image interface to manage image buffer access. */
new_image = mask_utils.PipelineManager.session.CreateImage(info);
results = m_blob.QueryBlobData(0, new_image, out blobData[0]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
/* Get all blob points */
blobPointsPos = new Vector2[6];
blobPointsPos[0] = new Vector2(blobData[0].centerPoint.x * -1, blobData[0].centerPoint.y * -1);
blobPointsPos[1] = new Vector2(blobData[0].topPoint.x * -1, blobData[0].topPoint.y * -1);
blobPointsPos[2] = new Vector2(blobData[0].bottomPoint.x * -1, blobData[0].bottomPoint.y * -1);
blobPointsPos[3] = new Vector2(blobData[0].leftPoint.x * -1, blobData[0].leftPoint.y * -1);
blobPointsPos[4] = new Vector2(blobData[0].rightPoint.x * -1, blobData[0].rightPoint.y * -1);
blobPointsPos[5] = new Vector2(blobData[0].closestPoint.x * -1, blobData[0].closestPoint.y * -1);
/* Sert blob points colors*/
blobPointColors = new Color[6];
blobPointColors[0] = Color.blue;
blobPointColors[1] = Color.yellow;
blobPointColors[2] = Color.yellow;
blobPointColors[3] = Color.yellow;
blobPointColors[4] = Color.yellow;
blobPointColors[5] = Color.green;
DisplayPoints();
results = m_contour.ProcessImage(new_image);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && m_contour.QueryNumberOfContours() > 0)
{
/* Retrieve the detected contour points.*/
results = m_contour.QueryContourData(0, out pointOuter[0]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (pointOuter[0] != null && pointOuter[0].Length > 0)
{
DisplayContour(pointOuter[0], 0);
}
}
}
}
new_image.Dispose();
}
void ProcessImage(PXCMImage depth)
{
if (depth == null)
{
return;
}
/* Returns the image properties. */
info = depth.QueryInfo();
/* Set the pixel format*/
info.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
/* Perform blob extraction on the specified depth image.*/
m_blob.ProcessImage(depth);
/* Create an instance of the PXC[M]Image interface to manage image buffer access. */
new_image = mask_utils.PipelineManager.session.CreateImage(info);
int blobCount = m_blob.QueryNumberOfBlobs();
/* To store all blob points */
blobPointsPos = new List <Vector2>();
/* To store all contour points */
countourPoints = new List <PXCMPointI32>();
for (int i = 0; i < blobCount; i++)
{
results = m_blob.QueryBlobData(i, new_image, out blobData[i]);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
blobPointsPos.Add(new Vector2(blobData[i].centerPoint.x * -1, blobData[i].centerPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].topPoint.x * -1, blobData[i].topPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].bottomPoint.x * -1, blobData[i].bottomPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].leftPoint.x * -1, blobData[i].leftPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].rightPoint.x * -1, blobData[i].rightPoint.y * -1));
blobPointsPos.Add(new Vector2(blobData[i].closestPoint.x * -1, blobData[i].closestPoint.y * -1));
DisplayPoints();
results = m_contour.ProcessImage(new_image);
if (results == pxcmStatus.PXCM_STATUS_NO_ERROR && m_contour.QueryNumberOfContours() > 0)
{
results = m_contour.QueryContourData(0, out pointOuter[i]);
DisplayContour(pointOuter[i], i);
}
}
}
new_image.Dispose();
}
private void UpdateSegmentationImage(PXCMImage segmentationImage)
{
if (segmentationImage == null)
{
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = segmentationImage.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// ピクセルデータを初期化する
Array.Clear(imageBuffer, 0, imageBuffer.Length);
// セグメンテーション画像をバイト列に変換する
var info = segmentationImage.QueryInfo();
var buffer = data.ToByteArray(0, data.pitches[0] * info.height);
for (int i = 0; i < (info.height * info.width); ++i)
{
var index = i * BYTE_PER_PIXEL;
// α値が0でない場合には有効な場所として色をコピーする
if (buffer[index + 3] != 0)
{
imageBuffer[index + 0] = buffer[index + 0];
imageBuffer[index + 1] = buffer[index + 1];
imageBuffer[index + 2] = buffer[index + 2];
imageBuffer[index + 3] = 255;
}
// α値が0の場合は、ピクセルデータのα値を0にする
else
{
imageBuffer[index + 3] = 0;
}
}
// ピクセルデータを更新する
imageBitmap.WritePixels(imageRect, imageBuffer, data.pitches[0], 0);
// データを解放する
segmentationImage.ReleaseAccess(data);
}
public byte[] DepthToColorCoordinatesByUVMAP(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (cpixels == null)
{
return(cpixels);
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
// float[] uvmap;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(short); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
projection.QueryUVMap(depth, uvmap);
int uvpitch = depth.QueryInfo().width;
depth.ReleaseAccess(ddata);
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++)
{
UInt16 d = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
if (d == invalid_value)
{
continue; // no mapping based on unreliable depth values
}
float uvx = uvmap[y * uvpitch + x].x, uvy = uvmap[y * uvpitch + x].y;
Int32 xx = (Int32)(uvx * cwidth), yy = (Int32)(uvy * cheight);
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 1);
}
}
}
return(cpixels);
}
public void Loop(LoopObjects loopObjects)
{
var segmentation = _camera.Manager.Query3DSeg();
if (segmentation == null)
{
return;
}
PXCMImage image = segmentation.AcquireSegmentedImage();
PXCMImage.ImageData imageData;
image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32,
out imageData);
PXCMImage.ImageInfo imageInfo = image.QueryInfo();
using (var bitmap = new Bitmap(imageData.ToBitmap(0, imageInfo.width, imageInfo.height))) {
using (var ms = new MemoryStream()) {
bitmap.Save(ms, ImageFormat.Bmp);
_camera.SegmentationStream.CurrentBitmapImage = ms.ToArray();
image.ReleaseAccess(imageData);
}
}
}
private void UpdateColorImage(PXCMImage colorFrame)
{
// データを取得する
PXCMImage.ImageData data;
PXCMImage.ImageInfo info = colorFrame.QueryInfo();
Width = info.width;
Height = info.height;
pxcmStatus ret = colorFrame.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// Bitmapに変換する
var buffer = data.ToByteArray(0, info.width * info.height * 3);
ImageColor.Source = BitmapSource.Create(info.width, info.height, 96, 96, PixelFormats.Bgr24, null, buffer, info.width * 3);
// データを解放する
colorFrame.ReleaseAccess(data);
}
private void UpdateBlobImage(PXCMImage depthFrame)
{
if (depthFrame == null)
{
return;
}
// Blobを更新する
var sts = blobData.Update();
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// Blobのための画像オブジェクトを作成する
var depthInfo = depthFrame.QueryInfo();
depthInfo.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
var session = senseManager.QuerySession();
var blobImage = session.CreateImage(depthInfo);
// 表示用画像を初期化する
Array.Clear(imageBuffer, 0, imageBuffer.Length);
CanvasHandParts.Children.Clear();
// Blobを取得する
int numOfBlobs = blobData.QueryNumberOfBlobs();
for (int i = 0; i < numOfBlobs; ++i)
{
// Blobデータを取得する
PXCMBlobData.IBlob blob;
sts = blobData.QueryBlobByAccessOrder(i,
PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, out blob);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
sts = blob.QuerySegmentationImage(out blobImage);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// Blob画像を取得する
PXCMImage.ImageData data;
sts = blobImage.AcquireAccess(PXCMImage.Access.ACCESS_READ,
depthInfo.format, out data);
if (sts < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
continue;
}
// データをコピーする
var buffer = data.ToByteArray(0, data.pitches[0] * depthInfo.height);
for (int j = 0; j < depthInfo.height * depthInfo.width; ++j)
{
if (buffer[j] != 0)
{
imageBuffer[j] = (byte)((i + 1) * 64);
}
}
// Blob画像を解放する
blobImage.ReleaseAccess(data);
// Blobの輪郭を表示する
UpdateContoursImage(blob, i);
}
// Blob画像オブジェクトを解放する
blobImage.Dispose();
// ピクセルデータを更新する
imageBitmap.WritePixels(imageRect, imageBuffer,
DEPTH_WIDTH * BYTE_PER_PIXEL, 0);
}
// Depth画像を更新する
private void UpdateDepthImage( PXCMImage depthFrame )
{
if ( depthFrame == null ) {
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = depthFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data );
if ( ret < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
throw new Exception( "Depth画像の取得に失敗" );
}
// Bitmapに変換する
var info = depthFrame.QueryInfo();
var length = data.pitches[0] * info.height;
var buffer = data.ToByteArray( 0, length );
ImageHand.Source = BitmapSource.Create( info.width, info.height, 96, 96,
PixelFormats.Bgr32, null, buffer, data.pitches[0] );
// データを解放する
depthFrame.ReleaseAccess( data );
}
void Update()
{
//Dynamically Pause/Enable Modules
int numberOfEnabledModules = 0;
foreach (var option in _senseOptions)
{
if (option.RefCounter == 0 && option.Enabled)
{
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, true);
}
option.Enabled = false;
}
else if (option.RefCounter > 0 && !option.Enabled)
{
if (!option.Initialized)
{
OnDisable();
OnEnable();
Start();
}
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, false);
}
option.Enabled = true;
}
if (option.Enabled)
{
numberOfEnabledModules++;
}
}
//Update Speech commands if changed
if (_speechCommandsChanged)
{
UpdateSpeechCommands();
SpeechManager.Reset();
}
// Every frame update all the data
if (Initialized && numberOfEnabledModules > 0)
{
_sts = SenseManager.AcquireFrame(true, 100);
if (_sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoColorStream).Enabled)
{
if (ImageRgbOutput != null)
{
ImageRgbOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.color != null)
{
ImageRgbOutput = _captureSample.color;
ImageRgbOutput.QueryInstance <PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoDepthStream).Enabled ||
_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (ImageDepthOutput != null)
{
ImageDepthOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.depth != null)
{
ImageDepthOutput = _captureSample.depth;
ImageDepthOutput.QueryInstance <PXCMAddRef>().AddRef();
if (!_isInitBlob)
{
PXCMImage.ImageInfo info = ImageDepthOutput.QueryInfo();
BlobExtractor.Init(info);
BlobExtractor.SetMaxBlobs(MaxBlobsToDetect);
_isInitBlob = true;
}
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
_sts = Projection.QueryVertices(ImageDepthOutput, PointCloud);
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.UVMap).Enabled)
{
if (UvMap == null)
{
UvMap = new PXCMPointF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
Projection.QueryUVMap(ImageDepthOutput, UvMap);
}
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoIRStream).Enabled)
{
if (ImageIROutput != null)
{
ImageIROutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.ir != null)
{
ImageIROutput = _captureSample.ir;
ImageIROutput.QueryInstance <PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoSegmentation).Enabled)
{
if (Image3DSegmentationOutput != null)
{
Image3DSegmentationOutput.Dispose();
}
PXCM3DSeg seg = SenseManager.Query3DSeg();
if (seg != null)
{
Image3DSegmentationOutput = seg.AcquireSegmentedImage();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Face).Enabled)
{
FaceModuleOutput.Update();
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Hand).Enabled)
{
HandDataOutput.Update();
}
_captureSample = null;
SenseManager.ReleaseFrame();
}
//Speech
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Speech).Enabled)
{
SpeechManager.QueryRecognizedCommands(out SpeechOutput);
}
}
}
private void UpdateSegmentationImage( PXCMImage segmentationImage )
{
if ( segmentationImage == null ) {
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = segmentationImage.AcquireAccess( PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data );
if ( ret < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
return;
}
// ピクセルデータを初期化する
Array.Clear( imageBuffer, 0, imageBuffer.Length );
// セグメンテーション画像をバイト列に変換する
var info = segmentationImage.QueryInfo();
var buffer = data.ToByteArray( 0, data.pitches[0] * info.height );
for ( int i = 0; i < (info.height * info.width); ++i ) {
var index = i * BYTE_PER_PIXEL;
// α値が0でない場合には有効な場所として色をコピーする
if ( buffer[index + 3] != 0 ) {
imageBuffer[index + 0] = buffer[index + 0];
imageBuffer[index + 1] = buffer[index + 1];
imageBuffer[index + 2] = buffer[index + 2];
imageBuffer[index + 3] = 255;
}
// α値が0の場合は、ピクセルデータのα値を0にする
else {
imageBuffer[index + 3] = 0;
}
}
// ピクセルデータを更新する
imageBitmap.WritePixels( imageRect, imageBuffer, data.pitches[0], 0 );
// データを解放する
segmentationImage.ReleaseAccess( data );
}
/* Displaying Depth/Mask Images - for depth image only we use a delay of NumberOfFramesToDelay to sync image with tracking */
private unsafe void DisplayPicture(PXCMImage depth, PXCMHandData handAnalysis)
{
if (depth == null)
{
return;
}
PXCMImage image = depth;
//Mask Image
if (form.GetLabelmapState())
{
Bitmap labeledBitmap = null;
try
{
labeledBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
}
catch (Exception)
{
image.Dispose();
return;
}
for (int j = 0; j < handAnalysis.QueryNumberOfHands(); j++)
{
int id;
PXCMImage.ImageData data;
handAnalysis.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out id);
//Get hand by time of appearance
PXCMHandData.IHand handData;
handAnalysis.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out handData);
if (handData != null &&
(handData.QuerySegmentationImage(out image) >= pxcmStatus.PXCM_STATUS_NO_ERROR))
{
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8,
out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = labeledBitmap.LockBits(rect, ImageLockMode.ReadWrite, labeledBitmap.PixelFormat);
byte * numPtr = (byte *)bitmapdata.Scan0; //dst
byte * numPtr2 = (byte *)data.planes[0]; //row
int imagesize = image.info.width * image.info.height;
byte num2 = (byte)handData.QueryBodySide();
byte tmp = 0;
for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
{
tmp = (byte)(LUT[numPtr2[0]] * num2 * 100);
numPtr[0] = (Byte)(tmp | numPtr[0]);
numPtr[1] = (Byte)(tmp | numPtr[1]);
numPtr[2] = (Byte)(tmp | numPtr[2]);
numPtr[3] = 0xff;
}
bool isError = false;
try
{
labeledBitmap.UnlockBits(bitmapdata);
}
catch (Exception)
{
isError = true;
}
try
{
image.ReleaseAccess(data);
}
catch (Exception)
{
isError = true;
}
if (isError)
{
labeledBitmap.Dispose();
image.Dispose();
return;
}
}
}
}
if (labeledBitmap != null)
{
form.DisplayBitmap(labeledBitmap);
labeledBitmap.Dispose();
}
image.Dispose();
}//end label image
//Depth Image
else
{
//collecting 3 images inside a queue and displaying the oldest image
PXCMImage.ImageInfo info;
PXCMImage image2;
info = image.QueryInfo();
image2 = form.g_session.CreateImage(info);
image2.CopyImage(image);
m_images.Enqueue(image2);
if (m_images.Count == NumberOfFramesToDelay)
{
Bitmap depthBitmap;
try
{
depthBitmap = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
}
catch (Exception)
{
image.Dispose();
PXCMImage queImage = m_images.Dequeue();
queImage.Dispose();
return;
}
PXCMImage.ImageData data3;
PXCMImage image3 = m_images.Dequeue();
if (image3.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data3) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
float fMaxValue = _maxRange;
byte cVal;
Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
BitmapData bitmapdata = depthBitmap.LockBits(rect, ImageLockMode.ReadWrite, depthBitmap.PixelFormat);
byte * pDst = (byte *)bitmapdata.Scan0;
short *pSrc = (short *)data3.planes[0];
int size = image.info.width * image.info.height;
for (int i = 0; i < size; i++, pSrc++, pDst += 4)
{
cVal = (byte)((*pSrc) / fMaxValue * 255);
if (cVal != 0)
{
cVal = (byte)(255 - cVal);
}
pDst[0] = cVal;
pDst[1] = cVal;
pDst[2] = cVal;
pDst[3] = 255;
}
try
{
depthBitmap.UnlockBits(bitmapdata);
}
catch (Exception)
{
image3.ReleaseAccess(data3);
depthBitmap.Dispose();
image3.Dispose();
return;
}
form.DisplayBitmap(depthBitmap);
image3.ReleaseAccess(data3);
}
depthBitmap.Dispose();
image3.Dispose();
}
}
}
void Update()
{
//Dynamically Pause/Enable Modules
int numberOfEnabledModules = 0;
foreach (var option in _senseOptions)
{
if (option.RefCounter == 0 && option.Enabled )
{
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, true);
}
option.Enabled = false;
}
else if (option.RefCounter > 0 && !option.Enabled)
{
if (!option.Initialized)
{
OnDisable();
OnEnable();
Start();
}
if (option.ModuleCUID > 0)
{
SenseManager.PauseModule(option.ModuleCUID, false);
}
option.Enabled = true;
}
if (option.Enabled)
{
numberOfEnabledModules++;
}
}
//Update Speech commands if changed
if (_speechCommandsChanged)
{
UpdateSpeechCommands();
SpeechManager.Reset();
}
// Every frame update all the data
if (Initialized && numberOfEnabledModules > 0)
{
_sts = SenseManager.AcquireFrame(true, 100);
if (_sts == pxcmStatus.PXCM_STATUS_NO_ERROR)
{
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoColorStream).Enabled)
{
if (ImageRgbOutput != null)
{
ImageRgbOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.color != null)
{
ImageRgbOutput = _captureSample.color;
ImageRgbOutput.QueryInstance<PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoDepthStream).Enabled ||
_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (ImageDepthOutput != null)
{
ImageDepthOutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.depth != null)
{
ImageDepthOutput = _captureSample.depth;
ImageDepthOutput.QueryInstance<PXCMAddRef>().AddRef();
if (!_isInitBlob)
{
PXCMImage.ImageInfo info = ImageDepthOutput.QueryInfo();
BlobExtractor.Init(info);
BlobExtractor.SetMaxBlobs(MaxBlobsToDetect);
_isInitBlob = true;
}
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.PointCloud).Enabled)
{
if (PointCloud == null)
{
PointCloud = new PXCMPoint3DF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
_sts = Projection.QueryVertices(ImageDepthOutput, PointCloud);
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.UVMap).Enabled)
{
if (UvMap == null)
{
UvMap = new PXCMPointF32[ImageDepthOutput.info.width * ImageDepthOutput.info.height];
}
Projection.QueryUVMap(ImageDepthOutput, UvMap);
}
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoIRStream).Enabled)
{
if (ImageIROutput != null)
{
ImageIROutput.Dispose();
}
if (_captureSample == null)
{
_captureSample = SenseManager.QuerySample();
}
if (_captureSample.ir != null)
{
ImageIROutput = _captureSample.ir;
ImageIROutput.QueryInstance<PXCMAddRef>().AddRef();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.VideoSegmentation).Enabled)
{
if (Image3DSegmentationOutput != null)
{
Image3DSegmentationOutput.Dispose();
}
PXCM3DSeg seg = SenseManager.Query3DSeg();
if (seg != null)
{
Image3DSegmentationOutput = seg.AcquireSegmentedImage();
}
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Face).Enabled)
{
FaceModuleOutput.Update();
}
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Hand).Enabled)
{
HandDataOutput.Update();
}
_captureSample = null;
SenseManager.ReleaseFrame();
}
//Speech
if (_senseOptions.Find(i => i.ID == SenseOption.SenseOptionID.Speech).Enabled)
{
SpeechManager.QueryRecognizedCommands(out SpeechOutput);
}
}
}
private void UpdateColorImage(PXCMImage colorFrame)
{
if (colorFrame != null)
{
PXCMImage.ImageData data = null;
var ret = colorFrame.AcquireAccess(
PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24,
out data);
if (ret >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
var info = colorFrame.QueryInfo();
var length = data.pitches[0] * info.height;
var buffer = data.ToByteArray(0, length);
this.ColorImageElement = BitmapSource.Create(
info.width,
info.height,
96,
96,
PixelFormats.Bgr24,
null,
buffer,
data.pitches[0]);
colorFrame.ReleaseAccess(data);
}
}
}
private void UpdateBlobImage( PXCMImage depthFrame )
{
if ( depthFrame == null ){
return;
}
// Blobを更新する
var sts = blobData.Update();
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
return;
}
// Blobのための画像オブジェクトを作成する
var depthInfo = depthFrame.QueryInfo();
depthInfo.format = PXCMImage.PixelFormat.PIXEL_FORMAT_Y8;
var session = senseManager.QuerySession();
var blobImage = session.CreateImage( depthInfo );
// 表示用画像を初期化する
Array.Clear( imageBuffer, 0, imageBuffer.Length );
CanvasHandParts.Children.Clear();
// Blobを取得する
int numOfBlobs = blobData.QueryNumberOfBlobs();
for ( int i = 0; i < numOfBlobs; ++i ) {
// Blobデータを取得する
PXCMBlobData.IBlob blob;
sts = blobData.QueryBlobByAccessOrder( i,
PXCMBlobData.AccessOrderType.ACCESS_ORDER_NEAR_TO_FAR, out blob);
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
sts = blob.QuerySegmentationImage( out blobImage );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// Blob画像を取得する
PXCMImage.ImageData data;
sts = blobImage.AcquireAccess( PXCMImage.Access.ACCESS_READ,
depthInfo.format, out data );
if ( sts < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
continue;
}
// データをコピーする
var buffer = data.ToByteArray( 0, data.pitches[0] * depthInfo.height );
for ( int j = 0; j < depthInfo.height * depthInfo.width; ++j ){
if ( buffer[j] != 0 ) {
imageBuffer[j] = (byte)((i + 1) * 64);
}
}
// Blob画像を解放する
blobImage.ReleaseAccess( data );
// Blobの輪郭を表示する
UpdateContoursImage( blob, i );
}
// Blob画像オブジェクトを解放する
blobImage.Dispose();
// ピクセルデータを更新する
imageBitmap.WritePixels( imageRect, imageBuffer,
DEPTH_WIDTH * BYTE_PER_PIXEL, 0 );
}
public byte[] ColorToDepthCoordinatesByInvUVMap(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (projection == null || cpixels == null) return cpixels;
if (dots >= 9)
{ // A sample for CreateDepthImageMappedToColor output visualization
PXCMImage.ImageData d2cDat;
PXCMImage d2c = projection.CreateDepthImageMappedToColor(depth, color);
if (d2c == null)
{
return cpixels;
}
UInt16[] d2cpixels;
if (d2c.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out d2cDat) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 d2cwidth = d2cDat.pitches[0] / sizeof(Int16); /* aligned width */
Int32 d2cheight = (Int32)d2c.info.height;
d2cpixels = d2cDat.ToUShortArray(0, d2cwidth * d2cheight);
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
if (d2cpixels[y * d2cwidth + x] == invalid_value) continue; // no mapping based on unreliable depth values
cpixels[(y * cwidth + x) * 4] = 0xFF;
}
}
d2c.ReleaseAccess(d2cDat);
}
d2c.Dispose();
return cpixels;
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
Int16[] dpixels;
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToShortArray(0, dpitch * dheight);
pxcmStatus sts = projection.QueryInvUVMap(depth, invuvmap);
Int32 invuvpitch = color.QueryInfo().width;
depth.ReleaseAccess(ddata);
if (dots > 1)
{ // If Depth data is valid just set a blue pixel
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
Int32 xx = (Int32)(invuvmap[y * cwidth + x].x * dwidth);
Int32 yy = (Int32)(invuvmap[y * cwidth + x].y * dheight);
if (xx >= 0 && yy >= 0)
{
if (dpixels[yy * dpitch + xx] > 0)
{
cpixels[(y * cwidth + x) * 4] = 0xFF;
}
}
}
}
}
else
{ // If Depth data is valid just set a blue pixel with briteness depends on Depth value
Int32 MAX_LOCAL_DEPTH_VALUE = 4000;
Int32[] depth_hist = new Int32[MAX_LOCAL_DEPTH_VALUE];
Array.Clear(depth_hist, 0, depth_hist.Length);
Int32 num_depth_points = 0;
for (Int32 y = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++)
{
Int16 d = dpixels[y * dpitch + x];
if (d > 0 && d < MAX_LOCAL_DEPTH_VALUE)
{
depth_hist[d]++;
num_depth_points++;
}
}
}
if (num_depth_points > 0)
{
for (Int32 i = 1; i < MAX_LOCAL_DEPTH_VALUE; i++)
{
depth_hist[i] += depth_hist[i - 1];
}
for (Int32 i = 1; i < MAX_LOCAL_DEPTH_VALUE; i++)
{
depth_hist[i] = 255 - (Int32)((float)255 * (float)depth_hist[i] / (float)num_depth_points);
}
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
Int32 xx = (Int32)(invuvmap[y * cwidth + x].x * dwidth);
Int32 yy = (Int32)(invuvmap[y * cwidth + x].y * dheight);
if (xx >= 0 && yy >= 0)
{
Int16 d = dpixels[yy * dpitch + xx];
if (d > 0 && d < MAX_LOCAL_DEPTH_VALUE)
{
cpixels[(y * cwidth + x) * 4] = (byte)depth_hist[d];
}
}
}
}
}
}
}
return cpixels;
}
public byte[] DepthToColorCoordinatesByUVMAP(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (cpixels == null) return cpixels;
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
// float[] uvmap;
bool isdepth = (depth.info.format == PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH);
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(short); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
projection.QueryUVMap(depth, uvmap);
int uvpitch = depth.QueryInfo().width;
depth.ReleaseAccess(ddata);
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++)
{
UInt16 d = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
if (d == invalid_value) continue; // no mapping based on unreliable depth values
float uvx = uvmap[y * uvpitch + x].x, uvy = uvmap[y * uvpitch + x].y;
Int32 xx = (Int32)(uvx * cwidth), yy = (Int32)(uvy * cheight);
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 1);
}
}
}
return cpixels;
}
public byte[] ColorToDepthCoordinatesByInvUVMap(PXCMImage color, PXCMImage depth, int dots, out int cwidth, out int cheight)
{
/* Retrieve the color pixels */
byte[] cpixels = color.GetRGB32Pixels(out cwidth, out cheight);
if (projection == null || cpixels == null)
{
return(cpixels);
}
if (dots >= 9)
{ // A sample for CreateDepthImageMappedToColor output visualization
PXCMImage.ImageData d2cDat;
PXCMImage d2c = projection.CreateDepthImageMappedToColor(depth, color);
if (d2c == null)
{
return(cpixels);
}
UInt16[] d2cpixels;
if (d2c.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out d2cDat) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 d2cwidth = d2cDat.pitches[0] / sizeof(Int16); /* aligned width */
Int32 d2cheight = (Int32)d2c.info.height;
d2cpixels = d2cDat.ToUShortArray(0, d2cwidth * d2cheight);
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
if (d2cpixels[y * d2cwidth + x] == invalid_value)
{
continue; // no mapping based on unreliable depth values
}
cpixels[(y * cwidth + x) * 4] = 0xFF;
}
}
d2c.ReleaseAccess(d2cDat);
}
d2c.Dispose();
return(cpixels);
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
Int16[] dpixels;
if (depth.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out ddata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
Int32 dpitch = ddata.pitches[0] / sizeof(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToShortArray(0, dpitch * dheight);
pxcmStatus sts = projection.QueryInvUVMap(depth, invuvmap);
Int32 invuvpitch = color.QueryInfo().width;
depth.ReleaseAccess(ddata);
if (dots > 1)
{ // If Depth data is valid just set a blue pixel
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
Int32 xx = (Int32)(invuvmap[y * cwidth + x].x * dwidth);
Int32 yy = (Int32)(invuvmap[y * cwidth + x].y * dheight);
if (xx >= 0 && yy >= 0)
{
if (dpixels[yy * dpitch + xx] > 0)
{
cpixels[(y * cwidth + x) * 4] = 0xFF;
}
}
}
}
}
else
{ // If Depth data is valid just set a blue pixel with briteness depends on Depth value
Int32 MAX_LOCAL_DEPTH_VALUE = 4000;
Int32[] depth_hist = new Int32[MAX_LOCAL_DEPTH_VALUE];
Array.Clear(depth_hist, 0, depth_hist.Length);
Int32 num_depth_points = 0;
for (Int32 y = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++)
{
Int16 d = dpixels[y * dpitch + x];
if (d > 0 && d < MAX_LOCAL_DEPTH_VALUE)
{
depth_hist[d]++;
num_depth_points++;
}
}
}
if (num_depth_points > 0)
{
for (Int32 i = 1; i < MAX_LOCAL_DEPTH_VALUE; i++)
{
depth_hist[i] += depth_hist[i - 1];
}
for (Int32 i = 1; i < MAX_LOCAL_DEPTH_VALUE; i++)
{
depth_hist[i] = 255 - (Int32)((float)255 * (float)depth_hist[i] / (float)num_depth_points);
}
/* Draw dots onto the color pixels */
for (Int32 y = 0; y < cheight; y++)
{
for (Int32 x = 0; x < cwidth; x++)
{
Int32 xx = (Int32)(invuvmap[y * cwidth + x].x * dwidth);
Int32 yy = (Int32)(invuvmap[y * cwidth + x].y * dheight);
if (xx >= 0 && yy >= 0)
{
Int16 d = dpixels[yy * dpitch + xx];
if (d > 0 && d < MAX_LOCAL_DEPTH_VALUE)
{
cpixels[(y * cwidth + x) * 4] = (byte)depth_hist[d];
}
}
}
}
}
}
}
return(cpixels);
}
private void UpdateColorImage(PXCMImage colorFrame)
{
// データを取得する
PXCMImage.ImageData data;
PXCMImage.ImageInfo info = colorFrame.QueryInfo();
Width = info.width;
Height = info.height;
pxcmStatus ret = colorFrame.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24, out data);
if (ret < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
// Bitmapに変換する
var buffer = data.ToByteArray(0, info.width * info.height * 3);
ImageColor.Source = BitmapSource.Create(info.width, info.height, 96, 96, PixelFormats.Bgr24, null, buffer, info.width * 3);
// データを解放する
colorFrame.ReleaseAccess(data);
}
// Depth(距離)データを更新する
private void UpdateDepthData( PXCMImage depthFrame )
{
if ( depthFrame == null ) {
return;
}
// データを取得する
PXCMImage.ImageData data;
pxcmStatus ret = depthFrame.AcquireAccess( PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data );
if ( ret < pxcmStatus.PXCM_STATUS_NO_ERROR ) {
throw new Exception( "Depth画像の取得に失敗" );
}
// Depthデータを取得する
var info = depthFrame.QueryInfo();
depthBuffer = data.ToShortArray( 0, info.width * info.height );
// データを解放する
depthFrame.ReleaseAccess( data );
}