center finder

danchia · Jun 4, 2023 · 8700a1e · 8700a1e
1 parent d5b9325
commit 8700a1e
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 47 deletions.
diff --git a/src/localization/localization_util.cc b/src/localization/localization_util.cc
@@ -14,23 +14,15 @@ struct ImgPoint {
   int u = 0;
   int v = 0;
 };
-struct FindRectAlgo {
+struct FindCenterAlgo {
   void Find(int u, int v) {
     if (img[v * width + u] < thres) return;
 
     img[v * width + u] = 0;
 
     ++area;
-    if (u < top_left.u) {
-      top_left.u = u;
-    } else if (u > bottom_right.u) {
-      bottom_right.u = u;
-    }
-    if (v < top_left.v) {
-      top_left.v = v;
-    } else if (v > bottom_right.v) {
-      bottom_right.v = v;
-    }
+    avg.u += u;
+    avg.v += v;
 
     for (int du = -1; du <= 1; ++du) {
       for (int dv = -1; dv <= 1; ++dv) {
@@ -47,39 +39,32 @@ struct FindRectAlgo {
   int height;
   uint8_t* img;
   uint8_t thres;
+  ImgPoint avg;
 
   int area = 0;
-  ImgPoint top_left = ImgPoint{
-      std::numeric_limits<int>::max(),
-      std::numeric_limits<int>::max(),
-  };
-  ImgPoint bottom_right = ImgPoint{
-      std::numeric_limits<int>::min(),
-      std::numeric_limits<int>::min(),
-  };
 };
 }  // namespace
 
-std::vector<Rect> FindRect(int width, int height, uint8_t* img,
-                           uint8_t pix_thres, int min_area) {
-  std::vector<Rect> res;
+std::vector<ImgPoint> FindCenter(int width, int height, uint8_t* img,
+                                 uint8_t pix_thres, int min_area) {
+  std::vector<ImgPoint> res;
 
   for (int v = 0; v < height; ++v) {
     for (int u = 0; u < width; ++u) {
       uint8_t x = img[v * width + u];
       if (x >= pix_thres) {
-        struct FindRectAlgo finder;
+        struct FindCenterAlgo finder;
         finder.width = width;
         finder.height = height;
         finder.img = img;
         finder.thres = pix_thres;
 
         finder.Find(u, v);
+        finder.avg.u /= finder.area;
+        finder.avg.v /= finder.area;
 
         if (finder.area > min_area) {
-          res.push_back({finder.top_left.u, finder.top_left.v,
-                         finder.bottom_right.u - finder.top_left.u,
-                         finder.bottom_right.v - finder.top_left.v});
+          res.push_back(finder.avg);
         }
       }
     }
@@ -115,12 +100,6 @@ float normAngle(float x) {
   return x;
 }
 
-std::ostream& operator<<(std::ostream& os, const Rect& rect) {
-  os << "{" << rect.x << "," << rect.y << "," << rect.width << ","
-     << rect.height << "}";
-  return os;
-}
-
 CameraModel::CameraModel(int width, int height, const std::string& lut_file)
     : width_(width), height_(height) {
   camera_lut_.resize(width_ * height_ * 2);
@@ -169,13 +148,13 @@ std::vector<LightFinder::Light> LightFinder::Find(
     }
   }
 
-  auto rects = FindRect(width_, height_, img, pix_thres_, min_area_);
+  auto centers = FindCenter(width_, height_, img, pix_thres_, min_area_);
 
   std::vector<Light> lights;
-  lights.reserve(rects.size());
-  for (const auto& rect : rects) {
-    int u = rect.x + rect.width / 2;
-    int v = rect.y + rect.height / 2;
+  lights.reserve(centers.size());
+  for (const auto& center : centers) {
+    int u = center.u;
+    int v = center.v;
     Light& l = lights.emplace_back();
     l.u = u;
     l.v = v;
@@ -186,4 +165,4 @@ std::vector<LightFinder::Light> LightFinder::Find(
   }
 
   return lights;
-}
+}
diff --git a/src/localization/localization_util_test.cc b/src/localization/localization_util_test.cc
@@ -18,14 +18,6 @@ std::vector<uint8_t> TestImage() {
   return img;
 }
 
-TEST(FindRects, Basic) {
-  auto img = TestImage();
-  auto res = FindRect(640, 480, img.data(), 220, 1000);
-  EXPECT_THAT(res, testing::UnorderedElementsAre(Rect{110, 116, 82, 33},
-                                                 Rect{287, 143, 62, 43},
-                                                 Rect{279, 336, 62, 38}));
-}
-
 const Eigen::Vector4f K{1.83260687e+02, 1.83044898e+02, 3.18091934e+02,
                         2.50873559e+02};
 const Eigen::Vector4f D{0.06782692, -0.03927174, 0.00502321, 0.00063159};
@@ -36,4 +28,4 @@ TEST(FisheyeProject, Basic) {
   EXPECT_THAT(r.y(), testing::FloatNear(360.75, 0.01));
 }
 
-}  // namespace
+}  // namespace