From ae72d34a42f23e9b295641e1b0f50515434d6d1f Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Wed, 3 Mar 2021 18:47:48 +0800
Subject: [PATCH] move intrinsic_matrix into modeldata.h, removed duplicate
 definitions. (#20051)

* move intrinsic_matrix into modeldata.h

* def __cplusplus
---
 selfdrive/common/modeldata.h | 20 ++++++++++++++++++++
 selfdrive/modeld/modeld.cc   | 11 +++--------
 selfdrive/ui/paint.cc        | 18 ++++--------------
 3 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 8d41b2b388..826f2089c4 100644
--- a/selfdrive/common/modeldata.h
+++ b/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
+
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index c43e9588b3..e69d96c2cf 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/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
 
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index a23fd444ef..28f88d20bf 100644
--- a/selfdrive/ui/paint.cc
+++ b/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 zy = zoom * 2 * intrinsic_matrix.v[5] / s->video_rect.h;
+  float zx = zoom * 2 * fcam_intrinsic_matrix.v[2] / s->video_rect.w;
+  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);