move intrinsic_matrix into modeldata.h, removed duplicate definitions. (#20051)

* move intrinsic_matrix into modeldata.h

* def __cplusplus
old-commit-hash: ae72d34a42

selfdrive/common/modeldata.h

@@ -16,3 +16,23 @@ const double X_IDXS[TRAJECTORY_SIZE] = { 0.    ,   0.1875,   0.75  ,   1.6875,
         60.75  ,  67.6875,  75.    ,  82.6875,  90.75  ,  99.1875,
        108.    , 117.1875, 126.75  , 136.6875, 147.    , 157.6875,
        168.75  , 180.1875, 192.};
+
+#ifdef __cplusplus
+
+#include "common/mat.h"
+#ifdef QCOM2
+const mat3 fcam_intrinsic_matrix = (mat3){{
+  2648.0, 0.0, 1928.0/2,
+  0.0, 2648.0, 1208.0/2,
+  0.0,   0.0,   1.0
+}};
+#else
+const mat3 fcam_intrinsic_matrix = (mat3){{
+  910., 0., 1164.0/2,
+  0., 910., 874.0/2,
+  0.,   0.,   1.
+}};
+#endif
+
+#endif
+

selfdrive/modeld/modeld.cc

@@ -37,17 +37,12 @@ void* live_thread(void *arg) {
     -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
 
   Eigen::Matrix<float, 3, 3> fcam_intrinsics;
+  for (auto &v : fcam_intrinsic_matrix.v) {
+    fcam_intrinsics << v;
+  }
 #ifndef QCOM2
-  fcam_intrinsics <<
-    910.0, 0.0, 582.0,
-    0.0, 910.0, 437.0,
-    0.0,   0.0,   1.0;
   float db_s = 0.5; // debayering does a 2x downscale
 #else
-  fcam_intrinsics <<
-    2648.0, 0.0, 1928.0/2,
-    0.0, 2648.0, 1208.0/2,
-    0.0,   0.0,   1.0;
   float db_s = 1.0;
 #endif
 

selfdrive/ui/paint.cc

@@ -20,19 +20,9 @@
 #ifdef QCOM2
 const float y_offset = 150.0;
 const float zoom = 1.1;
-const mat3 intrinsic_matrix = (mat3){{
-  2648.0, 0.0, 1928.0/2,
-  0.0, 2648.0, 1208.0/2,
-  0.0,   0.0,   1.0
-}};
 #else
 const float y_offset = 0.0;
 const float zoom = 2.35;
-const mat3 intrinsic_matrix = (mat3){{
-  910., 0., 1164.0/2,
-  0., 910., 874.0/2,
-  0.,   0.,   1.
-}};
 #endif
 
 // Projects a point in car to space to the corresponding point in full frame
@@ -41,7 +31,7 @@ bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float i
   const float margin = 500.0f;
   const vec3 pt = (vec3){{in_x, in_y, in_z}};
   const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt);
-  const vec3 KEp = matvecmul3(intrinsic_matrix, Ep);
+  const vec3 KEp = matvecmul3(fcam_intrinsic_matrix, Ep);
 
   // Project.
   float x = KEp.v[0] / KEp.v[2];
@@ -618,8 +608,8 @@ void ui_nvg_init(UIState *s) {
   }
 
   s->video_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s};
-  float zx = zoom * 2 * intrinsic_matrix.v[2] / s->video_rect.w;
+  float zx = zoom * 2 * fcam_intrinsic_matrix.v[2] / s->video_rect.w;
-  float zy = zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h;
+  float zy = zoom * 2 * fcam_intrinsic_matrix.v[5] / s->video_rect.h;
 
   const mat4 frame_transform = {{
     zx, 0.0, 0.0, 0.0,
@@ -639,7 +629,7 @@ void ui_nvg_init(UIState *s) {
   nvgScale(s->vg, zoom, zoom);
 
   // 3) Put (0, 0) in top left corner of video
-  nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
+  nvgTranslate(s->vg, -fcam_intrinsic_matrix.v[2], -fcam_intrinsic_matrix.v[5]);
 
   nvgCurrentTransform(s->vg, s->car_space_transform);
   nvgResetTransform(s->vg);