Move Nuitrack init outside of skeleton finder (6a9d9c93) · Commits · Filière informatique / 2019-20 / DLM-TB / NIFFF20 / The Hole

src/SkeletonFinder.cpp

+27 −422

Original line number	Diff line number	Diff line
		@@ -7,426 +7,6 @@

		#include "SkeletonFinder.h"

		void SkeletonFinder::initNuitrack() {
		tracker = ofxnui::Tracker::create();
		tracker->init("");

		// depth feed settings
		tracker->setConfigValue("Realsense2Module.Depth.FPS", "30");
		tracker->setConfigValue("Realsense2Module.Depth.RawWidth", "1280");
		tracker->setConfigValue("Realsense2Module.Depth.RawHeight", "720");
		tracker->setConfigValue("Realsense2Module.Depth.ProcessWidth", "1280");
		tracker->setConfigValue("Realsense2Module.Depth.ProcessHeight", "720");
		tracker->setConfigValue("Realsense2Module.Depth.ProcessMaxDepth", "7000");

		// rgb feed settings
		tracker->setConfigValue("Realsense2Module.RGB.FPS", "30");
		tracker->setConfigValue("Realsense2Module.RGB.RawWidth", "1280");
		tracker->setConfigValue("Realsense2Module.RGB.RawHeight", "720");
		tracker->setConfigValue("Realsense2Module.RGB.ProcessWidth", "1280");
		tracker->setConfigValue("Realsense2Module.RGB.ProcessHeight", "720");

		// post processing settings
		tracker->setConfigValue("Realsense2Module.Depth.PostProcessing.SpatialFilter.spatial_alpha", "0.1");
		tracker->setConfigValue("Realsense2Module.Depth.PostProcessing.SpatialFilter.spatial_delta", "50");

		// distance settings
		tracker->setConfigValue("Segmentation.MAX_DISTANCE", "7000");
		tracker->setConfigValue("Skeletonization.MaxDistance", "7000");

		tracker->setIssuesCallback([this](nuitrack::IssuesData::Ptr data) {
		auto issue = data->getIssue<nuitrack::Issue>();
		if (issue) {
		ofLogNotice() << "Issue detected! " << issue->getName() << " [" << issue->getId() << "]";
		}
		});
		//tracker->setRGBCallback([](nuitrack::RGBFrame::Ptr) {ofLog(OF_LOG_NOTICE) << "Skeleton found ! None position : "; });
		//tracker->setDepthCallback([](nuitrack::DepthFrame::Ptr) {});
		//// skeleton detection callback
		//tracker->setSkeletonCallback([this](nuitrack::SkeletonData::Ptr data) {
		// update(data);
		// });
		}

		void SkeletonFinder::setup(ofMatrix4x4* transformMatrix, ofxGui& gui) {
		this->transformMatrix = transformMatrix;
		initNuitrack();
		initGUI(gui);
		}

		void SkeletonFinder::update(nuitrack::SkeletonData::Ptr data) {
		skeletons.clear();
		// TODO: filter using the capture bounds
		for (nuitrack::Skeleton skel : data->getSkeletons()) {
		/*vector<Joint> joints;
		for (nuitrack::Joint joint : skel.joints) {
		ofVec3f pos = ofxnui::Tracker::fromVector3(joint.real);
		pos = transformMatrix pos;

		joints.push_back(Joint(joint.type, joint.confidence, pos));
		}

		skeletons.push_back(Skeleton(skel.id, joints));*/
		ofLog(OF_LOG_NOTICE) << "Skeleton found !";
		/*ofLog(OF_LOG_NOTICE) << "Skeleton found ! None position : " << joints[0].pos;
		ofLog(OF_LOG_NOTICE) << "Skeleton found ! Head position : " << joints[0].pos;*/
		}
		}

		void SkeletonFinder::run() {
		tracker->run();
		}


		// TODO: remove once it is safe to do so
		// There could be something of interest here
		void /SkeletonFinder::/update() {
		return;
		//ofColor white = ofColor::white;
		//ofColor black = ofColor::black;

		///****************************************************************
		// PREPARE NEW FRAME
		//*****************************************************************/

		//int minID = 0;

		////tells all the blobs that the next frame is comming
		//for (int e = 0; e < skeletonEvents.size(); e++) {
		// skeletonEvents[e].updatePrepare();
		// minID = (skeletonEvents[e].mID >= minID) ? skeletonEvents[e].mID + 1 : minID;
		//}

		//if (useMask.get()) {
		// fbo.begin();
		// // Cleaning everthing with alpha mask on 0 in order to make it transparent for default
		// ofClear(0, 0, 0, 0);

		// maskShader.begin();
		// maskShader.setUniformTexture("maskTex", maskImg.getTexture(), 1);

		// captureFBO.draw(0, 0);

		// maskShader.end();
		// fbo.end();

		// fbo.readToPixels(fbopixels);
		//}
		//else {
		// captureFBO.readToPixels(fbopixels);
		//}

		// colorImg.setFromPixels(fbopixels);

		// // load grayscale captured depth image from the color source
		// grayImage.setFromColorImage(colorImg);
		//
		// //grayImage.blurHeavily();
		//
		//ofPixelsRef blobRefPixls = skeletonRef.getPixels();
		//
		// ofPixelsRef greyref = grayImage.getPixels();
		//ofPixelsRef eyeRef = grayEyeLevel.getPixels();
		//
		//eyeRef.setColor(black);

		// float sensorFieldFront = sensorBoxFront.get() * SCALE;
		// float sensorFieldBack = sensorBoxBack.get() * SCALE;
		// float sensorFieldLeft = sensorBoxLeft.get() * SCALE;
		// float sensorFieldRight = sensorBoxRight.get() * SCALE;
		// float sensorFieldTop = sensorBoxTop .get() * SCALE;
		// float sensorFieldBottom = sensorBoxBottom.get() * SCALE;
		// float sensorFieldWidth = sensorFieldRight - sensorFieldLeft;
		// float sensorFieldHeigth = sensorFieldTop - sensorFieldBottom;
		// float sensorFieldDepth = sensorFieldBack - sensorFieldFront;
		//
		// int eyeLevelColor = eyeLevel.get() / sensorFieldHeigth * 255;
		//
		// int headTopThreshold = eyeLevelColor / 4;
		//
		// //ofLog(OF_LOG_NOTICE, "eyeLevelColor = " + ofToString(eyeLevelColor));
		//
		// //ofLog(OF_LOG_NOTICE, "eyref size : " + ofToString(eyeRef.size()));
		//
		///****************************************************************
		// FIND BODY CONTOURS
		//*****************************************************************/

		//int minBlobSize = pow(blobAreaMinStp1.get() * sensorFboSize.get(), 2);
		//int maxBlobSize = pow(blobAreaMax.get() * sensorFboSize.get(), 2);


		//detectedHeads.clear();

		//contourFinder.findContours(grayImage, minBlobSize, maxBlobSize, countBlob.get(), false);

		// for (int i = 0; i < contourFinder.nBlobs; i++){
		// ofRectangle bounds = contourFinder.blobs[i].boundingRect;
		// float pixelBrightness = 0;
		// float brightness = 0;
		//
		// // find the brightest pixel within the blob. this defines the height of the blob
		// for(int x = bounds.x; x < bounds.x + bounds.width; x++){
		// for(int y = bounds.y; y < bounds.y + bounds.height; y++){
		// pixelBrightness = greyref.getColor(x, y).getBrightness();
		// brightness = (pixelBrightness > brightness)?pixelBrightness: brightness;
		// }
		// }

		// /*
		// float averageBrightness = 0;
		// int averageCounter = 0;

		// // go through the pixels again and get the average brightness for the headTopThreshold
		// for(int x = bounds.x; x < bounds.x + bounds.width; x++){
		// for(int y = bounds.y; y < bounds.y + bounds.height; y++){
		// pixelBrightness = greyref.getColor(x, y).getBrightness();
		// if(pixelBrightness > brightness - headTopThreshold){
		// averageBrightness += pixelBrightness;
		// averageCounter++;
		// }
		// }
		// }
		//
		// brightness = averageBrightness / averageCounter;
		// */
		//
		// // find all the pixels down to the eyelevel threshold. this yealds an image with blobs that mark the size of the head at eyelevel.
		// ofVec2f headtop2d = ofVec2f();
		// int brighCounter = 0;
		// for(int x = bounds.x; x < bounds.x + bounds.width; x++){
		// for(int y = bounds.y; y < bounds.y + bounds.height; y++){
		// pixelBrightness = greyref.getColor(x, y).getBrightness();
		// if(pixelBrightness > (brightness - eyeLevelColor)){
		// //writes the pixels above the eyelevel into the eyeRef image
		// eyeRef.setColor(x, y, brightness);
		// }else{
		// eyeRef.setColor(x, y, black);
		// }
		// if(pixelBrightness >= brightness - (eyeLevelColor / 4)){
		// headtop2d += ofVec2f(x, y);
		// brighCounter++;
		// }
		// }
		// }
		// headtop2d /= brighCounter;
		//
		// //ofLog(OF_LOG_NOTICE, "headtop2d = " + ofToString(headtop2d));
		//
		// ofVec3f headTop = ofVec3f((headtop2d.x / captureScreenSize.x) * sensorFieldWidth + sensorFieldLeft, sensorFieldBack - (headtop2d.y / captureScreenSize.y ) * sensorFieldDepth, (brightness / 255.0) * sensorFieldHeigth + sensorFieldBottom);

		// //ofVec3f headCenter = ofVec3f(headTop.x, headTop.y, headTop.z - eyeLevel.get());

		// DetectedSkeleton newBodyBlob;
		// newBodyBlob.bound = bounds;
		// newBodyBlob.headTop = headTop;
		// newBodyBlob.hasBeenTaken = false;

		// detectedHeads.push_back(newBodyBlob);

		//}

		///****************************************************************
		// UPDATE BODY EVENTS
		//*****************************************************************/

		//int matchBlobID = -1;
		//int matchEventID = -1;
		//float minDistance = 1000000;

		//do {
		// matchBlobID = -1;
		// matchEventID = -1;
		// minDistance = 1000000;
		// for (int i = 0; i < skeletonEvents.size(); i++) { // iterate through all the current events
		// if (skeletonEvents[i].isAlive() && !skeletonEvents[i].hasBeenUpdated()) { // all those that haven't been updated yet
		// for (int j = 0; j < detectedHeads.size(); j++) { // now we go through all the new blobs
		// if (!detectedHeads[j].hasBeenTaken) { // but only if the blobs havent been taken before
		// float dist = skeletonEvents[i].getDistance(detectedHeads[j].headTop);
		// if (dist < eventMaxSize.get()*SCALE) { // and test if the distance is within the threshold
		// if (minDistance > dist) {
		// minDistance = dist;
		// matchBlobID = j;
		// matchEventID = i;
		// }
		// }
		// }
		// }
		// }
		// }
		// if (matchEventID >= 0) {
		// ofRectangle bounds = detectedHeads[matchBlobID].bound;
		// ofVec3f headTop = detectedHeads[matchBlobID].headTop;

		// //calculate the blob pos in worldspace
		// ofVec3f blobPos = ofVec3f(((float)bounds.getCenter().x / captureScreenSize.x) * sensorFieldWidth + sensorFieldLeft, sensorFieldBack - ((float)bounds.getCenter().y / captureScreenSize.y) * sensorFieldDepth, headTop.z);

		// //calculate the blob size in worldspace
		// ofVec2f blobSize = ofVec2f(((float)bounds.getWidth() / captureScreenSize.x) * sensorFieldWidth, ((float)bounds.getHeight() / captureScreenSize.y) * sensorFieldDepth);

		// //ofLogVerbose("Updating old event with ID: " + ofToString(blobEvents[matchEventID].mID));

		// skeletonEvents[matchEventID].update(bounds, blobPos, blobSize, headTop, smoothFactor.get());
		// detectedHeads[matchBlobID].hasBeenTaken = true;
		// }
		//} while (matchEventID >= 0);

		///****************************************************************
		// CREATE NEW BODY EVENTS
		//*****************************************************************/
		//for (int j = 0; j < detectedHeads.size(); j++) {
		// if (!detectedHeads[j].hasBeenTaken) {
		// ofRectangle bounds = detectedHeads[j].bound;
		// ofVec3f headTop = detectedHeads[j].headTop;

		// //calculate the blob pos in worldspace
		// ofVec3f blobPos = ofVec3f(((float)bounds.getCenter().x / captureScreenSize.x) * sensorFieldWidth + sensorFieldLeft, sensorFieldBack - ((float)bounds.getCenter().y / captureScreenSize.y) * sensorFieldDepth, headTop.z);

		// //calculate the blob size in worldspace
		// ofVec2f blobSize = ofVec2f(((float)bounds.getWidth() / captureScreenSize.x) * sensorFieldWidth, ((float)bounds.getHeight() / captureScreenSize.y) * sensorFieldDepth);

		// //ofLogVerbose("Creating new event with ID: " + ofToString(minID));

		// skeletonEvents.push_back(SkeletonTracker(minID++, eventBreathSize.get(), bounds, blobPos, blobSize, headTop));
		// }
		//}


		///****************************************************************
		// FIND HEAD CONTOURS
		//*****************************************************************/
		///**

		// //preprocesses the eyeRef image
		// //grayEyeLevel.setFromPixels(eyeRef.getPixels(), eyeRef.getWidth(), eyeRef.getHeight());
		// //grayEyeLevel.invert();
		// //grayEyeLevel.threshold(20);
		// //grayEyeLevel.invert();
		// grayEyeLevel.blurGaussian();

		// //ofLog(OF_LOG_NOTICE, "contourEyeFinder nBlobs : " + ofToString(contourEyeFinder.nBlobs));

		//int minBlobSize2 = pow(blobAreaMinStp2.get() * sensorFboSize.get(), 2);

		// //find head shape on eye height contours
		// contourEyeFinder.findContours(grayEyeLevel, minBlobSize2, maxBlobSize, countBlob.get(), false);
		// for(int i = 0; i < contourEyeFinder.nBlobs; i++){

		// ofRectangle bounds = contourEyeFinder.blobs[i].boundingRect;

		// int brightness = eyeRef.getColor((float)bounds.getCenter().x, (float)bounds.getCenter().y).getBrightness();
		// float height = (brightness / 255.0) * sensorFieldHeigth + sensorFieldBottom;

		// //calculate the blob pos in worldspace
		// ofVec3f headBlobCenter = ofVec3f(((float)bounds.getCenter().x / captureScreenSize.x) * sensorFieldWidth + sensorFieldLeft, sensorFieldBack - ((float)bounds.getCenter().y / captureScreenSize.y) * sensorFieldDepth, height);

		// //calculate the blob size in worldspace
		// ofVec2f headBlobSize = ofVec2f(((float)bounds.getWidth() / captureScreenSize.x) * sensorFieldWidth, ((float)bounds.getHeight() / captureScreenSize.y) * sensorFieldDepth);

		// //calculate the gazeVector
		// //ofVec3f gaze = blobEvents[bid].getCurrentHeadCenter() - gazePoint.get();

		// //gaze.z = 0;

		// float smalestAngle = 180;
		// ofVec3f eyePoint = headBlobCenter;

		// //clears the contour storage
		// blobEvents[bid].countour.clear();

		// // findes the closest contour point to the eyegave-vector, takes its distance to the headCenter and calculated
		// // the eye - center - point
		// for (int v = 0; v < contourEyeFinder.blobs[i].pts.size(); v++) {
		// ofVec3f headPoint = ofVec3f(((float)contourEyeFinder.blobs[i].pts[v].x / captureScreenSize.x) * sensorFieldWidth + sensorFieldLeft, sensorFieldBack - ((float)contourEyeFinder.blobs[i].pts[v].y / captureScreenSize.y) * sensorFieldDepth, blobEvents[bid].headCenter.z);

		// blobEvents[bid].countour.push_back(headPoint);

		// ofVec3f gaze2 = blobEvents[bid].getCurrentHeadCenter() - headPoint;

		// float angle = gaze.angle(gaze2);

		// if (smalestAngle > angle) {
		// smalestAngle = angle;
		// eyePoint = blobEvents[bid].getCurrentHeadCenter() - gaze.normalize().scale(gaze2.length() * eyeInset.get());
		// }
		// }

		// bool foundBlob = false;

		// for(int bid = 0; bid < blobEvents.size(); bid++){
		// // find the blob
		// if(!blobEvents[bid].hasBeenUpdated() && blobEvents[bid].isMatching(bounds, eventMaxSize.get())){
		//
		// /////////////////////////////////////////////////////////////////
		// UPDATE HEAD EVENTS
		// /////////////////////////////////////////////////////////////////

		// ofLogVerbose("Updating old event with ID: " + ofToString(blobEvents[bid].mID) + " headHight = " + ofToString(headBlobCenter.z));

		// blobEvents[bid].update(bounds, headBlobCenter, headBlobSize, eyePoint, smoothFactor.get());
		// foundBlob = true;
		// break;
		// }
		// }
		// // if none is found, create a new one.
		// if (!foundBlob) {
		// ofLogVerbose("Creating new event with ID: " + ofToString(minID));
		// blobEvents.push_back(BlobTracker(minID++, bounds, eventBreathSize.get()));
		// blobEvents.back().update(bounds, headBlobCenter, headBlobSize, eyePoint, smoothFactor.get());
		// }

		// }
		//*/

		///****************************************************************
		// SORT EVENTS
		//*****************************************************************/

		///*
		////sets the sort value to the current index.
		//int sortPos = 0;

		//for (int i = 0; i < blobEvents.size(); i++) {
		// blobEvents[i].sortPos = sortPos++;
		//}

		//// if we are using the gaze point
		//if (useGazePoint.get()) {
		// if (blobEvents.size() > 0) {
		// for (int i = 0; i < (blobEvents.size() - 1); i++) {
		// for (int j = 1; j < blobEvents.size(); j++) {
		// if ((blobEvents[i].headCenter - gazePoint.get()).length() < (blobEvents[j].headCenter - gazePoint.get()).length()) {
		// if (blobEvents[i].sortPos > blobEvents[j].sortPos) {
		// int savepos = blobEvents[j].sortPos;
		// blobEvents[j].sortPos = blobEvents[i].sortPos;
		// blobEvents[i].sortPos = savepos;
		// }
		// }
		// else {
		// if (blobEvents[i].sortPos < blobEvents[j].sortPos) {
		// int savepos = blobEvents[j].sortPos;
		// blobEvents[j].sortPos = blobEvents[i].sortPos;
		// blobEvents[i].sortPos = savepos;
		// }
		// }
		// }
		// }
		// }
		//}
		//*/

		//
		///****************************************************************
		// CLEANUP EVENTS
		//*****************************************************************/

		// //updates all alive blobs and removes all the blobs that havent had an update for a specific number of frames or have been killed
		// for(int e = skeletonEvents.size() - 1; e >= 0; e--){
		// if(skeletonEvents[e].isDead()){
		// skeletonEvents.erase(skeletonEvents.begin() + e);
		// }
		// }
		}

		void SkeletonFinder::initGUI(ofxGui& gui) {
		panel = gui.addPanel();
		@@ -454,6 +34,31 @@ void SkeletonFinder::initGUI(ofxGui& gui) {
		panel->loadFromFile("trackings.xml");
		}

		void SkeletonFinder::setTransformMatrix(ofMatrix4x4* mat) {
		transformMatrix = mat;
		}

		void SkeletonFinder::update(nuitrack::SkeletonData::Ptr data) {
		skeletons.clear();
		// TODO: filter using the capture bounds
		for (nuitrack::Skeleton skel : data->getSkeletons()) {
		vector<Joint> joints;
		for (nuitrack::Joint joint : skel.joints) {
		glm::vec3 pos = ofxnui::Tracker::fromVector3(joint.real);
		// pos = transformMatrix pos;
		pos = 0.001 * pos;

		joints.push_back(Joint(joint.type, joint.confidence, pos));
		}

		skeletons.push_back(Skeleton(skel.id, joints));
		}
		}

		vector<Skeleton> SkeletonFinder::getSkeletons() {
		return skeletons;
		}

		void SkeletonFinder::drawSensorBox()
		{
		sensorBox.draw();
		@@ -475,7 +80,7 @@ void SkeletonFinder::drawSkeletons2d(ofRectangle _rect) {
		}*/
		}

		// adapted from example
		// adapted from ofxNuitrack example
		void SkeletonFinder::drawSkeletons() {
		static vector<Bone> bones = {
		Bone(nuitrack::JOINT_WAIST, nuitrack::JOINT_TORSO, glm::vec3(0, 1, 0)),
		@@ -501,6 +106,7 @@ void SkeletonFinder::drawSkeletons() {
		Bone(nuitrack::JOINT_RIGHT_KNEE, nuitrack::JOINT_RIGHT_ANKLE, glm::vec3(0, -1, 0)),
		};

		ofSetColor(0, 255, 0);
		for (Skeleton skel : skeletons) {
		for (Bone bone : bones) {
		auto j1 = skel.joints[bone.from];
		@@ -510,7 +116,6 @@ void SkeletonFinder::drawSkeletons() {
		continue;
		}

		ofSetColor(ofColor(0, 1, 0));
		ofDrawLine(j1.pos, j2.pos);
		}
		}

src/SkeletonFinder.h

+6 −38

Original line number	Diff line number	Diff line
		@@ -26,41 +26,12 @@

		using namespace tdv;

		// TODO: This is the file which will change the most
		/* included in ofApp and TrackingNetworkManager
		methods and fields called in ofApp :
		setup(gui)
		panel->set{Width,Position}
		{
		//update
		skeletonFinder.captureMaskBegin();
		drawCapturePointCloud(true);
		skeletonFinder.captureMaskEnd();
		}
		update
		{
		//draw
		skeletonFinder.grayImage.draw(viewGrid[2]);
		skeletonFinder.contourFinder.draw(viewGrid[3]);
		skeletonFinder.maskFbo.draw(viewGrid[4]);
		}
		drawSkel2d
		drawSkel
		sensorBoxXXX
		clearMask
		saveMask
		loadMask
		methods and fields called in TrackingetworkManager :
		sensorBoxXXX
		blobEvents // should be bypassed via nuitrack itself
		*/

		struct Joint {
		nuitrack::JointType type;
		float confidence;
		ofVec3f pos;
		glm::vec3 pos;

		Joint(nuitrack::JointType type, float confidence, ofVec3f pos) :
		Joint(nuitrack::JointType type, float confidence, glm::vec3 pos) :
		type(type), confidence(confidence), pos(pos) {}
		};

		@@ -89,8 +60,9 @@ class SkeletonFinder {
		public:
		SkeletonFinder() {}

		void setup(ofMatrix4x4* transformMatrix, ofxGui &gui);
		void run();
		void initGUI(ofxGui& gui);
		void setTransformMatrix(ofMatrix4x4* mat);
		void update(nuitrack::SkeletonData::Ptr data);

		void updateSensorBox(int & value);

		@@ -98,18 +70,14 @@ public:
		void drawSkeletons2d(ofRectangle _rect);
		void drawSkeletons();

		//vector<Skeleton> getSkeletons();
		vector<Skeleton> getSkeletons();

		private:

		void initNuitrack();
		void initGUI(ofxGui& gui);

		ofxnui::TrackerRef tracker;
		vector<Skeleton> skeletons;

		ofMatrix4x4* transformMatrix;
		void update(nuitrack::SkeletonData::Ptr data);

		public:
		ofxGuiPanel *panel;