diff --git a/LiveScanServer/KinectSettings.cs b/LiveScanServer/KinectSettings.cs index fe5a063..075041e 100644 --- a/LiveScanServer/KinectSettings.cs +++ b/LiveScanServer/KinectSettings.cs @@ -33,8 +33,10 @@ namespace KinectServer public BindingList lMarkerPoses = new BindingList(); + public bool bStreamOnlyBodies = false; + public int nNumICPIterations = 10; - public int nMaxNumRefineIters = 10; + public int nNumRefineIters = 2; public bool bMergeScansForSave = false; public KinectSettings() @@ -87,6 +89,11 @@ namespace KinectServer lData.AddRange(bTemp); } + if (bStreamOnlyBodies) + lData.Add(1); + else + lData.Add(0); + return lData; } } diff --git a/LiveScanServer/MainWindowForm.cs b/LiveScanServer/MainWindowForm.cs index 968dd08..391ce32 100644 --- a/LiveScanServer/MainWindowForm.cs +++ b/LiveScanServer/MainWindowForm.cs @@ -316,16 +316,9 @@ namespace KinectServer //Use ICP to refine the sensor poses. //This part is explained in more detail in our article (name on top of this file). - float error = 1; - float oldError = 1; - bool firstPass = true; - int iter = 0; - while (true) - { - if (iter > oSettings.nMaxNumRefineIters) - break; - error = 0; + for (int refineIter = 0; refineIter < oSettings.nNumRefineIters; refineIter++) + { for (int i = 0; i < lAllFrameVertices.Count; i++) { List otherFramesVertices = new List(); @@ -345,25 +338,12 @@ namespace KinectServer Marshal.Copy(verts1, 0, pVerts1, verts1.Length); Marshal.Copy(verts2, 0, pVerts2, verts2.Length); - error += ICP(pVerts1, pVerts2, otherFramesVertices.Count / 3, lAllFrameVertices[i].Count / 3, Rs[i], Ts[i], oSettings.nNumICPIterations); + ICP(pVerts1, pVerts2, otherFramesVertices.Count / 3, lAllFrameVertices[i].Count / 3, Rs[i], Ts[i], oSettings.nNumICPIterations); Marshal.Copy(pVerts2, verts2, 0, verts2.Length); lAllFrameVertices[i].Clear(); - lAllFrameVertices[i].AddRange(verts2); + lAllFrameVertices[i].AddRange(verts2); } - - if (firstPass) - { - firstPass = false; - } - else - { - if (Math.Abs(oldError - error) / error < 0.01) - break; - } - oldError = error; - - iter++; } //Update the calibration data in client machines. diff --git a/LiveScanServer/SettingsForm.Designer.cs b/LiveScanServer/SettingsForm.Designer.cs index 1b86723..edf710a 100644 --- a/LiveScanServer/SettingsForm.Designer.cs +++ b/LiveScanServer/SettingsForm.Designer.cs @@ -71,11 +71,15 @@ this.grServer = new System.Windows.Forms.GroupBox(); this.txtRefinIters = new System.Windows.Forms.TextBox(); this.lbOuterIters = new System.Windows.Forms.Label(); + this.grBody = new System.Windows.Forms.GroupBox(); + this.chBodyData = new System.Windows.Forms.CheckBox(); + this.label1 = new System.Windows.Forms.Label(); this.grClient.SuspendLayout(); this.grMarkers.SuspendLayout(); this.grBounding.SuspendLayout(); this.grFiltering.SuspendLayout(); this.grServer.SuspendLayout(); + this.grBody.SuspendLayout(); this.SuspendLayout(); // // lbMerge @@ -90,7 +94,7 @@ // chMerge // this.chMerge.AutoSize = true; - this.chMerge.Location = new System.Drawing.Point(91, 48); + this.chMerge.Location = new System.Drawing.Point(98, 48); this.chMerge.Name = "chMerge"; this.chMerge.Size = new System.Drawing.Size(86, 17); this.chMerge.TabIndex = 23; @@ -109,7 +113,7 @@ // // txtICPIters // - this.txtICPIters.Location = new System.Drawing.Point(91, 23); + this.txtICPIters.Location = new System.Drawing.Point(98, 23); this.txtICPIters.Name = "txtICPIters"; this.txtICPIters.Size = new System.Drawing.Size(38, 20); this.txtICPIters.TabIndex = 25; @@ -117,12 +121,13 @@ // // grClient // + this.grClient.Controls.Add(this.grBody); this.grClient.Controls.Add(this.grMarkers); this.grClient.Controls.Add(this.grBounding); this.grClient.Controls.Add(this.grFiltering); this.grClient.Location = new System.Drawing.Point(12, 12); this.grClient.Name = "grClient"; - this.grClient.Size = new System.Drawing.Size(632, 212); + this.grClient.Size = new System.Drawing.Size(632, 327); this.grClient.TabIndex = 43; this.grClient.TabStop = false; this.grClient.Text = "KinectClient settings"; @@ -471,7 +476,7 @@ this.grServer.Controls.Add(this.txtICPIters); this.grServer.Controls.Add(this.chMerge); this.grServer.Controls.Add(this.lbICPIters); - this.grServer.Location = new System.Drawing.Point(12, 230); + this.grServer.Location = new System.Drawing.Point(12, 345); this.grServer.Name = "grServer"; this.grServer.Size = new System.Drawing.Size(632, 78); this.grServer.TabIndex = 44; @@ -480,7 +485,7 @@ // // txtRefinIters // - this.txtRefinIters.Location = new System.Drawing.Point(271, 23); + this.txtRefinIters.Location = new System.Drawing.Point(266, 23); this.txtRefinIters.Name = "txtRefinIters"; this.txtRefinIters.Size = new System.Drawing.Size(38, 20); this.txtRefinIters.TabIndex = 27; @@ -489,17 +494,48 @@ // lbOuterIters // this.lbOuterIters.AutoSize = true; - this.lbOuterIters.Location = new System.Drawing.Point(135, 26); + this.lbOuterIters.Location = new System.Drawing.Point(142, 26); this.lbOuterIters.Name = "lbOuterIters"; - this.lbOuterIters.Size = new System.Drawing.Size(139, 13); + this.lbOuterIters.Size = new System.Drawing.Size(118, 13); this.lbOuterIters.TabIndex = 26; - this.lbOuterIters.Text = "Max num of refinement iters:"; + this.lbOuterIters.Text = "Num of refinement iters:"; + // + // grBody + // + this.grBody.Controls.Add(this.label1); + this.grBody.Controls.Add(this.chBodyData); + this.grBody.Location = new System.Drawing.Point(9, 211); + this.grBody.Name = "grBody"; + this.grBody.Size = new System.Drawing.Size(249, 110); + this.grBody.TabIndex = 47; + this.grBody.TabStop = false; + this.grBody.Text = "Body data"; + // + // chBodyData + // + this.chBodyData.AutoSize = true; + this.chBodyData.Location = new System.Drawing.Point(11, 19); + this.chBodyData.Name = "chBodyData"; + this.chBodyData.Size = new System.Drawing.Size(113, 17); + this.chBodyData.TabIndex = 19; + this.chBodyData.Text = "stream only bodies"; + this.chBodyData.UseVisualStyleBackColor = true; + this.chBodyData.CheckedChanged += new System.EventHandler(this.chBodyData_CheckedChanged); + // + // label1 + // + this.label1.AutoSize = true; + this.label1.Location = new System.Drawing.Point(11, 43); + this.label1.Name = "label1"; + this.label1.Size = new System.Drawing.Size(187, 13); + this.label1.TabIndex = 20; + this.label1.Text = "More features will be added here soon"; // // SettingsForm // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.ClientSize = new System.Drawing.Size(656, 319); + this.ClientSize = new System.Drawing.Size(656, 435); this.Controls.Add(this.grServer); this.Controls.Add(this.grClient); this.FormBorderStyle = System.Windows.Forms.FormBorderStyle.FixedSingle; @@ -516,6 +552,8 @@ this.grFiltering.PerformLayout(); this.grServer.ResumeLayout(false); this.grServer.PerformLayout(); + this.grBody.ResumeLayout(false); + this.grBody.PerformLayout(); this.ResumeLayout(false); } @@ -565,6 +603,9 @@ private System.Windows.Forms.Label lbZ; private System.Windows.Forms.Label lbY; private System.Windows.Forms.Label lbX; + private System.Windows.Forms.GroupBox grBody; + private System.Windows.Forms.CheckBox chBodyData; + private System.Windows.Forms.Label label1; } } \ No newline at end of file diff --git a/LiveScanServer/SettingsForm.cs b/LiveScanServer/SettingsForm.cs index 31436e6..a0da674 100644 --- a/LiveScanServer/SettingsForm.cs +++ b/LiveScanServer/SettingsForm.cs @@ -50,11 +50,12 @@ namespace KinectServer txtFilterDistance.Text = oSettings.fFilterThreshold.ToString(CultureInfo.InvariantCulture); lisMarkers.DataSource = oSettings.lMarkerPoses; - + + chBodyData.Checked = oSettings.bStreamOnlyBodies; chMerge.Checked = oSettings.bMergeScansForSave; txtICPIters.Text = oSettings.nNumICPIterations.ToString(); - txtRefinIters.Text = oSettings.nMaxNumRefineIters.ToString(); + txtRefinIters.Text = oSettings.nNumRefineIters.ToString(); } void UpdateClients() @@ -156,7 +157,7 @@ namespace KinectServer private void txtRefinIters_TextChanged(object sender, EventArgs e) { - Int32.TryParse(txtRefinIters.Text, out oSettings.nMaxNumRefineIters); + Int32.TryParse(txtRefinIters.Text, out oSettings.nNumRefineIters); } private void chMerge_CheckedChanged(object sender, EventArgs e) @@ -281,5 +282,11 @@ namespace KinectServer UpdateClients(); } } + + private void chBodyData_CheckedChanged(object sender, EventArgs e) + { + oSettings.bStreamOnlyBodies = chBodyData.Checked; + UpdateClients(); + } } } diff --git a/include/LiveScanClient/iCapture.h b/include/LiveScanClient/iCapture.h index 736f658..67b24ea 100644 --- a/include/LiveScanClient/iCapture.h +++ b/include/LiveScanClient/iCapture.h @@ -43,6 +43,7 @@ public: int nDepthFrameHeight, nDepthFrameWidth; UINT16 *pDepth; + BYTE *pBodyIndex; RGB *pColorRGBX; std::vector vBodies; }; \ No newline at end of file diff --git a/include/LiveScanClient/kinectCapture.h b/include/LiveScanClient/kinectCapture.h index 733a44a..56a3f8c 100644 --- a/include/LiveScanClient/kinectCapture.h +++ b/include/LiveScanClient/kinectCapture.h @@ -39,4 +39,5 @@ private: void GetDepthFrame(IMultiSourceFrame* pMultiFrame); void GetColorFrame(IMultiSourceFrame* pMultiFrame); void GetBodyFrame(IMultiSourceFrame* pMultiFrame); + void GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame); }; diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index 9bc9842..5d20ee2 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -40,6 +40,7 @@ private: bool m_bCalibrate; bool m_bFilter; + bool m_bStreamOnlyBodies; ICapture *pCapture; @@ -86,7 +87,7 @@ private: void SendFrame(vector vertices, vector RGB, vector body); void SocketThreadFunction(); - void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies); + void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies, BYTE* bodyIndex); void ShowFPS(); void ReadIPFromFile(); void WriteIPToFile(); diff --git a/src/LiveScanClient/iCapture.cpp b/src/LiveScanClient/iCapture.cpp index 79bc8b9..0a23997 100644 --- a/src/LiveScanClient/iCapture.cpp +++ b/src/LiveScanClient/iCapture.cpp @@ -25,6 +25,7 @@ ICapture::ICapture() pDepth = NULL; pColorRGBX = NULL; + pBodyIndex = NULL; } ICapture::~ICapture() @@ -40,4 +41,10 @@ ICapture::~ICapture() delete[] pColorRGBX; pColorRGBX = NULL; } + + if (pBodyIndex != NULL) + { + delete[] pBodyIndex; + pBodyIndex = NULL; + } } \ No newline at end of file diff --git a/src/LiveScanClient/kinectCapture.cpp b/src/LiveScanClient/kinectCapture.cpp index 651228b..0ca7e20 100644 --- a/src/LiveScanClient/kinectCapture.cpp +++ b/src/LiveScanClient/kinectCapture.cpp @@ -97,6 +97,7 @@ bool KinectCapture::AcquireFrame() GetDepthFrame(pMultiFrame); GetColorFrame(pMultiFrame); GetBodyFrame(pMultiFrame); + GetBodyIndexFrame(pMultiFrame); return true; @@ -217,4 +218,27 @@ void KinectCapture::GetBodyFrame(IMultiSourceFrame* pMultiFrame) SafeRelease(pBodyFrame); SafeRelease(pBodyFrameReference); +} + +void KinectCapture::GetBodyIndexFrame(IMultiSourceFrame* pMultiFrame) +{ + IBodyIndexFrameReference* pBodyIndexFrameReference = NULL; + IBodyIndexFrame* pBodyIndexFrame = NULL; + pMultiFrame->get_BodyIndexFrameReference(&pBodyIndexFrameReference); + HRESULT hr = pBodyIndexFrameReference->AcquireFrame(&pBodyIndexFrame); + + + if (SUCCEEDED(hr)) + { + if (pBodyIndex == NULL) + { + pBodyIndex = new BYTE[nDepthFrameHeight * nDepthFrameWidth]; + } + + UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth; + hr = pBodyIndexFrame->CopyFrameDataToArray(nBufferSize, pBodyIndex); + } + + SafeRelease(pBodyIndexFrame); + SafeRelease(pBodyIndexFrameReference); } \ No newline at end of file diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index c4e4176..7dd4e46 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -50,6 +50,7 @@ LiveScanClient::LiveScanClient() : m_pDepthCoordinatesOfColor(NULL), m_bCalibrate(false), m_bFilter(false), + m_bStreamOnlyBodies(false), m_bCaptureFrame(false), m_bConnected(false), m_bConfirmCaptured(false), @@ -195,7 +196,7 @@ void LiveScanClient::UpdateFrame() pCapture->MapDepthFrameToColorSpace(m_pColorCoordinatesOfDepth); { std::lock_guard lock(m_mSocketThreadMutex); - StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies); + StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinatesOfDepth, pCapture->pColorRGBX, pCapture->vBodies, pCapture->pBodyIndex); if (m_bCaptureFrame) { @@ -497,6 +498,9 @@ void LiveScanClient::HandleSocket() i += sizeof(int); } + m_bStreamOnlyBodies = (received[i] != 0); + i++; + //so that we do not lose the next character in the stream i--; } @@ -683,7 +687,7 @@ void LiveScanClient::SendFrame(vector vertices, vector RGB, vector } -void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies) +void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, vector &bodies, BYTE* bodyIndex) { std::vector goodVertices; std::vector goodColorPoints; @@ -692,6 +696,9 @@ void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color, for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++) { + if (m_bStreamOnlyBodies && bodyIndex[vertexIndex] >= bodies.size()) + continue; + if (vertices[vertexIndex].Z >= 0 && mapping[vertexIndex].Y >= 0 && mapping[vertexIndex].Y < pCapture->nColorFrameHeight) { Point3f temp = vertices[vertexIndex];