private void UpdateColorImage(PXCMImage colorFrame)
{
// データを取得する
PXCMImage.ImageData data;
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, colorFrame.info.width * colorFrame.info.height * 3);
ImageColor.Source = BitmapSource.Create(
colorFrame.info.width, colorFrame.info.height, 96, 96,
PixelFormats.Bgr24, null, buffer, colorFrame.info.width * 3);
ImageColor.Width = colorFrame.info.width;
ImageColor.Height = colorFrame.info.height;
// データを解放する
colorFrame.ReleaseAccess(data);
}
private void Update()
{
PXCMImage sample = null;
lock (this)
{
if (m_sample == null)
{
return;
}
sample = m_sample;
m_sample = null;
}
// display the color image
PXCMImage.ImageData data;
pxcmStatus sts = sample.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
mBlockProfiler.BeginBlock();
data.ToTexture2D(0, m_texColor);
mBlockProfiler.EndBlock();
sample.ReleaseAccess(data);
m_texColor.Apply();
}
sample.Dispose();
}
/// <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);
}
private void DisplayPicture(PXCMImage depth, PXCMGesture gesture)
{
PXCMImage image = depth;
bool dispose = false;
if (form.GetLabelmapState())
{
if (gesture.QueryBlobImage(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out image) < pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
dispose = true;
}
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imageInfo = image.imageInfo;
form.DisplayBitmap(data.ToBitmap(imageInfo.width, imageInfo.height));
image.ReleaseAccess(ref data);
}
if (dispose)
{
image.Dispose();
}
}
/// <summary>
/// take a single depth grey color image shot
/// </summary>
/// <returns></returns>
public WriteableBitmap DepthSnapshot()
{
for (; ;)
{
if (senseManager.AcquireFrame(true).IsError())
{
throw new Exception("Failed to acquire frame");
break;
}
else
{
// capture
PXCMCapture.Sample sample = senseManager.QuerySample();
PXCMImage MyImg = sample.depth;
//test
PXCMImage.ImageData MyImgData;
MyImg.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out MyImgData);
WriteableBitmap WBmp = MyImgData.ToWritableBitmap(0, MyImg.info.width, MyImg.info.height, 96, 96);
// release
senseManager.ReleaseFrame();
return(WBmp);
//break;
}
}
////should be used if we want to shut down the cam .
//senseManager.Dispose();
return(null);
}
public static Bitmap GetRgb32Pixels(PXCMImage image)
{
var cwidth = Align16(image.info.width); /* aligned width */
var cheight = (int)image.info.height;
PXCMImage.ImageData cdata;
byte[] cpixels;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out cdata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
cpixels = cdata.ToByteArray(0, cwidth * cheight * 4);
image.ReleaseAccess(ref cdata);
}
else
{
cpixels = new byte[cwidth * cheight * 4];
}
var width = (int)image.info.width;
var height = (int)image.info.height;
Bitmap bitmap;
lock (image)
{
bitmap = new Bitmap(width, height, PixelFormat.Format32bppRgb);
var data = bitmap.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.WriteOnly, PixelFormat.Format32bppRgb);
Marshal.Copy(cpixels, 0, data.Scan0, width * height * 4);
bitmap.UnlockBits(data);
}
return(bitmap);
}
/// <summary>
/// Get UV Map of a PXCMImage.
/// </summary>
/// <param name="image">PXCMImage</param>
/// <returns>UVMap as EmguCV image.</returns>
public static Image <Rgb, float> GetDepthUvMap(PXCMImage image)
{
var inputWidth = (int)image.info.width;
var inputHeight = (int)image.info.height;
var uvMap = new Image <Rgb, float>(inputWidth, inputHeight);
PXCMImage.ImageData cdata;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) <
pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(uvMap);
}
var uv = new float[inputHeight * inputWidth * 2];
// read UV
var pData = cdata.buffer.planes[2];
Marshal.Copy(pData, uv, 0, inputWidth * inputHeight * 2);
image.ReleaseAccess(ref cdata);
Parallel.For(0, uv.Length / 2, i =>
{
var j = i * 2;
//Console.WriteLine(j + ": " + uv[j] * 255.0 + ", " + uv[j + 1] * 255.0);
uvMap[(j / 2) / inputWidth, (j / 2) % inputWidth] = new Rgb(uv[j] * 255.0, uv[j + 1] * 255.0, 0);
});
return(uvMap);
}
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);
}
}
}
/// <summary>
/// Sets the texture for the associated game object with the given image
/// </summary>
/// <param name='image'>
/// Image.
/// </param>
private void SetTexture(PXCMImage image)
{
if (image == null)
{
return;
}
if (_texture==null) {
/* Save size and preallocate the Texture2D */
size.width=image.info.width;
size.height=image.info.height;
_texture = new Texture2D((int)size.width, (int)size.height, TextureFormat.ARGB32, false);
/* Associate the texture to the game object */
GetComponent<Renderer>().sharedMaterial.mainTexture = _texture;
}
/* Retrieve the image data */
PXCMImage.ImageData data;
pxcmStatus sts=image.AcquireAccess(PXCMImage.Access.ACCESS_READ,PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32,out data);
if ( sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
data.ToTexture2D(0, _texture);
image.ReleaseAccess(data);
/* and display it */
_texture.Apply();
}
}
/// <summary>
/// Sets the texture for the associated game object with the given image
/// </summary>
/// <param name='image'>
/// Image.
/// </param>
private void SetTexture(PXCMImage image)
{
if (image == null)
{
return;
}
if (_texture == null)
{
/* Save size and preallocate the Texture2D */
size.width = image.info.width;
size.height = image.info.height;
_texture = new Texture2D((int)size.width, (int)size.height, TextureFormat.ARGB32, false);
/* Associate the texture to the game object */
GetComponent <Renderer>().sharedMaterial.mainTexture = _texture;
}
/* Retrieve the image data */
PXCMImage.ImageData data;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
data.ToTexture2D(0, _texture);
image.ReleaseAccess(data);
/* and display it */
_texture.Apply();
}
}
// 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);
}
public static void CopyToByteArray(this PXCMImage image, byte[] data, bool for_preview = false)
{
PXCMImage.ImageData cdata;
PXCMImage.PixelFormat pixelFormat = PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32;
if (!for_preview && (image.streamType == PXCMCapture.StreamType.STREAM_TYPE_DEPTH))
{
pixelFormat = PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH;
}
else if (image.streamType == PXCMCapture.StreamType.STREAM_TYPE_LEFT || image.streamType == PXCMCapture.StreamType.STREAM_TYPE_RIGHT)
{
pixelFormat = PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH;
}
pxcmStatus status = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, pixelFormat, out cdata);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidOperationException("AcquireAccess failed");
}
//cdata.ToByteArray(0, data);
//Pitches is the real stride width the sensor returns. it's 640 for the current resolution
byte[] rawData = cdata.ToByteArray(0, cdata.pitches[0] * image.info.height);
Buffer.BlockCopy(rawData, 0, data, 0, rawData.Length);
image.ReleaseAccess(cdata);
}
// 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);
}
private void Button_Click(object sender, RoutedEventArgs e)
{
// Get instance of SenseManager
PXCMSession session = PXCMSession.CreateInstance();
// Get RS version
PXCMSession.ImplVersion version = session.QueryVersion();
textBox1.Text = version.major.ToString() + "." + version.minor.ToString();
// setup Pipeline
PXCMSenseManager sm = session.CreateSenseManager();
// Get streams ready
sm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, 640, 480);
sm.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, 640, 480);
// Init Pipeline
sm.Init();
// Get samples
pxcmStatus status = sm.AcquireFrame(true); // Synchronous capturing
PXCMCapture.Sample sample = sm.QuerySample();
// Convert samples to image
PXCMImage image = sample.color;
PXCMImage dimage = sample.depth;
PXCMImage.ImageData data;
image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data);
WriteableBitmap wbm = data.ToWritableBitmap(0,
image.info.width,
image.info.height,
96.0, 96.0);
PXCMImage.ImageData data2;
dimage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH_RAW, out data2);
WriteableBitmap wbm2 = data2.ToWritableBitmap(
0,
dimage.info.width,
dimage.info.height,
96.0, 96.0);
// Display image
imageRGB.Source = wbm;
imageDepth.Source = wbm2;
// Clean up
image.ReleaseAccess(data);
image.ReleaseAccess(data2);
sm.ReleaseFrame();
sm.Close();
session.Dispose();
}
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.DisplayBitmap(data.ToBitmap(image.info.width, image.info.height));
image.ReleaseAccess(ref data);
}
}
public static IImage[] GetHighPrecisionDepthImage(PXCMImage depthImage, float minValue, float maxValue)
{
var inputWidth = Align16(depthImage.info.width); /* aligned width */
var inputHeight = (int)depthImage.info.height;
var returnImages = new IImage[2];
returnImages[0] = new Image <Gray, float>(inputWidth, inputHeight);
returnImages[1] = new Image <Rgb, Byte>(inputWidth, inputHeight);
PXCMImage.ImageData cdata;
if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) <
pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(returnImages);
}
var depthValues = cdata.ToShortArray(0, inputWidth * inputHeight);
depthImage.ReleaseAccess(ref cdata);
var depthReturnImage = ((Image <Gray, float>)returnImages[0]);
var confidenceReturnImage = ((Image <Rgb, Byte>)returnImages[1]);
var depthReturnImageData = depthReturnImage.Data;
var confidenceReturnImageData = confidenceReturnImage.Data;
Parallel.For(0, inputHeight, y =>
{
for (int x = 0; x < inputWidth; x++)
{
float depth = depthValues[y * inputWidth + x];
if (depth != EmguExtensions.LowConfidence && depth != EmguExtensions.Saturation)
{
var test = (depth - minValue) / (maxValue - minValue);
if (test < 0)
{
test = 0.0f;
}
else if (test > 1.0)
{
test = 1.0f;
}
test *= 255.0f;
depthReturnImageData[y, x, 0] = test;
}
else
{
depthReturnImageData[y, x, 0] = depth;
confidenceReturnImageData[y, x, 0] = 255;
}
}
});
return(returnImages);
}
private void DisplayPicture(PXCMImage image)
{
//change
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data) <
pxcmStatus.PXCM_STATUS_NO_ERROR) return;
m_form.DrawBitmap(data.ToBitmap(0, image.info.width, image.info.height));
m_timer.Tick("");
image.ReleaseAccess(data);
}
/// <summary>
/// 将图像缓存入m_form中的m_bitmap中
/// </summary>
/// <param name="image"></param>
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data) <
pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return;
}
m_form.DrawBitmap(data.ToBitmap(0, image.info.width, image.info.height));
image.ReleaseAccess(data);
}
public byte[] DepthToColorCoordinatesByFunction(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);
}
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
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(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
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
}
Int32 xx = (Int32)ccords[k].x, yy = (Int32)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return(cpixels);
}
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24, out data);
if (sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.DisplayBitmap(data.ToBitmap(0, image.info.width, image.info.height));
timer.Tick("");
image.ReleaseAccess(data);
}
}
private static UInt16[] getUInt16Depths(PXCMImage depthImage)
{
PXCMImage.ImageData depthImageData;
UInt16[] pixelDepths = null;
if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out depthImageData) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
int width = (int)depthImageData.pitches[0] / sizeof(Int16);
int height = (int)depthImage.info.height;
pixelDepths = depthImageData.ToUShortArray(0, width * height);
depthImage.ReleaseAccess(depthImageData);
}
return(pixelDepths);
}
private Bitmap ToBitmap(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ_WRITE, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out data) <
pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(null);
}
var result = data.ToBitmap(0, image.info.width, image.info.height);
image.ReleaseAccess(data);
return(result);
}
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
PXCMImage.ImageInfo imageInfo = image.imageInfo;
var bmp = data.ToBitmap(imageInfo.width, imageInfo.height);
if (ImageAvailable != null)
{
ImageAvailable(bmp);
}
image.ReleaseAccess(ref data);
}
}
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);
}
void createDepthMesh()
{
PXCMImage depthImage = getCameraSample().depth;
Texture2D tempTexture = new Texture2D(depthImage.info.width, depthImage.info.height, TextureFormat.ARGB32, false);
PXCMImage.ImageData imageData;
depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out imageData);
imageData.ToTexture2D(0, tempTexture);
depthImage.ReleaseAccess(imageData);
Mesh depthMesh = depthMeshObject.GetComponent <MeshFilter>().mesh;
depthMesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
depthMesh.Clear();
int width = tempTexture.width;
int height = tempTexture.height;
Vector3[] verts = new Vector3[(width + 1) * (height + 1)];
//Vector2[] uvs = new Vector2[verts.Length];
//Vector4[] tangents = new Vector4[verts.Length];
//Vector4 tangent = new Vector4(0, 0, -1.0f, -1.0f);
for (int i = 0, y = 0; y <= height; y++)
{
for (int x = 0; x <= width; x++, i++)
{
verts[i] = new Vector3(-width * 0.5f + x, -height * 0.5f + y, -tempTexture.GetPixel(x, y).grayscale * 100.0f) / 10.0f;
//uvs[i] = new Vector2(x / (float)width, y / (float)height);
//tangents[i] = tangent;
}
}
depthMesh.vertices = verts;
//depthMesh.uv = uvs;
//depthMesh.tangents = tangents;
int[] triangles = new int[width * height * 6];
for (int ti = 0, vi = 0, y = 0; y < height; y++, vi++)
{
for (int x = 0; x < width; x++, ti += 6, vi++)
{
triangles[ti] = vi;
triangles[ti + 3] = triangles[ti + 2] = vi + 1;
triangles[ti + 4] = triangles[ti + 1] = vi + width + 1;
triangles[ti + 5] = vi + width + 2;
}
}
depthMesh.triangles = triangles;
depthMesh.RecalculateNormals();
}
public unsafe override bool OnNewFrame()
{
/******************************RGB********************************/
PXCMImage rgbPXCMImage = QueryImage(PXCMImage.ImageType.IMAGE_TYPE_COLOR);
PXCMImage.ImageData rgbData;
rgbPXCMImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, out rgbData);
if (rgbImageByte == null)
{
rgbWitdh = rgbPXCMImage.info.width;
rgbHeight = rgbPXCMImage.info.height;
rgbImageByte = new byte[rgbWitdh * rgbHeight * 3];
}
byte *colorPointer = (byte *)(rgbData.buffer.planes[0]);
for (int i = 0; i < rgbWitdh * rgbHeight * 3; i++)
{
rgbImageByte[i] = colorPointer[i];
}
/*********************** Depth && UV *********************************/
PXCMImage pXCMIimage = QueryImage(PXCMImage.ImageType.IMAGE_TYPE_DEPTH);
PXCMImage.ImageData data;
pXCMIimage.AcquireAccess(PXCMImage.Access.ACCESS_READ, out data);
if (depthImageUInt16 == null)
{
dWitdh = pXCMIimage.info.width;
dHeight = pXCMIimage.info.height;
depthImageUInt16 = new UInt16[pXCMIimage.info.width * pXCMIimage.info.height];
}
UInt16 *depthPointer = (UInt16 *)(data.buffer.planes[0]);
for (uint i = 0; i < dWitdh * dHeight; i++)
{
depthImageUInt16[i] = depthPointer[i];
}
pXCMIimage.ReleaseAccess(ref data);
rgbPXCMImage.ReleaseAccess(ref rgbData);
this.ReleaseFrame();
if (handler != null)
{
handler(this, depthImageUInt16, rgbImageByte);
}
return(!finished);
}
public static byte[] GetRGB32Pixels(PXCMImage image, out int cwidth, out int cheight)
{
PXCMImage.ImageData cdata;
byte[] cpixels = null;
cwidth = cheight = 0;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out cdata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
cwidth = (int)cdata.pitches[0] / sizeof(Int32);
cheight = (int)image.info.height;
cpixels = cdata.ToByteArray(0, (int)cdata.pitches[0] * cheight);
image.ReleaseAccess(cdata);
}
return(cpixels);
}
//private void TrackLandmarks(PXCMFaceData.Face faceDataFace)
//{
// PXCMFaceData.LandmarksData landmarksData = faceDataFace.QueryLandmarks();
// PXCMFaceData.LandmarkPoint[] landmarkPoints;
// if (landmarksData.QueryPointsByGroup(PXCMFaceData.LandmarksGroupType.LANDMARK_GROUP_NOSE, out landmarkPoints))
// {
// foreach (var landmarkPoint in landmarkPoints)
// {
// Console.WriteLine("landmarkPoint: " + landmarkPoint.world.ToString());
// }
// }
//}
//private void TrackGaze(PXCMFaceData.Face faceDataFace)
//{
// faceDataFace.
// faceDataFace.QueryGaze().QueryGazePoint().confidence
//}
private void TrackImageData(PXCMCapture.Sample captureSample)
{
PXCMImage imageColor = captureSample.color;
PXCMImage.ImageData imageData;
pxcmStatus aquireAccessStatus = imageColor.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out imageData);
if (aquireAccessStatus >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
writeableBitmap = imageData.ToWritableBitmap(0, imageColor.info.width, imageColor.info.height, 96, 96);
writeableBitmap.Freeze();
}
imageColor.ReleaseAccess(imageData);
}
public static IImage GetDepthImage(PXCMImage depthImage, float minValue, float maxValue)
{
var inputWidth = Align16(depthImage.info.width); /* aligned width */
var inputHeight = (int)depthImage.info.height;
var returnImages = new Image <Gray, float>(inputWidth, inputHeight);
PXCMImage.ImageData cdata;
if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) <
pxcmStatus.PXCM_STATUS_NO_ERROR)
{
return(returnImages);
}
var depthValues = cdata.ToShortArray(0, inputWidth * inputHeight);
depthImage.ReleaseAccess(ref cdata);
var depthReturnImage = ((Image <Gray, float>)returnImages);
var depthReturnImageData = depthReturnImage.Data;
Parallel.For(0, inputHeight, y =>
{
for (int x = 0; x < inputWidth; x++)
{
float depth = depthValues[y * inputWidth + x];
if (depth != EmguExtensions.LowConfidence && depth != EmguExtensions.Saturation)
{
if (depth > maxValue)
{
depth = maxValue;
}
else if (depth < minValue)
{
depth = minValue;
}
depthReturnImageData[y, x, 0] = depth;
}
else
{
depthReturnImageData[y, x, 0] = depth;
}
}
});
return(returnImages);
}
public CaptureHand(CaptureForm form) : base()
{
pp = new UtilMPipeline();
pp.EnableGesture();
pp.EnableImage(PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH);
if (pp.Init())
{
pp.QueryCapture().SetFilter(PXCMCapture.Device.Property.PROPERTY_DEPTH_SMOOTHING, 100);
do
{
if (!pp.AcquireFrame(true))
{
MessageBox.Show("Failed to aquire a frame.", "Kwi-S", MessageBoxButtons.OK, MessageBoxIcon.Asterisk);
break;
}
gesture = pp.QueryGesture();
gesture.QueryBlobData(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bdata);
gesture.QueryBlobImage(PXCMGesture.Blob.Label.LABEL_SCENE, 0, out bimage);
bimage.AcquireAccess(PXCMImage.Access.ACCESS_READ, out data);
info = bimage.imageInfo;
color = (int)bdata.labelLeftHand;
form.setTextBox(color.ToString());
pp.ReleaseFrame();
} while (color == -1);
Coord[] locations = handLocation(data, (int)(info.width * info.height), (int)info.width);
bitmap = createBitmap(locations, (int)info.width, (int)info.height);
form.setImage(bitmap);
form.bitmap = bitmap;
}
}
void textureFromSample(PXCMImage camSample)
{
if (sampleTexture == null)
{
sampleTexture = new Texture2D(camSample.info.width, camSample.info.height, TextureFormat.ARGB32, false);
sampleImage.material.mainTexture = sampleTexture;
sampleImage.rectTransform.localScale = new Vector3(-1, 1, 1);
}
PXCMImage.ImageData imageData;
camSample.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out imageData);
imageData.ToTexture2D(0, sampleTexture);
camSample.ReleaseAccess(imageData);
sampleTexture.Apply();
}
public static WriteableBitmap GetImage(this PXCMImage image)
{
PXCMImage.ImageData imageData = null;
WriteableBitmap returnImage = null;
int width = 0;
int height = 0;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ,
PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32,
out imageData).IsSuccessful())
{
width = Convert.ToInt32(imageData.pitches[0] / 4);
height = image.info.height;
returnImage = imageData.ToWritableBitmap(width, height, 96, 96);
image.ReleaseAccess(imageData);
}
return(returnImage);
}
public static IImage[] GetHighPrecisionDepthImage(PXCMImage depthImage, float minValue, float maxValue)
{
var inputWidth = Align16(depthImage.info.width); /* aligned width */
var inputHeight = (int)depthImage.info.height;
var returnImages = new IImage[2];
returnImages[0] = new Image<Gray, float>(inputWidth, inputHeight);
returnImages[1] = new Image<Rgb, Byte>(inputWidth, inputHeight);
PXCMImage.ImageData cdata;
if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) <
pxcmStatus.PXCM_STATUS_NO_ERROR) return returnImages;
var depthValues = cdata.ToShortArray(0, inputWidth * inputHeight);
depthImage.ReleaseAccess(ref cdata);
var depthReturnImage = ((Image<Gray, float>)returnImages[0]);
var confidenceReturnImage = ((Image<Rgb, Byte>)returnImages[1]);
var depthReturnImageData = depthReturnImage.Data;
var confidenceReturnImageData = confidenceReturnImage.Data;
Parallel.For(0, inputHeight, y =>
{
for (int x = 0; x < inputWidth; x++)
{
float depth = depthValues[y * inputWidth + x];
if (depth != EmguExtensions.LowConfidence && depth != EmguExtensions.Saturation)
{
var test = (depth - minValue) / (maxValue - minValue);
if (test < 0)
test = 0.0f;
else if (test > 1.0)
test = 1.0f;
test *= 255.0f;
depthReturnImageData[y, x, 0] = test;
}
else
{
depthReturnImageData[y, x, 0] = depth;
confidenceReturnImageData[y, x, 0] = 255;
}
}
});
return returnImages;
}
/// <summary>
/// Get UV Map of a PXCMImage.
/// </summary>
/// <param name="image">PXCMImage</param>
/// <returns>UVMap as EmguCV image.</returns>
public static Image<Rgb, float> GetDepthUvMap(PXCMImage image)
{
var inputWidth = (int)image.info.width;
var inputHeight = (int)image.info.height;
var uvMap = new Image<Rgb, float>(inputWidth, inputHeight);
PXCMImage.ImageData cdata;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_DEPTH, out cdata) <
pxcmStatus.PXCM_STATUS_NO_ERROR) return uvMap;
var uv = new float[inputHeight * inputWidth * 2];
// read UV
var pData = cdata.buffer.planes[2];
Marshal.Copy(pData, uv, 0, inputWidth * inputHeight * 2);
image.ReleaseAccess(ref cdata);
Parallel.For(0, uv.Length / 2, i =>
{
var j = i * 2;
//Console.WriteLine(j + ": " + uv[j] * 255.0 + ", " + uv[j + 1] * 255.0);
uvMap[(j / 2) / inputWidth, (j / 2) % inputWidth] = new Rgb(uv[j] * 255.0, uv[j + 1] * 255.0, 0);
});
return uvMap;
}
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[] DepthToColorCoordinatesByFunction(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;
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
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(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
PXCMPointF32[] ccords = new PXCMPointF32[dwidth * dheight];
projection.MapDepthToColor(dcords, ccords);
/* Draw dots onto the color pixels */
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
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
Int32 xx = (Int32)ccords[k].x, yy = (Int32)ccords[k].y;
PlotXY(cpixels, xx, yy, cwidth, cheight, dots, 2);
}
}
}
return cpixels;
}
// 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 );
}
private void UpdateColorImage(PXCMImage colorFrame)
{
// データを取得する
PXCMImage.ImageData data;
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, COLOR_WIDTH * COLOR_HEIGHT * 3);
ImageColor.Source = BitmapSource.Create(COLOR_WIDTH, COLOR_HEIGHT, 96, 96, PixelFormats.Bgr24, null, buffer, COLOR_WIDTH * 3);
// データを解放する
colorFrame.ReleaseAccess(data);
}
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 );
}
private void DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
pxcmStatus sts = image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB24, out data);
if ( sts >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.DisplayBitmap(data.ToBitmap(0, image.info.width, image.info.height));
timer.Tick("");
image.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 );
}
public static byte[] GetRGB32Pixels(PXCMImage image, out int cwidth, out int cheight)
{
PXCMImage.ImageData cdata;
byte[] cpixels = null;
cwidth = cheight = 0;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_RGB32, out cdata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
cwidth = cdata.pitches[0] / sizeof(Int32);
cheight = image.info.height;
cpixels = cdata.ToByteArray(0, cdata.pitches[0] * cheight);
image.ReleaseAccess(cdata);
}
return cpixels;
}
public Bitmap ConvertImageToBitmap(PXCMImage image)
{
PXCMImage.ImageData data;
image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data);
Bitmap bitmap = new Bitmap((int)image.imageInfo.width, (int)image.imageInfo.height, data.buffer.pitches[0], PixelFormat.Format32bppRgb, data.buffer.planes[0]);
image.ReleaseAccess(ref data);
return bitmap;
}
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 DisplayPicture(PXCMImage image)
{
PXCMImage.ImageData data;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
form.DisplayBitmap(data.ToBitmap(image.info.width, image.info.height));
image.ReleaseAccess(ref data);
}
}
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);
}
public void DepthToCameraCoordinates(PXCMImage depth, PXCMPoint3DF32[] cameraSpacePts)
{
/* Retrieve the depth pixels and uvmap */
PXCMImage.ImageData ddata;
UInt16[] dpixels;
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(Int16); /* aligned width */
Int32 dwidth = (Int32)depth.info.width;
Int32 dheight = (Int32)depth.info.height;
dpixels = ddata.ToUShortArray(0, isdepth ? dpitch * dheight : dpitch * dheight * 3);
depth.ReleaseAccess(ddata);
/* Projection Calculation */
PXCMPoint3DF32[] dcords = new PXCMPoint3DF32[dwidth * dheight];
for (Int32 y = 0, k = 0; y < dheight; y++)
{
for (Int32 x = 0; x < dwidth; x++, k++)
{
dcords[k].x = x;
dcords[k].y = y;
dcords[k].z = isdepth ? dpixels[y * dpitch + x] : dpixels[3 * (y * dpitch + x) + 2];
}
}
pxcmStatus status = projection.ProjectDepthToCamera(dcords, cameraSpacePts);
if (status != pxcmStatus.PXCM_STATUS_NO_ERROR)
{
throw new InvalidOperationException("Projection depth to camera failed");
}
}
}
public static Bitmap GetRgb32Pixels(PXCMImage image)
{
var cwidth = Align16(image.info.width); /* aligned width */
var cheight = (int)image.info.height;
PXCMImage.ImageData cdata;
byte[] cpixels;
if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.ColorFormat.COLOR_FORMAT_RGB32, out cdata) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
cpixels = cdata.ToByteArray(0, cwidth * cheight * 4);
image.ReleaseAccess(ref cdata);
}
else
{
cpixels = new byte[cwidth * cheight * 4];
}
var width = (int)image.info.width;
var height = (int)image.info.height;
Bitmap bitmap;
lock (image)
{
bitmap = new Bitmap(width, height, PixelFormat.Format32bppRgb);
var data = bitmap.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.WriteOnly, PixelFormat.Format32bppRgb);
Marshal.Copy(cpixels, 0, data.Scan0, width * height * 4);
bitmap.UnlockBits(data);
}
return bitmap;
}
private static UInt16[] getUInt16Depths(PXCMImage depthImage)
{
PXCMImage.ImageData depthImageData;
UInt16[] pixelDepths = null;
if (depthImage.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out depthImageData) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
{
int width = (int)depthImageData.pitches[0] / sizeof(Int16);
int height = (int)depthImage.info.height;
pixelDepths = depthImageData.ToUShortArray(0, width * height);
depthImage.ReleaseAccess(depthImageData);
}
return pixelDepths;
}