From 9fef49e97523c3a879e22db6c0611afd0cbf30b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= <harald.the.engineer@gmail.com>
Date: Tue, 22 Nov 2022 20:20:58 -0800
Subject: [PATCH] Navd map renderer: update to larger image (#26580)

* rerender 512

* rerender 512

* Removed unnecessary code

* rgb -> grayscale

* Add constants

Co-authored-by: mitchellgoffpc <mitchellgoffpc@gmail.com>
old-commit-hash: c1e7bed061424387aaf5a91eaa43e96752995cd0
---
 selfdrive/navd/map_renderer.cc | 41 +++++++++++++---------------------
 selfdrive/navd/map_renderer.py |  3 ++-
 2 files changed, 17 insertions(+), 27 deletions(-)

diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc
index daf89b2636..247b69c4c5 100644
--- a/selfdrive/navd/map_renderer.cc
+++ b/selfdrive/navd/map_renderer.cc
@@ -11,33 +11,18 @@
 #include "selfdrive/ui/qt/maps/map_helpers.h"
 
 const float DEFAULT_ZOOM = 13.5; // Don't go below 13 or features will start to disappear
-const int RENDER_HEIGHT = 512, RENDER_WIDTH = 512;
-const int HEIGHT = 256, WIDTH = 256;
+const int HEIGHT = 512, WIDTH = 512;
 const int NUM_VIPC_BUFFERS = 4;
 
 const int EARTH_CIRCUMFERENCE_METERS = 40075000;
 const int PIXELS_PER_TILE = 256;
 
-float get_meters_per_pixel(float lat, float zoom) {
-  float num_tiles = pow(2, zoom+1);
-  float meters_per_tile = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / num_tiles;
-  return meters_per_tile / PIXELS_PER_TILE;
-}
-
 float get_zoom_level_for_scale(float lat, float meters_per_pixel) {
   float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE;
   float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile;
   return log2(num_tiles) - 1;
 }
 
-void downsample(uint8_t *src, uint8_t *dst) {
-  for (int r = 0; r < HEIGHT; r++) {
-    for (int c = 0; c < WIDTH; c++) {
-      dst[r*WIDTH + c] = src[(r*2*RENDER_WIDTH + c*2) * 3];
-    }
-  }
-}
-
 
 MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) {
   QSurfaceFormat fmt;
@@ -59,7 +44,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
   gl_functions->initializeOpenGLFunctions();
 
   QOpenGLFramebufferObjectFormat fbo_format;
-  fbo.reset(new QOpenGLFramebufferObject(RENDER_WIDTH, RENDER_HEIGHT, fbo_format));
+  fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format));
 
   std::string style = util::read_file(STYLE_PATH);
   m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1));
@@ -69,7 +54,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
 
   m_map->resize(fbo->size());
   m_map->setFramebufferObject(fbo->handle(), fbo->size());
-  gl_functions->glViewport(0, 0, RENDER_WIDTH, RENDER_HEIGHT);
+  gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
 
   if (online) {
     vipc_server.reset(new VisionIpcServer("navd"));
@@ -114,9 +99,9 @@ void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {
     return;
   }
 
-  // Choose a zoom level that matches the scale of zoom level 13 at latitude 80deg
-  float scale_lat80 = get_meters_per_pixel(80, 13);
-  float zoom = get_zoom_level_for_scale(position.first, scale_lat80);
+  // Choose a scale that ensures above 13 zoom level up to and above 75deg of lat
+  float meters_per_pixel = 2;
+  float zoom = get_zoom_level_for_scale(position.first, meters_per_pixel);
 
   m_map->setCoordinate(position);
   m_map->setBearing(bearing);
@@ -150,13 +135,15 @@ void MapRenderer::sendVipc() {
     .timestamp_eof = ts,
   };
 
-  assert(cap.sizeInBytes() >= buf->len*4);
+  assert(cap.sizeInBytes() >= buf->len);
   uint8_t* dst = (uint8_t*)buf->addr;
   uint8_t* src = cap.bits();
 
-  // 2x downsample + rgb to grayscale
+  // RGB to greyscale
   memset(dst, 128, buf->len);
-  downsample(src, dst);
+  for (int i = 0; i < WIDTH * HEIGHT; i++) {
+    dst[i] = src[i * 3];
+  }
 
   vipc_server->send(buf, &extra);
 
@@ -187,8 +174,10 @@ uint8_t* MapRenderer::getImage() {
   uint8_t* src = cap.bits();
   uint8_t* dst = new uint8_t[WIDTH * HEIGHT];
 
-  // 2x downsample + rgb to grayscale
-  downsample(src, dst);
+  // RGB to greyscale
+  for (int i = 0; i < WIDTH * HEIGHT; i++) {
+    dst[i] = src[i * 3];
+  }
 
   return dst;
 }
diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py
index 9000622928..868307bb63 100755
--- a/selfdrive/navd/map_renderer.py
+++ b/selfdrive/navd/map_renderer.py
@@ -9,7 +9,8 @@ from cffi import FFI
 from common.ffi_wrapper import suffix
 from common.basedir import BASEDIR
 
-HEIGHT = WIDTH = 256
+HEIGHT = WIDTH = SIZE = 512
+METERS_PER_PIXEL = 2
 
 
 def get_ffi():