halterung steam trakcing fixed to conroller

3_PointCloud/SteamTracking.cpp

@@ -99,6 +99,31 @@ void SteamTracking::processEvents()
 		}
 }
 
+int SteamTracking::getDeviceIDForSerial(char* Serial)
+{
+	if (!vr_system_) {
+		return 1;
+	}
+	for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+	{
+		vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
+		if (device_class == vr::TrackedDeviceClass_Invalid)
+		{
+			continue;
+		}
+
+		const uint32_t size = 1024;
+		char serial[size];
+		vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
+
+		if (strcmp(Serial, serial) == 0) {
+			std::cout << "Device " << Serial <<" has ID " << nDevice << std::endl;
+			return nDevice;
+		}
+	}
+	return 1;
+}
+
 
 void SteamTracking::printMatrix(const vr::HmdMatrix34_t &htm)
 {
@@ -148,14 +173,14 @@ void SteamTracking::processEvent(vr::VREvent_t event)
 
 }
 
-glm::mat4x4* SteamTracking::getTransformationForDevice(int nDevice)
+glm::mat4x4* SteamTracking::getTransformationForDevice(int nDevice, float timeoffset)
 {
 	if (!vr_system_) {
 		if (debug) std::cout << " NO VR System ";
 		return new glm::mat4x4();
 	}
 	//TrackingUniverseStanding TrackingUniverseRawAndUncalibrated
-	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding , 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
+	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding , timeoffset, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
 
 	if (!tracked_device_poses_[nDevice].bPoseIsValid)
 	{

3_PointCloud/SteamTracking.h

@@ -20,7 +20,7 @@ private:
 	void printMatrix(const vr::HmdMatrix34_t &htm);
 
 	void processEvent(vr::VREvent_t evnt);
-
+	
 
 
 
@@ -34,7 +34,9 @@ public:
 	void printVRDevices();
 	void processEvents();
 
-	glm::mat4x4* getTransformationForDevice(int nDevice);
+	int getDeviceIDForSerial(char* Serial);
+
+	glm::mat4x4* getTransformationForDevice(int nDevice, float timeoffset);
 
 	void (*onButtonPress)(vr::VREvent_t) = nullptr;
 

3_PointCloud/main.cpp

 #include "main.h"
 #include "glut.h"
 #include "SteamTracking.h"
+#include <ctime>
 
 #include <cmath>
 #include <cstdio>
@@ -59,6 +60,7 @@ std::vector<unsigned char> colors;
 IKinectSensor* sensor;             // Kinect sensor
 IMultiSourceFrameReader* reader;   // Kinect data source
 ICoordinateMapper* mapper;         // Converts between depth, color, and 3d coordinates
+TIMESPAN colorFrameTime = -1;
 float KinectOriginOffsetX = ((92 + 73) / 2.0f) / 1000.0f;
 float KinectOriginOffsetY = 21 / 1000.0f;
 float KinectSizeX = 0.249;
@@ -66,16 +68,20 @@ float KinectSizeY = 0.042; // 42 mm, 66mm ist die gesammthhe
 float KinectSizeZ = 0.067;
 
 float minTrackingKinect = 0.8f;
-float maxTrackingKinect = 3.5f;
+float maxTrackingKinect = 2.5f;
 
 //Vive Tracking
 SteamTracking* steamTracking = nullptr;
+char* controllerSerial = "LHR-FFFDBD41";
 int controllerID = 1;
 glm::mat4x4* currentControlerPos = nullptr;
+glm::vec3 lastClicketControlelrPos;
 glm::mat4x4 controlerToKinect;
 std::vector<glm::vec3> controllerPositions;
 
 
+
+
 Camera mainCam;
 
 static bool trackControllerPos = true;
@@ -195,6 +201,15 @@ void getRgbData(IMultiSourceFrame* frame) {
 	IColorFrameReference* frameref = NULL;
 	frame->get_ColorFrameReference(&frameref);
 	frameref->AcquireFrame(&colorframe);
+
+	TIMESPAN time;
+	if (SUCCEEDED(frameref->get_RelativeTime(&time))) {
+		colorFrameTime = time;
+	}
+	else {
+		colorFrameTime = -1;
+	}
+	
 	if (frameref) frameref->Release();
 
 	if (!colorframe) return;
@@ -241,10 +256,12 @@ void getRgbData(IMultiSourceFrame* frame) {
 
 void getKinectData() {
 	IMultiSourceFrame* frame = NULL;
+
 	if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
 		getDepthData(frame);
 		getRgbData(frame);
 
+
 		glBindBuffer(GL_ARRAY_BUFFER, framePoints.vboID);
 		glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
 		glUnmapBuffer(GL_ARRAY_BUFFER);
@@ -255,6 +272,7 @@ void getKinectData() {
 		glUnmapBuffer(GL_ARRAY_BUFFER);
 	}
 	if (frame) frame->Release();
+
 }
 
 void processCurrentFrameForExport() {
@@ -275,7 +293,7 @@ void processCurrentFrameForExport() {
 	glBindBuffer(GL_ARRAY_BUFFER, exportData.vboID);
 	glBufferData(GL_ARRAY_BUFFER, points.size() * sizeof(glm::vec3), &points[0], GL_DYNAMIC_DRAW);
 	glUnmapBuffer(GL_ARRAY_BUFFER);
-
+	
 	glBindBuffer(GL_ARRAY_BUFFER, exportData.cboID);
 	glBufferData(GL_ARRAY_BUFFER, colors.size() * sizeof(unsigned char), &colors[0], GL_DYNAMIC_DRAW);
 	exportData.size = points.size();
@@ -343,14 +361,21 @@ void mainRenderLoop() {
 
 	mainCam.tick();
 
+	getKinectData();
+	 
+	time_t currentTime;
 
+	time(&currentTime);
+	
+	//std::cout << "Relative Time " << colorFrameTime << " current "<< currentTime << " dif: " << (currentTime - colorFrameTime/10) <<  std::endl;
 	//get controler pos
 	delete currentControlerPos;
-	currentControlerPos = steamTracking->getTransformationForDevice(controllerID);
+	currentControlerPos = steamTracking->getTransformationForDevice(controllerID, 0.0f);
 
 	if (trackControllerPos) {
-		controllerPositions.push_back(glm::vec3(*currentControlerPos*glm::vec4(0,0,0,1)));
-		
+		glm::vec3 tempPos = glm::vec3(*currentControlerPos*glm::vec4(0, 0, 0, 1));
+		controllerPositions.push_back(tempPos);
+
 		if (controllerPositions.size() > 2) {
 
 			glBindBuffer(GL_ARRAY_BUFFER, controllerPosData.vboID);
@@ -362,11 +387,14 @@ void mainRenderLoop() {
 
 
 
-	getKinectData();
+
 
 	//add points to point cloud
 	if (captureFrame) {
 		processCurrentFrameForExport();
+		glm::vec3 current = controllerPositions.back();
+		std::cout << "Current Position [" << current.x << "/" << current.y << "/" << current.z << "] distance " << distance(lastClicketControlelrPos, current) << std::endl ;
+		lastClicketControlelrPos = controllerPositions.back();
 		captureFrame = false;
 	}
 
@@ -452,7 +480,7 @@ void keyPressed(unsigned char key, int x, int y)
 	mainCam.keyPressed(key, x, y);
 	switch (key) {
 	case 'y':
-		std::cout << "Capturing Frame Keyboard"<< std::endl;
+		std::cout << "Capturing Frame Keyboard ";
 		captureFrame = true;
 		break;
 	case 'x':
@@ -489,8 +517,8 @@ void mouseMove(int x, int y) {
 
 void captureNextFrame(vr::VREvent_t event)
 {
-	if (event.trackedDeviceIndex == 1 && event.data.controller.button == 33) {
-		std::cout << "Capturing Frame Controler Button" << std::endl;
+	if (event.trackedDeviceIndex == controllerID && event.data.controller.button == 33) {
+		std::cout << "Capturing Frame Controler Button ";
 		captureFrame = true;
 	}
 }
@@ -507,7 +535,7 @@ int main(int argc, char* argv[]) {
 	steamTracking = new SteamTracking();
 	steamTracking->printVRDevices();
 	steamTracking->onButtonPress = captureNextFrame;
-
+	controllerID = steamTracking->getDeviceIDForSerial(controllerSerial);
 
 
     // OpenGL setup
@@ -558,6 +586,9 @@ int main(int argc, char* argv[]) {
 	controlerToKinect = glm::rotate(controlerToKinect, degreeToRadioan(180.0f), glm::vec3(0.0f, 0.0f, 1.0f));
 
 	controlerToKinect = glm::translate(controlerToKinect, (glm::vec3(dx, -dy, dz)));
+
+	glm::vec3 tempPos = glm::vec3(controlerToKinect*glm::vec4(0, 0, 0, 1));
+	std::cout << "Controller Kinect Transformation  [" << tempPos.x << "/" << tempPos.y << "/" << tempPos.z << "]" << std::endl;
 	
 
 	std::cout << "Kinect to center: " << dx <<" "<< dy << " " << dz<<std::endl;

Halterungen/vive 2.0.scad

@@ -7,13 +7,13 @@ module base(){
     }
 }
 
-module HohlZylinder(size=10, thickness = 5){
+module HohlZylinder(size=20, thickness = 5){
     
     difference() {
-        cylinder(30,size,size,false);
-        cylinder(30,size-thickness,size-thickness,false);
+        cylinder(30,d = size);
+        cylinder(30,d= size - 2*thickness);
     }
 }
 
 base();
-translate([35,4,33]) rotate([-90,0,0])  HohlZylinder(20,5);
+translate([35,4,33]) rotate([-90,0,0])  HohlZylinder(37,5);
\ No newline at end of file