IQ.Pilot Release Commit @ 0798119

iqpilot/selfdrive/iqmodeld/native/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""

iqpilot/selfdrive/iqmodeld/native/iqmodel.cc

@@ -0,0 +1,100 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+*/
+#include "iqpilot/selfdrive/iqmodeld/native/iqmodel.h"
+
+#include <cstring>
+
+#include "common/clutil.h"
+
+namespace {
+
+void rotate_history_window(cl_command_queue queue, cl_mem timeline_cl, uint8_t history_slots, size_t frame_bytes) {
+  for (int slot = 0; slot < (history_slots - 1); slot++) {
+    CL_CHECK(clEnqueueCopyBuffer(queue, timeline_cl, timeline_cl,
+                                 (slot + 1) * frame_bytes, slot * frame_bytes,
+                                 frame_bytes, 0, nullptr, nullptr));
+  }
+}
+
+}  // namespace
+
+FrameCropperBase::FrameCropperBase(cl_device_id device_id, cl_context context) {
+  work_queue_ = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
+}
+
+FrameCropperBase::~FrameCropperBase() {
+  CL_CHECK(clReleaseCommandQueue(work_queue_));
+}
+
+void FrameCropperBase::configure_planar_tiles(cl_device_id device_id, cl_context context, int output_width, int output_height) {
+  y_plane_tile_cl_ = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, output_width * output_height, NULL, &err));
+  u_plane_tile_cl_ = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (output_width / 2) * (output_height / 2), NULL, &err));
+  v_plane_tile_cl_ = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (output_width / 2) * (output_height / 2), NULL, &err));
+  warp_sampler_init(&sampler_state_, context, device_id);
+}
+
+void FrameCropperBase::release_planar_tiles() {
+  warp_sampler_release(&sampler_state_);
+  CL_CHECK(clReleaseMemObject(v_plane_tile_cl_));
+  CL_CHECK(clReleaseMemObject(u_plane_tile_cl_));
+  CL_CHECK(clReleaseMemObject(y_plane_tile_cl_));
+}
+
+void FrameCropperBase::project_frame(cl_mem yuv_cl, int output_width, int output_height,
+                                     int frame_width, int frame_height, int frame_stride, int frame_uv_offset,
+                                     const mat3 &projection) {
+  warp_sampler_dispatch(&sampler_state_, work_queue_,
+                        yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+                        y_plane_tile_cl_, u_plane_tile_cl_, v_plane_tile_cl_,
+                        output_width, output_height, projection);
+}
+
+RoadHistoryAssembler::RoadHistoryAssembler(cl_device_id device_id, cl_context context, uint8_t history_slots)
+    : FrameCropperBase(device_id, context), frame_bytes_(kFrameBytes * sizeof(uint8_t)), history_slots_(history_slots) {
+  buf_size = kExportBytes;
+  staging_bytes_ = std::make_unique<uint8_t[]>(buf_size);
+  publish_pair_cl_ = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
+  timeline_cl_ = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, history_slots_ * frame_bytes_, NULL, &err));
+
+  latest_region_.origin = (history_slots_ - 1) * frame_bytes_;
+  latest_region_.size = frame_bytes_;
+  latest_slot_cl_ = CL_CHECK_ERR(clCreateSubBuffer(timeline_cl_, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, &latest_region_, &err));
+
+  packed_frame_kernels_init(&packer_, context, device_id, kOutputWidth, kOutputHeight);
+  configure_planar_tiles(device_id, context, kOutputWidth, kOutputHeight);
+}
+
+cl_mem *RoadHistoryAssembler::project_to_cl(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) {
+  project_frame(yuv_cl, kOutputWidth, kOutputHeight, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
+  rotate_history_window(work_queue_, timeline_cl_, history_slots_, frame_bytes_);
+  packed_frame_emit(&packer_, work_queue_, y_plane_tile_cl_, u_plane_tile_cl_, v_plane_tile_cl_, latest_slot_cl_);
+  packed_frame_clone_range(&packer_, work_queue_, timeline_cl_, publish_pair_cl_, 0, 0, frame_bytes_);
+  packed_frame_clone_range(&packer_, work_queue_, latest_slot_cl_, publish_pair_cl_, 0, frame_bytes_, frame_bytes_);
+  clFinish(work_queue_);
+  return &publish_pair_cl_;
+}
+
+RoadHistoryAssembler::~RoadHistoryAssembler() {
+  release_planar_tiles();
+  packed_frame_kernels_release(&packer_);
+  CL_CHECK(clReleaseMemObject(publish_pair_cl_));
+  CL_CHECK(clReleaseMemObject(timeline_cl_));
+  CL_CHECK(clReleaseMemObject(latest_slot_cl_));
+}
+
+CabinFrameSampler::CabinFrameSampler(cl_device_id device_id, cl_context context) : FrameCropperBase(device_id, context) {
+  buf_size = kExportBytes;
+  staging_bytes_ = std::make_unique<uint8_t[]>(buf_size);
+  configure_planar_tiles(device_id, context, kOutputWidth, kOutputHeight);
+}
+
+cl_mem *CabinFrameSampler::project_to_cl(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) {
+  project_frame(yuv_cl, kOutputWidth, kOutputHeight, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
+  clFinish(work_queue_);
+  return &y_plane_tile_cl_;
+}
+
+CabinFrameSampler::~CabinFrameSampler() {
+  release_planar_tiles();
+}

iqpilot/selfdrive/iqmodeld/native/iqmodel.h

@@ -0,0 +1,81 @@
+/*
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+*/
+#pragma once
+
+#include <cassert>
+#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 "iqpilot/selfdrive/iqmodeld/transforms/warp_geometry.h"
+#include "iqpilot/selfdrive/iqmodeld/transforms/yuv.h"
+
+class FrameCropperBase {
+public:
+  FrameCropperBase(cl_device_id device_id, cl_context context);
+  virtual ~FrameCropperBase();
+  virtual cl_mem *project_to_cl(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) = 0;
+
+  uint8_t *copy_to_host(cl_mem *source_frames, int buffer_size) {
+    CL_CHECK(clEnqueueReadBuffer(work_queue_, *source_frames, CL_TRUE, 0, buffer_size, staging_bytes_.get(), 0, nullptr, nullptr));
+    clFinish(work_queue_);
+    return &staging_bytes_[0];
+  }
+
+  int buf_size;
+
+protected:
+  cl_command_queue work_queue_;
+  std::unique_ptr<uint8_t[]> staging_bytes_;
+  cl_mem y_plane_tile_cl_;
+  cl_mem u_plane_tile_cl_;
+  cl_mem v_plane_tile_cl_;
+  WarpSamplerState sampler_state_;
+
+  void configure_planar_tiles(cl_device_id device_id, cl_context context, int output_width, int output_height);
+  void release_planar_tiles();
+  void project_frame(cl_mem yuv_cl, int output_width, int output_height,
+                     int frame_width, int frame_height, int frame_stride, int frame_uv_offset,
+                     const mat3 &projection);
+};
+
+class RoadHistoryAssembler : public FrameCropperBase {
+public:
+  RoadHistoryAssembler(cl_device_id device_id, cl_context context, uint8_t history_slots);
+  ~RoadHistoryAssembler() override;
+  cl_mem *project_to_cl(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) override;
+
+  static constexpr int kOutputWidth = 512;
+  static constexpr int kOutputHeight = 256;
+  static constexpr int kFrameBytes = kOutputWidth * kOutputHeight * 3 / 2;
+  static constexpr int kExportBytes = kFrameBytes * 2;
+
+private:
+  PackedFrameKernels packer_;
+  cl_mem timeline_cl_;
+  cl_mem latest_slot_cl_;
+  cl_mem publish_pair_cl_;
+  cl_buffer_region latest_region_;
+  size_t frame_bytes_;
+  uint8_t history_slots_;
+};
+
+class CabinFrameSampler : public FrameCropperBase {
+public:
+  CabinFrameSampler(cl_device_id device_id, cl_context context);
+  ~CabinFrameSampler() override;
+  cl_mem *project_to_cl(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection) override;
+
+  static constexpr int kOutputWidth = 1440;
+  static constexpr int kOutputHeight = 960;
+  static constexpr int kExportBytes = kOutputWidth * kOutputHeight;
+};

iqpilot/selfdrive/iqmodeld/native/iqmodel.pxd

@@ -0,0 +1,29 @@
+# -*- coding: utf-8 -*-
+# distutils: language = c++
+# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+from msgq.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)
+  void cl_release_context(cl_context)
+
+cdef extern from "iqpilot/selfdrive/iqmodeld/native/iqmodel.h":
+  cppclass NativeFrameBridge "FrameCropperBase":
+    int buf_size
+    unsigned char * copy_to_host(cl_mem*, int);
+    cl_mem * project_to_cl(cl_mem, int, int, int, int, mat3)
+
+  cppclass RoadFrameBridge "RoadHistoryAssembler":
+    int buf_size
+    RoadFrameBridge(cl_device_id, cl_context, unsigned char)
+
+  cppclass CabinFrameBridge "CabinFrameSampler":
+    int buf_size
+    CabinFrameBridge(cl_device_id, cl_context)

iqpilot/selfdrive/iqmodeld/native/iqmodel_pyx.pxd

@@ -0,0 +1,12 @@
+# -*- coding: utf-8 -*-
+# distutils: language = c++
+# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport CLContext as VisionIpcContextBase
+
+cdef class WarpContext(VisionIpcContextBase):
+  pass
+
+cdef class GpuMemorySlot:
+  cdef cl_mem * handle_ptr

iqpilot/selfdrive/iqmodeld/native/iqmodel_pyx.pyx

@@ -0,0 +1,100 @@
+# -*- coding: utf-8 -*-
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+import numpy as np
+cimport numpy as cnp
+from libc.string cimport memcpy
+from libc.stdint cimport uintptr_t
+
+from msgq.visionipc.visionipc cimport cl_mem
+from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as VisionIpcContextBase
+from .iqmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context
+from .iqmodel cimport mat3, NativeFrameBridge, RoadFrameBridge, CabinFrameBridge
+
+
+cdef inline mat3 _projection_to_mat3(float[:] values):
+  cdef mat3 warp_matrix
+  memcpy(warp_matrix.v, &values[0], 9 * sizeof(float))
+  return warp_matrix
+
+
+cdef inline GpuMemorySlot _borrow_cl_slot(void * raw_handle):
+  cdef GpuMemorySlot carrier = GpuMemorySlot()
+  carrier.handle_ptr = <cl_mem*>raw_handle
+  return carrier
+
+
+cdef inline object _read_u8_view(unsigned char * payload, int length):
+  return np.asarray(<cnp.uint8_t[:length]> payload)
+
+
+cdef class WarpContext(VisionIpcContextBase):
+  def __cinit__(self):
+    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
+    self.context = cl_create_context(self.device_id)
+
+  def __dealloc__(self):
+    if self.context:
+      cl_release_context(self.context)
+
+cdef class GpuMemorySlot:
+  @property
+  def mem_address(self):
+    return <uintptr_t>(self.handle_ptr)
+
+
+def cl_from_visionbuf(VisionBuf buf):
+  return _borrow_cl_slot(<void*>&buf.buf.buf_cl)
+
+
+cdef class _ProjectionBridge:
+  cdef NativeFrameBridge * _native_ptr
+  cdef int _export_bytes
+
+  def __dealloc__(self):
+    del self._native_ptr
+
+  cdef void _attach(self, NativeFrameBridge * native_frame, int export_bytes):
+    self._native_ptr = native_frame
+    self._export_bytes = export_bytes
+
+  cdef GpuMemorySlot _stage_image(self, VisionBuf buf, float[:] projection):
+    cdef mat3 projection_spec = _projection_to_mat3(projection)
+    cdef cl_mem * exported_slot = self._native_ptr.project_to_cl(
+      buf.buf.buf_cl,
+      buf.width,
+      buf.height,
+      buf.stride,
+      buf.uv_offset,
+      projection_spec,
+    )
+    return _borrow_cl_slot(exported_slot)
+
+  cdef object _export_host_bytes(self, GpuMemorySlot opencl_slot):
+    cdef unsigned char * payload = self._native_ptr.copy_to_host(opencl_slot.handle_ptr, self._export_bytes)
+    return _read_u8_view(payload, self._export_bytes)
+
+
+cdef class FrameProjector(_ProjectionBridge):
+  def stage(self, VisionBuf buf, float[:] projection):
+    return self._stage_image(buf, projection)
+
+  def as_numpy(self, GpuMemorySlot in_frames):
+    return self._export_host_bytes(in_frames)
+
+
+cdef class RoadProjector(FrameProjector):
+  cdef RoadFrameBridge * _road_ptr
+
+  def __cinit__(self, WarpContext context, int buffer_length=2):
+    self._road_ptr = new RoadFrameBridge(context.device_id, context.context, buffer_length)
+    self._attach(<NativeFrameBridge*>self._road_ptr, self._road_ptr.buf_size)
+
+cdef class CabinProjector(FrameProjector):
+  cdef CabinFrameBridge * _cabin_ptr
+
+  def __cinit__(self, WarpContext context):
+    self._cabin_ptr = new CabinFrameBridge(context.device_id, context.context)
+    self._attach(<NativeFrameBridge*>self._cabin_ptr, self._cabin_ptr.buf_size)