controller tracker

3_PointCloud/Camera.cpp

@@ -6,7 +6,7 @@
 
 
 
-Camera::Camera() : Camera(vec3(2.0f, 0.0f, 0.0f))
+Camera::Camera() : Camera(vec3(10.0f, 5.0f, 0.0f))
 {
 
 }

3_PointCloud/SteamTracking.cpp

@@ -82,6 +82,10 @@ void SteamTracking::printVRDevices() {
 		std::cout << "\t\tType: " << getDeviceTypeAsString(device_class) << std::endl;
 		std::cout << "\t\tSerial: " << serial << std::endl;
 	}
+
+	vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose();
+	std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl;
+	printMatrix(raw_to_standing_htm);
 }
 
 void SteamTracking::processEvents()
@@ -150,8 +154,8 @@ glm::mat4x4* SteamTracking::getTransformationForDevice(int nDevice)
 		if (debug) std::cout << " NO VR System ";
 		return new glm::mat4x4();
 	}
-
-	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
+	//TrackingUniverseStanding TrackingUniverseRawAndUncalibrated
+	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding , 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
 
 	if (!tracked_device_poses_[nDevice].bPoseIsValid)
 	{

3_PointCloud/main.cpp

@@ -39,6 +39,7 @@ drawData framePoints;
 drawData gridData;
 drawData exportData;
 drawData arrowData;
+drawData controllerPosData;
 
 // Intermediate Buffers
 unsigned char rgbimage[colorwidth*colorheight*4];    // Stores RGB color image
@@ -64,13 +65,17 @@ float KinectSizeX = 0.249;
 float KinectSizeY = 0.066;
 float KinectSizeZ = 0.067;
 
-
+//Vive Tracking
 SteamTracking* steamTracking = nullptr;
 int controllerID = 1;
 glm::mat4x4* currentControlerPos = nullptr;
 glm::mat4x4 controlerToKinect;
+std::vector<glm::vec3> controllerPositions;
+
+
 Camera mainCam;
 
+static bool trackControllerPos = false;
 
 static bool writeFile = false;
 static bool captureFrame = false;
@@ -208,12 +213,23 @@ void getRgbData(IMultiSourceFrame* frame) {
 			*fvalid++ = false;
 		}
 		else {
-			int idx = (int)p.X + colorwidth*(int)p.Y;
-			*fdest++ = rgbimage[4 * idx + 0];
-			*fdest++ = rgbimage[4 * idx + 1];
-			*fdest++ = rgbimage[4 * idx + 2];
-			*fvalid++ = true;
+			if (depth2xyz[i].Z > 1.7f) {
+				*fdest++ = 0;
+				*fdest++ = 255;
+				*fdest++ = 0;
+
+				*fvalid++ = false;
+			}
+			else {
+				int idx = (int)p.X + colorwidth*(int)p.Y;
+				*fdest++ = rgbimage[4 * idx + 0];
+				*fdest++ = rgbimage[4 * idx + 1];
+				*fdest++ = rgbimage[4 * idx + 2];
+				*fvalid++ = true;
+			}
 		}
+		
+
 		// Don't copy alpha channel
 	}
 
@@ -329,6 +345,20 @@ void mainRenderLoop() {
 	delete currentControlerPos;
 	currentControlerPos = steamTracking->getTransformationForDevice(controllerID);
 
+	if (trackControllerPos) {
+		controllerPositions.push_back(glm::vec3(*currentControlerPos*glm::vec4(0,0,0,1)));
+		
+		if (controllerPositions.size() > 2) {
+
+			glBindBuffer(GL_ARRAY_BUFFER, controllerPosData.vboID);
+			glBufferData(GL_ARRAY_BUFFER, controllerPositions.size() * sizeof(glm::vec3), &controllerPositions[0], GL_DYNAMIC_DRAW);
+			controllerPosData.size = controllerPositions.size();
+
+		}
+	}
+
+
+
 	getKinectData();
 
 	//add points to point cloud
@@ -357,15 +387,25 @@ void mainRenderLoop() {
 	glLineWidth(1.f);
 	drawElementArray(gridData, GL_LINES);
 
+	//draw Export
+	drawElementArray(exportData, GL_POINTS);
+
+	glDisableClientState(GL_COLOR_ARRAY);
+
+	//draw Controller Positions
+	
+	if (trackControllerPos && controllerPosData.size > 1){
+		glBindBuffer(GL_ARRAY_BUFFER, controllerPosData.vboID);
+		glVertexPointer(3, GL_FLOAT, 0, NULL);
+		glDrawArrays(GL_LINE_STRIP, 0, controllerPosData.size);
+	}
 
 
-	drawElementArray(exportData, GL_POINTS);
 	{ // Controller Space
 		glPushMatrix(); 
 		if (currentControlerPos) { //applay controler Transformation
 			glMultMatrixf(glm::value_ptr(*currentControlerPos));
 		}
-		glDisableClientState(GL_COLOR_ARRAY);
 		DrawCoordinateSystem();
 
 		{// draw controller
@@ -414,6 +454,11 @@ void keyPressed(unsigned char key, int x, int y)
 		std::cout << "writing File" << std::endl;
 		writeFile = true;
 		break;
+	case 'c':
+		points.clear();
+		colors.clear();
+		exportData.size = 0;
+		break;
 	case 27: // Escape key
 		exit(0);
 		break;
@@ -458,6 +503,8 @@ int main(int argc, char* argv[]) {
 	steamTracking->printVRDevices();
 	steamTracking->onButtonPress = captureNextFrame;
 
+
+
     // OpenGL setup
     glClearColor(0,0,0,0);
     glClearDepth(1.0f);
@@ -498,9 +545,9 @@ int main(int argc, char* argv[]) {
 
 	float dx = KinectOriginOffsetX - KinectSizeX/ 2;
 	float dy = KinectSizeY / 2 + 0.008; // dicke Halterung;
-	float dz = 0.04f; //TODO - offset nach hinten;
+	float dz = 0.037f +0.02; //TODO - offset nach hinten;
 	
-	controlerToKinect = glm::translate(glm::mat4x4(), (glm::vec3(0.0f, 0.0f,0.173 /*controller*/+0.03 ))); //TODO -controller gre + halterung
+	controlerToKinect = glm::translate(glm::mat4x4(), (glm::vec3(0.0f, 0.0f,0.173 /*controller*/+0.03 /*Halterung*/ )));
 
 	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180-64 /*63,9...*/), glm::vec3(1.0f, 0.0f, 0.0f));
 	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 0.0f, 1.0f));
@@ -541,10 +588,14 @@ int main(int argc, char* argv[]) {
 	glGenBuffers(1, &controllerData.index);
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, controllerData.index);
 	glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(unsigned short), &indices[0], GL_STATIC_DRAW);
-
 	controllerData.size = indices.size();
 
+	controllerPositions.clear();
+	glGenBuffers(1, &controllerPosData.vboID);
+	glBindBuffer(GL_ARRAY_BUFFER, controllerPosData.vboID);
 
+	glBufferData(GL_ARRAY_BUFFER, 0 * sizeof(glm::vec3), &out_vertices[0], GL_DYNAMIC_DRAW);
+	controllerPosData.size = 0;
 
     // Main loop
 	atexit(close);