diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index ef403b44fc..7dee592546 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/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() diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 5e28e9b95d..2f11572f9c 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/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(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)); +} diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 1a079da055..6ad2c012e9 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -45,3 +45,20 @@ private: cl_mem y_cl, u_cl, v_cl, net_input_cl; std::unique_ptr 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 input_frame; +}; diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index 57b79aeafb..c3956d5008 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/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) diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index e33d301aff..31ec98d9c9 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/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( 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( data) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 1dd33c9e91..7c1c297fff 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/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