static void Run()
{
string SAMPLE_XML_FILE = @"../../../Data/SamplesConfig.xml";
ScriptNode scriptNode;
Context context = Context.CreateFromXmlFile(SAMPLE_XML_FILE, out scriptNode);
DepthGenerator depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
if (depth == null)
{
Console.WriteLine("Sample must have a depth generator!");
return;
}
MapOutputMode mapMode = depth.MapOutputMode;
DepthMetaData depthMD = new DepthMetaData();
Console.WriteLine("Press any key to stop...");
while (!Console.KeyAvailable)
{
context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
Console.WriteLine("Frame {0} Middle point is: {1}.", depthMD.FrameID, depthMD[(int)mapMode.XRes/2, (int)mapMode.YRes/2]);
}
}
private void PaintGray16(WriteableBitmap b, DepthMetaData depthMeta)
{
b.Lock();
short* pDepthRow = (short*) depthMeta.DepthMapPtr;
int nTexMapX = b.BackBufferStride / (b.Format.BitsPerPixel / 8);
short* pTexRow = (short*) b.BackBuffer + depthMeta.YOffset*nTexMapX;
for (int y = 0; y < depthMeta.YRes; y++)
{
short* pDepth = pDepthRow;
short* pTex = pTexRow + depthMeta.XOffset;
for (int x = 0; x < depthMeta.XRes; x++)
{
if (*pDepth != 0)
{
*pTex = (short) _depthHist[*pDepth];
}
else
{
*pTex = 0;
}
pDepth++;
pTex++;
}
pDepthRow += depthMeta.XRes;
pTexRow += nTexMapX;
}
b.AddDirtyRect(new Int32Rect(0, 0, b.PixelWidth, b.PixelHeight));
b.Unlock();
}
private NuiSource()
{
context = new Context("openni.xml");
// Initialise generators
imageGenerator = this.context.FindExistingNode(NodeType.Image) as ImageGenerator;
depthGenerator = this.context.FindExistingNode(NodeType.Depth) as DepthGenerator;
imageMetadata = new ImageMetaData();
var imageMapMode = imageGenerator.GetMapOutputMode();
depthMetadata = new DepthMetaData();
var depthMapMode = depthGenerator.GetMapOutputMode();
depthHistogram = new int[depthGenerator.GetDeviceMaxDepth()];
// Initialise bitmaps
cameraImage = new WriteableBitmap((int)imageMapMode.nXRes, (int)imageMapMode.nYRes, 96, 96, PixelFormats.Rgb24, null);
depthImage = new WriteableBitmap((int)depthMapMode.nXRes, (int)depthMapMode.nYRes, 96, 96, PixelFormats.Rgb24, null);
// Initialise background thread
var cameraThread = new Thread(this.CameraThread) { IsBackground = true };
cameraThread.Start();
var userGenerator = new UserGenerator(context);
userGenerator.NewUser += this.UserGenerator_NewUser;
userGenerator.LostUser += this.UserGenerator_LostUser;
}
private NuiSource()
{
this.context = new Context("openni.xml");
// Initialise generators
this.imageGenerator = this.context.FindExistingNode(NodeType.Image) as ImageGenerator;
this.depthGenerator = this.context.FindExistingNode(NodeType.Depth) as DepthGenerator;
this.depthGenerator.GetAlternativeViewPointCap().SetViewPoint(this.imageGenerator);
this.userGenerator = new UserGenerator(this.context);
this.imageMetadata = new ImageMetaData();
var imageMapMode = this.imageGenerator.GetMapOutputMode();
this.depthMetadata = new DepthMetaData();
var depthMapMode = this.depthGenerator.GetMapOutputMode();
this.depthHistogram = new int[this.depthGenerator.GetDeviceMaxDepth()];
// Initialise bitmaps
this.cameraImage = new WriteableBitmap(
(int)imageMapMode.nXRes, (int)imageMapMode.nYRes, 96, 96, PixelFormats.Rgb24, null);
this.depthImage = new WriteableBitmap(
(int)depthMapMode.nXRes, (int)depthMapMode.nYRes, 96, 96, PixelFormats.Rgb24, null);
// Initialise user generator
this.userGenerator.NewUser += this.UserGenerator_NewUser;
this.userGenerator.LostUser += this.UserGenerator_LostUser;
this.userGenerator.StartGenerating();
this.ShowPlayerLabels = true;
// Initialise background thread
var cameraThread = new Thread(this.CameraThread) { IsBackground = true };
cameraThread.Start();
}
public ushort[] GetDepths(out int xResolution, out int yResolution)
{
xResolution = 0;
yResolution = 0;
if (m_depthGenerator == null)
{
return(null);
}
// calculate the core metadata
DepthMetaData metadata = m_depthGenerator.GetMetaData();
xResolution = metadata.XRes;
yResolution = metadata.YRes;
int totalDepths = metadata.XRes * metadata.YRes;
// copy the depths
// TODO: Is there a better way to marshal ushorts from an IntPtr?
IntPtr depthMapPtr = m_depthGenerator.GetDepthMapPtr();
short[] depthsTemp = new short[totalDepths];
Marshal.Copy(depthMapPtr, depthsTemp, 0, depthsTemp.Length);
ushort[] depths = new ushort[totalDepths];
Buffer.BlockCopy(depthsTemp, 0, depths, 0, totalDepths * metadata.BytesPerPixel);
return(depths);
}
public void OpenSavedColorDataWithDepth()
{
if (this.DepthMetaData == null)
{
this.DepthMetaData = new DepthMetaData();
}
byte[] colorInfo = null;
//DepthMetaData.ReadDepthWithColor_PLY(pathModels, FileNamePLY, ref this.DepthMetaData.FrameData, ref colorInfo);
DepthMetaData.ReadDepthWithColor_OBJ(pathModels, PointCloudScannerSettings.FileNameOBJ, ref this.DepthMetaData.FrameData, ref colorInfo);
if (colorInfo != null)
{
if (this.ColorMetaData == null)
{
this.ColorMetaData = new ColorMetaData();
}
this.ColorMetaData.SetColorPixels(colorInfo);
}
else
{
MessageBox.Show("Error reading a color Info With Depth: " + PointCloudScannerSettings.FileNamePLY + " - please create one first");
}
}
protected override bool InitTexture(out Texture2D refText, out int xSize, out int ySize)
{
if(base.InitTexture(out refText, out xSize, out ySize)==false)
return false;
// make sure we have an image to work with
if (m_context.CurrentContext.Depth == null)
{
m_context.m_Logger.Log("No depth", NIEventLogger.Categories.Initialization, NIEventLogger.Sources.BaseObjects, NIEventLogger.VerboseLevel.Errors);
return false;
}
// make sure we have an image to work with
if(m_factor<=0)
{
m_context.m_Logger.Log("Illegal factor", NIEventLogger.Categories.Initialization, NIEventLogger.Sources.Image, NIEventLogger.VerboseLevel.Errors);
return false;
}
// get the resolution from the image
MapOutputMode mom = m_context.CurrentContext.Depth.MapOutputMode;
// update the resolution by the factor
ySize = mom.YRes / m_factor;
xSize = mom.XRes / m_factor;
// create the texture
refText = new Texture2D(xSize, ySize);
// depthmap data
rawDepthMap = new short[(int)(mom.XRes * mom.YRes)];
// histogram stuff
int maxDepth = m_context.CurrentContext.Depth.DeviceMaxDepth;
depthHistogramMap = new float[maxDepth];
NIOpenNICheckVersion.Instance.ValidatePrerequisite();
m_metaData=new DepthMetaData();
return true;
}
static private OpenNI.Point3D Change(DepthMetaData depthMD, int[,] backgroundDepthMD)
{
OpenNI.Point3D newF = new OpenNI.Point3D(0, 0, 0);
bool skip = false;
int checkUp = 0;
for (int j = depthMD.FullYRes - 10; 10 < j; j--)
{
for (int i = 0; depthMD.FullXRes - 40 > i; i++)
{
if (Math.Abs(depthMD[i, j] - backgroundDepthMD[i, j]) > 50)
{
checkUp = j - 50;
if (checkUp >= depthMD.FullYRes || checkUp <= 0)
{
skip = true;
}
if (skip == false && (Math.Abs(depthMD[i, checkUp] - backgroundDepthMD[i, checkUp]) > 50))
{
//Console.WriteLine("change at {0},{1} size {2}", i, j, Math.Abs(depthMD[i, checkUp] - backgroundDepthMD[i, checkUp]));
newF = new OpenNI.Point3D(i, j, depthMD[i, j]);
j = 9; i = depthMD.FullXRes;
}
skip = false;
}
}
}
return(newF);
}
static void Run()
{
string SAMPLE_XML_FILE = @"../../../../Data/SamplesConfig.xml";
ScriptNode scriptNode;
Context context = Context.CreateFromXmlFile(SAMPLE_XML_FILE, out scriptNode);
DepthGenerator depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
if (depth == null)
{
Console.WriteLine("Sample must have a depth generator!");
return;
}
MapOutputMode mapMode = depth.MapOutputMode;
DepthMetaData depthMD = new DepthMetaData();
Console.WriteLine("Press any key to stop...");
while (!Console.KeyAvailable)
{
context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
Console.WriteLine("Frame {0} Middle point is: {1}.", depthMD.FrameID, depthMD[(int)mapMode.XRes / 2, (int)mapMode.YRes / 2]);
}
}
public void Paint(DepthMetaData depthMeta, WriteableBitmap b)
{
if (b.Format == PixelFormats.Gray16)
PaintGray16(b, depthMeta);
else if (b.Format == PixelFormats.Pbgra32)
PaintPbgra32(b, depthMeta);
}
/// <summary>
/// This method updates the image on the MainWindow page with the latest depth image.
/// </summary>
private unsafe void UpdateDepth()
{
// Get information about the depth image
DepthMetaData depthMD = new DepthMetaData();
// Lock the bitmap we will be copying to just in case. This will also give us a pointer to the bitmap.
System.Drawing.Rectangle rect = new System.Drawing.Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
depth.GetMetaData(depthMD);
// This will point to our depth image
ushort *pDepth = (ushort *)this.depth.GetDepthMapPtr().ToPointer();
// Go over the depth image and set the bitmap we're copying to based on our depth value.
for (int y = 0; y < depthMD.YRes; ++y)
{
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, pDest += 3)
{
// Change the color of the bitmap based on the depth value. You can make this
// whatever you want, my particular version is not that pretty.
pDest[0] = (byte)(*pDepth >> 2);
pDest[1] = (byte)(*pDepth >> 3);
pDest[2] = (byte)(*pDepth >> 4);
}
}
this.bitmap.UnlockBits(data);
// Update the image to have the bitmap image we just copied
image1.Source = getBitmapImage(bitmap);
}
// ヒストグラムの計算
private unsafe void CalcHist(DepthMetaData depthMD)
{
for (int i = 0; i < histogram.Length; ++i) {
histogram[i] = 0;
}
ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();
int points = 0;
for (int y = 0; y < depthMD.YRes; ++y) {
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth) {
ushort depthVal = *pDepth;
if (depthVal != 0) {
histogram[depthVal]++;
points++;
}
}
}
for (int i = 1; i < histogram.Length; i++) {
histogram[i] += histogram[i - 1];
}
if (points > 0) {
for (int i = 1; i < histogram.Length; i++) {
histogram[i] = (int)(256 * (1.0f - (histogram[i] / (float)points)));
}
}
}
public bool initializeSensor(String xmlPath)
{
try
{
pbuffer = new Point[6];
openpalm = new OpenPalm();
scrHeight = SystemInformation.PrimaryMonitorSize.Height;
scrWidth = SystemInformation.PrimaryMonitorSize.Width;
mouseSpeed = SystemInformation.MouseSpeed * 0.15;
pointCollections = new PointCollection();
/*OpenNI objects - Context, DepthGenerator and DepthMetaData are initialized here*/
cxt = new Context(xmlPath);
depthGen = cxt.FindExistingNode(NodeType.Depth) as DepthGenerator;
gsHandsGenerator = cxt.FindExistingNode(NodeType.Hands) as HandsGenerator;
gsHandsGenerator.SetSmoothing(0.1f);
depthMeta = new DepthMetaData();
if (depthGen == null)
{
return(false);
}
xRes = depthGen.MapOutputMode.XRes;
yRes = depthGen.MapOutputMode.YRes;
/*NITE objects - Session manager, PointControl is initialized here*/
sessionMgr = new SessionManager(cxt, "Wave", "RaiseHand");
pointCtrl = new PointControl("PointTracker");
steadydetector = new SteadyDetector();
flrouter = new FlowRouter();
brodcaster = new Broadcaster();
steadydetector.DetectionDuration = 200;
steadydetector.Steady += new EventHandler <SteadyEventArgs>(steadydetector_Steady);
steadydetector.NotSteady += new EventHandler <SteadyEventArgs>(steadydetector_NotSteady);
/* pointCtrl.PrimaryPointCreate += new EventHandler<HandFocusEventArgs>(pointCtrl_PrimaryPointCreate);
* pointCtrl.PrimaryPointUpdate += new EventHandler<HandEventArgs>(pointCtrl_PrimaryPointUpdate);
* pointCtrl.PrimaryPointDestroy += new EventHandler<IdEventArgs>(pointCtrl_PrimaryPointDestroy);*/
pointCtrl.PointCreate += new EventHandler <HandEventArgs>(pointCtrl_PointCreate);
pointCtrl.PointUpdate += new EventHandler <HandEventArgs>(pointCtrl_PointUpdate);
pointCtrl.PointDestroy += new EventHandler <IdEventArgs>(pointCtrl_PointDestroy);
sessionMgr.AddListener(steadydetector);
sessionMgr.AddListener(pointCtrl); //make the session manager listen to the point control
isActive = false; //set lifecycle flag to false
//fill the handpoint coordinates with invalid values
//initialize the clipping matrix
HandPointBuffer = new ArrayList();
}
catch (Exception e) { return(false); }
return(true);
}
//this method gets called, when Update() was called in evaluate,
//or a graphics device asks for its texture, here you fill the texture with the actual data
//this is called for each renderer, careful here with multiscreen setups, in that case
//calculate the pixels in evaluate and just copy the data to the device texture here
unsafe protected override void UpdateTexture(int Slice, Texture texture)
{
//lock the vvvv texture
DataRectangle rect;
if (texture.Device is DeviceEx)
{
rect = texture.LockRectangle(0, LockFlags.None);
}
else
{
rect = texture.LockRectangle(0, LockFlags.Discard);
}
try
{
if (FDepthMode[0] == DepthMode.Raw)
{
//copy full lines
for (int i = 0; i < FTexHeight; i++)
{
CopyMemory(rect.Data.DataPointer.Move(rect.Pitch * i), FDepthGenerator.DepthMapPtr.Move(FTexWidth * i * 2), FTexWidth * 2);
}
}
else
{
DepthMetaData DepthMD = FDepthGenerator.GetMetaData();
CalculateHistogram(DepthMD);
ushort *pSrc = (ushort *)FDepthGenerator.DepthMapPtr;
ushort *pDest = (ushort *)rect.Data.DataPointer;
// write the Depth pointer to Destination pointer
for (int y = 0; y < FTexHeight; y++)
{
var off = 0;
for (int x = 0; x < FTexWidth; x++, pSrc++, pDest++)
{
pDest[0] = (ushort)FHistogram[*pSrc];
off += 2;
}
//advance dest by rest of pitch
pDest += (rect.Pitch - off) / 2;
}
}
}
finally
{
//unlock the vvvv texture
texture.UnlockRectangle(0);
}
}
// 描画
private unsafe void xnDraw()
{
// ノードの更新を待ち、データを取得する
context.WaitAndUpdateAll();
ImageMetaData imageMD = image.GetMetaData();
DepthMetaData depthMD = depth.GetMetaData();
CalcHist(depthMD);
// カメラ画像の作成
lock (this) {
// 書き込み用のビットマップデータを作成
Rectangle rect = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.WriteOnly,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
// 生データへのポインタを取得
byte * dst = (byte *)data.Scan0.ToPointer();
byte * src = (byte *)image.ImageMapPtr.ToPointer();
ushort *dep = (ushort *)depth.DepthMapPtr.ToPointer();
for (int i = 0; i < imageMD.DataSize; i += 3, src += 3, dst += 3, ++dep)
{
byte pixel = (byte)histogram[*dep];
// ヒストグラムの対象外か、デプスを表示しない場合
if (pixel == 0 || !isShowDepth)
{
// イメージを表示する場合は、カメラ画像をコピーする
if (isShowImage)
{
dst[0] = src[2];
dst[1] = src[1];
dst[2] = src[0];
}
// イメージを描画しない場合は、白にする
else
{
dst[0] = 255;
dst[1] = 255;
dst[2] = 255;
}
}
// それ以外の場所はヒストラムを描画
else
{
dst[0] = 0;
dst[1] = pixel;
dst[2] = pixel;
}
}
bitmap.UnlockBits(data);
}
}
public void Start()
{
this.context = new Context (SAMPLE_XML_FILE);
this.depth = context.FindExistingNode (NodeType.Depth) as DepthGenerator;
if (this.depth == null) {
throw new Exception ("Viewer must have a depth node!");
}
this.depthMD = new DepthMetaData ();
this.histogram = new int[this.depth.GetDeviceMaxDepth ()];
}
public MainWindow()
{
InitializeComponent();
console = new Console();
console.Show();
console.Top = 0;
console.Left = 0;
Console.Write("TrackingNI by Richard Pianka and Ramsey Abouzahra");
context = new Context(CONFIG_FILE);
imageGenerator = new ImageGenerator(context);
depthGenerator = new DepthGenerator(context);
userGenerator = new UserGenerator(context);
poseDetectionCapability = userGenerator.PoseDetectionCapability;
skeletonCapability = userGenerator.SkeletonCapability;
MapOutputMode mapMode = depthGenerator.MapOutputMode;
int width = (int)mapMode.XRes;
int height = (int)mapMode.YRes;
imageBitmap = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
depthBitmap = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
depthBitmapCorrected = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
imageData = new ImageMetaData();
depthData = new DepthMetaData();
skeletonDraw = new SkeletonDraw();
Histogram = new int[depthGenerator.DeviceMaxDepth];
reader = new Thread(new ThreadStart(Reader));
reader.IsBackground = true;
worker = new BackgroundWorker();
stop = false;
CompositionTarget.Rendering += new EventHandler(Worker);
Closing += new System.ComponentModel.CancelEventHandler(MainWindow_Closing);
userGenerator.NewUser += new EventHandler <NewUserEventArgs>(NewUser);
userGenerator.LostUser += new EventHandler <UserLostEventArgs>(LostUser);
skeletonCapability.CalibrationStart += new EventHandler <CalibrationStartEventArgs>(CalibrationStart);
skeletonCapability.CalibrationEnd += new EventHandler <CalibrationEndEventArgs>(CalibrationEnd);
skeletonCapability.SetSkeletonProfile(SkeletonProfile.All);
poseDetectionCapability.PoseDetected += new EventHandler <PoseDetectedEventArgs>(PoseDetected);
poseDetectionCapability.PoseEnded += new EventHandler <PoseEndedEventArgs>(PoseEnded);
reader.Start();
worker.DoWork += new DoWorkEventHandler(WorkerTick);
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
lock (this)
{
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
if (this.shouldDrawPixels)
{
ushort *pDepth = (ushort *)this.depth.DepthMapPtr.ToPointer();
ushort *pLabels = (ushort *)this.userGenerator.GetUserPixels(0).LabelMapPtr.ToPointer();
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, ++pLabels, pDest += 3)
{
pDest[0] = pDest[1] = pDest[2] = 0;
ushort label = *pLabels;
if (this.shouldDrawBackground || *pLabels != 0)
{
pDest[0] = (byte)(Color.White.B);
pDest[1] = (byte)(Color.White.G);
pDest[2] = (byte)(Color.White.R);
}
}
}
}
this.bitmap.UnlockBits(data);
}
this.Invalidate();
}
}
/// <summary>
/// Draw the image from the depth sensor without optional background subtraction
/// or user markers to the <see cref="Image"/>-property.
/// </summary>
protected unsafe void drawDepthWithoutHighlightAndBackgroundSubtraction(
DepthMetaData depthMD, int[] histogram)
{
// Create a depth histogram.
CalcHist(depthMD, histogram);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.ReadWrite,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
double depthMax = (float)depthGenerator.DeviceMaxDepth;
#if PARALELLIZED
// Otherwise Parallelization does not work.
bitmap.UnlockBits(data);
Parallel.For(0, depthMD.YRes, (y) =>
{
ushort *pDepth = (ushort *)this.depthGenerator.DepthMapPtr.ToPointer() + y * depthMD.XRes;
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
//byte pixel = (byte)((*pDepth) / depthMax * 255.0);
byte pixel = (byte)histogram[*pDepth];
pDest[2] = pixel;
pDest[1] = pixel;
pDest[0] = pixel;
}
});
#else
try
{
byte pixel;
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
//pixel = ((*pDepth) / depthMax * 255.0);
pixel = (byte)histogram[*pDepth];
pDest[2] = pixel;
pDest[1] = pixel;
pDest[0] = pixel;
}
}
}
finally
{ bitmap.UnlockBits(data); }
#endif
}
/// <summary>
/// Draw the image from the depth sensor with optional background subtraction
/// or user markers to the <see cref="Image"/>-property.
/// </summary>
/// <param name="background_users">Users to regard as background.</param>
protected unsafe void drawDepthWithHighlightAndBackgroundSubtraction(
DepthMetaData depthMD, ushort[] userInformationMap,
bool drawBackground, bool drawHighlight, int[] histogram,
List <int> background_users)
{
fixed(ushort *userInformation = userInformationMap)
{
drawDepthWithHighlightAndBackgroundSubtraction(
depthMD, userInformation, drawBackground, drawHighlight,
histogram, background_users);
}
}
public void SaveDepthPoints()
{
if (this.DepthMetaData == null)
{
MessageBox.Show("No Depth Data to save - please capture, or open last saved depth data");
return;
}
//ushort[] rotatedPoints = DepthMetaData.RotateDepthFrame(this.DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
List <Vector3> listPoints = DepthMetaData.CreateListPoints_Depth(this.DepthMetaData.FrameData, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect);
GLSettings.FileNamePointCloudLast1 = DateTime.Now.Year.ToString() + "." + DateTime.Now.Month.ToString() + "." + DateTime.Now.Day.ToString() + "." + DateTime.Now.Hour.ToString() + "." + DateTime.Now.Minute.ToString() + "." + DateTime.Now.Second.ToString() + "_PointCloud.xyz";
UtilsPointCloudIO.ToXYZFile(listPoints, GLSettings.FileNamePointCloudLast1, pathModels);
}
// 描画
private unsafe void xnDraw()
{
// ノードの更新を待ち、データを取得する
context.WaitAndUpdateAll();
ImageMetaData imageMD = image.GetMetaData();
DepthMetaData depthMD = depth.GetMetaData();
CalcHist(depthMD);
// カメラ画像の作成
lock (this) {
// 書き込み用のビットマップデータを作成
Rectangle rect = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.WriteOnly,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
// 生データへのポインタを取得
byte * dst = (byte *)data.Scan0.ToPointer();
byte * src = (byte *)image.ImageMapPtr.ToPointer();
ushort *dep = (ushort *)depth.DepthMapPtr.ToPointer();
for (int i = 0; i < imageMD.DataSize; i += 3, src += 3, dst += 3, ++dep)
{
byte pixel = (byte)histogram[*dep];
// ヒストグラムの対象外の場合、カメライメージを描画する
if (pixel == 0)
{
dst[0] = src[2];
dst[1] = src[1];
dst[2] = src[0];
}
// それ以外の場所はヒストラムを描画する
else
{
dst[0] = 0;
dst[1] = pixel;
dst[2] = pixel;
}
}
bitmap.UnlockBits(data);
// 現在の状態を表示する
Graphics g = Graphics.FromImage(bitmap);
string message = "";
message += "ImageMirror:" + mirrorState[image.ToString()] + "\n";
message += "DepthMirror:" + mirrorState[depth.ToString()];
g.DrawString(message, font, brush, point);
}
}
private unsafe void ReadImageData()
{
var depthMD = new DepthMetaData();
while (FRunning)
{
try
{
FContext.GlobalMirror = FMirrored[0];
FContext.WaitOneUpdateAll(FDepthGenerator);
}
catch (Exception)
{}
if (FDepthMode[0] == DepthMode.Histogram)
{
FDepthGenerator.GetMetaData(depthMD);
CalculateHistogram(depthMD);
}
lock (FBufferedImageLock)
{
if (FDepthGenerator.IsDataNew)
{
try
{
if (FDepthMode[0] == DepthMode.Raw)
{
CopyMemory(FBufferedImage, FDepthGenerator.DepthMapPtr, FTexHeight * FTexWidth * 2);
}
else
{
ushort *pSrc = (ushort *)FDepthGenerator.DepthMapPtr.ToPointer();
ushort *pDest = (ushort *)FBufferedImage.ToPointer();
//write the Depth pointer to Destination pointer
for (int y = 0; y < FTexHeight; y++)
{
for (int x = 0; x < FTexWidth; x++, pSrc++, pDest++)
{
*pDest = (ushort)FHistogram[*pSrc];
}
}
}
}
catch (Exception)
{ }
}
}
}
}
private void SaveImageInterpolated()
{
if (listPointsInterpolated == null)
{
return;
}
//-----------------------------------------------
//now interpolate last 10 frames to one frame and save
//ushort[] rotatedPoints = DepthMetaData.RotateDepthFrame(this.DepthMetaData.DepthFrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
WriteableBitmap depthInterpolated = DepthMetaData.ToWriteableBitmap(listPointsInterpolated);
WriteableBitmapUtils.SaveImage(pathModels, "DepthInterpolated", depthInterpolated, true);
}
private bool ProcessDepthFrame(MultiSourceFrame multiSourceFrame)
{
// Depth
using (var frame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
if (frame != null)
{
DepthFrame frameDepth = frame;
if (PointCloudScannerSettings.ScannerMode == ScannerMode.Depth || PointCloudScannerSettings.ScannerMode == ScannerMode.Color_Depth || PointCloudScannerSettings.ScannerMode == ScannerMode.Color_Depth_3DDisplay)
{
this.DepthMetaData = new DepthMetaData(frameDepth, false);
if (PointCloudScannerSettings.BackgroundRemoved)
{
backgroundRemovalTool.DepthFrameData_RemoveBackground(this.DepthMetaData, this.BodyMetaData);
if (PointCloudScannerSettings.CutFrames)
{
this.DepthMetaData.FrameData = DepthMetaData.CutDepth(this.DepthMetaData.FrameData, PointCloudScannerSettings.CutFrameMaxDistance, PointCloudScannerSettings.CutFrameMinDistance, ref numberOfCutPoints);
}
if (PointCloudScannerSettings.ScannerMode != ScannerMode.Color_Depth_3DDisplay)
{
this.imageDepth.Source = WriteableBitmapUtils.FromByteArray_ToGray(this.DepthMetaData.Pixels, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect);
}
}
else
{
if (PointCloudScannerSettings.CutFrames)
{
this.DepthMetaData.FrameData = DepthMetaData.CutDepth(this.DepthMetaData.FrameData, PointCloudScannerSettings.CutFrameMaxDistance, PointCloudScannerSettings.CutFrameMinDistance, ref numberOfCutPoints);
}
if (PointCloudScannerSettings.ScannerMode != ScannerMode.Color_Depth_3DDisplay)
{
this.imageDepth.Source = DepthMetaData.FromUShort(DepthMetaData.FrameData);
}
}
if (PointCloudScannerSettings.InterpolateFrames)
{
CalculateInterpolatedPixels();
}
}
return(true);
}
}
return(false);
}
private unsafe void RenderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
lock (this)
{
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
ushort *pDepth = (ushort *)this.depth.DepthMapPtr.ToPointer();
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, pDest += 3)
{
byte pixel = (byte)this.histogram[*pDepth];
pDest[0] = 0;
pDest[1] = pixel;
pDest[2] = pixel;
}
}
this.bitmap.UnlockBits(data);
FPS_Temp = depthMD.FPS;
}
this.Invalidate();
this.sessionManager.Update(this.context);
}
}
public unsafe void GetDepthData(DepthMetaData depthMD, out byte[] data)
{
data = new byte[this.depthMD.XRes * this.depthMD.YRes * 3];
ushort* pDepth = (ushort*)this.depthMD.DepthMapPtr.ToPointer ();
int index = 0;
for (int y = 0; y < depthMD.YRes; y++) {
for (int x = 0; x < depthMD.XRes; x++,pDepth++) {
byte pixel = (byte)this.histogram[*pDepth];
data[index] = pixel;
data[index + 1] = pixel;
data[index + 2] = pixel;
index += 3;
}
}
}
public void SaveObjFile()
{
//open a file
DepthMetaData DepthMetaData = new DepthMetaData();
byte[] colorInfo = null;
//string fileName = "\\test.obj";
DepthMetaData.ReadDepthWithColor_OBJ(path, "KinectFace1.obj", ref DepthMetaData.FrameData, ref colorInfo);
PointCloudUtilsIO.Write_OBJ(colorInfo, DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault, path, "test.obj");
//PointCloudIO.Write_PLY(myColorPixels, this.DepthMetaData.FrameData, pathModels, FileNameColorInfoWithDepth);
//List<Vertex> myVertexReference = this.OpenGLControl.GLrender.Models3D[0].Vertices;
//List<Vertex> myVertexToBeMatched = this.OpenGLControl.GLrender.Models3D[1].Vertices;
}
private unsafe void updateRoutine()
{
while (isActive)
{
//try
{
cxt.WaitAndUpdateAll(); //update the depth node
sessionMgr.Update(cxt); //update the session manager
//get the meta data from the depth node
// Console.WriteLine("Updateting..\n");
depthMeta = depthGen.GetMetaData();
inputProvider.RaiseNewFrame(frameCounter); //clip that meta data if hand point is valid. The clip is saved in clipping matrix
}
frameCounter++;
//catch (Exception e) { }
}
}
private void HelperInterpolation_End()
{
iFrameInterpolation = 0;
if (bStopAfterFrameInterpolation)
{
ScannerClose();
}
//ushort[] rotatedPoints = DepthMetaData.RotateDepthFrame(this.DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
listPointsInterpolated = DepthMetaData.CreateListPoints_Depth(this.DepthMetaData.FrameData, DepthMetaData.XDepthMaxKinect, DepthMetaData.YDepthMaxKinect);
if (PointCloudScannerSettings.EntropyImage)
{
ShowImageEntropy();
}
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception e)
{
Console.WriteLine(e.Message + e.StackTrace);
//Debugger.Break();
}
this.depth.GetMetaData(depthMD);
lock (this)
{
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.skeletonCapbility.IsTracking(user))
{
DetectTouches(user);
}
else
{
//Do not wait for access
if (Monitor.TryEnter(userReleaseLock))
{
releaseUserTouches(user);
Monitor.Exit(userReleaseLock);
}
}
}
}
inputProvider.raiseFrame(framecounter++);
}
}
public void TransformPointCloud_SaveObjFile()
{
//open a file
DepthMetaData DepthMetaData = new DepthMetaData();
byte[] colorInfo = null;
DepthMetaData.ReadDepthWithColor_OBJ(path, "KinectFace1.obj", ref DepthMetaData.FrameData, ref colorInfo);
List <Vector3d> myVectors = Vertices.ConvertToVector3DList_FromArray(DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
List <float[]> myColorsFloats = PointCloudUtils.CreateColorInfo(colorInfo, DepthMetaData.FrameData, DepthMetaData.XResDefault, DepthMetaData.YResDefault);
List <Vertex> myVertexList = Model3D.CreateVertexList(myVectors, myColorsFloats);
VertexUtils.ScaleByFactor(myVertexList, 0.9);
VertexUtils.RotateVertices30Degrees(myVertexList);
VertexUtils.TranslateVertices(myVertexList, 10, 3, 5);
Model3D.Save_ListVertices_Obj(myVertexList, path, "transformed.obj");
}
static private void Background(DepthMetaData depthMD, int[,] backgroundDepthMD, int XRes, int YRes, int loadRuns)
{
for (int i = 0; XRes > i; i++)
{
for (int j = 0; YRes > j; j++)
{
backgroundDepthMD[i, j] = depthMD[i, j] + backgroundDepthMD[i, j];
}
}
if (depthMD.FrameID == loadRuns)
{
for (int i = 0; XRes > i; i++)
{
for (int j = 0; YRes > j; j++)
{
backgroundDepthMD[i, j] = backgroundDepthMD[i, j] / loadRuns;
}
}
}
}
// 描画
private unsafe void xnDraw()
{
// ノードの更新を待ち、データを取得する
context.WaitAndUpdateAll();
ImageMetaData imageMD = image.GetMetaData();
DepthMetaData depthMD = depth.GetMetaData();
// カメラ画像の作成
lock (this) {
// 書き込み用のビットマップデータを作成
Rectangle rect = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.WriteOnly,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
// 生データへのポインタを取得
byte *dst = (byte *)data.Scan0.ToPointer();
byte *src = (byte *)image.ImageMapPtr.ToPointer();
for (int i = 0; i < imageMD.DataSize; i += 3, src += 3, dst += 3)
{
dst[0] = src[2];
dst[1] = src[1];
dst[2] = src[0];
}
bitmap.UnlockBits(data);
// 中心点の距離を表示
Graphics g = Graphics.FromImage(bitmap);
int x = (int)image.MapOutputMode.XRes / 2;
int y = (int)image.MapOutputMode.YRes / 2;
g.FillEllipse(brush, x - 10, y - 10, 20, 20);
string message = depthMD[x, y] + "mm";
g.DrawString(message, font, brush, x, y);
}
}
private unsafe void CalcHist(DepthMetaData depthMD)
{
// reset
for (int i = 0; i < this.histogram.Length; ++i)
{
this.histogram[i] = 0;
}
ushort *pDepth = (ushort *)depthMD.DepthMapPtr.ToPointer();
// acclumulative histogram
int points = 0;
for (int y = 0; y < depthMD.YRes; ++y)
{
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
{
ushort depthVal = *pDepth;
if (depthVal != 0)
{
this.histogram[depthVal]++;
points++;
}
}
}
for (int i = 1; i < this.histogram.Length; i++)
{
this.histogram[i] += this.histogram[i - 1];
}
if (points > 0)
{
for (int i = 1; i < this.histogram.Length; i++)
{
this.histogram[i] = (int)(256 * (1.0f - (this.histogram[i] / (float)points)));
}
}
} // calc-hist
protected override bool InitTexture(out Texture2D refText, out int xSize, out int ySize)
{
if (base.InitTexture(out refText, out xSize, out ySize) == false)
{
return(false);
}
// make sure we have an image to work with
if (m_context.CurrentContext.Depth == null)
{
m_context.m_Logger.Log("No depth", NIEventLogger.Categories.Initialization, NIEventLogger.Sources.BaseObjects, NIEventLogger.VerboseLevel.Errors);
return(false);
}
// make sure we have an image to work with
if (m_factor <= 0)
{
m_context.m_Logger.Log("Illegal factor", NIEventLogger.Categories.Initialization, NIEventLogger.Sources.Image, NIEventLogger.VerboseLevel.Errors);
return(false);
}
// get the resolution from the image
MapOutputMode mom = m_context.CurrentContext.Depth.MapOutputMode;
// update the resolution by the factor
ySize = mom.YRes / m_factor;
xSize = mom.XRes / m_factor;
// create the texture
refText = new Texture2D(xSize, ySize);
// depthmap data
rawDepthMap = new short[(int)(mom.XRes * mom.YRes)];
// histogram stuff
int maxDepth = m_context.CurrentContext.Depth.DeviceMaxDepth;
depthHistogramMap = new float[maxDepth];
NIOpenNICheckVersion.Instance.ValidatePrerequisite();
m_metaData = new DepthMetaData();
return(true);
}
private unsafe void UpdateHistogram(DepthMetaData depthMD)
{
// Reset.
for (int i = 0; i < Histogram.Length; ++i)
{
Histogram[i] = 0;
}
ushort *pDepth = (ushort *)depthMD.DepthMapPtr.ToPointer();
int points = 0;
for (int y = 0; y < depthMD.YRes; ++y)
{
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
{
ushort depthVal = *pDepth;
if (depthVal != 0)
{
Histogram[depthVal]++;
points++;
}
}
}
for (int i = 1; i < Histogram.Length; i++)
{
Histogram[i] += Histogram[i - 1];
}
if (points > 0)
{
for (int i = 1; i < Histogram.Length; i++)
{
Histogram[i] = (int)(256 * (1.0f - (Histogram[i] / (float)points)));
}
}
}
private unsafe void CalculateHistogram(DepthMetaData DepthMD)
{
//initialize all slots to 0
for (int i = 0; i < FHistogram.Length; ++i)
{
FHistogram[i] = 0;
}
ushort *pDepth = (ushort *)DepthMD.DepthMapPtr;
int points = 0;
for (int y = 0; y < DepthMD.YRes; y++)
{
for (int x = 0; x < DepthMD.XRes; x++, pDepth++)
{
ushort depthVal = *pDepth;
if (depthVal != 0)
{
FHistogram[depthVal]++;
points++;
}
}
}
for (int i = 1; i < FHistogram.Length; i++)
{
FHistogram[i] += FHistogram[i - 1];
}
if (points > 0)
{
for (int i = 1; i < FHistogram.Length; i++)
{
FHistogram[i] = (ushort)(ushort.MaxValue * (1.0f - (FHistogram[i] / (float)points)));
}
}
}
//Starts up necessary files to take data
//Must run before TakeData()
Kinect()
{
//Sets locations of XML File
string SAMPLE_XML_FILE = @"..\\..\\..\\SamplesConfig.xml";
//Declares object of ScriptNode and defines context
ScriptNode scriptNode;
context = Context.CreateFromXmlFile(SAMPLE_XML_FILE, out scriptNode);
//Declares the depth generator
depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
//If the depth generator does not exist returns error messag
if (depth == null)
{
Console.WriteLine("Sample must have a depth generator!");
Console.ReadLine();
return;
}
//Declares necessary variables and classes to take depth
//DepthGenerator depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
mapMode = depth.MapOutputMode;
depthMD = new DepthMetaData();
}
public static unsafe void FillTexture(DepthMetaData depthMD, out ushort[] bytes)
{
ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();
bytes = new ushort[depthMD.XRes * depthMD.YRes];
int points = 0;
for (int y = 0; y < depthMD.YRes; ++y) {
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth) {
ushort depthVal = *pDepth;
//if (depthVal != 0) {
bytes[points] = depthVal;
points++;
//}
}
}
StreamWriter sw = File.CreateText("/Users/matt/Desktop/CustomCodeLog.txt");
sw.WriteLine("XRes: #{0}", depthMD.XRes);
sw.WriteLine("YRes: #{0}", depthMD.YRes);
sw.WriteLine("bytes[100]: #{0}", (int)(256 * bytes[100]));
sw.Close();
}
public void Update(DepthMetaData depthMeta)
{
if (_depthHist == null)
_depthHist = new float[MaxDepth];
Array.Clear(_depthHist, 0, _depthHist.Length);
int numPoints = 0;
short* ptrDepth = (short*)depthMeta.DepthMapPtr;
for (int y = 0; y < depthMeta.YRes; y++)
{
for (int x = 0; x < depthMeta.XRes; x++)
{
if (*ptrDepth != 0)
{
_depthHist[*ptrDepth]++;
numPoints++;
}
ptrDepth++;
}
}
for (int i = 1; i < MaxDepth; i++)
{
_depthHist[i] += _depthHist[i - 1];
}
if (numPoints > 0)
{
for (int nIndex = 1; nIndex < MaxDepth; nIndex++)
{
_depthHist[nIndex] = Int16.MaxValue * (1.0f - (_depthHist[nIndex] / numPoints));
}
}
}
private unsafe void updateRoutine()
{
while (isActive)
{
//try
{
cxt.WaitAndUpdateAll(); //update the depth node
sessionMgr.Update(cxt); //update the session manager
//get the meta data from the depth node
// Console.WriteLine("Updateting..\n");
depthMeta = depthGen.GetMetaData();
inputProvider.RaiseNewFrame(frameCounter); //clip that meta data if hand point is valid. The clip is saved in clipping matrix
}
frameCounter++;
//catch (Exception e) { }
}
}
public FrameUpdateEventArgs(ImageMetaData imgMD, DepthMetaData depthMD)
{
this.ImageMetaData = imgMD;
this.DepthMetaData = depthMD;
}
public bool initializeSensor(String xmlPath)
{
try
{
pbuffer =new Point[6];
openpalm = new OpenPalm();
scrHeight = SystemInformation.PrimaryMonitorSize.Height;
scrWidth = SystemInformation.PrimaryMonitorSize.Width;
mouseSpeed = SystemInformation.MouseSpeed * 0.15;
pointCollections = new PointCollection();
/*OpenNI objects - Context, DepthGenerator and DepthMetaData are initialized here*/
cxt = new Context(xmlPath);
depthGen = cxt.FindExistingNode(NodeType.Depth) as DepthGenerator;
gsHandsGenerator = cxt.FindExistingNode(NodeType.Hands) as HandsGenerator;
gsHandsGenerator.SetSmoothing(0.1f);
depthMeta = new DepthMetaData();
if (depthGen == null) return false;
xRes = depthGen.MapOutputMode.XRes;
yRes = depthGen.MapOutputMode.YRes;
/*NITE objects - Session manager, PointControl is initialized here*/
sessionMgr = new SessionManager(cxt, "Wave", "RaiseHand");
pointCtrl = new PointControl("PointTracker");
steadydetector = new SteadyDetector();
flrouter = new FlowRouter();
brodcaster = new Broadcaster();
steadydetector.DetectionDuration = 200;
steadydetector.Steady+=new EventHandler<SteadyEventArgs>(steadydetector_Steady);
steadydetector.NotSteady+=new EventHandler<SteadyEventArgs>(steadydetector_NotSteady);
/* pointCtrl.PrimaryPointCreate += new EventHandler<HandFocusEventArgs>(pointCtrl_PrimaryPointCreate);
pointCtrl.PrimaryPointUpdate += new EventHandler<HandEventArgs>(pointCtrl_PrimaryPointUpdate);
pointCtrl.PrimaryPointDestroy += new EventHandler<IdEventArgs>(pointCtrl_PrimaryPointDestroy);*/
pointCtrl.PointCreate += new EventHandler<HandEventArgs>(pointCtrl_PointCreate);
pointCtrl.PointUpdate += new EventHandler<HandEventArgs>(pointCtrl_PointUpdate);
pointCtrl.PointDestroy += new EventHandler<IdEventArgs>(pointCtrl_PointDestroy);
sessionMgr.AddListener(steadydetector);
sessionMgr.AddListener(pointCtrl); //make the session manager listen to the point control
isActive = false; //set lifecycle flag to false
//fill the handpoint coordinates with invalid values
//initialize the clipping matrix
HandPointBuffer = new ArrayList();
}
catch (Exception e) { return false; }
return true;
}
private unsafe void UpdateHistogram(DepthMetaData depthMD)
{
// Reset.
depthHistogram = new int[depthHistogram.Length];
var pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();
var points = 0;
for (var y = 0; y < depthMD.YRes; ++y)
{
for (var x = 0; x < depthMD.XRes; ++x, ++pDepth)
{
var depthVal = *pDepth;
if (depthVal != 0)
{
depthHistogram[depthVal]++;
points++;
}
}
}
for (var i = 1; i < depthHistogram.Length; i++)
{
depthHistogram[i] += depthHistogram[i - 1];
}
if (points > 0)
{
for (int i = 1; i < depthHistogram.Length; i++)
{
depthHistogram[i] = (int)(256 * (1.0f - (depthHistogram[i] / (float)points)));
}
}
}
public DepthMetaData GetMetaData()
{
DepthMetaData depthMD = new DepthMetaData();
GetMetaData(depthMD);
return depthMD;
}
public void GetMetaData(DepthMetaData depthMD)
{
using (IMarshaler marsh = depthMD.GetMarshaler(true))
{
SafeNativeMethods.xnGetDepthMetaData(this.InternalObject, marsh.Native);
}
}
public void GetMetaData(DepthMetaData depthMD)
{
using (IMarshaler marsh = depthMD.GetMarshaler(true))
{
OpenNIImporter.xnGetDepthMetaData(this.InternalObject, marsh.Native);
}
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
lock (this)
{
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
if (this.shouldDrawPixels)
{
ushort* pDepth = (ushort*)this.depth.DepthMapPtr.ToPointer();
ushort* pLabels = (ushort*)this.userGenerator.GetUserPixels(0).LabelMapPtr.ToPointer();
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, ++pLabels, pDest += 3)
{
pDest[0] = pDest[1] = pDest[2] = 0;
ushort label = *pLabels;
if (this.shouldDrawBackground || *pLabels != 0)
{
Color labelColor = Color.White;
if (label != 0)
{
labelColor = colors[label % ncolors];
}
byte pixel = (byte)this.histogram[*pDepth];
pDest[0] = (byte)(pixel * (labelColor.B / 256.0));
pDest[1] = (byte)(pixel * (labelColor.G / 256.0));
pDest[2] = (byte)(pixel * (labelColor.R / 256.0));
}
}
}
}
this.bitmap.UnlockBits(data);
Graphics g = Graphics.FromImage(this.bitmap);
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.shouldPrintID)
{
Point3D com = this.userGenerator.GetCoM(user);
com = this.depth.ConvertRealWorldToProjective(com);
string label = "";
if (!this.shouldPrintState)
label += user;
else if (this.skeletonCapbility.IsTracking(user))
label += user + " - Tracking";
else if (this.skeletonCapbility.IsCalibrating(user))
label += user + " - Calibrating...";
else
label += user + " - Looking for pose";
g.DrawString(label, new Font("Arial", 6), new SolidBrush(anticolors[user % ncolors]), com.X, com.Y);
}
if (this.shouldDrawSkeleton && this.skeletonCapbility.IsTracking(user))
// if (this.skeletonCapbility.IsTracking(user))
DrawSkeleton(g, anticolors[user % ncolors], user);
}
g.Dispose();
}
this.Invalidate();
}
}
/// <summary>
/// Draw the image from the depth sensor with optional background subtraction
/// or user markers to the <see cref="Image"/>-property.
/// </summary>
/// <param name="background_users">Users to regard as background.</param>
protected unsafe void drawDepthWithHighlightAndBackgroundSubtraction(
DepthMetaData depthMD, ushort[] userInformationMap,
bool drawBackground, bool drawHighlight, int[] histogram,
List<int> background_users)
{
fixed (ushort* userInformation = userInformationMap)
{
drawDepthWithHighlightAndBackgroundSubtraction(
depthMD, userInformation, drawBackground, drawHighlight,
histogram, background_users);
}
}
/// <summary>
/// Draw the image from the depth sensor without optional background subtraction
/// or user markers to the <see cref="Image"/>-property.
/// </summary>
protected unsafe void drawDepthWithoutHighlightAndBackgroundSubtraction(
DepthMetaData depthMD, int[] histogram)
{
// Create a depth histogram.
CalcHist(depthMD, histogram);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.ReadWrite,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
double depthMax = (float)depthGenerator.DeviceMaxDepth;
#if PARALELLIZED
// Otherwise Parallelization does not work.
bitmap.UnlockBits(data);
Parallel.For(0, depthMD.YRes, (y) =>
{
ushort* pDepth = (ushort*)this.depthGenerator.DepthMapPtr.ToPointer() + y * depthMD.XRes;
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
//byte pixel = (byte)((*pDepth) / depthMax * 255.0);
byte pixel = (byte)histogram[*pDepth];
pDest[2] = pixel;
pDest[1] = pixel;
pDest[0] = pixel;
}
});
#else
try
{
byte pixel;
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
//pixel = ((*pDepth) / depthMax * 255.0);
pixel = (byte)histogram[*pDepth];
pDest[2] = pixel;
pDest[1] = pixel;
pDest[0] = pixel;
}
}
}
finally
{ bitmap.UnlockBits(data); }
#endif
}
//Creates a text file of the matrix of depth values taking a string parameter
static string TakeData(string str)
{
//Declares necessary variables and classes to take depth
DepthGenerator depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
MapOutputMode mapMode = depth.MapOutputMode;
DepthMetaData depthMD = new DepthMetaData();
//Waits for the depth to update itself
context.WaitOneUpdateAll(depth);
//Gets the depth values
depth.GetMetaData(depthMD);
//Opens the text writer and creates a new file with the inputted string as the filename
TextWriter tw = new StreamWriter(@"C:\Users\CenSISS\Documents\Kinect Project\Test Files\" + str + ".txt");
//Sets height and width to the X and Y resolutions
int height = mapMode.YRes;
int width = mapMode.XRes;
//For each pixel, writes a line of text: x y depth
for (int h = 0; h < height; ++h)
{
for (int w = 0; w < width; ++w)
{
tw.WriteLine(w + " " + h + " " + depthMD[w, h]);
}
}
//Closes the text writer
tw.Close();
return str;
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception e)
{
Console.WriteLine(e.Message + e.StackTrace);
//Debugger.Break();
}
this.depth.GetMetaData(depthMD);
lock (this)
{
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.skeletonCapbility.IsTracking(user))
{
DetectTouches(user);
}
else
{
//Do not wait for access
if(Monitor.TryEnter(userReleaseLock))
{
releaseUserTouches(user);
Monitor.Exit(userReleaseLock);
}
}
}
}
inputProvider.raiseFrame(framecounter++);
}
}
/// <summary>
/// Re-creates the depth histogram.
/// </summary>
/// <param name="depthMD"></param>
public unsafe void UpdateHistogram(DepthMetaData depthMD)
{
// Reset.
for (int i = 0; i < Histogram.Length; ++i)
Histogram[i] = 0;
ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();
int points = 0;
for (int y = 0; y < depthMD.YRes; ++y)
{
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
{
ushort depthVal = *pDepth;
if (depthVal != 0)
{
Histogram[depthVal]++;
points++;
}
}
}
for (int i = 1; i < Histogram.Length; i++)
{
Histogram[i] += Histogram[i - 1];
}
if (points > 0)
{
for (int i = 1; i < Histogram.Length; i++)
{
Histogram[i] = (int)(256 * (1.0f - (Histogram[i] / (float)points)));
}
}
}
/// <summary>
/// Draw the image from the depth sensor with optional background subtraction
/// or user markers to the <see cref="Image"/>-property.
/// </summary>
/// <param name="background_users">Users to regard as background.</param>
protected unsafe void drawDepthWithHighlightAndBackgroundSubtraction(
DepthMetaData depthMD, ushort* userInformation,
bool drawBackground, bool drawHighlight, int[] histogram,
List<int> background_users)
{
// Create a depth histogram.
CalcHist(depthMD, histogram);
BitmapData data = bitmap.LockBits(rect, ImageLockMode.ReadWrite,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
#if PARALELLIZED
double depthMax = (float)depthGenerator.DeviceMaxDepth;
// Otherwise Parallelization does not work.
bitmap.UnlockBits(data);
Parallel.For(0, depthMD.YRes, (y) =>
{
ushort* pLabels = userInformation + y * depthMD.XRes;
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
ushort* pDepth = (ushort*)this.depthGenerator.DepthMapPtr.ToPointer() + y * depthMD.XRes;
for (int x = 0; x < depthMD.XRes; ++x, ++pLabels, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
ushort label = *pLabels;
if (drawBackground ||
*pLabels != 0 && !background_users.Contains(*pLabels))
{
Color labelColor = Color.White;
if (label != 0 && drawHighlight)
{
labelColor = COLORS[label % NCOLORS];
}
double pixel = (byte)histogram[*pDepth];
pDest[2] = (byte)(pixel * (labelColor.B / 256.0));
pDest[1] = (byte)(pixel * (labelColor.G / 256.0));
pDest[0] = (byte)(pixel * (labelColor.R / 256.0));
}
}
});
#else
try
{
ushort* pDepth = (ushort*)this.depthGenerator.DepthMapPtr.ToPointer();
ushort* pLabels = userInformation;
double pixel;
Color labelColor;
ushort label;
double depthMax = (float)depthGenerator.DeviceMaxDepth;
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pLabels, pDest += 3, pDepth++)
{
pDest[0] = pDest[1] = pDest[2] = 0;
label = *pLabels;
if (drawBackground ||
*pLabels != 0 && !background_users.Contains(*pLabels))
{
labelColor = Color.White;
if (label != 0 && drawHighlight)
{
labelColor = COLORS[label % NCOLORS];
}
//pixel = ((*pDepth) / depthMax * 255.0);
pixel = (byte)histogram[*pDepth];
pDest[2] = (byte)(pixel * (labelColor.B / 256.0));
pDest[1] = (byte)(pixel * (labelColor.G / 256.0));
pDest[0] = (byte)(pixel * (labelColor.R / 256.0));
}
}
}
}
finally
{
bitmap.UnlockBits(data);
}
#endif
}
public void SetData(DepthMetaData depthMD, UInt32 frameID, UInt64 timestamp)
{
SetData(frameID, timestamp, depthMD.DataSize, depthMD.DepthMapPtr);
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
lock (this)
{
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
ushort* pDepth = (ushort*)this.depth.DepthMapPtr.ToPointer();
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, pDest += 3)
{
byte pixel = (byte)this.histogram[*pDepth];
pDest[0] = 0;
pDest[1] = pixel;
pDest[2] = pixel;
}
}
this.bitmap.UnlockBits(data);
}
this.Invalidate();
}
}
private unsafe void ReaderThread()
{
DepthMetaData depthMD = new DepthMetaData();
while (this.shouldRun)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
lock (this)
{
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
if (this.shouldDrawPixels)
{
ushort *pDepth = (ushort *)this.depth.DepthMapPtr.ToPointer();
ushort *pLabels = (ushort *)this.userGenerator.GetUserPixels(0).LabelMapPtr.ToPointer();
// set pixels
for (int y = 0; y < depthMD.YRes; ++y)
{
byte *pDest = (byte *)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, ++pLabels, pDest += 3)
{
pDest[0] = pDest[1] = pDest[2] = 0;
ushort label = *pLabels;
if (this.shouldDrawBackground || *pLabels != 0)
{
Color labelColor = Color.White;
if (label != 0)
{
labelColor = colors[label % ncolors];
}
byte pixel = (byte)this.histogram[*pDepth];
pDest[0] = (byte)(pixel * (labelColor.B / 256.0));
pDest[1] = (byte)(pixel * (labelColor.G / 256.0));
pDest[2] = (byte)(pixel * (labelColor.R / 256.0));
}
}
}
}
this.bitmap.UnlockBits(data);
Graphics g = Graphics.FromImage(this.bitmap);
int[] users = this.userGenerator.GetUsers();
foreach (int user in users)
{
if (this.shouldPrintID)
{
Point3D com = this.userGenerator.GetCoM(user);
com = this.depth.ConvertRealWorldToProjective(com);
string label = "";
if (!this.shouldPrintState)
{
label += user;
}
else if (this.skeletonCapbility.IsTracking(user))
{
label += user + " - Tracking";
}
else if (this.skeletonCapbility.IsCalibrating(user))
{
label += user + " - Calibrating...";
}
else
{
label += user + " - Looking for pose";
}
g.DrawString(label, new Font("Arial", 6), new SolidBrush(anticolors[user % ncolors]), com.X, com.Y);
}
if (this.shouldDrawSkeleton && this.skeletonCapbility.IsTracking(user))
{
// if (this.skeletonCapbility.IsTracking(user))
DrawSkeleton(g, anticolors[user % ncolors], user);
}
}
g.Dispose();
}
this.Invalidate();
}
}
private unsafe void ReaderThread()
{
while (this.shouldRun)
{
lock (this)
{
try
{
this.context.WaitOneUpdateAll(this.depth);
}
catch (Exception)
{
}
if (this.shouldDrawPixels)
{
DepthMetaData depthMD = new DepthMetaData();
this.depth.GetMetaData(depthMD);
CalcHist(depthMD);
Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format32bppArgb);
ushort* pDepth = (ushort*)this.depth.GetDepthMapPtr().ToPointer();
ushort* pLabels = (ushort*)this.userGenerator.GetUserPixels(0).SceneMapPtr.ToPointer();
// set pixels
for (int y = 0; y < texH; ++y)
{
byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
for (int x = 0; x < texW; ++x, ++pDepth, ++pLabels, pDest += 4)
{
pDest[0] = pDest[1] = pDest[2]=0;
pDest[3] = 255;
ushort label = *pLabels;
if (this.shouldDrawBackground || *pLabels != 0)
{
Color labelColor = Color.White;
if (label != 0 && drawLabels)
{
if (setCustomColors)
{
labelColor = setUserColor(label);
}
else {
labelColor = colors[label % ncolors];
}
//
}
byte pixel = (byte)this.histogram[*pDepth];
pDest[0] = (byte)(pixel * (labelColor.B / 256.0));
pDest[1] = (byte)(pixel * (labelColor.G / 256.0));
pDest[2] = (byte)(pixel * (labelColor.R / 256.0));
}
}
}
this.bitmap.UnlockBits(data);
reading = true;
bitmapPointer = data.Scan0;
reading = false;
uint[] users = this.userGenerator.GetUsers();
//trackedUsers = new List<uint>();
Graphics g = Graphics.FromImage(this.bitmap);
foreach (uint user in users)
{
#region labels
if (this.shouldPrintID)
{
Point3D com = this.userGenerator.GetCoM(user);
com = this.depth.ConvertRealWorldToProjective(com);
string label = "";
if (!this.shouldPrintState)
label += user;
else if (this.skeletonCapbility.IsTracking(user))
label += user + " - Tracking";
else if (this.skeletonCapbility.IsCalibrating(user))
label += user + " - Calibrating...";
else
label += user + " - Looking for pose";
g.DrawString(label, new System.Drawing.Font("Arial", 10), new SolidBrush(anticolors[user % ncolors]), com.X, com.Y);
}
#endregion
}
g.Dispose();
}
uint[] userz = this.userGenerator.GetUsers();
trackedUsers = new List<uint>();
#region isolatemeplease
foreach (uint user in userz)
{
if (this.skeletonCapbility.IsTracking(user))
{
trackedUsers.Add(user);
}
}
int trackedUsersCount = trackedUsers.Count;
/// tracked user ID
FTrackedUsersId.SliceCount = trackedUsersCount;
for (int i = 0; i < trackedUsersCount; i++)
{
FTrackedUsersId[i] = (int)trackedUsers[i];
}
////////////// Position spreading count
FJointsPosition.SliceCount = trackedUsersCount;
int index = 0;
foreach (uint trackedUser in trackedUsers)
{
getSkeleton(trackedUser, index);
index++;
}
#endregion
}
}
}
public void SetData(DepthMetaData depthMD)
{
SetData(depthMD, depthMD.FrameID, depthMD.Timestamp);
}