kinect update

3_PointCloud/KinectTutorial3.vcxproj

@@ -93,6 +93,7 @@
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="glut.h" />
+    <ClInclude Include="SteamVrTrackingServer.h" />
     <ClInclude Include="main.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />

3_PointCloud/KinectTutorial3.vcxproj.filters

@@ -32,5 +32,8 @@
     <ClInclude Include="main.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="SteamVrTrackingServer.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file

3_PointCloud/SteamVrTrackingServer.cpp

-#include <chrono>
-#include <csignal>
-#include <iostream>
-#include <thread>
-#include <sstream>
-#include <openvr.h>
+#include "SteamVrTrackingServer.h"
+
+
 
 
 struct Config
 {
-	std::string hostname = "127.0.0.1";
-	int port = 5672;
-	std::string exchange = "irpose";
 	bool debug = false;
-	int messages_per_second = 60;
 } config_;
 
 
-bool running_ = true;
-
 vr::IVRSystem *vr_system_ = nullptr;
 vr::TrackedDevicePose_t tracked_device_poses_[vr::k_unMaxTrackedDeviceCount];
 
-void signalHandler(int signum)
-{
-	running_ = false;
-}
 
-bool init()
+bool initVR()
 {
 	
 	// Loading the SteamVR Runtime
@@ -57,7 +44,7 @@ bool init()
 	return true;
 }
 
-void shutdown()
+void shutdownVR()
 {
 	if (vr_system_)
 	{
@@ -134,162 +121,107 @@ void printMatrix(const vr::HmdMatrix34_t &htm)
 	}
 }
 
-int main2(int argc, char *argv[])
-{
-	if (!init())
-	{
-		shutdown();
-		return 1;
-	}
-
-	if (false)
+void printVRDevices() {
+	std::cout << "List of available VR devices:" << std::endl;
+	for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
 	{
-		std::cout << "List of available VR devices:" << std::endl;
-		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+		vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
+		if (device_class == vr::TrackedDeviceClass_Invalid)
 		{
-			vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
-			if (device_class == vr::TrackedDeviceClass_Invalid)
-			{
-				continue;
-			}
-
-			std::string type;
-			switch (device_class)
-			{
-			case vr::TrackedDeviceClass_Controller:        type = "Controller"; break;
-			case vr::TrackedDeviceClass_HMD:               type = "HMD"; break;
-			case vr::TrackedDeviceClass_Invalid:           type = "Invalid"; break;
-			case vr::TrackedDeviceClass_GenericTracker:    type = "GenericTracker"; break;
-			case vr::TrackedDeviceClass_TrackingReference: type = "TrackingReference"; break;
-			default:                                       type = "Unknown"; break;
-			}
-
-			const uint32_t size = 1024;
-			char serial[size];
-			vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
-
-			std::cout << "\tDevice " << nDevice << ":" << std::endl;
-			std::cout << "\t\tType: " << type << std::endl;
-			std::cout << "\t\tSerial: " << serial << std::endl;
+			continue;
 		}
 
-		shutdown();
-		return 0;
-	}
+		std::string type;
+		switch (device_class)
+		{
+		case vr::TrackedDeviceClass_Controller:        type = "Controller"; break;
+		case vr::TrackedDeviceClass_HMD:               type = "HMD"; break;
+		case vr::TrackedDeviceClass_Invalid:           type = "Invalid"; break;
+		case vr::TrackedDeviceClass_GenericTracker:    type = "GenericTracker"; break;
+		case vr::TrackedDeviceClass_TrackingReference: type = "TrackingReference"; break;
+		default:                                       type = "Unknown"; break;
+		}
 
-	signal(SIGINT, signalHandler);
-	std::cout << "Tracking started. Cancel with Ctrl-C" << std::endl;
+		const uint32_t size = 1024;
+		char serial[size];
+		vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
 
-	std::chrono::duration<double, std::milli> cycle_time;
-	if (config_.messages_per_second > 0)
-	{
-		cycle_time = std::chrono::duration<double, std::milli>(1000.0 / config_.messages_per_second);
+		std::cout << "\tDevice " << nDevice << ":" << std::endl;
+		std::cout << "\t\tType: " << type << std::endl;
+		std::cout << "\t\tSerial: " << serial << std::endl;
 	}
+}
 
-	while (running_)
+int main2(int argc, char *argv[])
+{
+	if (!initVR())
 	{
-		auto start = std::chrono::steady_clock::now();
+		shutdownVR();
+		return 1;
+	}
+	printVRDevices();
 
-		handleInput();
+	handleInput();
 
-		// Get device poses
-		vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
+	// Get device poses
+	vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
 
-		// TODO: remove
-		vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose();
-		std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl;
-		printMatrix(raw_to_standing_htm);
+	// TODO: remove
+	vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose();
+	std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl;
+	printMatrix(raw_to_standing_htm);
 
-		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+	for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+	{
+		if (!tracked_device_poses_[nDevice].bPoseIsValid)
 		{
-			if (!tracked_device_poses_[nDevice].bPoseIsValid)
-			{
-				continue;
-			}
-
-			vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
-
-			if (device_class == vr::TrackedDeviceClass_Invalid)
-			{
-				continue;
-			}
+			continue;
+		}
 
-			vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking;
+		vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
 
-			std::string type;
-			std::string model_type;
-			switch (device_class)
-			{
-			case vr::TrackedDeviceClass_Controller:
-				type = "Controller";
-				model_type = "vive_controller";
-				break;
-			case vr::TrackedDeviceClass_HMD:
-				type = "HMD";
-				model_type = "vive_hmd";
-				break;
-			case vr::TrackedDeviceClass_Invalid:
-				type = "Invalid";
-				break;
-			case vr::TrackedDeviceClass_GenericTracker:
-				type = "GenericTracker";
-				break;
-			case vr::TrackedDeviceClass_TrackingReference:
-				type = "TrackingReference";
-				model_type = "vive_base_station";
-				break;
-			default:
-				type = "Unknown";
-				break;
-			}
+		if (device_class == vr::TrackedDeviceClass_Invalid)
+		{
+			continue;
+		}
 
-			if (config_.debug)
-			{
-				std::cout << "Pose of device " << nDevice << " (" << type << ")" << std::endl;
-				printMatrix(htm);
-			}
+		vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking;
 
-			const uint32_t size = 1024;
-			char serial[size];
-			vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
-
-			auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-
-			std::ostringstream message;
-			message << "%YAML:1.0\n";
-			message << "object_type: \"" << model_type << "\"\n";
-			message << "serial: \"" << serial << "\"\n";
-			message << "timestamp: \"" << std::to_string(timestamp) << "\"\n";
-			message << "pose: !!opencv-matrix\n   rows: 4\n   cols: 4\n   dt: f\n   data: [ ";
-			message << htm.m[0][0] << ", ";
-			message << htm.m[0][1] << ", ";
-			message << htm.m[0][2] << ", ";
-			message << htm.m[0][3] << ", ";
-			message << htm.m[1][0] << ", ";
-			message << htm.m[1][1] << ", ";
-			message << htm.m[1][2] << ", ";
-			message << htm.m[1][3] << ", ";
-			message << htm.m[2][0] << ", ";
-			message << htm.m[2][1] << ", ";
-			message << htm.m[2][2] << ", ";
-			message << htm.m[2][3] << ", ";
-			message << "0.0, 0.0, 0.0, 1.0 ]\n";
-			message << "source_type: \"steam_vr\"\n";
-			message << "source_serial: \"steam_vr_1\"\n";
-
-			if (config_.debug)
-			{
-				std::cout << message.str() << std::endl;
-			}
+		std::string type;
+		std::string model_type;
+		switch (device_class)
+		{
+		case vr::TrackedDeviceClass_Controller:
+			type = "Controller";
+			model_type = "vive_controller";
+			break;
+		case vr::TrackedDeviceClass_HMD:
+			type = "HMD";
+			model_type = "vive_hmd";
+			break;
+		case vr::TrackedDeviceClass_Invalid:
+			type = "Invalid";
+			break;
+		case vr::TrackedDeviceClass_GenericTracker:
+			type = "GenericTracker";
+			break;
+		case vr::TrackedDeviceClass_TrackingReference:
+			type = "TrackingReference";
+			model_type = "vive_base_station";
+			break;
+		default:
+			type = "Unknown";
+			break;
 		}
 
-		if (config_.messages_per_second > 0)
+		if (config_.debug)
 		{
-			std::this_thread::sleep_until(start + cycle_time);
+			std::cout << "Pose of device " << nDevice << " (" << type << ")" << std::endl;
+			printMatrix(htm);
 		}
 	}
 
-	shutdown();
+	shutdownVR();
 
 	return 0;
 }

3_PointCloud/SteamVrTrackingServer.h

+#pragma once
+
+#include <iostream>
+#include <sstream>
+
+#include <openvr.h>
+
+
+bool initVR();
+void shutdownVR();
\ No newline at end of file

3_PointCloud/main.cpp

 #include "main.h"
 #include "glut.h"
+#include "SteamVRTrackingServer.h"
 
 #include <cmath>
 #include <cstdio>
@@ -92,13 +93,6 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) {
 
 	// Get data from frame
 	colorframe->CopyConvertedFrameDataToArray(colorwidth*colorheight*4, rgbimage, ColorImageFormat_Rgba);
-	
-	if (writFrameToFile) {
-		points.clear();
-		points.reserve(width*height * 3);
-		colors.clear();
-		colors.reserve(width*height * 3);
-	}
 
 	// Write color array for vertices
 	float* fdest = (float*)frameColors;
@@ -132,13 +126,19 @@ void getRgbData(IMultiSourceFrame* frame, bool writFrameToFile) {
 	if (colorframe) colorframe->Release();
 }
 
-void getKinectData() {
+void getKinectData(bool writeFrameInVector) {
 	IMultiSourceFrame* frame = NULL;
 	if (SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
-		getDepthData(frame);
-		getRgbData(frame, writeFile);
-		writeFile = false;
 
+		if (writeFrameInVector) {
+			points.clear();
+			points.reserve(width*height * 3);
+			colors.clear();
+			colors.reserve(width*height * 3);
+		}
+
+		getDepthData(frame);
+		getRgbData(frame, writeFrameInVector);
 
 		glBindBuffer(GL_ARRAY_BUFFER, vboId);
 		glBufferData(GL_ARRAY_BUFFER, width*height * 3 * sizeof(GLfloat), framePositions, GL_DYNAMIC_DRAW);
@@ -178,8 +178,6 @@ void writePointCloud() {
 
 
 	myfile.close();
-
-
 }
 
 void rotateCamera() {
@@ -190,11 +188,17 @@ void rotateCamera() {
 	glMatrixMode(GL_MODELVIEW);
     glLoadIdentity();
 	gluLookAt(x,0,z,0,0,radius/2,0,1,0);
-	angle += 0.002;
+	//angle += 0.002;
 }
 
 void drawKinectData() {
-	getKinectData();
+
+	getKinectData(writeFile);
+	if (writeFile) {
+		writePointCloud();
+		writeFile = false;
+	}
+
 	rotateCamera();
 
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
@@ -212,7 +216,7 @@ void drawKinectData() {
 
 	glDisableClientState(GL_VERTEX_ARRAY);
 	glDisableClientState(GL_COLOR_ARRAY);
-	writePointCloud();
+
 }
 
 void keyPressed(unsigned char key, int x, int y)
@@ -232,6 +236,12 @@ void keyPressed(unsigned char key, int x, int y)
 int main(int argc, char* argv[]) {
     if (!init(argc, argv)) return 1;
     if (!initKinect()) return 1;
+
+	if (!initVR())
+	{
+		shutdownVR();
+	}
+
     // OpenGL setup
     glClearColor(0,0,0,0);
     glClearDepth(1.0f);

3_PointCloud/main.h

@@ -6,6 +6,7 @@ const int colorheight = 1080;
 
 void drawKinectData();
 void keyPressed(unsigned char key, int x, int y);
+void writePointCloud();
 
 
 struct pntsHeader {