openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

selfdrive/modeld/models/commonmodel.cc

@@ -0,0 +1,72 @@
+#include "selfdrive/modeld/models/commonmodel.h"
+
+#include <algorithm>
+#include <cassert>
+#include <cmath>
+#include <cstring>
+
+#include "common/clutil.h"
+#include "common/mat.h"
+#include "common/timing.h"
+
+ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
+  input_frames = std::make_unique<float[]>(buf_size);
+
+  q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
+  y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_WIDTH * MODEL_HEIGHT, NULL, &err));
+  u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
+  v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (MODEL_WIDTH / 2) * (MODEL_HEIGHT / 2), NULL, &err));
+  net_input_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_FRAME_SIZE * sizeof(float), NULL, &err));
+
+  transform_init(&transform, context, device_id);
+  loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
+}
+
+float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
+  transform_queue(&this->transform, q,
+                  yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+                  y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
+
+  if (output == NULL) {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
+
+    std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
+    CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
+    clFinish(q);
+    return &input_frames[0];
+  } else {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
+    // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
+    clFinish(q);
+    return NULL;
+  }
+}
+
+ModelFrame::~ModelFrame() {
+  transform_destroy(&transform);
+  loadyuv_destroy(&loadyuv);
+  CL_CHECK(clReleaseMemObject(net_input_cl));
+  CL_CHECK(clReleaseMemObject(v_cl));
+  CL_CHECK(clReleaseMemObject(u_cl));
+  CL_CHECK(clReleaseMemObject(y_cl));
+  CL_CHECK(clReleaseCommandQueue(q));
+}
+
+void softmax(const float* input, float* output, size_t len) {
+  const float max_val = *std::max_element(input, input + len);
+  float denominator = 0;
+  for (int i = 0; i < len; i++) {
+    float const v_exp = expf(input[i] - max_val);
+    denominator += v_exp;
+    output[i] = v_exp;
+  }
+
+  const float inv_denominator = 1. / denominator;
+  for (int i = 0; i < len; i++) {
+    output[i] *= inv_denominator;
+  }
+}
+
+float sigmoid(float input) {
+  return 1 / (1 + expf(-input));
+}

selfdrive/modeld/models/commonmodel.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <cfloat>
+#include <cstdlib>
+
+#include <memory>
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/mat.h"
+#include "cereal/messaging/messaging.h"
+#include "selfdrive/modeld/transforms/loadyuv.h"
+#include "selfdrive/modeld/transforms/transform.h"
+
+const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
+
+void softmax(const float* input, float* output, size_t len);
+float sigmoid(float input);
+
+template<class T, size_t size>
+constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
+  return kj::ArrayPtr(arr.data(), arr.size());
+}
+
+class ModelFrame {
+public:
+  ModelFrame(cl_device_id device_id, cl_context context);
+  ~ModelFrame();
+  float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
+
+  const int MODEL_WIDTH = 512;
+  const int MODEL_HEIGHT = 256;
+  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
+  const int buf_size = MODEL_FRAME_SIZE * 2;
+
+private:
+  Transform transform;
+  LoadYUVState loadyuv;
+  cl_command_queue q;
+  cl_mem y_cl, u_cl, v_cl, net_input_cl;
+  std::unique_ptr<float[]> input_frames;
+};

selfdrive/modeld/models/commonmodel.pxd

@@ -0,0 +1,20 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
+
+cdef extern from "common/mat.h":
+  cdef struct mat3:
+    float v[9]
+
+cdef extern from "common/clutil.h":
+  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
+  cl_device_id cl_get_device_id(unsigned long)
+  cl_context cl_create_context(cl_device_id)
+
+cdef extern from "selfdrive/modeld/models/commonmodel.h":
+  float sigmoid(float)
+
+  cppclass ModelFrame:
+    int buf_size
+    ModelFrame(cl_device_id, cl_context)
+    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.pxd

@@ -0,0 +1,13 @@
+# distutils: language = c++
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
+
+cdef class CLContext(BaseCLContext):
+  pass
+
+cdef class CLMem:
+  cdef cl_mem * mem;
+
+  @staticmethod
+  cdef create(void*)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -0,0 +1,43 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+
+from cereal.visionipc.visionipc cimport cl_mem
+from cereal.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
+from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
+from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+
+def sigmoid(x):
+  return cppSigmoid(x)
+
+cdef class CLContext(BaseCLContext):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+cdef class CLMem:
+  @staticmethod
+  cdef create(void * cmem):
+    mem = CLMem()
+    mem.mem = <cl_mem*> cmem
+    return mem
+
+cdef class ModelFrame:
+  cdef cppModelFrame * frame
+
+  def __cinit__(self, CLContext context):
+    self.frame = new cppModelFrame(context.device_id, context.context)
+
+  def __dealloc__(self):
+    del self.frame
+
+  def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef float * data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
+    if not data:
+      return None
+    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)