Update

3_PointCloud/main.cpp

@@ -45,9 +45,9 @@ IMultiSourceFrameReader* reader;   // Kinect data source
 ICoordinateMapper* mapper;         // Converts between depth, color, and 3d coordinates
 TIMESPAN colorFrameTime = -1;
 
-float KinectSizeX = 0.249;
-float KinectSizeY = 0.042; // 42 mm, 66mm ist die gesammthhe
-float KinectSizeZ = 0.067;
+float KinectSizeX = 0.249f;
+float KinectSizeY = 0.042f; // 42 mm, 66mm ist die gesammthhe
+float KinectSizeZ = 0.067f;
 
 float KinectOriginOffsetX = ((92 + 73) / 2.0f) / 1000.0f;
 float KinectOriginOffsetY = KinectSizeY/2.0f;
@@ -60,7 +60,11 @@ glm::vec3 FrameTranslation(0.0,0.0,0.0);
 SteamTracking* steamTracking = nullptr;
 char* controllerSerial = "LHR-FFFDBD41";
 int controllerID = 1;
+char* trackerSerial = "LHR-18DC0D92";
+int trackerID = 5;
 glm::mat4x4* currentControlerPos = nullptr;
+glm::mat4x4* currentTrackerPos = nullptr;
+bool useTrackerTransform = true;
 glm::vec3 lastClicketControlelrPos;
 glm::mat4x4 controlerToKinect;
 std::vector<glm::vec3> controllerPositions;
@@ -80,10 +84,10 @@ int lastX, lastY;
 
 //Filter Settings
 float minTrackingKinect = 0.2f;//0.8f;
-float maxTrackingKinect = 1.8f;// 2.5f;
+float maxTrackingKinect = 1.0f;// 2.5f;
 	//Custom
 //float angleDegrees = 20.0f;
-float maxNormalAngle = degreeToRadioan(25.0f);
+float maxNormalAngle = degreeToRadian(25.0f);
 bool enableDepthFilter = false;
 bool enableNormalFilter = false;//true;
 	//Bilateral / FastDepthNoise
@@ -94,8 +98,8 @@ float repairThreshold = 100.0f;
 
 
 
-float degreeToRadioan(float degree) {
-	return degree*M_PI / 180.0;
+float degreeToRadian(float degree) {
+	return (float) (degree*M_PI / 180.0f);
 }
 
 
@@ -231,8 +235,8 @@ void getDepthData(IMultiSourceFrame* frame) {
 			if ((enableNormalFilter || enableDepthFilter) && x > 0 && y > 0 && x < width - 1 && y < height - 1) {
 				if (enableNormalFilter) {
 					//normalen
-					float dzdx = (curr[x + 1 + width * y] - curr[x - 1 + width * y]) / 2.0;
-					float dzdy = (curr[x + width * (y + 1)] - curr[x + width * (y - 1)]) / 2.0;
+					float dzdx = (curr[x + 1 + width * y] - curr[x - 1 + width * y]) / 2.0f;
+					float dzdy = (curr[x + width * (y + 1)] - curr[x + width * (y - 1)]) / 2.0f;
 
 					glm::vec3 d(-dzdx, -dzdy, 1.0f);
 					frameNormals[i] = normalize(d);
@@ -385,10 +389,13 @@ void processCurrentFrameForExport() {
 	float maxx, maxy, maxz;
 	maxx = maxy = maxz = -std::numeric_limits<float>::max();
 
+	bool captureFramesTXT = false;
 	std::ofstream myfile;
+	if (captureFramesTXT) {
 		char buffer[64]; // make sure it's big enough
 		snprintf(buffer, sizeof(buffer), "frames/pnts_%d.txt", counter++);
 		myfile.open(buffer, std::ios::out | std::ios::binary);
+	}	
 
 	for (unsigned int i = 0; i < width*height; i++) {
 		if (frameValidPoints[i]) {
@@ -411,17 +418,17 @@ void processCurrentFrameForExport() {
 			colors.push_back(frameColors[i * 3]);
 			colors.push_back(frameColors[i * 3 + 1]);
 			colors.push_back(frameColors[i * 3 + 2]);
-
-			myfile << positionGlobal.x << " "<<positionGlobal.y << " " << positionGlobal.z << " "
+			if (captureFramesTXT) {
+				myfile << positionGlobal.x << " " << positionGlobal.y << " " << positionGlobal.z << " "
 					<< (int)frameColors[i * 3] << " " << (int)frameColors[i * 3 + 1] << " " << (int)frameColors[i * 3 + 2] << std::endl;
-
+			}
 
 		}
 	}
 
-
+	if (captureFramesTXT) {
 		myfile.close();
-
+	}
 	std::cout << "Frame Local Boundingbox Min: [" << minx << "|" << miny << "|" << minz << "] Max: [" << maxx << "|" << maxy << "|" << maxz << "]"<< std::endl;
 	//update Buffers
 	if (points.size() == 0) return;
@@ -444,25 +451,47 @@ void writePointCloud() {
 		return;
 	}
 
+	if (!currentTrackerPos)
+		currentTrackerPos = new glm::mat4x4(glm::rotate(*steamTracking->getTransformationForDevice(trackerID, 0.0f), degreeToRadian(-00), glm::vec3(1.0f, 0.0f, 0.0f)));
+
+
+	std::vector<glm::vec3> exportpoints;
+	exportpoints.reserve(points.size());
+	for (size_t i = 0; i < points.size(); i++) {
+		
+
+		exportpoints.push_back(glm::vec3(glm::rotate(glm::inverse(*currentTrackerPos), degreeToRadian(00), glm::vec3(1.0f, 0.0f, 0.0f)) * glm::vec4(points[i], 1.0f)));
+	}
+
+	//exportpoints.clear();
+
+	for (int i = 0; i < 1000; i++) {
+		exportpoints.push_back(glm::vec3(i/1000.0f, 0.0f, 0.0f));
+		exportpoints.push_back(glm::vec3(0.0f, i / 1000.0f, 0.0f));
+		exportpoints.push_back(glm::vec3(0.0f, i / 2000.0f, 1.0f));
+		exportpoints.push_back(glm::vec3(0.0f, 0.0f, i / 1000.0f));
+	}
+
+
 	pntsHeader header;
-	size_t pointArrayByteSize = points.size() * sizeof(glm::vec3);
-	std::cout << points.size() << " " << sizeof(glm::vec3);
+	size_t pointArrayByteSize = exportpoints.size() * sizeof(glm::vec3);
+	std::cout<< "writing File: Number of Points:"  << exportpoints.size() ;
 	size_t coloArrayByteSize = colors.size() * sizeof(unsigned char);
 	header.featureTableByteLenght = pointArrayByteSize + coloArrayByteSize;
 
 	std::ostringstream os;
-	os << "{\"POINTS_LENGTH\":" << (points.size() ) << ", \"POSITION\" : {\"byteOffset\":0}, \"RGB\" : {\"byteOffset\":" << pointArrayByteSize << "}}";
+	os << "{\"POINTS_LENGTH\":" << (exportpoints.size() ) << ", \"POSITION\" : {\"byteOffset\":0}, \"RGB\" : {\"byteOffset\":" << pointArrayByteSize << "}}";
 	std::string json = os.str();
 
 	header.featureTableJSONByteLenght = json.size();
 	header.byteLenght = 28 /*header*/ + header.featureTableJSONByteLenght + header.featureTableByteLenght;
-	std::cout << "File Size " << header.byteLenght;
+	std::cout << " File Size in byte" << header.byteLenght <<std::endl;
 
 	std::ofstream myfile;
 	myfile.open("example.pnts", std::ios::out | std::ios::binary);
 	myfile.write((char*)&header, 28);
 	myfile << json;
-	myfile.write(reinterpret_cast<const char*>(&points[0]), pointArrayByteSize);
+	myfile.write(reinterpret_cast<const char*>(&exportpoints[0]), pointArrayByteSize);
 	myfile.write(reinterpret_cast<const char*>(&colors[0]), coloArrayByteSize);
 	//myfile << 
 
@@ -507,6 +536,11 @@ void mainRenderLoop() {
 	//get controler pos
 	delete currentControlerPos;
 	currentControlerPos = steamTracking->getTransformationForDevice(controllerID, 0.0f);
+	delete currentTrackerPos;
+	currentTrackerPos = steamTracking->getTransformationForDevice(trackerID, 0.0f);
+	currentTrackerPos = new glm::mat4x4(glm::rotate(*currentTrackerPos, degreeToRadian(-90), glm::vec3(1.0f, 0.0f, 0.0f)));
+
+	
 
 	if (trackControllerPos) {
 		glm::vec3 tempPos = glm::vec3(*currentControlerPos*glm::vec4(0, 0, 0, 1));
@@ -528,8 +562,11 @@ void mainRenderLoop() {
 	//add points to point cloud
 	if (captureFrame) {
 		processCurrentFrameForExport();
+		
+		glm::vec3 TrackerPos = glm::vec3(*steamTracking->getTransformationForDevice(trackerID, 0.0f)*glm::vec4(0, 0, 0, 1));
+
 		glm::vec3 current = controllerPositions.back();
-		std::cout << "Current Position [" << current.x << "/" << current.y << "/" << current.z << "] distance " << distance(lastClicketControlelrPos, current) << std::endl ;
+		std::cout << "Current Position [" << current.x << "/" << current.y << "/" << current.z << "] TrackerPos: [" << TrackerPos.x << "/" << TrackerPos.y << "/" << TrackerPos.z << "] distance " << distance(lastClicketControlelrPos, current) << std::endl ;
 		lastClicketControlelrPos = controllerPositions.back();
 
 		FrameTranslation = glm::vec3(0.0, 0.0, 0.0);
@@ -546,7 +583,7 @@ void mainRenderLoop() {
 	//rotateCamera(); //todo
 	if (toggleCam && trackControllerPos) {
 
-		glTranslatef(0.0, -0.3, -1.0);
+		glTranslatef(0.0f, -0.3f, -1.0f);
 		glRotatef(10.0, 1.0, 0.0, 0.0);
 		glRotatef(180.0, 0.0, 1.0, 0.0);
 		glMultMatrixf(glm::value_ptr(glm::inverse(controlerToKinect)));
@@ -582,10 +619,15 @@ void mainRenderLoop() {
 		glVertexPointer(3, GL_FLOAT, 0, NULL);
 		glDrawArrays(GL_LINE_STRIP, 0, controllerPosData.size);
 	}
-
+	glPushMatrix();
+	glMultMatrixf(glm::value_ptr(*currentTrackerPos));
+	DrawCoordinateSystem();
+	glPopMatrix();
 
 	{ // Controller Space
 		glPushMatrix(); 
+
+
 		if (currentControlerPos) { //applay controler Transformation
 			glMultMatrixf(glm::value_ptr(*currentControlerPos));
 		}
@@ -629,7 +671,7 @@ void mainRenderLoop() {
 }
 
 void moveCurrentFrame(glm::vec3 dir) {
-	float factor = 0.002;
+	float factor = 0.002f;
 	FrameTranslation += factor*dir;
 	std::cout << "Frame Transformation ";
 	printVector(FrameTranslation);
@@ -646,7 +688,6 @@ void keyPressed(unsigned char key, int x, int y)
 		captureFrame = true;
 		break;
 	case 'x':
-		std::cout << "writing File" << std::endl;
 		writeFile = true;
 		break;
 	case 'c':
@@ -727,6 +768,7 @@ int main(int argc, char* argv[]) {
 	steamTracking->printVRDevices();
 	steamTracking->onButtonPress = captureNextFrame;
 	controllerID = steamTracking->getDeviceIDForSerial(controllerSerial);
+	trackerID = steamTracking->getDeviceIDForSerial(trackerSerial);
 
 
     // OpenGL setup
@@ -771,13 +813,13 @@ int main(int argc, char* argv[]) {
 	
 	//controlerToKinect = glm::translate(glm::mat4x4(), (glm::vec3(0.0f, 0.0f,0.173 /*controller*/+0.03 /*Halterung*/ )));
 	controlerToKinect = glm::mat4x4();
-	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(-58+180 /*63,9...*/), glm::vec3(1.0f, 0.0f, 0.0f));
-	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 1.0f, 0.0f));
+	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadian(-58+180 /*63,9...*/), glm::vec3(1.0f, 0.0f, 0.0f));
+	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadian(180.0f), glm::vec3(0.0f, 1.0f, 0.0f));
 	//controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 0.0f, 1.0f));
 
-	float dx = 0.010;// 0.00;
-	float dy = KinectSizeY / 2.0f + 0.004 /*HAlterung Platte*/ + 0.011 /* Conroller*/;
-	float dz = -0.01; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
+	float dx = 0.000f;// 0.01;
+	float dy = KinectSizeY / 2.0f + 0.004f /*HAlterung Platte*/ + 0.011f /* Conroller*/;
+	float dz = -0.01f; //-0.023 + 0.02; // 0.045f /*= versatz auf der Halterung; Distanz zur Halterung:*/ + (KinectSizeZ - (0.066 /*halterung z*/ - 0.012 /*halterung berstand*/));
 
 	controlerToKinect = glm::translate(controlerToKinect, (glm::vec3(dx, -dy, dz)));
 
@@ -792,17 +834,17 @@ int main(int argc, char* argv[]) {
 	glm::mat4 kinectToController = glm::inverse(controlerToKinect);
 
 	float angleInDegrees = 30;
-	glm::vec4 test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(1.0f, 0.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
+	glm::vec4 test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(1.0f, 0.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
 	std::cout << "Angle Error x ";
 	printVector(test);
 	std::cout << std::endl;
 
-	test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(0.0f, 1.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
+	test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(0.0f, 1.0f, 0.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
 	std::cout << "Angle Error y ";
 	printVector(test);
 	std::cout << std::endl;
 
-	test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadioan(angleInDegrees), glm::vec3(0.0f, 0.0f, 1.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
+	test = controlerToKinect * glm::rotate(glm::mat4(), degreeToRadian(angleInDegrees), glm::vec3(0.0f, 0.0f, 1.0f)) * kinectToController * glm::vec4(0.0, 0.0, 1.0, 1.0);
 	std::cout << "Angle Error z ";
 	printVector(test);
 	std::cout << std::endl;

3_PointCloud/main.h

@@ -45,7 +45,7 @@ void setupFrameBuffer();
 void reshapeFunction(int width, int height);
 
 void printVector(glm::vec3 vector);
-float degreeToRadioan(float degree);
+float degreeToRadian(float degree);
 
 
 struct pntsHeader {

3_PointCloud/tileset.json → 3_PointCloud/tileset1.json

@@ -18,7 +18,7 @@
       {
 	    "transform": [
 			1.0, 0.0, 0.0, 0.0,
-			0.0, 1.0, 0.0, 0.0,
+			0.0, 1.0, 0.0, 50.0,
 			0.0, 0.0, 1.0, 0.0,
 			0.0, 0.0, 0.0, 1.0
 		  ],