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