diff --git a/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.Designer.cs b/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.Designer.cs index 1f6de77..e2e0990 100644 --- a/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.Designer.cs +++ b/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.Designer.cs @@ -37,9 +37,9 @@ private void InitializeComponent() this.bodyDataLabel = new System.Windows.Forms.Label(); this.pointCloudDataLabel = new System.Windows.Forms.Label(); this.valueLabel = new System.Windows.Forms.Label(); - this.ColorData = new System.Windows.Forms.CheckBox(); - this.BodyData = new System.Windows.Forms.CheckBox(); - this.PointCloudData = new System.Windows.Forms.CheckBox(); + this.ColorDataCheckBox = new System.Windows.Forms.CheckBox(); + this.BodyDataCheckBox = new System.Windows.Forms.CheckBox(); + this.PointCloudDataCheckBox = new System.Windows.Forms.CheckBox(); this.label1 = new System.Windows.Forms.Label(); this.startButton = new System.Windows.Forms.Button(); this.trayIcon = new System.Windows.Forms.NotifyIcon(this.components); @@ -73,9 +73,9 @@ private void InitializeComponent() this.tableLayoutPanel1.Controls.Add(this.bodyDataLabel, 0, 2); this.tableLayoutPanel1.Controls.Add(this.pointCloudDataLabel, 0, 3); this.tableLayoutPanel1.Controls.Add(this.valueLabel, 1, 0); - this.tableLayoutPanel1.Controls.Add(this.ColorData, 1, 1); - this.tableLayoutPanel1.Controls.Add(this.BodyData, 1, 2); - this.tableLayoutPanel1.Controls.Add(this.PointCloudData, 1, 3); + this.tableLayoutPanel1.Controls.Add(this.ColorDataCheckBox, 1, 1); + this.tableLayoutPanel1.Controls.Add(this.BodyDataCheckBox, 1, 2); + this.tableLayoutPanel1.Controls.Add(this.PointCloudDataCheckBox, 1, 3); this.tableLayoutPanel1.Location = new System.Drawing.Point(3, 10); this.tableLayoutPanel1.Margin = new System.Windows.Forms.Padding(3, 10, 3, 3); this.tableLayoutPanel1.Name = "tableLayoutPanel1"; @@ -136,36 +136,36 @@ private void InitializeComponent() // // ColorData // - this.ColorData.AutoSize = true; - this.ColorData.Location = new System.Drawing.Point(144, 37); - this.ColorData.Name = "ColorData"; - this.ColorData.Size = new System.Drawing.Size(80, 17); - this.ColorData.TabIndex = 5; - this.ColorData.Text = "checkBox1"; - this.ColorData.UseVisualStyleBackColor = true; - this.ColorData.CheckedChanged += new System.EventHandler(this.ColorData_CheckedChanged); + this.ColorDataCheckBox.AutoSize = true; + this.ColorDataCheckBox.Location = new System.Drawing.Point(144, 37); + this.ColorDataCheckBox.Name = "ColorData"; + this.ColorDataCheckBox.Size = new System.Drawing.Size(80, 17); + this.ColorDataCheckBox.TabIndex = 5; + this.ColorDataCheckBox.Text = "checkBox1"; + this.ColorDataCheckBox.UseVisualStyleBackColor = true; + this.ColorDataCheckBox.CheckedChanged += new System.EventHandler(this.ColorData_CheckedChanged); // // BodyData // - this.BodyData.AutoSize = true; - this.BodyData.Location = new System.Drawing.Point(144, 70); - this.BodyData.Name = "BodyData"; - this.BodyData.Size = new System.Drawing.Size(80, 17); - this.BodyData.TabIndex = 6; - this.BodyData.Text = "checkBox2"; - this.BodyData.UseVisualStyleBackColor = true; - this.BodyData.CheckedChanged += new System.EventHandler(this.BodyData_CheckedChanged); + this.BodyDataCheckBox.AutoSize = true; + this.BodyDataCheckBox.Location = new System.Drawing.Point(144, 70); + this.BodyDataCheckBox.Name = "BodyData"; + this.BodyDataCheckBox.Size = new System.Drawing.Size(80, 17); + this.BodyDataCheckBox.TabIndex = 6; + this.BodyDataCheckBox.Text = "checkBox2"; + this.BodyDataCheckBox.UseVisualStyleBackColor = true; + this.BodyDataCheckBox.CheckedChanged += new System.EventHandler(this.BodyData_CheckedChanged); // // PointCloudData // - this.PointCloudData.AutoSize = true; - this.PointCloudData.Location = new System.Drawing.Point(144, 103); - this.PointCloudData.Name = "PointCloudData"; - this.PointCloudData.Size = new System.Drawing.Size(80, 17); - this.PointCloudData.TabIndex = 7; - this.PointCloudData.Text = "checkBox3"; - this.PointCloudData.UseVisualStyleBackColor = true; - this.PointCloudData.CheckedChanged += new System.EventHandler(this.PointCloudData_CheckedChanged); + this.PointCloudDataCheckBox.AutoSize = true; + this.PointCloudDataCheckBox.Location = new System.Drawing.Point(144, 103); + this.PointCloudDataCheckBox.Name = "PointCloudData"; + this.PointCloudDataCheckBox.Size = new System.Drawing.Size(80, 17); + this.PointCloudDataCheckBox.TabIndex = 7; + this.PointCloudDataCheckBox.Text = "checkBox3"; + this.PointCloudDataCheckBox.UseVisualStyleBackColor = true; + this.PointCloudDataCheckBox.CheckedChanged += new System.EventHandler(this.PointCloudData_CheckedChanged); // // label1 // @@ -243,9 +243,9 @@ private void InitializeComponent() private System.Windows.Forms.Label bodyDataLabel; private System.Windows.Forms.Label pointCloudDataLabel; private System.Windows.Forms.Label valueLabel; - private System.Windows.Forms.CheckBox ColorData; - private System.Windows.Forms.CheckBox BodyData; - private System.Windows.Forms.CheckBox PointCloudData; + private System.Windows.Forms.CheckBox ColorDataCheckBox; + private System.Windows.Forms.CheckBox BodyDataCheckBox; + private System.Windows.Forms.CheckBox PointCloudDataCheckBox; private System.Windows.Forms.NotifyIcon trayIcon; private System.Windows.Forms.ContextMenuStrip contextMenuStrip; private System.Windows.Forms.ToolStripMenuItem closeToolStripMenuItem; diff --git a/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.cs b/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.cs index 137dfc9..a7f3190 100644 --- a/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.cs +++ b/Windows/Kinect Anywhere/Kinect Anywhere/Kinect Anywhere.cs @@ -21,18 +21,28 @@ public partial class KinectAnywhereForm : Form /// private MultiSourceFrameReader multiFrameSourceReader; - private List pointCloudData = new List(); - private List bodyFrameData = new List(); - + private int pointCloudSize = 0; + private byte[] pointCloudData; private byte[] colorFrameData; + private Body[] bodyArray; + + private List bodyFrameData = new List(); + /// - /// Intermediate storage for the extended depth data received from the camera in the current frame + /// Intermediate storage for the depth data /// private IntPtr depthFrameData; + + /// + /// Intermediate storage for the 3D camera points + /// private IntPtr camerSpacePoints; + + /// + /// Intermediate storage for 2D color points + /// private IntPtr colorSpacePoints; - private Body[] bodyArray; /// /// Intermediate storage for the color data received from the camera in 32bit color @@ -116,7 +126,71 @@ private void OnMultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEve // Copy color data (using Bgra format) colorFrame.CopyConvertedFrameDataToIntPtr(colorPixels, COLOR_PIXEL_BYTES, ColorImageFormat.Bgra); - ProcessFrames(ref depthFrame, ref bodyFrame); + if (ColorDataCheckBox.Checked) + { + Marshal.Copy(colorPixels, colorFrameData, 8, (int)COLOR_PIXEL_BYTES); + colorFramePublisher.Send(new ZFrame(colorFrameData)); + } + + if (BodyDataCheckBox.Checked) + { + // Copy data for Body tracking + bodyArray = new Body[bodyFrame.BodyCount]; + bodyFrame.GetAndRefreshBodyData(bodyArray); + + // Remove old bodies + bodyFrameData.Clear(); + + //At this point, we are just reserving 4 bytes for storing 'bodyCount' and we are going to modify it later + AddArrayToList(ref bodyFrameData, new byte[4] { 0, 0, 0, 0 }); + + int bodyCount = 0; + foreach (Body body in bodyArray) + { + if (!body.IsTracked) + { + continue; + } + + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(body.TrackingId));//add 8 bytes for ulong TrackingId + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(ALL_JOINTS.Length));//add 4 bytes for int TrackingId + + foreach (JointType jointType in ALL_JOINTS) + { + var joint = body.Joints[jointType]; + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes((int)joint.TrackingState));//add 4 bytes for int TrackingState + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes((int)joint.JointType));//add 4 bytes for int JointType + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.X));//add 4 bytes for float X + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.Y));//add 4 bytes for float Y + AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.Z));//add 4 bytes for float Z + } + bodyCount++; + } + + var bodyCountBytes = BitConverter.GetBytes(bodyCount);//4 bytes + UpdateList(bodyCountBytes, ref bodyFrameData); + + bodyFramePublisher.Send(new ZFrame(bodyFrameData.ToArray())); + } + + if (PointCloudDataCheckBox.Checked) + { + depthFrame.CopyFrameDataToIntPtr(depthFrameData, DEPTH_FRAME_BYTES); + coordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(depthFrameData, DEPTH_FRAME_BYTES, camerSpacePoints, CAMERA_SPACE_BYTES); + coordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(depthFrameData, DEPTH_FRAME_BYTES, colorSpacePoints, COLOR_SPACE_BYTES); + + // Remove old points + ClearPointCloud(); + + //At this point, we are just reserving 4 bytes for storing 'validPointsInCloud' and we are going to modify it later + AddPointsToCloud(new byte[4] { 0, 0, 0, 0 }); + + ComposePointCloud(); + + GetNonEmptyPointCloud(out byte[] pointCloud); + pointCloudPublisher.Send(new ZFrame(pointCloud)); + } + } finally { @@ -137,113 +211,95 @@ private void OnMultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEve } } - unsafe public void ProcessFrames(ref DepthFrame depthFrame, ref BodyFrame bodyFrame) + private void ClearPointCloud() { - if (PointCloudData.Checked) - { - depthFrame.CopyFrameDataToIntPtr(depthFrameData, DEPTH_FRAME_BYTES); - coordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(depthFrameData, DEPTH_FRAME_BYTES, camerSpacePoints, CAMERA_SPACE_BYTES); - coordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(depthFrameData, DEPTH_FRAME_BYTES, colorSpacePoints, COLOR_SPACE_BYTES); - - int validPointCount = 0; - - // Remove old points - pointCloudData.Clear(); - - //At this point, we are just reserving 4 bytes for storing 'validPointCount' and we are going to modify it later - AddArrayToList(ref pointCloudData, new byte[4] { 0, 0, 0, 0 }); - - var colorSpacePoint = (ColorSpacePoint*)colorSpacePoints; - var camerSpacePoint = (CameraSpacePoint*)camerSpacePoints; - var colorPixel = (byte*)colorPixels; - - for (var index = 0; index < DEPTH_FRAME_LENGTH; index++) - { - int colorX = (int)Math.Floor(colorSpacePoint[index].X); - int colorY = (int)Math.Floor(colorSpacePoint[index].Y); - - // Not every depth colorPixel has a corresponding color colorPixel. - // So always check whether colorX, colorY are valid or not - if (colorX < 0 || colorX >= COLOR_FRAME_WIDTH || colorY < 0 || colorY >= COLOR_FRAME_HEIGHT) - { - continue; - } - - // Now colorX, colorY are colorPixel coordinates in colorspace, - // so to get the index of the corresponding colorPixel, - // we need to multiply colorY by COLOR_FRAME_WIDTH - int pixelsBaseIndex = (colorY * COLOR_FRAME_WIDTH + colorX) * COLOR_BYTES_PER_PIXEL; - - byte blue = colorPixel[pixelsBaseIndex]; - byte green = colorPixel[pixelsBaseIndex + 1]; - byte red = colorPixel[pixelsBaseIndex + 2]; - byte alpha = colorPixel[pixelsBaseIndex + 3]; - - AddArrayToList(ref pointCloudData, BitConverter.GetBytes(camerSpacePoint[index].X)); //add 4 bytes for float x - AddArrayToList(ref pointCloudData, BitConverter.GetBytes(camerSpacePoint[index].Y)); //add 4 bytes for float y - AddArrayToList(ref pointCloudData, BitConverter.GetBytes(camerSpacePoint[index].Z)); //add 4 bytes for float z + pointCloudSize = 0; + } - uint bgra = (uint)((blue << 24) | (green << 16) | (red << 8) | alpha); - AddArrayToList(ref pointCloudData, BitConverter.GetBytes(bgra)); //add 4 bytes for unsigned int + unsafe private void AddUintPointToCloud(uint point) + { + byte* bytePtr = (byte*)&point; - //Added 16 bytes in one iteration - validPointCount++; - } + for (int i = 0; i < sizeof(uint); i++) + { + pointCloudData[pointCloudSize] = bytePtr[i]; + pointCloudSize++; + } + } - var validPointBytes = BitConverter.GetBytes(validPointCount);//4 bytes - UpdateList(validPointBytes, ref pointCloudData); + unsafe private void AddFloatPointToCloud(float point) + { + byte* bytePtr = (byte*)&point; - pointCloudPublisher.Send(new ZFrame(pointCloudData.ToArray())); + for (int i = 0; i < sizeof(float); i++) + { + pointCloudData[pointCloudSize] = bytePtr[i]; + pointCloudSize++; } + } - if (ColorData.Checked) + private void AddPointsToCloud(byte[] points) + { + for (int i = 0; i < points.Length; i++) { - Marshal.Copy(colorPixels, colorFrameData, 8, (int)COLOR_PIXEL_BYTES); - colorFramePublisher.Send(new ZFrame(colorFrameData)); + pointCloudData[pointCloudSize] = points[i]; + pointCloudSize++; } + } + + private void GetNonEmptyPointCloud(out byte[] pointCloud) + { + int validPointCount = (pointCloudSize - sizeof(int)) / (4 * sizeof(int)); + byte[] validPointCountBytes = BitConverter.GetBytes(validPointCount); + + pointCloud = new byte[pointCloudSize]; + Array.Copy(pointCloudData, 0, pointCloud, 0, pointCloudSize); - if (BodyData.Checked) + for (int i = 0; i < validPointCountBytes.Length; i++) { - // Copy data for Body tracking - bodyArray = new Body[bodyFrame.BodyCount]; - bodyFrame.GetAndRefreshBodyData(bodyArray); + pointCloud[i] = validPointCountBytes[i]; + } + } - // Remove old bodies - bodyFrameData.Clear(); + unsafe private void ComposePointCloud() + { + var colorSpacePoint = (ColorSpacePoint*)colorSpacePoints; + var camerSpacePoint = (CameraSpacePoint*)camerSpacePoints; + var colorPixel = (byte*)colorPixels; - //At this point, we are just reserving 4 bytes for storing 'bodyCount' and we are going to modify it later - AddArrayToList(ref bodyFrameData, new byte[4] { 0, 0, 0, 0 }); + for (var index = 0; index < DEPTH_FRAME_LENGTH; index++) + { + int colorX = (int)Math.Floor(colorSpacePoint[index].X); + int colorY = (int)Math.Floor(colorSpacePoint[index].Y); - int bodyCount = 0; - foreach (Body body in bodyArray) + // Not every depth colorPixel has a corresponding color colorPixel. + // So always check whether colorX, colorY are valid or not + if (colorX < 0 || colorX >= COLOR_FRAME_WIDTH || colorY < 0 || colorY >= COLOR_FRAME_HEIGHT) { - if (!body.IsTracked) - { - continue; - } + continue; + } - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(body.TrackingId));//add 8 bytes for ulong TrackingId - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(ALL_JOINTS.Length));//add 4 bytes for int TrackingId + // Now colorX, colorY are colorPixel coordinates in colorspace, + // so to get the index of the corresponding colorPixel, + // we need to multiply colorY by COLOR_FRAME_WIDTH + int pixelsBaseIndex = (colorY * COLOR_FRAME_WIDTH + colorX) * COLOR_BYTES_PER_PIXEL; - foreach (JointType jointType in ALL_JOINTS) - { - var joint = body.Joints[jointType]; - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes((int)joint.TrackingState));//add 4 bytes for int TrackingState - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes((int)joint.JointType));//add 4 bytes for int JointType - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.X));//add 4 bytes for float X - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.Y));//add 4 bytes for float Y - AddArrayToList(ref bodyFrameData, BitConverter.GetBytes(joint.Position.Z));//add 4 bytes for float Z - } - bodyCount++; - } + byte blue = colorPixel[pixelsBaseIndex]; + byte green = colorPixel[pixelsBaseIndex + 1]; + byte red = colorPixel[pixelsBaseIndex + 2]; + byte alpha = colorPixel[pixelsBaseIndex + 3]; + + AddFloatPointToCloud(camerSpacePoint[index].X); //add 4 bytes for float x + AddFloatPointToCloud(camerSpacePoint[index].Y); //add 4 bytes for float y + AddFloatPointToCloud(camerSpacePoint[index].Z); //add 4 bytes for float z - var bodyCountBytes = BitConverter.GetBytes(bodyCount);//4 bytes - UpdateList(bodyCountBytes, ref bodyFrameData); + uint bgra = (uint)((blue << 24) | (green << 16) | (red << 8) | alpha); + AddUintPointToCloud(bgra); //add 4 bytes for unsigned int - bodyFramePublisher.Send(new ZFrame(bodyFrameData.ToArray())); + //Added 16 bytes in one iteration } } - + byte[] nnn = new byte[4] { 1, 2, 3, 4 }; private void AddArrayToList(ref List destination, byte[] source) { for (int i = 0; i < source.Length; i++) @@ -267,19 +323,19 @@ private void CloseToolStripMenuItem_Click(object sender, EventArgs e) private void SetupPublishers() { - if (ColorData.Checked) + if (ColorDataCheckBox.Checked) { colorFramePublisher.SetOption(ZSocketOption.CONFLATE, 1); colorFramePublisher.Bind("tcp://*:10000"); } - if (BodyData.Checked) + if (BodyDataCheckBox.Checked) { bodyFramePublisher.SetOption(ZSocketOption.CONFLATE, 1); bodyFramePublisher.Bind("tcp://*:10001"); } - if (PointCloudData.Checked) + if (PointCloudDataCheckBox.Checked) { pointCloudPublisher.SetOption(ZSocketOption.CONFLATE, 1); pointCloudPublisher.Bind("tcp://*:10002"); @@ -288,13 +344,13 @@ private void SetupPublishers() private void StartButton_Click(object sender, EventArgs e) { - if (!ColorData.Checked && !BodyData.Checked && !PointCloudData.Checked) + if (!ColorDataCheckBox.Checked && !BodyDataCheckBox.Checked && !PointCloudDataCheckBox.Checked) { MessageBox.Show("Please select alteast one data.", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Warning); } else { - ConfigurationManager.SaveConfiguration(ColorData.Checked, BodyData.Checked, PointCloudData.Checked); + ConfigurationManager.SaveConfiguration(ColorDataCheckBox.Checked, BodyDataCheckBox.Checked, PointCloudDataCheckBox.Checked); kinectSensor = KinectSensor.GetDefault(); @@ -330,6 +386,8 @@ private void StartButton_Click(object sender, EventArgs e) COLOR_PIXEL_BYTES = (uint)(COLOR_FRAME_WIDTH * COLOR_FRAME_HEIGHT * COLOR_BYTES_PER_PIXEL); colorPixels = Marshal.AllocHGlobal((int)COLOR_PIXEL_BYTES); + pointCloudData = new byte[4 * sizeof(int) * DEPTH_FRAME_LENGTH]; //XYZBGRA + colorFrameData = new byte[(2 * sizeof(int)) + COLOR_PIXEL_BYTES]; var widthBytes = BitConverter.GetBytes(COLOR_FRAME_WIDTH); // 4 bytes @@ -346,29 +404,29 @@ private void KinectAnywhereForm_Load(object sender, EventArgs e) { var configuration = ConfigurationManager.GetConfiguration(); - ColorData.Checked = configuration[ConfigurationManager.COLOR_KEY]; - ColorData.Text = Convert.ToString(ColorData.Checked); + ColorDataCheckBox.Checked = configuration[ConfigurationManager.COLOR_KEY]; + ColorDataCheckBox.Text = Convert.ToString(ColorDataCheckBox.Checked); - BodyData.Checked = configuration[ConfigurationManager.BODY_KEY]; - BodyData.Text = Convert.ToString(BodyData.Checked); + BodyDataCheckBox.Checked = configuration[ConfigurationManager.BODY_KEY]; + BodyDataCheckBox.Text = Convert.ToString(BodyDataCheckBox.Checked); - PointCloudData.Checked = configuration[ConfigurationManager.POINT_CLOUD_KEY]; - PointCloudData.Text = Convert.ToString(PointCloudData.Checked); + PointCloudDataCheckBox.Checked = configuration[ConfigurationManager.POINT_CLOUD_KEY]; + PointCloudDataCheckBox.Text = Convert.ToString(PointCloudDataCheckBox.Checked); } private void ColorData_CheckedChanged(object sender, EventArgs e) { - ColorData.Text = Convert.ToString(ColorData.Checked); + ColorDataCheckBox.Text = Convert.ToString(ColorDataCheckBox.Checked); } private void BodyData_CheckedChanged(object sender, EventArgs e) { - BodyData.Text = Convert.ToString(BodyData.Checked); + BodyDataCheckBox.Text = Convert.ToString(BodyDataCheckBox.Checked); } private void PointCloudData_CheckedChanged(object sender, EventArgs e) { - PointCloudData.Text = Convert.ToString(PointCloudData.Checked); + PointCloudDataCheckBox.Text = Convert.ToString(PointCloudDataCheckBox.Checked); } private void KinectAnywhereForm_FormClosing(object sender, FormClosingEventArgs e) @@ -383,6 +441,7 @@ private void KinectAnywhereForm_FormClosing(object sender, FormClosingEventArgs DisposeSocket(colorFramePublisher); DisposeSocket(bodyFramePublisher); + DisposeIntPtr(colorPixels); DisposeIntPtr(depthFrameData); DisposeIntPtr(camerSpacePoints); DisposeIntPtr(colorSpacePoints); @@ -406,4 +465,4 @@ private void DisposeIntPtr(IntPtr intPtr) } } } -} +} \ No newline at end of file