From f90a824ff5cb6dfda1f6c4fd0f170124444fe48f Mon Sep 17 00:00:00 2001 From: madelinegannon Date: Mon, 22 Jun 2020 17:22:27 -0400 Subject: [PATCH 01/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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/19] 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; };