diff --git a/src/PointCloudManager.cpp b/src/PointCloudManager.cpp index 24ca684e2fc232be72d861737d28b6d46455526c..ba80aee1ad8f34c4cb1f24b0c0758c2258233415 100644 --- a/src/PointCloudManager.cpp +++ b/src/PointCloudManager.cpp @@ -2,19 +2,8 @@ void PointCloudManager::drawPointCloud() { - //ofLog(OF_LOG_NOTICE) << "Drawing cloud point (" << pointCloud.getVertices().size() << ") points)"; - ofPushMatrix(); ofNoFill(); pointCloud.draw(); - auto vs = pointCloud.getVertices(); - if (vs.size() > 0) { - auto v = vs[12000]; - ofLog(OF_LOG_NOTICE) << "Color at vertex 12000 (of " << vs.size() << "): \n" - << "R " << v.r << "\n" - << "G " << v.g << "\n" - << "B " << v.b; - } - ofPopMatrix(); } void PointCloudManager::drawRGB(const ofRectangle& viewRect) { @@ -68,7 +57,7 @@ void PointCloudManager::updateDepth(DepthFrame::Ptr data) const unsigned short d = depthData[index]; Vector3 v = depthSensor->convertProjToRealCoords(x, y, depthData[index]); - pointCloud.setVertex(skippedIndex, ofxnui::Tracker::fromVector3(v) * 0.001); + pointCloud.setVertex(skippedIndex, ofxnui::Tracker::fromVector3(v)); } } } diff --git a/src/SkeletonFinder.cpp b/src/SkeletonFinder.cpp index 8c1a78a27f1c48b3a6c639013858c8b5f186f512..ff237cbfe71ad5891776adcfed2eb57c5835180a 100644 --- a/src/SkeletonFinder.cpp +++ b/src/SkeletonFinder.cpp @@ -1,7 +1,7 @@ // // SkeletonFinder.cpp // -// Created by Pierre Bürki on 19.05.20. +// Created by Pierre B�rki on 19.05.20. // Adapted from BlobFinder.cpp by maybites (14.02.14). // @@ -14,8 +14,6 @@ void SkeletonFinder::initGUI(ofxGui& gui) { panel->loadTheme("theme/theme_light.json"); panel->setName("Tracking..."); - // TODO: consider adding nuitrack params - sensorBoxLeft.addListener(this, &SkeletonFinder::updateSensorBox); sensorBoxRight.addListener(this, &SkeletonFinder::updateSensorBox); sensorBoxFront.addListener(this, &SkeletonFinder::updateSensorBox); @@ -24,6 +22,7 @@ void SkeletonFinder::initGUI(ofxGui& gui) { sensorBoxBottom.addListener(this, &SkeletonFinder::updateSensorBox); sensorBoxGuiGroup = panel->addGroup("SensorBox"); + sensorBoxGuiGroup->add(filtering.set("Filtering", true)); sensorBoxGuiGroup->add(sensorBoxLeft.set("left", 1000)); sensorBoxGuiGroup->add(sensorBoxRight.set("right", -1000)); sensorBoxGuiGroup->add(sensorBoxFront.set("front", 1000)); @@ -45,17 +44,22 @@ void SkeletonFinder::update(nuitrack::SkeletonData::Ptr data) { vector joints; for (nuitrack::Joint joint : skel.joints) { glm::vec3 pos = ofxnui::Tracker::fromVector3(joint.real); - // pos = *transformMatrix * pos; - pos = 0.001 * pos; + + // ofMatrix multiplication works in reverse + pos = (ofVec3f)pos * *transformMatrix; joints.push_back(Joint(joint.type, joint.confidence, pos)); } - skeletons.push_back(Skeleton(skel.id, joints)); + Skeleton skeleton(skel.id, joints); + if (!filtering.get() || isSkeletonInBounds(skeleton)) { + skeletons.push_back(skeleton); + } + } } -vector SkeletonFinder::getSkeletons() { +vector SkeletonFinder::getSkeletons() const { return skeletons; } @@ -121,6 +125,22 @@ void SkeletonFinder::drawSkeletons() { } } +string SkeletonFinder::getShortDesc() +{ + if (skeletons.size() == 0) { + return "No skeleton found"; + } else { + ostringstream s; + Skeleton skel = skeletons[0]; + auto pos = skel.joints[nuitrack::JOINT_HEAD].pos; + s << "Head position : (" << + pos.x << ", " << + pos.y << ", " << + pos.z << ")"; + return s.str(); + } +} + void SkeletonFinder::updateSensorBox(int& value) { sensorBox.clear(); sensorBox.setMode(OF_PRIMITIVE_LINES); @@ -153,3 +173,13 @@ void SkeletonFinder::updateSensorBox(int& value) { //captureCam.setPosition(5, 5, 0); //captureCam. } + +bool SkeletonFinder::isSkeletonInBounds(const Skeleton& skel) { + glm::vec3 headPos = skel.joints[nuitrack::JOINT_HEAD].pos; + return headPos.x < sensorBoxLeft.get() * SCALE + && headPos.x > sensorBoxRight.get() * SCALE + && headPos.y < sensorBoxFront.get()* SCALE + && headPos.y > sensorBoxBack.get() * SCALE + && headPos.z < sensorBoxTop.get()* SCALE + && headPos.z > sensorBoxBottom.get() * SCALE; +} diff --git a/src/SkeletonFinder.h b/src/SkeletonFinder.h index ca3889621f6bfa504ffeed11d783f5935d1bffa5..6b65e07c2fdecf33e693ca4a5a923049c4278e8d 100644 --- a/src/SkeletonFinder.h +++ b/src/SkeletonFinder.h @@ -58,21 +58,21 @@ struct Bone { class SkeletonFinder { public: - SkeletonFinder() {} - void initGUI(ofxGui& gui); void setTransformMatrix(ofMatrix4x4* mat); void update(nuitrack::SkeletonData::Ptr data); - - void updateSensorBox(int & value); void drawSensorBox(); void drawSkeletons2d(ofRectangle _rect); void drawSkeletons(); - vector getSkeletons(); + string getShortDesc(); + + vector getSkeletons() const; private: + void updateSensorBox(int & value); + bool isSkeletonInBounds(const Skeleton& skel); ofxnui::TrackerRef tracker; vector skeletons; @@ -83,6 +83,7 @@ public: ofxGuiPanel *panel; ofxGuiGroup *sensorBoxGuiGroup; + ofParameter filtering; ofParameter sensorBoxLeft; ofParameter sensorBoxRight; ofParameter sensorBoxTop; diff --git a/src/TrackingNetworkManager.cpp b/src/TrackingNetworkManager.cpp index c146ab8c25312d4233d6928d00ba158ac6c2a126..d7aad497b3089750b544c200624ff1f199b20da7 100644 --- a/src/TrackingNetworkManager.cpp +++ b/src/TrackingNetworkManager.cpp @@ -8,45 +8,27 @@ #include "TrackingNetworkManager.h" -TrackingNetworkManager::TrackingNetworkManager(){ -} - - -void TrackingNetworkManager::setup(ofxGui &gui, string _kinectSerial){ - mDeviceSerial = _kinectSerial; - //RegularExpression regEx("\\b\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\b"); +void TrackingNetworkManager::setup(ofxGui &gui, string _realsenseSerial){ + mDeviceSerial = _realsenseSerial; string localAddress = "127.0.0.1"; for(int i = 0; i < localIpAddresses.size(); i++){ - ofLog(OF_LOG_NOTICE, "try to find local ip addresses.. not sure if this function works properly..."); + ofLog(OF_LOG_NOTICE) << "try to find local ip addresses.. not sure if this function works properly..."; if(matchesInRegex(localIpAddresses[i], "\\b^(?:(?!127).)+\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\b").size() == 1) { localAddress = localIpAddresses[i]; - ofLog(OF_LOG_NOTICE, "found valid address" + localAddress); - // broadcastAddress = serverAddress.substr(0, serverAddress.find_last_of(".") + 1 ) + "255"; + ofLog(OF_LOG_NOTICE) << "found valid address" << localAddress; } } panel = gui.addPanel(); - panel->loadTheme("theme/theme_light.json"); - panel->setName("Broadcasting.."); + panel->setName("Broadcasting"); panel->add(mServerID.set("ServerID", 0, 0, 10)); - - streamingBodyBlob.addListener(this, &TrackingNetworkManager::listenerBool); - streamingHeadBlob.addListener(this, &TrackingNetworkManager::listenerBool); - streamingHead.addListener(this, &TrackingNetworkManager::listenerBool); - streamingEye.addListener(this, &TrackingNetworkManager::listenerBool); - - broadcastIP.addListener(this, &TrackingNetworkManager::listenerString); - broadcastPort.addListener(this, &TrackingNetworkManager::listenerInt); - listeningIP.addListener(this, &TrackingNetworkManager::listenerString); - listeningPort.addListener(this, &TrackingNetworkManager::listenerInt); broadcastGroup = panel->addGroup("Broadcast TX"); - //panel->add(broadcastLabel.set("Broadcast")); broadcastGroup->add(broadcastIP.set("TX IP","127.0.0.1")); broadcastGroup->add(broadcastPort.set("TX Port", NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT + 99)); @@ -56,48 +38,18 @@ void TrackingNetworkManager::setup(ofxGui &gui, string _kinectSerial){ streamingGuiGroup.setName("Streaming"); - //streamingGuiGroup.add(streamingBodyBlob.set("bodyBlob", true)); - streamingGuiGroup.add(streamingHeadBlob.set("headBlob", true)); - //streamingGuiGroup.add(streamingHead.set("head", true)); - //streamingGuiGroup.add(streamingEye.set("eye", true)); + streamingGuiGroup.add(streamingWholeBody.set("Whole body", true)); panel->addGroup(streamingGuiGroup); panel->loadFromFile("broadcast.xml"); - - //Server side - //listen for incoming messages on a port; setup OSC receiver with usage: - serverReceiver.setup(listeningPort.get()); - broadcastSender.setup(broadcastIP.get(), broadcastPort.get()); - ofLog(OF_LOG_NOTICE, "Choosen BroadcastAddress: " + broadcastIP.get()); - - maxServerMessages = 38; - - broadCastTimer = ofGetElapsedTimeMillis(); - - scale = 0.001; // transform mm to m + // TODO: check usefulness frameNumber = 0; } -void TrackingNetworkManager::listenerString(string & _string){ - ofLog(OF_LOG_NOTICE, "listenerString " + _string + " from"); -} - -void TrackingNetworkManager::listenerInt(int & _int){ - ofLog(OF_LOG_NOTICE, "listenerInt " + ofToString(_int) + " "); -} - -void TrackingNetworkManager::listenerBool(bool & _bool){ - ofLog(OF_LOG_NOTICE, "listenerBool " + ofToString(_bool) + - " streamingBodyBlob:" + ofToString(streamingBodyBlob.get()) + - " streamingHeadBlob:" + ofToString(streamingHeadBlob.get()) + - " streamingHead:" + ofToString(streamingHead.get()) + - " streamingEye:" + ofToString(streamingEye.get())); -} - //-------------------------------------------------------------- -void TrackingNetworkManager::update(SkeletonFinder & _blobFinder, Frustum & _frustum, ofMatrix4x4 _trans){ +void TrackingNetworkManager::update(const SkeletonFinder& skeletonFinder){ frameNumber++; long currentMillis = ofGetElapsedTimeMillis(); @@ -110,52 +62,11 @@ void TrackingNetworkManager::update(SkeletonFinder & _blobFinder, Frustum & _fru } //send trackingdata to all connected clients - sendTrackingData(_blobFinder); + sendTrackingData(skeletonFinder); // OSC receiver queues up new messages, so you need to iterate // through waiting messages to get each incoming message - // check for waiting messages - while(serverReceiver.hasWaitingMessages()){ - // get the next message - ofxOscMessage m; - serverReceiver.getNextMessage(m); - //Log received message for easier debugging of participants' messages: - ofLog(OF_LOG_NOTICE, "Server recvd msg " + getOscMsgAsString(m) + " from " + m.getRemoteIp()); - - // check the address of the incoming message - if(m.getAddress() == "/ks/request/handshake"){ - //Identify host of incoming msg - string incomingHost = m.getRemoteIp(); - //See if incoming host is a new one: - // get the first argument (listeningport) as an int - if(m.getNumArgs() == 1 && m.getArgType(0) == OFXOSC_TYPE_INT32){ - knownClients[getTrackingClientIndex(incomingHost, m.getArgAsInt32(0))].update(currentMillis); - // Send calib-data - sendCalibFrustum(_frustum, incomingHost, m.getArgAsInt32(0)); - sendCalibSensorBox(_blobFinder, incomingHost, m.getArgAsInt32(0)); - sendCalibTrans(_trans, incomingHost, m.getArgAsInt32(0)); - sendGazePoint(_blobFinder, incomingHost, m.getArgAsInt32(0)); - }else{ - ofLog(OF_LOG_WARNING, "Server recvd malformed message. Expected: /ks/request/handshake | received: " + getOscMsgAsString(m) + " from " + incomingHost); - } - } else if(m.getAddress() == "/ks/request/update"){ - //Identify host of incoming msg - string incomingHost = m.getRemoteIp(); - //See if incoming host is a new one: - // get the first argument (listeningport) as an int - if(m.getNumArgs() == 1 && m.getArgType(0) == OFXOSC_TYPE_INT32){ - knownClients[getTrackingClientIndex(incomingHost, m.getArgAsInt32(0))].update(currentMillis); - }else{ - ofLog(OF_LOG_WARNING, "Server recvd malformed message. Expected: /ks/request/update | received: " + getOscMsgAsString(m) + " from " + incomingHost); - } - } - // handle getting random OSC messages here - else{ - ofLogWarning("Server got weird message: " + m.getAddress()); - } - } - //this is purely workaround for a mysterious OSCpack bug on 64bit linux // after startup, reinit the receivers // must be a timing problem, though - in debug, stepping through, it works. @@ -164,160 +75,34 @@ void TrackingNetworkManager::update(SkeletonFinder & _blobFinder, Frustum & _fru } } -void TrackingNetworkManager::sendTrackingData(SkeletonFinder & _blobFinder){ - return; - /* - // send frame number - ofxOscMessage frame; - frame.setAddress("/ks/server/track/frame/start"); - frame.addIntArg(mServerID.get()); - frame.addIntArg(frameNumber); - frame.addIntArg(streamingBodyBlob.get()); - frame.addIntArg(streamingHeadBlob.get()); - frame.addIntArg(streamingHead.get()); - frame.addIntArg(streamingEye.get()); - sendMessageToTrackingClients(frame); - - for(int i = 0; i < _blobFinder.blobEvents.size(); i++){ - if (_blobFinder.blobEvents[i].isActive() && _blobFinder.blobEvents[i].hasBeenUpdated()) { - if (streamingHeadBlob.get()) { - ofxOscMessage headBlob; - headBlob.setAddress("/ks/server/track/headblob"); - headBlob.addIntArg(mServerID.get()); - headBlob.addIntArg(frameNumber); - headBlob.addIntArg(_blobFinder.blobEvents[i].mID); - headBlob.addIntArg(_blobFinder.blobEvents[i].sortPos); - headBlob.addIntArg(_blobFinder.blobEvents[i].getAgeInMillis()); - headBlob.addFloatArg(_blobFinder.blobEvents[i].headBlobCenter.x); - headBlob.addFloatArg(_blobFinder.blobEvents[i].headBlobCenter.y); - headBlob.addFloatArg(_blobFinder.blobEvents[i].headBlobCenter.z); - headBlob.addFloatArg(_blobFinder.blobEvents[i].headBlobSize.x); - headBlob.addFloatArg(_blobFinder.blobEvents[i].headBlobSize.y); - - sendMessageToTrackingClients(headBlob); - } - if (streamingHead.get()) { - ofxOscMessage head; - head.setAddress("/ks/server/track/head"); - head.addIntArg(mServerID.get()); - head.addIntArg(frameNumber); - head.addIntArg(_blobFinder.blobEvents[i].mID); - head.addIntArg(_blobFinder.blobEvents[i].sortPos); - head.addIntArg(_blobFinder.blobEvents[i].getAgeInMillis()); - head.addFloatArg(_blobFinder.blobEvents[i].headTop.x); - head.addFloatArg(_blobFinder.blobEvents[i].headTop.y); - head.addFloatArg(_blobFinder.blobEvents[i].headTop.z); - - sendMessageToTrackingClients(head); - } - if (streamingEye.get()) { - ofxOscMessage eye; - eye.setAddress("/ks/server/track/eye"); - eye.addIntArg(mServerID.get()); - eye.addIntArg(_blobFinder.blobEvents[i].mID); - eye.addIntArg(_blobFinder.blobEvents[i].sortPos); - eye.addIntArg(_blobFinder.blobEvents[i].getAgeInMillis()); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeCenter.x); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeCenter.y); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeCenter.z); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeGaze.x); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeGaze.y); - eye.addFloatArg(_blobFinder.blobEvents[i].eyeGaze.z); - - sendMessageToTrackingClients(eye); - } - } - else if (!_blobFinder.blobEvents[i].isDead() && _blobFinder.blobEvents[i].isDying()) { - ofxOscMessage bodyBlob; - bodyBlob.setAddress("/ks/server/track/end"); - bodyBlob.addIntArg(mServerID.get()); - bodyBlob.addIntArg(frameNumber); - bodyBlob.addIntArg(_blobFinder.blobEvents[i].mID); - bodyBlob.addIntArg(_blobFinder.blobEvents[i].getAgeInMillis()); - - sendMessageToTrackingClients(bodyBlob); - _blobFinder.blobEvents[i].mIsDead = true; - } - } +void TrackingNetworkManager::sendTrackingData(const SkeletonFinder& skeletonFinder) { // send frame number - ofxOscMessage framedone; - framedone.setAddress("/ks/server/track/frame/end"); - framedone.addIntArg(mServerID.get()); - framedone.addIntArg(frameNumber); - sendMessageToTrackingClients(framedone); - */ -} - -void TrackingNetworkManager::sendCalibFrustum(Frustum & _frustum, string _ip, int _port){ - ofxOscMessage frustum; - frustum.setAddress("/ks/server/calib/frustum"); - frustum.addIntArg(mServerID.get()); - frustum.addFloatArg(_frustum.left); - frustum.addFloatArg(_frustum.right); - frustum.addFloatArg(_frustum.bottom); - frustum.addFloatArg(_frustum.top); - frustum.addFloatArg(_frustum.near1); - frustum.addFloatArg(_frustum.far1); - - broadcastSender.setup(_ip, _port); - broadcastSender.sendMessage(frustum); -} - -void TrackingNetworkManager::sendCalibTrans(ofMatrix4x4 & _trans, string _ip, int _port){ - ofxOscMessage trans; - trans.setAddress("/ks/server/calib/trans"); - trans.addIntArg(mServerID.get()); - trans.addFloatArg(_trans._mat[0].x); - trans.addFloatArg(_trans._mat[0].y); - trans.addFloatArg(_trans._mat[0].z); - trans.addFloatArg(_trans._mat[0].w); - trans.addFloatArg(_trans._mat[1].x); - trans.addFloatArg(_trans._mat[1].y); - trans.addFloatArg(_trans._mat[1].z); - trans.addFloatArg(_trans._mat[1].w); - trans.addFloatArg(_trans._mat[2].x); - trans.addFloatArg(_trans._mat[2].y); - trans.addFloatArg(_trans._mat[2].z); - trans.addFloatArg(_trans._mat[2].w); - trans.addFloatArg(_trans._mat[3].x); - trans.addFloatArg(_trans._mat[3].y); - trans.addFloatArg(_trans._mat[3].z); - trans.addFloatArg(_trans._mat[3].w); - - broadcastSender.setup(_ip, _port); - broadcastSender.sendMessage(trans); + ofxOscMessage frame; + frame.setAddress("/ks/server/track/frame/start"); + frame.addIntArg(mServerID.get()); + frame.addIntArg(frameNumber); + sendMessageToTrackingClients(frame); + + vector skeletons = skeletonFinder.getSkeletons(); + if (skeletons.size() > 0) { + // Only one skeleton is to be on the scene for the perspective to work + sendSkeletonData(skeletons[0]); + } } -void TrackingNetworkManager::sendCalibSensorBox(SkeletonFinder & _blobFinder, string _ip, int _port){ - return; - /* - ofxOscMessage sensorbox; - sensorbox.setAddress("/ks/server/calib/sensorbox"); - sensorbox.addIntArg(mServerID.get()); - sensorbox.addFloatArg(_blobFinder.sensorBoxLeft.get() * scale); - sensorbox.addFloatArg(_blobFinder.sensorBoxRight.get() * scale); - sensorbox.addFloatArg(_blobFinder.sensorBoxBottom.get() * scale); - sensorbox.addFloatArg(_blobFinder.sensorBoxTop.get() * scale); - sensorbox.addFloatArg(_blobFinder.sensorBoxFront.get() * scale); - sensorbox.addFloatArg(_blobFinder.sensorBoxBack.get() * scale); - - broadcastSender.setup(_ip, _port); - broadcastSender.sendMessage(sensorbox); - */ -} +void TrackingNetworkManager::sendSkeletonData(const Skeleton& skel) { + if (streamingWholeBody.get()) { + ofxOscMessage skeletonMsg; + skeletonMsg.setAddress("/ks/server/track/skeleton"); + for (auto joint = skel.joints.begin(); joint != skel.joints.end(); ++joint) { + skeletonMsg.addFloatArg(joint->pos.x); + skeletonMsg.addFloatArg(joint->pos.y); + skeletonMsg.addFloatArg(joint->pos.z); + skeletonMsg.addFloatArg(joint->confidence); + } -void TrackingNetworkManager::sendGazePoint(SkeletonFinder & _blobFinder, string _ip, int _port){ - return;/* - ofxOscMessage sensorbox; - sensorbox.setAddress("/ks/server/calib/gazepoint"); - sensorbox.addIntArg(mServerID.get()); - sensorbox.addFloatArg(_blobFinder.gazePoint.get().x); - sensorbox.addFloatArg(_blobFinder.gazePoint.get().y); - sensorbox.addFloatArg(_blobFinder.gazePoint.get().z); - - broadcastSender.setup(_ip, _port); - broadcastSender.sendMessage(sensorbox); - */ + sendMessageToTrackingClients(skeletonMsg); + } } void TrackingNetworkManager::sendMessageToTrackingClients(ofxOscMessage _msg){ @@ -330,7 +115,9 @@ void TrackingNetworkManager::sendMessageToTrackingClients(ofxOscMessage _msg){ void TrackingNetworkManager::checkTrackingClients(long _currentMillis){ for(int i = 0; i < knownClients.size(); i++){ if(!knownClients[i].stillAlive(_currentMillis)){ - ofLog(OF_LOG_NOTICE, "Server removed TrackingClient ip: " + knownClients[i].clientDestination + " port: " + ofToString(knownClients[i].clientSendPort)); + ofLog(OF_LOG_NOTICE) << "Server removed TrackingClient ip: " + << knownClients[i].clientDestination + << " port: " << ofToString(knownClients[i].clientSendPort); knownClients[i] = knownClients.back(); knownClients.pop_back(); i--; @@ -345,7 +132,9 @@ int TrackingNetworkManager::getTrackingClientIndex(string _ip, int _port){ } } knownClients.push_back(TrackingClient(_ip, _port)); - ofLog(OF_LOG_NOTICE, "Server added new TrackingClient ip: " + _ip + " port: " + ofToString(_port) + " knownClients: " + ofToString(knownClients.size())); + ofLog(OF_LOG_NOTICE) << "Server added new TrackingClient ip: " << _ip + << " port: " + ofToString(_port) + << " knownClients: " << ofToString(knownClients.size()); return knownClients.size() -1; } @@ -361,7 +150,6 @@ void TrackingNetworkManager::sendBroadCastAddress(){ broadcastSender.sendMessage(broadcast); broadCastTimer = ofGetElapsedTimeMillis(); - //ofLog(OF_LOG_NOTICE, "Sent Broadcastmessage"); } //-------------------------------------------------------------- diff --git a/src/TrackingNetworkManager.h b/src/TrackingNetworkManager.h index 70c3dfeba6b8c70421d4153caef354b88ef520f3..bbbdeb69f35b8bf52def8e9b8102bf9090705568 100644 --- a/src/TrackingNetworkManager.h +++ b/src/TrackingNetworkManager.h @@ -31,27 +31,17 @@ class TrackingNetworkManager { -public: - TrackingNetworkManager(); - - void setup(ofxGui &gui, string _kinectSerial); - void update(SkeletonFinder & _SkeletonFinder, Frustum & _frustum, ofMatrix4x4 _trans); - - void sendTrackingData(SkeletonFinder & _SkeletonFinder); +public: + void setup(ofxGui &gui, string _realsenseSerial); + void update(const SkeletonFinder& skeletonFinder); - void sendCalibFrustum(Frustum & _frustum, string ip, int port); - void sendCalibTrans(ofMatrix4x4 & _trans, string _ip, int _port); - void sendCalibSensorBox(SkeletonFinder & _SkeletonFinder, string _ip, int _port); - void sendGazePoint(SkeletonFinder & _SkeletonFinder, string _ip, int _port); + void sendTrackingData(const SkeletonFinder& skeletonFinder); + void sendSkeletonData(const Skeleton& skeleton); void sendMessageToTrackingClients(ofxOscMessage _msg); void checkTrackingClients(long _currentMillis); int getTrackingClientIndex(string _ip, int _port); - void listenerString(string & _string); - void listenerInt(int & _int); - void listenerBool(bool & _bool); - void sendBroadCastAddress(); string getOscMsgAsString(ofxOscMessage m); @@ -99,12 +89,7 @@ public: ofParameterGroup streamingGuiGroup; - ofParameter streamingBodyBlob; - ofParameter streamingHeadBlob; - ofParameter streamingHead; - ofParameter streamingEye; - - + ofParameter streamingWholeBody; }; diff --git a/src/ofApp.cpp b/src/ofApp.cpp index c0b72e488f179845ace36a4e004b57fc1dc89770..9b9fe9e1d69e3816540d14be92afd6e896e92292 100644 --- a/src/ofApp.cpp +++ b/src/ofApp.cpp @@ -5,6 +5,30 @@ #define DEPTH_X_RES 640 #define DEPTH_Y_RES 480 +ofMatrix4x4 ofApp::makeNuitrackToRealSenseTransform() { + // Thankfully this matrix is symmetric, so we need not worry about the row-major-ness + // of the matrix object + float mat[16] = { + 1e-3, 0, 0, 0, + 0, -1e-3, 0, 0, + 0, 0, 1e-3, 0, + 0, 0, 0, 1 + }; + // TODO: add scaling + return glm::make_mat4x4(mat); +} + +const ofMatrix4x4 ofApp::nuitrackViewportToRealSenseViewportTransform = makeNuitrackToRealSenseTransform(); + + +// TODO: remove +void debug_mat(glm::mat4 mat) { + ofLog(OF_LOG_NOTICE) << "MATRIX : \n" + << mat[0][0] << " " << mat[1][0] << " " << mat[2][0] << " " << mat[3][0] << "\n" + << mat[0][1] << " " << mat[1][1] << " " << mat[2][1] << " " << mat[3][1] << "\n" + << mat[0][2] << " " << mat[1][2] << " " << mat[2][2] << " " << mat[3][2] << "\n" + << mat[0][3] << " " << mat[1][3] << " " << mat[2][3] << " " << mat[3][3] << "\n"; +} //-------------------------------------------------------------- void ofApp::initNuitrack() { @@ -54,9 +78,6 @@ void ofApp::initNuitrack() { }); pointCloudManager.setDepthSensor(tracker->depthTracker); - - // TODO: maybe account for this, that used to be in old code : - // realSense->setVideoSize(REALSENSE_VIDEO_WIDTH, REALSENSE_VIDEO_HEIGHT); } void ofApp::initViewports() { @@ -83,161 +104,51 @@ void ofApp::initViewports() { previewCam.setNearClip(0.001f); } -void ofApp::setupCalibGui() { - // TODO: remove what is useless - - setupCalib = gui.addPanel(); - - setupCalib->loadTheme("theme/theme_light.json"); - setupCalib->setName("Calibration Panel"); - - setupCalib->add(blobGrain.set("Grain", 2, 1, 10)); - - setupCalib->add(calibPoint_X.set("calibrationPoint_X", ofVec2f(REALSENSE_VIDEO_WIDTH / 2, REALSENSE_VIDEO_HEIGHT / 2), ofVec2f(0, 0), ofVec2f(REALSENSE_VIDEO_WIDTH, REALSENSE_VIDEO_HEIGHT))); - setupCalib->add(calibPoint_Y.set("calibrationPoint_Y", ofVec2f(REALSENSE_VIDEO_WIDTH / 2, REALSENSE_VIDEO_HEIGHT / 2), ofVec2f(0, 0), ofVec2f(REALSENSE_VIDEO_WIDTH, REALSENSE_VIDEO_HEIGHT))); - setupCalib->add(calibPoint_Z.set("calibrationPoint_Z", ofVec2f(REALSENSE_VIDEO_WIDTH / 2, REALSENSE_VIDEO_HEIGHT / 2), ofVec2f(0, 0), ofVec2f(REALSENSE_VIDEO_WIDTH, REALSENSE_VIDEO_HEIGHT))); - - nearFrustum.addListener(this, &ofApp::updateFrustumCone); - farFrustum.addListener(this, &ofApp::updateFrustumCone); - - frustumGuiGroup.setName("frustumField"); - frustumGuiGroup.add(nearFrustum.set("nearFrustum", 400, 200, 2000)); - frustumGuiGroup.add(farFrustum.set("farFrustum", 4000, 2000, 10000)); - setupCalib->addGroup(frustumGuiGroup); - - //setupCalib->add(transformation.set("matrix rx ry tz", ofVec3f(0, 0, 0), ofVec3f(-90, -90, -6000), ofVec3f(90, 90, 6000))); - - setupCalib->loadFromFile("settings.xml"); -} - void ofApp::setupTransformGui() { guitransform = gui.addPanel(); - guitransform->loadTheme("theme/theme_light.json"); guitransform->setName("Transformation"); transformationGuiGroup.setName("Matrix"); transformationGuiGroup.add(transformation.set("Transform", ofMatrix4x4())); - guitransform->addGroup(transformationGuiGroup); - guitransform->loadFromFile("transformation.xml"); + // The GUI apparently cannot display matrices + // Also the method apparently requires a reference for some reason + bool visible = false; + guitransform->setVisible(visible); - bool invisible = false; + reloadTransformMatrix(); +} - guitransform->setVisible(invisible); +void ofApp::reloadTransformMatrix() { + guitransform->loadFromFile("transformation.xml"); + + // ofMatrices multiplication works in reverse + worldToDeviceTransform = nuitrackViewportToRealSenseViewportTransform * transformation.get(); + deviceToWorldTransform = nuitrackViewportToRealSenseViewportTransform * transformation.get(); } void ofApp::setup(){ - //ofLog(OF_LOG_NOTICE) << "Entered setup"; -#ifdef TARGET_OPENGLES - shader.load("shadersES2/shader"); -#else - if (ofIsGLProgrammableRenderer()) { - shader.load("shadersGL3/shader"); - } - else { - shader.load("shadersGL2/shader"); - } -#endif - ofLog(OF_LOG_NOTICE) << "Nuitrack setup started"; initNuitrack(); - skeletonFinder.setTransformMatrix(&deviceTransform); - ofLog(OF_LOG_NOTICE) << "Nuitrack setup ended"; - - // we don't want to be running too fast - //ofSetVerticalSync(true); - //ofSetFrameRate(30); + skeletonFinder.setTransformMatrix(&deviceToWorldTransform); ofLog(OF_LOG_NOTICE) << "Viewport setup started"; initViewports(); - ofLog(OF_LOG_NOTICE) << "Viewport setup ended"; - - ///////////////////////////// - // REALSENSE GUI SETUP // - ///////////////////////////// - /* - ofLog(OF_LOG_NOTICE, "MainAPP: loading postprocessing GUI"); - - post = gui.addPanel(); - post->loadTheme("theme/theme_light.json"); - post->setName("PostProcessing"); - post->add(realSense->param_usePostProcessing); - post->add(realSense->param_filterDecimation); - post->add(realSense->param_filterDecimation_mag); - post->add(realSense->param_filterDisparities); - post->add(realSense->param_filterSpatial); - post->add(realSense->param_filterSpatial_smoothAlpha); - post->add(realSense->param_filterSpatial_smoothDelta); - post->add(realSense->param_filterSpatial_mag); - post->add(realSense->param_filterTemporal); - post->add(realSense->param_filterTemporal_smoothAlpha); - post->add(realSense->param_filterTemporal_smoothDelta); - post->add(realSense->param_filterTemporal_persistency); - - post->loadFromFile("postprocessing.xml"); - */ - - ofLog(OF_LOG_NOTICE, "Loading clibration GUI"); - setupCalibGui(); - ofLog(OF_LOG_NOTICE, "Loading transformation GUI"); - setupTransformGui(); - - updateMatrix(); - - ///////////////////////////// - // GUI DEVICE PARAMS // - ///////////////////////////// - - ofLog(OF_LOG_NOTICE, "MainAPP: loading Device Operation GUI"); - - device = gui.addPanel(); - - ///////////////////////////// - // OPERATING GUI // - ///////////////////////////// - //operating = gui.addPanel(); - //operating->loadTheme("theme/theme_light.json"); - //operating->setName("Operating"); - - //operatingModes.setName("Modes"); - //operatingModes.add(mode0Capture.set("normal", false)); - //operatingModes.add(mode1Record.set("recording", false)); - //operatingModes.add(mode2Playback.set("playback", false)); - - //operatingToggles = operating->addGroup(operatingModes); - //operatingToggles->setExclusiveToggles(true); - //operatingToggles->setConfig(ofJson({ {"type", "radio"} })); - - //operatingToggles->setActiveToggle(0); - //operatingToggles->getActiveToggleIndex().addListener(this, &ofApp::changeOperation); - - //////////////////////// - // RealSense // - //////////////////////// - - ofLog(OF_LOG_NOTICE, "MainAPP: starting attached Device..."); + ofLog(OF_LOG_NOTICE) << "Loading GUI"; + setupTransformGui(); + skeletonFinder.initGUI(gui); - // firing up the device, creating the GUI and loading the device parameters + ofLog(OF_LOG_NOTICE) << "MainAPP: starting attached Device..."; tracker->run(); - createGUIDeviceParams(); - ofLog(OF_LOG_NOTICE, "...starting attached Device done."); - - ///////////////// - // creating preview point cloud is bogging the system down, so switched off at startup bPreviewPointCloud = false; - ofLog(OF_LOG_NOTICE, "MainAPP: setting up networking..."); - // TODO: remove useless arg - networkMng.setup(gui, "sauce"); - ofLog(OF_LOG_NOTICE, "...networking done."); - - int val = 0; - updateFrustumCone(val); - //crashes here + ofLog(OF_LOG_NOTICE) << "MainAPP: setting up networking..."; + networkMng.setup(gui, "TODO: remove useless arg"); + setupViewports(); createHelp(); @@ -245,363 +156,51 @@ void ofApp::setup(){ capMesh.reSize(4); - ofSetLogLevel(OF_LOG_VERBOSE); - //ofLogToFile("myLogFile.txt", true); ofLog(OF_LOG_NOTICE) << "Setup over"; - - if (ofIsGLProgrammableRenderer()) { - ofLog(OF_LOG_NOTICE, "ofIsGLProgrammableRenderer() = " + ofToString(ofIsGLProgrammableRenderer())); - } -} - -void ofApp::changeOperation(int& _index) {} - -void ofApp::createGUIDeviceParams() { - //ofLog(OF_LOG_NOTICE) << "Entered createGUIDeviceParams"; - device->clear(); - device->loadTheme("theme/theme_light.json"); - device->setName("RealSense Device"); - - intrinsicGuiGroup.clear(); - intrinsicGuiGroup.setName("Settings"); - //intrinsicGuiGroup.add(realSense->param_deviceLaser); - //intrinsicGuiGroup.add(realSense->param_deviceLaser_mag); - //intrinsicGuiGroup.add(realSense->param_deviceAutoExposure); - //intrinsicGuiGroup.add(realSense->param_deviceExposure_mag); - //intrinsicGuiGroup.add(realSense->param_deviceGain_mag); - //intrinsicGuiGroup.add(realSense->param_deviceFrameQueSize_mag); - //intrinsicGuiGroup.add(realSense->param_deviceAsicTemparature); - //intrinsicGuiGroup.add(realSense->param_deviceProjectorTemparature); - - device->addGroup(intrinsicGuiGroup); - - device->loadFromFile("device.xml"); } //-------------------------------------------------------------- void ofApp::setupViewports(){ - //ofLog(OF_LOG_NOTICE) << "Entered setupViewports"; //call here whenever we resize the window - device->setWidth(MENU_WIDTH / 4); - //post->setWidth(MENU_WIDTH / 4); - setupCalib->setWidth(MENU_WIDTH / 4); - //skeletonFinder.panel->setWidth(MENU_WIDTH / 4); - networkMng.panel->setWidth(MENU_WIDTH / 4); - //operating->setWidth(MENU_WIDTH / 4); - - device->setPosition(ofGetWidth() - MENU_WIDTH, 20); - //post->setPosition(ofGetWidth() - MENU_WIDTH, 400); - //operating->setPosition(ofGetWidth() - MENU_WIDTH, 800); - setupCalib->setPosition(ofGetWidth() - MENU_WIDTH / 4 * 3, 20); - //skeletonFinder.panel->setPosition(ofGetWidth() - MENU_WIDTH / 4 * 2, 20); - networkMng.panel->setPosition(ofGetWidth() - MENU_WIDTH / 4, 20); -} - -void ofApp::updateFrustumCone(int& val){ - //ofLog(OF_LOG_NOTICE) << "Entered updateFrustumCone"; - double ref_pix_size = 1;// kinect.getZeroPlanePixelSize(); - double ref_distance = 1;// kinect.getZeroPlaneDistance(); - - realSenseFrustum.near1 = nearFrustum.get(); - realSenseFrustum.far1 = farFrustum.get(); - - double factorNear = 2 * ref_pix_size * realSenseFrustum.near1 / ref_distance; - double factorFar = 2 * ref_pix_size * realSenseFrustum.far1 / ref_distance; - - //ofVec3f((x - DEPTH_X_RES/2) *factor, (y - DEPTH_Y_RES/2) *factor, raw[y * w + x])); - - realSenseFrustum.left = (0 - DEPTH_X_RES/2) *factorNear; - realSenseFrustum.right = (640 - DEPTH_X_RES/2) *factorNear; - realSenseFrustum.top = (0 - DEPTH_Y_RES/2) *factorNear; - realSenseFrustum.bottom = (480 - DEPTH_Y_RES/2) *factorNear; - - realSenseFrustum.leftFar = (0 - DEPTH_X_RES/2) *factorFar; - realSenseFrustum.rightFar = (640 - DEPTH_X_RES/2) *factorFar; - realSenseFrustum.topFar = (0 - DEPTH_Y_RES/2) *factorFar; - realSenseFrustum.bottomFar = (480 - DEPTH_Y_RES/2) *factorFar; - - realSenseFrustum.update(); - //createFrustumCone(); -} - -void ofApp::measurementCycleRaw(){ - //ofLog(OF_LOG_NOTICE) << "Entered measurementCycleRaw"; - if(cycleCounter < N_MEASURMENT_CYCLES){ - planePoint1Meas[cycleCounter] = calcPlanePoint(calibPoint_X, 0, 1); - planePoint2Meas[cycleCounter] = calcPlanePoint(calibPoint_Y, 0, 1); - planePoint3Meas[cycleCounter] = calcPlanePoint(calibPoint_Z, 0, 1); - cycleCounter++; - } else { - planePoint_X = ofVec3f(); - planePoint_Y = ofVec3f(); - planePoint_Z = ofVec3f(); - for(int y = 0; y < N_MEASURMENT_CYCLES; y++){ - planePoint_X += planePoint1Meas[y]; - planePoint_Y += planePoint2Meas[y]; - planePoint_Z += planePoint3Meas[y]; - } - planePoint_X /= N_MEASURMENT_CYCLES; - planePoint_Y /= N_MEASURMENT_CYCLES; - planePoint_Z /= N_MEASURMENT_CYCLES; - bUpdateMeasurment = false; - bUpdateMeasurmentFine = true; - cycleCounter = 0; - } -} - -void ofApp::measurementCycleFine(){ - //ofLog(OF_LOG_NOTICE) << "Entered measurementCycleFine"; - if(cycleCounter < N_MEASURMENT_CYCLES){ - ofVec3f p1meas = calcPlanePoint(calibPoint_X, 0, 1); - ofVec3f p2meas = calcPlanePoint(calibPoint_Y, 0, 1); - ofVec3f p3meas = calcPlanePoint(calibPoint_Z, 0, 1); - if(planePoint_X.z / 1.05 < p1meas.z && - p1meas.z < planePoint_X.z * 1.05 && - planePoint_Y.z / 1.05 < p2meas.z && - p2meas.z < planePoint_Y.z * 1.05 && - planePoint_Z.z / 1.05 < p3meas.z && - p3meas.z < planePoint_Z.z * 1.05){ - planePoint1Meas[cycleCounter] = p1meas; - planePoint2Meas[cycleCounter] = p2meas; - planePoint3Meas[cycleCounter] = p3meas; - cycleCounter++; - } - } else { - planePoint_X = ofVec3f(); - planePoint_Y = ofVec3f(); - planePoint_Z = ofVec3f(); - for(int y = 0; y < N_MEASURMENT_CYCLES; y++){ - planePoint_X += planePoint1Meas[y]; - planePoint_Y += planePoint2Meas[y]; - planePoint_Z += planePoint3Meas[y]; - } - planePoint_X /= N_MEASURMENT_CYCLES; - planePoint_Y /= N_MEASURMENT_CYCLES; - planePoint_Z /= N_MEASURMENT_CYCLES; - bUpdateMeasurmentFine = false; - cycleCounter = 0; - updateCalc(); - } -} - -//-------------------------------------------------------------- -void ofApp::updateCalc(){ - //ofLog(OF_LOG_NOTICE) << "Entered updateCalc"; - - // This algorithm calculates the transformation matrix to - // transform from the camera centered coordinate system to the - // calibration points defined coordinate system, where - // point z represents the coordinate center - // point x represents the x - axis from the coordinate center - // point y represents the y - axis from the coordinate center - - // translation vector to new coordinate system - glm::vec3 translate = glm::vec3(planePoint_Z); - - // TODO: redo this in order to coincide with unreal engine or unity's coordinate syste - - glm::vec3 newXAxis = glm::normalize(glm::vec3(planePoint_X - planePoint_Z)); - glm::vec3 newYAxis = glm::normalize(glm::vec3(planePoint_Y - planePoint_Z)); - glm::vec3 newZAxis = glm::cross(newXAxis, newYAxis); - - // we calculate the Y axis from the Z axis to make sure all the vectors are perpendicular to each other - // CAREFULL: It could be disabled because: - // Using nonperpendicular axis inspired from the point cloud data seems to - // correct some of point cloud distortions.... - newYAxis = glm::cross(newZAxis, newXAxis); - - // the following solution was inspired by this post: https://stackoverflow.com/questions/34391968/how-to-find-the-rotation-matrix-between-two-coordinate-systems - // however: it uses a 4x4 matrix and puts translation data as follows: - //{ x.x x.y x.z 0 y.x y.y y.z 0 z.x z.y z.z 0 t.x t.y t.z 1 } - - float mat[16] = { - newXAxis.x, - newXAxis.y, - newXAxis.z, - 0, - newYAxis.x, - newYAxis.y, - newYAxis.z, - 0, - newZAxis.x, - newZAxis.y, - newZAxis.z, - 0, - translate.x, - translate.y, - translate.z, - 1 - }; - - // and what we need at the end is the inverse of this: - glm::mat4 transform = glm::inverse(glm::make_mat4x4(mat)); - - geometry.clear(); - geometry.setMode(OF_PRIMITIVE_LINES); - - geometry.addColor(ofColor::red); - geometry.addVertex(translate); - geometry.addColor(ofColor::red); - geometry.addVertex(translate + newXAxis); - - geometry.addColor(ofColor::green); - geometry.addVertex(translate); - geometry.addColor(ofColor::green); - geometry.addVertex(translate + newYAxis); - - geometry.addColor(ofColor::blue); - geometry.addVertex(translate); - geometry.addColor(ofColor::blue); - geometry.addVertex(translate + newZAxis); - - calcdata = string("distance to new coordinate center point: " + ofToString(glm::length(translate)) + "\n"); - calcdata += "position point X: " + ofToString(planePoint_X) + "\n"; - calcdata += "position point Y: " + ofToString(planePoint_Y) + "\n"; - calcdata += "position point Z: " + ofToString(planePoint_Z) + "\n"; - calcdata += "distance to X: " + ofToString(planePoint_X.length()) + "\n"; - calcdata += "distance to Y: " + ofToString(planePoint_Y.length()) + "\n"; - calcdata += "distance to Z: " + ofToString(planePoint_Z.length()) + "\n"; - calcdata += "distance X to Z: " + ofToString(ofVec3f(planePoint_X - planePoint_Z).length()) + "\n"; - calcdata += "distance Y to Z: " + ofToString(ofVec3f(planePoint_Y - planePoint_Z).length()) + "\n"; - - frustumCenterSphere.setRadius(20); - - bUpdateCalc = false; - - // ofLog(OF_LOG_NOTICE, "updating... "); - - transformation.set(ofMatrix4x4(transform)); - - updateMatrix(); -} - -//-------------------------------------------------------------- -void ofApp::updateMatrix(){ - //ofLog(OF_LOG_NOTICE) << "Entered updateMatrix"; - - sphere_X.setPosition(planePoint_X); - sphere_Y.setPosition(planePoint_Y); - sphere_Z.setPosition(planePoint_Z); - - sphere_X.setRadius(0.05); - sphere_Y.setRadius(0.05); - sphere_Z.setRadius(0.05); + // TODO: update UI + skeletonFinder.panel->setWidth(MENU_WIDTH / 2); + networkMng.panel->setWidth(MENU_WIDTH / 2); - //deviceTransform = ofMatrix4x4(); - - deviceTransform = transformation.get(); - - //blobFinder.kinectPos = ofVec3f(0, 0, transformation.get().z); + skeletonFinder.panel->setPosition(ofGetWidth() - MENU_WIDTH, 20); + networkMng.panel->setPosition(ofGetWidth() - MENU_WIDTH / 2, 20); } -//-------------------------------------------------------------- -glm::vec3 ofApp::calcPlanePoint(ofParameter & cpoint, int _size, int _step){ - //ofLog(OF_LOG_NOTICE) << "Entered calcPlanePoint"; - glm::vec3 ppoint; - - // TODO: these values are in nuitrack.config - int width = 640;// realSense->getDepthWidth(); - int height = 480;// realSense->getDepthHeight(); - - int size = _size; - int step = _step; - float factor; - int counter = 0; - - int minX = ((cpoint.get().x - size) >= 0)?(cpoint.get().x - 1): 0; - int minY = ((cpoint.get().y - size) >= 0)?(cpoint.get().y - 1): 0; - int maxY = ((cpoint.get().y + size) < cpoint.getMax().y)?(cpoint.get().y + size): cpoint.getMax().y - 1; - int maxX = ((cpoint.get().x + size) < cpoint.getMax().x)?(cpoint.get().x + size): cpoint.getMax().x - 1; - - float corrDistance; - - glm::vec3 coord; - - for(int y = minY; y <= maxY; y = y + step) { - for(int x = minX; x <= maxX; x = x + step) { - coord = Tracker::fromVector3(tracker->depthTracker->convertProjToRealCoords(nuitrack::Vector3(x, y, 0))); - if(coord.z > 0) { - ppoint += coord; - counter++; - } - } - } - ppoint /= counter; - - return ppoint; - -} - - //-------------------------------------------------------------- void ofApp::update(){ tracker->poll(); ofBackground(100, 100, 100); - if(bUpdateMeasurment){ - measurementCycleRaw(); - } - - if(bUpdateMeasurmentFine){ - measurementCycleFine(); - } - - // TODO: remove useless (?) - drawCapturePointCloud(true); - - networkMng.update(skeletonFinder, realSenseFrustum, transformation.get()); + networkMng.update(skeletonFinder); } //-------------------------------------------------------------- -void ofApp::draw(){ - //ofLog(OF_LOG_NOTICE) << "Entered draw"; - +void ofApp::draw() { ofSetColor(255, 255, 255); - //ofLogNotice() << "draw next frame"; if(bShowVisuals){ - //Draw viewport previews + // Draw viewport previews pointCloudManager.drawRGB(viewGrid[0]); pointCloudManager.drawDepth(viewGrid[1]); - // TODO: These might all be useless - //skeletonFinder.grayImage.draw(viewGrid[2]); - //skeletonFinder.contourFinder.draw(viewGrid[3]); - //skeletonFinder.maskFbo.draw(viewGrid[4]); - + if (iMainCamera != 2) { // make sure the camera is drawn only once (so the interaction with the mouse works) + previewCam.begin(viewGrid[2]); + mainGrid.drawPlane(5., 5, false); + drawPreview(); + previewCam.end(); + } switch (iMainCamera) { case 0: pointCloudManager.drawRGB(viewMain); - drawCalibrationPoints(); break; case 1: pointCloudManager.drawDepth(viewMain); - drawCalibrationPoints(); break; case 2: - //skeletonFinder.grayImage.draw(viewMain); - ofSetColor(255, 0, 0, 255); - //skeletonFinder.contourFinder.draw(viewMain); - - break; - case 3: - //skeletonFinder.fbo.draw(viewMain); - ofSetColor(255, 0, 0, 255); - //skeletonFinder.contourFinder.draw(viewMain); - - ofNoFill(); - ofSetColor(255, 0, 255, 255); - //skeletonFinder.drawBodyBlobs2d(viewMain); - - break; - case 4: - //skeletonFinder.maskFbo.draw(viewMain); - - ofNoFill(); - ofSetColor(255, 0, 255, 255); - //skeletonFinder.drawBodyBlobs2d(viewMain); - break; - case 5: previewCam.begin(viewMain); mainGrid.drawPlane(5., 5, false); drawPreview(); @@ -610,44 +209,30 @@ void ofApp::draw(){ default: break; } - - //Draw opengl viewport previews (ofImages dont like opengl calls before they are drawn - if(iMainCamera != 5){ // make sure the camera is drawn only once (so the interaction with the mouse works) - previewCam.begin(viewGrid[5]); - mainGrid.drawPlane(5., 5, false); - drawPreview(); - previewCam.end(); - } glDisable(GL_DEPTH_TEST); ofPushStyle(); + // Highlight background of selected camera ofSetColor(255, 0, 255, 255); ofNoFill(); ofSetLineWidth(3); ofDrawRectangle(viewGrid[iMainCamera]); - } else { - - //skeletonFinder.contourEyeFinder.draw(viewMain); - - ofNoFill(); - ofSetColor(255, 0, 255, 255); - //skeletonFinder.drawBodyBlobs2d(viewMain); - } - - //-- - + } // draw instructions ofSetColor(255, 255, 255); if(bShowHelp) { - if(bShowCalcData){ - ofDrawBitmapString(calcdata, 20 ,VIEWPORT_HEIGHT + 20); - } else { - ofDrawBitmapString(help, 20 ,VIEWPORT_HEIGHT + 20); - } + ofDrawBitmapString(help, 20 ,VIEWPORT_HEIGHT + 50); } + if (bShowSkeletonData) { + string desc = skeletonFinder.getShortDesc(); + ofLog(OF_LOG_NOTICE) << desc; + ofDrawBitmapString(desc, + 20, + VIEWPORT_HEIGHT + 20); + } ofDrawBitmapString("fps: " + ofToString(ofGetFrameRate()), ofGetWidth() - 200, 10); @@ -655,137 +240,54 @@ void ofApp::draw(){ } void ofApp::drawPreview() { - //ofLog(OF_LOG_NOTICE) << "Entered drawPreview"; glPointSize(4); glEnable(GL_DEPTH_TEST); - ofPushMatrix(); - //This moves the crossingpoint of the kinect center line and the plane to the center of the stage //ofTranslate(-planeCenterPoint.x, -planeCenterPoint.y, 0); - ofMultMatrix(deviceTransform); if (bPreviewPointCloud) { - // TODO: what does this do + ofPushMatrix(); + ofMultMatrix(worldToDeviceTransform); pointCloudManager.drawPointCloud(); + ofPopMatrix(); } - ofFill(); - ofSetColor(255, 0, 0); - sphere_X.draw(); - sphere_Y.draw(); - sphere_Z.draw(); - /* - frustumCenterSphere.draw(); - */ - - geometry.draw(); - - //ofSetColor(0, 0, 255); - //realSenseFrustum.drawWireframe(); - - ofPopMatrix(); - - ofPushMatrix(); + // TODO: draw base + //ofFill(); + //ofSetColor(255, 0, 0); + //sphere_X.draw(); + //sphere_Y.draw(); + //sphere_Z.draw(); + ofSetColor(255, 255, 0); skeletonFinder.drawSensorBox(); - - ofNoFill(); + glLineWidth(5); ofSetColor(255, 100, 255); skeletonFinder.drawSkeletons(); - //skeletonFinder.drawBodyBlobsBox(); - //skeletonFinder.drawBodyBlobsHeadTop(); - - ofFill(); - ofSetColor(255, 100, 100); - //skeletonFinder.drawGazePoint(); - glDisable(GL_DEPTH_TEST); - ofPopMatrix(); - -} - -void ofApp::drawCapturePointCloud(bool _mask) { - //ofLog(OF_LOG_NOTICE) << "Entered drawCapturePointCloud"; - glEnable(GL_DEPTH_TEST); - - shader.begin(); - - float lowerLimit = skeletonFinder.sensorBoxBottom.get() / 1000.f; - float upperLimit = skeletonFinder.sensorBoxTop.get() / 1000.f; - - if (_mask) { - //ofClear(255, 255, 255, 255); - shader.setUniform1i("mask", 1); - glPointSize(blobGrain.get() * 4); - } - else { - shader.setUniform1i("mask", 0); - glPointSize(blobGrain.get() * 2); - } - shader.setUniform1f("lowerLimit", lowerLimit); - shader.setUniform1f("upperLimit", upperLimit); - shader.setUniformMatrix4f("viewMatrixInverse", glm::inverse(ofGetCurrentViewMatrix())); - - ofPushMatrix(); - ofMultMatrix(deviceTransform); - //realSense->draw(); - pointCloudManager.drawPointCloud(); - ofPopMatrix(); - - shader.end(); - - glDisable(GL_DEPTH_TEST); - -} - -void ofApp::drawCalibrationPoints(){ - //ofLog(OF_LOG_NOTICE) << "Entered drawCalibrationPoints"; - glDisable(GL_DEPTH_TEST); - ofPushStyle(); - ofSetColor(255, 0, 0); - ofNoFill(); - ofDrawBitmapString("x", calibPoint_X.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH + 5, calibPoint_X.get().y -5); - ofDrawBitmapString("y", calibPoint_Y.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH + 5, calibPoint_Y.get().y -5); - ofDrawBitmapString("z", calibPoint_Z.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH + 5, calibPoint_Z.get().y -5); - ofDrawCircle(calibPoint_X.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH, calibPoint_X.get().y, 2); - ofDrawCircle(calibPoint_Y.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH, calibPoint_Y.get().y, 2); - ofDrawCircle(calibPoint_Z.get().x/REALSENSE_DEPTH_WIDTH*viewMain.width + VIEWGRID_WIDTH, calibPoint_Z.get().y, 2); - ofPopStyle(); - glEnable(GL_DEPTH_TEST); + glDisable(GL_DEPTH_TEST); } //-------------------------------------------------------------- void ofApp::exit() { - ofLog(OF_LOG_NOTICE, "exiting application..."); + ofLog(OF_LOG_NOTICE) << "exiting application..."; + // Nuitrack auto-releases on destroy ... } void ofApp::createHelp(){ help = string("press v -> to show visualizations\n"); - help += "press 1 - 6 -> to change the viewport\n"; + help += "press 1 - 3 -> to change the viewport\n"; help += "press p -> to show pointcloud\n"; help += "press h -> to show help \n"; help += "press s -> to save current settings.\n"; help += "press l -> to load last saved settings\n"; - help += "press m -> to update mask image CAREFULL: press m again to stop updating (" + ofToString(bUpdateImageMask) + ")\n"; - help += "press c -> to clear mask image\n"; - help += "press x|y|z and then mouse-click -> to change the calibration points in viewport 1\n"; - help += "press k -> to update the calculation\n"; help += "press r -> to show calculation results \n"; - help += "press o -> to open the connection again\n"; - help += "ATTENTION: Setup-Settings (ServerID and Video) will only apply after restart\n"; - help += "Broadcasting ip: "+networkMng.broadcastIP.get()+" port: "+ofToString(networkMng.broadcastPort.get())+" serverID: "+ofToString(networkMng.mServerID)+" \n"; - /* - help += "using opencv threshold = " + ofToString(bThreshWithOpenCV) + " (press spacebar)\n"; - help += "set near threshold " + ofToString(nearThreshold) + " (press: + -)\n"; - help += "set far threshold " + ofToString(farThreshold) + " (press: < >) num blobs found " + ofToString(contourFinder.nBlobs) + "\n"; - */ } //-------------------------------------------------------------- void ofApp::keyPressed(int key){ - bUpdateSetMesurmentPoint = -1; switch (key) { case ' ': break; @@ -798,39 +300,19 @@ void ofApp::keyPressed(int key){ bShowVisuals = !bShowVisuals; break; - case 'c': - //skeletonFinder.clearMask(); - break; - - case 't': - //kinect.close(); - break; - case 'r': - bShowCalcData = !bShowCalcData; - break; - - case 'k': - bUpdateMeasurment = true; + bShowSkeletonData = !bShowSkeletonData; break; case 's': - setupCalib->saveToFile("settings.xml"); skeletonFinder.panel->saveToFile("trackings.xml"); - //skeletonFinder.saveMask(); networkMng.panel->saveToFile("broadcast.xml"); - post->saveToFile("postprocessing.xml"); - device->saveToFile("device.xml"); guitransform->saveToFile("transformation.xml"); break; case 'l': - setupCalib->loadFromFile("settings.xml"); skeletonFinder.panel->loadFromFile("trackings.xml"); - //skeletonFinder.loadMask(); networkMng.panel->loadFromFile("broadcast.xml"); - post->loadFromFile("postprocessing.xml"); - device->loadFromFile("device.xml"); guitransform->loadFromFile("transformation.xml"); break; @@ -841,82 +323,17 @@ void ofApp::keyPressed(int key){ } break; - case '>': - case '.': - //farThreshold ++; - //if (farThreshold > 255) farThreshold = 255; - break; - - case '<': - case ',': - //farThreshold --; - //if (farThreshold < 0) farThreshold = 0; - break; - - case '+': - case '=': - //nearThreshold ++; - //if (nearThreshold > 255) nearThreshold = 255; - break; - - case '-': - //nearThreshold --; - //if (nearThreshold < 0) nearThreshold = 0; - break; - - case 'm': - bUpdateImageMask = !bUpdateImageMask; - if (bUpdateImageMask) { - //skeletonFinder.clearMask(); - } - break; - - case 'x': - bUpdateSetMesurmentPoint = key; - break; - - case 'y': - bUpdateSetMesurmentPoint = key; - break; - - case 'z': - bUpdateSetMesurmentPoint = key; - break; - - case '0': - //kinect.setLed(ofxKinect::LED_OFF); - break; - case '1': iMainCamera = 0; - //kinect.setLed(ofxKinect::LED_GREEN); break; case '2': iMainCamera = 1; - //kinect.setLed(ofxKinect::LED_YELLOW); break; case '3': iMainCamera = 2; - //kinect.setLed(ofxKinect::LED_RED); - break; - - case '4': - iMainCamera = 3; - //kinect.setLed(ofxKinect::LED_BLINK_GREEN); - break; - - case '5': - iMainCamera = 4; - //kinect.setLed(ofxKinect::LED_BLINK_YELLOW_RED); - break; - - case '6': - iMainCamera = 5; - //kinect.setLed(ofxKinect::LED_BLINK_YELLOW_RED); - break; - + break; } } @@ -939,27 +356,6 @@ void ofApp::mouseDragged(int x, int y, int button){ //-------------------------------------------------------------- void ofApp::mousePressed(int x, int y, int button){ - if(iMainCamera == 0 || iMainCamera == 1 && bUpdateSetMesurmentPoint != -1) { - if(bUpdateSetMesurmentPoint == 'x'){ - int posX = (x - VIEWGRID_WIDTH) / viewMain.width * REALSENSE_DEPTH_WIDTH; - int posY = y; - if(0 <= posX && posX < REALSENSE_DEPTH_WIDTH && - 0 <= posY && posY < REALSENSE_DEPTH_HEIGHT) - calibPoint_X.set(glm::vec2(posX, posY)); - }else if(bUpdateSetMesurmentPoint == 'y'){ - int posX = (x - VIEWGRID_WIDTH) / viewMain.width * REALSENSE_DEPTH_WIDTH; - int posY = y; - if(0 <= posX && posX < REALSENSE_DEPTH_WIDTH && - 0 <= posY && posY < REALSENSE_DEPTH_HEIGHT) - calibPoint_Y.set(glm::vec2(posX, posY)); - }else if(bUpdateSetMesurmentPoint == 'z'){ - int posX = (x - VIEWGRID_WIDTH) / viewMain.width * REALSENSE_DEPTH_WIDTH; - int posY = y; - if(0 <= posX && posX < REALSENSE_DEPTH_WIDTH && - 0 <= posY && posY < REALSENSE_DEPTH_HEIGHT) - calibPoint_Z.set(glm::vec2(posX, posY)); - } - } } //-------------------------------------------------------------- @@ -968,12 +364,6 @@ void ofApp::mouseReleased(int x, int y, int button){ //-------------------------------------------------------------- void ofApp::windowResized(int w, int h){ - /* - cairo->setupMemoryOnly(ofCairoRenderer::IMAGE, - false, false, - ofRectangle(0, 0, w, h)); - render.allocate(w, h, GL_RGBA); - */ setupViewports(); } diff --git a/src/ofApp.h b/src/ofApp.h index 4cca18333be9c05290ea9e416a1f3c2e38a9816c..b7ef92ee0d2cb86e621e97ffeb73d67f9d1c0fad 100644 --- a/src/ofApp.h +++ b/src/ofApp.h @@ -17,11 +17,11 @@ #include -#define N_CAMERAS 6 +#define N_CAMERAS 3 -#define VIEWGRID_WIDTH 132 -#define MENU_WIDTH 1000 -#define VIEWPORT_HEIGHT 480 +#define VIEWGRID_WIDTH 264 +#define MENU_WIDTH 500 +#define VIEWPORT_HEIGHT 720 #define REALSENSE_DEPTH_WIDTH 848 #define REALSENSE_DEPTH_HEIGHT 480 @@ -39,31 +39,30 @@ using namespace ofxnui; class ofApp : public ofBaseApp{ - public: +public: - void setup(); - void update(); - void draw(); - void exit(); + 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 windowResized(int w, int h); - void dragEvent(ofDragInfo dragInfo); - void gotMessage(ofMessage msg); - - vector storeText; + 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 windowResized(int w, int h); + void dragEvent(ofDragInfo dragInfo); + void gotMessage(ofMessage msg); + + vector storeText; - //ofxUDPManager udpConnection; + //ofxUDPManager udpConnection; - ofTrueTypeFont mono; - ofTrueTypeFont monosm; - vector stroke; - + ofTrueTypeFont mono; + ofTrueTypeFont monosm; + vector stroke; bool bShowVisuals = false; @@ -97,16 +96,16 @@ class ofApp : public ofBaseApp{ ofTexture render; ///////////// - //RealSense// + //Nuitrack // ///////////// void initNuitrack(); + + void reloadTransformMatrix(); TrackerRef tracker; PointCloudManager pointCloudManager; - ofMatrix4x4 unprojection; - bool dispRaw; bool bPreviewPointCloud; @@ -114,22 +113,9 @@ class ofApp : public ofBaseApp{ ofVboMesh previewmesh;//, capturemesh; CaptureMeshArray capMesh; - - Frustum realSenseFrustum; - /** - Changes operation of application - @param _index 0=normal, 1= recording to file, 2=playback from file - */ - void changeOperation(int& _index); - void drawPreview(); - void drawCapturePointCloud(bool _mask); - - void createGUIDeviceParams(); - void createFrustumCone(); - void updateFrustumCone(int& val); ///////////////// //COLOR CONTOUR// @@ -139,99 +125,23 @@ class ofApp : public ofBaseApp{ // used for viewing the point cloud ofEasyCam previewCam; + + ofMatrix4x4 deviceToWorldTransform; + ofMatrix4x4 worldToDeviceTransform; - ofShader shader; - - /////////////// - //CALCULATION// - /////////////// - - void updateCalc(); - void updateMatrix(); - void measurementCycleRaw(); - void measurementCycleFine(); - - void drawCalibrationPoints(); - glm::vec3 calcPlanePoint(ofParameter & cpoint, int _size, int _step); - - bool bUpdateCalc = false; - bool bUpdateMeasurment = false; - bool bUpdateMeasurmentFine = false; - bool bUpdateImageMask = false; - char bUpdateSetMesurmentPoint = -1; - - int cycleCounter = 0; - - ofVec3f planePoint1Meas[N_MEASURMENT_CYCLES]; - ofVec3f planePoint2Meas[N_MEASURMENT_CYCLES]; - ofVec3f planePoint3Meas[N_MEASURMENT_CYCLES]; - - ofVec3f planePoint_X; - ofVec3f planePoint_Y; - ofVec3f planePoint_Z; - - ofVec3f planeCenterPoint; - - ofSpherePrimitive sphere_X; - ofSpherePrimitive sphere_Y; - ofSpherePrimitive sphere_Z; - - ofSpherePrimitive frustumCenterSphere; - ofSpherePrimitive frustumTopSphere; - - ofVboMesh geometry; - - ofMatrix4x4 deviceTransform; - - string calcdata; - - bool bShowCalcData; + bool bShowSkeletonData = true; ////////////// //PROPERTIES// ////////////// - void setupCalibGui(); void setupTransformGui(); ofxGui gui; - ofxGuiPanel *setupCalib; - ofxGuiPanel *device; - ofxGuiPanel *post; ofxGuiPanel *guitransform; - ofxGuiPanel *operating; - - //mode panel - ofxGuiGroup *operatingToggles; - - ofParameterGroup operatingModes; - ofParameter mode0Capture; - ofParameter mode1Record; - ofParameter mode2Playback; - - ofParameter calibPoint_X; - ofParameter calibPoint_Y; - ofParameter calibPoint_Z; - ofParameterGroup transformationGuiGroup; - ofParameter transformation; - ofParameterGroup frustumGuiGroup; - - ofParameter nearFrustum; - ofParameter farFrustum; - - ofParameterGroup intrinsicGuiGroup; - - ofParameter depthCorrectionBase; - ofParameter depthCorrectionDivisor; - ofParameter pixelSizeCorrector; - - ofParameter blobGrain; - - ofParameter captureVideo; - ////////// // HELP // ////////// @@ -241,5 +151,7 @@ class ofApp : public ofBaseApp{ void createHelp(); -}; - +private: + const static ofMatrix4x4 nuitrackViewportToRealSenseViewportTransform; + static ofMatrix4x4 makeNuitrackToRealSenseTransform(); +}; \ No newline at end of file