diff --git a/addon_config.mk b/addon_config.mk index c9e14f5..73f6073 100644 --- a/addon_config.mk +++ b/addon_config.mk @@ -71,8 +71,10 @@ 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 + ADDON_LDFLAGS += -lk4arecord + linux: linuxarmv6l: 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; }; 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/bin/data/.gitkeep b/example-playback/bin/data/.gitkeep new file mode 100644 index 0000000..e69de29 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..e3e6f8b --- /dev/null +++ b/example-playback/src/ofApp.h @@ -0,0 +1,33 @@ +#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; + + string filename; + bool play = false; + float play_head = 0; +}; 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/bin/data/.gitkeep b/example-record/bin/data/.gitkeep new file mode 100644 index 0000000..e69de29 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..280ddde --- /dev/null +++ b/example-record/src/ofApp.cpp @@ -0,0 +1,171 @@ +#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); + + 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); +} + +//-------------------------------------------------------------- +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; + +}; diff --git a/src/ofxAzureKinect/BodyTracker.cpp b/src/ofxAzureKinect/BodyTracker.cpp new file mode 100644 index 0000000..7491f22 --- /dev/null +++ b/src/ofxAzureKinect/BodyTracker.cpp @@ -0,0 +1,315 @@ +#include "BodyTracker.h" + +namespace ofxAzureKinect +{ + + 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) + { + 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 + 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->skeletons.resize(num_bodies); + this->bodyIDs.resize(num_bodies); + 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->skeletons[i] = skeleton; + uint32_t id = k4abt_frame_get_body_id(bodyFrame, i); + this->bodyIDs[i] = id; + + skeleton_meshes.push_back(new ofVboMesh()); + skeleton_meshes[skeleton_meshes.size() - 1]->setMode(OF_PRIMITIVE_LINES); + } + + // 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); + } + + 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. + // 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 = skeletons[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 = skeleton_meshes[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); + skeleton_meshes[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); + } + + size_t BodyTracker::get_num_bodies() const + { + return num_bodies; + } + + const vector &BodyTracker::get_skeletons() const + { + return skeletons; + } + + const k4abt_skeleton_t &BodyTracker::get_skeleton(int i) const + { + return skeletons[i]; + } + + const ofPixels &BodyTracker::get_body_index_map_pix() const + { + return bodyIndexPix; + } + + const ofTexture &BodyTracker::get_body_index_map_tex() const + { + return bodyIndexTex; + } + +} // 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..ae210fe --- /dev/null +++ b/src/ofxAzureKinect/BodyTracker.h @@ -0,0 +1,63 @@ +#pragma once + +#include +#include + +#include "ofMain.h" + +#include "Types.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 do I get a seg fault when called in update?? + + 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(); + + 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); + + private: + k4abt_tracker_t tracker; + + size_t num_bodies = 0; + + bool active = false; + + std::vector skeletons; + std::vector bodyIDs; + + // Visualization + ofPixels bodyIndexPix; + ofTexture bodyIndexTex; + + 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 f6e4596..b55cd72 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() { @@ -56,6 +33,82 @@ 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 = playback_config.depth_track_enabled; + + // 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(string filename, BodyTrackingSettings bodyTrackingSettings) + { + bool result = open(filename); + + if (result) + { + 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); + + } + + return result; + } + bool Device::open(int idx) { return this->open(DeviceSettings(idx), BodyTrackingSettings()); @@ -74,6 +127,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 +152,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 +186,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; @@ -155,14 +209,28 @@ namespace ofxAzureKinect this->bUpdateVbo = deviceSettings.updateWorld && deviceSettings.updateVbo; this->bUpdateBodies = bodyTrackingSettings.updateBodies; - if (this->bUpdateBodies) - { - this->eventListeners.push(this->jointSmoothing.newListener([this](float &) + + if (bUpdateBodies){ + + try { - k4abt_tracker_set_temporal_smoothing(this->bodyTracker, this->jointSmoothing); - })); + 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); + })); + ofLogNotice(__FUNCTION__) << "Successfully opened device " << this->index << " with serial number " << this->serialNumber << "."; return true; @@ -170,11 +238,25 @@ namespace ofxAzureKinect bool Device::close() { - if (!this->bOpen) return false; + if (!this->bOpen) + return false; - this->stopCameras(); + if (bPlayback) + { + this->stopCameras(); + } + else + { + // Stop IMU if cameras are enabled + if (this->enableIMU) + { + k4a_device_stop_imu(device.handle()); + } - this->device.close(); + this->stopCameras(); + + this->device.close(); + } this->eventListeners.unsubscribeAll(); @@ -194,14 +276,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) @@ -246,14 +345,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()); + } } this->startThread(); @@ -266,7 +378,8 @@ namespace ofxAzureKinect bool Device::stopCameras() { - if (!this->bStreaming) return false; + if (!this->bStreaming) + return false; std::unique_lock lock(this->mutex); this->stopThread(); @@ -284,7 +397,15 @@ namespace ofxAzureKinect this->bodyTracker = nullptr; } - this->device.stop_cameras(); + if (bPlayback) + { + playback->stop(); + playback->close(); + } + else + { + this->device.stop_cameras(); + } this->bStreaming = false; @@ -316,7 +437,7 @@ namespace ofxAzureKinect } } - void Device::update(ofEventArgs& args) + void Device::update(ofEventArgs &args) { this->bNewFrame = false; @@ -333,18 +454,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()) + { + 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()) { - ofLogWarning(__FUNCTION__) << "Timed out waiting for a capture for device " << this->index << "::" << this->serialNumber << "."; + 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. @@ -357,7 +507,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 +533,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 +569,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() << "."; @@ -432,56 +582,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(colorImg, this->colorToWorldImg); + this->updatePointsCache(depthImg, this->depthToWorldImg); //(colorImg, this->colorToWorldImg); } else { @@ -496,6 +604,13 @@ namespace ofxAzureKinect this->updateColorInDepthFrame(depthImg, colorImg); } + // Do any recording before releasing the capture + if (bRecord) + { + k4a_capture_t capture_handle = capture.handle(); + recording->record(&capture_handle); + } + // Release images. depthImg.reset(); colorImg.reset(); @@ -561,13 +676,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) @@ -656,9 +765,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 +776,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 +818,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 +828,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 +847,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 +861,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 +869,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 +896,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 +904,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 +923,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,108 +946,123 @@ 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 ofVbo &Device::getPointCloudVbo() const { - return this->bodyIndexPix; + return this->pointCloudVbo; } - const ofTexture& Device::getBodyIndexTex() const + void Device::handle_recording(bool val) { - return this->bodyIndexTex; + if (val) + { + recording = new Record(); + recording->setup(device.handle(), this->config, enableIMU); + recording->start(); + } + else + { + recording->stop(); + } } - size_t Device::getNumBodies() const + float Device::get_recording_timer_delay() { - return this->bodySkeletons.size(); + if (recording != nullptr) + return recording->get_timer_delay(); + else + return -1; } - const std::vector& Device::getBodySkeletons() const + void Device::listener_playback_play(bool val) { - return this->bodySkeletons; + playback->play(); } - - const std::vector& Device::getBodyIDs() const + void Device::listener_playback_pause(bool val) { - return this->bodyIDs; + playback->pause(); } - - const ofVbo& Device::getPointCloudVbo() const + void Device::listener_playback_stop(bool val) { - return this->pointCloudVbo; + 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 7dc0372..882874d 100644 --- a/src/ofxAzureKinect/Device.h +++ b/src/ofxAzureKinect/Device.h @@ -15,6 +15,9 @@ #include "ofVectorMath.h" #include "Types.h" +#include "Record.h" +#include "Playback.h" +#include "BodyTracker.h" namespace ofxAzureKinect { @@ -38,6 +41,8 @@ namespace ofxAzureKinect bool syncImages; + bool enableIMU; + DeviceSettings(int idx = 0); }; @@ -52,7 +57,7 @@ namespace ofxAzureKinect BodyTrackingSettings(); }; - class Device + class Device : ofThread { public: @@ -62,6 +67,8 @@ namespace ofxAzureKinect Device(); ~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); @@ -77,40 +84,40 @@ namespace ofxAzureKinect bool isStreaming() const; bool isFrameNew() const; - const std::string& getSerialNumber() const; - - const ofShortPixels& getDepthPix() const; - const ofTexture& getDepthTex() const; + const std::string &getSerialNumber() const; - const ofPixels& getColorPix() const; - const ofTexture& getColorTex() const; + const ofShortPixels &getDepthPix() const; + const ofTexture &getDepthTex() const; - const ofShortPixels& getIrPix() const; - const ofTexture& getIrTex() const; + const ofPixels &getColorPix() const; + const ofTexture &getColorTex() const; - const ofFloatPixels& getDepthToWorldPix() const; - const ofTexture& getDepthToWorldTex() const; + const ofShortPixels &getIrPix() const; + const ofTexture &getIrTex() const; - const ofFloatPixels& getColorToWorldPix() const; - const ofTexture& getColorToWorldTex() const; + const ofFloatPixels &getDepthToWorldPix() const; + const ofTexture &getDepthToWorldTex() const; - const ofShortPixels& getDepthInColorPix() const; - const ofTexture& getDepthInColorTex() const; + const ofFloatPixels &getColorToWorldPix() const; + const ofTexture &getColorToWorldTex() const; - const ofPixels& getColorInDepthPix() const; - const ofTexture& getColorInDepthTex() const; + const ofShortPixels &getDepthInColorPix() const; + const ofTexture &getDepthInColorTex() const; - const ofPixels& getBodyIndexPix() const; - const ofTexture& getBodyIndexTex() const; + const ofPixels &getColorInDepthPix() const; + const ofTexture &getColorInDepthTex() const; - size_t getNumBodies() const; - const std::vector& getBodySkeletons() const; - const std::vector& getBodyIDs() const; + const ofVbo &getPointCloudVbo() 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}; + ofParameter pause{"pause", false}; + ofParameter stop{"stop", false}; + ofParameter seek{"Seek", 0.0f, 0.0f, 1.0f}; protected: void threadedFunction() override; @@ -119,21 +126,22 @@ 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; bool bOpen; bool bStreaming; + bool bPlayback; bool bUpdateColor; bool bUpdateIr; @@ -160,6 +168,10 @@ namespace ofxAzureKinect tjhandle jpegDecompressor; + k4a_imu_sample_t imu_sample; + bool enableIMU = false; + void startIMU(); + ofShortPixels depthPix; ofTexture depthTex; @@ -194,5 +206,16 @@ namespace ofxAzureKinect ofVbo pointCloudVbo; ofEventListeners eventListeners; + + BodyTracker tracker; + + 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); }; -} \ No newline at end of file +} // 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..65f28c0 --- /dev/null +++ b/src/ofxAzureKinect/Playback.cpp @@ -0,0 +1,154 @@ +#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(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::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() + { + status = STOPPED; + 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..18bf85f --- /dev/null +++ b/src/ofxAzureKinect/Playback.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include +#include + +#include "ofMain.h" + + +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 diff --git a/src/ofxAzureKinect/Record.cpp b/src/ofxAzureKinect/Record.cpp new file mode 100644 index 0000000..cc270d2 --- /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(); + } + + // 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..16e708f --- /dev/null +++ b/src/ofxAzureKinect/Record.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include + +#include "ofMain.h" + +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