merge master

Author: deanlee

selfdrive/ui/qt/widgets/cameraview.cc

@@ -11,32 +11,34 @@ const char frame_vertex_shader[] =
 #else
   "#version 300 es\n"
 #endif
-  "in vec4 aPosition;\n"
-  "in vec4 aTexCoord;\n"
+  "layout(location = 0) in vec4 aPosition;\n"
+  "layout(location = 1) in vec2 aTexCoord;\n"
   "uniform mat4 uTransform;\n"
-  "out vec4 vTexCoord;\n"
+  "out vec2 vTexCoord;\n"
   "void main() {\n"
   "  gl_Position = uTransform * aPosition;\n"
   "  vTexCoord = aTexCoord;\n"
   "}\n";
 
-const char frame_fragment_shader[] =
-#ifdef NANOVG_GL3_IMPLEMENTATION
-  "#version 150 core\n"
-#else
-  "#version 300 es\n"
-#endif
-  "precision mediump float;\n"
-  "uniform sampler2D uTexture;\n"
-  "in vec4 vTexCoord;\n"
-  "out vec4 colorOut;\n"
-  "void main() {\n"
-  "  colorOut = texture(uTexture, vTexCoord.xy);\n"
-#ifdef QCOM
-  "  vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n"
-  "  colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n"
-#endif
-  "}\n";
+const char yuv_fragment_shader[] = R"(
+#version 300 es
+precision highp float;
+in vec2 vTexCoord;  
+out vec4 fragColor;
+uniform lowp sampler2D texture_y;  
+uniform lowp sampler2D texture_u;  
+uniform lowp sampler2D texture_v;   
+void main() {  
+  float y = texture2D(texture_y, vTexCoord).r;  
+  float u = texture2D(texture_u, vTexCoord).r - 0.5;  
+  float v = texture2D(texture_v, vTexCoord).r - 0.5;  
+  float r = y + 1.402 * v;  
+  float g = y - 0.344 * u - 0.714 * v;  
+  float b = y + 1.772 * u;  
+  fragColor = vec4(r, g, b, 1.0);
+}
+)";
+
 
 const mat4 device_transform = {{
   1.0,  0.0, 0.0, 0.0,
@@ -107,6 +109,7 @@ CameraViewWidget::~CameraViewWidget() {
     glDeleteVertexArrays(1, &frame_vao);
     glDeleteBuffers(1, &frame_vbo);
     glDeleteBuffers(1, &frame_ibo);
+    glDeleteBuffers(3, textures);
   }
   doneCurrent();
 }
@@ -117,14 +120,14 @@ void CameraViewWidget::initializeGL() {
   program = new QOpenGLShaderProgram(context());
   bool ret = program->addShaderFromSourceCode(QOpenGLShader::Vertex, frame_vertex_shader);
   assert(ret);
-  ret = program->addShaderFromSourceCode(QOpenGLShader::Fragment, frame_fragment_shader);
+  ret = program->addShaderFromSourceCode(QOpenGLShader::Fragment, yuv_fragment_shader);
   assert(ret);
 
   program->link();
   GLint frame_pos_loc = program->attributeLocation("aPosition");
   GLint frame_texcoord_loc = program->attributeLocation("aTexCoord");
 
-  auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f);
+  auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_YUV_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f);
   const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
   const float frame_coords[4][4] = {
     {-1.0, -1.0, x2, y1}, // bl
@@ -139,8 +142,7 @@ void CameraViewWidget::initializeGL() {
   glBindBuffer(GL_ARRAY_BUFFER, frame_vbo);
   glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW);
   glEnableVertexAttribArray(frame_pos_loc);
-  glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
-                        sizeof(frame_coords[0]), (const void *)0);
+  glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, sizeof(frame_coords[0]), (const void *)0);
   glEnableVertexAttribArray(frame_texcoord_loc);
   glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
                         sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2));
@@ -149,6 +151,11 @@ void CameraViewWidget::initializeGL() {
   glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW);
   glBindBuffer(GL_ARRAY_BUFFER, 0);
   glBindVertexArray(0);
+
+  glUseProgram(program->programId());
+  glUniform1i(program->uniformLocation("texture_y"), 0);
+  glUniform1i(program->uniformLocation("texture_u"), 1);
+  glUniform1i(program->uniformLocation("texture_v"), 2);
 }
 
 void CameraViewWidget::showEvent(QShowEvent *event) {
@@ -172,12 +179,12 @@ void CameraViewWidget::hideEvent(QHideEvent *event) {
 
 void CameraViewWidget::updateFrameMat(int w, int h) {
   if (zoomed_view) {
-    if (stream_type == VISION_STREAM_RGB_FRONT) {
+    if (stream_type == VISION_STREAM_YUV_FRONT) {
       frame_mat = matmul(device_transform, get_driver_view_transform());
     } else {
-      auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
+      auto intrinsic_matrix = stream_type == VISION_STREAM_YUV_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
       float zoom = ZOOM / intrinsic_matrix.v[0];
-      if (stream_type == VISION_STREAM_RGB_WIDE) {
+      if (stream_type == VISION_STREAM_YUV_WIDE) {
         zoom *= 0.5;
       }
       float zx = zoom * 2 * intrinsic_matrix.v[2] / width();
@@ -205,18 +212,22 @@ void CameraViewWidget::paintGL() {
     glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
     return;
   }
-  std::unique_lock lk(texture_lock);
 
   glViewport(0, 0, width(), height());
-
   glBindVertexArray(frame_vao);
-  glActiveTexture(GL_TEXTURE0);
-  glBindTexture(GL_TEXTURE_2D, texture[latest_frame->idx]->frame_tex);
 
   glUseProgram(program->programId());
-  glUniform1i(program->uniformLocation("uTexture"), 0);
-  glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
+  uint8_t *address[3] = {latest_frame->y, latest_frame->u, latest_frame->v};
+  for (int i = 0; i < 3; ++i) {
+    glActiveTexture(GL_TEXTURE0 + i);
+    glBindTexture(GL_TEXTURE_2D, textures[i]);
+    int width = i == 0 ? stream_width : stream_width / 2;
+    int height = i == 0 ? stream_height : stream_height / 2;
+    glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_LUMINANCE, GL_UNSIGNED_BYTE, address[i]);
+    assert(glGetError() == GL_NO_ERROR);
+  }
 
+  glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
   assert(glGetError() == GL_NO_ERROR);
   glEnableVertexAttribArray(0);
   glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
@@ -226,22 +237,24 @@ void CameraViewWidget::paintGL() {
 
 void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
   makeCurrent();
-  for (int i = 0; i < vipc_client->num_buffers; i++) {
-    texture[i].reset(new EGLImageTexture(&vipc_client->buffers[i]));
-
-    glBindTexture(GL_TEXTURE_2D, texture[i]->frame_tex);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-
-    // BGR
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
-    assert(glGetError() == GL_NO_ERROR);
-  }
   latest_frame = nullptr;
   stream_width = vipc_client->buffers[0].width;
   stream_height = vipc_client->buffers[0].height;
+
+  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+  glGenTextures(3, textures);
+  for (int i = 0; i < 3; ++i) {
+    glBindTexture(GL_TEXTURE_2D, textures[i]);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+    int width = i == 0 ? stream_width : stream_width / 2;
+    int height = i == 0 ? stream_height : stream_height / 2;
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, nullptr);
+    assert(glGetError() == GL_NO_ERROR);
+  }
+
   updateFrameMat(width(), height());
 }
 
@@ -254,25 +267,6 @@ void CameraViewWidget::vipcThread() {
   VisionStreamType cur_stream_type = stream_type;
   std::unique_ptr<VisionIpcClient> vipc_client;
 
-  std::unique_ptr<QOpenGLContext> ctx;
-  std::unique_ptr<QOffscreenSurface> surface;
-  std::unique_ptr<QOpenGLBuffer> gl_buffer;
-
-  if (!Hardware::EON()) {
-    ctx = std::make_unique<QOpenGLContext>();
-    ctx->setFormat(context()->format());
-    ctx->setShareContext(context());
-    ctx->create();
-    assert(ctx->isValid());
-
-    surface = std::make_unique<QOffscreenSurface>();
-    surface->setFormat(ctx->format());
-    surface->create();
-    ctx->makeCurrent(surface.get());
-    assert(QOpenGLContext::currentContext() == ctx.get());
-    initializeOpenGLFunctions();
-  }
-
   while (!QThread::currentThread()->isInterruptionRequested()) {
     if (!vipc_client || cur_stream_type != stream_type) {
       cur_stream_type = stream_type;
@@ -285,37 +279,14 @@ void CameraViewWidget::vipcThread() {
         continue;
       }
 
-      if (!Hardware::EON()) {
-        gl_buffer.reset(new QOpenGLBuffer(QOpenGLBuffer::PixelUnpackBuffer));
-        gl_buffer->create();
-        gl_buffer->bind();
-        gl_buffer->setUsagePattern(QOpenGLBuffer::StreamDraw);
-        gl_buffer->allocate(vipc_client->buffers[0].len);
-      }
+     
 
       emit vipcThreadConnected(vipc_client.get());
     }
 
     if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) {
-      if (!Hardware::EON()) {
-        std::unique_lock lk(texture_lock);
-
-        void *texture_buffer = gl_buffer->map(QOpenGLBuffer::WriteOnly);
-        memcpy(texture_buffer, buf->addr, buf->len);
-        gl_buffer->unmap();
-
-        // copy pixels from PBO to texture object
-        glBindTexture(GL_TEXTURE_2D, texture[buf->idx]->frame_tex);
-        glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, buf->width, buf->height, GL_RGB, GL_UNSIGNED_BYTE, 0);
-        glBindTexture(GL_TEXTURE_2D, 0);
-        assert(glGetError() == GL_NO_ERROR);
-
-        emit vipcThreadFrameReceived(buf);
-
-        glFlush();
-      } else {
-        emit vipcThreadFrameReceived(buf);
-      }
-    }
+       emit vipcThreadFrameReceived(buf);
+     }
+    
   }
 }