From e6d55da700af0d8b466f1a625966769a5a35cbd0 Mon Sep 17 00:00:00 2001 From: Urs Hofer Date: Mon, 26 May 2014 17:17:22 +0200 Subject: [PATCH 1/4] Update ofxOpenNI.cpp - Exporting skeletonData, imageData and depthData - Calculating Speed of Joints to do motion recognition - Supporting multiple Devices - Set Smoothing Factor on Startup --- src/ofxOpenNI.cpp | 158 ++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 152 insertions(+), 6 deletions(-) diff --git a/src/ofxOpenNI.cpp b/src/ofxOpenNI.cpp index e73b23d..78a2bcc 100644 --- a/src/ofxOpenNI.cpp +++ b/src/ofxOpenNI.cpp @@ -32,6 +32,10 @@ ofxOpenNI::ofxOpenNI(){ stop(); CreateRainbowPallet(); + _motion_detection_rate = 1000/MOTION_DETECTION_FPS; + _motion_detection_count = ceil(MOTION_DETECTION_CACHE_LENGTH_MS / (1000/MOTION_DETECTION_FPS)); + _md_threshold_motion = 0.5f; + _md_threshold_nomotion = 2.0f; } //-------------------------------------------------------------- @@ -39,8 +43,17 @@ ofxOpenNI::~ofxOpenNI(){ stop(); } +void ofxOpenNI::setMotionThresholds(float _threshold_motion, float _threshold_nomotion, float _cache_length = 0) { + ofScopedLock lock(mutex); + _md_threshold_motion = _threshold_motion; + _md_threshold_nomotion = _threshold_nomotion; + if (_cache_length == 0) _cache_length = MOTION_DETECTION_CACHE_LENGTH_MS; + _motion_detection_count = ceil(MOTION_DETECTION_CACHE_LENGTH_MS / (1000/MOTION_DETECTION_FPS)); +} + + //-------------------------------------------------------------- -bool ofxOpenNI::setup(){ +bool ofxOpenNI::setup(int _deviceid){ openni::Status rc = OpenNI::initialize(); if (rc != openni::STATUS_OK) { @@ -48,8 +61,25 @@ bool ofxOpenNI::setup(){ bUseDevice = false; } - const char* deviceUri = ANY_DEVICE; + /* Print Device List */ + openni::Array deviceInfoList; + OpenNI::enumerateDevices(&deviceInfoList); + ofLogNotice() << "***************************************************"; + ofLogNotice() << "* Available Devices *"; + ofLogNotice() << "***************************************************"; + + for(int i = 0; i < deviceInfoList.getSize(); ++i){ + ofLogNotice() << "Device " << ofToString(i) << " (" << deviceInfoList[i].getName() << "): " << deviceInfoList[i].getUri(); + ofLogNotice() << " Vendor: " << deviceInfoList[i].getVendor() << deviceInfoList[i].getUsbVendorId() << " / " << ofToString(deviceInfoList[i].getUsbProductId()); + + } + ofLogNotice() << "***************************************************"; + + const char* deviceUri = ANY_DEVICE; + if (_deviceid >= 0) { + deviceUri = deviceInfoList[_deviceid].getUri(); + } rc = device.open(deviceUri); if (rc != openni::STATUS_OK) { ofLogError() << "Failed to open device:" << OpenNI::getExtendedError(); @@ -59,7 +89,6 @@ bool ofxOpenNI::setup(){ device.setDepthColorSyncEnabled(true); bUseDevice = true; } - return bUseDevice; } @@ -140,7 +169,7 @@ bool ofxOpenNI::addImageStream(){ } //-------------------------------------------------------------- -bool ofxOpenNI::addUserTracker(){ +bool ofxOpenNI::addUserTracker(float _smoothing){ ofScopedLock lock(mutex); @@ -165,7 +194,7 @@ bool ofxOpenNI::addUserTracker(){ }else{ ofLogNotice() << "Succeded to add user tracker"; - userTracker.setSkeletonSmoothingFactor(0.3); + userTracker.setSkeletonSmoothingFactor(_smoothing); bUseUsers = true; } @@ -343,7 +372,10 @@ void ofxOpenNI::updateImageFrame(){ void ofxOpenNI::updateUserFrame(){ if(userFrame.isValid()){ const nite::Array& users = userFrame.getUsers(); + for(int i = 0; i < users.getSize(); ++i){ + + const UserData& user = users[i]; if(user.isNew()){ @@ -353,6 +385,7 @@ void ofxOpenNI::updateUserFrame(){ trackedUsers[user.getId()] = u; trackedUsers[user.getId()].setUserID(user.getId()); } + switch (user.getSkeleton().getState()) { case nite::SKELETON_TRACKED: @@ -360,10 +393,14 @@ void ofxOpenNI::updateUserFrame(){ //ofLogNotice() << "Skeleton Tracking: " << user.getId(); ofxOpenNIUser& u = trackedUsers[user.getId()]; u.bIsTracked = true; + float timestamp = ofGetElapsedTimeMillis(); vector& joints = u.getJoints(); float totalConfidence = 0.0f; for(int i = 0; i < joints.size(); i++){ ofxOpenNIJoint& joint = joints[i]; + + u.centerOfMass = toOf(user.getCenterOfMass()); + nite::Point3f position = user.getSkeleton().getJoint((nite::JointType)i).getPosition(); joint.positionReal = toOf(position); ofPoint p; @@ -373,7 +410,94 @@ void ofxOpenNI::updateUserFrame(){ totalConfidence += joint.positionConfidence; joint.orientation = toOf(user.getSkeleton().getJoint((nite::JointType)i).getOrientation()); joint.orientationConfidence = user.getSkeleton().getJoint((nite::JointType)i).getOrientationConfidence(); + + /* Caching Position, every 100ms */ + + if (joint.activateMotionDetection) { + + + if (timestamp - joint.lastMeasureTimeStamp >= _motion_detection_rate) { + + + joint.lastMeasureTimeStamp = timestamp; + + float _delta = (joint.pointCache.size()>0?p.distance(joint.pointCache.front().positionProjective):0) / 10; + joint.totalDistance += _delta; + joint.pointCache.push_front((ofxOpenNIJoint::_queue) { + .distanceMoved = _delta, + .positionProjective = p + }); + + + /* Limit Length to max 50 Points */ + if (joint.pointCache.size()>_motion_detection_count) { + + /* if (i==6) { + cout << "Tracking Active Point " << ofToString(i) << ": Distance: " << ofToString(joint.motionCache) << " / Speed: " << ofToString(joint.averageSpeed) << " / Current Distance: " << ofToString(joint.motionShortCache) << " / Current Speed: " << ofToString(joint.currentSpeed) << " / Count: " << ofToString(_motion_detection_count) << " / " << ofToString(ceil(_motion_detection_count/5)) << "\n"; + } + */ + + joint.pointCache.resize(_motion_detection_count); + + /* Calculate total and average */ + + + joint.motionCache = + // joint.averageSpeed = + // joint.motionShortCache = + joint.currentSpeed = 0.0f; + + int _index = 0; + //int _motion_detection_short_count = ceil(_motion_detection_count/10); + for (std::list::iterator _cache = joint.pointCache.begin(); _cache != joint.pointCache.end(); _cache++, _index++) { + // if (_index < _motion_detection_short_count) { + // joint.motionShortCache += _cache->distanceMoved; + // } + // else { + joint.motionCache += _cache->distanceMoved; + // } + } + //joint.currentSpeed = joint.motionShortCache / _motion_detection_short_count * _motion_detection_rate; + //joint.averageSpeed = joint.motionCache / (_motion_detection_count-_motion_detection_short_count) * _motion_detection_rate; + joint.currentSpeed = joint.motionCache / _motion_detection_count * _motion_detection_rate; + + + /* Motion Detection: Still */ + if (joint.currentSpeed < _md_threshold_nomotion) + { + joint.hasMotion = false; + if (joint.detectTime==0.0f) { + joint.detectTime = timestamp; + } + } + /* Motion Detection: Move */ + + else if (joint.motionCache > _md_threshold_motion ) { + joint.hasMotion = true; + joint.detectTime = 0.0f; + } + + /* Motion Detection: Move / + + if (joint.motionShortCache > _md_threshold_motion ) { + joint.hasMotion = true; + joint.detectTime = 0.0f; + } + / Motion Detection: Still / + else if (joint.averageSpeed > _md_threshold_nomotion_long && joint.currentSpeed < _md_threshold_nomotion_current) + { + joint.hasMotion = false; + if (joint.detectTime==0.0f) { + joint.detectTime = timestamp; + } + }*/ + } + } + } } + + u.box = user.getBoundingBox(); + //cout << totalConfidence/joints.size() << endl; if(totalConfidence == 0){ u.resetSkeleton(); @@ -415,6 +539,7 @@ void ofxOpenNI::updateUserFrame(){ trackedUsers.erase(trackedUsers.find(user.getId())); } } + } } @@ -478,6 +603,27 @@ void ofxOpenNI::threadedFunction(){ } } +//-------------------------------------------------------------- + +map ofxOpenNI::skeletonData() { + ofScopedLock lock(mutex); + return trackedUsers; +} + +//-------------------------------------------------------------- + +ofTexture & ofxOpenNI::depthData() { + return depthTexture; +} + +//-------------------------------------------------------------- + +ofTexture & ofxOpenNI::imageData() { + return imageTexture; +} + + + //-------------------------------------------------------------- void ofxOpenNI::draw(){ if(!bUseDevice) return; @@ -510,4 +656,4 @@ void ofxOpenNI::draw(){ } ofSetColor(255, 255, 255); -} \ No newline at end of file +} From 7b720ff7e7d92a8373c3c951bb09db4ac70f9fc1 Mon Sep 17 00:00:00 2001 From: Urs Hofer Date: Mon, 26 May 2014 17:18:32 +0200 Subject: [PATCH 2/4] Update ofxOpenNI.h see commit changes of ofxOpenNI.cpp --- src/ofxOpenNI.h | 35 ++++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/ofxOpenNI.h b/src/ofxOpenNI.h index f0b6574..d976401 100644 --- a/src/ofxOpenNI.h +++ b/src/ofxOpenNI.h @@ -35,6 +35,9 @@ #include "ofxOpenNIUtils.h" #include "ofxOpenNITypes.h" +#define MOTION_DETECTION_FPS 10 // FPS of motion detecion capturing +#define MOTION_DETECTION_CACHE_LENGTH_MS 500 // Cache length in ms to calculate average speed + using namespace openni; using namespace nite; @@ -45,7 +48,8 @@ class ofxOpenNI : public ofThread { ofxOpenNI(); ~ofxOpenNI(); - bool setup(); + + bool setup(int _deviceid = -1); void start(); void stop(); @@ -53,7 +57,7 @@ class ofxOpenNI : public ofThread { bool addDepthStream(); bool addImageStream(); // bool addInfraGenerator(); - bool addUserTracker(); + bool addUserTracker(float _smoothing = 0.6); // bool addGestureGenerator(); bool addHandsTracker(); // bool addAudioGenerator(); @@ -72,6 +76,16 @@ class ofxOpenNI : public ofThread { bool isDepthFrameNew(); bool isImageFrameNew(); + map skeletonData(); + ofTexture& depthData(); + ofTexture& imageData(); + + bool bUseDepth; + bool bUseImage; + + void setMotionThresholds(float _threshold_motion, float _threshold_nomotion, float _cache_length); + + protected: void updateGenerators(); @@ -117,11 +131,10 @@ class ofxOpenNI : public ofThread { bool bIsDepthFrameNew; bool bIsImageFrameNew; - bool bUseDevice; bool bUseNite; - - bool bUseDepth; - bool bUseImage; + bool bUseDevice; + + bool bUseInfra; bool bUseUsers; bool bUseGesture; @@ -131,6 +144,14 @@ class ofxOpenNI : public ofThread { bool bUseRecord; bool bUsePlayer; + + /* Motion Detection */ + float _motion_detection_rate; + int _motion_detection_count; + float _md_threshold_motion; + float _md_threshold_nomotion; +// float _md_threshold_nomotion_current; + }; -#endif \ No newline at end of file +#endif From d44a81e7e4a0428354cddc3715dfe0ff1339ae1a Mon Sep 17 00:00:00 2001 From: Urs Hofer Date: Mon, 26 May 2014 17:19:32 +0200 Subject: [PATCH 3/4] Update ofxOpenNITypes.h --- src/ofxOpenNITypes.h | 54 ++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 52 insertions(+), 2 deletions(-) diff --git a/src/ofxOpenNITypes.h b/src/ofxOpenNITypes.h index 0be3479..a329e83 100644 --- a/src/ofxOpenNITypes.h +++ b/src/ofxOpenNITypes.h @@ -54,6 +54,25 @@ class ofxOpenNIJoint { ofQuaternion orientation; float orientationConfidence; + + /* Point and Motion Caching - added by Hofer */ + + struct _queue + { + float distanceMoved; + ofPoint positionProjective; + }; + + std::list < _queue > pointCache; + float motionCache; +// float averageSpeed; +// float motionShortCache; + float currentSpeed; + float totalDistance; + bool hasMotion; + bool activateMotionDetection; + float lastMeasureTimeStamp; + float detectTime; }; class ofxOpenNIUser { @@ -87,10 +106,23 @@ class ofxOpenNIUser { joints[i].positionProjective = ofPoint(0,0,0); joints[i].orientation = ofQuaternion(0,0,0,0); joints[i].orientationConfidence = 0.0f; + joints[i].pointCache.clear(); +// joints[i].averageSpeed = 0.0f; + joints[i].motionCache = 0.0f; +// joints[i].motionShortCache = 0.0f; + joints[i].currentSpeed = 0.0f; + joints[i].detectTime = 0.0f; + joints[i].totalDistance = 0.0f; + + joints[i].hasMotion = true; + joints[i].activateMotionDetection = false; + for(int x = 0; x < MOTION_DETECTION; x++){ + if (_motion_detection[x]==i) joints[i].activateMotionDetection = true; + } } bIsTracked = false; } - + bool isTracking(){ return bIsTracked; } @@ -102,6 +134,10 @@ class ofxOpenNIUser { vector& getJoints(){ return joints; } + + ofPoint& getCenterOfMass(){ + return centerOfMass; + } void draw(){ ofPushMatrix(); @@ -119,15 +155,29 @@ class ofxOpenNIUser { ofPopMatrix(); } + //-------------------------------------------------------------- + + nite::BoundingBox& skeletonBox() { + return box; + } + protected: friend class ofxOpenNI; vector joints; + nite::BoundingBox box; int userID; bool bIsTracked; bool bIsVisible; + ofPoint centerOfMass; + +// int MOTION_DETECTION = 4; + // float _motion_detection[4] = {6,7,13,14}; + int MOTION_DETECTION = 10; + float _motion_detection[10] = {2,3,4,5,6,7,11,12,13,14}; + }; @@ -205,4 +255,4 @@ class ofxOpenNIHand { }; -#endif \ No newline at end of file +#endif From 6e96c3b7cc80f01fb491bbec0acf610341c00dba Mon Sep 17 00:00:00 2001 From: Urs Hofer Date: Mon, 26 May 2014 17:21:18 +0200 Subject: [PATCH 4/4] Update README.markdown --- README.markdown | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.markdown b/README.markdown index 9b4d8e9..c369a3c 100755 --- a/README.markdown +++ b/README.markdown @@ -1,5 +1,7 @@ OFXOPENNI ========= -This is ofxOpenNI using OpenNI2 and NITE2 libraries. - -It's highly experimental. Currently only tested with PrimeSense Carmine 1.08x sensor on MacOSX 10.8.4 with of007x. \ No newline at end of file +This is ofxOpenNI using OpenNI2 and NITE2 libraries. + +It's highly experimental. Currently only tested with PrimeSense Carmine 1.08x sensor on MacOSX 10.8.4 with of007x. + +This fork incoorporates motion measurements per joint and was used for an installation at Bauhaus Dessau.