multiple threads for smooth oculus performance :>

1 files changed, 162 insertions, 118 deletions

diff --git a/client2.cpp b/client2.cpp
index d72fe29..9411b72 100644
--- a/client2.cpp
+++ b/client2.cpp
@@ -10,6 +10,8 @@
 #include <string.h>
 #include <sys/un.h>
 #include <time.h>
+#include <pthread.h>
+#include <semaphore.h>
 
 #include <opencv2/opencv.hpp>
 #include "lib.h"
@@ -459,95 +461,33 @@ float deadzone(float val, float dead)
 }
 
 
-int main(int argc, const char** argv)
-{
-	bool no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
-	GLFWwindow* window = initOpenGL();
-
-
-	GLuint vaoCanvas, vaoEye, vaoWholescreenQuad;
-	glGenVertexArrays(1, &vaoCanvas);
-	glGenVertexArrays(1, &vaoEye);
-	glGenVertexArrays(1, &vaoWholescreenQuad);
-
-
-	GLuint vboCanvas, vboEye, vboWholescreenQuad;
-	glGenBuffers(1, &vboCanvas);
-	glGenBuffers(1, &vboEye);
-	glGenBuffers(1, &vboWholescreenQuad);
-
-	glBindBuffer(GL_ARRAY_BUFFER, vboCanvas);
-	glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnCanvasVertices), drawOnCanvasVertices, GL_DYNAMIC_DRAW);
-
-	glBindBuffer(GL_ARRAY_BUFFER, vboEye);
-	glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnEyeVertices), drawOnEyeVertices, GL_STATIC_DRAW);
-
-	glBindBuffer(GL_ARRAY_BUFFER, vboWholescreenQuad);
-	glBufferData(GL_ARRAY_BUFFER, sizeof(wholescreenVertices), wholescreenVertices, GL_STATIC_DRAW);
+pthread_mutex_t my_mutex;
+sem_t oculus_thread_go;
 
+ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180);
+ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180);
+float yaw_cam_global, pitch_cam_global, roll_cam_global;
+Mat frame_global;
 
+void* video_fetcher_thread(void* ptr)
+{
+	#define RINGBUF_SIZE 4
+	ModuloRingbuffer ringbuf_yaw_cam(RINGBUF_SIZE, -180, 180);
+	Ringbuffer ringbuf_pitch_cam(RINGBUF_SIZE);
+	ModuloRingbuffer ringbuf_roll_cam(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_roll_sensor(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_yaw_sensor(RINGBUF_SIZE, -180,180);
+	ModuloRingbuffer ringbuf_pitch_sensor(RINGBUF_SIZE, -180,180);
 
 
+	#define DELAY_SIZE 6
+	Ringbuffer delay_phi(DELAY_SIZE);   // should delay sensor data by ~0.2sec
+	Ringbuffer delay_psi(DELAY_SIZE);   // that's the amount the video lags behind
+	Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
 	
-	// compile shaders
-	GLuint drawOnOculusProgram = newOculusShaderProgram(vaoWholescreenQuad, vboWholescreenQuad);
-	GLint uniAberrR = glGetUniformLocation(drawOnOculusProgram, "aberr_r");
-	GLint uniAberrB = glGetUniformLocation(drawOnOculusProgram, "aberr_b");
-	
-	GLuint drawOnCanvasProgram = newCanvasShaderProgram(vaoCanvas, vboCanvas);
-	GLint uniCamYaw = glGetUniformLocation(drawOnCanvasProgram, "cam_yaw");
-	GLint uniCamPitch = glGetUniformLocation(drawOnCanvasProgram, "cam_pitch");
-	GLint uniCamRoll = glGetUniformLocation(drawOnCanvasProgram, "cam_roll");
+	int adjust_phi=10;
 	
-	GLuint drawFromCanvasProgram = newEyeShaderProgram(vaoEye, vboEye);
-	GLint uniEyeYaw = glGetUniformLocation(drawFromCanvasProgram, "eye_yaw");
-	GLint uniEyePitch = glGetUniformLocation(drawFromCanvasProgram, "eye_pitch");
-	GLint uniEyeRoll = glGetUniformLocation(drawFromCanvasProgram, "eye_roll");
-
-	// texture
-	GLuint texVideo;
-	glGenTextures(1, &texVideo);
-	glBindTexture(GL_TEXTURE_2D, texVideo);
-	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
-	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
-	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
-	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	
-
-
-// Framebuffer stuff
-	GLuint canvasFB, canvasTex;
-	genFramebuffer(canvasFB, canvasTex, CANVAS_WIDTH, CANVAS_HEIGHT, true);
-
-	GLuint eyeFB, eyeTex;
-	genFramebuffer(eyeFB, eyeTex, EYE_WIDTH, EYE_HEIGHT, false);
-
-
-
-
-
-
-
-	glEnable (GL_BLEND);
-	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-
-
-
-
-	ohmd_context* my_ohmd_context;
-	ohmd_device* my_ohmd_device;
-	if (no_oculus || init_ohmd(&my_ohmd_context, &my_ohmd_device))
-	{
-		printf("no oculus rift!\n");
-		my_ohmd_device=0;
-		my_ohmd_context=0;
-	}
-
-
-
-
-
 	DroneConnection drone(SOCKETPATH);
 	navdata_t navdata;
 
@@ -558,26 +498,12 @@ int main(int argc, const char** argv)
 	float px_per_deg = diag / 92.;
 
 
-	float yaw_cam = 100/px_per_deg, pitch_cam = 0;
+	float yaw_cam = 100/px_per_deg;
+	float pitch_cam = 0;
 	float roll_cam = 0.0;
 
 	Mat frame(Size(1280,720), CV_8UC3), frame_(Size(1280,720), CV_8UC3), gray, oldgray;
 
-	#define RINGBUF_SIZE 4
-	ModuloRingbuffer ringbuf_yaw_cam(RINGBUF_SIZE, -180, 180);
-	Ringbuffer ringbuf_pitch_cam(RINGBUF_SIZE);
-	ModuloRingbuffer ringbuf_roll_cam(RINGBUF_SIZE, -180,180);
-	ModuloRingbuffer ringbuf_roll_sensor(RINGBUF_SIZE, -180,180);
-	ModuloRingbuffer ringbuf_yaw_sensor(RINGBUF_SIZE, -180,180);
-	ModuloRingbuffer ringbuf_yaw_sensor_slow(40, -180,180);
-	ModuloRingbuffer ringbuf_pitch_sensor_slow(40, -180,180);
-	ModuloRingbuffer ringbuf_pitch_sensor(RINGBUF_SIZE, -180,180);
-
-
-	#define DELAY_SIZE 6
-	Ringbuffer delay_phi(DELAY_SIZE);   // should delay sensor data by ~0.2sec
-	Ringbuffer delay_psi(DELAY_SIZE);   // that's the amount the video lags behind
-	Ringbuffer delay_theta(DELAY_SIZE); // the sensor data
 
 
 	for (int i=0; i<400;i++)
@@ -587,18 +513,9 @@ int main(int argc, const char** argv)
 		cvtColor(frame, oldgray, COLOR_BGR2GRAY);
 	}
 
-	char key;
-	int adjust_phi=10;
-	float aberr_r=0.989, aberr_b=1.021;
-	int i=-1;
-	while (key=' ')
-	//while ((key=waitKey(1)) != 'e')
+	bool first_time = true;
+	while(true)
 	{
-		i++;
-		//printf("\033[H");
-
-		if (i%10==0)
-		{
 		drone.get(frame_, &navdata);
 		delay_phi.put(navdata.phi);
 		delay_psi.put(navdata.psi);
@@ -609,6 +526,7 @@ int main(int argc, const char** argv)
 
 		navdata.phi = fixup_angle(navdata.phi + adjust_phi);
 
+		/*
 		if (key=='q') adjust_phi++;
 		if (key=='w') adjust_phi--;
 		
@@ -616,6 +534,7 @@ int main(int argc, const char** argv)
 		if (key=='z') aberr_r-=0.001;
 		if (key=='s') aberr_b+=0.001;
 		if (key=='x') aberr_b-=0.001;
+		*/
 
 		//printf("aberr_r/b = \t%f\t%f\n",aberr_r,aberr_b);
 		
@@ -624,7 +543,6 @@ int main(int argc, const char** argv)
 
 		remap(frame_, frame, map1, map2, INTER_LINEAR);
 		cvtColor(frame, gray, COLOR_BGR2GRAY);
-		}
 		
 
 
@@ -636,7 +554,7 @@ int main(int argc, const char** argv)
 
 		Mat mat = estimateRigidTransform(gray, oldgray, false);
 
-		float angle; int shift_x, shift_y;
+		float angle; float shift_x, shift_y;
 		if (mat.total() > 0)
 		{
 			angle = atan2(mat.at<double>(0,1), mat.at<double>(0,0)) / PI * 180.;
@@ -661,8 +579,6 @@ int main(int argc, const char** argv)
 		ringbuf_roll_cam.put(roll_cam);
 		ringbuf_roll_sensor.put(navdata.phi);
 		ringbuf_yaw_sensor.put(navdata.psi);
-		ringbuf_yaw_sensor_slow.put(navdata.psi);
-		ringbuf_pitch_sensor_slow.put(navdata.theta);
 		ringbuf_pitch_sensor.put(navdata.theta);
 
 		double yaw_diff = fixup_range( ringbuf_yaw_cam.get() - ringbuf_yaw_sensor.get(), -180, 180);
@@ -686,24 +602,153 @@ int main(int argc, const char** argv)
 		//yaw_cam = navdata.psi;
 		//pitch_cam = - navdata.theta * px_per_deg;
 		//roll_cam = -navdata.phi;
+		
 
+		pthread_mutex_lock(&my_mutex);
+		yaw_cam_global = yaw_cam;
+		pitch_cam_global = pitch_cam;
+		roll_cam_global = roll_cam;
+		frame_global = frame.clone();
 
+		ringbuf_yaw_sensor_slow.put(navdata.psi);
+		ringbuf_pitch_sensor_slow.put(navdata.theta);
+		pthread_mutex_unlock(&my_mutex);
 
+		if (first_time)
+		{
+			sem_post(&oculus_thread_go);
+			first_time=false;
+		}
+		
+		oldgray = gray.clone();
+	}
+	return NULL;
+}
+
+int main(int argc, const char** argv)
+{
+	bool no_oculus = (argc>=2 && (strcmp(argv[1],"--no-oculus")==0));
 
+	pthread_mutex_init(&my_mutex, NULL);
+	sem_init(&oculus_thread_go, 0, 0);
 
 
+	GLFWwindow* window = initOpenGL();
 
 
+	GLuint vaoCanvas, vaoEye, vaoWholescreenQuad;
+	glGenVertexArrays(1, &vaoCanvas);
+	glGenVertexArrays(1, &vaoEye);
+	glGenVertexArrays(1, &vaoWholescreenQuad);
 
 
+	GLuint vboCanvas, vboEye, vboWholescreenQuad;
+	glGenBuffers(1, &vboCanvas);
+	glGenBuffers(1, &vboEye);
+	glGenBuffers(1, &vboWholescreenQuad);
+
+	glBindBuffer(GL_ARRAY_BUFFER, vboCanvas);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnCanvasVertices), drawOnCanvasVertices, GL_DYNAMIC_DRAW);
+
+	glBindBuffer(GL_ARRAY_BUFFER, vboEye);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(drawOnEyeVertices), drawOnEyeVertices, GL_STATIC_DRAW);
+
+	glBindBuffer(GL_ARRAY_BUFFER, vboWholescreenQuad);
+	glBufferData(GL_ARRAY_BUFFER, sizeof(wholescreenVertices), wholescreenVertices, GL_STATIC_DRAW);
+
+
+
+
+	
+	// compile shaders
+	GLuint drawOnOculusProgram = newOculusShaderProgram(vaoWholescreenQuad, vboWholescreenQuad);
+	GLint uniAberrR = glGetUniformLocation(drawOnOculusProgram, "aberr_r");
+	GLint uniAberrB = glGetUniformLocation(drawOnOculusProgram, "aberr_b");
+	
+	GLuint drawOnCanvasProgram = newCanvasShaderProgram(vaoCanvas, vboCanvas);
+	GLint uniCamYaw = glGetUniformLocation(drawOnCanvasProgram, "cam_yaw");
+	GLint uniCamPitch = glGetUniformLocation(drawOnCanvasProgram, "cam_pitch");
+	GLint uniCamRoll = glGetUniformLocation(drawOnCanvasProgram, "cam_roll");
+	
+	GLuint drawFromCanvasProgram = newEyeShaderProgram(vaoEye, vboEye);
+	GLint uniEyeYaw = glGetUniformLocation(drawFromCanvasProgram, "eye_yaw");
+	GLint uniEyePitch = glGetUniformLocation(drawFromCanvasProgram, "eye_pitch");
+	GLint uniEyeRoll = glGetUniformLocation(drawFromCanvasProgram, "eye_roll");
+
+	// texture
+	GLuint texVideo;
+	glGenTextures(1, &texVideo);
+	glBindTexture(GL_TEXTURE_2D, texVideo);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	
+
+
+// Framebuffer stuff
+	GLuint canvasFB, canvasTex;
+	genFramebuffer(canvasFB, canvasTex, CANVAS_WIDTH, CANVAS_HEIGHT, true);
+
+	GLuint eyeFB, eyeTex;
+	genFramebuffer(eyeFB, eyeTex, EYE_WIDTH, EYE_HEIGHT, false);
+
+
+
+
+
+
+
+	glEnable (GL_BLEND);
+	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+
+
+
+
+	ohmd_context* my_ohmd_context;
+	ohmd_device* my_ohmd_device;
+	if (no_oculus || init_ohmd(&my_ohmd_context, &my_ohmd_device))
+	{
+		printf("no oculus rift!\n");
+		my_ohmd_device=0;
+		my_ohmd_context=0;
+	}
+
+
+	pthread_t video_thread;
+	pthread_create(&video_thread, NULL, video_fetcher_thread, NULL);
+	sem_wait(&oculus_thread_go);
+
+
+
+	char key;
+	float aberr_r=0.989, aberr_b=1.021;
+	int i=-1;
+	while (key=' ')
+	//while ((key=waitKey(1)) != 'e')
+	{
+		i++;
+		//printf("\033[H");
+
+
+
+
+
+
+
+
+
+		pthread_mutex_lock(&my_mutex);
+		Mat frame_gl = frame_global.clone();
+		float yaw_cam = yaw_cam_global;
+		float pitch_cam = pitch_cam_global;
+		float roll_cam = roll_cam_global;
+		pthread_mutex_unlock(&my_mutex);
 
 
-		if (i%1==0)
-		{
 		glBindTexture(GL_TEXTURE_2D, texVideo);
-		Mat frame_gl = frame.clone();
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, frame_gl.size().width, frame_gl.size().height, 0, GL_RGB, GL_UNSIGNED_BYTE, frame_gl.ptr<unsigned char>(0));
-		}
 
 		
 		if(i!=0) setDrawOnCanvasRange((float)yaw_cam/180.*PI,-(float)pitch_cam/180.*PI,80./180.*PI,45/180.*PI);
@@ -803,7 +848,6 @@ int main(int argc, const char** argv)
 
 
 
-		oldgray = gray.clone();
 
 
 	}