diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 7dee592546..ef403b44fc 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -14,10 +14,12 @@ 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, CLContext, MonitoringModelFrame +from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid 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 = { @@ -52,15 +54,13 @@ class DMonitoringModelResult(ctypes.Structure): class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - frame: MonitoringModelFrame model: ModelRunner - def __init__(self, context: CLContext): + def __init__(self): 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 - transform = np.array([ - [1.5, 0.0, 264.0], - [0.0, 1.5, 226.0], - [0.0, 0.0, 1.0], - ], dtype=np.float32) + 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] t1 = time.perf_counter() - self.model.setInputBuffer("input_img", self.frame.prepare(buf, transform.flatten()).view(np.float32)) + self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32)) self.model.execute() t2 = time.perf_counter() return self.output, t2 - t1 @@ -117,13 +117,12 @@ def main(): gc.disable() set_realtime_priority(1) - cl_context = CLContext() - model = ModelState(cl_context) + model = ModelState() 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, cl_context) + vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True) 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 2f11572f9c..5e28e9b95d 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -70,32 +70,3 @@ 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 6ad2c012e9..1a079da055 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -45,20 +45,3 @@ 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 c3956d5008..57b79aeafb 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -18,8 +18,3 @@ 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 31ec98d9c9..e33d301aff 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, MonitoringModelFrame as cppMonitoringModelFrame +from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame def sigmoid(x): return cppSigmoid(x) @@ -41,18 +41,3 @@ 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 7c1c297fff..1dd33c9e91 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 = driver_data.faceProb > self.settings._FACE_THRESHOLD + self.face_detected = False #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