Commit 38f1d30c authored by Pierre Bürki's avatar Pierre Bürki
Browse files

Fix basis changes

parent f6a671c9

src/PointCloudManager.cpp

+1 −1
Original line number Diff line number Diff line
@@ -57,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));

		}

	}

}

src/SkeletonFinder.cpp

+4 −4
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).

//
@@ -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);
@@ -45,7 +43,9 @@ 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 * (ofVec3f)pos;



			// ofMatrix multiplication works in reverse

			pos = (ofVec3f)pos * *transformMatrix;



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

		}

src/ofApp.cpp

+21 −16
Original line number Diff line number Diff line
@@ -5,6 +5,21 @@ 
#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();





//--------------------------------------------------------------

void ofApp::initNuitrack() {
@@ -98,14 +113,9 @@ void ofApp::setupTransformGui() { 
void ofApp::reloadTransformMatrix() {

	guitransform->loadFromFile("transformation.xml");



	deviceToWorldTransform = transformation.get().getInverse();



	auto d = deviceToWorldTransform.getPtr();

	ofLog(OF_LOG_NOTICE) << "Loaded new transform matrix (device to world):"

		<< "\n" << d[0] << " " << d[1] << " " << d[2] << " " << d[3]

		<< "\n" << d[4] << " " << d[5] << " " << d[6] << " " << d[7]

		<< "\n" << d[8] << " " << d[9] << " " << d[10] << " " << d[11]

		<< "\n" << d[12] << " " << d[13] << " " << d[14] << " " << d[15];

	// ofMatrices multiplication works in reverse

	worldToDeviceTransform = nuitrackViewportToRealSenseViewportTransform * transformation.get();

	deviceToWorldTransform = nuitrackViewportToRealSenseViewportTransform * transformation.get();

}



void ofApp::setup(){
@@ -234,15 +244,14 @@ void ofApp::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(transformation.get().getInverse());

	if (bPreviewPointCloud) {

		ofPushMatrix();

		ofMultMatrix(worldToDeviceTransform);

		pointCloudManager.drawPointCloud();

	}

		ofPopMatrix();

	}



	// TODO: draw base

	//ofFill();
@@ -251,18 +260,14 @@ void ofApp::drawPreview() { 
	//sphere_Y.draw();

	//sphere_Z.draw();

	

	ofPushMatrix();



    ofSetColor(255, 255, 0);

    skeletonFinder.drawSensorBox();



    ofNoFill();

	glLineWidth(5);

    ofSetColor(255, 100, 255);

	skeletonFinder.drawSkeletons();

    

	glDisable(GL_DEPTH_TEST);

	ofPopMatrix();  

}



//--------------------------------------------------------------

src/ofApp.h

+4 −3
Original line number Diff line number Diff line
@@ -127,6 +127,7 @@ public: 
    ofEasyCam previewCam;

            

    ofMatrix4x4 deviceToWorldTransform;

    ofMatrix4x4 worldToDeviceTransform;

    

    bool bShowSkeletonData = true;
@@ -138,9 +139,7 @@ public: 
    ofxGui gui;

    

	ofxGuiPanel *guitransform;

 

	ofParameterGroup transformationGuiGroup;



    ofParameter<ofMatrix4x4> transformation;

    

    //////////
@@ -152,5 +151,7 @@ public: 


    void createHelp();



private:

    const static ofMatrix4x4 nuitrackViewportToRealSenseViewportTransform;

    static ofMatrix4x4 makeNuitrackToRealSenseTransform();

};