diff options
| -rw-r--r-- | client2.cpp | 280 |
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(); } |