dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

common/transformations/camera.py

@@ -1,6 +1,6 @@
 import numpy as np
 
-import common.transformations.orientation as orient
+import openpilot.common.transformations.orientation as orient
 
 ## -- hardcoded hardware params --
 eon_f_focal_length = 910.0

common/transformations/coordinates.hpp

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <eigen3/Eigen/Dense>
+
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 #define RAD2DEG(x) ((x) * 180.0 / M_PI)
 

common/transformations/coordinates.py

@@ -1,8 +1,7 @@
-# pylint: skip-file
-from common.transformations.orientation import numpy_wrap
-from common.transformations.transformations import (ecef2geodetic_single,
+from openpilot.common.transformations.orientation import numpy_wrap
+from openpilot.common.transformations.transformations import (ecef2geodetic_single,
                                                     geodetic2ecef_single)
-from common.transformations.transformations import LocalCoord as LocalCoord_single
+from openpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
 
 
 class LocalCoord(LocalCoord_single):

common/transformations/model.py

@@ -1,7 +1,9 @@
 import numpy as np
 
-from common.transformations.camera import (FULL_FRAME_SIZE,
-                                           get_view_frame_from_calib_frame)
+from openpilot.common.transformations.orientation import rot_from_euler
+from openpilot.common.transformations.camera import (
+  FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame,
+  eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics)
 
 # segnet
 SEGNET_SIZE = (512, 384)
@@ -57,61 +59,20 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
 
 medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
 
-### This function mimics the update_calibration logic in modeld.cc
-### Manually verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
+# This function is verified to give similar results to xx.uncommon.utils.transform_img
+def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray:
+  if tici and wide_camera:
+    cam_intrinsics = tici_ecam_intrinsics
   elif tici:
-    intrinsics = tici_fcam_intrinsics
+    cam_intrinsics = tici_fcam_intrinsics
   else:
-    intrinsics = eon_fcam_intrinsics
+    cam_intrinsics = eon_fcam_intrinsics
 
-  if big_model:
-    sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(sbigmodel_from_calib)
-  else:
-    medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(medmodel_from_calib)
-  device_from_calib = rot_from_euler(rpy_calib)
-  camera_from_calib = intrinsics.dot(view_frame_from_device_frame.dot(device_from_calib))
-  warp_matrix = camera_from_calib.dot(calib_from_model)
-  return warp_matrix
-
-
-### This is old, just for debugging
-def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-
-  def get_view_frame_from_road_frame(roll, pitch, yaw, height):
-    device_from_road = rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
-    view_from_road = view_frame_from_device_frame.dot(device_from_road)
-    return np.hstack((view_from_road, [[0], [height], [0]]))
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
-  elif tici:
-    intrinsics = tici_fcam_intrinsics
-  else:
-    intrinsics = eon_fcam_intrinsics
-
-  model_height = 1.22
-  if big_model:
-    model_from_road = np.dot(sbigmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  else:
-    model_from_road = np.dot(medmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  ground_from_model = np.linalg.inv(model_from_road[:, (0, 1, 3)])
-
-  E = get_view_frame_from_road_frame(*rpy_calib, 1.22)
-  camera_frame_from_road_frame = intrinsics.dot(E)
-  camera_frame_from_ground = camera_frame_from_road_frame[:,(0,1,3)]
-  warp_matrix = camera_frame_from_ground .dot(ground_from_model)
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix

common/transformations/orientation.hpp

@@ -1,6 +1,6 @@
 #pragma once
 #include <eigen3/Eigen/Dense>
-#include "coordinates.hpp"
+#include "common/transformations/coordinates.hpp"
 
 
 Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat);

common/transformations/orientation.py

@@ -1,8 +1,7 @@
-# pylint: skip-file
 import numpy as np
 from typing import Callable
 
-from common.transformations.transformations import (ecef_euler_from_ned_single,
+from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
                                                     euler2quat_single,
                                                     euler2rot_single,
                                                     ned_euler_from_ecef_single,

common/transformations/transformations.cpp

@@ -7,15 +7,20 @@
             "common/transformations/coordinates.cc",
             "common/transformations/coordinates.hpp",
             "common/transformations/orientation.cc",
-            "common/transformations/orientation.hpp"
+            "common/transformations/orientation.hpp",
+            "openpilot/common/transformations/coordinates.cc",
+            "openpilot/common/transformations/coordinates.hpp",
+            "openpilot/common/transformations/orientation.cc",
+            "openpilot/common/transformations/orientation.hpp"
         ],
         "include_dirs": [
-            "common/transformations"
+            "common/transformations",
+            "openpilot/common/transformations"
         ],
         "language": "c++",
         "name": "common.transformations.transformations",
         "sources": [
-            "/data/openpilot/common/transformations/transformations.pyx"
+            "/data/dp-private/common/transformations/transformations.pyx"
         ]
     },
     "module_name": "common.transformations.transformations"
@@ -2528,6 +2533,8 @@ static CYTHON_INLINE char *__pyx_f_5numpy_7ndarray_4data_data(PyArrayObject *__p
 
 /* Module declarations from "libcpp" */
 
+/* Module declarations from "openpilot.common.transformations.transformations" */
+
 /* Module declarations from "cython" */
 
 /* Module declarations from "libc.string" */
@@ -2720,6 +2727,8 @@ typedef struct {
   #endif
   #if CYTHON_USE_MODULE_STATE
   #endif
+  #if CYTHON_USE_MODULE_STATE
+  #endif
   PyTypeObject *__pyx_ptype_7cpython_4type_type;
   #if CYTHON_USE_MODULE_STATE
   #endif
@@ -3269,6 +3278,8 @@ static int __pyx_m_traverse(PyObject *m, visitproc visit, void *arg) {
 #endif
 #if CYTHON_USE_MODULE_STATE
 #endif
+#if CYTHON_USE_MODULE_STATE
+#endif
 #define __pyx_ptype_7cpython_4type_type __pyx_mstate_global->__pyx_ptype_7cpython_4type_type
 #if CYTHON_USE_MODULE_STATE
 #endif
@@ -10123,7 +10134,7 @@ if (!__Pyx_RefNanny) {
   /* "common/transformations/transformations.pyx":1
  * # distutils: language = c++             # <<<<<<<<<<<<<<
  * # cython: language_level = 3
- * from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+ * from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
  */
   __pyx_t_2 = __Pyx_PyDict_NewPresized(0); if (unlikely(!__pyx_t_2)) __PYX_ERR(0, 1, __pyx_L1_error)
   __Pyx_GOTREF(__pyx_t_2);

common/transformations/transformations.pyx

@@ -1,20 +1,20 @@
 # distutils: language = c++
 # cython: language_level = 3
-from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
-from common.transformations.transformations cimport ECEF, NED, Geodetic
+from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+from openpilot.common.transformations.transformations cimport ECEF, NED, Geodetic
 
-from common.transformations.transformations cimport euler2quat as euler2quat_c
-from common.transformations.transformations cimport quat2euler as quat2euler_c
-from common.transformations.transformations cimport quat2rot as quat2rot_c
-from common.transformations.transformations cimport rot2quat as rot2quat_c
-from common.transformations.transformations cimport euler2rot as euler2rot_c
-from common.transformations.transformations cimport rot2euler as rot2euler_c
-from common.transformations.transformations cimport rot_matrix as rot_matrix_c
-from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
-from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
-from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
-from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
-from common.transformations.transformations cimport LocalCoord_c
+from openpilot.common.transformations.transformations cimport euler2quat as euler2quat_c
+from openpilot.common.transformations.transformations cimport quat2euler as quat2euler_c
+from openpilot.common.transformations.transformations cimport quat2rot as quat2rot_c
+from openpilot.common.transformations.transformations cimport rot2quat as rot2quat_c
+from openpilot.common.transformations.transformations cimport euler2rot as euler2rot_c
+from openpilot.common.transformations.transformations cimport rot2euler as rot2euler_c
+from openpilot.common.transformations.transformations cimport rot_matrix as rot_matrix_c
+from openpilot.common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
+from openpilot.common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
+from openpilot.common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
+from openpilot.common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
+from openpilot.common.transformations.transformations cimport LocalCoord_c
 
 
 import cython