storytold/LiveScan3D
2
0
Fork 0
mirror of https://github.com/storytold/LiveScan3D.git synced 2026-10-12 23:48:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

Finished Temporal Sync Feature

Browse Source
This commit is contained in:
Christopher Remde
2020-07-08 13:55:48 +02:00
parent da2c304db8
commit 1544abd276
17 changed files with 390 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/LiveScanClient/azureKinectCapture.cpp
+104 -14
View File
@@ -17,7 +17,14 @@ AzureKinectCapture::~AzureKinectCapture()
k4a_device_close(kinectSensor);
}
bool AzureKinectCapture::Initialize()
/// <summary>
/// Initialize the device. If you want to initialize it as Standalone, set "asMaster" and "asSubordinate" to false
/// </summary>
/// <param name="asMaster">Initializes this device as master</param>
/// <param name="asSubordinate">Initalizes this devices as subordinate</param>
/// <param name="syncOffsetMultiplier">Should only be set when initializing as Subordinate. Each subordinate should have a unique, ascending value</param>
/// <returns></returns>
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<std::chrono::system_clock> 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<double> elapsedSeconds = std::chrono::system_clock::now() - start;
if (elapsedSeconds.count() > 5.0)
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
bool bTemp;
do
{
bInitialized = false;
break;
}
} while (!bTemp);
bTemp = AcquireFrame();
std::chrono::duration<double> 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));
}
}
/// <summary>
/// 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
/// </summary>
/// <returns>Returns int -1 for Subordinate, int 0 for Master and int 1 for Standalone</returns>
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;
}
src/LiveScanClient/frameFileWriterReader.cpp
+3 -2
View File
@@ -86,7 +86,7 @@ bool FrameFileWriterReader::readFrame(std::vector<Point3s> &outPoints, std::vect
}
void FrameFileWriterReader::writeFrame(std::vector<Point3s> points, std::vector<RGB> colors)
void FrameFileWriterReader::writeFrame(std::vector<Point3s> points, std::vector<RGB> colors, uint64_t timestamp)
{
if (!m_bFileOpenedForWriting)
openNewFileForWriting();
@@ -94,7 +94,8 @@ void FrameFileWriterReader::writeFrame(std::vector<Point3s> points, std::vector<
FILE *f = m_pFileHandle;
int nPoints = static_cast<int>(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);
src/LiveScanClient/liveScanClient.cpp
+72 -24
View File
@@ -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;