From e2a03f9c8ae3bef09e6803fccb27f62d97ae38e2 Mon Sep 17 00:00:00 2001
From: Marek Kowalski <makowals@microsoft.com>
Date: Fri, 16 Aug 2019 18:03:41 +0100
Subject: [PATCH] Added support for multiple Azure Kinects on same machine

---
 include/LiveScanClient/azureKinectCapture.h |  1 -
 include/LiveScanClient/calibration.h        |  4 ++--
 include/LiveScanClient/iCapture.h           |  2 ++
 src/LiveScanClient/azureKinectCapture.cpp   | 21 +++++++++++++--------
 src/LiveScanClient/calibration.cpp          | 10 ++++------
 src/LiveScanClient/liveScanClient.cpp       |  9 ++++++---
 6 files changed, 27 insertions(+), 20 deletions(-)

diff --git a/include/LiveScanClient/azureKinectCapture.h b/include/LiveScanClient/azureKinectCapture.h
index c0c5352..facd9b0 100644
--- a/include/LiveScanClient/azureKinectCapture.h
+++ b/include/LiveScanClient/azureKinectCapture.h
@@ -12,7 +12,6 @@ public:
 	~AzureKinectCapture();
 
 	bool Initialize();
-	bool Initialize(int deviceIdx);
 	bool AcquireFrame();
 	void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints);
 	void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints);
diff --git a/include/LiveScanClient/calibration.h b/include/LiveScanClient/calibration.h
index c16be34..b9cbebb 100644
--- a/include/LiveScanClient/calibration.h
+++ b/include/LiveScanClient/calibration.h
@@ -42,8 +42,8 @@ public:
 	~Calibration();
 
 	bool Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight);
-	bool LoadCalibration();
-	void SaveCalibration();
+	bool LoadCalibration(const string &serialNumber);
+	void SaveCalibration(const string &serialNumber);
 private:
 	IMarker *pDetector;
 	int nSampleCounter;
diff --git a/include/LiveScanClient/iCapture.h b/include/LiveScanClient/iCapture.h
index e39a334..6a85fdb 100644
--- a/include/LiveScanClient/iCapture.h
+++ b/include/LiveScanClient/iCapture.h
@@ -56,4 +56,6 @@ public:
 	BYTE *pBodyIndex;
 	RGB *pColorRGBX;
 	std::vector<Body> vBodies;
+
+	std::string serialNumber;
 };
\ No newline at end of file
diff --git a/src/LiveScanClient/azureKinectCapture.cpp b/src/LiveScanClient/azureKinectCapture.cpp
index 70aa04b..f8310f2 100644
--- a/src/LiveScanClient/azureKinectCapture.cpp
+++ b/src/LiveScanClient/azureKinectCapture.cpp
@@ -18,19 +18,19 @@ AzureKinectCapture::~AzureKinectCapture()
 }
 
 bool AzureKinectCapture::Initialize()
-{
-	return Initialize(K4A_DEVICE_DEFAULT);
-}
-
-bool AzureKinectCapture::Initialize(int deviceIdx)
 {
 	uint32_t count = k4a_device_get_installed_count();
+	int deviceIdx = 0;
 
 	kinectSensor = NULL;
-	if (K4A_FAILED(k4a_device_open(deviceIdx, &kinectSensor)))
+	while (K4A_FAILED(k4a_device_open(deviceIdx, &kinectSensor)))
 	{
-		bInitialized = false;
-		return bInitialized;
+		deviceIdx++;
+		if (deviceIdx >= count)
+		{
+			bInitialized = false;
+			return bInitialized;
+		}
 	}
 
 	k4a_device_configuration_t config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
@@ -64,6 +64,11 @@ bool AzureKinectCapture::Initialize(int deviceIdx)
 		}
 	} while (!bTemp);
 
+	size_t serialNoSize;
+	k4a_device_get_serialnum(kinectSensor, NULL, &serialNoSize);
+	serialNumber = std::string(serialNoSize, '\0');
+	k4a_device_get_serialnum(kinectSensor, (char*)serialNumber.c_str(), &serialNoSize);
+
 
 	return bInitialized;
 }
diff --git a/src/LiveScanClient/calibration.cpp b/src/LiveScanClient/calibration.cpp
index e255209..a5024fd 100644
--- a/src/LiveScanClient/calibration.cpp
+++ b/src/LiveScanClient/calibration.cpp
@@ -124,15 +124,13 @@ bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColo
 	marker3DSamples.clear();
 	nSampleCounter = 0;
 
-	SaveCalibration();
-
 	return true;
 }
 
-bool Calibration::LoadCalibration()
+bool Calibration::LoadCalibration(const string &serialNumber)
 {
 	ifstream file;
-	file.open("calibration.txt");
+	file.open("calibration_" + serialNumber + ".txt");
 	if (!file.is_open())
 		return false;
 
@@ -149,10 +147,10 @@ bool Calibration::LoadCalibration()
 	return true;
 }
 
-void Calibration::SaveCalibration()
+void Calibration::SaveCalibration(const string &serialNumber)
 {
 	ofstream file;
-	file.open("calibration.txt");
+	file.open("calibration_" + serialNumber + ".txt");
 	for (int i = 0; i < 3; i++)
 		file << worldT[i] << " ";
 	file << endl;
diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp
index 288fc1d..5ea01f7 100644
--- a/src/LiveScanClient/liveScanClient.cpp
+++ b/src/LiveScanClient/liveScanClient.cpp
@@ -78,8 +78,6 @@ LiveScanClient::LiveScanClient() :
 	m_vBounds.push_back(0.5);
 	m_vBounds.push_back(0.5);
 	m_vBounds.push_back(0.5);
-
-	calibration.LoadCalibration();
 }
 
 LiveScanClient::~LiveScanClient()
@@ -164,11 +162,14 @@ int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
     // Main message loop
     while (WM_QUIT != msg.message)
     {
-		//HandleSocket();
 		UpdateFrame();
 
         while (PeekMessageW(&msg, NULL, 0, 0, PM_REMOVE))
         {
+			if (WM_QUIT == msg.message)
+			{
+				break;
+			}
             // If a dialog message will be taken care of by the dialog proc
             if (hWndApp && IsDialogMessageW(hWndApp, &msg))
             {
@@ -223,6 +224,7 @@ void LiveScanClient::UpdateFrame()
 
 		if (res)
 		{
+			calibration.SaveCalibration(pCapture->serialNumber);
 			m_bConfirmCalibrated = true;
 			m_bCalibrate = false;
 		}
@@ -277,6 +279,7 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam,
 			bool res = pCapture->Initialize();
 			if (res)
 			{
+				calibration.LoadCalibration(pCapture->serialNumber);
 				m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
 				m_pDepthInColorSpace = new UINT16[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];