diff --git a/ICP/ICP.vcxproj b/ICP/ICP.vcxproj index 227df44..18f8595 100644 --- a/ICP/ICP.vcxproj +++ b/ICP/ICP.vcxproj @@ -29,46 +29,46 @@ {973EE923-B423-4BCD-AA08-B03DA40CB51F} ICP - 10.0.17763.0 + 10.0 Application true - v141 + v142 MultiByte Application true - v141 + v142 MultiByte Application false - v141 + v142 true MultiByte Application false - v141 + v142 true MultiByte DynamicLibrary false - v141 + v142 true MultiByte DynamicLibrary false - v141 + v142 true MultiByte diff --git a/LiveScanClient/KinectClient.vcxproj b/LiveScanClient/KinectClient.vcxproj index 9900606..eaf094b 100644 --- a/LiveScanClient/KinectClient.vcxproj +++ b/LiveScanClient/KinectClient.vcxproj @@ -60,32 +60,32 @@ {9B550BBA-EAFB-4D12-8B1C-8FDA39361F52} KinectClient LiveScanClient - 10.0.17763.0 + 10.0 Application true - v141 + v142 Unicode Application true - v141 + v142 Unicode Application false - v141 + v142 true Unicode Application false - v141 + v142 true Unicode @@ -193,12 +193,12 @@ - + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. - + \ No newline at end of file diff --git a/LiveScanClient/packages.config b/LiveScanClient/packages.config index 49caf13..bd5be72 100644 --- a/LiveScanClient/packages.config +++ b/LiveScanClient/packages.config @@ -1,4 +1,4 @@  - + \ No newline at end of file diff --git a/LiveScanServer/KinectServer.cs b/LiveScanServer/KinectServer.cs index 4f9045c..1d7d69c 100644 --- a/LiveScanServer/KinectServer.cs +++ b/LiveScanServer/KinectServer.cs @@ -31,6 +31,7 @@ namespace KinectServer Socket oServerSocket; bool bServerRunning = false; + bool waitForSubToStart = false; KinectSettings oSettings; object oClientSocketLock = new object(); @@ -232,19 +233,71 @@ namespace KinectServer } } + /// + /// Send temporal Sync Data to all client + /// public void SendTemporalSyncData() { lock (oClientSocketLock) - { - lClientSockets[0].SendTemporalSyncStatus(true, false); - - for (int i = 1; i < lClientSockets.Count; i++) + { + if(lClientSockets.Count > 1 && oSettings.bEnableSync) { - lClientSockets[i].SendTemporalSyncStatus(false, true); + byte syncOffSetCounter = 1; + + for (int i = 0; i < lClientSockets.Count; i++) + { + lClientSockets[i].SendTemporalSyncStatus(true, syncOffSetCounter); + syncOffSetCounter++; + } + + waitForSubToStart = true; + } + + else + { + for (int i = 0; i < lClientSockets.Count; i++) + { + lClientSockets[i].SendTemporalSyncStatus(false, 0); + } } } } + /// + /// Called when a sub client has started. This checks if all sub clients have already started. If yes, we initialize the master + /// + public void CheckForMasterStart() + { + if (!oSettings.bEnableSync || !waitForSubToStart) + { + return; + } + + bool allSubsStarted = true; + + lock (oClientSocketLock) + { + for (int i = 0; i < lClientSockets.Count; i++) + { + if (!lClientSockets[i].bSubStarted && lClientSockets[i].currentTempSyncState == KinectSocket.eTempSyncConfig.SUBORDINATE) + { + allSubsStarted = false; + break; + } + } + + if (allSubsStarted) + { + for (int i = 0; i < lClientSockets.Count; i++) + { + lClientSockets[i].SendMasterInitialize(); + } + } + } + + + } + public bool GetStoredFrame(List> lFramesRGB, List> lFramesVerts) { bool bNoMoreStoredFrames; @@ -373,6 +426,8 @@ namespace KinectServer lClientSockets.Add(new KinectSocket(newClient)); lClientSockets[lClientSockets.Count - 1].SendSettings(oSettings); lClientSockets[lClientSockets.Count - 1].eChanged += new SocketChangedHandler(SocketListChanged); + lClientSockets[lClientSockets.Count - 1].eSubInitialized += new SubOrdinateInitialized(CheckForMasterStart); + if (eSocketListChanged != null) { eSocketListChanged(lClientSockets); diff --git a/LiveScanServer/KinectSocket.cs b/LiveScanServer/KinectSocket.cs index 065a9a0..7d1f9b1 100644 --- a/LiveScanServer/KinectSocket.cs +++ b/LiveScanServer/KinectSocket.cs @@ -23,6 +23,7 @@ using System.Net.Sockets; namespace KinectServer { public delegate void SocketChangedHandler(); + public delegate void SubOrdinateInitialized(); public class KinectSocket { @@ -33,8 +34,10 @@ namespace KinectServer public bool bStoredFrameReceived = false; public bool bNoMoreStoredFrames = true; public bool bCalibrated = false; - public bool bIsMaster = false; - public bool bIsSubOrdinate = false; + public bool bSubStarted = false; + + public enum eTempSyncConfig { MASTER, SUBORDINATE, STANDALONE } + public eTempSyncConfig currentTempSyncState = eTempSyncConfig.STANDALONE; //The pose of the sensor in the scene (used by the OpenGLWindow to show the sensor) public AffineTransform oCameraPose = new AffineTransform(); //The transform that maps the vertices in the sensor coordinate system to the world corrdinate system. @@ -47,11 +50,14 @@ namespace KinectServer public List lBodies = new List(); public event SocketChangedHandler eChanged; + public event SubOrdinateInitialized eSubInitialized; public KinectSocket(Socket clientSocket) { oSocket = clientSocket; sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = false"; + + UpdateSocketState(); } public void CaptureFrame() @@ -123,23 +129,38 @@ namespace KinectServer SendByte(); } - public void SendTemporalSyncStatus(bool isMaster, bool isSubOrdinate) - { - if (isMaster) + /// + /// Send the device the command to restart with Temporal Sync Enabled or Disabled + /// + /// Enable/Disable Temporal Sync + /// Only set when this device should be a subordinate. Should be a unique number in ascending order for each sub + public void SendTemporalSyncStatus(bool tempSyncOn, byte syncOffSetMultiplier) + { + if (tempSyncOn) { - byteToSend[0] = 7; - SendByte(); - return; + byte[] data = new byte[2]; + data[0] = 7; + data[1] = syncOffSetMultiplier; + if (SocketConnected()) + oSocket.Send(data); } - if (isSubOrdinate) + else { byteToSend[0] = 8; SendByte(); - return; } } + /// + /// Sends the master device the command to Start. Should only be called when all subs have started + /// + public void SendMasterInitialize() + { + byteToSend[0] = 9; + SendByte(); + } + public void ReceiveCalibrationData() { bCalibrated = true; @@ -167,9 +188,10 @@ namespace KinectServer UpdateSocketState(); } + //Gets the TemporalSyncStatus from the device public void ReceiveTemporalSyncStatus() { - int nToRead; + byte tempSyncState; byte[] buffer = new byte[1024]; while (oSocket.Available == 0) @@ -178,24 +200,40 @@ namespace KinectServer return; } - oSocket.Receive(buffer, 4, SocketFlags.None); - nToRead = BitConverter.ToInt32(buffer, 0); + oSocket.Receive(buffer, 3, SocketFlags.None); + tempSyncState = buffer[0]; - if (nToRead == -1) + if (tempSyncState == 0) { - bIsSubOrdinate = true; - bIsMaster = false; - return; + currentTempSyncState = eTempSyncConfig.SUBORDINATE; + bSubStarted = true; + eSubInitialized?.Invoke(); } - if(nToRead == 0) + else if(tempSyncState == 1) { - bIsMaster = true; - bIsSubOrdinate = false; - return; + currentTempSyncState = eTempSyncConfig.MASTER; } + else if (tempSyncState == 2) + { + currentTempSyncState = eTempSyncConfig.STANDALONE; + } + + //MARKED FOR DELETION + //if(currentTempSyncState == eTempSyncConfig.SUBORDINATE && confirmSubStarted == 1) + //{ + + //} + + //else if(currentTempSyncState == eTempSyncConfig.SUBORDINATE && confirmSubStarted == 0) + //{ + // bSubStarted = false; + // eSubInitialized?.Invoke(); + //} + UpdateSocketState(); + } public void ReceiveFrame() @@ -343,22 +381,23 @@ namespace KinectServer public void UpdateSocketState() { - string tempSyncStatus = ""; + string tempSyncMessage = ""; - if (bIsMaster) + switch (currentTempSyncState) { - tempSyncStatus = "[Master]"; + case eTempSyncConfig.MASTER: + tempSyncMessage = "[MASTER]"; + break; + case eTempSyncConfig.SUBORDINATE: + tempSyncMessage = "[SUBORDINATE]"; + break; + default: + break; } - if (bIsSubOrdinate) - { - tempSyncStatus = "[Subordinate]"; - } + sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = " + bCalibrated + " " + tempSyncMessage; - sSocketState = oSocket.RemoteEndPoint.ToString() + " Calibrated = " + bCalibrated + " " + tempSyncStatus; - - if (eChanged != null) - eChanged(); + eChanged?.Invoke(); } } } diff --git a/LiveScanServer/MainWindowForm.cs b/LiveScanServer/MainWindowForm.cs index 93eaff0..7b7fdc2 100644 --- a/LiveScanServer/MainWindowForm.cs +++ b/LiveScanServer/MainWindowForm.cs @@ -75,6 +75,8 @@ namespace KinectServer IFormatter formatter = new System.Runtime.Serialization.Formatters.Binary.BinaryFormatter(); Stream stream = new FileStream("settings.bin", FileMode.Open, FileAccess.Read); oSettings = (KinectSettings)formatter.Deserialize(stream); + //This always needs to be set to false on startup, as we can only activate it when all Kinects are already connected + oSettings.bEnableSync = false; stream.Close(); } catch(Exception) @@ -496,6 +498,7 @@ namespace KinectServer for (int i = 0; i < socketList.Count; i++) listBoxItems.Add(socketList[i].sSocketState); + lClientListBox.DataSource = listBoxItems; } } diff --git a/LiveScanServer/SettingsForm.Designer.cs b/LiveScanServer/SettingsForm.Designer.cs index c8b4a6c..0387c58 100644 --- a/LiveScanServer/SettingsForm.Designer.cs +++ b/LiveScanServer/SettingsForm.Designer.cs @@ -28,13 +28,13 @@ /// private void InitializeComponent() { - System.Windows.Forms.Label lbSyncWarning; this.lbMerge = new System.Windows.Forms.Label(); this.chMerge = new System.Windows.Forms.CheckBox(); this.lbICPIters = new System.Windows.Forms.Label(); this.txtICPIters = new System.Windows.Forms.TextBox(); this.grClient = new System.Windows.Forms.GroupBox(); this.grSync = new System.Windows.Forms.GroupBox(); + this.label1 = new System.Windows.Forms.Label(); this.chSync = new System.Windows.Forms.CheckBox(); this.grBody = new System.Windows.Forms.GroupBox(); this.chSkeletons = new System.Windows.Forms.CheckBox(); @@ -82,7 +82,7 @@ this.rAsciiPly = new System.Windows.Forms.RadioButton(); this.txtRefinIters = new System.Windows.Forms.TextBox(); this.lbOuterIters = new System.Windows.Forms.Label(); - lbSyncWarning = new System.Windows.Forms.Label(); + this.label3 = new System.Windows.Forms.Label(); this.grClient.SuspendLayout(); this.grSync.SuspendLayout(); this.grBody.SuspendLayout(); @@ -145,24 +145,27 @@ // // grSync // - this.grSync.Controls.Add(lbSyncWarning); + this.grSync.Controls.Add(this.label3); + this.grSync.Controls.Add(this.label1); this.grSync.Controls.Add(this.chSync); this.grSync.Location = new System.Drawing.Point(271, 211); this.grSync.Name = "grSync"; - this.grSync.Size = new System.Drawing.Size(355, 66); + this.grSync.Size = new System.Drawing.Size(355, 80); this.grSync.TabIndex = 48; this.grSync.TabStop = false; - this.grSync.Text = "Temporal Sync (Azure Kinect Only)"; + this.grSync.Text = "Temporal Sync"; + this.grSync.Enter += new System.EventHandler(this.grSync_Enter); // - // lbSyncWarning + // label1 // - lbSyncWarning.AutoSize = true; - lbSyncWarning.CausesValidation = false; - lbSyncWarning.Location = new System.Drawing.Point(3, 41); - lbSyncWarning.Name = "lbSyncWarning"; - lbSyncWarning.Size = new System.Drawing.Size(347, 13); - lbSyncWarning.TabIndex = 1; - lbSyncWarning.Text = "Notice: Devices need to be connected via sync cabel before enabeling!\r\n"; + this.label1.Location = new System.Drawing.Point(7, 41); + this.label1.MaximumSize = new System.Drawing.Size(0, 60); + this.label1.MinimumSize = new System.Drawing.Size(0, 30); + this.label1.Name = "label1"; + this.label1.Size = new System.Drawing.Size(0, 30); + this.label1.TabIndex = 1; + this.label1.Text = "Notice: All devices need to be connected to the server and via sync cabel before" + + " enabeling! First Device in Server List will become the master"; // // chSync // @@ -174,7 +177,6 @@ this.chSync.Text = "Enable Temporal Sync"; this.chSync.UseVisualStyleBackColor = true; this.chSync.CheckedChanged += new System.EventHandler(this.chSync_CheckedChanged); - // // grBody // @@ -647,6 +649,16 @@ this.lbOuterIters.Size = new System.Drawing.Size(118, 13); this.lbOuterIters.TabIndex = 26; this.lbOuterIters.Text = "Num of refinement iters:"; + // + // label3 + // + this.label3.Location = new System.Drawing.Point(7, 36); + this.label3.Name = "label3"; + this.label3.Size = new System.Drawing.Size(342, 44); + this.label3.TabIndex = 2; + this.label3.Text = "Before activating, make sure that all Kinects are connected to the server and pro" + + "perly connected via the sync cables. The first device in the server list will be" + + "come the master!"; // // SettingsForm // @@ -732,5 +744,7 @@ private System.Windows.Forms.Label label2; private System.Windows.Forms.GroupBox grSync; private System.Windows.Forms.CheckBox chSync; + private System.Windows.Forms.Label label1; + private System.Windows.Forms.Label label3; } } \ No newline at end of file diff --git a/LiveScanServer/SettingsForm.cs b/LiveScanServer/SettingsForm.cs index ddb9b65..b7a0665 100644 --- a/LiveScanServer/SettingsForm.cs +++ b/LiveScanServer/SettingsForm.cs @@ -355,6 +355,14 @@ namespace KinectServer UpdateClients(); } - + private void grSync_Enter(object sender, EventArgs e) + { + + } + + private void lbSyncWarning_Click(object sender, EventArgs e) + { + + } } } diff --git a/LiveScanServer/SettingsForm.resx b/LiveScanServer/SettingsForm.resx index 7c1ce7f..1af7de1 100644 --- a/LiveScanServer/SettingsForm.resx +++ b/LiveScanServer/SettingsForm.resx @@ -117,7 +117,4 @@ System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - False - \ No newline at end of file diff --git a/include/LiveScanClient/azureKinectCapture.h b/include/LiveScanClient/azureKinectCapture.h index facd9b0..b1ccda3 100644 --- a/include/LiveScanClient/azureKinectCapture.h +++ b/include/LiveScanClient/azureKinectCapture.h @@ -11,12 +11,17 @@ public: AzureKinectCapture(); ~AzureKinectCapture(); - bool Initialize(); + bool Initialize(bool asMaster, bool asSubOrdinate, int syncOffset); + bool Close(); bool AcquireFrame(); void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints); void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints); void MapDepthFrameToColorSpace(UINT16 *pDepthInColorSpace); void MapColorFrameToDepthSpace(RGB *pColorInDepthSpace); + int GetSyncJackState(); + uint64_t GetTimeStamp(); + + private: k4a_device_t kinectSensor = NULL; int32_t captureTimeoutMs = 1000; @@ -27,6 +32,8 @@ private: k4a_image_t colorImageInDepth = NULL; k4a_image_t depthImageInColor = NULL; k4a_transformation_t transformation = NULL; - + bool syncInConnected = false; + bool syncOutConnected = false; + uint64_t currentTimeStamp; void UpdateDepthPointCloud(); }; \ No newline at end of file diff --git a/include/LiveScanClient/frameFileWriterReader.h b/include/LiveScanClient/frameFileWriterReader.h index cb212c5..5f80668 100644 --- a/include/LiveScanClient/frameFileWriterReader.h +++ b/include/LiveScanClient/frameFileWriterReader.h @@ -16,7 +16,7 @@ public: // leave filename blank if you want the filename to be generated from the date void setCurrentFilename(std::string filename = ""); - void writeFrame(std::vector points, std::vector colors); + void writeFrame(std::vector points, std::vector colors, uint64_t timestamp); bool readFrame(std::vector &outPoints, std::vector &outColors); bool openedForWriting() { return m_bFileOpenedForWriting; } diff --git a/include/LiveScanClient/iCapture.h b/include/LiveScanClient/iCapture.h index 6a85fdb..a0bf2e8 100644 --- a/include/LiveScanClient/iCapture.h +++ b/include/LiveScanClient/iCapture.h @@ -40,12 +40,15 @@ public: ICapture(); ~ICapture(); - virtual bool Initialize() = 0; + virtual bool Initialize(bool asMaster, bool asSubOrdinate, int syncOffset) = 0; virtual bool AcquireFrame() = 0; + virtual bool Close() = 0; virtual void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0; virtual void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0; virtual void MapDepthFrameToColorSpace(UINT16 *pColorSpacePoints) = 0; virtual void MapColorFrameToDepthSpace(RGB *pDepthSpacePoints) = 0; + virtual int GetSyncJackState() = 0; + virtual uint64_t GetTimeStamp() = 0; bool bInitialized; @@ -57,5 +60,7 @@ public: RGB *pColorRGBX; std::vector vBodies; + + std::string serialNumber; }; \ No newline at end of file diff --git a/include/LiveScanClient/liveScanClient.h b/include/LiveScanClient/liveScanClient.h index cf4b33d..41f550f 100644 --- a/include/LiveScanClient/liveScanClient.h +++ b/include/LiveScanClient/liveScanClient.h @@ -54,13 +54,16 @@ private: bool m_bCaptureFrame; bool m_bConnected; bool m_bConfirmCaptured; - bool m_bConfirmMaster; - bool m_bConfirmSubOrdinate; + bool m_bConfirmTempSyncState; + bool m_bConfirmSubOrdinateStarted; bool m_bConfirmCalibrated; bool m_bShowDepth; bool m_bFrameCompression; int m_iCompressionLevel; + enum tempSyncConfig { MASTER, SUBORDINATE, STANDALONE }; + tempSyncConfig currentTempSyncState; + FrameFileWriterReader m_framesFileWriterReader; @@ -76,6 +79,7 @@ private: double m_fFreq; INT64 m_nNextStatusTime; DWORD m_nFramesSinceUpdate; + int frameRecordCounter; Point3f* m_pCameraSpaceCoordinates; RGB* m_pColorInDepthSpace; diff --git a/include/LiveScanClient/utils.h b/include/LiveScanClient/utils.h index 8dcb5ca..a6eeed9 100644 --- a/include/LiveScanClient/utils.h +++ b/include/LiveScanClient/utils.h @@ -28,8 +28,9 @@ enum INCOMING_MESSAGE_TYPE MSG_REQUEST_LAST_FRAME, MSG_RECEIVE_CALIBRATION, MSG_CLEAR_STORED_FRAMES, - MSG_SET_MASTER, - MSG_SET_SUBORDINATE + MSG_SET_TEMPSYNC_ON, + MSG_SET_TEMPSYNC_OFF, + MSG_START_MASTER }; enum OUTGOING_MESSAGE_TYPE diff --git a/src/LiveScanClient/azureKinectCapture.cpp b/src/LiveScanClient/azureKinectCapture.cpp index 33d9140..06798fe 100644 --- a/src/LiveScanClient/azureKinectCapture.cpp +++ b/src/LiveScanClient/azureKinectCapture.cpp @@ -17,7 +17,14 @@ AzureKinectCapture::~AzureKinectCapture() k4a_device_close(kinectSensor); } -bool AzureKinectCapture::Initialize() +/// +/// Initialize the device. If you want to initialize it as Standalone, set "asMaster" and "asSubordinate" to false +/// +/// Initializes this device as master +/// Initalizes this devices as subordinate +/// Should only be set when initializing as Subordinate. Each subordinate should have a unique, ascending value +/// +bool AzureKinectCapture::Initialize(bool asMaster, bool asSubordinate, int syncOffsetMultiplier) { uint32_t count = k4a_device_get_installed_count(); int deviceIdx = 0; @@ -40,6 +47,27 @@ bool AzureKinectCapture::Initialize() config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED; config.synchronized_images_only = true; + if (asMaster) + { + config.wired_sync_mode = K4A_WIRED_SYNC_MODE_MASTER; + } + + else if (asSubordinate) + { + config.wired_sync_mode = K4A_WIRED_SYNC_MODE_SUBORDINATE; + //Sets the offset on subordinate devices. Should be a multiple of 160, each subordinate having a different multiplier in ascending order. + //This is very important, as it avoids firing the Kinects Lasers at the same time. + + config.subordinate_delay_off_master_usec = 0; + + config.subordinate_delay_off_master_usec = 160 * syncOffsetMultiplier; + } + + else + { + config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE; + } + // Start the camera with the given configuration bInitialized = K4A_SUCCEEDED(k4a_device_start_cameras(kinectSensor, &config)); @@ -49,31 +77,51 @@ bool AzureKinectCapture::Initialize() bInitialized = false; return bInitialized; } + transformation = k4a_transformation_create(&calibration); - std::chrono::time_point start = std::chrono::system_clock::now(); - bool bTemp; - do + //If this device is a subordinate, it is expected to start capturing at a later time (When the master has started), so we skip this check + if (!asSubordinate) { - bTemp = AcquireFrame(); - - std::chrono::duration elapsedSeconds = std::chrono::system_clock::now() - start; - if (elapsedSeconds.count() > 5.0) + std::chrono::time_point start = std::chrono::system_clock::now(); + bool bTemp; + do { - bInitialized = false; - break; - } - } while (!bTemp); + bTemp = AcquireFrame(); + + std::chrono::duration elapsedSeconds = std::chrono::system_clock::now() - start; + if (elapsedSeconds.count() > 5.0) + { + bInitialized = false; + break; + } + } 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; } +bool AzureKinectCapture::Close() +{ + if (!bInitialized) + { + return false; + } + + k4a_device_stop_cameras(kinectSensor); + k4a_device_close(kinectSensor); + + bInitialized = false; + + + return true; +} + bool AzureKinectCapture::AcquireFrame() { if (!bInitialized) @@ -96,6 +144,9 @@ bool AzureKinectCapture::AcquireFrame() colorImage = k4a_capture_get_color_image(capture); depthImage = k4a_capture_get_depth_image(capture); + + currentTimeStamp = k4a_image_get_device_timestamp_usec(colorImage); + if (colorImage == NULL || depthImage == NULL) { k4a_capture_release(capture); @@ -119,6 +170,7 @@ bool AzureKinectCapture::AcquireFrame() memcpy(pColorRGBX, k4a_image_get_buffer(colorImage), nColorFrameWidth * nColorFrameHeight * sizeof(RGB)); memcpy(pDepth, k4a_image_get_buffer(depthImage), nDepthFrameHeight * nDepthFrameWidth * sizeof(UINT16)); + k4a_capture_release(capture); return true; @@ -260,4 +312,42 @@ void AzureKinectCapture::MapColorFrameToDepthSpace(RGB *pColorInDepthSpace) k4a_transformation_color_image_to_depth_camera(transformation, depthImage, colorImage, colorImageInDepth); memcpy(pColorInDepthSpace, k4a_image_get_buffer(colorImageInDepth), nDepthFrameHeight * nDepthFrameWidth * 4 * (int)sizeof(uint8_t)); -} \ No newline at end of file +} + + +/// +/// Determines if this camera is configured as a Master, Subordinate or Standalone. +/// This is achieved by looking at how the sync out and sync in ports of the device are connected +/// +/// Returns int -1 for Subordinate, int 0 for Master and int 1 for Standalone +int AzureKinectCapture::GetSyncJackState() +{ + if (K4A_RESULT_SUCCEEDED == k4a_device_get_sync_jack(kinectSensor, &syncInConnected, &syncOutConnected)) + { + if (syncInConnected) + { + return -1; //Device is Subordinate, as it recieves a signal via its "Sync In" Port + } + + else if (!syncInConnected && syncOutConnected) + { + return 0; //Device is Master, as it doens't recieve a signal from its "Sync In" Port, but sends one through its "Sync Out" Port + } + + else + { + return 1; //Device is Standalone, as it doesn't have a valid cabel configuration on its Sync Ports + } + } + + else + { + return 1; //Probably failed because there are no cabels connected, this means the device should be set as standalone + } +} + +uint64_t AzureKinectCapture::GetTimeStamp() +{ + return currentTimeStamp; +} + diff --git a/src/LiveScanClient/frameFileWriterReader.cpp b/src/LiveScanClient/frameFileWriterReader.cpp index fe33503..fbb3259 100644 --- a/src/LiveScanClient/frameFileWriterReader.cpp +++ b/src/LiveScanClient/frameFileWriterReader.cpp @@ -86,7 +86,7 @@ bool FrameFileWriterReader::readFrame(std::vector &outPoints, std::vect } -void FrameFileWriterReader::writeFrame(std::vector points, std::vector colors) +void FrameFileWriterReader::writeFrame(std::vector points, std::vector colors, uint64_t timestamp) { if (!m_bFileOpenedForWriting) openNewFileForWriting(); @@ -94,7 +94,8 @@ void FrameFileWriterReader::writeFrame(std::vector points, std::vector< FILE *f = m_pFileHandle; int nPoints = static_cast(points.size()); - fprintf(f, "n_points= %d\nframe_timestamp= %d\n", nPoints, getRecordingTimeMilliseconds()); + //fprintf(f, "n_points= %d\nframe_timestamp= %d\n", nPoints, getRecordingTimeMilliseconds()); + fprintf(f, "n_points= %d\nframe_timestamp= %d\n", nPoints, timestamp); if (nPoints > 0) { fwrite((void*)points.data(), sizeof(points[0]), nPoints, f); diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index f3d69a1..19c2c9c 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -206,7 +206,8 @@ void LiveScanClient::UpdateFrame() if (m_bCaptureFrame) { - m_framesFileWriterReader.writeFrame(m_vLastFrameVertices, m_vLastFrameRGB); + uint64_t timeStamp = pCapture->GetTimeStamp(); + m_framesFileWriterReader.writeFrame(m_vLastFrameVertices, m_vLastFrameRGB, timeStamp); m_bConfirmCaptured = true; m_bCaptureFrame = false; } @@ -275,8 +276,8 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, // Init Direct2D D2D1CreateFactory(D2D1_FACTORY_TYPE_SINGLE_THREADED, &m_pD2DFactory); - // Get and initialize the default Kinect sensor - bool res = pCapture->Initialize(); + // Get and initialize the default Kinect sensor as standalone + bool res = pCapture->Initialize(false, false, 0); if (res) { calibration.LoadCalibration(pCapture->serialNumber); @@ -444,16 +445,62 @@ void LiveScanClient::HandleSocket() //calibrate else if (received[i] == MSG_CALIBRATE) m_bCalibrate = true; - //Set this client as master - else if (received[i] == MSG_SET_MASTER) { - m_bIsMaster = true; - m_bIsSubOrdinate = false; + + //Enables Temporal sync on this client + else if (received[i] == MSG_SET_TEMPSYNC_ON) { + + i++; //Get next byte (the sync Offset) + int syncOffset = received[i]; + + //Determine if this device is a subordinate, master, or standalone + int jackState = pCapture->GetSyncJackState(); + + switch (jackState) + { + case -1: + currentTempSyncState = SUBORDINATE; + //Restart this device as Subordinate, with a unique syncOffset (send by the server) + pCapture->Close(); + pCapture->Initialize(false, true, syncOffset); + //Confirm to the server, that we set this device as subordinate + m_bConfirmTempSyncState = true; + break; + + case 0: + currentTempSyncState = MASTER; + //Only Close this device, as it needs to wait for all subordinates to start, before starting itself + pCapture->Close(); + m_bConfirmTempSyncState = true; + break; + + case 1://Device is Standalone + currentTempSyncState = STANDALONE; + //Restart this device as Standalone + pCapture->Close(); + pCapture->Initialize(false, false, 0); + m_bConfirmTempSyncState = true; + break; + default: + break; + } } - //Set this client as subordinate - else if (received[i] == MSG_SET_SUBORDINATE) { - m_bIsMaster = false; - m_bIsSubOrdinate = true; - } + + //Sets this device as Standalone + else if (received[i] == MSG_SET_TEMPSYNC_OFF) { + currentTempSyncState = STANDALONE; + pCapture->Close(); + pCapture->Initialize(false, false, 0); + m_bConfirmTempSyncState = true; + } + + //Got confirmation from the server that all subs have started, and we can now start the master + else if (received[i] == MSG_START_MASTER) { + if (currentTempSyncState == MASTER) + { + pCapture->Initialize(true, false, 0); + } + } + //receive settings //TODO: what if packet is split? else if (received[i] == MSG_RECEIVE_SETTINGS) @@ -577,24 +624,25 @@ void LiveScanClient::HandleSocket() m_bConfirmCaptured = false; } - if (m_bConfirmMaster) + //Send validation to the server that this device has been set to a specific Sync State + if (m_bConfirmTempSyncState) { - int size = 2; - char *buffer = new char[size]; - buffer[0] = MSG_CONFIRM_TEMP_SYNC_STATUS; - buffer[1] = 0; - m_pClientSocket->SendBytes(buffer, size); - } - - if (m_bConfirmSubOrdinate) - { - int size = 2; + int size = 3; //Somehow it doesn't work when sending only two bytes?? (So we send an extra one) char* buffer = new char[size]; buffer[0] = MSG_CONFIRM_TEMP_SYNC_STATUS; - buffer[1] = -1; + + switch (currentTempSyncState) + { + case SUBORDINATE: buffer[1] = 0; break; + case MASTER: buffer[1] = 1; break; + case STANDALONE: buffer[1] = 2; break; + } + m_pClientSocket->SendBytes(buffer, size); + m_bConfirmTempSyncState = false; } + if (m_bConfirmCalibrated) { int size = (9 + 3) * sizeof(float) + sizeof(int) + 1;