shane owes me m4a4

selfdrive/modeld/dmonitoringmodeld.py

@@ -14,12 +14,10 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_realtime_priority
 from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
-from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid, CLContext, MonitoringModelFrame
 
 CALIB_LEN = 3
 REG_SCALE = 0.25
-MODEL_WIDTH = 1440
-MODEL_HEIGHT = 960
 OUTPUT_SIZE = 84
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 MODEL_PATHS = {
@@ -54,13 +52,15 @@ class DMonitoringModelResult(ctypes.Structure):
 class ModelState:
   inputs: dict[str, np.ndarray]
   output: np.ndarray
+  frame: MonitoringModelFrame
   model: ModelRunner
 
-  def __init__(self):
+  def __init__(self, context: CLContext):
     assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
+    self.frame = MonitoringModelFrame(context)
     self.output = np.zeros(OUTPUT_SIZE, dtype=np.float32)
     self.inputs = {
-      'input_img': np.zeros(MODEL_HEIGHT * MODEL_WIDTH, dtype=np.uint8),
+      # 'input_img': np.zeros(MODEL_HEIGHT * MODEL_WIDTH, dtype=np.uint8),
       'calib': np.zeros(CALIB_LEN, dtype=np.float32)}
 
     self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None)
@@ -70,14 +70,14 @@ class ModelState:
   def run(self, buf:VisionBuf, calib:np.ndarray) -> tuple[np.ndarray, float]:
     self.inputs['calib'][:] = calib
 
-    v_offset = buf.height - MODEL_HEIGHT
-    h_offset = (buf.width - MODEL_WIDTH) // 2
-    buf_data = buf.data.reshape(-1, buf.stride)
-    input_data = self.inputs['input_img'].reshape(MODEL_HEIGHT, MODEL_WIDTH)
-    input_data[:] = buf_data[v_offset:v_offset+MODEL_HEIGHT, h_offset:h_offset+MODEL_WIDTH]
+    transform = np.array([
+      [1.5, 0.0, 264.0],
+      [0.0, 1.5, 226.0],
+      [0.0, 0.0, 1.0],
+    ], dtype=np.float32)
 
     t1 = time.perf_counter()
-    self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    self.model.setInputBuffer("input_img", self.frame.prepare(buf, transform.flatten()).view(np.float32))
     self.model.execute()
     t2 = time.perf_counter()
     return self.output, t2 - t1
@@ -117,12 +117,13 @@ def main():
   gc.disable()
   set_realtime_priority(1)
 
-  model = ModelState()
+  cl_context = CLContext()
+  model = ModelState(cl_context)
   cloudlog.warning("models loaded, dmonitoringmodeld starting")
   Params().put_bool("DmModelInitialized", True)
 
   cloudlog.warning("connecting to driver stream")
-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True, cl_context)
   while not vipc_client.connect(False):
     time.sleep(0.1)
   assert vipc_client.is_connected()

selfdrive/modeld/models/commonmodel.cc

@@ -70,3 +70,32 @@ void softmax(const float* input, float* output, size_t len) {
 float sigmoid(float input) {
   return 1 / (1 + expf(-input));
 }
+
+MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) {
+  input_frame = std::make_unique<unsigned char[]>(MODEL_FRAME_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));
+
+  transform_init(&transform, context, device_id);
+}
+
+unsigned char* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) {
+  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);
+
+  CL_CHECK(clEnqueueReadBuffer(q, y_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(uint8_t), input_frame.get(), 0, nullptr, nullptr));
+  clFinish(q);
+  return input_frame.get();
+}
+
+MonitoringModelFrame::~MonitoringModelFrame() {
+  transform_destroy(&transform);
+  CL_CHECK(clReleaseMemObject(v_cl));
+  CL_CHECK(clReleaseMemObject(u_cl));
+  CL_CHECK(clReleaseMemObject(y_cl));
+  CL_CHECK(clReleaseCommandQueue(q));
+}

selfdrive/modeld/models/commonmodel.h

@@ -45,3 +45,20 @@ private:
   cl_mem y_cl, u_cl, v_cl, net_input_cl;
   std::unique_ptr<float[]> input_frames;
 };
+
+class MonitoringModelFrame {
+public:
+  MonitoringModelFrame(cl_device_id device_id, cl_context context);
+  ~MonitoringModelFrame();
+  unsigned char* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform);
+
+  const int MODEL_WIDTH = 1440;
+  const int MODEL_HEIGHT = 960;
+  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT;
+
+private:
+  Transform transform;
+  cl_command_queue q;
+  cl_mem y_cl, u_cl, v_cl;
+  std::unique_ptr<unsigned char[]> input_frame;
+};

selfdrive/modeld/models/commonmodel.pxd

@@ -18,3 +18,8 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h":
     int buf_size
     ModelFrame(cl_device_id, cl_context)
     float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
+
+  cppclass MonitoringModelFrame:
+    int MODEL_FRAME_SIZE
+    MonitoringModelFrame(cl_device_id, cl_context)
+    unsigned char * prepare(cl_mem, int, int, int, int, mat3)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -8,7 +8,7 @@ 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
+from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
 
 def sigmoid(x):
   return cppSigmoid(x)
@@ -41,3 +41,18 @@ cdef class ModelFrame:
     if not data:
       return None
     return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
+
+cdef class MonitoringModelFrame(ModelFrame):
+  cdef cppMonitoringModelFrame * dmframe
+
+  def __cinit__(self, CLContext context):
+    self.dmframe = new cppMonitoringModelFrame(context.device_id, context.context)
+
+  def __dealloc__(self):
+    del self.dmframe
+
+  def prepare(self, VisionBuf buf, float[:] projection):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef unsigned char * data = self.dmframe.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection)
+    return np.asarray(<cnp.uint8_t[:self.dmframe.MODEL_FRAME_SIZE]> data)

selfdrive/monitoring/driver_monitor.py

@@ -259,7 +259,7 @@ class DriverStatus():
                                     driver_data.readyProb, driver_data.notReadyProb)):
       return
 
-    self.face_detected = False #driver_data.faceProb > self.settings._FACE_THRESHOLD
+    self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
     self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
     if self.wheel_on_right:
       self.pose.yaw *= -1