refactor imgproc/utils (#2766)

* refactor imgproc/utils

* const

* space

* return value

selfdrive/camerad/cameras/camera_qcom.cc

@@ -2106,12 +2106,12 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
   CL_CHECK(clEnqueueReadBuffer(b->q, s->rgb_conv_result_cl, true, 0,
                              b->rgb_width / NUM_SEGMENTS_X * b->rgb_height / NUM_SEGMENTS_Y * sizeof(int16_t), s->conv_result.get(), 0, 0, 0));
 
-  get_lapmap_one(s->conv_result.get(), &s->lapres[roi_id], b->rgb_width / NUM_SEGMENTS_X, b->rgb_height / NUM_SEGMENTS_Y);
+  s->lapres[roi_id] = get_lapmap_one(s->conv_result.get(), b->rgb_width / NUM_SEGMENTS_X, b->rgb_height / NUM_SEGMENTS_Y);
 
   // setup self recover
   const float lens_true_pos = s->rear.lens_true_pos;
   std::atomic<int>& self_recover = s->rear.self_recover;
-  if (is_blur(&s->lapres[0]) &&
+  if (is_blur(&s->lapres[0], std::size(s->lapres)) &&
       (lens_true_pos < (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_DOWN : OP3T_AF_DAC_DOWN) + 1 ||
        lens_true_pos > (s->rear.device == DEVICE_LP3 ? LP3_AF_DAC_UP : OP3T_AF_DAC_UP) - 1) &&
       self_recover < 2) {

selfdrive/camerad/imgproc/utils.cc

@@ -1,43 +1,36 @@
 #include "utils.h"
 #include <stdio.h>
 #include <algorithm>
-
+#include <cmath>
 // calculate score based on laplacians in one area
-void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch) {
+uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
-  int size = x_pitch * y_pitch;
+  const int size = x_pitch * y_pitch;
   // avg and max of roi
-  float fsum = 0;
+  int16_t max = 0;
-  int16_t mean, max;
+  int sum = 0;
-  max = 0;
+  for (int i = 0; i < size; ++i) {
-
+    const int16_t v = lap[i % x_pitch + (i / x_pitch) * x_pitch];
-  for (int i = 0; i < size; i++) {
+    sum += v;
-    int x_offset = i % x_pitch;
+    if (v > max) max = v;
-    int y_offset = i / x_pitch;
-    fsum += lap[x_offset + y_offset*x_pitch];
-    max = std::max(lap[x_offset + y_offset*x_pitch], max);
   }
 
-  mean = fsum / size;
+  const int16_t mean = sum / size;
 
   // var of roi
-  float fvar = 0;
+  int var = 0;
-  for (int i = 0; i < size; i++) {
+  for (int i = 0; i < size; ++i) {
-    int x_offset = i % x_pitch;
+    var += std::pow(lap[i % x_pitch + (i / x_pitch) * x_pitch] - mean, 2);
-    int y_offset = i / x_pitch;
-    fvar += (float)((lap[x_offset + y_offset*x_pitch] - mean) * (lap[x_offset + y_offset*x_pitch] - mean));
   }
 
-  fvar = fvar / size;
+  const float fvar = (float)var / size;
-
+  return std::min(5 * fvar + max, (float)65535);
-  *res = std::min(5 * fvar + max, (float)65535);
 }
 
-bool is_blur(uint16_t *lapmap) {
+bool is_blur(const uint16_t *lapmap, const size_t size) {
-  int n_roi = (ROI_X_MAX - ROI_X_MIN + 1) * (ROI_Y_MAX - ROI_Y_MIN + 1);
   float bad_sum = 0;
-  for (int i = 0; i < n_roi; i++) {
+  for (int i = 0; i < size; i++) {
-    if (*(lapmap + i) < LM_THRESH) {
+    if (lapmap[i] < LM_THRESH) {
-      bad_sum += 1/(float)n_roi;
+      bad_sum += 1 / (float)size;
     }
   }
   return (bad_sum > LM_PREC_THRESH);

selfdrive/camerad/imgproc/utils.h

@@ -1,8 +1,7 @@
-#ifndef IMGPROC_UTILS
+#pragma once
-#define IMGPROC_UTILS
 
 #include <stdint.h>
-
+#include <stddef.h>
 #define NUM_SEGMENTS_X 8
 #define NUM_SEGMENTS_Y 6
 
@@ -24,7 +23,5 @@ const int16_t lapl_conv_krnl[9] = {0, 1, 0,
                                   1, -4, 1,
                                   0, 1, 0};
 
-void get_lapmap_one(int16_t *lap, uint16_t *res, int x_pitch, int y_pitch);
+uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch);
-bool is_blur(uint16_t *lapmap);
+bool is_blur(const uint16_t *lapmap, const size_t size);
-
-#endif