From fd5efdba41c8fa12d162ac671e0321adc4c00f5a Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Tue, 31 May 2022 14:47:52 +0200
Subject: [PATCH] Refactor equalization

---
 .../processor/processor_visual_odometry.h     | 10 +++
 src/processor/processor_visual_odometry.cpp   | 66 +++++++++----------
 2 files changed, 42 insertions(+), 34 deletions(-)

diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 92d314648..c9147af4f 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -290,6 +290,16 @@ class ProcessorVisualOdometry : public ProcessorTracker
     private:
         void retainBest(std::vector<cv::KeyPoint> &_keypoints, int n);
 
+        /* Equalize image for better detection and tracking
+        * available methods:
+        *      0. none
+        *      1. average
+        *      2. opencv: histogram_equalization
+        *      3. opencv: CLAHE
+        */
+        void equalize_img(cv::Mat &img_incoming, ParamsProcessorVisualOdometry::EqualizationParams equalization)
+;
+
 };
 
 } //namespace wolf
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 81bcfe224..77c688a45 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -87,40 +87,7 @@ void ProcessorVisualOdometry::preProcess()
 
     cv::Mat img_incoming = capture_image_incoming_->getImage();
 
-
-    /* Equalize image for better detection and tracking
-     * available methods:
-     *      0. none
-     *      1. average
-     *      2. opencv: histogram_equalization
-     *      3. opencv: CLAHE
-     */
-    switch (params_visual_odometry_->equalization.method)
-    {
-        case 0:
-            break;
-        case 1:
-        {
-            // average to central brightness
-            auto img_avg = (cv::mean(img_incoming)).val[0];
-            img_incoming += cv::Scalar(round(params_visual_odometry_->equalization.average.median - img_avg) );
-            break;
-        }
-        case 2:
-        {
-            cv::equalizeHist( img_incoming, img_incoming );
-            break;
-        }
-        case 3:
-        {
-            // Contrast Limited Adaptive Histogram Equalization  CLAHE
-            // -> more continuous lighting and higher contrast images
-            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(params_visual_odometry_->equalization.clahe.clip_limit,
-                                                       params_visual_odometry_->equalization.clahe.tile_grid_size);
-            clahe->apply(img_incoming, img_incoming);
-            break;
-        }
-    }
+    equalize_img(img_incoming, params_visual_odometry_->equalization);
 
 
     // Time to PREPreprocess the image if necessary: greyscale if BGR, CLAHE etc...
@@ -706,6 +673,37 @@ void ProcessorVisualOdometry::retainBest(std::vector<cv::KeyPoint> &_keypoints,
     }
 }
 
+
+void ProcessorVisualOdometry::equalize_img(cv::Mat &img_incoming, ParamsProcessorVisualOdometry::EqualizationParams equalization)
+{
+    switch (equalization.method)
+    {
+        case 0:
+            break;
+        case 1:
+        {
+            // average to central brightness
+            auto img_avg = (cv::mean(img_incoming)).val[0];
+            img_incoming += cv::Scalar(round(equalization.average.median - img_avg) );
+            break;
+        }
+        case 2:
+        {
+            cv::equalizeHist( img_incoming, img_incoming );
+            break;
+        }
+        case 3:
+        {
+            // Contrast Limited Adaptive Histogram Equalization  CLAHE
+            // -> more continuous lighting and higher contrast images
+            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(equalization.clahe.clip_limit,
+                                                       equalization.clahe.tile_grid_size);
+            clahe->apply(img_incoming, img_incoming);
+            break;
+        }
+    }
+}
+
 } //namespace wolf
 
 // Register in the FactoryProcessor
-- 
GitLab