From f90a824ff5cb6dfda1f6c4fd0f170124444fe48f Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:22:27 -0400 Subject: [PATCH 01/48] fixed path to turbojpeg shared library --- addon_config.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/addon_config.mk b/addon_config.mk index c9e14f5..f24d0ac 100644 --- a/addon_config.mk +++ b/addon_config.mk @@ -71,8 +71,8 @@ linux64: ADDON_INCLUDES += /usr/include/k4a ADDON_LIBS += /usr/lib/libk4abt.so ADDON_LIBS += /usr/lib/x86_64-linux-gnu/libk4a.so - ADDON_LIBS += /opt/libjpeg-turbo/lib64/libturbojpeg.a - + # ADDON_LIBS += /opt/libjpeg-turbo/lib64/libturbojpeg.a + ADDON_LIBS += /usr/lib/x86_64-linux-gnu/libturbojpeg.so.0 linux: linuxarmv6l: From 6a51651943509dc54fd035fd4450e325c50de54e Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:23:05 -0400 Subject: [PATCH 02/48] added include for k4arecord library --- addon_config.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/addon_config.mk b/addon_config.mk index f24d0ac..73f6073 100644 --- a/addon_config.mk +++ b/addon_config.mk @@ -73,6 +73,8 @@ linux64: ADDON_LIBS += /usr/lib/x86_64-linux-gnu/libk4a.so # ADDON_LIBS += /opt/libjpeg-turbo/lib64/libturbojpeg.a ADDON_LIBS += /usr/lib/x86_64-linux-gnu/libturbojpeg.so.0 + ADDON_LDFLAGS += -lk4arecord + linux: linuxarmv6l: From 5782308f53d0ffab4e2e5f9c798e93706bb9b475 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:28:57 -0400 Subject: [PATCH 03/48] added imu support --- src/ofxAzureKinect/Device.cpp | 184 ++++++++++++++++------------------ src/ofxAzureKinect/Device.h | 61 +++++------ 2 files changed, 120 insertions(+), 125 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index f6e4596..e978b28 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -7,27 +7,14 @@ const int32_t TIMEOUT_IN_MS = 1000; namespace ofxAzureKinect { DeviceSettings::DeviceSettings(int idx) - : deviceIndex(idx) - , deviceSerial("") - , depthMode(K4A_DEPTH_MODE_WFOV_2X2BINNED) - , colorResolution(K4A_COLOR_RESOLUTION_2160P) - , colorFormat(K4A_IMAGE_FORMAT_COLOR_BGRA32) - , cameraFps(K4A_FRAMES_PER_SECOND_30) - , wiredSyncMode(K4A_WIRED_SYNC_MODE_STANDALONE) - , subordinateDelayUsec(0) - , updateColor(true) - , updateIr(true) - , updateWorld(true) - , updateVbo(true) - , syncImages(true) - {} + : deviceIndex(idx), deviceSerial(""), depthMode(K4A_DEPTH_MODE_WFOV_2X2BINNED), colorResolution(K4A_COLOR_RESOLUTION_2160P), colorFormat(K4A_IMAGE_FORMAT_COLOR_BGRA32), cameraFps(K4A_FRAMES_PER_SECOND_30), wiredSyncMode(K4A_WIRED_SYNC_MODE_STANDALONE), subordinateDelayUsec(0), updateColor(true), updateIr(true), updateWorld(true), updateVbo(true), syncImages(true), enableIMU(false) + { + } BodyTrackingSettings::BodyTrackingSettings() - : sensorOrientation(K4ABT_SENSOR_ORIENTATION_DEFAULT) - , processingMode(K4ABT_TRACKER_PROCESSING_MODE_GPU) - , gpuDeviceID(0) - , updateBodies(false) - {} + : sensorOrientation(K4ABT_SENSOR_ORIENTATION_DEFAULT), processingMode(K4ABT_TRACKER_PROCESSING_MODE_GPU), gpuDeviceID(0), updateBodies(false) + { + } int Device::getInstalledCount() { @@ -35,19 +22,9 @@ namespace ofxAzureKinect } Device::Device() - : index(-1) - , pixFrameNum(0) - , texFrameNum(0) - , bOpen(false) - , bStreaming(false) - , bUpdateColor(false) - , bUpdateIr(false) - , bUpdateBodies(false) - , bUpdateWorld(false) - , bUpdateVbo(false) - , bodyTracker(nullptr) - , jpegDecompressor(tjInitDecompress()) - {} + : index(-1), pixFrameNum(0), texFrameNum(0), bOpen(false), bStreaming(false), bUpdateColor(false), bUpdateIr(false), bUpdateBodies(false), bUpdateWorld(false), bUpdateVbo(false), bodyTracker(nullptr), jpegDecompressor(tjInitDecompress()) + { + } Device::~Device() { @@ -74,6 +51,7 @@ namespace ofxAzureKinect this->config.color_resolution = deviceSettings.colorResolution; this->config.camera_fps = deviceSettings.cameraFps; this->config.synchronized_images_only = deviceSettings.syncImages; + this->enableIMU = deviceSettings.enableIMU; this->config.wired_sync_mode = deviceSettings.wiredSyncMode; this->config.subordinate_delay_off_master_usec = deviceSettings.subordinateDelayUsec; @@ -98,7 +76,7 @@ namespace ofxAzureKinect // Get the device serial number. this->serialNumber = this->device.get_serialnum(); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); @@ -132,7 +110,7 @@ namespace ofxAzureKinect this->device.close(); } } - catch (const k4a::error& e) + catch (const k4a::error &e) { // Don't worry about it; we just might be trying to access an already open device. continue; @@ -157,8 +135,7 @@ namespace ofxAzureKinect this->bUpdateBodies = bodyTrackingSettings.updateBodies; if (this->bUpdateBodies) { - this->eventListeners.push(this->jointSmoothing.newListener([this](float &) - { + this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); })); } @@ -170,7 +147,14 @@ namespace ofxAzureKinect bool Device::close() { - if (!this->bOpen) return false; + if (!this->bOpen) + return false; + + // Start IMU is cameras are enabled + if (this->enableIMU) + { + k4a_device_stop_imu(device.handle()); + } this->stopCameras(); @@ -198,7 +182,7 @@ namespace ofxAzureKinect { this->calibration = this->device.get_calibration(this->config.depth_mode, this->config.color_resolution); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return false; @@ -250,12 +234,18 @@ namespace ofxAzureKinect { this->device.start_cameras(&this->config); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return false; } + // Can only start the IMU if cameras are enabled + if (this->enableIMU) + { + k4a_device_start_imu(device.handle()); + } + this->startThread(); ofAddListener(ofEvents().update, this, &Device::update); @@ -266,7 +256,8 @@ namespace ofxAzureKinect bool Device::stopCameras() { - if (!this->bStreaming) return false; + if (!this->bStreaming) + return false; std::unique_lock lock(this->mutex); this->stopThread(); @@ -316,7 +307,7 @@ namespace ofxAzureKinect } } - void Device::update(ofEventArgs& args) + void Device::update(ofEventArgs &args) { this->bNewFrame = false; @@ -341,7 +332,7 @@ namespace ofxAzureKinect return; } } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return; @@ -357,7 +348,7 @@ namespace ofxAzureKinect this->depthPix.allocate(depthDims.x, depthDims.y, 1); } - const auto depthData = reinterpret_cast(depthImg.get_buffer()); + const auto depthData = reinterpret_cast(depthImg.get_buffer()); this->depthPix.setFromPixels(depthData, depthDims.x, depthDims.y, 1); ofLogVerbose(__FUNCTION__) << "Capture Depth16 " << depthDims.x << "x" << depthDims.y << " stride: " << depthImg.get_stride_bytes() << "."; @@ -383,18 +374,18 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { const int decompressStatus = tjDecompress2(this->jpegDecompressor, - colorImg.get_buffer(), - static_cast(colorImg.get_size()), - this->colorPix.getData(), - colorDims.x, - 0, // pitch - colorDims.y, - TJPF_BGRA, - TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); + colorImg.get_buffer(), + static_cast(colorImg.get_size()), + this->colorPix.getData(), + colorDims.x, + 0, // pitch + colorDims.y, + TJPF_BGRA, + TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); } else { - const auto colorData = reinterpret_cast(colorImg.get_buffer()); + const auto colorData = reinterpret_cast(colorImg.get_buffer()); this->colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); } @@ -419,7 +410,7 @@ namespace ofxAzureKinect this->irPix.allocate(irSize.x, irSize.y, 1); } - const auto irData = reinterpret_cast(irImg.get_buffer()); + const auto irData = reinterpret_cast(irImg.get_buffer()); this->irPix.setFromPixels(irData, irSize.x, irSize.y, 1); ofLogVerbose(__FUNCTION__) << "Capture Ir16 " << irSize.x << "x" << irSize.y << " stride: " << irImg.get_stride_bytes() << "."; @@ -451,7 +442,7 @@ namespace ofxAzureKinect this->bodyIndexPix.allocate(bodyIndexSize.x, bodyIndexSize.y, 1); } - const auto bodyIndexData = reinterpret_cast(bodyIndexImg.get_buffer()); + const auto bodyIndexData = reinterpret_cast(bodyIndexImg.get_buffer()); this->bodyIndexPix.setFromPixels(bodyIndexData, bodyIndexSize.x, bodyIndexSize.y, 1); ofLogVerbose(__FUNCTION__) << "Capture BodyIndex " << bodyIndexSize.x << "x" << bodyIndexSize.y << " stride: " << bodyIndexImg.get_stride_bytes() << "."; @@ -656,9 +647,9 @@ namespace ofxAzureKinect return false; } - bool Device::setupImageToWorldTable(k4a_calibration_type_t type, k4a::image& img) + bool Device::setupImageToWorldTable(k4a_calibration_type_t type, k4a::image &img) { - const k4a_calibration_camera_t& calibrationCamera = (type == K4A_CALIBRATION_TYPE_DEPTH) ? this->calibration.depth_camera_calibration : this->calibration.color_camera_calibration; + const k4a_calibration_camera_t &calibrationCamera = (type == K4A_CALIBRATION_TYPE_DEPTH) ? this->calibration.depth_camera_calibration : this->calibration.color_camera_calibration; const auto dims = glm::ivec2( calibrationCamera.resolution_width, @@ -667,16 +658,16 @@ namespace ofxAzureKinect try { img = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM, - dims.x, dims.y, - dims.x * static_cast(sizeof(k4a_float2_t))); + dims.x, dims.y, + dims.x * static_cast(sizeof(k4a_float2_t))); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return false; } - auto imgData = reinterpret_cast(img.get_buffer()); + auto imgData = reinterpret_cast(img.get_buffer()); k4a_float2_t p; k4a_float3_t ray; @@ -709,7 +700,7 @@ namespace ofxAzureKinect return true; } - bool Device::updatePointsCache(k4a::image& frameImg, k4a::image& tableImg) + bool Device::updatePointsCache(k4a::image &frameImg, k4a::image &tableImg) { const auto frameDims = glm::ivec2(frameImg.get_width_pixels(), frameImg.get_height_pixels()); const auto tableDims = glm::ivec2(tableImg.get_width_pixels(), tableImg.get_height_pixels()); @@ -719,8 +710,8 @@ namespace ofxAzureKinect return false; } - const auto frameData = reinterpret_cast(frameImg.get_buffer()); - const auto tableData = reinterpret_cast(tableImg.get_buffer()); + const auto frameData = reinterpret_cast(frameImg.get_buffer()); + const auto tableData = reinterpret_cast(tableImg.get_buffer()); this->positionCache.resize(frameDims.x * frameDims.y); this->uvCache.resize(frameDims.x * frameDims.y); @@ -738,8 +729,7 @@ namespace ofxAzureKinect this->positionCache[count] = glm::vec3( tableData[idx].xy.x * depthVal, tableData[idx].xy.y * depthVal, - depthVal - ); + depthVal); this->uvCache[count] = glm::vec2(x, y); @@ -753,7 +743,7 @@ namespace ofxAzureKinect return true; } - bool Device::updateDepthInColorFrame(const k4a::image& depthImg, const k4a::image& colorImg) + bool Device::updateDepthInColorFrame(const k4a::image &depthImg, const k4a::image &colorImg) { const auto colorDims = glm::ivec2(colorImg.get_width_pixels(), colorImg.get_height_pixels()); @@ -761,18 +751,18 @@ namespace ofxAzureKinect try { transformedDepthImg = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16, - colorDims.x, colorDims.y, - colorDims.x * static_cast(sizeof(uint16_t))); + colorDims.x, colorDims.y, + colorDims.x * static_cast(sizeof(uint16_t))); this->transformation.depth_image_to_color_camera(depthImg, &transformedDepthImg); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return false; } - const auto transformedColorData = reinterpret_cast(transformedDepthImg.get_buffer()); + const auto transformedColorData = reinterpret_cast(transformedDepthImg.get_buffer()); if (!this->depthInColorPix.isAllocated()) { @@ -788,7 +778,7 @@ namespace ofxAzureKinect return true; } - bool Device::updateColorInDepthFrame(const k4a::image& depthImg, const k4a::image& colorImg) + bool Device::updateColorInDepthFrame(const k4a::image &depthImg, const k4a::image &colorImg) { const auto depthDims = glm::ivec2(depthImg.get_width_pixels(), depthImg.get_height_pixels()); @@ -796,18 +786,18 @@ namespace ofxAzureKinect try { transformedColorImg = k4a::image::create(K4A_IMAGE_FORMAT_COLOR_BGRA32, - depthDims.x, depthDims.y, - depthDims.x * 4 * static_cast(sizeof(uint8_t))); + depthDims.x, depthDims.y, + depthDims.x * 4 * static_cast(sizeof(uint8_t))); this->transformation.color_image_to_depth_camera(depthImg, colorImg, &transformedColorImg); } - catch (const k4a::error& e) + catch (const k4a::error &e) { ofLogError(__FUNCTION__) << e.what(); return false; } - const auto transformedColorData = reinterpret_cast(transformedColorImg.get_buffer()); + const auto transformedColorData = reinterpret_cast(transformedColorImg.get_buffer()); if (!this->colorInDepthPix.isAllocated()) { @@ -815,7 +805,7 @@ namespace ofxAzureKinect } this->colorInDepthPix.setFromPixels(transformedColorData, depthDims.x, depthDims.y, 4); - + ofLogVerbose(__FUNCTION__) << "Color in Depth " << depthDims.x << "x" << depthDims.y << " stride: " << transformedColorImg.get_stride_bytes() << "."; transformedColorImg.reset(); @@ -838,87 +828,87 @@ namespace ofxAzureKinect return this->bNewFrame; } - const std::string& Device::getSerialNumber() const + const std::string &Device::getSerialNumber() const { return this->serialNumber; } - const ofShortPixels& Device::getDepthPix() const + const ofShortPixels &Device::getDepthPix() const { return this->depthPix; } - const ofTexture& Device::getDepthTex() const + const ofTexture &Device::getDepthTex() const { return this->depthTex; } - const ofPixels& Device::getColorPix() const + const ofPixels &Device::getColorPix() const { return this->colorPix; } - const ofTexture& Device::getColorTex() const + const ofTexture &Device::getColorTex() const { return this->colorTex; } - const ofShortPixels& Device::getIrPix() const + const ofShortPixels &Device::getIrPix() const { return this->irPix; } - const ofTexture& Device::getIrTex() const + const ofTexture &Device::getIrTex() const { return this->irTex; } - const ofFloatPixels& Device::getDepthToWorldPix() const + const ofFloatPixels &Device::getDepthToWorldPix() const { return this->depthToWorldPix; } - const ofTexture& Device::getDepthToWorldTex() const + const ofTexture &Device::getDepthToWorldTex() const { return this->depthToWorldTex; } - const ofFloatPixels& Device::getColorToWorldPix() const + const ofFloatPixels &Device::getColorToWorldPix() const { return this->colorToWorldPix; } - const ofTexture& Device::getColorToWorldTex() const + const ofTexture &Device::getColorToWorldTex() const { return this->colorToWorldTex; } - const ofShortPixels& Device::getDepthInColorPix() const + const ofShortPixels &Device::getDepthInColorPix() const { return this->depthInColorPix; } - const ofTexture& Device::getDepthInColorTex() const + const ofTexture &Device::getDepthInColorTex() const { return this->depthInColorTex; } - const ofPixels& Device::getColorInDepthPix() const + const ofPixels &Device::getColorInDepthPix() const { return this->colorInDepthPix; } - const ofTexture& Device::getColorInDepthTex() const + const ofTexture &Device::getColorInDepthTex() const { return this->colorInDepthTex; } - const ofPixels& Device::getBodyIndexPix() const + const ofPixels &Device::getBodyIndexPix() const { return this->bodyIndexPix; } - const ofTexture& Device::getBodyIndexTex() const + const ofTexture &Device::getBodyIndexTex() const { return this->bodyIndexTex; } @@ -928,18 +918,18 @@ namespace ofxAzureKinect return this->bodySkeletons.size(); } - const std::vector& Device::getBodySkeletons() const + const std::vector &Device::getBodySkeletons() const { return this->bodySkeletons; } - const std::vector& Device::getBodyIDs() const + const std::vector &Device::getBodyIDs() const { return this->bodyIDs; } - const ofVbo& Device::getPointCloudVbo() const + const ofVbo &Device::getPointCloudVbo() const { return this->pointCloudVbo; } -} +} // namespace ofxAzureKinect diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 7dc0372..a0f61b9 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -38,6 +38,8 @@ namespace ofxAzureKinect bool syncImages; + bool enableIMU; + DeviceSettings(int idx = 0); }; @@ -52,7 +54,7 @@ namespace ofxAzureKinect BodyTrackingSettings(); }; - class Device + class Device : ofThread { public: @@ -77,40 +79,40 @@ namespace ofxAzureKinect bool isStreaming() const; bool isFrameNew() const; - const std::string& getSerialNumber() const; + const std::string &getSerialNumber() const; - const ofShortPixels& getDepthPix() const; - const ofTexture& getDepthTex() const; + const ofShortPixels &getDepthPix() const; + const ofTexture &getDepthTex() const; - const ofPixels& getColorPix() const; - const ofTexture& getColorTex() const; + const ofPixels &getColorPix() const; + const ofTexture &getColorTex() const; - const ofShortPixels& getIrPix() const; - const ofTexture& getIrTex() const; + const ofShortPixels &getIrPix() const; + const ofTexture &getIrTex() const; - const ofFloatPixels& getDepthToWorldPix() const; - const ofTexture& getDepthToWorldTex() const; + const ofFloatPixels &getDepthToWorldPix() const; + const ofTexture &getDepthToWorldTex() const; - const ofFloatPixels& getColorToWorldPix() const; - const ofTexture& getColorToWorldTex() const; + const ofFloatPixels &getColorToWorldPix() const; + const ofTexture &getColorToWorldTex() const; - const ofShortPixels& getDepthInColorPix() const; - const ofTexture& getDepthInColorTex() const; + const ofShortPixels &getDepthInColorPix() const; + const ofTexture &getDepthInColorTex() const; - const ofPixels& getColorInDepthPix() const; - const ofTexture& getColorInDepthTex() const; + const ofPixels &getColorInDepthPix() const; + const ofTexture &getColorInDepthTex() const; - const ofPixels& getBodyIndexPix() const; - const ofTexture& getBodyIndexTex() const; + const ofPixels &getBodyIndexPix() const; + const ofTexture &getBodyIndexTex() const; size_t getNumBodies() const; - const std::vector& getBodySkeletons() const; - const std::vector& getBodyIDs() const; + const std::vector &getBodySkeletons() const; + const std::vector &getBodyIDs() const; - const ofVbo& getPointCloudVbo() const; + const ofVbo &getPointCloudVbo() const; public: - ofParameter jointSmoothing{ "Joint Smoothing", 0.0f, 0.0f, 1.0f }; + ofParameter jointSmoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; protected: void threadedFunction() override; @@ -119,16 +121,16 @@ namespace ofxAzureKinect void updatePixels(); void updateTextures(); - void update(ofEventArgs& args); + void update(ofEventArgs &args); bool setupDepthToWorldTable(); bool setupColorToWorldTable(); - bool setupImageToWorldTable(k4a_calibration_type_t type, k4a::image& img); + bool setupImageToWorldTable(k4a_calibration_type_t type, k4a::image &img); - bool updatePointsCache(k4a::image& frameImg, k4a::image& tableImg); + bool updatePointsCache(k4a::image &frameImg, k4a::image &tableImg); - bool updateDepthInColorFrame(const k4a::image& depthImg, const k4a::image& colorImg); - bool updateColorInDepthFrame(const k4a::image& depthImg, const k4a::image& colorImg); + bool updateDepthInColorFrame(const k4a::image &depthImg, const k4a::image &colorImg); + bool updateColorInDepthFrame(const k4a::image &depthImg, const k4a::image &colorImg); private: int index; @@ -160,6 +162,9 @@ namespace ofxAzureKinect tjhandle jpegDecompressor; + bool enableIMU = false; + void startIMU(); + ofShortPixels depthPix; ofTexture depthTex; @@ -195,4 +200,4 @@ namespace ofxAzureKinect ofEventListeners eventListeners; }; -} \ No newline at end of file +} // namespace ofxAzureKinect \ No newline at end of file From 7e59381ca774a604e86edf62e12a2ac1a5ab40f7 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:34:34 -0400 Subject: [PATCH 04/48] added Record class --- src/ofxAzureKinect/Device.cpp | 34 +++++++ src/ofxAzureKinect/Device.h | 6 ++ src/ofxAzureKinect/Record.cpp | 161 ++++++++++++++++++++++++++++++++++ src/ofxAzureKinect/Record.h | 45 ++++++++++ 4 files changed, 246 insertions(+) create mode 100644 src/ofxAzureKinect/Record.cpp create mode 100644 src/ofxAzureKinect/Record.h diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index e978b28..b4fe218 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -140,6 +140,11 @@ namespace ofxAzureKinect })); } + // Add Recording Listener + this->eventListeners.push(this->bRecord.newListener([this](bool) { + handle_recording(this->bRecord); + })); + ofLogNotice(__FUNCTION__) << "Successfully opened device " << this->index << " with serial number " << this->serialNumber << "."; return true; @@ -487,6 +492,13 @@ namespace ofxAzureKinect this->updateColorInDepthFrame(depthImg, colorImg); } + // Do any recording before releaseing the capture + if (bRecord) + { + k4a_capture_t capture_handle = capture.handle(); + recording->record(&capture_handle); + } + // Release images. depthImg.reset(); colorImg.reset(); @@ -932,4 +944,26 @@ namespace ofxAzureKinect { return this->pointCloudVbo; } + + void Device::handle_recording(bool val) + { + if (val) + { + recording = new Record(); + recording->setup(device.handle(), this->config, enableIMU); + recording->start(); + } + else + { + recording->stop(); + } + } + + float Device::get_recording_timer_delay() + { + if (recording != nullptr) + return recording->get_timer_delay(); + else + return -1; + } } // namespace ofxAzureKinect diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index a0f61b9..6c3a366 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -15,6 +15,7 @@ #include "ofVectorMath.h" #include "Types.h" +#include "Record.h" namespace ofxAzureKinect { @@ -113,6 +114,8 @@ namespace ofxAzureKinect public: ofParameter jointSmoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; + ofParameter bRecord{"bRecord", false}; + float get_recording_timer_delay(); protected: void threadedFunction() override; @@ -199,5 +202,8 @@ namespace ofxAzureKinect ofVbo pointCloudVbo; ofEventListeners eventListeners; + + Record *recording; + void handle_recording(bool val); }; } // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Record.cpp b/src/ofxAzureKinect/Record.cpp new file mode 100644 index 0000000..61b1cb7 --- /dev/null +++ b/src/ofxAzureKinect/Record.cpp @@ -0,0 +1,161 @@ +#include "Record.h" + +namespace ofxAzureKinect +{ + + void Record::setup(k4a_device_t device, k4a_device_configuration_t config, bool recording_imu_enabled, float delay, string filename) + { + this->device = device; + this->recording_imu_enabled = recording_imu_enabled; + this->delay = delay; + + // Set a timestamped or user defined filename + if (filename == "") + filename = ofToDataPath("output_" + ofGetTimestampString("%Y%m%d_%H-%M-%S") + ".mkv"); + else + { + filename = ofToDataPath(filename); + } + + // Convert filename from string to char* + char *temp = new char[filename.size() + 1]; + copy(filename.begin(), filename.end(), temp); + temp[filename.size()] = '\0'; + + // Set the filename for recording output + this->filename = temp; + + // Set up the default recording tracks + if (K4A_FAILED(k4a_record_create(this->filename, device, config, &recording))) + { + printf("Unable to create recording file: %s\n", this->filename); + k4a_failed = true; + return; + } + + // Add any custom tracks and tags you want to record + // See example code: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/develop/examples/k4arecord_custom_track/main.c + // ... + + // Add IMU track + if (this->recording_imu_enabled) + { + k4a_record_add_imu_track(recording); + } + + // Write the recording header after all the track metadata is set up. + k4a_record_write_header(recording); + + cout << "Recording Setup to file: " << this->filename << endl; + } + + void Record::start() + { + delay_start = ofGetElapsedTimef(); + if (delay != 0) + { + cout << "Recording Will Begin In " << delay << " seconds!" << endl; + } + } + + void Record::stop() + { + if (k4a_failed) + { + ofLogError(__FUNCTION__) << "Recording Failed ... see error above."; + } + else + { + cout << "\nSaving Recording to: " << filename << endl; + k4a_record_flush(recording); + k4a_record_close(recording); + cout << "Done." << endl; + } + } + + void Record::record(k4a_capture_t *capture) + { + if (k4a_failed) + { + ofLogError(__FUNCTION__) << "Recording Failed ... see error above."; + } + else + { + // Wait for any recording delay + if (ofGetElapsedTimef() - delay_start > delay) + { + + // Write the capture to any built-in tracks + k4a_record_write_capture(recording, *capture); + + // Write the capture for any other custom tracks (not the IMU; do that after releasing the capture) + // ... + + if (recording_imu_enabled) + { + // Record IMU IMU data + record_imu(); //device, recording); + } + + // Indicate that we are recording + cout << "."; + cout.flush(); + } + else + { + // Print the countdown + // cout << (delay - (ofGetElapsedTimef() - delay_start)) << endl; + } + } + } + + void Record::record_imu() + { + // Code from k4arecorder (Line 213) + // https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/master/tools/k4arecorder/recorder.cpp + // and https://docs.microsoft.com/en-us/azure/kinect-dk/retrieve-imu-samples + + k4a_wait_result_t result; + + // Loop to get the queued IMU samples after every capture. + // We kick out of the loop when result returns K4A_WAIT_RESULT_TIMEOUT + do + { + k4a_imu_sample_t sample; + result = k4a_device_get_imu_sample(device, &sample, 0); + if (result == K4A_WAIT_RESULT_SUCCEEDED) + { + // Write the IMU data to file + k4a_result_t write_result = k4a_record_write_imu_sample(recording, sample); + if (K4A_FAILED(write_result)) + { + ofLogError(__FUNCTION__) << "Runtime error: k4a_record_write_imu_sample() returned " << write_result; + break; + } + } + else if (result == K4A_WAIT_RESULT_TIMEOUT) + { + // Indicates that there are no queued samples and none have arrived in the timeout specified. + break; + } + else + { + ofLogError(__FUNCTION__) << "Runtime error: k4a_device_get_imu_sample() returned " << result; + break; + } + + // printf(" | Accelerometer temperature:%.2f x:%.4f y:%.4f z: %.4f\n", + // sample.temperature, + // sample.acc_sample.xyz.x, + // sample.acc_sample.xyz.y, + // sample.acc_sample.xyz.z); + + } while (result != K4A_WAIT_RESULT_FAILED); + } + + float Record::get_timer_delay() + { + return MAX(delay - (ofGetElapsedTimef() - delay_start), 0); + } + +} // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Record.h b/src/ofxAzureKinect/Record.h new file mode 100644 index 0000000..dc83741 --- /dev/null +++ b/src/ofxAzureKinect/Record.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include + +#include "ofMain.h" + +// BUG: Playback is too fast ... maybe frames are dropping and not being recorded? +// Duration seems to be correct, but actual playback is 2X speed (10 sec recording playsback in 5sec) +// syncImages doesn't seem to have an effect + +namespace ofxAzureKinect +{ + class Record + { + + public: + Record(){}; + ~Record(); + + void setup(k4a_device_t device, k4a_device_configuration_t config, bool recording_imu_enabled = true, float delay = 3, string filename = ""); + void start(); + void stop(); + void record(k4a_capture_t *capture); + + // Set a recording delay (in seconds) + void set_delay(float _delay) { this->delay = _delay; } + float get_timer_delay(); + + + private: + k4a_device_t device; + k4a_record_t recording; + + char *filename; + bool recording_imu_enabled = true; + + void record_imu(); + + bool k4a_failed = false; + + float delay = 0; + float delay_start = 0; + }; +} // namespace ofxAzureKinect \ No newline at end of file From 8633ea393377385fbeeb4b6555f2a8c0c304c76a Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:35:19 -0400 Subject: [PATCH 05/48] added record example --- example-record/addons.make | 1 + example-record/src/main.cpp | 13 +++ example-record/src/ofApp.cpp | 165 +++++++++++++++++++++++++++++++++++ example-record/src/ofApp.h | 34 ++++++++ 4 files changed, 213 insertions(+) create mode 100644 example-record/addons.make create mode 100644 example-record/src/main.cpp create mode 100644 example-record/src/ofApp.cpp create mode 100644 example-record/src/ofApp.h diff --git a/example-record/addons.make b/example-record/addons.make new file mode 100644 index 0000000..b155bef --- /dev/null +++ b/example-record/addons.make @@ -0,0 +1 @@ +ofxAzureKinect diff --git a/example-record/src/main.cpp b/example-record/src/main.cpp new file mode 100644 index 0000000..e57370b --- /dev/null +++ b/example-record/src/main.cpp @@ -0,0 +1,13 @@ +#include "ofMain.h" +#include "ofApp.h" + +//======================================================================== +int main( ){ + ofSetupOpenGL(1024,768,OF_WINDOW); // <-------- setup the GL context + + // this kicks off the running of my app + // can be OF_WINDOW or OF_FULLSCREEN + // pass in width and height too: + ofRunApp(new ofApp()); + +} diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp new file mode 100644 index 0000000..6405950 --- /dev/null +++ b/example-record/src/ofApp.cpp @@ -0,0 +1,165 @@ +#include "ofApp.h" + +//-------------------------------------------------------------- +void ofApp::setup() +{ + ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices."; + + auto settings = ofxAzureKinect::DeviceSettings(); + settings.colorResolution = K4A_COLOR_RESOLUTION_1080P; + settings.depthMode = K4A_DEPTH_MODE_NFOV_2X2BINNED; + settings.colorFormat = K4A_IMAGE_FORMAT_COLOR_MJPG; // BRGA32 not supported for recording + settings.cameraFps = K4A_FRAMES_PER_SECOND_30; + settings.syncImages = true; + settings.updateWorld = false; + settings.enableIMU = true; + if (this->sensor.open(settings)) + { + this->sensor.startCameras(); + } +} + +//-------------------------------------------------------------- +void ofApp::exit() +{ + this->sensor.close(); +} + +//-------------------------------------------------------------- +void ofApp::update() +{ + + // If we are recording, update the total recording time + if (sensor.bRecord && ofGetElapsedTimef() > recording_start) + recording_duration = ofGetElapsedTimef() - recording_start; + +} + +//-------------------------------------------------------------- +void ofApp::draw() +{ + ofBackground(128); + + // visualize kinect streams + if (this->sensor.isStreaming()) + { + this->sensor.getColorTex().draw(0, 0, 1280, 720); + this->sensor.getDepthTex().draw(1280, 0, 360, 360); + this->sensor.getIrTex().draw(1280, 360, 360, 360); + } + + if (sensor.bRecord) + { + draw_recording_animation(); + } + + ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20); +} + +//-------------------------------------------------------------- +void ofApp::draw_recording_animation() +{ + + float a = ofMap(sin(ofGetElapsedTimef() * 3.5), -1, 1, 0, 120); + ofColor col; + string msg; + + float recording_countdown = sensor.get_recording_timer_delay(); + if (recording_countdown > 0) + { + msg = "Starting Recording In: " + ofToString(recording_countdown, 2); + col = ofColor(120, a); + } + if (recording_duration > 0) + { + msg = "Recording Time: " + ofToString(recording_duration, 2); + col = ofColor(255, 0, 0, a); + } + + ofPushStyle(); + ofSetColor(col); + ofDrawRectangle(0, 0, ofGetWidth(), ofGetHeight()); + ofPopStyle(); + + ofDrawBitmapStringHighlight(msg, 10, 40); +} + +//-------------------------------------------------------------- +void ofApp::keyPressed(int key) +{ + switch (key) + { + case ' ': + { + sensor.bRecord = !sensor.bRecord; + if (sensor.bRecord) + { + recording_delay = sensor.get_recording_timer_delay(); + recording_start = ofGetElapsedTimef() + recording_delay; + } + else + { + recording_delay = 0; + recording_start = 0; + recording_duration = 0; + } + break; + } + case 'f': + case 'F': + ofToggleFullscreen(); + break; + default: + break; + } +} + +//-------------------------------------------------------------- +void ofApp::keyReleased(int key) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseMoved(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseDragged(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mousePressed(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseReleased(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseEntered(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseExited(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::windowResized(int w, int h) +{ +} + +//-------------------------------------------------------------- +void ofApp::gotMessage(ofMessage msg) +{ +} + +//-------------------------------------------------------------- +void ofApp::dragEvent(ofDragInfo dragInfo) +{ +} diff --git a/example-record/src/ofApp.h b/example-record/src/ofApp.h new file mode 100644 index 0000000..76f74f9 --- /dev/null +++ b/example-record/src/ofApp.h @@ -0,0 +1,34 @@ +#pragma once + +#include "ofMain.h" +#include "ofxAzureKinect.h" + +class ofApp : public ofBaseApp{ + + public: + void setup(); + void exit(); + void update(); + void draw(); + + void keyPressed(int key); + void keyReleased(int key); + void mouseMoved(int x, int y ); + void mouseDragged(int x, int y, int button); + void mousePressed(int x, int y, int button); + void mouseReleased(int x, int y, int button); + void mouseEntered(int x, int y); + void mouseExited(int x, int y); + void windowResized(int w, int h); + void dragEvent(ofDragInfo dragInfo); + void gotMessage(ofMessage msg); + + ofxAzureKinect::Device sensor; + + void draw_recording_animation(); + + float recording_delay=0; + float recording_start=0; + float recording_duration=0; + +}; From 1dfbd5feeb81c6231c0770f20e7fdb5161675dff Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Tue, 23 Jun 2020 22:10:00 -0400 Subject: [PATCH 06/48] cleaned up comments --- src/ofxAzureKinect/Record.cpp | 2 +- src/ofxAzureKinect/Record.h | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/ofxAzureKinect/Record.cpp b/src/ofxAzureKinect/Record.cpp index 61b1cb7..cc270d2 100644 --- a/src/ofxAzureKinect/Record.cpp +++ b/src/ofxAzureKinect/Record.cpp @@ -94,7 +94,7 @@ namespace ofxAzureKinect if (recording_imu_enabled) { // Record IMU IMU data - record_imu(); //device, recording); + record_imu(); } // Indicate that we are recording diff --git a/src/ofxAzureKinect/Record.h b/src/ofxAzureKinect/Record.h index dc83741..16e708f 100644 --- a/src/ofxAzureKinect/Record.h +++ b/src/ofxAzureKinect/Record.h @@ -5,10 +5,6 @@ #include "ofMain.h" -// BUG: Playback is too fast ... maybe frames are dropping and not being recorded? -// Duration seems to be correct, but actual playback is 2X speed (10 sec recording playsback in 5sec) -// syncImages doesn't seem to have an effect - namespace ofxAzureKinect { class Record From e0d6657d84b218b1ca30b451aeb3977e1c14ab1f Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Tue, 23 Jun 2020 22:11:02 -0400 Subject: [PATCH 07/48] added playback class --- src/ofxAzureKinect/Device.cpp | 187 +++++++++++++++++++++++++++----- src/ofxAzureKinect/Device.h | 17 ++- src/ofxAzureKinect/Playback.cpp | 153 ++++++++++++++++++++++++++ src/ofxAzureKinect/Playback.h | 68 ++++++++++++ 4 files changed, 395 insertions(+), 30 deletions(-) create mode 100644 src/ofxAzureKinect/Playback.cpp create mode 100644 src/ofxAzureKinect/Playback.h diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index b4fe218..928fac3 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -33,6 +33,50 @@ namespace ofxAzureKinect tjDestroy(jpegDecompressor); } + bool Device::open(string filename) + { + bPlayback = true; + playback = new Playback(); + + if (playback->load_file(filename)) + { + k4a_record_configuration_t playback_config = playback->get_device_settings(); + + this->config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL; + this->config.depth_mode = playback_config.depth_mode; + this->config.color_format = playback_config.color_format; + this->config.color_resolution = playback_config.color_resolution; + this->config.camera_fps = playback_config.camera_fps; + this->enableIMU = playback_config.imu_track_enabled; + + this->serialNumber = playback->get_serial_number(); + this->bUpdateColor = playback_config.color_track_enabled; + this->bUpdateIr = playback_config.ir_track_enabled; + this->bUpdateWorld = playback_config.depth_track_enabled; + this->bUpdateVbo = false; //bUpdateWorld; + + // Add Playback Listeners + this->eventListeners.push(this->play.newListener([this](bool) { + listener_playback_play(this->play); + })); + this->eventListeners.push(this->pause.newListener([this](bool) { + listener_playback_pause(this->pause); + })); + this->eventListeners.push(this->stop.newListener([this](bool) { + listener_playback_stop(this->stop); + })); + this->eventListeners.push(this->seek.newListener([this](bool) { + listener_playback_seek(this->seek); + })); + + ofLogNotice(__FUNCTION__) << "Successfully opened device " << this->index << " with serial number " << this->serialNumber << "."; + + bOpen = true; + return true; + } + return false; + } + bool Device::open(int idx) { return this->open(DeviceSettings(idx), BodyTrackingSettings()); @@ -155,15 +199,22 @@ namespace ofxAzureKinect if (!this->bOpen) return false; - // Start IMU is cameras are enabled - if (this->enableIMU) + if (bPlayback) { - k4a_device_stop_imu(device.handle()); + this->stopCameras(); } + else + { + // Stop IMU if cameras are enabled + if (this->enableIMU) + { + k4a_device_stop_imu(device.handle()); + } - this->stopCameras(); + this->stopCameras(); - this->device.close(); + this->device.close(); + } this->eventListeners.unsubscribeAll(); @@ -183,14 +234,31 @@ namespace ofxAzureKinect } // Get calibration. - try + if (bPlayback) { - this->calibration = this->device.get_calibration(this->config.depth_mode, this->config.color_resolution); + auto calibration_handle = playback->get_calibration(); + this->calibration.depth_camera_calibration = calibration_handle.depth_camera_calibration; + this->calibration.color_camera_calibration = calibration_handle.color_camera_calibration; + this->calibration.depth_mode = calibration_handle.depth_mode; + this->calibration.color_resolution = calibration_handle.color_resolution; + + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + this->calibration.extrinsics[i][j] = calibration_handle.extrinsics[i][j]; + } } - catch (const k4a::error &e) + else { - ofLogError(__FUNCTION__) << e.what(); - return false; + try + { + this->calibration = this->device.get_calibration(this->config.depth_mode, this->config.color_resolution); + } + catch (const k4a::error &e) + { + ofLogError(__FUNCTION__) << e.what(); + return false; + } } if (this->bUpdateColor) @@ -235,20 +303,27 @@ namespace ofxAzureKinect } // Start cameras. - try + if (bPlayback) { - this->device.start_cameras(&this->config); + playback->play(); } - catch (const k4a::error &e) + else { - ofLogError(__FUNCTION__) << e.what(); - return false; - } + try + { + this->device.start_cameras(&this->config); + } + catch (const k4a::error &e) + { + ofLogError(__FUNCTION__) << e.what(); + return false; + } - // Can only start the IMU if cameras are enabled - if (this->enableIMU) - { - k4a_device_start_imu(device.handle()); + // Can only start the IMU if cameras are enabled + if (this->enableIMU) + { + k4a_device_start_imu(device.handle()); + } } this->startThread(); @@ -280,7 +355,14 @@ namespace ofxAzureKinect this->bodyTracker = nullptr; } - this->device.stop_cameras(); + if (bPlayback) + { + playback->close(); + } + else + { + this->device.stop_cameras(); + } this->bStreaming = false; @@ -329,18 +411,47 @@ namespace ofxAzureKinect void Device::updatePixels() { // Get a capture. - try + if (bPlayback) { - if (!this->device.get_capture(&this->capture, std::chrono::milliseconds(TIMEOUT_IN_MS))) + if (playback->is_playing()) { - ofLogWarning(__FUNCTION__) << "Timed out waiting for a capture for device " << this->index << "::" << this->serialNumber << "."; + capture = k4a::capture(playback->get_next_capture()); + if (enableIMU) + { + imu_sample = playback->get_next_imu_sample(); + // printf(" | Accelerometer temperature:%.2f x:%.4f y:%.4f z: %.4f\n", + // imu_sample.temperature, + // imu_sample.acc_sample.xyz.x, + // imu_sample.acc_sample.xyz.y, + // imu_sample.acc_sample.xyz.z); + } + } + else if (playback->is_paused()) + { + playback->seek(); + capture = k4a::capture(playback->get_next_capture()); + } + else + { + // if we are stopped, just return return; } } - catch (const k4a::error &e) + else { - ofLogError(__FUNCTION__) << e.what(); - return; + try + { + if (!this->device.get_capture(&this->capture, std::chrono::milliseconds(TIMEOUT_IN_MS))) + { + ofLogWarning(__FUNCTION__) << "Timed out waiting for a capture for device " << this->index << "::" << this->serialNumber << "."; + return; + } + } + catch (const k4a::error &e) + { + ofLogError(__FUNCTION__) << e.what(); + return; + } } // Probe for a depth16 image. @@ -477,7 +588,7 @@ namespace ofxAzureKinect { if (this->bUpdateColor) { - this->updatePointsCache(colorImg, this->colorToWorldImg); + this->updatePointsCache(depthImg, this->colorToWorldImg); } else { @@ -945,7 +1056,7 @@ namespace ofxAzureKinect return this->pointCloudVbo; } - void Device::handle_recording(bool val) + void Device::handle_recording(bool val) { if (val) { @@ -966,4 +1077,22 @@ namespace ofxAzureKinect else return -1; } + + void Device::listener_playback_play(bool val) + { + playback->play(); + } + void Device::listener_playback_pause(bool val) + { + playback->pause(); + } + void Device::listener_playback_stop(bool val) + { + playback->stop(); + } + void Device::listener_playback_seek(float val) + { + playback->seek(val); + } + } // namespace ofxAzureKinect diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 6c3a366..71ed4b3 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -16,6 +16,7 @@ #include "Types.h" #include "Record.h" +#include "Playback.h" namespace ofxAzureKinect { @@ -65,6 +66,7 @@ namespace ofxAzureKinect Device(); ~Device(); + bool open(string filename); bool open(int idx = 0); bool open(DeviceSettings settings); bool open(DeviceSettings settings, BodyTrackingSettings bodyTrackingSettings); @@ -114,8 +116,12 @@ namespace ofxAzureKinect public: ofParameter jointSmoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; - ofParameter bRecord{"bRecord", false}; + ofParameter bRecord{"bRecord", false}; float get_recording_timer_delay(); + ofParameter play{"play", false}; + ofParameter pause{"pause", false}; + ofParameter stop{"stop", false}; + ofParameter seek{"Seek",0.0f, 0.0f, 1.0f}; protected: void threadedFunction() override; @@ -139,6 +145,7 @@ namespace ofxAzureKinect int index; bool bOpen; bool bStreaming; + bool bPlayback; bool bUpdateColor; bool bUpdateIr; @@ -165,6 +172,7 @@ namespace ofxAzureKinect tjhandle jpegDecompressor; + k4a_imu_sample_t imu_sample; bool enableIMU = false; void startIMU(); @@ -205,5 +213,12 @@ namespace ofxAzureKinect Record *recording; void handle_recording(bool val); + + Playback *playback; + void listener_playback_play(bool val); + void listener_playback_pause(bool val); + void listener_playback_stop(bool val); + void listener_playback_seek(float val); + }; } // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp new file mode 100644 index 0000000..6f46fa8 --- /dev/null +++ b/src/ofxAzureKinect/Playback.cpp @@ -0,0 +1,153 @@ +#include "Playback.h" + +namespace ofxAzureKinect +{ + + bool Playback::load_file(string _filename) + { + // Convert filename from string to char* + char *temp = new char[_filename.size() + 1]; + copy(_filename.begin(), _filename.end(), temp); + temp[_filename.size()] = '\0'; + + // Set the filename for recording output + this->filename = temp; + + if (k4a_playback_open(this->filename, &playback) != K4A_RESULT_SUCCEEDED) + { + ofLogError(__FUNCTION__) << "Failed to open recording: " << filename; + k4a_failed = true; + return false; + } + + recording_length = k4a_playback_get_last_timestamp_usec(playback); + printf("Recording is %lld seconds long\n", recording_length / 1000000); + + return true; + } + + void Playback::play() + { + status = PLAYING; + cout << "play" << endl; + } + + void Playback::pause() + { + status = PAUSED; + cout << "pause" << endl; + } + + void Playback::stop() + { + seek(0); + status = STOPPED; + cout << "stop" << endl; + } + + k4a_capture_t Playback::get_next_capture() + { + k4a_stream_result_t result = k4a_playback_get_next_capture(playback, &capture); + if (result == K4A_STREAM_RESULT_SUCCEEDED) + { + return capture; + } + else if (result == K4A_STREAM_RESULT_EOF) + { + // End of file reached + // ofLog() << "End of file reached." << endl; + seek(0); + return get_next_capture(); + } + else if (result == K4A_STREAM_RESULT_FAILED) + { + ofLogError(__FUNCTION__) << "Failed to read entire recording." << endl; + return nullptr; + } + return nullptr; + } + + k4a_imu_sample_t Playback::get_next_imu_sample() + { + // k4a_imu_sample_t imu_sample; + // checking for the IMU tracking isn't working ... may have to add a special tag when recording? + // cout << "IMU TRACK: " << k4a_playback_check_track_exists (playback, "K4A_IMU_TRACK") << endl; + if (config.imu_track_enabled) + k4a_playback_get_next_imu_sample(playback, &imu_sample); + return imu_sample; + } + + void Playback::seek(float amt) + { + seek_head = amt; + int play_head = int(ofMap(amt, 0, 1, 0, recording_length, true)); + + // Seek to 10 seconds from the start + if (k4a_playback_seek_timestamp(playback, play_head, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) + { + ofLogError(__FUNCTION__) << "K4A_PLAYBACK_SEEK FAILED."; + return; + } + } + + void Playback::seek() + { + int play_head = int(ofMap(seek_head, 0, 1, 0, recording_length, true)); + + // Seek to 10 seconds from the start + if (k4a_playback_seek_timestamp(playback, play_head, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) + { + ofLogError(__FUNCTION__) << "K4A_PLAYBACK_SEEK FAILED."; + return; + } + } + + void Playback::close() + { + k4a_playback_close(playback); + } + + k4a_record_configuration_t Playback::get_device_settings() + { + k4a_playback_get_record_configuration(playback, &config); + return config; + } + + k4a_calibration_t Playback::get_calibration() + { + k4a_calibration_t calibration; + if (K4A_RESULT_SUCCEEDED != k4a_playback_get_calibration(playback, &calibration)) + { + ofLogError(__FUNCTION__) << "Failed to get calibration data from recording."; + } + return calibration; + } + + string Playback::get_serial_number() + { + return get_tag("K4A_DEVICE_SERIAL_NUMBER"); + } + + string Playback::get_tag(string tag_name) + { + char result_buffer[256]; + size_t result_size = 256; + + k4a_buffer_result_t result = k4a_playback_get_tag( + playback, tag_name.c_str(), result_buffer, &result_size); + if (K4A_BUFFER_RESULT_SUCCEEDED == result) + { + return result_buffer; + } + else if (K4A_BUFFER_RESULT_TOO_SMALL == result) + { + ofLogError(__FUNCTION__) << "Tag's {" << tag_name << "} has content that is too long."; + } + else + { + ofLogError(__FUNCTION__) << "Tag {" << tag_name << "} does not exist."; + } + return ""; + } + +} // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Playback.h b/src/ofxAzureKinect/Playback.h new file mode 100644 index 0000000..f0f1786 --- /dev/null +++ b/src/ofxAzureKinect/Playback.h @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include +#include + +#include "ofMain.h" + +// TO DO: +// Add Event Listeners to play, pause, seek +// Fix Playback open and close in Device for + +namespace ofxAzureKinect +{ + class Playback + { + + public: + Playback(){}; + ~Playback(){}; + + bool load_file(string _filename); + k4a_record_configuration_t get_device_settings(); + string get_serial_number(); + k4a_calibration_t get_calibration(); + + k4a_capture_t get_next_capture(); + k4a_imu_sample_t get_next_imu_sample(); + + void seek(); + void seek(float t); + + void play(); + void pause(); + void stop(); + void close(); + + bool is_playing() { return status == PLAYING; } + bool is_paused() { return status == PAUSED; } + bool is_stopped() { return status == STOPPED; } + + private: + k4a_playback_t playback; + k4a_capture_t capture; + k4a_imu_sample_t imu_sample; + k4a_record_configuration_t config; + + char *filename; + bool k4a_failed = false; + + int recording_length = 0; + float seek_head = 0; + + float in = 0, out = 0; + + string get_tag(string tag_name); + string serial_number; + + enum Status + { + STOPPED, + PAUSED, + PLAYING + }; + Status status = STOPPED; + }; +} // namespace ofxAzureKinect \ No newline at end of file From c370ce7de75090c60efe02ed6dbfe34f7096e26a Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Tue, 23 Jun 2020 22:15:49 -0400 Subject: [PATCH 08/48] added playback example --- example-playback/addons.make | 1 + example-playback/src/main.cpp | 13 ++++ example-playback/src/ofApp.cpp | 129 +++++++++++++++++++++++++++++++++ example-playback/src/ofApp.h | 35 +++++++++ 4 files changed, 178 insertions(+) create mode 100644 example-playback/addons.make create mode 100644 example-playback/src/main.cpp create mode 100644 example-playback/src/ofApp.cpp create mode 100644 example-playback/src/ofApp.h diff --git a/example-playback/addons.make b/example-playback/addons.make new file mode 100644 index 0000000..b155bef --- /dev/null +++ b/example-playback/addons.make @@ -0,0 +1 @@ +ofxAzureKinect diff --git a/example-playback/src/main.cpp b/example-playback/src/main.cpp new file mode 100644 index 0000000..e57370b --- /dev/null +++ b/example-playback/src/main.cpp @@ -0,0 +1,13 @@ +#include "ofMain.h" +#include "ofApp.h" + +//======================================================================== +int main( ){ + ofSetupOpenGL(1024,768,OF_WINDOW); // <-------- setup the GL context + + // this kicks off the running of my app + // can be OF_WINDOW or OF_FULLSCREEN + // pass in width and height too: + ofRunApp(new ofApp()); + +} diff --git a/example-playback/src/ofApp.cpp b/example-playback/src/ofApp.cpp new file mode 100644 index 0000000..13d1158 --- /dev/null +++ b/example-playback/src/ofApp.cpp @@ -0,0 +1,129 @@ +#include "ofApp.h" + +//-------------------------------------------------------------- +void ofApp::setup() +{ + // place a recording from k4arecorder or example-record in the data folder + filename = ofToDataPath("output.mkv"); + + device.open(filename); + device.startCameras(); +} + +//-------------------------------------------------------------- +void ofApp::update() +{ +} + +//-------------------------------------------------------------- +void ofApp::draw() +{ + ofBackground(128); + + // visualize kinect streams + if (this->device.isStreaming()) + { + this->device.getColorTex().draw(0, 0, 1280, 720); + this->device.getDepthTex().draw(1280, 0, 360, 360); + this->device.getIrTex().draw(1280, 360, 360, 360); + } + + ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20); + + stringstream ss; + ss << "Press SPACEBAR to play / pause the recording." << endl; + ss << "Press + / - to seek when the recording is paused." << endl; + ss << "Press 'f' to toggle fullscreen."; + ofDrawBitmapStringHighlight(ss.str(), 10, ofGetHeight() - 50); +} + +//-------------------------------------------------------------- +void ofApp::exit() +{ + device.close(); +} + +//-------------------------------------------------------------- +void ofApp::keyPressed(int key) +{ + switch (key) + { + case ' ': + { + play = !play; + if (play) + { + device.play = true; + } + else + { + device.pause = false; + } + break; + } + case '+': + play_head = ofClamp(play_head + .01, 0, 1); + device.seek = play_head; + break; + case '-': + play_head = ofClamp(play_head - .01, 0, 1); + device.seek = play_head; + break; + case 'f': + case 'F': + ofToggleFullscreen(); + break; + default: + break; + } +} + +//-------------------------------------------------------------- +void ofApp::keyReleased(int key) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseMoved(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseDragged(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mousePressed(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseReleased(int x, int y, int button) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseEntered(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::mouseExited(int x, int y) +{ +} + +//-------------------------------------------------------------- +void ofApp::windowResized(int w, int h) +{ +} + +//-------------------------------------------------------------- +void ofApp::gotMessage(ofMessage msg) +{ +} + +//-------------------------------------------------------------- +void ofApp::dragEvent(ofDragInfo dragInfo) +{ +} diff --git a/example-playback/src/ofApp.h b/example-playback/src/ofApp.h new file mode 100644 index 0000000..b612064 --- /dev/null +++ b/example-playback/src/ofApp.h @@ -0,0 +1,35 @@ +#pragma once + +#include "ofMain.h" + +#include "ofxAzureKinect.h" +#include "ofxAzureKinect/Device.h" + +class ofApp : public ofBaseApp{ + + public: + void setup(); + void update(); + void draw(); + void exit(); + + void keyPressed(int key); + void keyReleased(int key); + void mouseMoved(int x, int y ); + void mouseDragged(int x, int y, int button); + void mousePressed(int x, int y, int button); + void mouseReleased(int x, int y, int button); + void mouseEntered(int x, int y); + void mouseExited(int x, int y); + void windowResized(int w, int h); + void dragEvent(ofDragInfo dragInfo); + void gotMessage(ofMessage msg); + + ofxAzureKinect::Device device; + + // ofxAzureKinect::Playback playback; + string filename; + + bool play = false; + float play_head = 0; +}; From 30fd3cbb2e88367e2ca3675ef5562a99e83bba48 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Wed, 24 Jun 2020 14:09:34 -0400 Subject: [PATCH 09/48] added data folder --- example-playback/bin/data/.gitkeep | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 example-playback/bin/data/.gitkeep diff --git a/example-playback/bin/data/.gitkeep b/example-playback/bin/data/.gitkeep new file mode 100644 index 0000000..e69de29 From 9135433508e5632a1bb6bf36678e9846797f0cdb Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Wed, 24 Jun 2020 14:10:04 -0400 Subject: [PATCH 10/48] added data folder --- example-record/bin/data/.gitkeep | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 example-record/bin/data/.gitkeep diff --git a/example-record/bin/data/.gitkeep b/example-record/bin/data/.gitkeep new file mode 100644 index 0000000..e69de29 From 9a8a6795016b9ce484439c87c23a582c07fd4757 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Wed, 24 Jun 2020 15:59:35 -0400 Subject: [PATCH 11/48] added body tracking to playback --- src/ofxAzureKinect/Device.cpp | 19 +++++++++++++++++-- src/ofxAzureKinect/Device.h | 1 + src/ofxAzureKinect/Playback.cpp | 2 +- src/ofxAzureKinect/Playback.h | 3 --- 4 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 928fac3..97c2dff 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -53,7 +53,7 @@ namespace ofxAzureKinect this->bUpdateColor = playback_config.color_track_enabled; this->bUpdateIr = playback_config.ir_track_enabled; this->bUpdateWorld = playback_config.depth_track_enabled; - this->bUpdateVbo = false; //bUpdateWorld; + this->bUpdateVbo = playback_config.depth_track_enabled; // Add Playback Listeners this->eventListeners.push(this->play.newListener([this](bool) { @@ -77,6 +77,21 @@ namespace ofxAzureKinect return false; } + bool Device::open(string filename, BodyTrackingSettings bodyTrackingSettings) + { + this->trackerConfig.sensor_orientation = bodyTrackingSettings.sensorOrientation; + this->trackerConfig.gpu_device_id = bodyTrackingSettings.gpuDeviceID; + + this->bUpdateBodies = bodyTrackingSettings.updateBodies; + if (this->bUpdateBodies) + { + this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { + k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); + })); + } + return open(filename); + } + bool Device::open(int idx) { return this->open(DeviceSettings(idx), BodyTrackingSettings()); @@ -588,7 +603,7 @@ namespace ofxAzureKinect { if (this->bUpdateColor) { - this->updatePointsCache(depthImg, this->colorToWorldImg); + this->updatePointsCache(depthImg, this->depthToWorldImg);//(colorImg, this->colorToWorldImg); } else { diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 71ed4b3..edc3ce3 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -67,6 +67,7 @@ namespace ofxAzureKinect ~Device(); bool open(string filename); + bool open(string filename, BodyTrackingSettings bodyTrackingSettings); bool open(int idx = 0); bool open(DeviceSettings settings); bool open(DeviceSettings settings, BodyTrackingSettings bodyTrackingSettings); diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp index 6f46fa8..91711e9 100644 --- a/src/ofxAzureKinect/Playback.cpp +++ b/src/ofxAzureKinect/Playback.cpp @@ -80,7 +80,7 @@ namespace ofxAzureKinect void Playback::seek(float amt) { seek_head = amt; - int play_head = int(ofMap(amt, 0, 1, 0, recording_length, true)); + int play_head = int(ofMap(seek_head, 0, 1, 0, recording_length, true)); // Seek to 10 seconds from the start if (k4a_playback_seek_timestamp(playback, play_head, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) diff --git a/src/ofxAzureKinect/Playback.h b/src/ofxAzureKinect/Playback.h index f0f1786..18bf85f 100644 --- a/src/ofxAzureKinect/Playback.h +++ b/src/ofxAzureKinect/Playback.h @@ -7,9 +7,6 @@ #include "ofMain.h" -// TO DO: -// Add Event Listeners to play, pause, seek -// Fix Playback open and close in Device for namespace ofxAzureKinect { From 70790cc8d8d3ed28a2d9659593093bbecd60769b Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 11:45:04 -0400 Subject: [PATCH 12/48] moved body tracking into its own class --- src/ofxAzureKinect/BodyTracker.cpp | 102 ++++++++++++++++++++++++++ src/ofxAzureKinect/BodyTracker.h | 53 ++++++++++++++ src/ofxAzureKinect/Device.cpp | 113 ++++++++++++----------------- src/ofxAzureKinect/Device.h | 8 +- 4 files changed, 208 insertions(+), 68 deletions(-) create mode 100644 src/ofxAzureKinect/BodyTracker.cpp create mode 100644 src/ofxAzureKinect/BodyTracker.h diff --git a/src/ofxAzureKinect/BodyTracker.cpp b/src/ofxAzureKinect/BodyTracker.cpp new file mode 100644 index 0000000..b1adbd2 --- /dev/null +++ b/src/ofxAzureKinect/BodyTracker.cpp @@ -0,0 +1,102 @@ +#include "BodyTracker.h" + +namespace ofxAzureKinect +{ + BodyTracker::BodyTracker() + { + } + + BodyTracker::BodyTracker(k4a_calibration_t calibration, k4abt_tracker_configuration_t config) + { + k4abt_tracker_create(&calibration, config, &tracker); + } + + void BodyTracker::update(k4a_capture_t capture) + { + k4a_wait_result_t enqueueResult = k4abt_tracker_enqueue_capture(tracker, capture, K4A_WAIT_INFINITE); + if (enqueueResult == K4A_WAIT_RESULT_FAILED) + { + ofLogError(__FUNCTION__) << "Failed adding capture to tracker process queue!"; + } + else + { + k4abt_frame_t bodyFrame = nullptr; + k4a_wait_result_t result = k4abt_tracker_pop_result(tracker, &bodyFrame, K4A_WAIT_INFINITE); + if (result == K4A_WAIT_RESULT_SUCCEEDED) + { + // Probe for a body index map image. + k4a::image bodyIndexImg = k4abt_frame_get_body_index_map(bodyFrame); + + // Update the Body Index Map ofPixels and ofTexture + const auto bodyIndexSize = glm::ivec2(bodyIndexImg.get_width_pixels(), bodyIndexImg.get_height_pixels()); + if (!this->bodyIndexPix.isAllocated()) + { + this->bodyIndexPix.allocate(bodyIndexSize.x, bodyIndexSize.y, 1); + } + + // Set the ofPixels + const auto bodyIndexData = reinterpret_cast(bodyIndexImg.get_buffer()); + this->bodyIndexPix.setFromPixels(bodyIndexData, bodyIndexSize.x, bodyIndexSize.y, 1); + + // Release the body index map image + ofLogVerbose(__FUNCTION__) << "Capture BodyIndex " << bodyIndexSize.x << "x" << bodyIndexSize.y << " stride: " << bodyIndexImg.get_stride_bytes() << "."; + bodyIndexImg.reset(); + + // Set number of bodies found + num_bodies = k4abt_frame_get_num_bodies(bodyFrame); + ofLogVerbose(__FUNCTION__) << num_bodies << " bodies found!"; + + // Update Found Skeletons + this->bodySkeletons.resize(num_bodies); + this->bodyIDs.resize(num_bodies); + for (size_t i = 0; i < num_bodies; i++) + { + k4abt_skeleton_t skeleton; + k4abt_frame_get_body_skeleton(bodyFrame, i, &skeleton); + this->bodySkeletons[i] = skeleton; + uint32_t id = k4abt_frame_get_body_id(bodyFrame, i); + this->bodyIDs[i] = id; + } + + // Release body frame once we're finished. + k4abt_frame_release(bodyFrame); + } + } + } + + void BodyTracker::update_texture() + { + if (!this->bodyIndexTex.isAllocated()) + { + this->bodyIndexTex.allocate(this->bodyIndexPix); + this->bodyIndexTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); + } + this->bodyIndexTex.loadData(this->bodyIndexPix); + } + + const ofPixels &BodyTracker::getBodyIndexPix() const + { + return this->bodyIndexPix; + } + + const ofTexture &BodyTracker::getBodyIndexTex() const + { + return this->bodyIndexTex; + } + + size_t BodyTracker::getNumBodies() const + { + return this->bodySkeletons.size(); + } + + const std::vector &BodyTracker::getBodySkeletons() const + { + return this->bodySkeletons; + } + + const std::vector &BodyTracker::getBodyIDs() const + { + return this->bodyIDs; + } + +} // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/BodyTracker.h b/src/ofxAzureKinect/BodyTracker.h new file mode 100644 index 0000000..2e95fd7 --- /dev/null +++ b/src/ofxAzureKinect/BodyTracker.h @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include "ofMain.h" + +namespace ofxAzureKinect +{ + class BodyTracker + { + + public: + BodyTracker(); + BodyTracker(k4a_calibration_t calibration, k4abt_tracker_configuration_t config); + ~BodyTracker(){}; + + void update(k4a_capture_t capture); + void update_texture(); // why a seg fault when called in update?? + + // std::vector get_skeletons(); + // k4abt_skeleton_t get_skeletons(int id); + + + const ofPixels &getBodyIndexPix() const; + const ofTexture &getBodyIndexTex() const; + + size_t getNumBodies() const; + const std::vector &getBodySkeletons() const; + const std::vector &getBodyIDs() const; + + private: + k4abt_tracker_t tracker; + + size_t num_bodies = 0; + + bool active = false; + + std::vector bodySkeletons; + std::vector bodyIDs; + + // Visualization + ofPixels bodyIndexPix; + ofTexture bodyIndexTex; + void draw_skeleton(); + void draw_pointcloud(); + void draw_mask(); + + void load_shader(); + ofShader shader; + + ofVbo pointsVbo; + }; +} // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 97c2dff..ea85608 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -79,17 +79,41 @@ namespace ofxAzureKinect bool Device::open(string filename, BodyTrackingSettings bodyTrackingSettings) { - this->trackerConfig.sensor_orientation = bodyTrackingSettings.sensorOrientation; - this->trackerConfig.gpu_device_id = bodyTrackingSettings.gpuDeviceID; - - this->bUpdateBodies = bodyTrackingSettings.updateBodies; - if (this->bUpdateBodies) + bool result = open(filename); + + if (result) { - this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { - k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); - })); + this->bUpdateBodies = bodyTrackingSettings.updateBodies; + + // Setup Body Tracking Config + this->trackerConfig.sensor_orientation = bodyTrackingSettings.sensorOrientation; + this->trackerConfig.gpu_device_id = bodyTrackingSettings.gpuDeviceID; + + // Setup Device Calibration + auto calibration_handle = playback->get_calibration(); + this->calibration.depth_camera_calibration = calibration_handle.depth_camera_calibration; + this->calibration.color_camera_calibration = calibration_handle.color_camera_calibration; + this->calibration.depth_mode = calibration_handle.depth_mode; + this->calibration.color_resolution = calibration_handle.color_resolution; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + this->calibration.extrinsics[i][j] = calibration_handle.extrinsics[i][j]; + } + + // Create Body Tracker + tracker = BodyTracker(calibration, trackerConfig); + + // Add Body Tracking Listeners + if (this->bUpdateBodies) + { + this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { + k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); + })); + } } - return open(filename); + + return result; } bool Device::open(int idx) @@ -554,56 +578,14 @@ namespace ofxAzureKinect if (this->bUpdateBodies) { - k4a_wait_result_t enqueueResult = k4abt_tracker_enqueue_capture(this->bodyTracker, this->capture.handle(), K4A_WAIT_INFINITE); - if (enqueueResult == K4A_WAIT_RESULT_FAILED) - { - ofLogError(__FUNCTION__) << "Failed adding capture to tracker process queue!"; - } - else - { - k4abt_frame_t bodyFrame = nullptr; - k4a_wait_result_t popResult = k4abt_tracker_pop_result(this->bodyTracker, &bodyFrame, K4A_WAIT_INFINITE); - if (popResult == K4A_WAIT_RESULT_SUCCEEDED) - { - // Probe for a body index map image. - k4a::image bodyIndexImg = k4abt_frame_get_body_index_map(bodyFrame); - const auto bodyIndexSize = glm::ivec2(bodyIndexImg.get_width_pixels(), bodyIndexImg.get_height_pixels()); - if (!this->bodyIndexPix.isAllocated()) - { - this->bodyIndexPix.allocate(bodyIndexSize.x, bodyIndexSize.y, 1); - } - - const auto bodyIndexData = reinterpret_cast(bodyIndexImg.get_buffer()); - this->bodyIndexPix.setFromPixels(bodyIndexData, bodyIndexSize.x, bodyIndexSize.y, 1); - - ofLogVerbose(__FUNCTION__) << "Capture BodyIndex " << bodyIndexSize.x << "x" << bodyIndexSize.y << " stride: " << bodyIndexImg.get_stride_bytes() << "."; - bodyIndexImg.reset(); - - size_t numBodies = k4abt_frame_get_num_bodies(bodyFrame); - ofLogVerbose(__FUNCTION__) << numBodies << " bodies found!"; - - this->bodySkeletons.resize(numBodies); - this->bodyIDs.resize(numBodies); - for (size_t i = 0; i < numBodies; i++) - { - k4abt_skeleton_t skeleton; - k4abt_frame_get_body_skeleton(bodyFrame, i, &skeleton); - this->bodySkeletons[i] = skeleton; - uint32_t id = k4abt_frame_get_body_id(bodyFrame, i); - this->bodyIDs[i] = id; - } - - // Release body frame once we're finished. - k4abt_frame_release(bodyFrame); - } - } + tracker.update(capture.handle()); } if (this->bUpdateVbo) { if (this->bUpdateColor) { - this->updatePointsCache(depthImg, this->depthToWorldImg);//(colorImg, this->colorToWorldImg); + this->updatePointsCache(depthImg, this->depthToWorldImg); //(colorImg, this->colorToWorldImg); } else { @@ -618,7 +600,7 @@ namespace ofxAzureKinect this->updateColorInDepthFrame(depthImg, colorImg); } - // Do any recording before releaseing the capture + // Do any recording before releasing the capture if (bRecord) { k4a_capture_t capture_handle = capture.handle(); @@ -690,13 +672,7 @@ namespace ofxAzureKinect if (this->bUpdateBodies) { - if (!this->bodyIndexTex.isAllocated()) - { - this->bodyIndexTex.allocate(this->bodyIndexPix); - this->bodyIndexTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); - } - - this->bodyIndexTex.loadData(this->bodyIndexPix); + tracker.update_texture(); } if (this->bUpdateVbo) @@ -1043,27 +1019,32 @@ namespace ofxAzureKinect const ofPixels &Device::getBodyIndexPix() const { - return this->bodyIndexPix; + return tracker.getBodyIndexPix(); + // return this->bodyIndexPix; } const ofTexture &Device::getBodyIndexTex() const { - return this->bodyIndexTex; + return tracker.getBodyIndexTex(); + // return this->bodyIndexTex; } size_t Device::getNumBodies() const { - return this->bodySkeletons.size(); + return tracker.getNumBodies(); + // return this->bodySkeletons.size(); } const std::vector &Device::getBodySkeletons() const { - return this->bodySkeletons; + return tracker.getBodySkeletons(); + // return this->bodySkeletons; } const std::vector &Device::getBodyIDs() const { - return this->bodyIDs; + return tracker.getBodyIDs(); + // return this->bodyIDs; } const ofVbo &Device::getPointCloudVbo() const diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index edc3ce3..7b286c2 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -17,6 +17,7 @@ #include "Types.h" #include "Record.h" #include "Playback.h" +#include "BodyTracker.h" namespace ofxAzureKinect { @@ -115,6 +116,8 @@ namespace ofxAzureKinect const ofVbo &getPointCloudVbo() const; + BodyTracker *get_body_tracker() { return &tracker; } + public: ofParameter jointSmoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; ofParameter bRecord{"bRecord", false}; @@ -122,7 +125,7 @@ namespace ofxAzureKinect ofParameter play{"play", false}; ofParameter pause{"pause", false}; ofParameter stop{"stop", false}; - ofParameter seek{"Seek",0.0f, 0.0f, 1.0f}; + ofParameter seek{"Seek", 0.0f, 0.0f, 1.0f}; protected: void threadedFunction() override; @@ -212,6 +215,8 @@ namespace ofxAzureKinect ofEventListeners eventListeners; + BodyTracker tracker; + Record *recording; void handle_recording(bool val); @@ -220,6 +225,5 @@ namespace ofxAzureKinect void listener_playback_pause(bool val); void listener_playback_stop(bool val); void listener_playback_seek(float val); - }; } // namespace ofxAzureKinect \ No newline at end of file From b2d3f2f401fb34a80739e2ce1048f104e6292483 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 16:08:34 -0400 Subject: [PATCH 13/48] moved visualization into BodyTracker class --- src/ofxAzureKinect/BodyTracker.cpp | 210 ++++++++++++++++++++++++++++- src/ofxAzureKinect/BodyTracker.h | 28 +++- src/ofxAzureKinect/Device.cpp | 1 + 3 files changed, 229 insertions(+), 10 deletions(-) diff --git a/src/ofxAzureKinect/BodyTracker.cpp b/src/ofxAzureKinect/BodyTracker.cpp index b1adbd2..f36c8ea 100644 --- a/src/ofxAzureKinect/BodyTracker.cpp +++ b/src/ofxAzureKinect/BodyTracker.cpp @@ -2,13 +2,12 @@ namespace ofxAzureKinect { - BodyTracker::BodyTracker() - { - } BodyTracker::BodyTracker(k4a_calibration_t calibration, k4abt_tracker_configuration_t config) { k4abt_tracker_create(&calibration, config, &tracker); + active = true; + load_shaders(); } void BodyTracker::update(k4a_capture_t capture) @@ -27,7 +26,7 @@ namespace ofxAzureKinect // Probe for a body index map image. k4a::image bodyIndexImg = k4abt_frame_get_body_index_map(bodyFrame); - // Update the Body Index Map ofPixels and ofTexture + // Update the Body Index Map ofPixels const auto bodyIndexSize = glm::ivec2(bodyIndexImg.get_width_pixels(), bodyIndexImg.get_height_pixels()); if (!this->bodyIndexPix.isAllocated()) { @@ -49,6 +48,7 @@ namespace ofxAzureKinect // Update Found Skeletons this->bodySkeletons.resize(num_bodies); this->bodyIDs.resize(num_bodies); + skeletonMeshes.clear(); for (size_t i = 0; i < num_bodies; i++) { k4abt_skeleton_t skeleton; @@ -56,6 +56,9 @@ namespace ofxAzureKinect this->bodySkeletons[i] = skeleton; uint32_t id = k4abt_frame_get_body_id(bodyFrame, i); this->bodyIDs[i] = id; + + skeletonMeshes.push_back(new ofVboMesh()); + skeletonMeshes[skeletonMeshes.size() - 1]->setMode(OF_PRIMITIVE_LINES); } // Release body frame once we're finished. @@ -74,6 +77,205 @@ namespace ofxAzureKinect this->bodyIndexTex.loadData(this->bodyIndexPix); } + // The value for each pixel represents which body the pixel belongs to. + // It can be either background (K4ABT_BODY_INDEX_MAP_BACKGROUND) + // or the index of a detected k4abt_body_t. + // https://docs.microsoft.com/en-us/azure/kinect-dk/body-index-map + void BodyTracker::draw_body_map(int x, int y, int w, int h) + { + // Draw the 2D Map of all Detected Bodies + bodyIndexTex.draw(x, y, w, h); + } + + void BodyTracker::draw_point_clouds(ofTexture depth_texture, ofTexture depth_to_world_texture) + { + ofEnableDepthTest(); + constexpr int kMaxBodies = 6; + int _bodyIDs[kMaxBodies]; + int i = 0; + while (i < num_bodies) + { + _bodyIDs[i] = bodyIDs[i]; + ++i; + } + while (i < kMaxBodies) + { + _bodyIDs[i] = 0; + ++i; + } + + this->shader.begin(); + { + this->shader.setUniformTexture("uDepthTex", depth_texture, 1); + this->shader.setUniformTexture("uBodyIndexTex", bodyIndexTex, 2); + this->shader.setUniformTexture("uWorldTex", depth_to_world_texture, 3); + this->shader.setUniform2i("uFrameSize", depth_texture.getWidth(), depth_texture.getHeight()); + this->shader.setUniform1iv("uBodyIDs", _bodyIDs, kMaxBodies); + + int numPoints = depth_texture.getWidth() * depth_texture.getHeight(); + this->pointsVbo.drawInstanced(GL_POINTS, 0, 1, numPoints); + } + this->shader.end(); + ofDisableDepthTest(); + } + + void BodyTracker::draw_skeletons() + { + for (int i = 0; i < num_bodies; i++) + { + draw_skeleton(i); + } + } + + void BodyTracker::draw_skeleton(int id) + { + if (id >= num_bodies) + { + ofLogError(__FUNCTION__) << "Invaild Body Index {" << id << "}. Body does not exist."; + return; + } + ofPushStyle(); + + auto skeleton = bodySkeletons[id]; + // Draw Joints + for (int i = 0; i < K4ABT_JOINT_COUNT; ++i) + { + auto joint = skeleton.joints[i]; + ofPushMatrix(); + { + glm::mat4 transform = glm::translate(toGlm(joint.position)) * glm::toMat4(toGlm(joint.orientation)); + ofMultMatrix(transform); + ofNoFill(); + ofDrawBox(15); + ofDrawAxis(75.0f); + + if (i == K4ABT_JOINT_SPINE_CHEST) + { + ofPushMatrix(); + ofTranslate(500, 0, 0); + ofDrawBitmapStringHighlight(ofToString(bodyIDs[id]), 0, 0); + ofPopMatrix(); + } + } + ofPopMatrix(); + } + + // Draw Bones + auto &vertices = skeletonMeshes[id]->getVertices(); + vertices.resize(50); + + int vdx = 0; + + // Spine. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_NAVEL].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_NAVEL].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_CHEST].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_CHEST].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HEAD].position); + + // Head. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HEAD].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EAR_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EAR_RIGHT].position); + + // Left Leg. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_FOOT_LEFT].position); + + // Right leg. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_FOOT_RIGHT].position); + + // Left arm. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_LEFT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_LEFT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_WRIST_LEFT].position); + + // Right arm. + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_RIGHT].position); + + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_RIGHT].position); + vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_WRIST_RIGHT].position); + + ofSetColor(200); + skeletonMeshes[id]->draw(); + + ofPopStyle(); + } + + //-------------------------------------------------------------- + void BodyTracker::load_shaders() + { + auto shaderSettings = ofShaderSettings(); + shaderSettings.shaderFiles[GL_VERTEX_SHADER] = "shaders/render.vert"; + shaderSettings.shaderFiles[GL_FRAGMENT_SHADER] = "shaders/render.frag"; + shaderSettings.intDefines["BODY_INDEX_MAP_BACKGROUND"] = K4ABT_BODY_INDEX_MAP_BACKGROUND; + shaderSettings.bindDefaults = true; + + if (this->shader.setup(shaderSettings)) + { + ofLogNotice(__FUNCTION__) << "Success loading shader!"; + } + else + { + ofLogError(__FUNCTION__) << "Could not load shader. Check that they are in the /bin/data/shader directory."; + } + + // Setup PointCloud VBO + std::vector verts(1); + this->pointsVbo.setVertexData(verts.data(), verts.size(), GL_STATIC_DRAW); + } + const ofPixels &BodyTracker::getBodyIndexPix() const { return this->bodyIndexPix; diff --git a/src/ofxAzureKinect/BodyTracker.h b/src/ofxAzureKinect/BodyTracker.h index 2e95fd7..4b506ce 100644 --- a/src/ofxAzureKinect/BodyTracker.h +++ b/src/ofxAzureKinect/BodyTracker.h @@ -2,24 +2,38 @@ #include #include + #include "ofMain.h" +#include "Types.h" + namespace ofxAzureKinect { class BodyTracker { public: - BodyTracker(); + BodyTracker(){}; BodyTracker(k4a_calibration_t calibration, k4abt_tracker_configuration_t config); ~BodyTracker(){}; void update(k4a_capture_t capture); - void update_texture(); // why a seg fault when called in update?? + void update_texture(); // why do I get a seg fault when called in update?? + + vector get_skeletons(); + k4abt_skeleton_t get_skeleton(int id); - // std::vector get_skeletons(); - // k4abt_skeleton_t get_skeletons(int id); + vector get_bodies(); + k4abt_body_t get_body(int id); + + bool is_active() { return active; } + void draw_body_map(int x=0, int y=0, int w=360, int h=360); + void draw_point_clouds(ofTexture depth_texture, ofTexture depth_to_world_texture); + void draw_skeletons(); + void draw_skeleton(int id); + + const ofPixels &getBodyIndexPix() const; const ofTexture &getBodyIndexTex() const; @@ -38,14 +52,16 @@ namespace ofxAzureKinect std::vector bodySkeletons; std::vector bodyIDs; + vector skeletonMeshes; + // Visualization ofPixels bodyIndexPix; ofTexture bodyIndexTex; void draw_skeleton(); void draw_pointcloud(); - void draw_mask(); + void draw_mask(); // <-- should have access to segmented body image? - void load_shader(); + void load_shaders(); ofShader shader; ofVbo pointsVbo; diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index ea85608..f23437d 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -396,6 +396,7 @@ namespace ofxAzureKinect if (bPlayback) { + playback->stop(); playback->close(); } else From d530c560a5fa69b510d1843bb283bbadb9973fd7 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 16:30:29 -0400 Subject: [PATCH 14/48] fixed crash on close bug --- src/ofxAzureKinect/Playback.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp index 91711e9..65f28c0 100644 --- a/src/ofxAzureKinect/Playback.cpp +++ b/src/ofxAzureKinect/Playback.cpp @@ -104,6 +104,7 @@ namespace ofxAzureKinect void Playback::close() { + status = STOPPED; k4a_playback_close(playback); } From d15dacdcee4035416e60099613aad6413fc8e0a2 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 16:31:58 -0400 Subject: [PATCH 15/48] start play as true --- example-record/src/ofApp.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp index 6405950..280ddde 100644 --- a/example-record/src/ofApp.cpp +++ b/example-record/src/ofApp.cpp @@ -54,6 +54,12 @@ void ofApp::draw() } ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20); + + stringstream ss; + ss << "Press SPACEBAR to start/stop a recording." << endl; + ss << "\tRecordings are saved to the bin/data/ directory." << endl; + ss << "Press 'f' to toggle fullscreen."; + ofDrawBitmapStringHighlight(ss.str(), 10, ofGetHeight() - 50); } //-------------------------------------------------------------- From a5dbf3235db1c2a6ae9cd40722b7dbbd70ee9f47 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 17:19:06 -0400 Subject: [PATCH 16/48] start play as true --- example-playback/src/ofApp.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/example-playback/src/ofApp.h b/example-playback/src/ofApp.h index b612064..e3e6f8b 100644 --- a/example-playback/src/ofApp.h +++ b/example-playback/src/ofApp.h @@ -27,9 +27,7 @@ class ofApp : public ofBaseApp{ ofxAzureKinect::Device device; - // ofxAzureKinect::Playback playback; string filename; - bool play = false; float play_head = 0; }; From c0c82608e470d4513d87ec81b81f48fa10758921 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Thu, 25 Jun 2020 17:20:18 -0400 Subject: [PATCH 17/48] code cleanup ... still need to test with live stream. --- src/ofxAzureKinect/BodyTracker.cpp | 47 ++++++++++++++++++------------ src/ofxAzureKinect/BodyTracker.h | 38 ++++++++++-------------- src/ofxAzureKinect/Device.cpp | 43 --------------------------- src/ofxAzureKinect/Device.h | 8 ----- 4 files changed, 45 insertions(+), 91 deletions(-) diff --git a/src/ofxAzureKinect/BodyTracker.cpp b/src/ofxAzureKinect/BodyTracker.cpp index f36c8ea..7491f22 100644 --- a/src/ofxAzureKinect/BodyTracker.cpp +++ b/src/ofxAzureKinect/BodyTracker.cpp @@ -46,19 +46,19 @@ namespace ofxAzureKinect ofLogVerbose(__FUNCTION__) << num_bodies << " bodies found!"; // Update Found Skeletons - this->bodySkeletons.resize(num_bodies); + this->skeletons.resize(num_bodies); this->bodyIDs.resize(num_bodies); - skeletonMeshes.clear(); + skeleton_meshes.clear(); for (size_t i = 0; i < num_bodies; i++) { k4abt_skeleton_t skeleton; k4abt_frame_get_body_skeleton(bodyFrame, i, &skeleton); - this->bodySkeletons[i] = skeleton; + this->skeletons[i] = skeleton; uint32_t id = k4abt_frame_get_body_id(bodyFrame, i); this->bodyIDs[i] = id; - skeletonMeshes.push_back(new ofVboMesh()); - skeletonMeshes[skeletonMeshes.size() - 1]->setMode(OF_PRIMITIVE_LINES); + skeleton_meshes.push_back(new ofVboMesh()); + skeleton_meshes[skeleton_meshes.size() - 1]->setMode(OF_PRIMITIVE_LINES); } // Release body frame once we're finished. @@ -77,6 +77,17 @@ namespace ofxAzureKinect this->bodyIndexTex.loadData(this->bodyIndexPix); } + void BodyTracker::set_joint_smoothing(float val) + { + joint_smoothing = val; + k4abt_tracker_set_temporal_smoothing(tracker, joint_smoothing); + } + + float BodyTracker::get_joint_smoothing() + { + return joint_smoothing; + } + // The value for each pixel represents which body the pixel belongs to. // It can be either background (K4ABT_BODY_INDEX_MAP_BACKGROUND) // or the index of a detected k4abt_body_t. @@ -136,7 +147,7 @@ namespace ofxAzureKinect } ofPushStyle(); - auto skeleton = bodySkeletons[id]; + auto skeleton = skeletons[id]; // Draw Joints for (int i = 0; i < K4ABT_JOINT_COUNT; ++i) { @@ -161,7 +172,7 @@ namespace ofxAzureKinect } // Draw Bones - auto &vertices = skeletonMeshes[id]->getVertices(); + auto &vertices = skeleton_meshes[id]->getVertices(); vertices.resize(50); int vdx = 0; @@ -248,7 +259,7 @@ namespace ofxAzureKinect vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_WRIST_RIGHT].position); ofSetColor(200); - skeletonMeshes[id]->draw(); + skeleton_meshes[id]->draw(); ofPopStyle(); } @@ -276,29 +287,29 @@ namespace ofxAzureKinect this->pointsVbo.setVertexData(verts.data(), verts.size(), GL_STATIC_DRAW); } - const ofPixels &BodyTracker::getBodyIndexPix() const + size_t BodyTracker::get_num_bodies() const { - return this->bodyIndexPix; + return num_bodies; } - const ofTexture &BodyTracker::getBodyIndexTex() const + const vector &BodyTracker::get_skeletons() const { - return this->bodyIndexTex; + return skeletons; } - size_t BodyTracker::getNumBodies() const + const k4abt_skeleton_t &BodyTracker::get_skeleton(int i) const { - return this->bodySkeletons.size(); + return skeletons[i]; } - const std::vector &BodyTracker::getBodySkeletons() const + const ofPixels &BodyTracker::get_body_index_map_pix() const { - return this->bodySkeletons; + return bodyIndexPix; } - const std::vector &BodyTracker::getBodyIDs() const + const ofTexture &BodyTracker::get_body_index_map_tex() const { - return this->bodyIDs; + return bodyIndexTex; } } // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/BodyTracker.h b/src/ofxAzureKinect/BodyTracker.h index 4b506ce..ae210fe 100644 --- a/src/ofxAzureKinect/BodyTracker.h +++ b/src/ofxAzureKinect/BodyTracker.h @@ -20,28 +20,25 @@ namespace ofxAzureKinect void update(k4a_capture_t capture); void update_texture(); // why do I get a seg fault when called in update?? - vector get_skeletons(); - k4abt_skeleton_t get_skeleton(int id); + const vector &get_skeletons() const; + const k4abt_skeleton_t &get_skeleton(int id) const; + const vector &get_body_ids() const; + + size_t get_num_bodies() const; + + const ofPixels &get_body_index_map_pix() const; + const ofTexture &get_body_index_map_tex() const; + + void set_joint_smoothing(float val); + float get_joint_smoothing(); - vector get_bodies(); - k4abt_body_t get_body(int id); - bool is_active() { return active; } - void draw_body_map(int x=0, int y=0, int w=360, int h=360); + void draw_body_map(int x = 0, int y = 0, int w = 360, int h = 360); void draw_point_clouds(ofTexture depth_texture, ofTexture depth_to_world_texture); void draw_skeletons(); void draw_skeleton(int id); - - - const ofPixels &getBodyIndexPix() const; - const ofTexture &getBodyIndexTex() const; - - size_t getNumBodies() const; - const std::vector &getBodySkeletons() const; - const std::vector &getBodyIDs() const; - private: k4abt_tracker_t tracker; @@ -49,21 +46,18 @@ namespace ofxAzureKinect bool active = false; - std::vector bodySkeletons; + std::vector skeletons; std::vector bodyIDs; - vector skeletonMeshes; - // Visualization ofPixels bodyIndexPix; ofTexture bodyIndexTex; - void draw_skeleton(); - void draw_pointcloud(); - void draw_mask(); // <-- should have access to segmented body image? void load_shaders(); ofShader shader; - + vector skeleton_meshes; ofVbo pointsVbo; + + ofParameter joint_smoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; }; } // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index f23437d..18e2a88 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -104,13 +104,6 @@ namespace ofxAzureKinect // Create Body Tracker tracker = BodyTracker(calibration, trackerConfig); - // Add Body Tracking Listeners - if (this->bUpdateBodies) - { - this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { - k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); - })); - } } return result; @@ -216,12 +209,6 @@ namespace ofxAzureKinect this->bUpdateVbo = deviceSettings.updateWorld && deviceSettings.updateVbo; this->bUpdateBodies = bodyTrackingSettings.updateBodies; - if (this->bUpdateBodies) - { - this->eventListeners.push(this->jointSmoothing.newListener([this](float &) { - k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); - })); - } // Add Recording Listener this->eventListeners.push(this->bRecord.newListener([this](bool) { @@ -1018,36 +1005,6 @@ namespace ofxAzureKinect return this->colorInDepthTex; } - const ofPixels &Device::getBodyIndexPix() const - { - return tracker.getBodyIndexPix(); - // return this->bodyIndexPix; - } - - const ofTexture &Device::getBodyIndexTex() const - { - return tracker.getBodyIndexTex(); - // return this->bodyIndexTex; - } - - size_t Device::getNumBodies() const - { - return tracker.getNumBodies(); - // return this->bodySkeletons.size(); - } - - const std::vector &Device::getBodySkeletons() const - { - return tracker.getBodySkeletons(); - // return this->bodySkeletons; - } - - const std::vector &Device::getBodyIDs() const - { - return tracker.getBodyIDs(); - // return this->bodyIDs; - } - const ofVbo &Device::getPointCloudVbo() const { return this->pointCloudVbo; diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 7b286c2..882874d 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -107,19 +107,11 @@ namespace ofxAzureKinect const ofPixels &getColorInDepthPix() const; const ofTexture &getColorInDepthTex() const; - const ofPixels &getBodyIndexPix() const; - const ofTexture &getBodyIndexTex() const; - - size_t getNumBodies() const; - const std::vector &getBodySkeletons() const; - const std::vector &getBodyIDs() const; - const ofVbo &getPointCloudVbo() const; BodyTracker *get_body_tracker() { return &tracker; } public: - ofParameter jointSmoothing{"Joint Smoothing", 0.0f, 0.0f, 1.0f}; ofParameter bRecord{"bRecord", false}; float get_recording_timer_delay(); ofParameter play{"play", false}; From 6300a9318f573451ba05f7bf532857deb47d477f Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Fri, 26 Jun 2020 13:10:41 -0400 Subject: [PATCH 18/48] restructured body tracker now working with streaming too --- src/ofxAzureKinect/Device.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 18e2a88..b55cd72 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -210,6 +210,22 @@ namespace ofxAzureKinect this->bUpdateBodies = bodyTrackingSettings.updateBodies; + if (bUpdateBodies){ + + try + { + this->calibration = this->device.get_calibration(this->config.depth_mode, this->config.color_resolution); + } + catch (const k4a::error &e) + { + ofLogError(__FUNCTION__) << e.what(); + return false; + } + + // Create Body Tracker + tracker = BodyTracker(calibration, trackerConfig); + } + // Add Recording Listener this->eventListeners.push(this->bRecord.newListener([this](bool) { handle_recording(this->bRecord); From 18fa25051e7f3345e4441603651cd9c077507711 Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Fri, 26 Jun 2020 13:21:58 -0400 Subject: [PATCH 19/48] modified example for playback and new BodyTracker class --- example-bodies/src/ofApp.cpp | 291 ++++++++++++----------------------- example-bodies/src/ofApp.h | 13 +- 2 files changed, 103 insertions(+), 201 deletions(-) diff --git a/example-bodies/src/ofApp.cpp b/example-bodies/src/ofApp.cpp index 1ee6a7b..f016716 100644 --- a/example-bodies/src/ofApp.cpp +++ b/example-bodies/src/ofApp.cpp @@ -7,36 +7,37 @@ void ofApp::setup() ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices."; - auto deviceSettings = ofxAzureKinect::DeviceSettings(); - deviceSettings.syncImages = false; - deviceSettings.depthMode = K4A_DEPTH_MODE_NFOV_UNBINNED; - deviceSettings.updateIr = false; - deviceSettings.updateColor = false; - //deviceSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P; - deviceSettings.updateWorld = true; - deviceSettings.updateVbo = false; + // Load Body Tracking Settings auto bodyTrackingSettings = ofxAzureKinect::BodyTrackingSettings(); - //bodyTrackingSettings.processingMode = K4ABT_TRACKER_PROCESSING_MODE_CPU; bodyTrackingSettings.updateBodies = true; - if (this->kinectDevice.open(deviceSettings, bodyTrackingSettings)) + + // Load a recording with Body Tracking Settings + if (!streaming) { - this->kinectDevice.startCameras(); + filename = ofToDataPath("output_2d_movements.mkv"); + if (!kinectDevice.open(filename, bodyTrackingSettings)) + { + exit(); + } } - - // Load shader. - auto shaderSettings = ofShaderSettings(); - shaderSettings.shaderFiles[GL_VERTEX_SHADER] = "shaders/render.vert"; - shaderSettings.shaderFiles[GL_FRAGMENT_SHADER] = "shaders/render.frag"; - shaderSettings.intDefines["BODY_INDEX_MAP_BACKGROUND"] = K4ABT_BODY_INDEX_MAP_BACKGROUND; - shaderSettings.bindDefaults = true; - if (this->shader.setup(shaderSettings)) + else { - ofLogNotice(__FUNCTION__) << "Success loading shader!"; + auto deviceSettings = ofxAzureKinect::DeviceSettings(); + deviceSettings.syncImages = false; + deviceSettings.depthMode = K4A_DEPTH_MODE_NFOV_UNBINNED; + deviceSettings.updateIr = false; + deviceSettings.updateColor = false; + //deviceSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P; + deviceSettings.updateWorld = true; + deviceSettings.updateVbo = false; + + if (!kinectDevice.open(deviceSettings, bodyTrackingSettings)) + { + exit(); + } } - - // Setup vbo. - std::vector verts(1); - this->pointsVbo.setVertexData(verts.data(), verts.size(), GL_STATIC_DRAW); + // Start Playback or Streaming + kinectDevice.startCameras(); } //-------------------------------------------------------------- @@ -48,7 +49,6 @@ void ofApp::exit() //-------------------------------------------------------------- void ofApp::update() { - } //-------------------------------------------------------------- @@ -56,178 +56,77 @@ void ofApp::draw() { ofBackground(0); - if (this->kinectDevice.isStreaming()) - { - this->kinectDevice.getBodyIndexTex().draw(0, 0, 360, 360); - } + ofxAzureKinect::BodyTracker *tracker = kinectDevice.get_body_tracker(); - this->camera.begin(); - { - ofPushMatrix(); - { - ofRotateXDeg(180); - - ofEnableDepthTest(); + tracker->draw_body_map(); - constexpr int kMaxBodies = 6; - int bodyIDs[kMaxBodies]; - int i = 0; - while (i < this->kinectDevice.getNumBodies()) - { - bodyIDs[i] = this->kinectDevice.getBodyIDs()[i]; - ++i; - } - while (i < kMaxBodies) - { - bodyIDs[i] = 0; - ++i; - } + camera.begin(); - this->shader.begin(); - { - this->shader.setUniformTexture("uDepthTex", this->kinectDevice.getDepthTex(), 1); - this->shader.setUniformTexture("uBodyIndexTex", this->kinectDevice.getBodyIndexTex(), 2); - this->shader.setUniformTexture("uWorldTex", this->kinectDevice.getDepthToWorldTex(), 3); - this->shader.setUniform2i("uFrameSize", this->kinectDevice.getDepthTex().getWidth(), this->kinectDevice.getDepthTex().getHeight()); - this->shader.setUniform1iv("uBodyIDs", bodyIDs, kMaxBodies); + ofPushMatrix(); + ofRotateXDeg(180); + tracker->draw_point_clouds(kinectDevice.getDepthTex(), kinectDevice.getDepthToWorldTex()); + tracker->draw_skeletons(); + ofPopMatrix(); - int numPoints = this->kinectDevice.getDepthTex().getWidth() * this->kinectDevice.getDepthTex().getHeight(); - this->pointsVbo.drawInstanced(GL_POINTS, 0, 1, numPoints); - } - this->shader.end(); + camera.end(); - ofDisableDepthTest(); + stringstream ss; + ss << ofToString(ofGetFrameRate(), 2) + " FPS" << std::endl; + ss << "Joint Smoothing: " << kinectDevice.get_body_tracker()->get_joint_smoothing(); + ofDrawBitmapStringHighlight(ss.str(), 10, 20); +} - auto& bodySkeletons = this->kinectDevice.getBodySkeletons(); - for (auto& skeleton : bodySkeletons) +//-------------------------------------------------------------- +void ofApp::keyPressed(int key) +{ + // handle playback hotkeys + if (!streaming) + { + switch (key) + { + case ' ': + { + play = !play; + if (play) { - // Draw joints. - for (int i = 0; i < K4ABT_JOINT_COUNT; ++i) - { - auto joint = skeleton.joints[i]; - ofPushMatrix(); - { - glm::mat4 transform = glm::translate(toGlm(joint.position)) * glm::toMat4(toGlm(joint.orientation)); - ofMultMatrix(transform); - - ofDrawAxis(50.0f); - } - ofPopMatrix(); - } - - // Draw connections. - this->skeletonMesh.setMode(OF_PRIMITIVE_LINES); - auto& vertices = this->skeletonMesh.getVertices(); - vertices.resize(50); - int vdx = 0; - - // Spine. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_NAVEL].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_NAVEL].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_CHEST].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SPINE_CHEST].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HEAD].position); - - // Head. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HEAD].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EAR_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NOSE].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EYE_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_EAR_RIGHT].position); - - // Left Leg. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_FOOT_LEFT].position); - - // Right leg. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_PELVIS].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_HIP_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_KNEE_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ANKLE_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_FOOT_RIGHT].position); - - // Left arm. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_LEFT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_LEFT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_WRIST_LEFT].position); - - // Right arm. - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_NECK].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_CLAVICLE_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_SHOULDER_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_RIGHT].position); - - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_ELBOW_RIGHT].position); - vertices[vdx++] = toGlm(skeleton.joints[K4ABT_JOINT_WRIST_RIGHT].position); - - this->skeletonMesh.draw(); + kinectDevice.play = true; } + else + { + kinectDevice.pause = false; + } + break; + } + case '+': + play_head = ofClamp(play_head + .01, 0, 1); + kinectDevice.seek = play_head; + break; + case '-': + play_head = ofClamp(play_head - .01, 0, 1); + kinectDevice.seek = play_head; + break; } - ofPopMatrix(); - } - this->camera.end(); - - std::ostringstream oss; - oss << ofToString(ofGetFrameRate(), 2) + " FPS" << std::endl; - oss << "Joint Smoothing: " << this->kinectDevice.jointSmoothing; - ofDrawBitmapStringHighlight(oss.str(), 10, 20); -} - -//-------------------------------------------------------------- -void ofApp::keyPressed(int key){ + switch (key) + { + case 'f': + case 'F': + ofToggleFullscreen(); + break; + default: + break; + } + } } //-------------------------------------------------------------- -void ofApp::keyReleased(int key){ - +void ofApp::keyReleased(int key) +{ } //-------------------------------------------------------------- -void ofApp::mouseMoved(int x, int y ){ - +void ofApp::mouseMoved(int x, int y) +{ } //-------------------------------------------------------------- @@ -235,41 +134,41 @@ void ofApp::mouseDragged(int x, int y, int button) { if (button == 1) { - this->kinectDevice.jointSmoothing = ofMap(x, 0, ofGetWidth(), 0.0f, 1.0f, true); + this->kinectDevice.get_body_tracker()->set_joint_smoothing(ofMap(x, 0, ofGetWidth(), 0.0f, 1.0f, true)); } } //-------------------------------------------------------------- -void ofApp::mousePressed(int x, int y, int button){ - +void ofApp::mousePressed(int x, int y, int button) +{ } //-------------------------------------------------------------- -void ofApp::mouseReleased(int x, int y, int button){ - +void ofApp::mouseReleased(int x, int y, int button) +{ } //-------------------------------------------------------------- -void ofApp::mouseEntered(int x, int y){ - +void ofApp::mouseEntered(int x, int y) +{ } //-------------------------------------------------------------- -void ofApp::mouseExited(int x, int y){ - +void ofApp::mouseExited(int x, int y) +{ } //-------------------------------------------------------------- -void ofApp::windowResized(int w, int h){ - +void ofApp::windowResized(int w, int h) +{ } //-------------------------------------------------------------- -void ofApp::gotMessage(ofMessage msg){ - +void ofApp::gotMessage(ofMessage msg) +{ } //-------------------------------------------------------------- -void ofApp::dragEvent(ofDragInfo dragInfo){ - +void ofApp::dragEvent(ofDragInfo dragInfo) +{ } diff --git a/example-bodies/src/ofApp.h b/example-bodies/src/ofApp.h index 4d5e0c2..9cc8abe 100644 --- a/example-bodies/src/ofApp.h +++ b/example-bodies/src/ofApp.h @@ -4,8 +4,8 @@ #include "ofxAzureKinect.h" -class ofApp - : public ofBaseApp +class ofApp + : public ofBaseApp { public: void setup(); @@ -31,8 +31,11 @@ class ofApp ofEasyCam camera; - ofVbo pointsVbo; - ofShader shader; + bool streaming = true; + + // Playback Params + string filename; + bool play = false; + float play_head = 0; - ofVboMesh skeletonMesh; }; From 789cdf2cc2eabecab9ea4b15e16f7e928631694a Mon Sep 17 00:00:00 2001 From: Paul Ferragut Date: Mon, 17 Aug 2020 15:06:58 +0200 Subject: [PATCH 20/48] Update addon_config.mk fix missing libraries when compiling --- addon_config.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/addon_config.mk b/addon_config.mk index 73f6073..d080a15 100644 --- a/addon_config.mk +++ b/addon_config.mk @@ -64,6 +64,7 @@ vs: ADDON_INCLUDES += $(AZUREKINECT_SDK)\sdk\include ADDON_INCLUDES += $(AZUREKINECT_BODY_SDK)\sdk\include ADDON_LIBS += $(AZUREKINECT_SDK)\sdk\windows-desktop\amd64\release\lib\k4a.lib + ADDON_LIBS += $(AZUREKINECT_SDK)\sdk\windows-desktop\amd64\release\lib\k4arecord.lib ADDON_LIBS += $(AZUREKINECT_BODY_SDK)\sdk\windows-desktop\amd64\release\lib\k4abt.lib linux64: From 63a587c3acebaffd5d24777c4ee950ce25943677 Mon Sep 17 00:00:00 2001 From: Paul Ferragut Date: Tue, 18 Aug 2020 16:19:37 +0200 Subject: [PATCH 21/48] "Add checks to ensure pixels are allocated" from prisonerjohn --- .gitignore | 76 +++++++++++++++++++++++++++++++++++ src/ofxAzureKinect/Device.cpp | 67 +++++++++++++++++------------- 2 files changed, 116 insertions(+), 27 deletions(-) diff --git a/.gitignore b/.gitignore index 76d5505..a236de5 100644 --- a/.gitignore +++ b/.gitignore @@ -106,3 +106,79 @@ Desktop.ini *.log *.sql *.sqlite +*.sln +*.vcxproj +example-bodies/addons.make +example-bodies/bin/data/shaders/render.frag +example-bodies/bin/data/shaders/render.vert +example-bodies/example-bodies.sln +example-bodies/example-bodies.vcxproj +example-bodies/example-bodies.vcxproj.filters +example-bodies/example-bodies.vcxproj.user +example-bodies/icon.rc +example-bodies/src/main.cpp +example-bodies/src/ofApp.cpp +example-bodies/src/ofApp.h +example-multi/addons.make +example-multi/example-multi.sln +example-multi/example-multi.vcxproj +example-multi/example-multi.vcxproj.filters +example-multi/example-multi.vcxproj.user +example-multi/icon.rc +example-multi/src/main.cpp +example-multi/src/ofApp.cpp +example-multi/src/ofApp.h +example-playback/addons.make +example-playback/bin/data/output.mkv +example-playback/example-playback.vcxproj.filters +example-playback/example-playback.vcxproj.user +example-playback/icon.rc +example-playback/src/main.cpp +example-playback/src/ofApp.cpp +example-playback/src/ofApp.h +example-pointcloud/addons.make +example-pointcloud/example-pointcloud.sln +example-pointcloud/example-pointcloud.vcxproj +example-pointcloud/example-pointcloud.vcxproj.filters +example-pointcloud/example-pointcloud.vcxproj.user +example-pointcloud/icon.rc +example-pointcloud/src/main.cpp +example-pointcloud/src/ofApp.cpp +example-pointcloud/src/ofApp.h +example-record/addons.make +example-record/bin/data/output_20200817_15-12-17.mkv +example-record/bin/data/output_20200818_16-13-41.mkv +example-record/example-record.vcxproj.filters +example-record/example-record.vcxproj.user +example-record/icon.rc +example-record/src/main.cpp +example-record/src/ofApp.cpp +example-record/src/ofApp.h +example-shader/addons.make +example-shader/bin/data/shaders/render.frag +example-shader/bin/data/shaders/render.geom +example-shader/bin/data/shaders/render.vert +example-shader/example-shader.vcxproj +example-shader/example-shader.vcxproj.filters +example-shader/icon.rc +example-shader/src/main.cpp +example-shader/src/ofApp.cpp +example-shader/src/ofApp.h +example-streams/addons.make +example-streams/example-streams.vcxproj +example-streams/example-streams.vcxproj.filters +example-streams/icon.rc +example-streams/src/main.cpp +example-streams/src/ofApp.cpp +example-streams/src/ofApp.h +libs/turbojpeg/include/turbojpeg.h +LICENSE +README.md +src/ofxAzureKinect.h +src/ofxAzureKinect/BodyTracker.cpp +src/ofxAzureKinect/BodyTracker.h +src/ofxAzureKinect/Playback.cpp +src/ofxAzureKinect/Playback.h +src/ofxAzureKinect/Record.cpp +src/ofxAzureKinect/Record.h +src/ofxAzureKinect/Types.h diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index b55cd72..9181dca 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -625,17 +625,20 @@ namespace ofxAzureKinect void Device::updateTextures() { - // Update the depth texture. - if (!this->depthTex.isAllocated()) + if (this->depthPix.isAllocated()) { - this->depthTex.allocate(this->depthPix); - this->depthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); - } + // Update the depth texture. + if (!this->depthTex.isAllocated()) + { + this->depthTex.allocate(this->depthPix); + this->depthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); + } - this->depthTex.loadData(this->depthPix); - ofLogVerbose(__FUNCTION__) << "Update Depth16 " << this->depthTex.getWidth() << "x" << this->depthTex.getHeight() << "."; + this->depthTex.loadData(this->depthPix); + ofLogVerbose(__FUNCTION__) << "Update Depth16 " << this->depthTex.getWidth() << "x" << this->depthTex.getHeight() << "."; + } - if (this->bUpdateColor) + if (this->bUpdateColor && this->colorPix.isAllocated()) { // Update the color texture. if (!this->colorTex.isAllocated()) @@ -655,11 +658,10 @@ namespace ofxAzureKinect } this->colorTex.loadData(this->colorPix); - ofLogVerbose(__FUNCTION__) << "Update Color " << this->colorTex.getWidth() << "x" << this->colorTex.getHeight() << "."; } - if (this->bUpdateIr) + if (this->bUpdateIr && this->irPix.isAllocated()) { // Update the IR16 image. if (!this->irTex.isAllocated()) @@ -670,13 +672,18 @@ namespace ofxAzureKinect } this->irTex.loadData(this->irPix); - ofLogVerbose(__FUNCTION__) << "Update Ir16 " << this->irTex.getWidth() << "x" << this->irTex.getHeight() << "."; } - if (this->bUpdateBodies) + if (this->bUpdateBodies && this->bodyIndexPix.isAllocated()) { - tracker.update_texture(); + if (!this->bodyIndexTex.isAllocated()) + { + this->bodyIndexTex.allocate(this->bodyIndexPix); + this->bodyIndexTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); + } + + this->bodyIndexTex.loadData(this->bodyIndexPix); } if (this->bUpdateVbo) @@ -687,27 +694,33 @@ namespace ofxAzureKinect if (this->bUpdateColor && this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) { - if (this->depthInColorPix.isAllocated() && !this->depthInColorTex.isAllocated()) + if (this->depthInColorPix.isAllocated()) { - this->depthInColorTex.allocate(this->depthInColorPix); - this->depthInColorTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); - } + if (!this->depthInColorTex.isAllocated()) + { + this->depthInColorTex.allocate(this->depthInColorPix); + this->depthInColorTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); + } - this->depthInColorTex.loadData(this->depthInColorPix); + this->depthInColorTex.loadData(this->depthInColorPix); + } - if (this->colorInDepthPix.isAllocated() && !this->colorInDepthTex.isAllocated()) + if (this->colorInDepthPix.isAllocated()) { - this->colorInDepthTex.allocate(this->colorInDepthPix); - this->colorInDepthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); - this->colorInDepthTex.bind(); + if (!this->colorInDepthTex.isAllocated()) { - glTexParameteri(this->colorInDepthTex.texData.textureTarget, GL_TEXTURE_SWIZZLE_R, GL_BLUE); - glTexParameteri(this->colorInDepthTex.texData.textureTarget, GL_TEXTURE_SWIZZLE_B, GL_RED); + this->colorInDepthTex.allocate(this->colorInDepthPix); + this->colorInDepthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); + this->colorInDepthTex.bind(); + { + glTexParameteri(this->colorInDepthTex.texData.textureTarget, GL_TEXTURE_SWIZZLE_R, GL_BLUE); + glTexParameteri(this->colorInDepthTex.texData.textureTarget, GL_TEXTURE_SWIZZLE_B, GL_RED); + } + this->colorInDepthTex.unbind(); } - this->colorInDepthTex.unbind(); - } - this->colorInDepthTex.loadData(this->colorInDepthPix); + this->colorInDepthTex.loadData(this->colorInDepthPix); + } } // Update frame number. From cc8f618b9b26c9ec1e4e3505009f8579e0562505 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 14:19:40 +0900 Subject: [PATCH 22/48] Update multi example. Auto master/subordinate device detection, with subordinate delay. --- example-multi/src/ofApp.cpp | 65 +++++++++++++++++++++++++------------ 1 file changed, 45 insertions(+), 20 deletions(-) diff --git a/example-multi/src/ofApp.cpp b/example-multi/src/ofApp.cpp index c30e4e2..37629bc 100644 --- a/example-multi/src/ofApp.cpp +++ b/example-multi/src/ofApp.cpp @@ -9,10 +9,10 @@ void ofApp::setup() ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices."; // The following will start all connected devices as standalone (no sync). - this->setupStandalone(); + //this->setupStandalone(); // The following will assign sync to devices based on serial number. - //this->setupMasterSubordinate(); + this->setupMasterSubordinate(); // Add FPS counter for each device. this->fpsCounters.resize(this->kinectDevices.size()); @@ -43,38 +43,56 @@ void ofApp::setupStandalone() //-------------------------------------------------------------- void ofApp::setupMasterSubordinate() { - // Make sure to replace the following serials by the ones on your devices. - const std::string serialMaster = "000224694712"; - const std::string serialSubordinate = "000569192412"; - auto kinectSettings = ofxAzureKinect::DeviceSettings(); kinectSettings.colorResolution = K4A_COLOR_RESOLUTION_720P; kinectSettings.syncImages = true; kinectSettings.updateWorld = false; - // Open Master device. + // Open all device and check sync status. + int numConnected = ofxAzureKinect::Device::getInstalledCount(); + int syncOutDeviceIndex = -1; + int syncInOutDeviceIndex = -1; + + int connectedDeviceIndex = 0; + for (int i = 0; i < numConnected; ++i) { auto device = std::make_shared(); - if (device->load(serialMaster)) + if (device->open(i)) { - kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_MASTER; - device->startCameras(kinectSettings); - this->kinectDevices.push_back(device); + if (!device->isSyncInConnected() && device->isSyncOutConnected()) { + syncOutDeviceIndex = connectedDeviceIndex; + } + else if (device->isSyncInConnected() && device->isSyncOutConnected()) { + syncInOutDeviceIndex = connectedDeviceIndex; + } + connectedDeviceIndex++; } } - // Open Subordinate device. - { - auto device = std::make_shared(); - if (device->load(serialSubordinate)) - { - kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_SUBORDINATE; - //kinectSettings.subordinateDelayUsec = 100; - device->startCameras(kinectSettings); + int masterDeviceIndex = syncOutDeviceIndex >= 0 ? syncOutDeviceIndex : syncInOutDeviceIndex; - this->kinectDevices.push_back(device); + if (masterDeviceIndex < 0) { + ofLogWarning() << "No master device is detected. Start streams as standalone mode."; + for (auto& device : this->kinectDevices) { + device->startCameras(kinectSettings); + } + } else { + // Open Subordinate devices first + for (int i = 0; i < this->kinectDevices.size(); ++i) { + if (i != masterDeviceIndex) { + cerr << "sub device : " << this->kinectDevices[i]->getSerialNumber() << endl; + kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_SUBORDINATE; + kinectSettings.subordinateDelayUsec += 160; + this->kinectDevices[i]->startCameras(kinectSettings); + } } + + // Open Master device + cerr << "master device : " << this->kinectDevices[masterDeviceIndex]->getSerialNumber() << endl; + kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_MASTER; + kinectSettings.subordinateDelayUsec = 0; + this->kinectDevices[masterDeviceIndex]->startCameras(kinectSettings); } } @@ -118,6 +136,13 @@ void ofApp::draw() ofDrawBitmapStringHighlight(ofToString(this->fpsCounters[i].getFps(), 2) + " FPS", x + 10, 350, device->isFrameNew() ? ofColor::red : ofColor::black); + if (device->isSyncInConnected()) { + ofDrawBitmapStringHighlight("SyncIn", x + 100, 350, ofColor::blue); + } + if (device->isSyncOutConnected()) { + ofDrawBitmapStringHighlight("SyncOut", x + 200, 350, ofColor::green); + } + x += 640; } } From 0b9d2d08518cb877317063ea74812d05fa13846b Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 17:15:02 +0900 Subject: [PATCH 23/48] change name of Record Playback methods from snake to camel, to fit to original ofxAzureKinect style --- example-record/src/ofApp.cpp | 4 ++-- src/ofxAzureKinect/Device.cpp | 24 +++++++++++++----------- src/ofxAzureKinect/Device.h | 10 +++++++--- src/ofxAzureKinect/Playback.cpp | 14 +++++++------- src/ofxAzureKinect/Playback.h | 18 +++++++++--------- src/ofxAzureKinect/Record.cpp | 2 +- src/ofxAzureKinect/Record.h | 4 ++-- 7 files changed, 41 insertions(+), 35 deletions(-) diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp index d6802a4..0703e34 100644 --- a/example-record/src/ofApp.cpp +++ b/example-record/src/ofApp.cpp @@ -70,7 +70,7 @@ void ofApp::draw_recording_animation() ofColor col; string msg; - float recording_countdown = sensor.get_recording_timer_delay(); + float recording_countdown = sensor.getRecordingTimerDelay(); if (recording_countdown > 0) { msg = "Starting Recording In: " + ofToString(recording_countdown, 2); @@ -100,7 +100,7 @@ void ofApp::keyPressed(int key) sensor.bRecord = !sensor.bRecord; if (sensor.bRecord) { - recording_delay = sensor.get_recording_timer_delay(); + recording_delay = sensor.getRecordingTimerDelay(); recording_start = ofGetElapsedTimef() + recording_delay; } else diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 8d93c05..a7e775a 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -64,9 +64,9 @@ namespace ofxAzureKinect bPlayback = true; playback = new Playback(); - if (playback->load_file(filename)) + if (playback->load(filename)) { - k4a_record_configuration_t playback_config = playback->get_device_settings(); + k4a_record_configuration_t playback_config = playback->getDeviceSettings(); this->config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL; this->config.depth_mode = playback_config.depth_mode; @@ -79,7 +79,7 @@ namespace ofxAzureKinect this->config.depth_delay_off_color_usec = playback_config.depth_delay_off_color_usec; this->config.subordinate_delay_off_master_usec = playback_config.subordinate_delay_off_master_usec; - this->serialNumber = playback->get_serial_number(); + this->serialNumber = playback->getSerialNumber(); this->bUpdateColor = playback_config.color_track_enabled; this->bUpdateIr = playback_config.ir_track_enabled; this->bUpdateWorld = playback_config.depth_track_enabled; @@ -259,7 +259,7 @@ namespace ofxAzureKinect // Get calibration. if (bPlayback) { - auto calibration_handle = playback->get_calibration(); + auto calibration_handle = playback->getCalibration(); this->calibration.depth_camera_calibration = calibration_handle.depth_camera_calibration; this->calibration.color_camera_calibration = calibration_handle.color_camera_calibration; this->calibration.depth_mode = calibration_handle.depth_mode; @@ -444,12 +444,12 @@ namespace ofxAzureKinect // Get a capture. if (bPlayback) { - if (playback->is_playing()) + if (playback->isPlaying()) { - capture = k4a::capture(playback->get_next_capture()); + capture = k4a::capture(playback->getNextCapture()); if (bEnableIMU) { - imu_sample = playback->get_next_imu_sample(); + imu_sample = playback->getNextImuSample(); // printf(" | Accelerometer temperature:%.2f x:%.4f y:%.4f z: %.4f\n", // imu_sample.temperature, // imu_sample.acc_sample.xyz.x, @@ -457,10 +457,10 @@ namespace ofxAzureKinect // imu_sample.acc_sample.xyz.z); } } - else if (playback->is_paused()) + else if (playback->isPaused()) { playback->seek(); - capture = k4a::capture(playback->get_next_capture()); + capture = k4a::capture(playback->getNextCapture()); } else { @@ -536,6 +536,8 @@ namespace ofxAzureKinect this->colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); } + colorImg.get_device_timestamp(); + ofLogVerbose(__FUNCTION__) << "Capture Color " << colorDims.x << "x" << colorDims.y << " stride: " << colorImg.get_stride_bytes() << "."; } else @@ -1041,10 +1043,10 @@ namespace ofxAzureKinect } } - float Device::get_recording_timer_delay() + float Device::getRecordingTimerDelay() { if (recording != nullptr) - return recording->get_timer_delay(); + return recording->getTimerDelay(); else return -1; } diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 559f801..1fa5ecc 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -106,11 +106,11 @@ namespace ofxAzureKinect const ofVbo &getPointCloudVbo() const; - BodyTracker *get_body_tracker() { return &tracker; } + BodyTracker *getBodyTracker() { return &tracker; } public: ofParameter bRecord{"bRecord", false}; - float get_recording_timer_delay(); + float getRecordingTimerDelay(); ofParameter play{"play", false}; ofParameter pause{"pause", false}; ofParameter stop{"stop", false}; @@ -134,7 +134,7 @@ namespace ofxAzureKinect bool updateDepthInColorFrame(const k4a::image &depthImg, const k4a::image &colorImg); bool updateColorInDepthFrame(const k4a::image &depthImg, const k4a::image &colorImg); - private: + protected: int index; bool bOpen; bool bStreaming; @@ -173,6 +173,8 @@ namespace ofxAzureKinect ofPixels colorPix; ofTexture colorTex; + std::chrono::microseconds colorPixDeviceTime; + std::chrono::microseconds colorTexDeviceTime; ofShortPixels irPix; ofTexture irTex; @@ -180,6 +182,8 @@ namespace ofxAzureKinect k4a::image depthToWorldImg; ofFloatPixels depthToWorldPix; ofTexture depthToWorldTex; + std::chrono::microseconds depthPixDeviceTime; + std::chrono::microseconds depthTexDeviceTime; k4a::image colorToWorldImg; ofFloatPixels colorToWorldPix; diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp index 65f28c0..ca12149 100644 --- a/src/ofxAzureKinect/Playback.cpp +++ b/src/ofxAzureKinect/Playback.cpp @@ -3,7 +3,7 @@ namespace ofxAzureKinect { - bool Playback::load_file(string _filename) + bool Playback::load(string _filename) { // Convert filename from string to char* char *temp = new char[_filename.size() + 1]; @@ -45,7 +45,7 @@ namespace ofxAzureKinect cout << "stop" << endl; } - k4a_capture_t Playback::get_next_capture() + k4a_capture_t Playback::getNextCapture() { k4a_stream_result_t result = k4a_playback_get_next_capture(playback, &capture); if (result == K4A_STREAM_RESULT_SUCCEEDED) @@ -57,7 +57,7 @@ namespace ofxAzureKinect // End of file reached // ofLog() << "End of file reached." << endl; seek(0); - return get_next_capture(); + return getNextCapture(); } else if (result == K4A_STREAM_RESULT_FAILED) { @@ -67,7 +67,7 @@ namespace ofxAzureKinect return nullptr; } - k4a_imu_sample_t Playback::get_next_imu_sample() + k4a_imu_sample_t Playback::getNextImuSample() { // k4a_imu_sample_t imu_sample; // checking for the IMU tracking isn't working ... may have to add a special tag when recording? @@ -108,13 +108,13 @@ namespace ofxAzureKinect k4a_playback_close(playback); } - k4a_record_configuration_t Playback::get_device_settings() + k4a_record_configuration_t Playback::getDeviceSettings() { k4a_playback_get_record_configuration(playback, &config); return config; } - k4a_calibration_t Playback::get_calibration() + k4a_calibration_t Playback::getCalibration() { k4a_calibration_t calibration; if (K4A_RESULT_SUCCEEDED != k4a_playback_get_calibration(playback, &calibration)) @@ -124,7 +124,7 @@ namespace ofxAzureKinect return calibration; } - string Playback::get_serial_number() + string Playback::getSerialNumber() { return get_tag("K4A_DEVICE_SERIAL_NUMBER"); } diff --git a/src/ofxAzureKinect/Playback.h b/src/ofxAzureKinect/Playback.h index 18bf85f..c5d2231 100644 --- a/src/ofxAzureKinect/Playback.h +++ b/src/ofxAzureKinect/Playback.h @@ -17,13 +17,13 @@ namespace ofxAzureKinect Playback(){}; ~Playback(){}; - bool load_file(string _filename); - k4a_record_configuration_t get_device_settings(); - string get_serial_number(); - k4a_calibration_t get_calibration(); + bool load(string _filename); + k4a_record_configuration_t getDeviceSettings(); + string getSerialNumber(); + k4a_calibration_t getCalibration(); - k4a_capture_t get_next_capture(); - k4a_imu_sample_t get_next_imu_sample(); + k4a_capture_t getNextCapture(); + k4a_imu_sample_t getNextImuSample(); void seek(); void seek(float t); @@ -33,9 +33,9 @@ namespace ofxAzureKinect void stop(); void close(); - bool is_playing() { return status == PLAYING; } - bool is_paused() { return status == PAUSED; } - bool is_stopped() { return status == STOPPED; } + bool isPlaying() { return status == PLAYING; } + bool isPaused() { return status == PAUSED; } + bool isStopped() { return status == STOPPED; } private: k4a_playback_t playback; diff --git a/src/ofxAzureKinect/Record.cpp b/src/ofxAzureKinect/Record.cpp index cc270d2..13abe16 100644 --- a/src/ofxAzureKinect/Record.cpp +++ b/src/ofxAzureKinect/Record.cpp @@ -153,7 +153,7 @@ namespace ofxAzureKinect } while (result != K4A_WAIT_RESULT_FAILED); } - float Record::get_timer_delay() + float Record::getTimerDelay() { return MAX(delay - (ofGetElapsedTimef() - delay_start), 0); } diff --git a/src/ofxAzureKinect/Record.h b/src/ofxAzureKinect/Record.h index 16e708f..9ca1376 100644 --- a/src/ofxAzureKinect/Record.h +++ b/src/ofxAzureKinect/Record.h @@ -20,8 +20,8 @@ namespace ofxAzureKinect void record(k4a_capture_t *capture); // Set a recording delay (in seconds) - void set_delay(float _delay) { this->delay = _delay; } - float get_timer_delay(); + void setDelay(float _delay) { this->delay = _delay; } + float getTimerDelay(); private: From f4c95162ff71466c3adb47620f14ab006f13bd29 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 17:26:13 +0900 Subject: [PATCH 24/48] add device timestamp getter --- example-multi/src/ofApp.cpp | 1 + src/ofxAzureKinect/Device.cpp | 5 ++++- src/ofxAzureKinect/Device.h | 6 ++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/example-multi/src/ofApp.cpp b/example-multi/src/ofApp.cpp index 37629bc..fe65d52 100644 --- a/example-multi/src/ofApp.cpp +++ b/example-multi/src/ofApp.cpp @@ -142,6 +142,7 @@ void ofApp::draw() if (device->isSyncOutConnected()) { ofDrawBitmapStringHighlight("SyncOut", x + 200, 350, ofColor::green); } + ofDrawBitmapStringHighlight("DeviceTime : " + ofToString(device->getColorTexDeviceTime().count()), x + 10, 40); x += 640; } diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index a7e775a..a4d374c 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -497,6 +497,7 @@ namespace ofxAzureKinect const auto depthData = reinterpret_cast(depthImg.get_buffer()); this->depthPix.setFromPixels(depthData, depthDims.x, depthDims.y, 1); + this->depthPixDeviceTime = depthImg.get_device_timestamp(); ofLogVerbose(__FUNCTION__) << "Capture Depth16 " << depthDims.x << "x" << depthDims.y << " stride: " << depthImg.get_stride_bytes() << "."; } @@ -536,7 +537,7 @@ namespace ofxAzureKinect this->colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); } - colorImg.get_device_timestamp(); + this->colorPixDeviceTime = colorImg.get_device_timestamp(); ofLogVerbose(__FUNCTION__) << "Capture Color " << colorDims.x << "x" << colorDims.y << " stride: " << colorImg.get_stride_bytes() << "."; } @@ -625,6 +626,7 @@ namespace ofxAzureKinect } this->depthTex.loadData(this->depthPix); + this->depthTexDeviceTime = this->depthPixDeviceTime; ofLogVerbose(__FUNCTION__) << "Update Depth16 " << this->depthTex.getWidth() << "x" << this->depthTex.getHeight() << "."; } @@ -648,6 +650,7 @@ namespace ofxAzureKinect } this->colorTex.loadData(this->colorPix); + this->colorTexDeviceTime = this->colorPixDeviceTime; ofLogVerbose(__FUNCTION__) << "Update Color " << this->colorTex.getWidth() << "x" << this->colorTex.getHeight() << "."; } diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 1fa5ecc..66d0cf2 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -85,9 +85,15 @@ namespace ofxAzureKinect const ofShortPixels &getDepthPix() const; const ofTexture &getDepthTex() const; + const std::chrono::microseconds& getDepthTexDeviceTime() const { + return this->depthTexDeviceTime; + } const ofPixels &getColorPix() const; const ofTexture &getColorTex() const; + const std::chrono::microseconds& getColorTexDeviceTime() const { + return this->colorTexDeviceTime; + } const ofShortPixels &getIrPix() const; const ofTexture &getIrTex() const; From 3d2165d22fef5d2514b8ef3fd3b1cbea72365111 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 18:49:31 +0900 Subject: [PATCH 25/48] add color camera control method --- example-record/src/ofApp.cpp | 19 +++++++++++++++++-- src/ofxAzureKinect/Device.cpp | 28 ++++++++++++++++++++++++++++ src/ofxAzureKinect/Device.h | 6 ++++++ 3 files changed, 51 insertions(+), 2 deletions(-) diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp index 0703e34..126177c 100644 --- a/example-record/src/ofApp.cpp +++ b/example-record/src/ofApp.cpp @@ -16,6 +16,7 @@ void ofApp::setup() if (this->sensor.open()) { this->sensor.startCameras(settings); + this->sensor.setExposureTimeAbsolute(10000); } } @@ -32,7 +33,7 @@ void ofApp::update() // If we are recording, update the total recording time if (sensor.bRecord && ofGetElapsedTimef() > recording_start) recording_duration = ofGetElapsedTimef() - recording_start; - + } //-------------------------------------------------------------- @@ -115,7 +116,21 @@ void ofApp::keyPressed(int key) case 'F': ofToggleFullscreen(); break; - default: + case OF_KEY_UP: + { + auto exposure = this->sensor.getExposureTimeAbsolute(); + cerr << "exposure : " << exposure << endl; + this->sensor.setExposureTimeAbsolute(exposure + 1); + break; + } + case OF_KEY_DOWN: + { + auto exposure = this->sensor.getExposureTimeAbsolute(); + cerr << "exposure : " << exposure << endl; + this->sensor.setExposureTimeAbsolute(exposure / 3); + break; + } + default: break; } } diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index a4d374c..d0a31e7 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -1032,6 +1032,34 @@ namespace ofxAzureKinect return this->pointCloudVbo; } + int32_t Device::getColorCameraControlValue(k4a_color_control_command_t command) const + { + k4a_color_control_mode_t mode; + int32_t ret; + this->device.get_color_control(command, &mode, &ret); + if (mode == K4A_COLOR_CONTROL_MODE_AUTO) { + return INT_MIN; + } + else { + return ret; + } + } + + void Device::setColorCameraControlValue(k4a_color_control_command_t command, int32_t value) + { + this->device.set_color_control(command, K4A_COLOR_CONTROL_MODE_MANUAL, value); + } + + int32_t Device::getExposureTimeAbsolute() const + { + return getColorCameraControlValue(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE); + } + + void Device::setExposureTimeAbsolute(int32_t exposure_usec) + { + setColorCameraControlValue(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, exposure_usec); + } + void Device::handle_recording(bool val) { if (val) diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 66d0cf2..39f41e2 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -114,6 +114,12 @@ namespace ofxAzureKinect BodyTracker *getBodyTracker() { return &tracker; } + int32_t getColorCameraControlValue(k4a_color_control_command_t command) const; + void setColorCameraControlValue(k4a_color_control_command_t command, int32_t value); + + int32_t getExposureTimeAbsolute() const; + void setExposureTimeAbsolute(int32_t exposure_usec); + public: ofParameter bRecord{"bRecord", false}; float getRecordingTimerDelay(); From 6cfa2f64dad719978124aebb89798a21824a9095 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 21:58:34 +0900 Subject: [PATCH 26/48] add MultiDeviceSyncCapture class for getting exact sync frames --- example-multi/src/main.cpp | 2 +- example-multi/src/ofApp.cpp | 10 +- example-multi/src/ofApp.h | 1 + example-record/src/main.cpp | 2 +- example-record/src/ofApp.cpp | 18 ++- example-record/src/ofApp.h | 1 + src/ofxAzureKinect/Device.cpp | 233 ++++++++++++++++++++++++++++++++-- src/ofxAzureKinect/Device.h | 42 +++++- src/ofxAzureKinect/Record.cpp | 7 +- src/ofxAzureKinect/Record.h | 5 +- 10 files changed, 295 insertions(+), 26 deletions(-) diff --git a/example-multi/src/main.cpp b/example-multi/src/main.cpp index aaae62e..249044c 100644 --- a/example-multi/src/main.cpp +++ b/example-multi/src/main.cpp @@ -4,7 +4,7 @@ int main() { ofGLFWWindowSettings settings; settings.setGLVersion(3, 2); - settings.setSize(640 * 2, 360 + 320); + settings.setSize(640 * 3, 360 + 320); ofCreateWindow(settings); ofRunApp(new ofApp()); diff --git a/example-multi/src/ofApp.cpp b/example-multi/src/ofApp.cpp index fe65d52..d4859ed 100644 --- a/example-multi/src/ofApp.cpp +++ b/example-multi/src/ofApp.cpp @@ -44,7 +44,7 @@ void ofApp::setupStandalone() void ofApp::setupMasterSubordinate() { auto kinectSettings = ofxAzureKinect::DeviceSettings(); - kinectSettings.colorResolution = K4A_COLOR_RESOLUTION_720P; + kinectSettings.colorResolution = K4A_COLOR_RESOLUTION_1080P; kinectSettings.syncImages = true; kinectSettings.updateWorld = false; @@ -78,12 +78,17 @@ void ofApp::setupMasterSubordinate() device->startCameras(kinectSettings); } } else { + sync.setMasterDevice(kinectDevices[masterDeviceIndex].get()); + // Open Subordinate devices first for (int i = 0; i < this->kinectDevices.size(); ++i) { + this->kinectDevices[i]->setExposureTimeAbsolute(8000); if (i != masterDeviceIndex) { cerr << "sub device : " << this->kinectDevices[i]->getSerialNumber() << endl; kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_SUBORDINATE; kinectSettings.subordinateDelayUsec += 160; + sync.addSubordinateDevice(kinectDevices[i].get()); + this->kinectDevices[i]->startCameras(kinectSettings); } } @@ -93,12 +98,15 @@ void ofApp::setupMasterSubordinate() kinectSettings.wiredSyncMode = K4A_WIRED_SYNC_MODE_MASTER; kinectSettings.subordinateDelayUsec = 0; this->kinectDevices[masterDeviceIndex]->startCameras(kinectSettings); + + sync.start(); } } //-------------------------------------------------------------- void ofApp::exit() { + sync.stop(); for (auto device : this->kinectDevices) { device->close(); diff --git a/example-multi/src/ofApp.h b/example-multi/src/ofApp.h index 0d001e1..1dbcd4c 100644 --- a/example-multi/src/ofApp.h +++ b/example-multi/src/ofApp.h @@ -31,5 +31,6 @@ class ofApp private: std::vector> kinectDevices; + ofxAzureKinect::MultiDeviceSyncCapture sync; std::vector fpsCounters; }; diff --git a/example-record/src/main.cpp b/example-record/src/main.cpp index e57370b..c00ea15 100644 --- a/example-record/src/main.cpp +++ b/example-record/src/main.cpp @@ -3,7 +3,7 @@ //======================================================================== int main( ){ - ofSetupOpenGL(1024,768,OF_WINDOW); // <-------- setup the GL context + ofSetupOpenGL(2048,1536,OF_WINDOW); // <-------- setup the GL context // this kicks off the running of my app // can be OF_WINDOW or OF_FULLSCREEN diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp index 126177c..acae0d9 100644 --- a/example-record/src/ofApp.cpp +++ b/example-record/src/ofApp.cpp @@ -3,16 +3,19 @@ //-------------------------------------------------------------- void ofApp::setup() { + ofSetVerticalSync(false); ofLogNotice(__FUNCTION__) << "Found " << ofxAzureKinect::Device::getInstalledCount() << " installed devices."; auto settings = ofxAzureKinect::DeviceSettings(); - settings.colorResolution = K4A_COLOR_RESOLUTION_1080P; + settings.colorResolution = K4A_COLOR_RESOLUTION_1536P; settings.depthMode = K4A_DEPTH_MODE_NFOV_UNBINNED; settings.colorFormat = K4A_IMAGE_FORMAT_COLOR_MJPG; // BRGA32 not supported for recording settings.cameraFps = K4A_FRAMES_PER_SECOND_30; settings.syncImages = true; settings.updateWorld = false; settings.enableIMU = true; + settings.updateVbo = false; + if (this->sensor.open()) { this->sensor.startCameras(settings); @@ -34,6 +37,10 @@ void ofApp::update() if (sensor.bRecord && ofGetElapsedTimef() > recording_start) recording_duration = ofGetElapsedTimef() - recording_start; + if (sensor.isFrameNew()) { + fpsCounter.newFrame(); + } + } //-------------------------------------------------------------- @@ -44,9 +51,9 @@ void ofApp::draw() // visualize kinect streams if (this->sensor.isStreaming()) { - this->sensor.getColorTex().draw(0, 0, 1280, 720); - this->sensor.getDepthTex().draw(1280, 0, 360, 360); - this->sensor.getIrTex().draw(1280, 360, 360, 360); + this->sensor.getColorTex().draw(0, 0, 2048, 1536); + this->sensor.getDepthTex().draw(0, 0, 360, 360); + this->sensor.getIrTex().draw(0, 360, 360, 360); } if (sensor.bRecord) @@ -54,7 +61,8 @@ void ofApp::draw() draw_recording_animation(); } - ofDrawBitmapStringHighlight(ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20); + ofDrawBitmapStringHighlight("Render : " + ofToString(ofGetFrameRate(), 2) + " FPS", 10, 20); + ofDrawBitmapStringHighlight("Camera : " + ofToString(this->fpsCounter.getFps(), 2) + " FPS", 10, 40); stringstream ss; ss << "Press SPACEBAR to start/stop a recording." << endl; diff --git a/example-record/src/ofApp.h b/example-record/src/ofApp.h index 76f74f9..11ff419 100644 --- a/example-record/src/ofApp.h +++ b/example-record/src/ofApp.h @@ -24,6 +24,7 @@ class ofApp : public ofBaseApp{ void gotMessage(ofMessage msg); ofxAzureKinect::Device sensor; + ofFpsCounter fpsCounter; void draw_recording_animation(); diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index d0a31e7..b394b0a 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -2,10 +2,27 @@ #include "ofLog.h" -const int32_t TIMEOUT_IN_MS = 1000; namespace ofxAzureKinect { + static constexpr int32_t TIMEOUT_IN_MS = 1000; + static constexpr int64_t WAIT_FOR_SYNCHRONIZED_CAPTURE_TIMEOUT = 60000; + + static void log_lagging_time(const char *lagger, k4a::capture &master, k4a::capture &sub) + { + int32_t diff = master.get_color_image().get_device_timestamp().count() - sub.get_color_image().get_device_timestamp().count(); + std::cout << std::setw(6) << lagger << " lagging: mc:" << std::setw(6) + << master.get_color_image().get_device_timestamp().count() << "us sc:" << std::setw(6) + << sub.get_color_image().get_device_timestamp().count() << "us diff:" << diff << endl; + } + + static void log_synced_image_time(k4a::capture &master, k4a::capture &sub) + { + int32_t diff = master.get_color_image().get_device_timestamp().count() - sub.get_color_image().get_device_timestamp().count(); + std::cout << "Sync'd capture: mc:" << std::setw(6) << master.get_color_image().get_device_timestamp().count() + << "us sc:" << std::setw(6) << sub.get_color_image().get_device_timestamp().count() << "us diff:" << diff << endl; + } + DeviceSettings::DeviceSettings(int idx) : depthMode(K4A_DEPTH_MODE_WFOV_2X2BINNED) , colorResolution(K4A_COLOR_RESOLUTION_2160P) @@ -50,6 +67,7 @@ namespace ofxAzureKinect , jpegDecompressor(tjInitDecompress()) , bPlayback(false) , bEnableIMU(false) + , bMultiDeviceSyncCapture(false) {} Device::~Device() @@ -349,7 +367,12 @@ namespace ofxAzureKinect } } - this->startThread(); + if (!bMultiDeviceSyncCapture) { + this->startThread(); + } + else { + this->waitForThread(); + } ofAddListener(ofEvents().update, this, &Device::update); this->bStreaming = true; @@ -421,7 +444,9 @@ namespace ofxAzureKinect this->condition.wait(lock); } - this->updatePixels(); + if (!bMultiDeviceSyncCapture) { + this->updatePixels(); + } } } @@ -442,7 +467,7 @@ namespace ofxAzureKinect void Device::updatePixels() { // Get a capture. - if (bPlayback) + if (this->bPlayback) { if (playback->isPlaying()) { @@ -468,6 +493,8 @@ namespace ofxAzureKinect return; } } + else if (this->bMultiDeviceSyncCapture && master_device_capture != nullptr) { + } else { try @@ -521,15 +548,18 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { - const int decompressStatus = tjDecompress2(this->jpegDecompressor, - colorImg.get_buffer(), - static_cast(colorImg.get_size()), - this->colorPix.getData(), - colorDims.x, - 0, // pitch - colorDims.y, - TJPF_BGRA, - TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); + // during recording, preview frame rate is dropped not to drop recording frames. + if (!this->bRecord || this->recording->getRecordedFrameNum() % preview_interval_during_recording == 0) { + const int decompressStatus = tjDecompress2(this->jpegDecompressor, + colorImg.get_buffer(), + static_cast(colorImg.get_size()), + this->colorPix.getData(), + colorDims.x, + 0, // pitch + colorDims.y, + TJPF_BGRA, + TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); + } } else { @@ -596,7 +626,7 @@ namespace ofxAzureKinect } // Do any recording before releasing the capture - if (bRecord) + if (this->bRecord) { k4a_capture_t capture_handle = capture.handle(); recording->record(&capture_handle); @@ -1099,4 +1129,179 @@ namespace ofxAzureKinect playback->seek(val); } + + void MultiDeviceSyncCapture::setMasterDevice(Device * p) + { + master_device = p; + master_device->bMultiDeviceSyncCapture = true; + master_device->master_device_capture = this; + } + void MultiDeviceSyncCapture::addSubordinateDevice(Device * p) + { + p->bMultiDeviceSyncCapture = true; + p->master_device_capture = this; + subordinate_devices.push_back(p); + } + + void MultiDeviceSyncCapture::start() + { + if (master_device == nullptr || subordinate_devices.empty()) { + cerr << "MultiDeviceSyncCapture::setMasterDevice, addSubordinateDevice must be called before start." << endl; + return; + } + startThread(); + } + + void MultiDeviceSyncCapture::stop() + { + waitForThread(); + } + + void MultiDeviceSyncCapture::setMaxAllowableTimeOffsetUsec(uint32_t usec) + { + max_allowable_time_offset_error_for_image_timestamp = std::chrono::microseconds(usec); + } + + void MultiDeviceSyncCapture::threadedFunction() + { + while (isThreadRunning()) { + // Dealing with the synchronized cameras is complex. The Azure Kinect DK: + // (a) does not guarantee exactly equal timestamps between depth and color or between cameras (delays can + // be configured but timestamps will only be approximately the same) + // (b) does not guarantee that, if the two most recent images were synchronized, that calling get_capture + // just once on each camera will still be synchronized. + // There are several reasons for all of this. Internally, devices keep a queue of a few of the captured images + // and serve those images as requested by get_capture(). However, images can also be dropped at any moment, and + // one device may have more images ready than another device at a given moment, et cetera. + // + // Also, the process of synchronizing is complex. The cameras are not guaranteed to exactly match in all of + // their timestamps when synchronized (though they should be very close). All delays are relative to the master + // camera's color camera. To deal with these complexities, we employ a fairly straightforward algorithm. Start + // by reading in two captures, then if the camera images were not taken at roughly the same time read a new one + // from the device that had the older capture until the timestamps roughly match. + + // The captures used in the loop are outside of it so that they can persist across loop iterations. This is + // necessary because each time this loop runs we'll only update the older capture. + // The captures are stored in a vector where the first element of the vector is the master capture and + // subsequent elements are subordinate captures + //std::vector captures(subordinate_devices.size() + 1); // add 1 for the master + auto devices = subordinate_devices; + devices.push_back(master_device); + + size_t current_index = 0; + master_device->device.get_capture(&master_device->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + ++current_index; + for (auto &d : subordinate_devices) + { + d->device.get_capture(&d->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + ++current_index; + } + + bool have_synced_images = false; + std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); + while (!have_synced_images) + { + // Timeout if this is taking too long + int64_t duration_ms = + std::chrono::duration_cast(std::chrono::system_clock::now() - start).count(); + if (duration_ms > WAIT_FOR_SYNCHRONIZED_CAPTURE_TIMEOUT) + { + cerr << "ERROR: Timedout waiting for synchronized captures\n"; + } + + k4a::image master_color_image = master_device->capture.get_color_image(); + std::chrono::microseconds master_color_image_time = master_color_image.get_device_timestamp(); + + for (size_t i = 0; i < subordinate_devices.size(); ++i) + { + k4a::image sub_image; + if (compare_sub_depth_instead_of_color) + { + sub_image = subordinate_devices[i]->capture.get_depth_image(); + } + else + { + sub_image = subordinate_devices[i]->capture.get_color_image(); + } + + if (master_color_image && sub_image) + { + std::chrono::microseconds sub_image_time = sub_image.get_device_timestamp(); + // The subordinate's color image timestamp, ideally, is the master's color image timestamp plus the + // delay we configured between the master device color camera and subordinate device color camera + std::chrono::microseconds expected_sub_image_time = + master_color_image_time + + std::chrono::microseconds{ subordinate_devices[i]->config.subordinate_delay_off_master_usec } + + std::chrono::microseconds{ subordinate_devices[i]->config.depth_delay_off_color_usec }; + std::chrono::microseconds sub_image_time_error = sub_image_time - expected_sub_image_time; + // The time error's absolute value must be within the permissible range. So, for example, if + // MAX_ALLOWABLE_TIME_OFFSET_ERROR_FOR_IMAGE_TIMESTAMP is 2, offsets of -2, -1, 0, 1, and -2 are + // permitted + if (sub_image_time_error < -max_allowable_time_offset_error_for_image_timestamp) + { + // Example, where MAX_ALLOWABLE_TIME_OFFSET_ERROR_FOR_IMAGE_TIMESTAMP is 1 + // time t=1 t=2 t=3 + // actual timestamp x . . + // expected timestamp . . x + // error: 1 - 3 = -2, which is less than the worst-case-allowable offset of -1 + // the subordinate camera image timestamp was earlier than it is allowed to be. This means the + // subordinate is lagging and we need to update the subordinate to get the subordinate caught up + log_lagging_time("sub", master_device->capture, subordinate_devices[i]->capture); + subordinate_devices[i]->device.get_capture(&subordinate_devices[i]->capture, + std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + break; + } + else if (sub_image_time_error > max_allowable_time_offset_error_for_image_timestamp) + { + // Example, where MAX_ALLOWABLE_TIME_OFFSET_ERROR_FOR_IMAGE_TIMESTAMP is 1 + // time t=1 t=2 t=3 + // actual timestamp . . x + // expected timestamp x . . + // error: 3 - 1 = 2, which is more than the worst-case-allowable offset of 1 + // the subordinate camera image timestamp was later than it is allowed to be. This means the + // subordinate is ahead and we need to update the master to get the master caught up + log_lagging_time("master", master_device->capture, subordinate_devices[i]->capture); + master_device->device.get_capture(&master_device->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + break; + } + else + { + // These captures are sufficiently synchronized. If we've gotten to the end, then all are + // synchronized. + if (i == subordinate_devices.size() - 1) + { + log_synced_image_time(master_device->capture, subordinate_devices[i]->capture); + have_synced_images = true; // now we'll finish the for loop and then exit the while loop + } + } + } + else if (!master_color_image) + { + std::cout << "Master image was bad!\n"; + master_device->device.get_capture(&master_device->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + break; + } + else if (!sub_image) + { + std::cout << "Subordinate image was bad!" << endl; + subordinate_devices[i]->device.get_capture(&subordinate_devices[i]->capture, + std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + break; + } + } + } + // if we've made it to here, it means that we have synchronized captures. + for (auto& device : devices) { + device->mutex.lock(); + } + for (auto& device : devices) { + device->updatePixels(); + } + for (auto& device : devices) { + device->mutex.unlock(); + } + + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + } } // namespace ofxAzureKinect diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 39f41e2..77ff56e 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -55,10 +55,14 @@ namespace ofxAzureKinect BodyTrackingSettings(); }; + class MultiDeviceSyncCapture; + class Device : ofThread { public: + friend class MultiDeviceSyncCapture; + static int getInstalledCount(); public: @@ -131,7 +135,6 @@ namespace ofxAzureKinect protected: void threadedFunction() override; - private: void updatePixels(); void updateTextures(); @@ -159,6 +162,8 @@ namespace ofxAzureKinect bool bUpdateWorld; bool bUpdateVbo; + bool bMultiDeviceSyncCapture; + std::condition_variable condition; uint64_t pixFrameNum; uint64_t texFrameNum; @@ -222,6 +227,7 @@ namespace ofxAzureKinect BodyTracker tracker; Record *recording; + int preview_interval_during_recording = 10; void handle_recording(bool val); Playback *playback; @@ -229,5 +235,39 @@ namespace ofxAzureKinect void listener_playback_pause(bool val); void listener_playback_stop(bool val); void listener_playback_seek(float val); + + MultiDeviceSyncCapture* master_device_capture = nullptr; + }; + + // reference implementation + // https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/develop/examples/green_screen/MultiDeviceCapturer.h + // + // Note for use this class. + // device firmware version must be matched and latest. + // set manual exposure time to shorter, for acquiring correct sync. + // more info on : https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1261 + // + // Not sophisticated implementation. + class MultiDeviceSyncCapture : public ofThread + { + protected: + friend class Device; + Device* master_device = nullptr; + std::vector subordinate_devices; + bool compare_sub_depth_instead_of_color = false; + std::chrono::microseconds max_allowable_time_offset_error_for_image_timestamp = std::chrono::microseconds(1000); + public: + + // these funcs must be called before Device::startCameras(); + void setMasterDevice(Device* p); + void addSubordinateDevice(Device* p); + + void start(); + void stop(); + + void setMaxAllowableTimeOffsetUsec(uint32_t usec); + + protected: + void threadedFunction() override; }; } // namespace ofxAzureKinect \ No newline at end of file diff --git a/src/ofxAzureKinect/Record.cpp b/src/ofxAzureKinect/Record.cpp index 13abe16..0c5af01 100644 --- a/src/ofxAzureKinect/Record.cpp +++ b/src/ofxAzureKinect/Record.cpp @@ -52,6 +52,7 @@ namespace ofxAzureKinect void Record::start() { delay_start = ofGetElapsedTimef(); + record_frame_count = 0; if (delay != 0) { cout << "Recording Will Begin In " << delay << " seconds!" << endl; @@ -98,8 +99,10 @@ namespace ofxAzureKinect } // Indicate that we are recording - cout << "."; - cout.flush(); + //cout << "."; + //cout.flush(); + + record_frame_count++; } else { diff --git a/src/ofxAzureKinect/Record.h b/src/ofxAzureKinect/Record.h index 9ca1376..68a232b 100644 --- a/src/ofxAzureKinect/Record.h +++ b/src/ofxAzureKinect/Record.h @@ -23,7 +23,9 @@ namespace ofxAzureKinect void setDelay(float _delay) { this->delay = _delay; } float getTimerDelay(); - + uint32_t getRecordedFrameNum() const { + return record_frame_count; + } private: k4a_device_t device; k4a_record_t recording; @@ -37,5 +39,6 @@ namespace ofxAzureKinect float delay = 0; float delay_start = 0; + uint32_t record_frame_count = 0; }; } // namespace ofxAzureKinect \ No newline at end of file From 2c9c5d24c6e905e9b4ceecac8014f96fe5c020ed Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 22:00:51 +0900 Subject: [PATCH 27/48] update readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index fb79667..3672854 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ The instructions below are based on the [Azure Kinect Sensor SDK Usage](https:// * Install the [Azure Kinect Sensor SDK](https://docs.microsoft.com/en-us/azure/Kinect-dk/sensor-sdk-download). * Install the [Azure Kinect Body Tracking SDK](https://docs.microsoft.com/en-us/azure/Kinect-dk/body-sdk-download). -* Add an environment variable for `AZUREKINECT_SDK` and set it to the Sensor SDK installation path (no trailing slash). The default is `C:\Program Files\Azure Kinect SDK v1.3.0`. +* Add an environment variable for `AZUREKINECT_SDK` and set it to the Sensor SDK installation path (no trailing slash). The default is `C:\Program Files\Azure Kinect SDK v1.4.1`. * Add an environment variable for `AZUREKINECT_BODY_SDK` and set it to the Body SDK installation path (no trailing slash). The default is `C:\Program Files\Azure Kinect Body Tracking SDK`. ![Environment Variables](Install-EnvVars.png) From d9d2a8cbf98728fea665a1b0708518a956966c95 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 17 Nov 2020 23:30:48 +0900 Subject: [PATCH 28/48] No wait for render thread during recording, not to drop frames --- src/ofxAzureKinect/Device.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index b394b0a..39eab16 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -437,16 +437,15 @@ namespace ofxAzureKinect { while (this->isThreadRunning()) { - std::unique_lock lock(this->mutex); - - while (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) - { - this->condition.wait(lock); - } - - if (!bMultiDeviceSyncCapture) { - this->updatePixels(); + // During recording, do not wait for render thread, not to drop frames. + if (!this->bRecord) { + std::unique_lock lock(this->mutex); + while (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) + { + this->condition.wait(lock); + } } + this->updatePixels(); } } From 75b8f01bca6649dee22b6a4978f07905ac631ea3 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 00:37:41 +0900 Subject: [PATCH 29/48] add recording funcs --- src/ofxAzureKinect/Device.cpp | 30 ++++++++++++++++++++++++++++++ src/ofxAzureKinect/Device.h | 11 ++++++++++- src/ofxAzureKinect/Record.h | 4 ++-- 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 39eab16..a40ba65 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -68,6 +68,7 @@ namespace ofxAzureKinect , bPlayback(false) , bEnableIMU(false) , bMultiDeviceSyncCapture(false) + , bRecording(false) {} Device::~Device() @@ -1089,6 +1090,34 @@ namespace ofxAzureKinect setColorCameraControlValue(K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE, exposure_usec); } + void Device::startRecording(std::string filename, float delay) + { + if (isRecording()) { + return; + } + + recording = new Record(); + recording->setup(device.handle(), this->config, bEnableIMU, delay, filename); + recording->start(); + + bRecording = true; + } + + void Device::stopRecording() + { + if (recording) { + recording->stop(); + delete recording; + recording = nullptr; + } + bRecording = false; + } + + bool Device::isRecording() const + { + return bRecording; + } + void Device::handle_recording(bool val) { if (val) @@ -1101,6 +1130,7 @@ namespace ofxAzureKinect { recording->stop(); } + bRecording = val; } float Device::getRecordingTimerDelay() diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 77ff56e..895204a 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -124,6 +124,14 @@ namespace ofxAzureKinect int32_t getExposureTimeAbsolute() const; void setExposureTimeAbsolute(int32_t exposure_usec); + void startRecording(std::string filename, float delay = 0.0f); + void stopRecording(); + bool isRecording() const; + + void setPreviewIntervalDuringRecording(int interval) { + preview_interval_during_recording = interval; + } + public: ofParameter bRecord{"bRecord", false}; float getRecordingTimerDelay(); @@ -154,6 +162,7 @@ namespace ofxAzureKinect bool bOpen; bool bStreaming; bool bPlayback; + bool bRecording; bool bEnableIMU; bool bUpdateColor; @@ -227,7 +236,7 @@ namespace ofxAzureKinect BodyTracker tracker; Record *recording; - int preview_interval_during_recording = 10; + int preview_interval_during_recording = 3; void handle_recording(bool val); Playback *playback; diff --git a/src/ofxAzureKinect/Record.h b/src/ofxAzureKinect/Record.h index 68a232b..32b0a8c 100644 --- a/src/ofxAzureKinect/Record.h +++ b/src/ofxAzureKinect/Record.h @@ -11,8 +11,8 @@ namespace ofxAzureKinect { public: - Record(){}; - ~Record(); + Record(){} + ~Record(){} void setup(k4a_device_t device, k4a_device_configuration_t config, bool recording_imu_enabled = true, float delay = 3, string filename = ""); void start(); From 2f07ceb24bf7f987dbee47287bf0e505b28a4b34 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 00:51:42 +0900 Subject: [PATCH 30/48] bRecord is now just UI (dirty, need refactor) --- src/ofxAzureKinect/Device.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index a40ba65..a72d11f 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -439,7 +439,7 @@ namespace ofxAzureKinect while (this->isThreadRunning()) { // During recording, do not wait for render thread, not to drop frames. - if (!this->bRecord) { + if (!this->bRecording) { std::unique_lock lock(this->mutex); while (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) { @@ -549,7 +549,7 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { // during recording, preview frame rate is dropped not to drop recording frames. - if (!this->bRecord || this->recording->getRecordedFrameNum() % preview_interval_during_recording == 0) { + if (!this->bRecording || this->recording->getRecordedFrameNum() % preview_interval_during_recording == 0) { const int decompressStatus = tjDecompress2(this->jpegDecompressor, colorImg.get_buffer(), static_cast(colorImg.get_size()), @@ -626,7 +626,7 @@ namespace ofxAzureKinect } // Do any recording before releasing the capture - if (this->bRecord) + if (this->bRecording) { k4a_capture_t capture_handle = capture.handle(); recording->record(&capture_handle); From 1656c894b95cef5cc09b0c8c5e033c292381c358 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 16:37:53 +0900 Subject: [PATCH 31/48] remove bRecord --- example-record/src/ofApp.cpp | 13 +++++++++---- src/ofxAzureKinect/Device.cpp | 22 ---------------------- src/ofxAzureKinect/Device.h | 4 +--- 3 files changed, 10 insertions(+), 29 deletions(-) diff --git a/example-record/src/ofApp.cpp b/example-record/src/ofApp.cpp index acae0d9..63922ef 100644 --- a/example-record/src/ofApp.cpp +++ b/example-record/src/ofApp.cpp @@ -34,7 +34,7 @@ void ofApp::update() { // If we are recording, update the total recording time - if (sensor.bRecord && ofGetElapsedTimef() > recording_start) + if (sensor.isRecording() && ofGetElapsedTimef() > recording_start) recording_duration = ofGetElapsedTimef() - recording_start; if (sensor.isFrameNew()) { @@ -56,7 +56,7 @@ void ofApp::draw() this->sensor.getIrTex().draw(0, 360, 360, 360); } - if (sensor.bRecord) + if (sensor.isRecording()) { draw_recording_animation(); } @@ -106,8 +106,13 @@ void ofApp::keyPressed(int key) { case ' ': { - sensor.bRecord = !sensor.bRecord; - if (sensor.bRecord) + if (!sensor.isRecording()) { + sensor.startRecording(); + } + else { + sensor.stopRecording(); + } + if (sensor.isRecording()) { recording_delay = sensor.getRecordingTimerDelay(); recording_start = ofGetElapsedTimef() + recording_delay; diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index a72d11f..479b160 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -378,13 +378,6 @@ namespace ofxAzureKinect this->bStreaming = true; - if (!bPlayback) { - // Add Recording Listener - this->eventListeners.push(this->bRecord.newListener([this](bool) { - handle_recording(this->bRecord); - })); - } - return true; } @@ -1118,21 +1111,6 @@ namespace ofxAzureKinect return bRecording; } - void Device::handle_recording(bool val) - { - if (val) - { - recording = new Record(); - recording->setup(device.handle(), this->config, bEnableIMU); - recording->start(); - } - else - { - recording->stop(); - } - bRecording = val; - } - float Device::getRecordingTimerDelay() { if (recording != nullptr) diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 895204a..1c3c707 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -124,7 +124,7 @@ namespace ofxAzureKinect int32_t getExposureTimeAbsolute() const; void setExposureTimeAbsolute(int32_t exposure_usec); - void startRecording(std::string filename, float delay = 0.0f); + void startRecording(std::string filename = "", float delay = 0.0f); void stopRecording(); bool isRecording() const; @@ -133,7 +133,6 @@ namespace ofxAzureKinect } public: - ofParameter bRecord{"bRecord", false}; float getRecordingTimerDelay(); ofParameter play{"play", false}; ofParameter pause{"pause", false}; @@ -237,7 +236,6 @@ namespace ofxAzureKinect Record *recording; int preview_interval_during_recording = 3; - void handle_recording(bool val); Playback *playback; void listener_playback_play(bool val); From f133adc071e3619122e8215b62ea3c5aed05f89c Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 17:33:19 +0900 Subject: [PATCH 32/48] change pixels to triple buffer style to reduce main thread occupancy. --- src/ofxAzureKinect/Device.cpp | 158 +++++++++++++++++++++------------- src/ofxAzureKinect/Device.h | 64 ++++++++------ 2 files changed, 134 insertions(+), 88 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 479b160..81f17fb 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -46,6 +46,26 @@ namespace ofxAzureKinect , updateBodies(false) {} + void Device::Frame::swapFrame(Frame & f) + { + this->depthPix.swap(f.depthPix); + std::swap(this->depthPixDeviceTime, f.depthPixDeviceTime); + + this->colorPix.swap(f.colorPix); + std::swap(this->colorPixDeviceTime, f.colorPixDeviceTime); + + this->irPix.swap(f.irPix); + this->depthInColorPix.swap(f.depthInColorPix); + this->colorInDepthPix.swap(f.colorInDepthPix); + this->bodyIndexPix.swap(f.bodyIndexPix); + std::swap(this->bodySkeletons, f.bodySkeletons); + std::swap(this->bodyIDs, f.bodyIDs); + + std::swap(this->positionCache, f.positionCache); + std::swap(this->uvCache, f.uvCache); + std::swap(this->numPoints, f.numPoints); + } + int Device::getInstalledCount() { return k4a_device_get_installed_count(); @@ -58,6 +78,7 @@ namespace ofxAzureKinect , bOpen(false) , bStreaming(false) , bNewFrame(false) + , bNewBuffer(false) , bUpdateColor(false) , bUpdateIr(false) , bUpdateBodies(false) @@ -440,6 +461,14 @@ namespace ofxAzureKinect } } this->updatePixels(); + + if (this->lock()) { + this->frameBack.swapFrame(this->frameSwap); + this->bNewBuffer = true; + this->unlock(); + } + + std::this_thread::sleep_for(std::chrono::microseconds(100)); } } @@ -447,12 +476,14 @@ namespace ofxAzureKinect { this->bNewFrame = false; - if (this->texFrameNum != this->pixFrameNum) + if (this->bNewBuffer) { - std::unique_lock lock(this->mutex); - + if (this->lock()) { + this->frameFront.swapFrame(this->frameSwap); + this->bNewBuffer = false; + this->unlock(); + } this->updateTextures(); - this->condition.notify_all(); } } @@ -506,18 +537,19 @@ namespace ofxAzureKinect } // Probe for a depth16 image. + auto& f = this->frameBack; auto depthImg = this->capture.get_depth_image(); if (depthImg) { const auto depthDims = glm::ivec2(depthImg.get_width_pixels(), depthImg.get_height_pixels()); - if (!depthPix.isAllocated()) + if (!f.depthPix.isAllocated()) { - this->depthPix.allocate(depthDims.x, depthDims.y, 1); + f.depthPix.allocate(depthDims.x, depthDims.y, 1); } const auto depthData = reinterpret_cast(depthImg.get_buffer()); - this->depthPix.setFromPixels(depthData, depthDims.x, depthDims.y, 1); - this->depthPixDeviceTime = depthImg.get_device_timestamp(); + f.depthPix.setFromPixels(depthData, depthDims.x, depthDims.y, 1); + f.depthPixDeviceTime = depthImg.get_device_timestamp(); ofLogVerbose(__FUNCTION__) << "Capture Depth16 " << depthDims.x << "x" << depthDims.y << " stride: " << depthImg.get_stride_bytes() << "."; } @@ -534,9 +566,9 @@ namespace ofxAzureKinect if (colorImg) { const auto colorDims = glm::ivec2(colorImg.get_width_pixels(), colorImg.get_height_pixels()); - if (!colorPix.isAllocated()) + if (!f.colorPix.isAllocated()) { - this->colorPix.allocate(colorDims.x, colorDims.y, OF_PIXELS_BGRA); + f.colorPix.allocate(colorDims.x, colorDims.y, OF_PIXELS_BGRA); } if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) @@ -546,7 +578,7 @@ namespace ofxAzureKinect const int decompressStatus = tjDecompress2(this->jpegDecompressor, colorImg.get_buffer(), static_cast(colorImg.get_size()), - this->colorPix.getData(), + f.colorPix.getData(), colorDims.x, 0, // pitch colorDims.y, @@ -557,10 +589,10 @@ namespace ofxAzureKinect else { const auto colorData = reinterpret_cast(colorImg.get_buffer()); - this->colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); + f.colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); } - this->colorPixDeviceTime = colorImg.get_device_timestamp(); + f.colorPixDeviceTime = colorImg.get_device_timestamp(); ofLogVerbose(__FUNCTION__) << "Capture Color " << colorDims.x << "x" << colorDims.y << " stride: " << colorImg.get_stride_bytes() << "."; } @@ -578,13 +610,13 @@ namespace ofxAzureKinect if (irImg) { const auto irSize = glm::ivec2(irImg.get_width_pixels(), irImg.get_height_pixels()); - if (!this->irPix.isAllocated()) + if (!f.irPix.isAllocated()) { - this->irPix.allocate(irSize.x, irSize.y, 1); + f.irPix.allocate(irSize.x, irSize.y, 1); } const auto irData = reinterpret_cast(irImg.get_buffer()); - this->irPix.setFromPixels(irData, irSize.x, irSize.y, 1); + f.irPix.setFromPixels(irData, irSize.x, irSize.y, 1); ofLogVerbose(__FUNCTION__) << "Capture Ir16 " << irSize.x << "x" << irSize.y << " stride: " << irImg.get_stride_bytes() << "."; } @@ -603,11 +635,11 @@ namespace ofxAzureKinect { if (this->bUpdateColor) { - this->updatePointsCache(depthImg, this->depthToWorldImg); //(colorImg, this->colorToWorldImg); + this->updatePointsCache(depthImg, depthToWorldImg); //(colorImg, this->colorToWorldImg); } else { - this->updatePointsCache(depthImg, this->depthToWorldImg); + this->updatePointsCache(depthImg, depthToWorldImg); } } @@ -639,26 +671,26 @@ namespace ofxAzureKinect void Device::updateTextures() { - if (this->depthPix.isAllocated()) + auto& f = this->frameFront; + if (f.depthPix.isAllocated()) { // Update the depth texture. if (!this->depthTex.isAllocated()) { - this->depthTex.allocate(this->depthPix); + this->depthTex.allocate(f.depthPix); this->depthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); } - this->depthTex.loadData(this->depthPix); - this->depthTexDeviceTime = this->depthPixDeviceTime; + this->depthTex.loadData(f.depthPix); ofLogVerbose(__FUNCTION__) << "Update Depth16 " << this->depthTex.getWidth() << "x" << this->depthTex.getHeight() << "."; } - if (this->bUpdateColor && this->colorPix.isAllocated()) + if (this->bUpdateColor && f.colorPix.isAllocated()) { // Update the color texture. if (!this->colorTex.isAllocated()) { - this->colorTex.allocate(this->colorPix); + this->colorTex.allocate(f.colorPix); this->colorTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) @@ -672,60 +704,59 @@ namespace ofxAzureKinect } } - this->colorTex.loadData(this->colorPix); - this->colorTexDeviceTime = this->colorPixDeviceTime; + this->colorTex.loadData(f.colorPix); ofLogVerbose(__FUNCTION__) << "Update Color " << this->colorTex.getWidth() << "x" << this->colorTex.getHeight() << "."; } - if (this->bUpdateIr && this->irPix.isAllocated()) + if (this->bUpdateIr && f.irPix.isAllocated()) { // Update the IR16 image. if (!this->irTex.isAllocated()) { - this->irTex.allocate(this->irPix); + this->irTex.allocate(f.irPix); this->irTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); this->irTex.setRGToRGBASwizzles(true); } - this->irTex.loadData(this->irPix); + this->irTex.loadData(f.irPix); ofLogVerbose(__FUNCTION__) << "Update Ir16 " << this->irTex.getWidth() << "x" << this->irTex.getHeight() << "."; } - if (this->bUpdateBodies && this->bodyIndexPix.isAllocated()) + if (this->bUpdateBodies && f.bodyIndexPix.isAllocated()) { if (!this->bodyIndexTex.isAllocated()) { - this->bodyIndexTex.allocate(this->bodyIndexPix); + this->bodyIndexTex.allocate(f.bodyIndexPix); this->bodyIndexTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); } - this->bodyIndexTex.loadData(this->bodyIndexPix); + this->bodyIndexTex.loadData(f.bodyIndexPix); } if (this->bUpdateVbo) { - this->pointCloudVbo.setVertexData(this->positionCache.data(), this->numPoints, GL_STREAM_DRAW); - this->pointCloudVbo.setTexCoordData(this->uvCache.data(), this->numPoints, GL_STREAM_DRAW); + this->pointCloudVbo.setVertexData(f.positionCache.data(), f.numPoints, GL_STREAM_DRAW); + this->pointCloudVbo.setTexCoordData(f.uvCache.data(), f.numPoints, GL_STREAM_DRAW); } if (this->bUpdateColor && this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) { - if (this->depthInColorPix.isAllocated()) + if (f.depthInColorPix.isAllocated()) { if (!this->depthInColorTex.isAllocated()) { - this->depthInColorTex.allocate(this->depthInColorPix); + this->depthInColorTex.allocate(f.depthInColorPix); this->depthInColorTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); } - this->depthInColorTex.loadData(this->depthInColorPix); + this->depthInColorTex.loadData(f.depthInColorPix); } - if (this->colorInDepthPix.isAllocated()) + if (f.colorInDepthPix.isAllocated()) { if (!this->colorInDepthTex.isAllocated()) { - this->colorInDepthTex.allocate(this->colorInDepthPix); + this->colorInDepthTex.allocate(f.colorInDepthPix); this->colorInDepthTex.setTextureMinMagFilter(GL_NEAREST, GL_NEAREST); this->colorInDepthTex.bind(); { @@ -735,7 +766,7 @@ namespace ofxAzureKinect this->colorInDepthTex.unbind(); } - this->colorInDepthTex.loadData(this->colorInDepthPix); + this->colorInDepthTex.loadData(f.colorInDepthPix); } } @@ -847,6 +878,7 @@ namespace ofxAzureKinect return true; } + // Kinect Thread function. bool Device::updatePointsCache(k4a::image &frameImg, k4a::image &tableImg) { const auto frameDims = glm::ivec2(frameImg.get_width_pixels(), frameImg.get_height_pixels()); @@ -860,8 +892,9 @@ namespace ofxAzureKinect const auto frameData = reinterpret_cast(frameImg.get_buffer()); const auto tableData = reinterpret_cast(tableImg.get_buffer()); - this->positionCache.resize(frameDims.x * frameDims.y); - this->uvCache.resize(frameDims.x * frameDims.y); + auto& f = this->frameBack; + f.positionCache.resize(frameDims.x * frameDims.y); + f.uvCache.resize(frameDims.x * frameDims.y); int count = 0; for (int y = 0; y < frameDims.y; ++y) @@ -873,23 +906,24 @@ namespace ofxAzureKinect tableData[idx].xy.x != 0 && tableData[idx].xy.y != 0) { float depthVal = static_cast(frameData[idx]); - this->positionCache[count] = glm::vec3( + f.positionCache[count] = glm::vec3( tableData[idx].xy.x * depthVal, tableData[idx].xy.y * depthVal, depthVal); - this->uvCache[count] = glm::vec2(x, y); + f.uvCache[count] = glm::vec2(x, y); ++count; } } } - this->numPoints = count; + f.numPoints = count; return true; } + // Kinect Thread function. bool Device::updateDepthInColorFrame(const k4a::image &depthImg, const k4a::image &colorImg) { const auto colorDims = glm::ivec2(colorImg.get_width_pixels(), colorImg.get_height_pixels()); @@ -911,12 +945,13 @@ namespace ofxAzureKinect const auto transformedColorData = reinterpret_cast(transformedDepthImg.get_buffer()); - if (!this->depthInColorPix.isAllocated()) + auto& f = this->frameBack; + if (!f.depthInColorPix.isAllocated()) { - this->depthInColorPix.allocate(colorDims.x, colorDims.y, 1); + f.depthInColorPix.allocate(colorDims.x, colorDims.y, 1); } - this->depthInColorPix.setFromPixels(transformedColorData, colorDims.x, colorDims.y, 1); + f.depthInColorPix.setFromPixels(transformedColorData, colorDims.x, colorDims.y, 1); ofLogVerbose(__FUNCTION__) << "Depth in Color " << colorDims.x << "x" << colorDims.y << " stride: " << transformedDepthImg.get_stride_bytes() << "."; @@ -925,6 +960,7 @@ namespace ofxAzureKinect return true; } + // Kinect Thread function. bool Device::updateColorInDepthFrame(const k4a::image &depthImg, const k4a::image &colorImg) { const auto depthDims = glm::ivec2(depthImg.get_width_pixels(), depthImg.get_height_pixels()); @@ -946,12 +982,13 @@ namespace ofxAzureKinect const auto transformedColorData = reinterpret_cast(transformedColorImg.get_buffer()); - if (!this->colorInDepthPix.isAllocated()) + auto& f = this->frameBack; + if (!f.colorInDepthPix.isAllocated()) { - this->colorInDepthPix.allocate(depthDims.x, depthDims.y, OF_PIXELS_BGRA); + f.colorInDepthPix.allocate(depthDims.x, depthDims.y, OF_PIXELS_BGRA); } - this->colorInDepthPix.setFromPixels(transformedColorData, depthDims.x, depthDims.y, 4); + f.colorInDepthPix.setFromPixels(transformedColorData, depthDims.x, depthDims.y, 4); ofLogVerbose(__FUNCTION__) << "Color in Depth " << depthDims.x << "x" << depthDims.y << " stride: " << transformedColorImg.get_stride_bytes() << "."; @@ -982,7 +1019,7 @@ namespace ofxAzureKinect const ofShortPixels &Device::getDepthPix() const { - return this->depthPix; + return this->frameFront.depthPix; } const ofTexture &Device::getDepthTex() const @@ -992,7 +1029,7 @@ namespace ofxAzureKinect const ofPixels &Device::getColorPix() const { - return this->colorPix; + return this->frameFront.colorPix; } const ofTexture &Device::getColorTex() const @@ -1002,7 +1039,7 @@ namespace ofxAzureKinect const ofShortPixels &Device::getIrPix() const { - return this->irPix; + return this->frameFront.irPix; } const ofTexture &Device::getIrTex() const @@ -1032,7 +1069,7 @@ namespace ofxAzureKinect const ofShortPixels &Device::getDepthInColorPix() const { - return this->depthInColorPix; + return this->frameFront.depthInColorPix; } const ofTexture &Device::getDepthInColorTex() const @@ -1042,7 +1079,7 @@ namespace ofxAzureKinect const ofPixels &Device::getColorInDepthPix() const { - return this->colorInDepthPix; + return this->frameFront.colorInDepthPix; } const ofTexture &Device::getColorInDepthTex() const @@ -1298,14 +1335,15 @@ namespace ofxAzureKinect } } // if we've made it to here, it means that we have synchronized captures. - for (auto& device : devices) { - device->mutex.lock(); - } for (auto& device : devices) { device->updatePixels(); } for (auto& device : devices) { - device->mutex.unlock(); + if (device->lock()) { + device->frameBack.swapFrame(device->frameSwap); + device->bNewBuffer = true; + device->unlock(); + } } std::this_thread::sleep_for(std::chrono::microseconds(100)); diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 1c3c707..39adf03 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -60,6 +60,29 @@ namespace ofxAzureKinect class Device : ofThread { + protected: + struct Frame + { + ofShortPixels depthPix; + std::chrono::microseconds depthPixDeviceTime; + + ofPixels colorPix; + std::chrono::microseconds colorPixDeviceTime; + + ofShortPixels irPix; + ofShortPixels depthInColorPix; + ofPixels colorInDepthPix; + + ofPixels bodyIndexPix; + std::vector bodySkeletons; + std::vector bodyIDs; + + std::vector positionCache; + std::vector uvCache; + size_t numPoints; + + void swapFrame(Frame& f); + }; public: friend class MultiDeviceSyncCapture; @@ -90,13 +113,13 @@ namespace ofxAzureKinect const ofShortPixels &getDepthPix() const; const ofTexture &getDepthTex() const; const std::chrono::microseconds& getDepthTexDeviceTime() const { - return this->depthTexDeviceTime; + return this->frameFront.depthPixDeviceTime; } const ofPixels &getColorPix() const; const ofTexture &getColorTex() const; const std::chrono::microseconds& getColorTexDeviceTime() const { - return this->colorTexDeviceTime; + return this->frameFront.colorPixDeviceTime; } const ofShortPixels &getIrPix() const; @@ -176,6 +199,7 @@ namespace ofxAzureKinect uint64_t pixFrameNum; uint64_t texFrameNum; + bool bNewBuffer; bool bNewFrame; std::string serialNumber; @@ -193,41 +217,25 @@ namespace ofxAzureKinect k4a_imu_sample_t imu_sample; - ofShortPixels depthPix; - ofTexture depthTex; - - ofPixels colorPix; - ofTexture colorTex; - std::chrono::microseconds colorPixDeviceTime; - std::chrono::microseconds colorTexDeviceTime; - - ofShortPixels irPix; - ofTexture irTex; + // triple buffer + Frame frameBack; + Frame frameSwap; + Frame frameFront; + // these are thread safe k4a::image depthToWorldImg; ofFloatPixels depthToWorldPix; - ofTexture depthToWorldTex; - std::chrono::microseconds depthPixDeviceTime; - std::chrono::microseconds depthTexDeviceTime; - k4a::image colorToWorldImg; ofFloatPixels colorToWorldPix; - ofTexture colorToWorldTex; - ofShortPixels depthInColorPix; + ofTexture depthTex; + ofTexture colorTex; + ofTexture irTex; + ofTexture depthToWorldTex; + ofTexture colorToWorldTex; ofTexture depthInColorTex; - - ofPixels colorInDepthPix; ofTexture colorInDepthTex; - - ofPixels bodyIndexPix; ofTexture bodyIndexTex; - std::vector bodySkeletons; - std::vector bodyIDs; - - std::vector positionCache; - std::vector uvCache; - size_t numPoints; ofVbo pointCloudVbo; ofEventListeners eventListeners; From 2ba353f941fb7db4c8d79a93a1477f7974068ebe Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 17:45:24 +0900 Subject: [PATCH 33/48] fix color pix update during recording --- example-bodies/src/ofApp.cpp | 6 +++--- src/ofxAzureKinect/Device.cpp | 18 ++++++++++++++---- src/ofxAzureKinect/Device.h | 1 + 3 files changed, 18 insertions(+), 7 deletions(-) diff --git a/example-bodies/src/ofApp.cpp b/example-bodies/src/ofApp.cpp index 593e9e7..af62460 100644 --- a/example-bodies/src/ofApp.cpp +++ b/example-bodies/src/ofApp.cpp @@ -58,7 +58,7 @@ void ofApp::draw() { ofBackground(0); - ofxAzureKinect::BodyTracker *tracker = kinectDevice.get_body_tracker(); + ofxAzureKinect::BodyTracker *tracker = kinectDevice.getBodyTracker(); tracker->draw_body_map(); @@ -74,7 +74,7 @@ void ofApp::draw() stringstream ss; ss << ofToString(ofGetFrameRate(), 2) + " FPS" << std::endl; - ss << "Joint Smoothing: " << kinectDevice.get_body_tracker()->get_joint_smoothing(); + ss << "Joint Smoothing: " << tracker->get_joint_smoothing(); ofDrawBitmapStringHighlight(ss.str(), 10, 20); } @@ -136,7 +136,7 @@ void ofApp::mouseDragged(int x, int y, int button) { if (button == 1) { - this->kinectDevice.get_body_tracker()->set_joint_smoothing(ofMap(x, 0, ofGetWidth(), 0.0f, 1.0f, true)); + this->kinectDevice.getBodyTracker()->set_joint_smoothing(ofMap(x, 0, ofGetWidth(), 0.0f, 1.0f, true)); } } diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 81f17fb..f6773ed 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -51,8 +51,11 @@ namespace ofxAzureKinect this->depthPix.swap(f.depthPix); std::swap(this->depthPixDeviceTime, f.depthPixDeviceTime); - this->colorPix.swap(f.colorPix); - std::swap(this->colorPixDeviceTime, f.colorPixDeviceTime); + if (this->bColorPixUpdated) { + this->colorPix.swap(f.colorPix); + std::swap(this->colorPixDeviceTime, f.colorPixDeviceTime); + std::swap(this->bColorPixUpdated, f.bColorPixUpdated); + } this->irPix.swap(f.irPix); this->depthInColorPix.swap(f.depthInColorPix); @@ -479,7 +482,7 @@ namespace ofxAzureKinect if (this->bNewBuffer) { if (this->lock()) { - this->frameFront.swapFrame(this->frameSwap); + this->frameSwap.swapFrame(this->frameFront); this->bNewBuffer = false; this->unlock(); } @@ -584,15 +587,22 @@ namespace ofxAzureKinect colorDims.y, TJPF_BGRA, TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); + f.bColorPixUpdated = true; + } + else { + f.bColorPixUpdated = false; } } else { const auto colorData = reinterpret_cast(colorImg.get_buffer()); f.colorPix.setFromPixels(colorData, colorDims.x, colorDims.y, 4); + f.bColorPixUpdated = true; } - f.colorPixDeviceTime = colorImg.get_device_timestamp(); + if (f.bColorPixUpdated) { + f.colorPixDeviceTime = colorImg.get_device_timestamp(); + } ofLogVerbose(__FUNCTION__) << "Capture Color " << colorDims.x << "x" << colorDims.y << " stride: " << colorImg.get_stride_bytes() << "."; } diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 39adf03..7d8a6fa 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -67,6 +67,7 @@ namespace ofxAzureKinect std::chrono::microseconds depthPixDeviceTime; ofPixels colorPix; + bool bColorPixUpdated = false; std::chrono::microseconds colorPixDeviceTime; ofShortPixels irPix; From d3c8e8d6d2393ed74c64161721c953046cf21306 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 18:44:49 +0900 Subject: [PATCH 34/48] During recording, jpeg decode task is dispatched to another thread not to drop recording frames --- src/ofxAzureKinect/Device.cpp | 90 +++++++++++++++++++++++++++++++++-- src/ofxAzureKinect/Device.h | 36 +++++++++++--- 2 files changed, 114 insertions(+), 12 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index f6773ed..84c8e80 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -69,6 +69,65 @@ namespace ofxAzureKinect std::swap(this->numPoints, f.numPoints); } + Device::JpegDecodeThread::JpegDecodeThread() : jpegDecompressor(tjInitDecompress()) + { + } + + Device::JpegDecodeThread::~JpegDecodeThread() + { + toProcess.close(); + processed.close(); + tjDestroy(jpegDecompressor); + } + + bool Device::JpegDecodeThread::pushTaskIfEmpty(JpegTask & b) + { + if (toProcess.empty()) { + toProcess.send(b); + return true; + } + return false; + } + + bool Device::JpegDecodeThread::update(ofPixels & outPix, std::chrono::microseconds & outTime) + { + DecodedPix ret; + bool onceReceived = false; + while (processed.tryReceive(ret)) { + onceReceived = true; + } + + if (onceReceived) { + outPix = std::move(ret.colorPix); + outTime = ret.colorPixDeviceTime; + } + return onceReceived; + } + + void Device::JpegDecodeThread::threadedFunction() + { + while (isThreadRunning()) { + JpegTask b; + if (toProcess.tryReceive(b)) { + DecodedPix pix; + int width, height, jpegSubsamp, jpegColorspace; + const int header = tjDecompressHeader3(this->jpegDecompressor, + (const unsigned char*)b.colorPixBuf.getData(), + static_cast(b.colorPixBuf.size()), + &width, &height, &jpegSubsamp, &jpegColorspace); + pix.colorPix.allocate(width, height, OF_PIXELS_BGRA); + const int decompressStatus = tjDecompress2(this->jpegDecompressor, + (const unsigned char*)b.colorPixBuf.getData(), + static_cast(b.colorPixBuf.size()), + pix.colorPix.getData(), + width, 0, height, TJPF_BGRA, TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); + pix.colorPixDeviceTime = b.colorPixDeviceTime; + processed.send(pix); + } + std::this_thread::sleep_for(std::chrono::microseconds(100)); + } + } + int Device::getInstalledCount() { return k4a_device_get_installed_count(); @@ -398,6 +457,14 @@ namespace ofxAzureKinect else { this->waitForThread(); } + + if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { + this->decodeThread.startThread(); + } + else { + this->decodeThread.waitForThread(); + } + ofAddListener(ofEvents().update, this, &Device::update); this->bStreaming = true; @@ -411,6 +478,9 @@ namespace ofxAzureKinect return false; std::unique_lock lock(this->mutex); + if (this->decodeThread.isThreadRunning()) { + this->decodeThread.waitForThread(); + } this->stopThread(); this->condition.notify_all(); @@ -481,6 +551,12 @@ namespace ofxAzureKinect if (this->bNewBuffer) { + if (this->decodeThread.isThreadRunning()) { + auto ret = this->decodeThread.update(this->frameSwap.colorPix, this->frameSwap.colorPixDeviceTime); + if (ret) { + this->frameSwap.bColorPixUpdated = true; + } + } if (this->lock()) { this->frameSwap.swapFrame(this->frameFront); this->bNewBuffer = false; @@ -576,8 +652,15 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { - // during recording, preview frame rate is dropped not to drop recording frames. - if (!this->bRecording || this->recording->getRecordedFrameNum() % preview_interval_during_recording == 0) { + // during recording, jpeg decode task is dispatched to another thread, not to drop recording frames. + if (this->bRecording) { + JpegTask task; + task.colorPixBuf.set((const char*)colorImg.get_buffer(), colorImg.get_size()); + task.colorPixDeviceTime = colorImg.get_device_timestamp(); + this->decodeThread.pushTaskIfEmpty(task); + f.bColorPixUpdated = false; + } + else { const int decompressStatus = tjDecompress2(this->jpegDecompressor, colorImg.get_buffer(), static_cast(colorImg.get_size()), @@ -589,9 +672,6 @@ namespace ofxAzureKinect TJFLAG_FASTDCT | TJFLAG_FASTUPSAMPLE); f.bColorPixUpdated = true; } - else { - f.bColorPixUpdated = false; - } } else { diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 7d8a6fa..be43039 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -84,6 +84,35 @@ namespace ofxAzureKinect void swapFrame(Frame& f); }; + + struct JpegTask + { + ofBuffer colorPixBuf; + std::chrono::microseconds colorPixDeviceTime; + }; + + struct DecodedPix + { + ofPixels colorPix; + std::chrono::microseconds colorPixDeviceTime; + }; + + struct JpegDecodeThread : public ofThread + { + protected: + tjhandle jpegDecompressor; + ofThreadChannel toProcess; + ofThreadChannel processed; + void threadedFunction() override; + + public: + + JpegDecodeThread(); + ~JpegDecodeThread(); + + bool pushTaskIfEmpty(JpegTask& b); + bool update(ofPixels& outPix, std::chrono::microseconds& outTime); + } decodeThread; public: friend class MultiDeviceSyncCapture; @@ -151,11 +180,6 @@ namespace ofxAzureKinect void startRecording(std::string filename = "", float delay = 0.0f); void stopRecording(); bool isRecording() const; - - void setPreviewIntervalDuringRecording(int interval) { - preview_interval_during_recording = interval; - } - public: float getRecordingTimerDelay(); ofParameter play{"play", false}; @@ -244,8 +268,6 @@ namespace ofxAzureKinect BodyTracker tracker; Record *recording; - int preview_interval_during_recording = 3; - Playback *playback; void listener_playback_play(bool val); void listener_playback_pause(bool val); From f5031ba585b5025eb44283ba427749cc981c5374 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 19:54:15 +0900 Subject: [PATCH 35/48] small fix --- src/ofxAzureKinect/Device.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 84c8e80..6cff235 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -559,6 +559,7 @@ namespace ofxAzureKinect } if (this->lock()) { this->frameSwap.swapFrame(this->frameFront); + this->frameFront.bColorPixUpdated = false; this->bNewBuffer = false; this->unlock(); } From 03b0ed6f02d801414fa8fd1d2cf89b9a7e501b27 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 18 Nov 2020 20:15:40 +0900 Subject: [PATCH 36/48] add async jpeg decode mode, add virtual on some func for further inheritance, fix problem when start with different resolution after once stopped --- src/ofxAzureKinect/Device.cpp | 24 +++++++++++++++++++++++- src/ofxAzureKinect/Device.h | 13 +++++++++---- 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 6cff235..b0e2a8c 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -69,6 +69,11 @@ namespace ofxAzureKinect std::swap(this->numPoints, f.numPoints); } + void Device::Frame::reset() + { + *this = Frame(); + } + Device::JpegDecodeThread::JpegDecodeThread() : jpegDecompressor(tjInitDecompress()) { } @@ -152,6 +157,7 @@ namespace ofxAzureKinect , bEnableIMU(false) , bMultiDeviceSyncCapture(false) , bRecording(false) + , bAsyncJpegDecode(false) {} Device::~Device() @@ -487,6 +493,7 @@ namespace ofxAzureKinect ofRemoveListener(ofEvents().update, this, &Device::update); this->depthToWorldImg.reset(); + this->colorToWorldImg.reset(); this->transformation.destroy(); if (this->bUpdateBodies) @@ -506,6 +513,21 @@ namespace ofxAzureKinect this->device.stop_cameras(); } + this->frameBack.reset(); + this->frameSwap.reset(); + this->frameFront.reset(); + this->depthToWorldPix.clear(); + this->colorToWorldPix.clear(); + this->depthTex.clear(); + this->colorTex.clear(); + this->irTex.clear(); + this->depthToWorldTex.clear(); + this->colorToWorldTex.clear(); + this->depthInColorTex.clear(); + this->colorInDepthTex.clear(); + this->bodyIndexTex.clear(); + this->pointCloudVbo.clear(); + this->bStreaming = false; return true; @@ -654,7 +676,7 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { // during recording, jpeg decode task is dispatched to another thread, not to drop recording frames. - if (this->bRecording) { + if (this->bRecording || this->bAsyncJpegDecode) { JpegTask task; task.colorPixBuf.set((const char*)colorImg.get_buffer(), colorImg.get_size()); task.colorPixDeviceTime = colorImg.get_device_timestamp(); diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index be43039..af3cbfc 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -83,6 +83,7 @@ namespace ofxAzureKinect size_t numPoints; void swapFrame(Frame& f); + void reset(); }; struct JpegTask @@ -180,6 +181,9 @@ namespace ofxAzureKinect void startRecording(std::string filename = "", float delay = 0.0f); void stopRecording(); bool isRecording() const; + + bool isAsyncJpegDecode() const { return this->bAsyncJpegDecode; } + void setAsyncJpegDecode(bool b) { this->bAsyncJpegDecode = b; } public: float getRecordingTimerDelay(); ofParameter play{"play", false}; @@ -188,12 +192,12 @@ namespace ofxAzureKinect ofParameter seek{"Seek", 0.0f, 0.0f, 1.0f}; protected: - void threadedFunction() override; + virtual void threadedFunction() override; - void updatePixels(); - void updateTextures(); + virtual void updatePixels(); + virtual void updateTextures(); - void update(ofEventArgs &args); + virtual void update(ofEventArgs &args); bool setupDepthToWorldTable(); bool setupColorToWorldTable(); @@ -219,6 +223,7 @@ namespace ofxAzureKinect bool bUpdateVbo; bool bMultiDeviceSyncCapture; + bool bAsyncJpegDecode; std::condition_variable condition; uint64_t pixFrameNum; From 93d1e0597d6b09f0de359d5bc931b5d8c38eda22 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Thu, 19 Nov 2020 13:09:02 +0900 Subject: [PATCH 37/48] Fix : after once stopped, texture size is not cleared correctly when using async jpeg decoder. --- src/ofxAzureKinect/Device.cpp | 34 +++++++++++++++++++++++++++++----- src/ofxAzureKinect/Device.h | 3 +++ 2 files changed, 32 insertions(+), 5 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index b0e2a8c..03026bb 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -85,6 +85,32 @@ namespace ofxAzureKinect tjDestroy(jpegDecompressor); } + void Device::JpegDecodeThread::start() + { + if (isThreadRunning()) { + return; + } + startThread(); + } + + void Device::JpegDecodeThread::stop() + { + if (!isThreadRunning()) { + return; + } + waitForThread(); + + // clear remaining tasks + if (!toProcess.empty()) { + JpegTask b; + while (toProcess.tryReceive(b)) {} + } + if (!processed.empty()) { + DecodedPix b; + while (processed.tryReceive(b)) {} + } + } + bool Device::JpegDecodeThread::pushTaskIfEmpty(JpegTask & b) { if (toProcess.empty()) { @@ -465,10 +491,10 @@ namespace ofxAzureKinect } if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { - this->decodeThread.startThread(); + this->decodeThread.start(); } else { - this->decodeThread.waitForThread(); + this->decodeThread.stop(); } ofAddListener(ofEvents().update, this, &Device::update); @@ -484,9 +510,7 @@ namespace ofxAzureKinect return false; std::unique_lock lock(this->mutex); - if (this->decodeThread.isThreadRunning()) { - this->decodeThread.waitForThread(); - } + this->decodeThread.stop(); this->stopThread(); this->condition.notify_all(); diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index af3cbfc..5527435 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -111,6 +111,9 @@ namespace ofxAzureKinect JpegDecodeThread(); ~JpegDecodeThread(); + void start(); + void stop(); + bool pushTaskIfEmpty(JpegTask& b); bool update(ofPixels& outPix, std::chrono::microseconds& outTime); } decodeThread; From b7ea2d20b5aaefff61c1ecc223cf10b653634ca5 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Thu, 19 Nov 2020 13:42:40 +0900 Subject: [PATCH 38/48] add MultiDeviceSyncCapture::removeAllDevices --- src/ofxAzureKinect/Device.cpp | 16 ++++++++++++++++ src/ofxAzureKinect/Device.h | 2 ++ 2 files changed, 18 insertions(+) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 03026bb..34e78f7 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -1324,6 +1324,22 @@ namespace ofxAzureKinect subordinate_devices.push_back(p); } + void MultiDeviceSyncCapture::removeAllDevices() + { + if (master_device != nullptr) { + master_device->bMultiDeviceSyncCapture = false; + master_device->master_device_capture = nullptr; + master_device = nullptr; + } + + for (auto &p : subordinate_devices) + { + p->bMultiDeviceSyncCapture = false; + p->master_device_capture = nullptr; + } + subordinate_devices.clear(); + } + void MultiDeviceSyncCapture::start() { if (master_device == nullptr || subordinate_devices.empty()) { diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 5527435..ec3dced 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -308,6 +308,8 @@ namespace ofxAzureKinect void setMasterDevice(Device* p); void addSubordinateDevice(Device* p); + void removeAllDevices(); + void start(); void stop(); From 19354a6343779e1165c6ddac60ec642f266a00af Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Fri, 20 Nov 2020 00:15:40 +0900 Subject: [PATCH 39/48] dont wait when bAsyncJpegDecode --- src/ofxAzureKinect/Device.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 34e78f7..ceeb9cb 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -572,7 +572,7 @@ namespace ofxAzureKinect while (this->isThreadRunning()) { // During recording, do not wait for render thread, not to drop frames. - if (!this->bRecording) { + if (!this->bRecording || this->bAsyncJpegDecode) { std::unique_lock lock(this->mutex); while (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) { From ca43bf6be295fe7144fce87dc769896054d20b16 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 24 Nov 2020 18:22:09 +0900 Subject: [PATCH 40/48] fix playback --- src/ofxAzureKinect/Device.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index ceeb9cb..7ab9552 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -443,20 +443,22 @@ namespace ofxAzureKinect } // Check compatible sync mode and connection. - if (this->config.wired_sync_mode == K4A_WIRED_SYNC_MODE_MASTER && !this->isSyncOutConnected()) - { - ofLogWarning(__FUNCTION__) << "Wired sync mode set to Master but Sync Out not connected! Reverting to Standalone."; - this->config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE; - } - else if (this->config.wired_sync_mode == K4A_WIRED_SYNC_MODE_SUBORDINATE && !this->isSyncInConnected()) - { - ofLogWarning(__FUNCTION__) << "Wired sync mode set to Subordinate but Sync In not connected! Reverting to Standalone."; - this->config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE; - } + if (!bPlayback) { + if (this->config.wired_sync_mode == K4A_WIRED_SYNC_MODE_MASTER && !this->isSyncOutConnected()) + { + ofLogWarning(__FUNCTION__) << "Wired sync mode set to Master but Sync Out not connected! Reverting to Standalone."; + this->config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE; + } + else if (this->config.wired_sync_mode == K4A_WIRED_SYNC_MODE_SUBORDINATE && !this->isSyncInConnected()) + { + ofLogWarning(__FUNCTION__) << "Wired sync mode set to Subordinate but Sync In not connected! Reverting to Standalone."; + this->config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE; + } - if (this->config.wired_sync_mode != K4A_WIRED_SYNC_MODE_SUBORDINATE) - { - this->config.subordinate_delay_off_master_usec = 0; + if (this->config.wired_sync_mode != K4A_WIRED_SYNC_MODE_SUBORDINATE) + { + this->config.subordinate_delay_off_master_usec = 0; + } } // Start cameras. From a2066b933eaa9bca897d6ec9c27648a2e96953fc Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 24 Nov 2020 18:59:23 +0900 Subject: [PATCH 41/48] support color texture mapping on MJPG --- src/ofxAzureKinect/Device.cpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 7ab9552..9eaf198 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -782,11 +782,23 @@ namespace ofxAzureKinect } } - if (colorImg && this->bUpdateColor && this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) + if (colorImg && this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bRecording && !bAsyncJpegDecode))) { - // TODO: Fix this for non-BGRA formats, maybe always keep a BGRA k4a::image around. - this->updateDepthInColorFrame(depthImg, colorImg); - this->updateColorInDepthFrame(depthImg, colorImg); + if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) + { + // TODO: Fix this for non-BGRA formats, maybe always keep a BGRA k4a::image around. + this->updateDepthInColorFrame(depthImg, colorImg); + this->updateColorInDepthFrame(depthImg, colorImg); + } + else + { + k4a::image bgraColorImg = k4a::image::create_from_buffer( + K4A_IMAGE_FORMAT_COLOR_BGRA32, f.colorPix.getWidth(), f.colorPix.getHeight(), + f.colorPix.getBytesStride(), f.colorPix.getData(), f.colorPix.size(), nullptr, nullptr); + // TODO: Fix this for non-BGRA formats, maybe always keep a BGRA k4a::image around. + this->updateDepthInColorFrame(depthImg, bgraColorImg); + this->updateColorInDepthFrame(depthImg, bgraColorImg); + } } // Do any recording before releasing the capture @@ -878,7 +890,7 @@ namespace ofxAzureKinect this->pointCloudVbo.setTexCoordData(f.uvCache.data(), f.numPoints, GL_STREAM_DRAW); } - if (this->bUpdateColor && this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) + if (this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bRecording && !bAsyncJpegDecode))) { if (f.depthInColorPix.isAllocated()) { From e048da59c28a1ed1e1beed1096c30aada62e948d Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 24 Nov 2020 19:35:55 +0900 Subject: [PATCH 42/48] playback --- src/ofxAzureKinect/Device.cpp | 65 ++++++++++++++++++++++----------- src/ofxAzureKinect/Device.h | 10 +++++ src/ofxAzureKinect/Playback.cpp | 22 ++++++++--- src/ofxAzureKinect/Playback.h | 11 ++++++ 4 files changed, 81 insertions(+), 27 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index ceeb9cb..7fc2f6a 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -184,6 +184,8 @@ namespace ofxAzureKinect , bMultiDeviceSyncCapture(false) , bRecording(false) , bAsyncJpegDecode(false) + , bEnableAutoUpdate(true) + , bEnableThread(true) {} Device::~Device() @@ -483,7 +485,7 @@ namespace ofxAzureKinect } } - if (!bMultiDeviceSyncCapture) { + if (!bMultiDeviceSyncCapture && bEnableThread) { this->startThread(); } else { @@ -497,7 +499,9 @@ namespace ofxAzureKinect this->decodeThread.stop(); } - ofAddListener(ofEvents().update, this, &Device::update); + if (bEnableAutoUpdate) { + ofAddListener(ofEvents().update, this, &Device::update); + } this->bStreaming = true; @@ -514,7 +518,9 @@ namespace ofxAzureKinect this->stopThread(); this->condition.notify_all(); - ofRemoveListener(ofEvents().update, this, &Device::update); + if (bEnableAutoUpdate) { + ofRemoveListener(ofEvents().update, this, &Device::update); + } this->depthToWorldImg.reset(); this->colorToWorldImg.reset(); @@ -557,6 +563,39 @@ namespace ofxAzureKinect return true; } + void Device::update() + { + if (!bEnableThread && !bMultiDeviceSyncCapture) { + this->updatePixels(); + + if (this->lock()) { + this->frameBack.swapFrame(this->frameSwap); + this->bNewBuffer = true; + this->unlock(); + } + } + + this->bNewFrame = false; + + if (this->bNewBuffer) + { + if (this->decodeThread.isThreadRunning()) { + auto ret = this->decodeThread.update(this->frameSwap.colorPix, this->frameSwap.colorPixDeviceTime); + if (ret) { + this->frameSwap.bColorPixUpdated = true; + } + } + if (this->lock()) { + this->frameSwap.swapFrame(this->frameFront); + this->frameFront.bColorPixUpdated = false; + this->bNewBuffer = false; + this->unlock(); + } + this->updateTextures(); + this->condition.notify_all(); + } + } + bool Device::isSyncInConnected() const { return this->device.is_sync_in_connected(); @@ -593,25 +632,7 @@ namespace ofxAzureKinect void Device::update(ofEventArgs &args) { - this->bNewFrame = false; - - if (this->bNewBuffer) - { - if (this->decodeThread.isThreadRunning()) { - auto ret = this->decodeThread.update(this->frameSwap.colorPix, this->frameSwap.colorPixDeviceTime); - if (ret) { - this->frameSwap.bColorPixUpdated = true; - } - } - if (this->lock()) { - this->frameSwap.swapFrame(this->frameFront); - this->frameFront.bColorPixUpdated = false; - this->bNewBuffer = false; - this->unlock(); - } - this->updateTextures(); - this->condition.notify_all(); - } + this->update(); } void Device::updatePixels() diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index ec3dced..577875b 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -135,6 +135,8 @@ namespace ofxAzureKinect bool startCameras(DeviceSettings deviceSettings = DeviceSettings(), BodyTrackingSettings bodyTrackingSettings = BodyTrackingSettings()); bool stopCameras(); + void update(); + bool isSyncInConnected() const; bool isSyncOutConnected() const; @@ -187,6 +189,12 @@ namespace ofxAzureKinect bool isAsyncJpegDecode() const { return this->bAsyncJpegDecode; } void setAsyncJpegDecode(bool b) { this->bAsyncJpegDecode = b; } + + bool isEnableAutoUpdate() const { return this->bEnableAutoUpdate; } + void setEnableAutoUpdate(bool b) { this->bEnableAutoUpdate = b; } + + bool isEnableThread() const { return this->bEnableThread; } + void setEnableThread(bool b) { this->bEnableThread = b; } public: float getRecordingTimerDelay(); ofParameter play{"play", false}; @@ -227,6 +235,8 @@ namespace ofxAzureKinect bool bMultiDeviceSyncCapture; bool bAsyncJpegDecode; + bool bEnableAutoUpdate; + bool bEnableThread; std::condition_variable condition; uint64_t pixFrameNum; diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp index ca12149..509625e 100644 --- a/src/ofxAzureKinect/Playback.cpp +++ b/src/ofxAzureKinect/Playback.cpp @@ -20,7 +20,7 @@ namespace ofxAzureKinect return false; } - recording_length = k4a_playback_get_last_timestamp_usec(playback); + recording_length = k4a_playback_get_recording_length_usec(playback); printf("Recording is %lld seconds long\n", recording_length / 1000000); return true; @@ -79,6 +79,7 @@ namespace ofxAzureKinect void Playback::seek(float amt) { + b_seek_by_device_time = false; seek_head = amt; int play_head = int(ofMap(seek_head, 0, 1, 0, recording_length, true)); @@ -90,18 +91,29 @@ namespace ofxAzureKinect } } - void Playback::seek() + void Playback::seekByDeviceTime(uint32_t device_usec) { - int play_head = int(ofMap(seek_head, 0, 1, 0, recording_length, true)); + b_seek_by_device_time = true; + last_seek_device_usec = device_usec; - // Seek to 10 seconds from the start - if (k4a_playback_seek_timestamp(playback, play_head, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) + if (k4a_playback_seek_timestamp(playback, last_seek_device_usec, K4A_PLAYBACK_SEEK_DEVICE_TIME) != K4A_RESULT_SUCCEEDED) { ofLogError(__FUNCTION__) << "K4A_PLAYBACK_SEEK FAILED."; return; } } + void Playback::seek() + { + if (b_seek_by_device_time) { + this->seekByDeviceTime(last_seek_device_usec); + } + else + { + this->seek(this->seek_head); + } + } + void Playback::close() { status = STOPPED; diff --git a/src/ofxAzureKinect/Playback.h b/src/ofxAzureKinect/Playback.h index c5d2231..38d8dda 100644 --- a/src/ofxAzureKinect/Playback.h +++ b/src/ofxAzureKinect/Playback.h @@ -27,6 +27,7 @@ namespace ofxAzureKinect void seek(); void seek(float t); + void seekByDeviceTime(uint32_t device_usec); void play(); void pause(); @@ -37,6 +38,13 @@ namespace ofxAzureKinect bool isPaused() { return status == PAUSED; } bool isStopped() { return status == STOPPED; } + uint32_t getStartTimeOffsetUsec() const { + return config.start_timestamp_offset_usec; + } + uint32_t getRecordingLengthUsec() const { + return recording_length; + } + private: k4a_playback_t playback; k4a_capture_t capture; @@ -61,5 +69,8 @@ namespace ofxAzureKinect PLAYING }; Status status = STOPPED; + + bool b_seek_by_device_time = false; + uint32_t last_seek_device_usec = 0; }; } // namespace ofxAzureKinect \ No newline at end of file From 38f08a47c3b08f74f2563893ac65310ba37cdd37 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Tue, 24 Nov 2020 20:09:16 +0900 Subject: [PATCH 43/48] add getters for playback / record --- src/ofxAzureKinect/Device.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 577875b..f9d110f 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -202,6 +202,9 @@ namespace ofxAzureKinect ofParameter stop{"stop", false}; ofParameter seek{"Seek", 0.0f, 0.0f, 1.0f}; + Playback* getPlayback() {return this->playback; } + Record* getRecord() { return this->recording; } + protected: virtual void threadedFunction() override; From e406ea25920e5690e7f07787243b831b93ef14b0 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Thu, 17 Dec 2020 19:31:04 +0900 Subject: [PATCH 44/48] fix some variables to 64bit --- src/ofxAzureKinect/Playback.cpp | 4 ++-- src/ofxAzureKinect/Playback.h | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/ofxAzureKinect/Playback.cpp b/src/ofxAzureKinect/Playback.cpp index 509625e..5c8d686 100644 --- a/src/ofxAzureKinect/Playback.cpp +++ b/src/ofxAzureKinect/Playback.cpp @@ -81,7 +81,7 @@ namespace ofxAzureKinect { b_seek_by_device_time = false; seek_head = amt; - int play_head = int(ofMap(seek_head, 0, 1, 0, recording_length, true)); + int64_t play_head = int64_t(ofMap(seek_head, 0, 1, 0, recording_length, true)); // Seek to 10 seconds from the start if (k4a_playback_seek_timestamp(playback, play_head, K4A_PLAYBACK_SEEK_BEGIN) != K4A_RESULT_SUCCEEDED) @@ -91,7 +91,7 @@ namespace ofxAzureKinect } } - void Playback::seekByDeviceTime(uint32_t device_usec) + void Playback::seekByDeviceTime(int64_t device_usec) { b_seek_by_device_time = true; last_seek_device_usec = device_usec; diff --git a/src/ofxAzureKinect/Playback.h b/src/ofxAzureKinect/Playback.h index 38d8dda..6a3ebc8 100644 --- a/src/ofxAzureKinect/Playback.h +++ b/src/ofxAzureKinect/Playback.h @@ -27,7 +27,7 @@ namespace ofxAzureKinect void seek(); void seek(float t); - void seekByDeviceTime(uint32_t device_usec); + void seekByDeviceTime(int64_t device_usec); void play(); void pause(); @@ -38,10 +38,10 @@ namespace ofxAzureKinect bool isPaused() { return status == PAUSED; } bool isStopped() { return status == STOPPED; } - uint32_t getStartTimeOffsetUsec() const { + int64_t getStartTimeOffsetUsec() const { return config.start_timestamp_offset_usec; } - uint32_t getRecordingLengthUsec() const { + uint64_t getRecordingLengthUsec() const { return recording_length; } @@ -54,7 +54,7 @@ namespace ofxAzureKinect char *filename; bool k4a_failed = false; - int recording_length = 0; + uint64_t recording_length = 0; float seek_head = 0; float in = 0, out = 0; @@ -71,6 +71,6 @@ namespace ofxAzureKinect Status status = STOPPED; bool b_seek_by_device_time = false; - uint32_t last_seek_device_usec = 0; + int64_t last_seek_device_usec = 0; }; } // namespace ofxAzureKinect \ No newline at end of file From d8f4c3e3e8e043972bf5ce775fac96d9d9ced5a9 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Wed, 2 Nov 2022 18:57:20 +0900 Subject: [PATCH 45/48] some fix --- src/ofxAzureKinect/Device.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index f9d110f..51ad04b 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -58,7 +58,7 @@ namespace ofxAzureKinect class MultiDeviceSyncCapture; class Device - : ofThread + : public ofThread { protected: struct Frame @@ -135,7 +135,7 @@ namespace ofxAzureKinect bool startCameras(DeviceSettings deviceSettings = DeviceSettings(), BodyTrackingSettings bodyTrackingSettings = BodyTrackingSettings()); bool stopCameras(); - void update(); + virtual void update(); bool isSyncInConnected() const; bool isSyncOutConnected() const; From 5edc833f4b85f5064c1621874565e131d31adb18 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Fri, 11 Nov 2022 18:40:44 +0900 Subject: [PATCH 46/48] add IMU sample getter --- src/ofxAzureKinect/Device.cpp | 14 +++++++++++++- src/ofxAzureKinect/Device.h | 2 ++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index f6441fb..84c9d35 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -672,7 +672,13 @@ namespace ofxAzureKinect { try { - if (!this->device.get_capture(&this->capture, std::chrono::milliseconds(TIMEOUT_IN_MS))) + if (this->device.get_capture(&this->capture, std::chrono::milliseconds(TIMEOUT_IN_MS))) + { + if (bEnableIMU) { + this->device.get_imu_sample(&this->imu_sample); + } + } + else { ofLogWarning(__FUNCTION__) << "Timed out waiting for a capture for device " << this->index << "::" << this->serialNumber << "."; return; @@ -1422,10 +1428,16 @@ namespace ofxAzureKinect size_t current_index = 0; master_device->device.get_capture(&master_device->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + if (master_device->bEnableIMU) { + master_device->device.get_imu_sample(&master_device->imu_sample); + } ++current_index; for (auto &d : subordinate_devices) { d->device.get_capture(&d->capture, std::chrono::milliseconds{ K4A_WAIT_INFINITE }); + if (d->bEnableIMU) { + d->device.get_imu_sample(&d->imu_sample); + } ++current_index; } diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 51ad04b..3241ca4 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -195,6 +195,8 @@ namespace ofxAzureKinect bool isEnableThread() const { return this->bEnableThread; } void setEnableThread(bool b) { this->bEnableThread = b; } + + k4a_imu_sample_t getIMUSample() const { return imu_sample; } public: float getRecordingTimerDelay(); ofParameter play{"play", false}; From 6239af72b2692caa4473502833b4da7c9d623954 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Fri, 11 Nov 2022 22:30:22 +0900 Subject: [PATCH 47/48] fix IMU & thread deadlock --- src/ofxAzureKinect/Device.cpp | 58 +++++++++++++++++++++++++---------- src/ofxAzureKinect/Device.h | 8 ++++- 2 files changed, 48 insertions(+), 18 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 84c9d35..7541312 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -338,12 +338,6 @@ namespace ofxAzureKinect } else { - // Stop IMU if cameras are enabled - if (this->bEnableIMU) - { - k4a_device_stop_imu(device.handle()); - } - this->stopCameras(); this->device.close(); @@ -473,6 +467,12 @@ namespace ofxAzureKinect try { this->device.start_cameras(&this->config); + + // Can only start the IMU if cameras are enabled + if (this->bEnableIMU) + { + k4a_device_start_imu(device.handle()); + } } catch (const k4a::error &e) { @@ -480,11 +480,6 @@ namespace ofxAzureKinect return false; } - // Can only start the IMU if cameras are enabled - if (this->bEnableIMU) - { - k4a_device_start_imu(device.handle()); - } } if (!bMultiDeviceSyncCapture && bEnableThread) { @@ -515,9 +510,9 @@ namespace ofxAzureKinect if (!this->bStreaming) return false; - std::unique_lock lock(this->mutex); + std::unique_lock lock(this->timed_mtx); this->decodeThread.stop(); - this->stopThread(); + this->waitForThread(); this->condition.notify_all(); if (bEnableAutoUpdate) { @@ -542,6 +537,11 @@ namespace ofxAzureKinect } else { + // Stop IMU if cameras are enabled + if (this->bEnableIMU) + { + k4a_device_stop_imu(device.handle()); + } this->device.stop_cameras(); } @@ -615,14 +615,15 @@ namespace ofxAzureKinect // During recording, do not wait for render thread, not to drop frames. if (!this->bRecording || this->bAsyncJpegDecode) { std::unique_lock lock(this->mutex); - while (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) + if (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) { - this->condition.wait(lock); + this->condition.wait_for(lock, std::chrono::milliseconds(100)); } } + this->updatePixels(); - if (this->lock()) { + if (this->isThreadRunning() && this->tryLockFor(100)) { this->frameBack.swapFrame(this->frameSwap); this->bNewBuffer = true; this->unlock(); @@ -630,6 +631,7 @@ namespace ofxAzureKinect std::this_thread::sleep_for(std::chrono::microseconds(100)); } + cerr << "done thread : " << getThreadName() << endl; } void Device::update(ofEventArgs &args) @@ -1326,6 +1328,27 @@ namespace ofxAzureKinect return bRecording; } + bool Device::lock() + { + timed_mtx.lock(); + return true; + } + + bool Device::tryLock() + { + return timed_mtx.try_lock(); + } + + bool Device::tryLockFor(uint64_t millisec) + { + return timed_mtx.try_lock_for(std::chrono::milliseconds(millisec)); + } + + void Device::unlock() + { + timed_mtx.unlock(); + } + float Device::getRecordingTimerDelay() { if (recording != nullptr) @@ -1539,7 +1562,7 @@ namespace ofxAzureKinect device->updatePixels(); } for (auto& device : devices) { - if (device->lock()) { + if (device->tryLockFor(100)) { device->frameBack.swapFrame(device->frameSwap); device->bNewBuffer = true; device->unlock(); @@ -1548,5 +1571,6 @@ namespace ofxAzureKinect std::this_thread::sleep_for(std::chrono::microseconds(100)); } + std::cout << "Multi Device Sync Capture thread finished" << endl; } } // namespace ofxAzureKinect diff --git a/src/ofxAzureKinect/Device.h b/src/ofxAzureKinect/Device.h index 3241ca4..0825a35 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -197,6 +197,11 @@ namespace ofxAzureKinect void setEnableThread(bool b) { this->bEnableThread = b; } k4a_imu_sample_t getIMUSample() const { return imu_sample; } + + bool lock(); + bool tryLock(); + bool tryLockFor(uint64_t millisec); + void unlock(); public: float getRecordingTimerDelay(); ofParameter play{"play", false}; @@ -243,7 +248,8 @@ namespace ofxAzureKinect bool bEnableAutoUpdate; bool bEnableThread; - std::condition_variable condition; + std::timed_mutex timed_mtx; + std::condition_variable_any condition; uint64_t pixFrameNum; uint64_t texFrameNum; From 75ba67a841c36e2ce2a3b1f647525201076c6120 Mon Sep 17 00:00:00 2001 From: Yuya Hanai Date: Thu, 17 Nov 2022 16:10:51 +0900 Subject: [PATCH 48/48] do not force change async jpeg decoding mode when recording. (if you want to do so, set it to true manually) --- src/ofxAzureKinect/Device.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/ofxAzureKinect/Device.cpp b/src/ofxAzureKinect/Device.cpp index 7541312..25cd82c 100644 --- a/src/ofxAzureKinect/Device.cpp +++ b/src/ofxAzureKinect/Device.cpp @@ -612,8 +612,7 @@ namespace ofxAzureKinect { while (this->isThreadRunning()) { - // During recording, do not wait for render thread, not to drop frames. - if (!this->bRecording || this->bAsyncJpegDecode) { + if (this->bAsyncJpegDecode) { std::unique_lock lock(this->mutex); if (this->isThreadRunning() && this->texFrameNum != this->pixFrameNum) { @@ -730,8 +729,7 @@ namespace ofxAzureKinect if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_MJPG) { - // during recording, jpeg decode task is dispatched to another thread, not to drop recording frames. - if (this->bRecording || this->bAsyncJpegDecode) { + if (this->bAsyncJpegDecode) { JpegTask task; task.colorPixBuf.set((const char*)colorImg.get_buffer(), colorImg.get_size()); task.colorPixDeviceTime = colorImg.get_device_timestamp(); @@ -811,7 +809,7 @@ namespace ofxAzureKinect } } - if (colorImg && this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bRecording && !bAsyncJpegDecode))) + if (colorImg && this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bAsyncJpegDecode))) { if (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32) { @@ -919,7 +917,7 @@ namespace ofxAzureKinect this->pointCloudVbo.setTexCoordData(f.uvCache.data(), f.numPoints, GL_STREAM_DRAW); } - if (this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bRecording && !bAsyncJpegDecode))) + if (this->bUpdateColor && (this->config.color_format == K4A_IMAGE_FORMAT_COLOR_BGRA32 || (!bAsyncJpegDecode))) { if (f.depthInColorPix.isAllocated()) {