diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 92d31464836a13f82835819d47ac8e60e6f14f70..c9147af4f53572563d49abac0bce8e158bf9ece5 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 81bcfe2240be499e6b2401a4318bb6ad4028ecaa..77c688a45de6c7b43a401059a3e0a89dd373bf8b 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