set_exposure_target test (#20318)

* build

* remove junk

* clean up

* clean up rebase

* new patterns

* add gts

* add to jenkis

* this more useful

* typo

* test only

* Update Jenkinsfile

* test flag

* remove from jenkins

* these should all just be common:wq

* oops

* unigt

* add to unit tests?

* build all is fine

Co-authored-by: Comma Device <device@comma.ai>

.github/workflows/selfdrive_tests.yaml

@@ -195,7 +195,7 @@ jobs:
       run: eval "$BUILD"
     - name: Run unit tests
       run: |
-        ${{ env.RUN }} "scons -j$(nproc) && \
+        ${{ env.RUN }} "scons -j$(nproc) --test && \
                         coverage run selfdrive/test/test_fingerprints.py && \
                         $UNIT_TEST common && \
                         $UNIT_TEST opendbc/can && \
@@ -207,7 +207,8 @@ jobs:
                         $UNIT_TEST selfdrive/locationd && \
                         $UNIT_TEST selfdrive/athena && \
                         $UNIT_TEST selfdrive/thermald && \
-                        $UNIT_TEST tools/lib/tests"
+                        $UNIT_TEST tools/lib/tests && \
+                        ./selfdrive/camerad/test/ae_gray_test"
     - name: Upload coverage to Codecov
       run: bash <(curl -s https://codecov.io/bash) -v -F unit_tests
 

.gitignore

@@ -48,6 +48,7 @@ selfdrive/loggerd/bootlog
 selfdrive/sensord/_gpsd
 selfdrive/sensord/_sensord
 selfdrive/camerad/camerad
+selfdrive/camerad/test/ae_gray_test
 selfdrive/modeld/_modeld
 selfdrive/modeld/_dmonitoringmodeld
 /src/

selfdrive/camerad/SConscript

@@ -35,3 +35,10 @@ env.Program('camerad', [
     'imgproc/utils.cc',
     cameras,
   ], LIBS=libs)
+
+if GetOption("test"):
+  env.Program('test/ae_gray_test', [
+      'test/ae_gray_test.cc',
+      'cameras/camera_common.cc',
+      'transforms/rgb_to_yuv.cc',
+    ], LIBS=libs)

selfdrive/camerad/cameras/camera_common.cc

@@ -280,19 +280,16 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
   free(thumbnail_buffer);
 }
 
-void set_exposure_target(CameraState *c, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
+float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted) {
-  const CameraBuf *b = &c->buf;
   const uint8_t *pix_ptr = b->cur_yuv_buf->y;
   uint32_t lum_binning[256] = {0};
   unsigned int lum_total = 0;
   for (int y = y_start; y < y_end; y += y_skip) {
     for (int x = x_start; x < x_end; x += x_skip) {
       uint8_t lum = pix_ptr[(y * b->rgb_width) + x];
-#ifdef QCOM2
+      if (hist_ceil && lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
-      if (lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
         continue;
       }
-#endif
       lum_binning[lum]++;
       lum_total += 1;
     }
@@ -303,20 +300,21 @@ void set_exposure_target(CameraState *c, int x_start, int x_end, int x_skip, int
   int lum_med_alt = 0;
   for (lum_med=255; lum_med>=0; lum_med--) {
     lum_cur += lum_binning[lum_med];
-#ifdef QCOM2
+    if (hl_weighted) {
-    int lum_med_tmp = 0;
+      int lum_med_tmp = 0;
-    int hb = HLC_THRESH + (10 - c->analog_gain);
+      int hb = HLC_THRESH + (10 - analog_gain);
-    if (lum_cur > 0 && lum_med > hb) {
+      if (lum_cur > 0 && lum_med > hb) {
-      lum_med_tmp = (lum_med - hb) + 100;
+        lum_med_tmp = (lum_med - hb) + 100;
+      }
+      lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
     }
-    lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
-#endif
     if (lum_cur >= lum_total / 2) {
       break;
     }
   }
-  lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*(lum_med_alt - lum_med)/lum_total:lum_med;
+  lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*abs(lum_med_alt - lum_med)/lum_total:lum_med;
-  camera_autoexposure(c, lum_med / 256.0);
+
+  return lum_med / 256.0;
 }
 
 extern ExitHandler do_exit;
@@ -406,7 +404,11 @@ void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c,
 #endif
     }
 
-    set_exposure_target(c, x_min, x_max, 2, y_min, y_max, skip);
+#ifdef QCOM2
+    camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, (int)c->analog_gain, true, true));
+#else
+    camera_autoexposure(c, set_exposure_target(b, x_min, x_max, 2, y_min, y_max, skip, -1, false, false));
+#endif
   }
 
   MessageBuilder msg;

selfdrive/camerad/cameras/camera_common.h

@@ -35,6 +35,10 @@
 #define LOG_CAMERA_ID_QCAMERA 3
 #define LOG_CAMERA_ID_MAX 4
 
+#define HLC_THRESH 222
+#define HLC_A 80
+#define HISTO_CEIL_K 5
+
 const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
 const bool env_send_road = getenv("SEND_ROAD") != NULL;
 const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
@@ -96,7 +100,7 @@ private:
 
   FrameMetadata yuv_metas[YUV_COUNT];
   VisionStreamType rgb_type, yuv_type;
-  
+
   int cur_buf_idx;
 
   SafeQueue<int> safe_queue;
@@ -112,7 +116,7 @@ public:
   std::unique_ptr<VisionBuf[]> camera_bufs;
   std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
   int rgb_width, rgb_height, rgb_stride;
-  
+
   mat3 yuv_transform;
 
   CameraBuf() = default;
@@ -127,7 +131,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
 
 void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data);
 kj::Array<uint8_t> get_frame_image(const CameraBuf *b);
-void set_exposure_target(CameraState *c, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
+float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted);
 std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
 void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);
 

selfdrive/camerad/cameras/camera_qcom.cc

@@ -1156,7 +1156,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
   if (cnt % 3 == 0) {
     const int x = 290, y = 322, width = 560, height = 314;
     const int skip = 1;
-    set_exposure_target(c, x, x + width, skip, y, y + height, skip);
+    camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip, -1, false, false));
   }
 }
 

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -1108,7 +1108,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
   if (cnt % 3 == 0) {
     const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
     const int skip = 2;
-    set_exposure_target(c, x, x + w, skip, y, y + h, skip);
+    camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip, (int)c->analog_gain, true, true));
   }
 }
 

selfdrive/camerad/cameras/camera_qcom2.h

@@ -13,10 +13,6 @@
 #define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss
 #define EXPOSURE_TIME_MAX 1757 // with HDR, slowest ss
 
-#define HLC_THRESH 222
-#define HLC_A 80
-#define HISTO_CEIL_K 5
-
 #define EF_LOWPASS_K 0.35
 
 #define DEBAYER_LOCAL_WORKSIZE 16

selfdrive/camerad/test/ae_gray_test.cc

@@ -0,0 +1,70 @@
+// unittest for set_exposure_target
+
+#include <assert.h>
+#include <cstring>
+#include <cmath>
+
+#include "selfdrive/camerad/cameras/camera_common.h"
+#include "selfdrive/camerad/test/ae_gray_test.h"
+
+void camera_autoexposure(CameraState *s, float grey_frac) {}
+
+int main() {
+  // set up fake camerabuf
+  CameraBuf cb = {};
+  VisionBuf vb = {};
+  uint8_t * fb_y = new uint8_t[W*H];
+  vb.y = fb_y;
+  cb.cur_yuv_buf = &vb;
+  cb.rgb_width = W;
+  cb.rgb_height = H;
+
+  printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height);
+
+  // mix of 5 tones
+  uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max
+
+  bool passed = true;
+  float rtol = 0.05;
+  // generate pattern and calculate EV
+  int cnt = 0;
+  for (int is_qcom2=0; is_qcom2<2; is_qcom2++) {
+    for (int g=0; g<GAIN_SPLITS; g++) {
+      for (int i_0=0; i_0<TONE_SPLITS; i_0++) {
+        for (int i_1=0; i_1<TONE_SPLITS; i_1++) {
+          for (int i_2=0; i_2<TONE_SPLITS; i_2++) {
+            for (int i_3=0; i_3<TONE_SPLITS; i_3++) {
+              int h_0 = i_0 * H / TONE_SPLITS;
+              int h_1 = i_1 * (H - h_0) / TONE_SPLITS;
+              int h_2 = i_2 * (H - h_0 - h_1) / TONE_SPLITS;
+              int h_3 = i_3 * (H - h_0 - h_1 - h_2) / TONE_SPLITS;
+              int h_4 = H - h_0 - h_1 - h_2 - h_3;
+              memset(&fb_y[0], l[0], h_0*W);
+              memset(&fb_y[h_0*W], l[1], h_1*W);
+              memset(&fb_y[h_0*W+h_1*W], l[2], h_2*W);
+              memset(&fb_y[h_0*W+h_1*W+h_2*W], l[3], h_3*W);
+              memset(&fb_y[h_0*W+h_1*W+h_2*W+h_3*W], l[4], h_4*W);
+              float ev = set_exposure_target((const CameraBuf*) &cb, 0, W-1, 1, 0, H-1, 1, g*10, (bool)is_qcom2, (bool)is_qcom2);
+              // printf("%d/%d/%d/%d/%d ev is %f\n", h_0, h_1, h_2, h_3, h_4, ev);
+              // printf("%f\n", ev);
+
+              // compare to gt
+              float evgt = gts[cnt];
+              if (fabs(ev - evgt) > rtol*evgt) {
+                passed = false;
+              }
+
+              // report
+              printf("%d/%d/%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", is_qcom2, g*10, h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f));
+              cnt++;
+            }
+          }
+        }
+      }
+    }
+  }
+  assert(passed);
+
+  delete[] fb_y;
+  return 0;
+}

selfdrive/camerad/test/ae_gray_test.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#define W 240
+#define H 160
+
+#define TONE_SPLITS 3
+#define GAIN_SPLITS 2
+
+float gts[2*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*TONE_SPLITS*GAIN_SPLITS] = {
+  0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.917969,
+  0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000,0.375000,
+  0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.917969,0.917969,0.375000,0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,
+  0.917969,0.375000,0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.917969,0.375000,
+  0.375000,0.187500,0.187500,0.187500,0.187500,0.187500,0.187500,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,0.093750,
+  0.093750,0.093750,0.093750,0.093750,0.093750,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+  0.000000,0.000000,4.527344,3.324219,0.457031,4.421875,3.265625,0.453125,4.324219,3.167969,
+  0.449219,4.421875,3.265625,0.453125,4.234375,3.113281,0.449219,3.980469,2.929688,0.441406,
+  4.324219,3.167969,0.449219,3.980469,2.929688,0.441406,3.558594,0.433594,0.433594,4.421875,
+  3.265625,0.453125,4.234375,3.113281,0.449219,3.980469,2.929688,0.441406,4.234375,3.113281,
+  0.449219,3.929688,2.902344,0.441406,3.484375,0.429688,0.429688,3.980469,2.929688,0.441406,
+  3.484375,0.429688,0.429688,2.871094,0.417969,0.417969,4.324219,3.167969,0.449219,3.980469,
+  2.929688,0.441406,3.558594,0.433594,0.433594,3.980469,2.929688,0.441406,3.484375,0.429688,
+  0.429688,2.871094,0.417969,0.417969,3.558594,0.433594,0.433594,2.871094,0.417969,0.417969,
+  0.308594,0.308594,0.308594,4.253906,3.140625,0.574219,4.156250,3.085938,0.566406,4.066406,
+  2.996094,0.562500,4.156250,3.085938,0.566406,3.984375,2.945312,0.554688,3.750000,2.777344,
+  0.542969,4.066406,2.996094,0.562500,3.750000,2.777344,0.542969,3.359375,0.519531,0.519531,
+  4.156250,3.085938,0.566406,3.984375,2.945312,0.554688,3.750000,2.777344,0.542969,3.984375,
+  2.945312,0.554688,3.699219,2.753906,0.539062,3.289062,0.515625,0.515625,3.750000,2.777344,
+  0.542969,3.289062,0.515625,0.515625,2.722656,0.480469,0.480469,4.066406,2.996094,0.562500,
+  3.750000,2.777344,0.542969,3.359375,0.519531,0.519531,3.750000,2.777344,0.542969,3.289062,
+  0.515625,0.515625,2.722656,0.480469,0.480469,3.359375,0.519531,0.519531,2.722656,0.480469,
+  0.480469,0.328125,0.328125,0.328125,
+};

selfdrive/camerad/test/tici_zclient/livergb.py

@@ -1,48 +0,0 @@
-# flake8: noqa
-# pylint: disable=W
-
-#!/usr/bin/env python
-import numpy as np
-import cv2
-from time import time, sleep
-
-H, W = (604*2//6, 964*2//6)
-# H, W = (604, 964)
-
-cam_bufs = np.zeros((3,H,W,3), dtype=np.uint8)
-hist_bufs = np.zeros((3,H,200,3), dtype=np.uint8)
-
-if __name__ == '__main__':
-  import zmq
-  context = zmq.Context()
-  socket = context.socket(zmq.PULL)
-  socket.bind("tcp://192.168.3.4:7768")
-  while True:
-    try:
-      message = socket.recv()
-    except Exception as ex:
-      print(ex)
-      message = b"123"
-
-    dat = np.frombuffer(message, dtype=np.uint8)
-    cam_id = (dat[0] + 1) % 3
-    # import pdb; pdb.set_trace()
-    b = dat[::3].reshape(H, W)
-    g = dat[1::3].reshape(H, W)
-    r = dat[2::3].reshape(H, W)
-    cam_bufs[cam_id] = cv2.merge((r, g, b))
-    cam_bufs[cam_id]= cv2.cvtColor(cam_bufs[cam_id], cv2.COLOR_RGB2BGR)
-
-    hist = cv2.calcHist([cv2.cvtColor(cam_bufs[cam_id], cv2.COLOR_BGR2GRAY)],[0],None,[32],[0,256])
-    hist = (H*hist/hist.max()).astype(np.uint8)
-    hist_bufs[cam_id] = 0
-    for i,bb in enumerate(hist):
-        hist_bufs[cam_id, H-bb[0]:,i*(200//32):(i+1)*(200//32), :] = (222,222,222)
-
-    out = cam_bufs.reshape((3*H,W,3))
-    hist_bufs_out = hist_bufs.reshape((3*H,200,3))
-    out = np.hstack([out, hist_bufs_out])
-    cv2.imshow('RGB', out)
-    cv2.waitKey(55)
-    #dat.tofile('/tmp/c3rgb.img')
-    #cv2.imwrite('/tmp/c3rgb.png', out)

selfdrive/camerad/test/tici_zclient/liveyuv.py

@@ -1,34 +0,0 @@
-# flake8: noqa
-# pylint: disable=W
-
-#!/usr/bin/env python
-import numpy as np
-import cv2
-from time import time, sleep
-
-H, W = (256, 512)
-
-if __name__ == '__main__':
-  import zmq
-  context = zmq.Context()
-  socket = context.socket(zmq.PULL)
-  socket.bind("tcp://192.168.3.4:7769")
-  while True:
-    try:
-      message = socket.recv()
-    except Exception as ex:
-      print(ex)
-      message = b"123"
-
-    dat = np.frombuffer(message, dtype=np.float32)
-    mc = (dat.reshape(H//2, W//2)).astype(np.uint8)
-
-    hist = cv2.calcHist([mc],[0],None,[32],[0,256])
-    hist = (H*hist/hist.max()).astype(np.uint8)
-    himg = np.zeros((H//2, W//2), dtype=np.uint8)
-    for i,b in enumerate(hist):
-      himg[H//2-b[0]:,i*(W//2//32):(i+1)*(W//2//32)] = 222
-
-    cv2.imshow('model fov', np.hstack([mc, himg]))
-    cv2.waitKey(20)
-    dat.tofile('/tmp/c3yuv.img')