asdf

3_PointCloud/SteamTracking.cpp

@@ -17,9 +17,9 @@ std::string SteamTracking::getDeviceTypeAsString(vr::ETrackedDeviceClass device_
 	return type;
 }
 
-glm::mat3x4 SteamTracking::convertMatrix(vr::HmdMatrix34_t in)
+glm::mat3x4* SteamTracking::convertMatrix(vr::HmdMatrix34_t in)
 {
-	return glm::mat3x4(in.m[0][0], in.m[0][1], in.m[0][2], in.m[0][3],
+	return new glm::mat3x4(in.m[0][0], in.m[0][1], in.m[0][2], in.m[0][3],
 		in.m[1][0], in.m[1][1], in.m[1][2], in.m[1][3],
 		in.m[2][0], in.m[2][1], in.m[2][2], in.m[2][3]	);
 }
@@ -93,10 +93,10 @@ void SteamTracking::printMatrix(const vr::HmdMatrix34_t &htm)
 	}
 }
 
-glm::mat3x4 SteamTracking::getTransformationForDevice(int nDevice)
+glm::mat3x4* SteamTracking::getTransformationForDevice(int nDevice)
 {
 	if (!vr_system_) {
-		return glm::mat3x4();
+		return new glm::mat3x4();
 	}
 
 	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
@@ -104,7 +104,7 @@ glm::mat3x4 SteamTracking::getTransformationForDevice(int nDevice)
 	if (!tracked_device_poses_[nDevice].bPoseIsValid)
 	{
 		if (debug) std::cout << " Device " << nDevice << "Pois is Invalid";
-		return glm::mat3x4();
+		return new glm::mat3x4();
 	}
 
 	vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
@@ -112,7 +112,7 @@ glm::mat3x4 SteamTracking::getTransformationForDevice(int nDevice)
 	if (device_class == vr::TrackedDeviceClass_Invalid)
 	{
 		if (debug) std::cout << " Device " << nDevice << "ist Invalid";
-		return glm::mat3x4();
+		return new glm::mat3x4();
 	}
 
 	vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking;

3_PointCloud/SteamTracking.h

@@ -15,7 +15,7 @@ private:
 
 	std::string getDeviceTypeAsString(vr::ETrackedDeviceClass device_class);
 
-	glm::mat3x4 convertMatrix(vr::HmdMatrix34_t in);
+	glm::mat3x4* convertMatrix(vr::HmdMatrix34_t in);
 
 	void printMatrix(const vr::HmdMatrix34_t &htm);
 
@@ -26,7 +26,7 @@ public:
 
 	void printVRDevices();
 
-	glm::mat3x4 getTransformationForDevice(int nDevice);
+	glm::mat3x4* getTransformationForDevice(int nDevice);
 
 };
 

3_PointCloud/main.cpp

@@ -22,6 +22,12 @@
 GLuint vboId;
 GLuint cboId;
 
+GLuint vboIdArrow;
+GLuint cboIdArrow;
+
+float arrowPoints[] = { 1, 0 , 0, 0, 1, 0, 0, 0, 1
+};
+
 // Intermediate Buffers
 unsigned char rgbimage[colorwidth*colorheight*4];    // Stores RGB color image
 ColorSpacePoint depth2rgb[width*height];             // Maps depth pixels to rgb pixels
@@ -41,8 +47,42 @@ IMultiSourceFrameReader* reader;   // Kinect data source
 ICoordinateMapper* mapper;         // Converts between depth, color, and 3d coordinates
 
 SteamTracking* steamTracking = nullptr;
+glm::mat3x4* currentControlerPos = nullptr;
 
 static bool writeFile = false;
+static bool captureFrame = false;
+
+
+void drawArrow() {
+	//TETAEDRON BY HAND
+	glBegin(GL_TRIANGLES);
+
+	//front triangle
+	glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
+	glVertex3f(0.0f, 1.0f, 0.0f);
+	glVertex3f(-1.0f, -1.0f, 0.0f);
+	glVertex3f(1.0f, -1.0f, 0.0f);
+
+	//right side triangle
+	glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
+	glVertex3f(1.0f, -1.0f, 0.0f);
+	glVertex3f(0.0f, 1.0f, 0.0f);
+	glVertex3f(0.0f, -1.0f, -1.0f);
+
+	//left side triangle
+	glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
+	glVertex3f(-1.0f, -1.0f, 0.0f);
+	glVertex3f(0.0f, 1.0f, 0.0f);
+	glVertex3f(0.0f, -1.0f, -1.0f);
+
+	//bottom triangle
+	glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
+	glVertex3f(-1.0f, -1.0f, 0.0f);
+	glVertex3f(1.0f, -1.0f, 0.0f);
+	glVertex3f(0.0f, -1.0f, -1.0f);
+
+	glEnd();
+}
 
 bool initKinect() {
     if (FAILED(GetDefaultKinectSensor(&sensor))) {
@@ -132,6 +172,8 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) {
 void getKinectData(bool writeFrameInVector) {
 	IMultiSourceFrame* frame = NULL;
 	if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
+		delete currentControlerPos;
+		currentControlerPos = steamTracking->getTransformationForDevice(0);
 
 		if (writeFrameInVector) {
 			snapshotPoints.clear();
@@ -151,10 +193,9 @@ void getKinectData(bool writeFrameInVector) {
 			colors.reserve(colors.capacity() + width*height);
 
 			for(int i = 0; i < width*height; i++) {
-				glm::mat3x4 transform = steamTracking->getTransformationForDevice(0);
 
-				points.push_back(transform * framePositions[i]);
-				colors.push_back(frameColors[i]);
+				points.push_back(*currentControlerPos * framePositions[i]);
+				colors.push_back(snapshotColors[i]);
 
 
 			}
@@ -203,23 +244,27 @@ void writePointCloud() {
 
 void rotateCamera() {
 	static double angle = 0.;
-	static double radius = 3.;
+	static double radius = 4.;
 	double x = radius*sin(angle);
 	double z = radius*(1-cos(angle)) - radius/2;
-	glMatrixMode(GL_MODELVIEW);
-    glLoadIdentity();
-	gluLookAt(x,0,z,0,0,radius/2,0,1,0);
-	//angle += 0.002;
+	
+	//gluLookAt(x,0,z,0,0,radius/2,0,1,0);
+	angle += 0.002;
 }
 
 void drawKinectData() {
 
-	getKinectData(writeFile);
+	getKinectData(captureFrame);
+	captureFrame = false;
 	if (writeFile) {
 		writePointCloud();
 		writeFile = false;
 	}
 
+	glMatrixMode(GL_MODELVIEW);
+	glLoadIdentity();
+
+
 	rotateCamera();
 
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
@@ -235,6 +280,15 @@ void drawKinectData() {
 	glPointSize(1.f);
 	glDrawArrays(GL_POINTS, 0, width*height);
 
+	glPushMatrix();
+	float scale = 1.0 / 10;
+	glScalef(scale, scale, scale);
+	
+	//drawArrow();
+	glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
+	glutSolidTeapot(1);
+	glPopMatrix();
+
 	glDisableClientState(GL_VERTEX_ARRAY);
 	glDisableClientState(GL_COLOR_ARRAY);
 
@@ -244,6 +298,9 @@ void keyPressed(unsigned char key, int x, int y)
 {
 	switch (key) {
 	case 'a':
+		captureFrame = true;
+		break;
+	case 's':
 		writeFile = true;
 		break;
 	default:
@@ -254,6 +311,7 @@ void keyPressed(unsigned char key, int x, int y)
 						 instantly */
 }
 
+
 int main(int argc, char* argv[]) {
 	std::cout << "Starting Kinect VR Point Cloud creator";
     if (!init(argc, argv)) return 1;
@@ -282,6 +340,7 @@ int main(int argc, char* argv[]) {
 	glBindBuffer(GL_ARRAY_BUFFER, cboId);
 	glBufferData(GL_ARRAY_BUFFER, dataSize, 0, GL_DYNAMIC_DRAW);
 
+
     // Camera setup
     glViewport(0, 0, width, height);
     glMatrixMode(GL_PROJECTION);