public void getData()
{
context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
System.Windows.Media.Media3D.Point3D u_new, u_real, f, c;
c = new System.Windows.Media.Media3D.Point3D(0, 0, centerpointDepth);
OpenNI.Point3D newF;
newF = Change(depthMD, backgroundDepthMD); //returns the pixal pos
if (newF.X != 0 && newF.Z != 0 && newF.Z != 0)
{
newF = depth.ConvertProjectiveToRealWorld(newF);
f = new System.Windows.Media.Media3D.Point3D(newF.X, newF.Y, newF.Z); // the point of intress
u_new = Translation(f, c); //A translation that brings point 1 to the origin
float d = Distance(scene.Floor, newF);
double angleInRadians = (Math.PI / 180) * angleBetween;;
double angleInRadiansY = (Math.PI / 180) * angleBetweenY;;
var matrix = NewRotateAroundX(angleInRadians); // Rotation around the origin by the required angle
matrix = NewRotateAroundY(angleInRadiansY);
u_real = Multiplication(matrix, u_new);
//Console.WriteLine("new U {0}, {1}, {2} ", (int)u_new.X, (int)u_new.Y, (int)u_new.Z);
//Console.WriteLine("u_real , {1}, {2} and {3} distance {4}", depthMD.FrameID, (int)u_real.X, (int)u_real.Y, (int)u_real.Z, d);
xYDepth = new System.Windows.Media.Media3D.Point3D(u_real.X, u_real.Y, d);
}
}
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]);
}
}
/// <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);
}
/// <summary>
/// Updates image and depth values.
/// </summary>
private unsafe void CameraThread()
{
while (_isRunning)
{
Context.WaitAndUpdateAll();
ImageGenerator.GetMetaData(_imgMD);
DepthGenerator.GetMetaData(_depthMD);
}
}
// 描画
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);
}
}
//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 void Reader()
{
while (!stop)
{
try
{
context.WaitAndUpdateAll();
imageGenerator.GetMetaData(imageData);
depthGenerator.GetMetaData(depthData);
}
catch (Exception) { }
}
}
// 描画
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 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 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);
}
}
public void Initialize()
{
string SAMPLE_XML_FILE = @"C:\Users\blub\Desktop\MusicWall2\KinectLibrary\KinectLibrary\bin\Release\SamplesConfig.xml";
context = Context.CreateFromXmlFile(SAMPLE_XML_FILE, out scriptNode);
scene = new SceneAnalyzer(context);
// DepthGenerator
depth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
if (depth == null)
{
//Console.WriteLine("Sample must have a depth generator!");
return;
}
mapMode = depth.MapOutputMode;
xRes = (int)mapMode.XRes;
yRes = (int)mapMode.YRes;
backgroundDepthMD = new int[xRes, yRes];
while (true)
{
context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
// SceneAnalyzer
if (depthMD.FrameID <= loadRuns)
{
Background(depthMD, backgroundDepthMD, depthMD.FullXRes, depthMD.FullYRes, loadRuns);
//if (depthMD.FrameID == 1)
// Console.WriteLine("loading background data... ", depthMD.FrameID * 10);
//Console.Write(" . ", (double)depthMD.FrameID / loadRuns * 100);
centerpointDepth = centerpointDepth + depthMD[(int)mapMode.XRes / 2, (int)mapMode.YRes / 2];
if (depthMD.FrameID == loadRuns)
{
centerpointDepth = centerpointDepth / loadRuns;
}
}
if (depthMD.FrameID == loadRuns)
{
scene = new SceneAnalyzer(context);
if (scene == null)
{
//Console.WriteLine("Retry!");
}
else
{
wallNorm = scene.Floor.Normal;
wallPoint = scene.Floor.Point;
//Console.WriteLine("\n Wall Normal : {0}, {1}, {2}", wallNorm.X, wallNorm.Y, wallNorm.Z);
//Console.WriteLine(" Wall Point: {0}, {1}, {2}", wallPoint.X, wallPoint.Y, wallPoint.Z);
Vector n1 = new Vector(0, 1);
Vector n2 = new Vector(wallNorm.Y, wallNorm.Z);
Vector n3 = new Vector(wallNorm.X, wallNorm.Z);
angleBetween = (double)Vector.AngleBetween(n1, n2); // need to get the angle for the last axis in degrees
angleBetweenY = (double)Vector.AngleBetween(n1, n3);
//Console.WriteLine(" Angle: {0}", angleBetween);
}
break;
}
}
}
/// <summary>
/// This method updates the image on the MainWindow page with the latest depth image.
/// </summary>
private unsafe void UpdateDepth()
{
//Reset Global Depth Values
globalMaximumDepth = 0;
globalMinimumDepth = 99999;
globalROIMaximumDepth = 0;
globalROIMinimumDepth = 99999;
// 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();
System.Drawing.Point point;
// 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 >> 1);
pDest[1] = (byte)(*pDepth >> 0);
pDest[2] = (byte)(*pDepth >> 2);
// Write vales to depth matrix.
// TODO: Check coordinate system. top/left = origin
depthMatrix[y, x] = *pDepth;
//Using line filters:
if (filterBoundariesExists)
{
point = new System.Drawing.Point(x, y); //declare current point
int temp0 = filterBoundary[0].Side(point);
int temp1 = filterBoundary[1].Side(point);
int temp2 = filterBoundary[2].Side(point);
int temp3 = filterBoundary[3].Side(point);
//if((filterBoundary[0].Side(point)<0)||(filterBoundary[1].Side(point)>0)||(filterBoundary[2].Side(point)>0)||(filterBoundary[3].Side(point)<0))
if (!((temp0 > 0) && (temp2 < 0) && (temp1 > 0) && (temp3 < 0)))
{
//Outside bounds, set to zero
depthMatrixROI[y, x] = 0;
}
else
{
//Inside the bounds of the table
if (depthMatrix[y, x] != 0)
{
if (backgroundImageExists)
{
//Background subtraction (scaled)
depthMatrixROI[y, x] = Math.Pow((backgroundDepthMatrix[y, x] - depthMatrix[y, x]), slider1.Value);
}
else
{
depthMatrixROI[y, x] = depthMatrix[y, x];
}
// ROI Min & Max
if ((depthMatrixROI[y, x] < globalROIMinimumDepth) && (depthMatrixROI[y, x] > 0))
{
globalROIMinimumDepth = depthMatrixROI[y, x];
}
if (depthMatrixROI[y, x] > globalROIMaximumDepth)
{
globalROIMaximumDepth = depthMatrixROI[y, x];
}
}
else
{
depthMatrixROI[y, x] = 1;
}
}
}
//Global Min & Max
if ((depthMatrix[y, x] < globalMinimumDepth) && (depthMatrix[y, x] > 0))
{
globalMinimumDepth = depthMatrix[y, x];
}
if (depthMatrix[y, x] > globalMaximumDepth)
{
globalMaximumDepth = depthMatrix[y, x];
}
} //end for - x
} //end for - y
this.bitmap.UnlockBits(data);
// Update the image to have the bitmap image we just copied
image1.Source = getBitmapImage(bitmap);
// Update display variables for Global Depth values
DisplayGlobalMaximumDepth = globalMaximumDepth;
DisplayGlobalMinimumDepth = globalMinimumDepth;
//if (backgroundImageExists)
//{
// //Matrix<Double> temp = new Matrix<Double>(480, 640);
// //temp = backgroundDepthMatrix - depthMatrix;
// //temp *= 10000;
// //temp.CopyTo(depthMatrixImage);
// Image<Gray, Double> temp = new Image<Gray, Double>(640, 480);
// depthMatrix.CopyTo(depthMatrixImage);
// temp = backgroundDepthImage - depthMatrixImage;
// //temp = temp.Pow(0.8);
// image2.Source = getBitmapImage(temp.Bitmap);
//}
if (filterBoundariesExists)
{
//Calculate the slope.
double m = ((255 - 5) / (globalROIMaximumDepth - globalROIMinimumDepth));
//Scale use depthMatrixROI to depthMatrixROIByte
for (int y = 0; y < depthMD.YRes; ++y)
{
for (int x = 0; x < depthMD.XRes; ++x)
{
if (depthMatrixROI[y, x] < 2) //Then this is outside the region.
{
depthMatrixROIByte[y, x] = 0;
}
else
{
depthMatrixROIByte[y, x] = (byte)((m * (depthMatrixROI[y, x] - globalROIMinimumDepth)) + 255);
}
}
}
depthMatrixROIByte.CopyTo(depthMatrixROIImage);
image2.Source = getBitmapImage(depthMatrixROIImage.Bitmap);
#region Blob Tracking Algorithm
//Binary Thresholded image, reduces noise even more.
depthMatrixROIImage = depthMatrixROIImage.ThresholdBinary(new Gray(slider2.Value), new Gray(255));
//using a foreground mask.
blobTracker.Process(depthMatrixROIImage, depthMatrixROIImage);
//blobTracker.Process(depthMatrixROIImage);
//Image<Gray, Byte> tempImage = new Image<Gray, Byte>(640, 480, new Gray(0));
//Image<Gray, Byte> tempImage = blobTracker.ForgroundMask;
//blobTracker.Process(depthMatrixROIImage, tempImage);
// Dimming
if (_dimming)
{
if (_dimCounter > progressBar1.Maximum)
{
Image <Bgr, Byte> temp = trackingImage - new Image <Bgr, Byte>(640, 480, new Bgr(slider_DimStep.Value, slider_DimStep.Value, slider_DimStep.Value));
//temp.CopyTo(trackingImage);
trackingImage = temp.Clone();
_dimCounter = 0;
}
else
{
_dimCounter++;
}
progressBar1.Value = _dimCounter;
}
// Tag each blob
foreach (MCvBlob blob in blobTracker)
{
try
{
// Draw Rectangle
//tempImage.Draw(System.Drawing.Rectangle.Round(blob), new Gray(250), 2);
trackingImage.Draw(System.Drawing.Rectangle.Round(blob), targetColour[blob.ID], 2);
// Draw Text
//tempImage.Draw(blob.ID.ToString(), ref font, System.Drawing.Point.Round(blob.Center), new Gray(200));
}
catch (IndexOutOfRangeException ex_Index)
{
MessageBox.Show(ex_Index.Message);
MessageBox.Show("Tracking limit reached, application terminating...");
this.Close();
}
}
//Set image3 to the blob tracking.
//image2.Source = getBitmapImage(depthMatrixROIImage.Bitmap);
image3.Source = getBitmapImage(trackingImage.Bitmap);
#endregion
}//end if(filterBoundariesExists)
else
{
// Create image
depthMatrix.CopyTo(depthMatrixImage);
image2.Source = getBitmapImage(depthMatrixImage.Bitmap);
}
}//end UpdateDepth()
