IQ.Pilot Prebuilt Release @ ab07000

selfdrive/modeld/constants.py

@@ -0,0 +1,87 @@
+import numpy as np
+
+def index_function(idx, max_val=192, max_idx=32):
+  return (max_val) * ((idx/max_idx)**2)
+
+class ModelConstants:
+  # time and distance indices
+  IDX_N = 33
+  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
+  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
+  LEAD_T_OFFSETS = [0., 2., 4.]
+  META_T_IDXS = [2., 4., 6., 8., 10.]
+
+  # model inputs constants
+  N_FRAMES = 2
+  MODEL_RUN_FREQ = 20
+  MODEL_CONTEXT_FREQ = 5 # "model_trained_fps"
+
+  FEATURE_LEN = 512
+
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  # model outputs constants
+  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
+  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+  DESIRED_CURV_WIDTH = 1
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+
+  POLY_PATH_DEGREE = 4
+
+# model outputs slices
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+class Meta:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

selfdrive/modeld/dmonitoringmodeld.py

@@ -0,0 +1,157 @@
+#!/usr/bin/env python3
+import os
+from openpilot.system.hardware import TICI
+os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
+from tinygrad.tensor import Tensor
+from tinygrad.dtype import dtypes
+import time
+import pickle
+import numpy as np
+from pathlib import Path
+
+from cereal import messaging
+from cereal.messaging import PubMaster, SubMaster
+from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.realtime import config_realtime_process
+from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
+from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
+from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
+
+PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
+METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
+
+
+class ModelState:
+  inputs: dict[str, np.ndarray]
+  output: np.ndarray
+
+  def __init__(self, cl_ctx):
+    with open(METADATA_PATH, 'rb') as f:
+      model_metadata = pickle.load(f)
+      self.input_shapes = model_metadata['input_shapes']
+      self.output_slices = model_metadata['output_slices']
+
+    self.frame = MonitoringModelFrame(cl_ctx)
+    self.numpy_inputs = {
+      'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
+    }
+
+    self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
+    with open(MODEL_PKL_PATH, "rb") as f:
+      self.model_run = pickle.load(f)
+
+  def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
+    self.numpy_inputs['calib'][0,:] = calib
+
+    t1 = time.perf_counter()
+
+    input_img_cl = self.frame.prepare(buf, transform.flatten())
+    if TICI:
+      # The imgs tensors are backed by opencl memory, only need init once
+      if 'input_img' not in self.tensor_inputs:
+        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8)
+    else:
+      self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize()
+
+
+    output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
+
+    t2 = time.perf_counter()
+    return output, t2 - t1
+
+def slice_outputs(model_outputs, output_slices):
+  return  {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
+
+def parse_model_output(model_output):
+  parsed = {}
+  parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
+  for ds_suffix in ['lhd', 'rhd']:
+    face_descs = model_output[f'face_descs_{ds_suffix}']
+    parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
+    parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
+    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
+      parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
+  return parsed
+
+def fill_driver_data(msg, model_output, ds_suffix):
+  msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
+  msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
+  msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
+  msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
+  msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
+  msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
+  msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
+  msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
+  msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
+
+def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
+  msg = messaging.new_message('driverStateV2', valid=True)
+  ds = msg.driverStateV2
+  ds.frameId = frame_id
+  ds.modelExecutionTime = exec_time
+  ds.gpuExecutionTime = gpu_exec_time
+  ds.rawPredictions = model_output['raw_pred']
+  ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
+  fill_driver_data(ds.leftDriverData, model_output, 'lhd')
+  fill_driver_data(ds.rightDriverData, model_output, 'rhd')
+  return msg
+
+
+def main():
+  config_realtime_process(7, 5)
+
+  cl_context = CLContext()
+  model = ModelState(cl_context)
+  cloudlog.warning("models loaded, dmonitoringmodeld starting")
+
+  cloudlog.warning("connecting to driver stream")
+  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()
+  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
+
+  sm = SubMaster(["liveCalibration"])
+  pm = PubMaster(["driverStateV2"])
+
+  calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
+  model_transform = None
+
+  while True:
+    buf = vipc_client.recv()
+    if buf is None:
+      continue
+
+    if model_transform is None:
+      cam = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
+      model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32)
+
+    sm.update(0)
+    if sm.updated["liveCalibration"]:
+      calib_rpy = get_calibrated_rpy(sm["liveCalibration"])
+      calib[:] = calib_rpy if calib_rpy is not None else np.zeros_like(calib)
+
+    t1 = time.perf_counter()
+    model_output, gpu_execution_time = model.run(buf, calib, model_transform)
+    t2 = time.perf_counter()
+    raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
+    model_output = slice_outputs(model_output, model.output_slices)
+    model_output = parse_model_output(model_output)
+    model_output['raw_pred'] = raw_pred
+    msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
+    pm.send("driverStateV2", msg)
+
+
+if __name__ == "__main__":
+  try:
+    main()
+  except KeyboardInterrupt:
+    cloudlog.warning("got SIGINT")

selfdrive/modeld/fill_model_msg.py

@@ -0,0 +1,194 @@
+import os
+import capnp
+import numpy as np
+from cereal import log
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
+from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import plan_x_idxs_helper
+
+SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
+
+ConfidenceClass = log.ModelDataV2.ConfidenceClass
+
+
+class PublishState:
+  def __init__(self):
+    self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
+    self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
+    self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
+
+def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
+  builder.t = t
+  builder.x = x.tolist()
+  builder.y = y.tolist()
+  builder.z = z.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if z_std is not None:
+    builder.zStd = z_std.tolist()
+
+def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
+  builder.t = t
+  builder.x = x.tolist()
+  builder.y = y.tolist()
+  builder.v = v.tolist()
+  builder.a = a.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if v_std is not None:
+    builder.vStd = v_std.tolist()
+  if a_std is not None:
+    builder.aStd = a_std.tolist()
+
+def fill_xyz_poly(builder, degree, x, y, z):
+  xyz = np.stack([x, y, z], axis=1)
+  coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
+  builder.xCoefficients = coeffs[:, 0].tolist()
+  builder.yCoefficients = coeffs[:, 1].tolist()
+  builder.zCoefficients = coeffs[:, 2].tolist()
+
+def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
+  builder.leftY = lane_lines[1].y[0]
+  builder.leftProb = lane_line_probs[1]
+  builder.rightY = lane_lines[2].y[0]
+  builder.rightProb = lane_line_probs[2]
+
+def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
+                   net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
+                   publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
+                   frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
+                   valid: bool) -> None:
+  frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
+  frame_drop_perc = frame_drop * 100
+  extended_msg.valid = valid
+  base_msg.valid = valid
+
+  driving_model_data = base_msg.drivingModelData
+
+  driving_model_data.frameId = vipc_frame_id
+  driving_model_data.frameIdExtra = vipc_frame_id_extra
+  driving_model_data.frameDropPerc = frame_drop_perc
+  driving_model_data.modelExecutionTime = model_execution_time
+
+  driving_model_data.action = action
+
+  modelV2 = extended_msg.modelV2
+  modelV2.frameId = vipc_frame_id
+  modelV2.frameIdExtra = vipc_frame_id_extra
+  modelV2.frameAge = frame_age
+  modelV2.frameDropPerc = frame_drop_perc
+  modelV2.timestampEof = timestamp_eof
+  modelV2.modelExecutionTime = model_execution_time
+
+  # plan
+  fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
+  fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
+  fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
+  fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
+  fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
+
+  # poly path
+  fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
+
+  # action
+  modelV2.action = action
+
+  # times at X_IDXS of edges and lines
+  LINE_T_IDXS: list[float] = plan_x_idxs_helper(ModelConstants, Plan, net_output_data)
+
+  # lane lines
+  modelV2.init('laneLines', 4)
+  for i in range(4):
+    lane_line = modelV2.laneLines[i]
+    fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+  modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
+  modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+
+  fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
+
+  # road edges
+  modelV2.init('roadEdges', 2)
+  for i in range(2):
+    road_edge = modelV2.roadEdges[i]
+    fill_xyzt(road_edge, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
+  modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
+
+  # leads
+  modelV2.init('leadsV3', 3)
+  for i in range(3):
+    lead = modelV2.leadsV3[i]
+    fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
+    lead.prob = net_output_data['lead_prob'][0,i].tolist()
+    lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
+
+  # meta
+  meta = modelV2.meta
+  meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
+  meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
+  meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
+  meta.init('disengagePredictions')
+  disengage_predictions = meta.disengagePredictions
+  disengage_predictions.t = ModelConstants.META_T_IDXS
+  disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
+  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
+  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
+  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
+  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
+  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
+  disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
+  disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
+
+  publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
+  publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
+  publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
+  publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
+  hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
+    (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
+  meta.hardBrakePredicted = hard_brake_predicted.item()
+
+  # confidence
+  if vipc_frame_id % (2*ModelConstants.MODEL_RUN_FREQ) == 0:
+    # any disengage prob
+    brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
+    gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
+    steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
+    any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
+    # independent disengage prob for each 2s slice
+    ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
+    # rolling buf for 2, 4, 6, 8, 10s
+    publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
+    publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
+
+  score = 0.
+  for i in range(ModelConstants.DISENGAGE_WIDTH):
+    score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
+  if score < ModelConstants.RYG_GREEN:
+    modelV2.confidence = ConfidenceClass.green
+  elif score < ModelConstants.RYG_YELLOW:
+    modelV2.confidence = ConfidenceClass.yellow
+  else:
+    modelV2.confidence = ConfidenceClass.red
+
+  # raw prediction if enabled
+  if SEND_RAW_PRED:
+    modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
+
+def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray],
+                  vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
+  msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
+  cameraOdometry = msg.cameraOdometry
+
+  cameraOdometry.frameId = vipc_frame_id
+  cameraOdometry.timestampEof = timestamp_eof
+
+  cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
+  cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
+  cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
+  cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
+  cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
+  cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
+  cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
+  cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()

selfdrive/modeld/get_model_metadata.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python3
+import sys
+import pathlib
+import onnx
+import codecs
+import pickle
+from typing import Any
+
+def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
+  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
+  name = value_info.name
+  return name, shape
+
+def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any:
+  for prop in model.metadata_props:
+    if prop.key == name:
+      return prop.value
+  return None
+
+if __name__ == "__main__":
+  model_path = pathlib.Path(sys.argv[1])
+  model = onnx.load(str(model_path))
+  output_slices = get_metadata_value_by_name(model, 'output_slices')
+  assert output_slices is not None, 'output_slices not found in metadata'
+
+  metadata = {
+    'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
+    'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
+    'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
+    'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
+  }
+
+  metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
+  with open(metadata_path, 'wb') as f:
+    pickle.dump(metadata, f)
+
+  print(f'saved metadata to {metadata_path}')

selfdrive/modeld/modeld.py

@@ -0,0 +1 @@
+LAT_SMOOTH_SECONDS = 0.0

selfdrive/modeld/models/README.md

@@ -0,0 +1,66 @@
+## Neural networks in openpilot
+To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
+
+## Driving Model (vision model + temporal policy model)
+### Vision inputs (Full size: 799906 x float32)
+* **image stream**
+  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
+    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
+      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
+      * Channel 4 represents the half-res U channel
+      * Channel 5 represents the half-res V channel
+* **wide image stream**
+  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
+    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
+      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
+      * Channel 4 represents the half-res U channel
+      * Channel 5 represents the half-res V channel
+### Policy inputs
+* **desire**
+  * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
+* **traffic convention**
+  * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
+* **lateral control params**
+  * speed and steering delay for predicting the desired curvature: 2
+* **previous desired curvatures**
+  * vector of previously predicted desired curvatures: 100 * 1
+* **feature buffer**
+  * a buffer of intermediate features including the current feature to form a 5 seconds temporal context (at 20FPS) : 100 * 512
+
+
+### Driving Model output format (Full size: XXX x float32)
+Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in modeld.
+
+
+## Driver Monitoring Model
+* .onnx model can be run with onnx runtimes
+* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
+
+### input format
+* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
+  * full input size is 1440 * 960 = 1382400
+  * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
+* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
+
+### output format
+* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
+  * for each person in the front seats (2 * 41)
+    * face pose: 12 = 6 + 6
+      * face orientation [pitch, yaw, roll] in camera frame: 3
+      * face position [dx, dy] relative to image center: 2
+      * normalized face size: 1
+      * standard deviations for above outputs: 6
+    * face visible probability: 1
+    * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
+      * eye position and size, and their standard deviations: 8
+      * eye visible probability: 1
+      * eye closed probability: 1
+    * wearing sunglasses probability: 1
+    * face occluded probability: 1
+    * touching wheel probability: 1
+    * paying attention probability: 1
+    * (deprecated) distracted probabilities: 2
+    * using phone probability: 1
+    * distracted probability: 1
+  * common outputs 1
+    * left hand drive probability: 1

selfdrive/modeld/models/prebuilt_check.json

@@ -0,0 +1,9 @@
+{
+  "dmonitoring_model": {
+    "outputs": {
+      "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
+      "dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
+    },
+    "signature": "64740782bfd5b1720978aab24a2b1b37c99542f97db3c8637227c7412bb30bbf"
+  }
+}

selfdrive/modeld/parse_model_outputs.py

@@ -0,0 +1,122 @@
+import numpy as np
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+def safe_exp(x, out=None):
+  # -11 is around 10**14, more causes float16 overflow
+  return np.exp(np.clip(x, -np.inf, 11), out=out)
+
+def sigmoid(x):
+  return 1. / (1. + safe_exp(-x))
+
+def softmax(x, axis=-1):
+  x -= np.max(x, axis=axis, keepdims=True)
+  if x.dtype == np.float32 or x.dtype == np.float64:
+    safe_exp(x, out=x)
+  else:
+    x = safe_exp(x)
+  x /= np.sum(x, axis=axis, keepdims=True)
+  return x
+
+class Parser:
+  def __init__(self, ignore_missing=False):
+    self.ignore_missing = ignore_missing
+
+  def check_missing(self, outs, name):
+    missing = name not in outs
+    if missing and not self.ignore_missing:
+      raise ValueError(f"Missing output {name}")
+    return missing
+
+  def parse_categorical_crossentropy(self, name, outs, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    if out_shape is not None:
+      raw = raw.reshape((raw.shape[0],) + out_shape)
+    outs[name] = softmax(raw, axis=-1)
+
+  def parse_binary_crossentropy(self, name, outs):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    outs[name] = sigmoid(raw)
+
+  def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
+
+    n_values = (raw.shape[2] - out_N)//2
+    pred_mu = raw[:,:,:n_values]
+    pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
+
+    if in_N > 1:
+      weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
+      for i in range(out_N):
+        weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
+
+      if out_N == 1:
+        for fidx in range(weights.shape[0]):
+          idxs = np.argsort(weights[fidx][:,0])[::-1]
+          weights[fidx] = weights[fidx][idxs]
+          pred_mu[fidx] = pred_mu[fidx][idxs]
+          pred_std[fidx] = pred_std[fidx][idxs]
+      full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
+      outs[name + '_weights'] = weights
+      outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
+      outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
+
+      pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      for fidx in range(weights.shape[0]):
+        for hidx in range(out_N):
+          idxs = np.argsort(weights[fidx,:,hidx])[::-1]
+          pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
+          pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
+    else:
+      pred_mu_final = pred_mu
+      pred_std_final = pred_std
+
+    if out_N > 1:
+      final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
+    else:
+      final_shape = tuple([raw.shape[0],] + list(out_shape))
+    outs[name] = pred_mu_final.reshape(final_shape)
+    outs[name + '_stds'] = pred_std_final.reshape(final_shape)
+
+  def is_mhp(self, outs, name, shape):
+    if self.check_missing(outs, name):
+      return False
+    if outs[name].shape[1] == 2 * shape:
+      return False
+    return True
+
+  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
+    self.parse_binary_crossentropy('lane_lines_prob', outs)
+    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
+    self.parse_binary_crossentropy('meta', outs)
+    self.parse_binary_crossentropy('lead_prob', outs)
+    lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH)
+    lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0)
+    lead_out_shape = (ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) if lead_mhp else \
+        (ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)
+    self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape)
+    return outs
+
+  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    plan_mhp = self.is_mhp(outs, 'plan',  ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH)
+    plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
+    self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    return outs
+
+  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    outs = self.parse_vision_outputs(outs)
+    outs = self.parse_policy_outputs(outs)
+    return outs

selfdrive/modeld/prebuilt_models.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+import functools
+import hashlib
+import json
+import sys
+from pathlib import Path
+
+MODELD_DIR = Path(__file__).resolve().parent
+MODELS_DIR = MODELD_DIR / 'models'
+BASEDIR = MODELD_DIR.parents[1]
+TINYGRAD_DIR = BASEDIR / 'tinygrad_repo'
+METADATA_SCRIPT = MODELD_DIR / 'get_model_metadata.py'
+
+MODEL_NAMES = ['dmonitoring_model']
+
+
+def _hash_file(h, path: Path) -> None:
+  with open(path, 'rb') as f:
+    for chunk in iter(lambda: f.read(1024 * 1024), b''):
+      h.update(chunk)
+
+
+def _file_sha256(path: Path) -> str:
+  h = hashlib.sha256()
+  _hash_file(h, path)
+  return h.hexdigest()
+
+
+@functools.lru_cache(maxsize=1)
+def _tinygrad_digest() -> str:
+  h = hashlib.sha256()
+  # mirror the SConscript glob: no hidden files/dirs (also excludes .git, which differs per clone), no pycache.
+  # 'weights' holds untracked/gitignored model weights (e.g. iqvd yolov8n .safetensors) present on dev machines
+  # but absent from a clean CI/device clone — hashing them makes the signature unreproducible (false STALE).
+  def _included(p: Path) -> bool:
+    rel = p.relative_to(TINYGRAD_DIR).parts
+    return p.is_file() and '__pycache__' not in rel and 'weights' not in rel and not any(part.startswith('.') for part in rel)
+  files = sorted(p for p in TINYGRAD_DIR.rglob('*') if _included(p))
+  for p in files:
+    h.update(str(p.relative_to(BASEDIR)).encode())
+    _hash_file(h, p)
+  return h.hexdigest()
+
+
+CHECK_PATH = MODELS_DIR / 'prebuilt_check.json'
+
+
+def _load_checks() -> dict:
+  try:
+    data = json.loads(CHECK_PATH.read_text())
+  except (json.JSONDecodeError, OSError):
+    return {}
+  return data if isinstance(data, dict) else {}
+
+
+def _output_names(model_name: str) -> list[str]:
+  return [f'{model_name}_tinygrad.pkl', f'{model_name}_metadata.pkl']
+
+
+def compute_signature(model_name: str, flags: str) -> str:
+  h = hashlib.sha256()
+  h.update(flags.encode())
+  h.update(_tinygrad_digest().encode())
+  _hash_file(h, METADATA_SCRIPT)
+  _hash_file(h, MODELS_DIR / f'{model_name}.onnx')
+  return h.hexdigest()
+
+
+def outputs_match(model_name: str) -> bool:
+  """The committed artifacts on disk are exactly the ones the check file pins."""
+  data = _load_checks().get(model_name, {})
+  outputs = data.get('outputs', {})
+  if set(outputs) != set(_output_names(model_name)):
+    return False
+  for fn, expected in outputs.items():
+    p = MODELS_DIR / fn
+    if not p.is_file() or _file_sha256(p) != expected:
+      return False
+  return True
+
+
+def packaged_prebuilt_matches(model_name: str) -> bool:
+  return not (MODELS_DIR / f'{model_name}.onnx').is_file() and outputs_match(model_name)
+
+
+def verify_prebuilt(model_name: str, flags: str) -> bool:
+  if not (MODELS_DIR / f'{model_name}.onnx').is_file():
+    return False
+  data = _load_checks().get(model_name, {})
+  if data.get('signature') != compute_signature(model_name, flags):
+    return False
+  return outputs_match(model_name)
+
+
+def write_check(model_name: str, flags: str) -> None:
+  outputs = {}
+  for fn in _output_names(model_name):
+    p = MODELS_DIR / fn
+    if not p.is_file():
+      raise FileNotFoundError(f'missing build output: {p}')
+    outputs[fn] = _file_sha256(p)
+  checks = _load_checks()
+  checks[model_name] = {'signature': compute_signature(model_name, flags), 'outputs': outputs}
+  CHECK_PATH.write_text(json.dumps(checks, indent=2, sort_keys=True) + '\n')
+
+
+def _larch64_flags() -> str:
+  return "DEV=QCOM IMAGE=2 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+
+
+if __name__ == '__main__':
+  mode = sys.argv[1] if len(sys.argv) > 1 else 'verify'
+  flags = _larch64_flags()
+  for name in MODEL_NAMES:
+    if mode == 'write':
+      write_check(name, flags)
+      print(f'{name}: check written')
+    else:
+      print(f'{name}: {"OK" if verify_prebuilt(name, flags) else "STALE"}')

selfdrive/modeld/runners/tinygrad_helpers.py

@@ -0,0 +1,8 @@
+
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import to_mv
+
+def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
+  cl_buf_desc_ptr = to_mv(opencl_address, 8).cast('Q')[0]
+  rawbuf_ptr = to_mv(cl_buf_desc_ptr, 0x100).cast('Q')[20] # offset 0xA0 is a raw gpu pointer.
+  return Tensor.from_blob(rawbuf_ptr, shape, dtype=dtype, device='QCOM')

selfdrive/modeld/test_prebuilt_models.py

@@ -0,0 +1,51 @@
+import hashlib
+import json
+
+from openpilot.selfdrive.modeld import prebuilt_models
+
+
+def write_outputs(models_dir, check_path):
+  outputs = {}
+  for name, contents in {
+    'dmonitoring_model_tinygrad.pkl': b'tinygrad',
+    'dmonitoring_model_metadata.pkl': b'metadata',
+  }.items():
+    (models_dir / name).write_bytes(contents)
+    outputs[name] = hashlib.sha256(contents).hexdigest()
+  check_path.write_text(json.dumps({'dmonitoring_model': {'outputs': outputs}}))
+
+
+def test_packaged_prebuilt_without_onnx(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+  assert not prebuilt_models.verify_prebuilt('dmonitoring_model', 'flags')
+
+
+def test_packaged_prebuilt_rejects_corrupt_output(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model_tinygrad.pkl').write_bytes(b'corrupt')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')
+
+
+def test_source_checkout_is_not_packaged_prebuilt(tmp_path, monkeypatch):
+  models_dir = tmp_path / 'models'
+  models_dir.mkdir()
+  check_path = models_dir / 'prebuilt_check.json'
+  write_outputs(models_dir, check_path)
+  (models_dir / 'dmonitoring_model.onnx').write_bytes(b'onnx')
+  monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir)
+  monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path)
+
+  assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model')

selfdrive/modeld/transforms/loadyuv.cl

@@ -0,0 +1,47 @@
+#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
+
+__kernel void loadys(__global uchar8 const * const Y,
+                     __global uchar * out,
+                     int out_offset)
+{
+    const int gid = get_global_id(0);
+    const int ois = gid * 8;
+    const int oy = ois / TRANSFORMED_WIDTH;
+    const int ox = ois % TRANSFORMED_WIDTH;
+
+    const uchar8 ys = Y[gid];
+
+    // 02
+    // 13
+
+    __global uchar* outy0;
+    __global uchar* outy1;
+    if ((oy & 1) == 0) {
+      outy0 = out + out_offset; //y0
+      outy1 = out + out_offset + UV_SIZE*2; //y2
+    } else {
+      outy0 = out + out_offset + UV_SIZE; //y1
+      outy1 = out + out_offset + UV_SIZE*3; //y3
+    }
+
+    vstore4(ys.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
+    vstore4(ys.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
+}
+
+__kernel void loaduv(__global uchar8 const * const in,
+                     __global uchar8 * out,
+                     int out_offset)
+{
+  const int gid = get_global_id(0);
+  const uchar8 inv = in[gid];
+  out[gid + out_offset / 8] = inv;
+}
+
+__kernel void copy(__global uchar8 * in,
+                   __global uchar8 * out,
+                   int in_offset,
+                   int out_offset)
+{
+  const int gid = get_global_id(0);
+  out[gid + out_offset / 8] = in[gid + in_offset / 8];
+}

selfdrive/modeld/transforms/transform.cl

@@ -0,0 +1,54 @@
+#define INTER_BITS 5
+#define INTER_TAB_SIZE (1 << INTER_BITS)
+#define INTER_SCALE 1.f / INTER_TAB_SIZE
+
+#define INTER_REMAP_COEF_BITS 15
+#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
+
+__kernel void warpPerspective(__global const uchar * src,
+                              int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols,
+                              __global uchar * dst,
+                              int dst_row_stride, int dst_offset, int dst_rows, int dst_cols,
+                              __constant float * M)
+{
+    int dx = get_global_id(0);
+    int dy = get_global_id(1);
+
+    if (dx < dst_cols && dy < dst_rows)
+    {
+        float X0 = M[0] * dx + M[1] * dy + M[2];
+        float Y0 = M[3] * dx + M[4] * dy + M[5];
+        float W = M[6] * dx + M[7] * dy + M[8];
+        W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
+        int X = rint(X0 * W), Y = rint(Y0 * W);
+
+        int sx = convert_short_sat(X >> INTER_BITS);
+        int sy = convert_short_sat(Y >> INTER_BITS);
+
+        short sx_clamp = clamp(sx, 0, src_cols - 1);
+        short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1);
+        short sy_clamp = clamp(sy, 0, src_rows - 1);
+        short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1);
+        int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
+        int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
+        int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
+        int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
+
+        short ay = (short)(Y & (INTER_TAB_SIZE - 1));
+        short ax = (short)(X & (INTER_TAB_SIZE - 1));
+        float taby = 1.f/INTER_TAB_SIZE*ay;
+        float tabx = 1.f/INTER_TAB_SIZE*ax;
+
+        int dst_index = mad24(dy, dst_row_stride, dst_offset + dx);
+
+        int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
+        int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
+
+        int val = v0 * itab0 +  v1 * itab1 + v2 * itab2 + v3 * itab3;
+
+        uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
+        dst[dst_index] = pix;
+    }
+}