Add CLAHE preproc option to the processor

demos/processor_visual_odometry.yaml

@@ -14,6 +14,11 @@ max_new_features: 100
 
 ####################################
 # ProcessorVisualOdometry parameters
+
+# CLAHE = Contrast Limited Adaptive Histogram Equalization  
+# -> more continuous lighting and higher contrast images, 1-1.5 ms overhead
+use_clahe: true
+
 # FAST KeyPoint detection 
 fast_params:
     # Threshold on the keypoint pixel intensity (in uchar [0-255]) 

include/vision/processor/processor_visual_odometry.h

@@ -82,6 +82,7 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
     };
 
     double std_pix_;
+    bool use_clahe_;
     RansacParams ransac_params_;
     KltParams klt_params_;
     FastParams fast_params_;
@@ -95,6 +96,8 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
     {
         std_pix_ = _server.getParam<int>(prefix + _unique_name + "/std_pix");
 
+        use_clahe_ = _server.getParam<bool>(prefix + _unique_name + "/use_clahe");
+
         ransac_params_.ransac_prob_   = _server.getParam<double>(prefix + _unique_name + "/ransac_params/ransac_prob");
         ransac_params_.ransac_thresh_ = _server.getParam<double>(prefix + _unique_name + "/ransac_params/ransac_thresh");
 
@@ -119,6 +122,7 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
     std::string print() const override
     {
         return ParamsProcessorTracker::print()                                                                   + "\n"
+            + "use_clahe_:                       " + std::to_string(use_clahe_)                                                   + "\n"
             + "ransac_params_.ransac_prob_:      " + std::to_string(ransac_params_.ransac_prob_)                 + "\n"
             + "ransac_params_.ransac_thresh_:    " + std::to_string(ransac_params_.ransac_thresh_)               + "\n"
             + "klt_params_.tracker_width_:       " + std::to_string(klt_params_.patch_width_)                    + "\n"

src/processor/processor_visual_odometry.cpp

@@ -86,10 +86,12 @@ void ProcessorVisualOdometry::preProcess()
 
     cv::Mat img_incoming = capture_image_incoming_->getImage();
 
-    // Adaptive Histogram Correction to get more continuous lighting and higher contrast
-    // Seems to give better tracking but a bit slow, TOBENCHMARK
-    // cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(2.0, cv::Size(8,8));
-    // clahe->apply(img_incoming, img_incoming);
+    if (params_visual_odometry_->use_clahe_){
+        // Contrast Limited Adaptive Histogram Equalization  
+        // -> more continuous lighting and higher contrast images
+        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(2.0, cv::Size(8,8));
+        clahe->apply(img_incoming, img_incoming);
+    }
 
 
     // Time to PREPreprocess the image if necessary: greyscale if BGR, CLAHE etc...