Loading src/PointCloudManager.cpp +1 −12 Original line number Diff line number Diff line Loading @@ -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) { Loading Loading @@ -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)); } } } Loading src/SkeletonFinder.cpp +37 −7 Original line number Diff line number Diff line // // SkeletonFinder.cpp // // Created by Pierre Brki on 19.05.20. // Created by Pierre B�rki on 19.05.20. // Adapted from BlobFinder.cpp by maybites (14.02.14). // Loading @@ -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); Loading @@ -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<ofxGuiIntInputField>(sensorBoxLeft.set("left", 1000)); sensorBoxGuiGroup->add<ofxGuiIntInputField>(sensorBoxRight.set("right", -1000)); sensorBoxGuiGroup->add<ofxGuiIntInputField>(sensorBoxFront.set("front", 1000)); Loading @@ -45,17 +44,22 @@ void SkeletonFinder::update(nuitrack::SkeletonData::Ptr data) { vector<Joint> 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<Skeleton> SkeletonFinder::getSkeletons() { vector<Skeleton> SkeletonFinder::getSkeletons() const { return skeletons; } Loading Loading @@ -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); Loading Loading @@ -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; } src/SkeletonFinder.h +6 −5 Original line number Diff line number Diff line Loading @@ -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<Skeleton> getSkeletons(); string getShortDesc(); vector<Skeleton> getSkeletons() const; private: void updateSensorBox(int & value); bool isSkeletonInBounds(const Skeleton& skel); ofxnui::TrackerRef tracker; vector<Skeleton> skeletons; Loading @@ -83,6 +83,7 @@ public: ofxGuiPanel *panel; ofxGuiGroup *sensorBoxGuiGroup; ofParameter<bool> filtering; ofParameter<int> sensorBoxLeft; ofParameter<int> sensorBoxRight; ofParameter<int> sensorBoxTop; Loading src/TrackingNetworkManager.cpp +39 −251 Original line number Diff line number Diff line Loading @@ -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<ofxGuiIntInputField>(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<ofxGuiTextField>(broadcastIP.set("TX IP","127.0.0.1")); broadcastGroup->add<ofxGuiIntInputField>(broadcastPort.set("TX Port", NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT + 99)); Loading @@ -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(); Loading @@ -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 <ClientListeningPort> | 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 <ClientListeningPort> | 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. Loading @@ -164,160 +75,34 @@ void TrackingNetworkManager::update(SkeletonFinder & _blobFinder, Frustum & _fru } } void TrackingNetworkManager::sendTrackingData(SkeletonFinder & _blobFinder){ return; /* void TrackingNetworkManager::sendTrackingData(const SkeletonFinder& skeletonFinder) { // 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); vector<Skeleton> skeletons = skeletonFinder.getSkeletons(); if (skeletons.size() > 0) { // Only one skeleton is to be on the scene for the perspective to work sendSkeletonData(skeletons[0]); } 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; } } // 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); 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::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); */ sendMessageToTrackingClients(skeletonMsg); } 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); */ } void TrackingNetworkManager::sendMessageToTrackingClients(ofxOscMessage _msg){ Loading @@ -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--; Loading @@ -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; } Loading @@ -361,7 +150,6 @@ void TrackingNetworkManager::sendBroadCastAddress(){ broadcastSender.sendMessage(broadcast); broadCastTimer = ofGetElapsedTimeMillis(); //ofLog(OF_LOG_NOTICE, "Sent Broadcastmessage"); } //-------------------------------------------------------------- Loading src/TrackingNetworkManager.h +6 −21 Original line number Diff line number Diff line Loading @@ -32,26 +32,16 @@ class TrackingNetworkManager { public: TrackingNetworkManager(); void setup(ofxGui &gui, string _realsenseSerial); void update(const SkeletonFinder& skeletonFinder); void setup(ofxGui &gui, string _kinectSerial); void update(SkeletonFinder & _SkeletonFinder, Frustum & _frustum, ofMatrix4x4 _trans); void sendTrackingData(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); Loading Loading @@ -99,12 +89,7 @@ public: ofParameterGroup streamingGuiGroup; ofParameter<bool> streamingBodyBlob; ofParameter<bool> streamingHeadBlob; ofParameter<bool> streamingHead; ofParameter<bool> streamingEye; ofParameter<bool> streamingWholeBody; }; Loading
src/PointCloudManager.cpp +1 −12 Original line number Diff line number Diff line Loading @@ -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) { Loading Loading @@ -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)); } } } Loading
src/SkeletonFinder.cpp +37 −7 Original line number Diff line number Diff line // // SkeletonFinder.cpp // // Created by Pierre Brki on 19.05.20. // Created by Pierre B�rki on 19.05.20. // Adapted from BlobFinder.cpp by maybites (14.02.14). // Loading @@ -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); Loading @@ -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<ofxGuiIntInputField>(sensorBoxLeft.set("left", 1000)); sensorBoxGuiGroup->add<ofxGuiIntInputField>(sensorBoxRight.set("right", -1000)); sensorBoxGuiGroup->add<ofxGuiIntInputField>(sensorBoxFront.set("front", 1000)); Loading @@ -45,17 +44,22 @@ void SkeletonFinder::update(nuitrack::SkeletonData::Ptr data) { vector<Joint> 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<Skeleton> SkeletonFinder::getSkeletons() { vector<Skeleton> SkeletonFinder::getSkeletons() const { return skeletons; } Loading Loading @@ -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); Loading Loading @@ -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; }
src/SkeletonFinder.h +6 −5 Original line number Diff line number Diff line Loading @@ -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<Skeleton> getSkeletons(); string getShortDesc(); vector<Skeleton> getSkeletons() const; private: void updateSensorBox(int & value); bool isSkeletonInBounds(const Skeleton& skel); ofxnui::TrackerRef tracker; vector<Skeleton> skeletons; Loading @@ -83,6 +83,7 @@ public: ofxGuiPanel *panel; ofxGuiGroup *sensorBoxGuiGroup; ofParameter<bool> filtering; ofParameter<int> sensorBoxLeft; ofParameter<int> sensorBoxRight; ofParameter<int> sensorBoxTop; Loading
src/TrackingNetworkManager.cpp +39 −251 Original line number Diff line number Diff line Loading @@ -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<ofxGuiIntInputField>(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<ofxGuiTextField>(broadcastIP.set("TX IP","127.0.0.1")); broadcastGroup->add<ofxGuiIntInputField>(broadcastPort.set("TX Port", NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT, NETWORK_BROADCAST_PORT + 99)); Loading @@ -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(); Loading @@ -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 <ClientListeningPort> | 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 <ClientListeningPort> | 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. Loading @@ -164,160 +75,34 @@ void TrackingNetworkManager::update(SkeletonFinder & _blobFinder, Frustum & _fru } } void TrackingNetworkManager::sendTrackingData(SkeletonFinder & _blobFinder){ return; /* void TrackingNetworkManager::sendTrackingData(const SkeletonFinder& skeletonFinder) { // 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); vector<Skeleton> skeletons = skeletonFinder.getSkeletons(); if (skeletons.size() > 0) { // Only one skeleton is to be on the scene for the perspective to work sendSkeletonData(skeletons[0]); } 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; } } // 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); 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::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); */ sendMessageToTrackingClients(skeletonMsg); } 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); */ } void TrackingNetworkManager::sendMessageToTrackingClients(ofxOscMessage _msg){ Loading @@ -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--; Loading @@ -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; } Loading @@ -361,7 +150,6 @@ void TrackingNetworkManager::sendBroadCastAddress(){ broadcastSender.sendMessage(broadcast); broadCastTimer = ofGetElapsedTimeMillis(); //ofLog(OF_LOG_NOTICE, "Sent Broadcastmessage"); } //-------------------------------------------------------------- Loading
src/TrackingNetworkManager.h +6 −21 Original line number Diff line number Diff line Loading @@ -32,26 +32,16 @@ class TrackingNetworkManager { public: TrackingNetworkManager(); void setup(ofxGui &gui, string _realsenseSerial); void update(const SkeletonFinder& skeletonFinder); void setup(ofxGui &gui, string _kinectSerial); void update(SkeletonFinder & _SkeletonFinder, Frustum & _frustum, ofMatrix4x4 _trans); void sendTrackingData(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); Loading Loading @@ -99,12 +89,7 @@ public: ofParameterGroup streamingGuiGroup; ofParameter<bool> streamingBodyBlob; ofParameter<bool> streamingHeadBlob; ofParameter<bool> streamingHead; ofParameter<bool> streamingEye; ofParameter<bool> streamingWholeBody; };