├── CMakeLists.txt ├── data └── stereo_forward_day │ ├── vignetting_mean_left.png │ ├── vignetting_mean_right.png │ ├── vignetting_std_left.png │ └── vignetting_std_right.png ├── include └── image_preprocessing │ ├── clahs.h │ ├── color_correction.h │ ├── dehazer.h │ ├── flicker_remover.h │ ├── mono_clahs.h │ ├── mono_dehazer.h │ ├── stereo_clahs.h │ └── stereo_dehazer.h ├── launch ├── clahs.launch ├── clahs_emisari27092017.launch ├── clahs_test.launch ├── color_correction_image.launch ├── flicker_remover.launch ├── remove_vignetting.launch ├── test.launch ├── train_vignetting.launch └── vignetting.launch ├── package.xml └── src ├── clahs.cpp ├── color_correction.cpp ├── dehazer.cpp ├── flicker_remover.cpp ├── mono_clahs.cpp ├── mono_dehazer.cpp ├── nodes ├── clahs_node.cpp ├── color_correction_image_node.cpp ├── color_correction_node.cpp ├── dehazer_node.cpp ├── flicker_remover_node.cpp ├── stereo_clahs_node.cpp ├── stereo_dehazer_node.cpp ├── vignetting_extractor_node.cpp └── vignetting_remover_node.cpp ├── stereo_clahs.cpp └── stereo_dehazer.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(image_preprocessing) 3 | 4 | # C++11 support 5 | include(CheckCXXCompilerFlag) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 12 | else() 13 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 14 | endif() 15 | 16 | ## Find catkin macros and libraries 17 | find_package(catkin REQUIRED COMPONENTS 18 | image_transport 19 | roscpp 20 | sensor_msgs 21 | cv_bridge 22 | stereo_msgs 23 | image_geometry 24 | stereo_image_proc 25 | pcl_ros 26 | ) 27 | 28 | find_package(OpenCV REQUIRED) 29 | 30 | find_package(Boost REQUIRED COMPONENTS thread) 31 | 32 | find_package(OpenMP) 33 | if (OPENMP_FOUND) 34 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 35 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 36 | endif() 37 | 38 | # Dependencies - PCL: 39 | find_package(PCL REQUIRED) 40 | 41 | catkin_package( 42 | INCLUDE_DIRS include 43 | CATKIN_DEPENDS image_transport cv_bridge roscpp sensor_msgs 44 | DEPENDS OpenCV 45 | ) 46 | 47 | ########### 48 | ## Build ## 49 | ########### 50 | include_directories( 51 | include 52 | ${catkin_INCLUDE_DIRS} 53 | ${PCL_INCLUDE_DIRS} 54 | ) 55 | 56 | ## Executables 57 | add_executable(dehazer_node 58 | src/nodes/dehazer_node.cpp 59 | src/mono_dehazer.cpp 60 | src/dehazer.cpp) 61 | target_link_libraries(dehazer_node 62 | ${catkin_LIBRARIES} 63 | ) 64 | 65 | add_executable(color_correction_node 66 | src/nodes/color_correction_node.cpp 67 | src/color_correction.cpp) 68 | target_link_libraries(color_correction_node 69 | ${catkin_LIBRARIES} 70 | ) 71 | 72 | add_executable(stereo_dehazer_node 73 | src/nodes/stereo_dehazer_node.cpp 74 | src/stereo_dehazer.cpp 75 | src/dehazer.cpp) 76 | target_link_libraries(stereo_dehazer_node 77 | ${catkin_LIBRARIES} 78 | ${Boost_Thread_LIBRARIES} 79 | ) 80 | 81 | add_executable(vignetting_extractor 82 | src/nodes/vignetting_extractor_node.cpp) 83 | target_link_libraries(vignetting_extractor 84 | ${catkin_LIBRARIES} 85 | ) 86 | 87 | add_executable(vignetting_remover 88 | src/nodes/vignetting_remover_node.cpp) 89 | target_link_libraries(vignetting_remover 90 | ${catkin_LIBRARIES} 91 | ) 92 | 93 | add_executable(clahs_node 94 | src/nodes/clahs_node.cpp 95 | src/mono_clahs.cpp 96 | src/clahs.cpp) 97 | target_link_libraries(clahs_node 98 | ${catkin_LIBRARIES} 99 | ) 100 | 101 | add_executable(stereo_clahs_node 102 | src/nodes/stereo_clahs_node.cpp 103 | src/stereo_clahs.cpp 104 | src/clahs.cpp) 105 | target_link_libraries(stereo_clahs_node 106 | ${catkin_LIBRARIES} 107 | ${Boost_Thread_LIBRARIES} 108 | ) 109 | 110 | add_executable(color_correction_image 111 | src/nodes/color_correction_image_node.cpp) 112 | target_link_libraries(color_correction_image 113 | ${catkin_LIBRARIES} 114 | ) 115 | 116 | # add_executable(flicker_remover_node 117 | # src/nodes/flicker_remover_node.cpp 118 | # src/flicker_remover.cpp) 119 | # target_link_libraries(flicker_remover_node 120 | # ${catkin_LIBRARIES} 121 | # ) -------------------------------------------------------------------------------- /data/stereo_forward_day/vignetting_mean_left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/image_preprocessing/f689b5d514f02a5f1c0f7763d7c38665aaac908c/data/stereo_forward_day/vignetting_mean_left.png -------------------------------------------------------------------------------- /data/stereo_forward_day/vignetting_mean_right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/image_preprocessing/f689b5d514f02a5f1c0f7763d7c38665aaac908c/data/stereo_forward_day/vignetting_mean_right.png -------------------------------------------------------------------------------- /data/stereo_forward_day/vignetting_std_left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/image_preprocessing/f689b5d514f02a5f1c0f7763d7c38665aaac908c/data/stereo_forward_day/vignetting_std_left.png -------------------------------------------------------------------------------- /data/stereo_forward_day/vignetting_std_right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/srv/image_preprocessing/f689b5d514f02a5f1c0f7763d7c38665aaac908c/data/stereo_forward_day/vignetting_std_right.png -------------------------------------------------------------------------------- /include/image_preprocessing/clahs.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef CLAHS_H 7 | #define CLAHS_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace cv; 17 | 18 | class Clahs { 19 | public: 20 | Clahs(); 21 | 22 | void clahsGrayscale(const Mat& Image, Mat& Out, const int& NrY, const int& NrX); 23 | void clahsRGB(const Mat& Image, Mat& Out, const int& NrY, const int& NrX); 24 | 25 | protected: 26 | Mat MakeLUT(const int& NrGreyLevels, 27 | const int& NrBins); 28 | 29 | Mat ClipHistogram(const Mat& Histogram, 30 | const int& NrGreyLevels, 31 | const double& ClipLimit); 32 | 33 | Mat ClipHistogramSimple(const Mat& Histogram, 34 | const double& ClipLimit); 35 | 36 | Mat MapHistogram(const Mat& Histogram, 37 | const int& Min, 38 | const int& Max, 39 | const int& NrGreyLevels, 40 | const int& NrofPixels, 41 | const int& heq_type, 42 | const double& heq_alpha); 43 | 44 | Mat Interpolate(const Mat& SubRegion, 45 | const Mat& MapLU, 46 | const Mat& MapRU, 47 | const Mat& MapLB, 48 | const Mat& MapRB, 49 | const int& XSize, 50 | const int& YSize, 51 | const Mat& LUT); 52 | 53 | Mat buildbyIndices(const Mat& vector, 54 | const Mat& indices); 55 | 56 | Mat linspace(const double& startP, 57 | const double& Endp, 58 | const int& interval); 59 | 60 | Mat cumsum(const Mat& in); 61 | }; 62 | #endif 63 | -------------------------------------------------------------------------------- /include/image_preprocessing/color_correction.h: -------------------------------------------------------------------------------- 1 | #ifndef COLOR_CORRECTION_H 2 | #define COLOR_CORRECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class ColorCorrection { 12 | public: 13 | ColorCorrection(ros::NodeHandle nh, ros::NodeHandle nhp); 14 | cv::Mat adjustLevelsAndGamma(const cv::Mat& channel, 15 | double gamma, 16 | double max_value); 17 | void imageCallback(const sensor_msgs::ImageConstPtr &image_msg, 18 | const sensor_msgs::CameraInfoConstPtr &info_msg); 19 | double gamma_r, gamma_g, gamma_b; 20 | int max_r, max_b, max_g; 21 | private: 22 | ros::NodeHandle nh_; 23 | ros::NodeHandle nhp_; 24 | image_transport::ImageTransport it_; 25 | image_transport::CameraSubscriber camera_sub_; 26 | image_transport::CameraPublisher img_pub_; 27 | }; 28 | 29 | #endif // COLOR_CORRECTION_H 30 | -------------------------------------------------------------------------------- /include/image_preprocessing/dehazer.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef DEHAZER_H 7 | #define DEHAZER_H 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | class Dehazer { 14 | public: 15 | Dehazer() {} 16 | cv::Mat dehazeRGB(cv::Mat img); 17 | cv::Mat dehazeGrayscale(cv::Mat img); 18 | void guidedFilter(cv::Mat img, cv::Mat guidance_img, cv::Mat& out, 19 | int local_window_radius, double eps); 20 | }; 21 | #endif 22 | -------------------------------------------------------------------------------- /include/image_preprocessing/flicker_remover.h: -------------------------------------------------------------------------------- 1 | #ifndef FLICKER_REMOVER_H 2 | #define FLICKER_REMOVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace enc = sensor_msgs::image_encodings; 17 | using namespace std; 18 | 19 | class FlickerRemover 20 | { 21 | 22 | public: 23 | FlickerRemover(ros::NodeHandle nh, ros::NodeHandle nhp); 24 | 25 | void run(); 26 | 27 | protected: 28 | 29 | void msgsCallback(const sensor_msgs::ImageConstPtr& l_img_msg, 30 | const sensor_msgs::ImageConstPtr& r_img_msg, 31 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 32 | const sensor_msgs::CameraInfoConstPtr& r_info_msg); 33 | 34 | bool imgMsgToMat(sensor_msgs::Image l_img_msg, 35 | sensor_msgs::Image r_img_msg, 36 | cv::Mat &l_img, cv::Mat &r_img); 37 | 38 | void featureExtraction(cv::Mat img, vector& kp, cv::Mat& desc); 39 | 40 | void registerImages(); 41 | 42 | void thresholdMatching(const cv::Mat& descriptors1, const cv::Mat& descriptors2, 43 | double threshold, const cv::Mat& match_mask, vector& matches); 44 | 45 | void crossCheckFilter(const vector& matches1to2, 46 | const vector& matches2to1, 47 | vector& checked_matches); 48 | 49 | void crossCheckThresholdMatching(const cv::Mat& descriptors1, const cv::Mat& descriptors2, 50 | double threshold, vector& matches); 51 | 52 | private: 53 | 54 | ros::NodeHandle nh_; 55 | ros::NodeHandle nhp_; 56 | 57 | double desc_thresh_; 58 | double epipolar_thresh_; 59 | double min_inliers_; 60 | 61 | cv::Mat next_l_, next_r_; 62 | cv::Mat curr_l_, curr_r_; 63 | cv::Mat prev_l_, prev_r_; 64 | 65 | int frame_counter_; 66 | 67 | // Topic sync 68 | typedef message_filters::sync_policies::ApproximateTime SyncPolicy; 72 | typedef message_filters::Synchronizer Sync; 73 | 74 | }; 75 | #endif 76 | -------------------------------------------------------------------------------- /include/image_preprocessing/mono_clahs.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef MONO_CLAHS_H 7 | #define MONO_CLAHS_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | class MonoClahs { 20 | public: 21 | MonoClahs(ros::NodeHandle nh, ros::NodeHandle nhp); 22 | private: 23 | ros::NodeHandle nh_; 24 | ros::NodeHandle nhp_; 25 | image_transport::ImageTransport it_; 26 | image_transport::CameraSubscriber camera_sub_; 27 | image_transport::CameraPublisher img_pub_; 28 | 29 | bool is_rgb_; //!> Set to true when RGB 30 | 31 | void imageCallback(const sensor_msgs::ImageConstPtr &image_msg, 32 | const sensor_msgs::CameraInfoConstPtr &info_msg); 33 | }; 34 | #endif 35 | -------------------------------------------------------------------------------- /include/image_preprocessing/mono_dehazer.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef MONO_DEHAZER_H 7 | #define MONO_DEHAZER_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | class MonoDehazer { 20 | public: 21 | MonoDehazer(ros::NodeHandle nh, ros::NodeHandle nhp); 22 | private: 23 | ros::NodeHandle nh_; 24 | ros::NodeHandle nhp_; 25 | image_transport::ImageTransport it_; 26 | image_transport::CameraSubscriber camera_sub_; 27 | image_transport::CameraPublisher img_pub_; 28 | 29 | bool is_rgb_; //!> Set to true when RGB 30 | 31 | void imageCallback(const sensor_msgs::ImageConstPtr &image_msg, 32 | const sensor_msgs::CameraInfoConstPtr &info_msg); 33 | }; 34 | #endif 35 | -------------------------------------------------------------------------------- /include/image_preprocessing/stereo_clahs.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef STEREO_CLAHS_H 7 | #define STEREO_CLAHS_H 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | class StereoClahs { 33 | public: 34 | StereoClahs() {} 35 | StereoClahs(ros::NodeHandle nh, ros::NodeHandle nhp); 36 | 37 | private: 38 | // Node handlers 39 | ros::NodeHandle nh_; 40 | ros::NodeHandle nhp_; 41 | 42 | Clahs c_; 43 | 44 | // Topic subscribers 45 | std::string left_color_topic_, right_color_topic_; 46 | std::string left_mono_topic_, right_mono_topic_; 47 | std::string left_info_topic_, right_info_topic_; 48 | std::string cloud_topic_; 49 | 50 | // Subscribers 51 | image_transport::SubscriberFilter left_color_sub_, 52 | right_color_sub_, 53 | left_mono_sub_, 54 | right_mono_sub_; 55 | message_filters::Subscriber left_color_info_sub_, 56 | right_color_info_sub_, 57 | left_mono_info_sub_, 58 | right_mono_info_sub_; 59 | message_filters::Subscriber cloud_color_sub_; 60 | 61 | // Topic sync properties (with pointcloud) 62 | typedef message_filters::sync_policies::ApproximateTime SyncColorPolicy; 67 | typedef message_filters::Synchronizer SyncColor; 68 | boost::shared_ptr sync_color_; 69 | 70 | typedef message_filters::sync_policies::ApproximateTime SyncMonoPolicy; 74 | typedef message_filters::Synchronizer SyncMono; 75 | boost::shared_ptr sync_mono_; 76 | 77 | // ROS Camera publisher 78 | //image_transport::CameraPublisher left_color_pub_, right_color_pub_; 79 | image_transport::CameraPublisher left_mono_pub_, right_mono_pub_, left_color_pub_, right_color_pub_; 80 | 81 | // points2 publisher 82 | ros::Publisher pub_points2_; 83 | 84 | 85 | void colorCb(const sensor_msgs::ImageConstPtr& l_img_msg, 86 | const sensor_msgs::ImageConstPtr& r_img_msg, 87 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 88 | const sensor_msgs::CameraInfoConstPtr& r_info_msg, 89 | const sensor_msgs::PointCloud2ConstPtr& cloud_msg); 90 | void monoCb(const sensor_msgs::ImageConstPtr& l_img_msg, 91 | const sensor_msgs::ImageConstPtr& r_img_msg, 92 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 93 | const sensor_msgs::CameraInfoConstPtr& r_info_msg); 94 | }; 95 | #endif 96 | -------------------------------------------------------------------------------- /include/image_preprocessing/stereo_dehazer.h: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #ifndef STEREO_DEHAZER_H 7 | #define STEREO_DEHAZER_H 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | class StereoDehazer { 33 | public: 34 | StereoDehazer() {} 35 | StereoDehazer(ros::NodeHandle nh, ros::NodeHandle nhp); 36 | 37 | private: 38 | // Node handlers 39 | ros::NodeHandle nh_; 40 | ros::NodeHandle nhp_; 41 | 42 | Dehazer d_; 43 | 44 | // Topic subscribers 45 | std::string left_color_topic_, right_color_topic_; 46 | std::string left_mono_topic_, right_mono_topic_; 47 | std::string left_info_topic_, right_info_topic_; 48 | std::string cloud_topic_; 49 | 50 | // Subscribers 51 | image_transport::SubscriberFilter left_color_sub_, 52 | right_color_sub_, 53 | left_mono_sub_, 54 | right_mono_sub_; 55 | message_filters::Subscriber left_color_info_sub_, 56 | right_color_info_sub_, 57 | left_mono_info_sub_, 58 | right_mono_info_sub_; 59 | message_filters::Subscriber cloud_color_sub_; 60 | 61 | // Topic sync properties (with pointcloud) 62 | typedef message_filters::sync_policies::ApproximateTime SyncColorPolicy; 67 | typedef message_filters::Synchronizer SyncColor; 68 | boost::shared_ptr sync_color_; 69 | 70 | typedef message_filters::sync_policies::ApproximateTime SyncMonoPolicy; 74 | typedef message_filters::Synchronizer SyncMono; 75 | boost::shared_ptr sync_mono_; 76 | 77 | // ROS Camera publisher 78 | //image_transport::CameraPublisher left_color_pub_, right_color_pub_; 79 | image_transport::CameraPublisher left_mono_pub_, right_mono_pub_, left_color_pub_, right_color_pub_; 80 | 81 | // points2 publisher 82 | ros::Publisher pub_points2_; 83 | 84 | 85 | void colorCb(const sensor_msgs::ImageConstPtr& l_img_msg, 86 | const sensor_msgs::ImageConstPtr& r_img_msg, 87 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 88 | const sensor_msgs::CameraInfoConstPtr& r_info_msg, 89 | const sensor_msgs::PointCloud2ConstPtr& cloud_msg); 90 | void monoCb(const sensor_msgs::ImageConstPtr& l_img_msg, 91 | const sensor_msgs::ImageConstPtr& r_img_msg, 92 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 93 | const sensor_msgs::CameraInfoConstPtr& r_info_msg); 94 | }; 95 | #endif 96 | -------------------------------------------------------------------------------- /launch/clahs.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/clahs_emisari27092017.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /launch/clahs_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/color_correction_image.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/flicker_remover.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/remove_vignetting.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 28 | 29 | 30 | 31 | 32 | 33 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /launch/train_vignetting.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/vignetting.launch: -------------------------------------------------------------------------------- 1 | 2 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | image_preprocessing 4 | 0.0.0 5 | The image_preprocessing package 6 | 7 | Miquel Massot 8 | 9 | BSD 10 | 11 | http://wiki.ros.org/image_preprocessing 12 | 13 | Miquel Massot 14 | 15 | 16 | 17 | catkin 18 | image_transport 19 | cv_bridge 20 | roscpp 21 | sensor_msgs 22 | opencv2 23 | stereo_msgs 24 | image_geometry 25 | stereo_image_proc 26 | pcl_ros 27 | 28 | opencv2 29 | image_transport 30 | cv_bridge 31 | roscpp 32 | sensor_msgs 33 | stereo_msgs 34 | stereo_image_proc 35 | image_geometry 36 | pcl_ros 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/clahs.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | 8 | Clahs::Clahs() {} 9 | 10 | void Clahs::clahsRGB(const Mat& Image, Mat& Out, const int& NrY, const int& NrX) { 11 | 12 | Mat I; 13 | Image.copyTo(I); 14 | 15 | // Split image in channels 16 | std::vector channels(3); 17 | split(I, channels); 18 | Mat r, g, b; 19 | channels[0].convertTo(r,CV_8U); 20 | channels[1].convertTo(g,CV_8U); 21 | channels[2].convertTo(b,CV_8U); 22 | 23 | // Compute 24 | Mat cr, cg, cb; 25 | boost::thread thread1( boost::bind( &Clahs::clahsGrayscale, this, r, boost::ref(cr), NrY, NrX ) ); 26 | boost::thread thread2( boost::bind( &Clahs::clahsGrayscale, this, g, boost::ref(cg), NrY, NrX ) ); 27 | boost::thread thread3( boost::bind( &Clahs::clahsGrayscale, this, b, boost::ref(cb), NrY, NrX ) ); 28 | thread1.join(); 29 | thread2.join(); 30 | thread3.join(); 31 | 32 | // Join channels 33 | Mat q[] = {cr, cg, cb}; 34 | merge(q, 3, Out); 35 | } 36 | 37 | void Clahs::clahsGrayscale(const Mat& Image, Mat& Out, const int& NrY, const int& NrX) { 38 | // Initializations 39 | const int Min = 0; 40 | const int Max = 255; 41 | const int NrBins = 256; 42 | const int NrGreyLevels = 256; 43 | const int YRes = Image.rows; 44 | const int XRes = Image.cols; 45 | const int heq_type = 3; 46 | const double alpha = 0.4; 47 | const int MAX_REG_X = 128; 48 | const int MAX_REG_Y = 128; 49 | const int HEQ_CLIP_SIMPLE = 1; 50 | const int HEQ_CLIP_REDISTRIBUTE = 2; 51 | 52 | double ClipLimit = 5; 53 | int clip_type = 2; 54 | 55 | Mat I; 56 | Image.copyTo(I); 57 | 58 | // Initialize output 59 | Mat CLAHSImage = Mat::zeros(YRes,XRes,CV_64FC1); 60 | 61 | // Check if image is grayscale 62 | if (I.channels() != 1) { 63 | ROS_WARN("[ImagePreprocessing]: Input image has more than one channel. It will be converted to grayscale. Use clahsRGB if image is RGB."); 64 | cvtColor(I, I, CV_BGR2GRAY); 65 | } 66 | 67 | // Check if image is uint8 68 | if (I.type() != CV_8U) { 69 | ROS_WARN("[ImagePreprocessing]: Input image type is not CV_8U. It will be converted to CV_8U."); 70 | I.convertTo(I,CV_8U); 71 | } 72 | 73 | // Sanity checks 74 | if ( NrX > MAX_REG_X ) 75 | ROS_ERROR("[ImagePreprocessing]: NrX > MAX_REG_X."); 76 | if (NrY > MAX_REG_Y) 77 | ROS_ERROR("[ImagePreprocessing]: NrY > MAX_REG_Y."); 78 | if ( (XRes/NrX) % 2 != 0 ) 79 | ROS_ERROR_STREAM("[ImagePreprocessing]: XRes/NrX = " << XRes/NrX << ", noninteger value."); 80 | if ( (YRes/NrY) % 2 != 0 ) 81 | ROS_ERROR_STREAM("[ImagePreprocessing]: YRes/NrY = " << YRes/NrY << ", noninteger value."); 82 | if ( Max > NrGreyLevels ) 83 | ROS_ERROR_STREAM("[ImagePreprocessing]: Max image value > number of greylevels."); 84 | if ( NrX<2 || NrY<2 ) 85 | ROS_ERROR_STREAM("[ImagePreprocessing]: NrX<2 or NrY<2, number of subregions must be>=4."); 86 | 87 | // Setup/initialization 88 | Mat MapArray = Mat::zeros(NrX*NrY*NrBins,1,CV_64FC1); 89 | 90 | // Determine properties of each subregion 91 | double XSize = XRes/NrX; 92 | double YSize = YRes/NrY; 93 | double NrPixels = XSize*YSize; 94 | 95 | // calculate actual clip limit 96 | if (ClipLimit > 1) { 97 | ClipLimit = (ClipLimit*NrPixels/(double)NrBins); 98 | if (ClipLimit < 1) 99 | ClipLimit = 1; 100 | else 101 | ClipLimit = ClipLimit; 102 | } else { 103 | // large value, do not clip (AHE) 104 | ClipLimit = std::numeric_limits::max(); 105 | clip_type = 0; 106 | } 107 | 108 | // calculate greylevel mappings for each subregion 109 | // calculate and process histograms for each subregion 110 | for (int Y=0; Y<=(NrY-1); Y++) { 111 | for (int X=0; X<=(NrX-1); X++) { 112 | int Xtmp = X*XSize+1; 113 | int Ytmp = Y*YSize+1; 114 | 115 | Mat SubRegion = I(Range(Ytmp, Ytmp+YSize-1), Range(Xtmp, Xtmp+XSize-1)); 116 | 117 | Mat Hist; 118 | float range[] = { 0, 256 } ; 119 | const float* hist_range = { range }; 120 | if (NrBins >= NrGreyLevels) { 121 | calcHist( &SubRegion, 1, 0, Mat(), Hist, 1, &NrBins, &hist_range, true, false ); 122 | } else { 123 | ROS_ERROR("[ImagePreprocessing]: NrBins must be greater or equal to NrGreyLevels"); 124 | } 125 | Hist.convertTo(Hist,CV_64FC1); 126 | 127 | // clip histogram, simple or redistribute depending on input parameters 128 | if (clip_type == HEQ_CLIP_REDISTRIBUTE) { 129 | Hist = ClipHistogram(Hist,NrBins,ClipLimit); 130 | } else if (clip_type == HEQ_CLIP_SIMPLE) { 131 | Hist = ClipHistogramSimple(Hist,ClipLimit); 132 | } 133 | 134 | // create histogram mapping (uniform,exponential,or rayleigh) 135 | Hist = MapHistogram(Hist, Min, Max, NrBins, NrPixels, heq_type, alpha); 136 | 137 | // write working histogram into appropriate part of MapArray 138 | Hist.copyTo( MapArray( Rect( 0, (NrBins*(Y*NrX+X)), Hist.cols, Hist.rows) ) ); 139 | } 140 | } 141 | 142 | // Convert to double 143 | I.convertTo(I,CV_64FC1); 144 | 145 | // Unsaturate the image 146 | I.setTo(250, I > 250); 147 | 148 | // interpolate greylevel mappings to get CLAHE image 149 | // make lookup table for mapping of grey values 150 | Mat LUT = MakeLUT(NrGreyLevels, NrBins); 151 | 152 | int lenY = 0; 153 | for (int Y=0; Y<=NrY; Y++) { 154 | int SubY, YU, YB; 155 | if (Y == 0) { // special case top row 156 | SubY = floor(YSize/2); 157 | YU = 0; 158 | YB = 0; 159 | } else if (Y == NrY) { //special case bottom row 160 | SubY = floor(YSize/2); 161 | YU = NrY-1; 162 | YB = YU; 163 | } else { 164 | SubY = YSize; 165 | YU = Y-1; 166 | YB = YU+1; 167 | } 168 | int lenX = 0; 169 | for (int X=0; X<=NrX; X++) { 170 | int SubX, XL, XR; 171 | if (X==0) { // special case Left column 172 | SubX = floor(XSize/2); 173 | XL = 0; 174 | XR = 0; 175 | } else if (X == NrX) { //special case right column 176 | SubX = floor(XSize/2); 177 | XL = NrX-1; 178 | XR = XL; 179 | } else { 180 | SubX = XSize; 181 | XL = X-1; 182 | XR = XL+1; 183 | } 184 | 185 | // retrieve the appropriate histogram mappings from MapArray 186 | Mat LU = MapArray( Range((NrBins*(YU*NrX +XL)), ((NrBins*(YU*NrX +XL)))+NrBins), Range(0, 1) ); 187 | Mat RU = MapArray( Range((NrBins*(YU*NrX +XR)), ((NrBins*(YU*NrX +XR)))+NrBins), Range(0, 1) ); 188 | Mat LB = MapArray( Range((NrBins*(YB*NrX +XL)), ((NrBins*(YB*NrX +XL)))+NrBins), Range(0, 1) ); 189 | Mat RB = MapArray( Range((NrBins*(YB*NrX +XR)), ((NrBins*(YB*NrX +XR)))+NrBins), Range(0, 1) ); 190 | 191 | // interpolate the appropriate subregion 192 | Mat SubRegion = I(Range(lenY, lenY+SubY), Range(lenX, lenX+SubX)); 193 | 194 | Mat InterpD = Interpolate(SubRegion,LU,RU,LB,RB,SubX,SubY,LUT); 195 | InterpD.copyTo( CLAHSImage( Rect( lenX, lenY, InterpD.cols, InterpD.rows) ) ); 196 | 197 | lenX = lenX+SubX; 198 | } 199 | lenY = lenY+SubY; 200 | } 201 | 202 | // Convert to CV_8U 203 | CLAHSImage.convertTo(CLAHSImage,CV_8U); 204 | CLAHSImage.copyTo(Out); 205 | } 206 | 207 | Mat Clahs::MakeLUT(const int& NrGreyLevels, 208 | const int& NrBins) { 209 | Mat LUT = Mat::zeros(NrGreyLevels,1,CV_64FC1); 210 | double count = 2.0; 211 | for (int j = 0; j < LUT.rows; ++j) { 212 | if (count > NrBins) 213 | count = NrBins; 214 | LUT.at(j,0) = count; 215 | count++; 216 | } 217 | return LUT; 218 | } 219 | 220 | Mat Clahs::ClipHistogram(const Mat& Histogram, 221 | const int& NrGreyLevels, 222 | const double& ClipLimit) { 223 | Mat NewHistogram; 224 | 225 | // number of excess pixels created by clipping 226 | Mat max_hist; 227 | max(Histogram-ClipLimit,0,max_hist); 228 | double NrExcess = sum(max_hist)[0]; 229 | 230 | // # of elements to be redist'ed to each bin 231 | double BinIncr = floor(NrExcess/NrGreyLevels); 232 | 233 | // max bin value where redist. will be above Climit 234 | double Upper = ClipLimit - BinIncr; 235 | 236 | // clip the histogram to ClipLimit 237 | NewHistogram = min(Histogram, ClipLimit); 238 | 239 | // add partial BinIncr pixels to bins up to ClipLimit 240 | Mat ii = NewHistogram > Upper; 241 | ii.convertTo(ii,CV_64FC1); 242 | Mat H; 243 | for (int i = 0; i < ii.rows; i++) { 244 | if (ii.at(i,0) == 255) { 245 | H.push_back(NewHistogram.row(i)); 246 | } 247 | } 248 | NrExcess -= sum(ClipLimit-H)[0]; 249 | 250 | for (int j = 0; j < ii.rows; j++) { 251 | if (ii.at(j,0) == 255) { 252 | NewHistogram.at(j,0) = ClipLimit; 253 | } 254 | } 255 | 256 | // add BinIncr to all other bins 257 | int counter = 0; 258 | Mat jj = NewHistogram <= Upper; 259 | jj.convertTo(jj,CV_64FC1); 260 | for (int i=0; i(i,0) == 255) { 262 | counter++; 263 | } 264 | } 265 | 266 | NrExcess -= counter*BinIncr; 267 | 268 | for (int j = 0; j < jj.rows; ++j) { 269 | if (jj.at(j,0) == 255) { 270 | NewHistogram.at(j,0) += BinIncr; 271 | } 272 | } 273 | 274 | /////////// 275 | // evenly redistribute remaining excess pixels 276 | 277 | while (NrExcess>0) { 278 | int h = 0; 279 | while ((h < NrGreyLevels) && (NrExcess > 0)) { 280 | // choose step to distribute the most excess evenly in one pass 281 | int StepSize = ceil(NrGreyLevels/NrExcess); 282 | int i = h; 283 | while ( (i < NrGreyLevels) && (NrExcess > 0)) { 284 | if (NewHistogram.at(i,0) < ClipLimit) { 285 | NewHistogram.row(i) = NewHistogram.row(i) + 1; 286 | NrExcess = NrExcess - 1; 287 | } 288 | i = i + StepSize; // step along the histogram 289 | } 290 | h = h + 1; // avoid concentrating pixels in bin 1 291 | } 292 | } 293 | return NewHistogram; 294 | } 295 | 296 | Mat Clahs::ClipHistogramSimple(const Mat& Histogram, 297 | const double& ClipLimit) { 298 | // This function performs clipping of the histogram 299 | // any bin with a value above the cliplimit is assigned the value of ClipLimit 300 | 301 | return min(Histogram,ClipLimit); 302 | } 303 | 304 | Mat Clahs::MapHistogram(const Mat& Histogram, 305 | const int& Min, 306 | const int& Max, 307 | const int& NrGreyLevels, 308 | const int& NrofPixels, 309 | const int& heq_type, 310 | const double& heq_alpha) { 311 | // This function calculates the equalized lookup table (mapping) 312 | // by cumulating the input histogram 313 | // Note: Lookup table is rescaled in the range [Min..Max]. 314 | Mat output; 315 | Histogram.copyTo(output); 316 | 317 | const int HEQ_NONE = 0; 318 | const int HEQ_UNIFORM = 1; 319 | const int HEQ_EXPONENTIAL = 2; 320 | const int HEQ_RAYLEIGH = 3; 321 | 322 | double Scale = ((double)Max-(double)Min)/(double)NrofPixels; 323 | 324 | switch (heq_type) { 325 | case HEQ_UNIFORM: 326 | { //accummulate histogram uniformly 327 | Mat Sum = cumsum(Histogram); 328 | // scale it from min to max 329 | output = min(Min + Sum*Scale, Max); //limit range to Max 330 | break; 331 | } 332 | case HEQ_EXPONENTIAL: 333 | { //accumulate histogram exponentially 334 | Mat Sum = cumsum(Histogram); 335 | double vmax = 1.0 - exp(-heq_alpha); 336 | Mat dst; 337 | log(1-(vmax*Sum/NrofPixels), dst); 338 | Mat temp = (-1/heq_alpha)*dst; 339 | output = min(temp*Max,Max); //limit range to Max 340 | break; 341 | } 342 | case HEQ_RAYLEIGH: 343 | { //accumulate histogram using rayleigh 344 | Mat Sum = cumsum(Histogram); 345 | double hconst = 2*heq_alpha*heq_alpha; 346 | double vmax = 1 - exp((-1/hconst)); 347 | Mat dst; 348 | log(1-vmax*(Sum/(double)NrofPixels), dst); 349 | Mat temp; 350 | sqrt(-hconst*dst, temp); 351 | output = min(temp*Max,Max); //limit range to Max 352 | break; 353 | } 354 | default: 355 | { //just do UNIFORM if heq_type has a wacky value 356 | Mat Sum = cumsum(Histogram); 357 | output = min(Min + Sum*Scale,Max); //limit range to Max 358 | break; 359 | } 360 | } 361 | 362 | return output; 363 | } 364 | 365 | 366 | Mat Clahs::Interpolate(const Mat& SubRegion, 367 | const Mat& MapLU, 368 | const Mat& MapRU, 369 | const Mat& MapLB, 370 | const Mat& MapRB, 371 | const int& XSize, 372 | const int& YSize, 373 | const Mat& LUT) { 374 | 375 | int Num = XSize * YSize; //Normalization factor 376 | Mat BinValues = buildbyIndices(LUT, SubRegion); 377 | 378 | Mat XInvCoef; 379 | int dec1 = XSize; 380 | Mat row1 = Mat::zeros(1,XSize,CV_64FC1); 381 | for (uint i=0; i(0,i) = dec1; 383 | dec1--; 384 | } 385 | for (uint j=0; j(i,j) = dec2; 394 | } 395 | dec2--; 396 | } 397 | 398 | Mat XCoef; 399 | int inc1 = 0; 400 | Mat row2 = Mat::zeros(1,XSize,CV_64FC1); 401 | for (uint i=0; i(0,i) = inc1; 403 | inc1++; 404 | } 405 | for (uint j=0; j(i,j) = inc2; 414 | } 415 | inc2++; 416 | } 417 | 418 | Mat MapLU_red = buildbyIndices(MapLU, BinValues); 419 | Mat MapRU_red = buildbyIndices(MapRU, BinValues); 420 | Mat MapLB_red = buildbyIndices(MapLB, BinValues); 421 | Mat MapRB_red = buildbyIndices(MapRB, BinValues); 422 | 423 | Mat InterpRegion = Mat::zeros(MapLU_red.rows,MapLU_red.cols,CV_64FC1); 424 | for (uint i=0; i(i,j) = ( YInvCoef.at(i,j) * ( XInvCoef.at(i,j)*MapLU_red.at(i,j) + XCoef.at(i,j)*MapRU_red.at(i,j) ) + 427 | YCoef.at(i,j) * ( XInvCoef.at(i,j)*MapLB_red.at(i,j) + XCoef.at(i,j)*MapRB_red.at(i,j) ) ) / Num; 428 | } 429 | } 430 | 431 | return InterpRegion; 432 | } 433 | 434 | Mat Clahs::buildbyIndices(const Mat& vector, const Mat& indices) { 435 | Mat output = Mat::zeros(indices.rows,indices.cols,CV_64FC1); 436 | for (int i=0; i(i,j) + 1; 439 | output.at(i,j) = vector.at(idx,0); 440 | } 441 | } 442 | return output; 443 | } 444 | 445 | Mat Clahs::linspace(const double& startP, 446 | const double& Endp, 447 | const int& interval) { 448 | double spacing = interval-1; 449 | Mat y(spacing,1,CV_64FC1); 450 | for (int i = 0; i < y.rows; ++i) { 451 | y.at(i) = startP + i*(Endp - startP)/spacing; 452 | } 453 | return y; 454 | } 455 | 456 | Mat Clahs::cumsum(const Mat& in) { 457 | Mat output = Mat::zeros(in.rows,1,in.type()); 458 | for (uint i=0; i0) { 460 | output.at(i,0) = output.at(i-1,0) + in.at(i,0); 461 | } else { 462 | output.at(i,0) = in.at(i,0); 463 | } 464 | } 465 | return output; 466 | } -------------------------------------------------------------------------------- /src/color_correction.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2016 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | 8 | #include 9 | 10 | using namespace cv; 11 | using namespace std; 12 | 13 | ColorCorrection::ColorCorrection(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), it_(nh) { 14 | camera_sub_ = it_.subscribeCamera("image", 15 | 1, // queue size 16 | &ColorCorrection::imageCallback, 17 | this); // transport 18 | 19 | string output_namespace; 20 | nhp_.param("output_namespace", output_namespace, string("/cc")); 21 | nhp_.param("gamma_r", gamma_r, 1.0); 22 | nhp_.param("gamma_g", gamma_g, 0.7); 23 | nhp_.param("gamma_b", gamma_b, 0.7); 24 | nhp_.param("max_r", max_r, 127); 25 | nhp_.param("max_b", max_b, 255); 26 | nhp_.param("max_g", max_g, 255); 27 | 28 | img_pub_ = it_.advertiseCamera( 29 | ros::names::clean(output_namespace + "/image_rect_color"), 1); 30 | } 31 | 32 | cv::Mat ColorCorrection::adjustLevelsAndGamma(const cv::Mat& channel, double gamma, double max_value) { 33 | cv::Mat output = channel; 34 | size_t i, j; 35 | #pragma omp parallel for collapse(2) 36 | for (i = 0; i < channel.rows; i++) { 37 | for (j = 0; j < channel.cols; j++) { 38 | double v = channel.at(i,j); 39 | double gain = max_value / 255.0; 40 | unsigned char ov = static_cast(gain*pow(v, gamma)); 41 | output.at(i, j) = ov; 42 | } 43 | } 44 | } 45 | 46 | void ColorCorrection::imageCallback( 47 | const sensor_msgs::ImageConstPtr &image_msg, 48 | const sensor_msgs::CameraInfoConstPtr &info_msg) { 49 | cv_bridge::CvImagePtr cv_image_ptr; 50 | 51 | try { 52 | cv_image_ptr = cv_bridge::toCvCopy(image_msg, 53 | sensor_msgs::image_encodings::BGR8); 54 | } catch (cv_bridge::Exception& e) { 55 | ROS_ERROR("cv_bridge exception: %s", e.what()); 56 | return; 57 | } 58 | 59 | // Compute 60 | Mat out; 61 | Mat img = cv_image_ptr->image; 62 | 63 | // Check number of channels 64 | if (img.channels() != 3) { 65 | ROS_WARN_STREAM("[PreProcessing]: Invalid number of channels: " << img.channels()); 66 | return; 67 | } 68 | 69 | // Split the image in channels 70 | std::vector channels(3); 71 | std::vector output_channels(3); 72 | cv::split(img, channels); 73 | 74 | // Apply gamma correction 75 | output_channels[0] = adjustLevelsAndGamma(channels[0], gamma_b, max_b); 76 | output_channels[1] = adjustLevelsAndGamma(channels[1], gamma_g, max_g); 77 | output_channels[2] = adjustLevelsAndGamma(channels[2], gamma_r, max_r); 78 | cv::merge(output_channels, out); 79 | 80 | // convert OpenCV image to ROS message 81 | cv_bridge::CvImage mono_cvi; 82 | mono_cvi.header.stamp = image_msg->header.stamp; 83 | mono_cvi.header.frame_id = image_msg->header.frame_id; 84 | mono_cvi.encoding = "bgr8"; 85 | mono_cvi.image = out; 86 | sensor_msgs::Image mono_im; 87 | mono_cvi.toImageMsg(mono_im); 88 | img_pub_.publish(mono_im, *info_msg); 89 | } 90 | -------------------------------------------------------------------------------- /src/dehazer.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | 8 | using namespace cv; 9 | 10 | Mat Dehazer::dehazeRGB(Mat img) { 11 | int local_window_radius = 16; 12 | double eps = 0.01; 13 | 14 | Mat img_float; 15 | img.convertTo(img_float, CV_32F, 1/255.0); 16 | 17 | std::vector channels(3); 18 | split(img_float, channels); 19 | 20 | Mat qr, qg, qb; 21 | boost::thread thread1( boost::bind( &Dehazer::guidedFilter, this, channels[2], channels[2], boost::ref(qr), local_window_radius, eps ) ); 22 | boost::thread thread2( boost::bind( &Dehazer::guidedFilter, this, channels[1], channels[1], boost::ref(qg), local_window_radius, eps ) ); 23 | boost::thread thread3( boost::bind( &Dehazer::guidedFilter, this, channels[0], channels[0], boost::ref(qb), local_window_radius, eps ) ); 24 | thread1.join(); 25 | thread2.join(); 26 | thread3.join(); 27 | 28 | Mat deh, deh_float; 29 | Mat q[] = {qb, qg, qr}; 30 | merge(q, 3, deh_float); 31 | 32 | Mat result = (img_float - deh_float) * 5 + deh_float; 33 | result.convertTo(deh, CV_8U, 255.0); 34 | return deh; 35 | } 36 | 37 | Mat Dehazer::dehazeGrayscale(Mat img) { 38 | int local_window_radius = 16; 39 | double eps = 0.01; 40 | 41 | Mat img_float; 42 | img.convertTo(img_float, CV_32F, 1/255.0); 43 | 44 | Mat filt; 45 | guidedFilter(img_float, img_float, filt, local_window_radius, eps); 46 | 47 | Mat deh; 48 | Mat result = (img_float - filt) * 5 + filt; 49 | result.convertTo(deh, CV_8U, 255.0); 50 | return deh; 51 | } 52 | 53 | void Dehazer::guidedFilter(Mat img, // p 54 | Mat guidance_img, // I 55 | Mat& out, 56 | int local_window_radius, // r 57 | double eps) { 58 | // [hei, wid] = size(I); 59 | int h = guidance_img.rows; 60 | int w = guidance_img.cols; 61 | Mat temp1; 62 | 63 | Size size_r(2*local_window_radius+1, 2*local_window_radius+1); 64 | 65 | // N = boxfilter(ones(hei, wid), r); 66 | // % the size of each local patch; N=(2r+1)^2 except for boundary pixels. 67 | Mat N; 68 | boxFilter(Mat::ones(h, w, CV_32FC1), N, -1, size_r); 69 | 70 | // mean_I = boxfilter(I, r) ./ N; 71 | Mat mean_guidance; 72 | boxFilter(guidance_img, temp1, -1, size_r); 73 | divide(temp1, N, mean_guidance); 74 | // mean_p = boxfilter(p, r) ./ N; 75 | Mat mean_img; 76 | boxFilter(img, temp1, -1, size_r); 77 | divide(temp1, N, mean_img); 78 | // mean_Ip = boxfilter(I.*p, r) ./ N; 79 | Mat mean_product; 80 | boxFilter(guidance_img.mul(img), temp1, -1, size_r); 81 | divide(temp1, N, mean_product); 82 | // cov_Ip = mean_Ip - mean_I .* mean_p; 83 | // % this is the covariance of (I, p) in each local patch. 84 | Mat cov_product = mean_product - mean_guidance.mul(mean_img); 85 | // mean_II = boxfilter(I.*I, r) ./ N; 86 | Mat mean_guidance_sqr; 87 | boxFilter(guidance_img.mul(guidance_img), temp1, -1, size_r); 88 | divide(temp1, N, mean_guidance_sqr); 89 | // var_I = mean_II - mean_I .* mean_I; 90 | Mat var_guidance = mean_guidance_sqr - mean_guidance.mul(mean_guidance); 91 | 92 | // a = cov_Ip ./ (var_I + eps); % Eqn. (5) in the paper; 93 | Mat a = cov_product / (var_guidance + eps); 94 | // b = mean_p - a .* mean_I; % Eqn. (6) in the paper; 95 | Mat b = mean_img - a.mul(mean_guidance); 96 | 97 | // mean_a = boxfilter(a, r) ./ N; 98 | Mat mean_a; 99 | boxFilter(a, temp1, -1, size_r); 100 | divide(temp1, N, mean_a); 101 | // mean_b = boxfilter(b, r) ./ N; 102 | Mat mean_b; 103 | boxFilter(b, temp1, -1, size_r); 104 | divide(temp1, N, mean_b); 105 | 106 | // q = mean_a .* I + mean_b; % Eqn. (8) in the paper; 107 | Mat q = mean_a.mul(guidance_img) + mean_b; 108 | q.copyTo(out); 109 | } 110 | -------------------------------------------------------------------------------- /src/flicker_remover.cpp: -------------------------------------------------------------------------------- 1 | #include "image_preprocessing/flicker_remover.h" 2 | 3 | FlickerRemover::FlickerRemover(ros::NodeHandle nh, ros::NodeHandle nhp) : nh_(nh), nhp_(nhp), frame_counter_(0) {} 4 | 5 | void FlickerRemover::run() 6 | { 7 | // Read parameters 8 | string camera_topic; 9 | nhp_.param("camera_topic", camera_topic, string("")); 10 | nhp_.param("desc_thresh", desc_thresh_, string("")); 11 | nhp_.param("epipolar_thresh", epipolar_thresh_, string("")); 12 | nhp_.param("min_inliers", min_inliers_, string("")); 13 | 14 | 15 | // Message sync 16 | boost::shared_ptr sync; 17 | image_transport::ImageTransport it(nh_); 18 | image_transport::SubscriberFilter left_sub, right_sub; 19 | message_filters::Subscriber left_info_sub, right_info_sub; 20 | left_sub .subscribe(it, camera_topic+"/left/image_rect_color", 5); 21 | right_sub .subscribe(it, camera_topic+"/right/image_rect_color", 5); 22 | left_info_sub .subscribe(nh_, camera_topic+"/left/camera_info", 5); 23 | right_info_sub.subscribe(nh_, camera_topic+"/right/camera_info", 5); 24 | sync.reset(new Sync(SyncPolicy(10), left_sub, right_sub, left_info_sub, right_info_sub) ); 25 | sync->registerCallback(bind(&FlickerRemover::msgsCallback, this, _1, _2, _3, _4)); 26 | 27 | ros::spin(); 28 | } 29 | 30 | void FlickerRemover::msgsCallback( 31 | const sensor_msgs::ImageConstPtr& l_img_msg, 32 | const sensor_msgs::ImageConstPtr& r_img_msg, 33 | const sensor_msgs::CameraInfoConstPtr& l_info_msg, 34 | const sensor_msgs::CameraInfoConstPtr& r_info_msg) 35 | { 36 | // Convert images to opencv 37 | cv::Mat l_img, r_img; 38 | bool convert_valid = imgMsgToMat(*l_img_msg, *r_img_msg, l_img, r_img); 39 | if (!convert_valid) 40 | { 41 | ROS_WARN("[FlickerRemover:] Impossible to convert images to opencv"); 42 | return; 43 | } 44 | 45 | // Proceed depending if system is initilized or not 46 | if (frame_counter_ == 0) 47 | { 48 | prev_l_ = l_img; 49 | prev_r_ = r_img; 50 | frame_counter_++; 51 | return; 52 | } 53 | else if (frame_counter_ == 1) 54 | { 55 | curr_l_ = l_img; 56 | curr_r_ = r_img; 57 | frame_counter_++; 58 | return; 59 | } 60 | else 61 | { 62 | next_l_ = l_img; 63 | next_r_ = r_img; 64 | frame_counter_++; 65 | 66 | // Register the set of images 67 | registerImages(); 68 | 69 | 70 | // Update 71 | prev_l_ = curr_l_; 72 | prev_r_ = curr_r_; 73 | curr_l_ = next_l_; 74 | curr_r_ = next_r_; 75 | } 76 | } 77 | 78 | void FlickerRemover::registerImages() 79 | { 80 | vector next_l_kp, next_r_kp; 81 | vector curr_l_kp, curr_r_kp; 82 | vector prev_l_kp, prev_r_kp; 83 | 84 | cv::Mat next_l_desc, next_r_desc; 85 | cv::Mat curr_l_desc, curr_r_desc; 86 | cv::Mat prev_l_desc, prev_r_desc; 87 | 88 | // Extract features 89 | featureExtraction(next_l_, next_l_kp, next_l_desc); 90 | featureExtraction(next_r_, next_r_kp, next_r_desc); 91 | featureExtraction(curr_l_, curr_l_kp, curr_l_desc); 92 | featureExtraction(curr_r_, curr_r_kp, curr_r_desc); 93 | featureExtraction(prev_l_, prev_l_kp, prev_l_desc); 94 | featureExtraction(prev_r_, prev_r_kp, prev_r_desc); 95 | 96 | // Feature matching 97 | vector nl_nr_matches, nl_cl_matches, nl_cr_matches; 98 | vector nl_pl_matches, nl_pr_matches, nr_cl_matches; 99 | vector nr_cr_matches, nr_pl_matches, nr_pr_matches; 100 | vector cl_cr_matches, cl_pl_matches, cl_pr_matches; 101 | vector cr_pl_matches, cr_pr_matches, pl_pr_matches; 102 | crossCheckThresholdMatching(next_l_desc, next_r_desc, 0.8, nl_nr_matches); 103 | crossCheckThresholdMatching(next_l_desc, curr_l_desc, 0.8, nl_cl_matches); 104 | crossCheckThresholdMatching(next_l_desc, curr_r_desc, 0.8, nl_cr_matches); 105 | crossCheckThresholdMatching(next_l_desc, prev_l_desc, 0.8, nl_pl_matches); 106 | crossCheckThresholdMatching(next_l_desc, prev_r_desc, 0.8, nl_pr_matches); 107 | crossCheckThresholdMatching(next_r_desc, curr_l_desc, 0.8, nr_cl_matches); 108 | crossCheckThresholdMatching(next_r_desc, curr_r_desc, 0.8, nr_cr_matches); 109 | crossCheckThresholdMatching(next_r_desc, prev_l_desc, 0.8, nr_pl_matches); 110 | crossCheckThresholdMatching(next_r_desc, prev_r_desc, 0.8, nr_pr_matches); 111 | crossCheckThresholdMatching(curr_l_desc, curr_r_desc, 0.8, cl_cr_matches); 112 | crossCheckThresholdMatching(curr_l_desc, prev_l_desc, 0.8, cl_pl_matches); 113 | crossCheckThresholdMatching(curr_l_desc, prev_r_desc, 0.8, cl_pr_matches); 114 | crossCheckThresholdMatching(curr_r_desc, prev_l_desc, 0.8, cr_pl_matches); 115 | crossCheckThresholdMatching(curr_r_desc, prev_r_desc, 0.8, cr_pr_matches); 116 | crossCheckThresholdMatching(prev_l_desc, prev_r_desc, 0.8, pl_pr_matches); 117 | 118 | // Registration 119 | cv::Mat nl_nr, nl_cl, nl_cr, nl_pl, nl_pr; 120 | cv::Mat nr_cl, nr_cr, nr_pl, nr_pr; 121 | cv::Mat cl_cr, cl_pl, cl_pr; 122 | cv::Mat cr_pl, cr_pr; 123 | cv::Mat pl_pr; 124 | 125 | 126 | ROS_INFO("-------------------------------"); 127 | ROS_INFO("-------------------------------"); 128 | ROS_INFO_STREAM("nl_nr: " << nl_nr_matches.size()); 129 | ROS_INFO_STREAM("nl_cl: " << nl_cl_matches.size()); 130 | ROS_INFO_STREAM("nl_cr: " << nl_cr_matches.size()); 131 | ROS_INFO_STREAM("nl_pl: " << nl_pl_matches.size()); 132 | ROS_INFO_STREAM("nl_pr: " << nl_pr_matches.size()); 133 | ROS_INFO_STREAM("nr_cl: " << nr_cl_matches.size()); 134 | ROS_INFO_STREAM("nr_cr: " << nr_cr_matches.size()); 135 | ROS_INFO_STREAM("nr_pl: " << nr_pl_matches.size()); 136 | ROS_INFO_STREAM("nr_pr: " << nr_pr_matches.size()); 137 | ROS_INFO_STREAM("cl_cr: " << cl_cr_matches.size()); 138 | ROS_INFO_STREAM("cl_pl: " << cl_pl_matches.size()); 139 | ROS_INFO_STREAM("cl_pr: " << cl_pr_matches.size()); 140 | ROS_INFO_STREAM("cr_pl: " << cr_pl_matches.size()); 141 | ROS_INFO_STREAM("cr_pr: " << cr_pr_matches.size()); 142 | ROS_INFO_STREAM("pl_pr: " << pl_pr_matches.size()); 143 | 144 | } 145 | 146 | void FlickerRemover::register(vector matches, ) 147 | { 148 | if (matches.size() >= min_inliers_) 149 | { 150 | vector matched_a; 151 | vector matched_b; 152 | for(int i=0; i& kp, cv::Mat& desc) 162 | { 163 | kp.clear(); 164 | desc.release(); 165 | cv::Ptr orb; 166 | orb = cv::ORB::create(2000, 1.2, 8, 10, 0, 2, cv::ORB::HARRIS_SCORE, 10); 167 | orb->detectAndCompute (img, cv::noArray(), kp, desc); 168 | } 169 | 170 | bool FlickerRemover::imgMsgToMat(sensor_msgs::Image l_img_msg, 171 | sensor_msgs::Image r_img_msg, 172 | cv::Mat &l_img, cv::Mat &r_img) 173 | { 174 | // Convert message to cv::Mat 175 | cv_bridge::CvImagePtr l_img_ptr, r_img_ptr; 176 | try 177 | { 178 | l_img_ptr = cv_bridge::toCvCopy(l_img_msg, enc::BGR8); 179 | r_img_ptr = cv_bridge::toCvCopy(r_img_msg, enc::BGR8); 180 | } 181 | catch (cv_bridge::Exception& e) 182 | { 183 | ROS_ERROR("[FlickerRemover:] cv_bridge exception: %s", e.what()); 184 | return false; 185 | } 186 | 187 | // Set the images 188 | l_img = l_img_ptr->image; 189 | r_img = r_img_ptr->image; 190 | return true; 191 | } 192 | 193 | void FlickerRemover::thresholdMatching(const cv::Mat& descriptors1, const cv::Mat& descriptors2, 194 | double threshold, vector& matches) 195 | { 196 | cv::Mat match_mask; 197 | matches.clear(); 198 | if (descriptors1.empty() || descriptors2.empty()) 199 | return; 200 | assert(descriptors1.type() == descriptors2.type()); 201 | assert(descriptors1.cols == descriptors2.cols); 202 | 203 | const int knn = 2; 204 | cv::Ptr descriptor_matcher; 205 | // choose matcher based on feature type 206 | if (descriptors1.type() == CV_8U) 207 | { 208 | descriptor_matcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 209 | } 210 | else 211 | { 212 | descriptor_matcher = cv::DescriptorMatcher::create("BruteForce"); 213 | } 214 | vector > knn_matches; 215 | descriptor_matcher->knnMatch(descriptors1, descriptors2, 216 | knn_matches, knn, match_mask); 217 | 218 | for (size_t m = 0; m < knn_matches.size(); m++) 219 | { 220 | if (knn_matches[m].size() < 2) continue; 221 | float dist1 = knn_matches[m][0].distance; 222 | float dist2 = knn_matches[m][1].distance; 223 | if (dist1 / dist2 < threshold) 224 | { 225 | matches.push_back(knn_matches[m][0]); 226 | } 227 | } 228 | } 229 | 230 | void FlickerRemover::crossCheckFilter( 231 | const vector& matches1to2, 232 | const vector& matches2to1, 233 | vector& checked_matches) 234 | { 235 | checked_matches.clear(); 236 | for (size_t i = 0; i < matches1to2.size(); ++i) 237 | { 238 | bool match_found = false; 239 | const cv::DMatch& forward_match = matches1to2[i]; 240 | for (size_t j = 0; j < matches2to1.size() && match_found == false; ++j) 241 | { 242 | const cv::DMatch& backward_match = matches2to1[j]; 243 | if (forward_match.trainIdx == backward_match.queryIdx && 244 | forward_match.queryIdx == backward_match.trainIdx) 245 | { 246 | checked_matches.push_back(forward_match); 247 | match_found = true; 248 | } 249 | } 250 | } 251 | } 252 | 253 | void FlickerRemover::crossCheckThresholdMatching( 254 | const cv::Mat& descriptors1, const cv::Mat& descriptors2, 255 | double threshold, const cv::Mat& match_mask, 256 | vector& matches) 257 | { 258 | vector query_to_train_matches; 259 | thresholdMatching(descriptors1, descriptors2, threshold, match_mask, query_to_train_matches); 260 | vector train_to_query_matches; 261 | cv::Mat match_mask_t; 262 | if (!match_mask.empty()) match_mask_t = match_mask.t(); 263 | thresholdMatching(descriptors2, descriptors1, threshold, match_mask_t, train_to_query_matches); 264 | 265 | crossCheckFilter(query_to_train_matches, train_to_query_matches, matches); 266 | } -------------------------------------------------------------------------------- /src/mono_clahs.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | 8 | #include 9 | 10 | using namespace cv; 11 | using namespace std; 12 | 13 | MonoClahs::MonoClahs(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), it_(nh) { 14 | camera_sub_ = it_.subscribeCamera("image", 15 | 1, // queue size 16 | &MonoClahs::imageCallback, 17 | this); // transport 18 | 19 | string output_namespace; 20 | nhp_.param("output_namespace", output_namespace, string("/clahs")); 21 | nhp_.param("is_rgb", is_rgb_, true); 22 | 23 | img_pub_ = it_.advertiseCamera( 24 | ros::names::clean(output_namespace + "/image_rect_color"), 1); 25 | } 26 | 27 | void MonoClahs::imageCallback( 28 | const sensor_msgs::ImageConstPtr &image_msg, 29 | const sensor_msgs::CameraInfoConstPtr &info_msg) { 30 | cv_bridge::CvImagePtr cv_image_ptr; 31 | 32 | try { 33 | if (is_rgb_) { 34 | cv_image_ptr = cv_bridge::toCvCopy(image_msg, 35 | sensor_msgs::image_encodings::BGR8); 36 | } else { 37 | cv_image_ptr = cv_bridge::toCvCopy(image_msg, 38 | sensor_msgs::image_encodings::MONO8); 39 | } 40 | } catch (cv_bridge::Exception& e) { 41 | ROS_ERROR("cv_bridge exception: %s", e.what()); 42 | return; 43 | } 44 | 45 | // Compute 46 | Clahs c; 47 | Mat out; 48 | string encoding; 49 | Mat img = cv_image_ptr->image; 50 | if (img.channels() == 1) { 51 | c.clahsGrayscale(img,out,5,4); 52 | encoding = "mono8"; 53 | } 54 | else if (img.channels() == 3) { 55 | c.clahsRGB(img,out,5,4); 56 | encoding = "bgr8"; 57 | } 58 | else { 59 | ROS_WARN_STREAM("[PreProcessing]: Invalid number of channels: " << img.channels()); 60 | return; 61 | } 62 | 63 | // convert OpenCV image to ROS message 64 | cv_bridge::CvImage mono_cvi; 65 | mono_cvi.header.stamp = image_msg->header.stamp; 66 | mono_cvi.header.frame_id = image_msg->header.frame_id; 67 | mono_cvi.encoding = encoding; 68 | mono_cvi.image = out; 69 | sensor_msgs::Image mono_im; 70 | mono_cvi.toImageMsg(mono_im); 71 | img_pub_.publish(mono_im, *info_msg); 72 | } 73 | -------------------------------------------------------------------------------- /src/mono_dehazer.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | 8 | #include 9 | 10 | using namespace cv; 11 | using namespace std; 12 | 13 | MonoDehazer::MonoDehazer(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh), nhp_(nhp), it_(nh) { 14 | camera_sub_ = it_.subscribeCamera("image", 15 | 1, // queue size 16 | &MonoDehazer::imageCallback, 17 | this); // transport 18 | 19 | string output_namespace; 20 | nhp_.param("output_namespace", output_namespace, string("/dehazed")); 21 | nhp_.param("is_rgb", is_rgb_, true); 22 | 23 | img_pub_ = it_.advertiseCamera( 24 | ros::names::clean(output_namespace + "/image_rect_color"), 1); 25 | } 26 | 27 | void MonoDehazer::imageCallback( 28 | const sensor_msgs::ImageConstPtr &image_msg, 29 | const sensor_msgs::CameraInfoConstPtr &info_msg) { 30 | cv_bridge::CvImagePtr cv_image_ptr; 31 | 32 | try { 33 | if (is_rgb_) { 34 | cv_image_ptr = cv_bridge::toCvCopy(image_msg, 35 | sensor_msgs::image_encodings::BGR8); 36 | } else { 37 | cv_image_ptr = cv_bridge::toCvCopy(image_msg, 38 | sensor_msgs::image_encodings::MONO8); 39 | } 40 | } catch (cv_bridge::Exception& e) { 41 | ROS_ERROR("cv_bridge exception: %s", e.what()); 42 | return; 43 | } 44 | 45 | // Compute 46 | Dehazer d; 47 | Mat out; 48 | string encoding; 49 | Mat img = cv_image_ptr->image; 50 | if (img.channels() == 1) { 51 | out = d.dehazeGrayscale(img); 52 | encoding = "mono8"; 53 | } 54 | else if (img.channels() == 3) { 55 | out = d.dehazeRGB(img); 56 | encoding = "bgr8"; 57 | } 58 | else { 59 | ROS_WARN_STREAM("[PreProcessing]: Invalid number of channels: " << img.channels()); 60 | return; 61 | } 62 | 63 | // convert OpenCV image to ROS message 64 | cv_bridge::CvImage mono_cvi; 65 | mono_cvi.header.stamp = image_msg->header.stamp; 66 | mono_cvi.header.frame_id = image_msg->header.frame_id; 67 | mono_cvi.encoding = encoding; 68 | mono_cvi.image = out; 69 | sensor_msgs::Image mono_im; 70 | mono_cvi.toImageMsg(mono_im); 71 | img_pub_.publish(mono_im, *info_msg); 72 | } 73 | -------------------------------------------------------------------------------- /src/nodes/clahs_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) { 10 | ros::init(argc, argv, "clahs"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nhp("~"); 14 | 15 | MonoClahs c(nh, nhp); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/nodes/color_correction_image_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2015 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | using namespace std; 16 | using namespace cv; 17 | 18 | 19 | image_transport::Subscriber img_sub_; 20 | image_transport::Publisher img_pub_; 21 | 22 | void cv_clahe(cv::Mat _src, cv::Mat& _dst, int gain) { 23 | cv::Ptr clahe = cv::createCLAHE(); 24 | clahe->setClipLimit(gain); 25 | clahe->apply(_src, _dst); 26 | return; 27 | } 28 | 29 | void imageCallback(const sensor_msgs::ImageConstPtr &image_msg) { 30 | 31 | // Exit if no subscribers 32 | if (img_pub_.getNumSubscribers() == 0) return; 33 | 34 | cv_bridge::CvImagePtr img_ptr; 35 | try { 36 | img_ptr = cv_bridge::toCvCopy(image_msg, 37 | sensor_msgs::image_encodings::BGR8); 38 | } catch (cv_bridge::Exception& e) { 39 | ROS_ERROR("[ColorCorrectionImage:] cv_bridge exception: %s", e.what()); 40 | } 41 | cv::Mat img = img_ptr->image; 42 | 43 | // Split channels 44 | std::vector BGR; 45 | cv::split(img, BGR); 46 | 47 | // Equalizes the histogram of a one channel image (8UC1) using Contrast 48 | // Limited Adaptive Histogram Equalization. 49 | std::thread thread_b(cv_clahe, BGR[0], std::ref(BGR[0]),2); 50 | std::thread thread_g(cv_clahe, BGR[1], std::ref(BGR[1]),2); 51 | std::thread thread_r(cv_clahe, BGR[2], std::ref(BGR[2]),2); 52 | thread_b.join(); 53 | thread_g.join(); 54 | thread_r.join(); 55 | 56 | cv::Mat img_out; 57 | cv::merge(BGR, img_out); 58 | 59 | img_ptr->image = img_out; 60 | img_pub_.publish(img_ptr->toImageMsg()); 61 | } 62 | 63 | void colorCorrection(ros::NodeHandle nh, ros::NodeHandle nhp) { 64 | // Prepare publisher 65 | image_transport::ImageTransport it(nh); 66 | img_pub_ = it.advertise("/color_correction_image/output", 1); 67 | 68 | // Subscribe to images 69 | img_sub_ = it.subscribe("/color_correction_image/input", 1, &imageCallback); 70 | } 71 | 72 | int main(int argc, char **argv) { 73 | ros::init(argc, argv, "color_correction_image"); 74 | ros::NodeHandle nh; 75 | ros::NodeHandle nhp("~"); 76 | colorCorrection(nh, nhp); 77 | ros::spin(); 78 | } 79 | -------------------------------------------------------------------------------- /src/nodes/color_correction_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) { 10 | ros::init(argc, argv, "color_correction"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nhp("~"); 14 | 15 | ColorCorrection d(nh, nhp); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/nodes/dehazer_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) { 10 | ros::init(argc, argv, "dehazer"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nhp("~"); 14 | 15 | MonoDehazer d(nh, nhp); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/nodes/flicker_remover_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | ros::init(argc, argv, "clahs"); 7 | ros::start(); 8 | 9 | ros::NodeHandle nh; 10 | ros::NodeHandle nhp("~"); 11 | 12 | FlickerRemover fr(nh, nhp); 13 | boost::thread trackingThread(&FlickerRemover::run, &fr); 14 | 15 | // ROS spin 16 | ros::Rate r(10); 17 | while (ros::ok()) 18 | { 19 | r.sleep(); 20 | } 21 | 22 | ros::shutdown(); 23 | 24 | return 0; 25 | } 26 | -------------------------------------------------------------------------------- /src/nodes/stereo_clahs_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) { 10 | ros::init(argc, argv, "stereo_clahs"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nhp("~"); 14 | 15 | StereoClahs c(nh, nhp); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/nodes/stereo_dehazer_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | 9 | int main(int argc, char** argv) { 10 | ros::init(argc, argv, "stereo_dehazer"); 11 | 12 | ros::NodeHandle nh; 13 | ros::NodeHandle nhp("~"); 14 | 15 | StereoDehazer d(nh, nhp); 16 | 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/nodes/vignetting_extractor_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2015 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | using namespace std; 16 | using namespace cv; 17 | 18 | Mat acc_x; 19 | Mat acc_xx; 20 | bool first = true; 21 | double image_counter = 0.0; 22 | // Stop handler binding 23 | boost::function stopHandlerCb; 24 | 25 | void imageCallback(const sensor_msgs::ImageConstPtr& msg) 26 | { 27 | try 28 | { 29 | Mat img = cv_bridge::toCvShare(msg, "bgr8")->image; 30 | Mat imgd; 31 | img.convertTo(imgd, CV_64FC3); 32 | if (first) 33 | { 34 | ROS_INFO("Images received correctly."); 35 | first = false; 36 | acc_x = Mat::zeros(img.size(), CV_64FC3); 37 | acc_xx = Mat::zeros(img.size(), CV_64FC3); 38 | } 39 | acc_x = acc_x + imgd; 40 | multiply(imgd, imgd, imgd); 41 | acc_xx = acc_xx + imgd; 42 | image_counter++; 43 | } 44 | catch (cv_bridge::Exception& e) 45 | { 46 | ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); 47 | } 48 | } 49 | 50 | /** \brief Catches the Ctrl+C signal. 51 | */ 52 | void stopHandler(int s) 53 | { 54 | printf("Caught signal %d\n",s); 55 | stopHandlerCb(s); 56 | ros::shutdown(); 57 | } 58 | 59 | void finalize(int s) 60 | { 61 | ROS_INFO("Finalizing..."); 62 | if (image_counter > 0) 63 | { 64 | acc_x = acc_x / image_counter; 65 | imwrite("vignetting_mean.png", acc_x); 66 | multiply(acc_x, acc_x, acc_x); 67 | sqrt(acc_xx / image_counter - acc_x, acc_xx); 68 | imwrite("vignetting_std.png", acc_xx); 69 | ROS_INFO("Image saved!"); 70 | } 71 | ROS_INFO("Done!"); 72 | } 73 | 74 | int main(int argc, char **argv) 75 | { 76 | ros::init(argc, argv, "vignetting_extractor"); 77 | ros::NodeHandle nh; 78 | 79 | // Bind the finalize member to stopHandler signal 80 | stopHandlerCb = &finalize; 81 | // Setup the signal handler 82 | struct sigaction sigIntHandler; 83 | sigIntHandler.sa_handler = stopHandler; 84 | sigemptyset(&sigIntHandler.sa_mask); 85 | sigIntHandler.sa_flags = 0; 86 | sigaction(SIGINT, &sigIntHandler, NULL); 87 | 88 | image_transport::ImageTransport it(nh); 89 | image_transport::Subscriber sub = it.subscribe("image", 1, imageCallback); 90 | ros::spin(); 91 | } 92 | -------------------------------------------------------------------------------- /src/nodes/vignetting_remover_node.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2015 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace std; 14 | using namespace cv; 15 | 16 | Mat gain_; 17 | Mat offset_; 18 | Mat avg_; 19 | image_transport::CameraSubscriber img_sub_; 20 | image_transport::CameraPublisher img_pub_; 21 | 22 | void imageCallback(const sensor_msgs::ImageConstPtr &image_msg, 23 | const sensor_msgs::CameraInfoConstPtr &info_msg) { 24 | 25 | // Exit if no subscribers 26 | if (img_pub_.getNumSubscribers() == 0) return; 27 | 28 | const cv::Mat img(image_msg->height, image_msg->width, CV_8UC1, 29 | const_cast(&image_msg->data[0]), image_msg->step); 30 | 31 | // Convert image to double 32 | Mat img_d(img.size(), CV_64FC1); 33 | img.convertTo(img_d, CV_64F, 1/255.0); 34 | 35 | // Correct 36 | Mat corrected; 37 | multiply(img_d - avg_, gain_, corrected); 38 | corrected = corrected + avg_; // + offset_; 39 | 40 | Mat corrected_uchar; 41 | corrected = corrected * 255.0; 42 | corrected.convertTo(corrected_uchar, CV_8U); 43 | 44 | // Publish 45 | sensor_msgs::ImagePtr out_msg = cv_bridge::CvImage(image_msg->header, 46 | image_msg->encoding, 47 | corrected_uchar).toImageMsg(); 48 | img_pub_.publish(out_msg, info_msg); 49 | } 50 | 51 | void vignettingRemover(ros::NodeHandle nh, ros::NodeHandle nhp) { 52 | // Read the images 53 | string mean_filename, std_filename; 54 | nhp.param("mean_filename", mean_filename, string("")); 55 | nhp.param("std_filename", std_filename, string("")); 56 | ROS_INFO_STREAM("Reading mean image: " << mean_filename.c_str() << 57 | " and std image: " << std_filename.c_str()); 58 | Mat mean_uchar = imread(mean_filename, CV_LOAD_IMAGE_GRAYSCALE); 59 | Mat std_uchar = imread(std_filename, CV_LOAD_IMAGE_GRAYSCALE); 60 | 61 | // Convert them to double 62 | Mat mean_d(mean_uchar.size(), CV_64FC1); 63 | Mat std_d(std_uchar.size(), CV_64FC1); 64 | mean_uchar.convertTo(mean_d, CV_64F, 1/255.0); 65 | Scalar uc = mean_uchar.at(100, 100); 66 | Scalar db = mean_d.at(100, 100); 67 | std::cout << "Values: " << uc[0] << " and " << db[0]; 68 | std_uchar.convertTo(std_d, CV_64F, 1/255.0); 69 | avg_ = mean_d; 70 | 71 | // Compute gain and offset for posterior correction 72 | Mat desired_avg = 0.05*Mat::ones(mean_d.size(), CV_64FC1); 73 | double desired_sigma = 1.0/6; 74 | divide(Mat::ones(std_d.size(), CV_64FC1), std_d, gain_, desired_sigma); 75 | offset_ = desired_avg - mean_d; 76 | ROS_INFO_STREAM("Gain and offset ready!"); 77 | 78 | // Prepare publisher 79 | string output_camera_namespace; 80 | string image_topic = ros::names::remap("image"); 81 | image_transport::ImageTransport it(nh); 82 | nhp.param("output_namespace", output_camera_namespace, string("vig")); 83 | img_pub_ = it.advertiseCamera(output_camera_namespace+"/image_raw", 1); 84 | 85 | // Subscribe to images 86 | ROS_INFO_STREAM("Subscribing to " << image_topic); 87 | img_sub_ = it.subscribeCamera(image_topic, 1, &imageCallback); 88 | } 89 | 90 | int main(int argc, char **argv) { 91 | ros::init(argc, argv, "vignetting_remover"); 92 | ros::NodeHandle nh; 93 | ros::NodeHandle nhp("~"); 94 | vignettingRemover(nh, nhp); 95 | ros::spin(); 96 | } 97 | -------------------------------------------------------------------------------- /src/stereo_clahs.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | using namespace cv; 12 | using namespace sensor_msgs; 13 | 14 | StereoClahs::StereoClahs(ros::NodeHandle nh, ros::NodeHandle nhp) 15 | : nh_(nh), nhp_(nhp) { 16 | // Topic parameters 17 | string stereo_ns = nh_.resolveName("stereo"); 18 | 19 | nhp_.param("left_mono_topic", left_mono_topic_, 20 | string("/left/image_rect")); 21 | nhp_.param("right_mono_topic", right_mono_topic_, 22 | string("/right/image_rect")); 23 | 24 | nhp_.param("left_color_topic", left_color_topic_, 25 | string("/left/image_rect_color")); 26 | nhp_.param("right_color_topic", right_color_topic_, 27 | string("/right/image_rect_color")); 28 | 29 | nhp_.param("left_info_topic", left_info_topic_, 30 | string("/left/camera_info")); 31 | nhp_.param("right_info_topic", right_info_topic_, 32 | string("/right/camera_info")); 33 | 34 | nhp_.param("cloud_topic", cloud_topic_, 35 | string("/points2")); 36 | 37 | // Topics subscriptions 38 | image_transport::ImageTransport it(nh_); 39 | left_color_sub_.subscribe(it, 40 | ros::names::clean(stereo_ns + left_color_topic_), 3); 41 | right_color_sub_.subscribe(it, 42 | ros::names::clean(stereo_ns + right_color_topic_), 3); 43 | left_color_info_sub_.subscribe(nh_, 44 | ros::names::clean(stereo_ns + left_info_topic_), 3); 45 | right_color_info_sub_.subscribe(nh_, 46 | ros::names::clean(stereo_ns + right_info_topic_), 3); 47 | cloud_color_sub_.subscribe(nh_, 48 | ros::names::clean(stereo_ns + cloud_topic_), 3); 49 | 50 | left_mono_sub_.subscribe(it, 51 | ros::names::clean(stereo_ns + left_mono_topic_), 3); 52 | right_mono_sub_.subscribe(it, 53 | ros::names::clean(stereo_ns + right_mono_topic_), 3); 54 | left_mono_info_sub_.subscribe(nh_, 55 | ros::names::clean(stereo_ns + left_info_topic_), 3); 56 | right_mono_info_sub_.subscribe(nh_, 57 | ros::names::clean(stereo_ns + right_info_topic_), 3); 58 | 59 | sync_color_.reset(new SyncColor(SyncColorPolicy(5), 60 | left_color_sub_, 61 | right_color_sub_, 62 | left_color_info_sub_, 63 | right_color_info_sub_, 64 | cloud_color_sub_) ); 65 | sync_color_->registerCallback(boost::bind( 66 | &StereoClahs::colorCb, 67 | this, _1, _2, _3, _4, _5)); 68 | 69 | sync_mono_.reset(new SyncMono(SyncMonoPolicy(5), 70 | left_mono_sub_, 71 | right_mono_sub_, 72 | left_mono_info_sub_, 73 | right_mono_info_sub_) ); 74 | sync_mono_->registerCallback(boost::bind( 75 | &StereoClahs::monoCb, 76 | this, _1, _2, _3, _4)); 77 | 78 | // Set the image publishers before the streaming 79 | left_mono_pub_ = it.advertiseCamera( 80 | ros::names::clean(stereo_ns + "/clahs/left/image_rect"), 1); 81 | right_mono_pub_ = it.advertiseCamera( 82 | ros::names::clean(stereo_ns + "/clahs/right/image_rect"), 1); 83 | left_color_pub_ = it.advertiseCamera( 84 | ros::names::clean(stereo_ns + "/clahs/left/image_rect_color"), 1); 85 | right_color_pub_ = it.advertiseCamera( 86 | ros::names::clean(stereo_ns + "/clahs/right/image_rect_color"), 1); 87 | 88 | // Create the callback with the clouds 89 | pub_points2_ = nh.advertise< pcl::PointCloud >( 90 | ros::names::clean(stereo_ns + "/clahs/points2"), 1); 91 | 92 | ROS_INFO("Stereo Image Clahs Initialized!"); 93 | } 94 | 95 | /** \brief Stereo callback. This function is called when synchronized 96 | * mono image pairs are received. 97 | * @return 98 | * \param l_img left stereo image message of type sensor_msgs::Image 99 | * \param r_img right stereo image message of type sensor_msgs::Image 100 | * \param l_info left stereo info message of type sensor_msgs::CameraInfo 101 | * \param r_info right stereo info message of type sensor_msgs::CameraInfo 102 | */ 103 | void StereoClahs::monoCb(const ImageConstPtr& l_img_msg, 104 | const ImageConstPtr& r_img_msg, 105 | const CameraInfoConstPtr& l_info_msg, 106 | const CameraInfoConstPtr& r_info_msg) { 107 | if (left_mono_pub_.getNumSubscribers() > 0) { 108 | Mat l_img_mono = cv_bridge::toCvShare(l_img_msg, 109 | image_encodings::MONO8)->image; 110 | Mat clahs_mono_left; 111 | c_.clahsGrayscale(l_img_mono,clahs_mono_left,5,4); 112 | // convert OpenCV image to ROS message 113 | cv_bridge::CvImage mono_left_cvi; 114 | mono_left_cvi.header.stamp = l_img_msg->header.stamp; 115 | mono_left_cvi.header.frame_id = l_img_msg->header.frame_id; 116 | mono_left_cvi.encoding = "mono8"; 117 | mono_left_cvi.image = clahs_mono_left; 118 | sensor_msgs::Image mono_left_im; 119 | mono_left_cvi.toImageMsg(mono_left_im); 120 | left_mono_pub_.publish(mono_left_im, *l_info_msg); 121 | } 122 | if (right_mono_pub_.getNumSubscribers() > 0) { 123 | Mat r_img_mono = cv_bridge::toCvShare(r_img_msg, 124 | image_encodings::MONO8)->image; 125 | Mat clahs_mono_right; 126 | c_.clahsGrayscale(r_img_mono,clahs_mono_right,5,4); 127 | // convert OpenCV image to ROS message 128 | cv_bridge::CvImage mono_right_cvi; 129 | mono_right_cvi.header.stamp = r_img_msg->header.stamp; 130 | mono_right_cvi.header.frame_id = r_img_msg->header.frame_id; 131 | mono_right_cvi.encoding = "mono8"; 132 | mono_right_cvi.image = clahs_mono_right; 133 | sensor_msgs::Image mono_right_im; 134 | mono_right_cvi.toImageMsg(mono_right_im); 135 | right_mono_pub_.publish(mono_right_im, *r_info_msg); 136 | } 137 | } 138 | 139 | 140 | void StereoClahs::colorCb(const ImageConstPtr& l_img_msg, 141 | const ImageConstPtr& r_img_msg, 142 | const CameraInfoConstPtr& l_info_msg, 143 | const CameraInfoConstPtr& r_info_msg, 144 | const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { 145 | 146 | // Left image must be clahs if exists a subscription to points2 147 | Mat l_img_color; 148 | Mat clahs_color_left; 149 | if (left_color_pub_.getNumSubscribers() > 0 || pub_points2_.getNumSubscribers() > 0) { 150 | l_img_color = cv_bridge::toCvShare(l_img_msg, 151 | image_encodings::BGR8)->image; 152 | c_.clahsRGB(l_img_color,clahs_color_left,5,4); 153 | // convert OpenCV image to ROS message 154 | cv_bridge::CvImage color_left_cvi; 155 | color_left_cvi.header.stamp = l_img_msg->header.stamp; 156 | color_left_cvi.header.frame_id = l_img_msg->header.frame_id; 157 | color_left_cvi.encoding = "bgr8"; 158 | color_left_cvi.image = clahs_color_left; 159 | sensor_msgs::Image color_left_im; 160 | color_left_cvi.toImageMsg(color_left_im); 161 | left_color_pub_.publish(color_left_im, *l_info_msg); 162 | } 163 | if (right_color_pub_.getNumSubscribers() > 0) { 164 | Mat r_img_color = cv_bridge::toCvShare(r_img_msg, 165 | image_encodings::BGR8)->image; 166 | Mat clahs_color_right; 167 | c_.clahsRGB(r_img_color,clahs_color_right,5,4); 168 | // convert OpenCV image to ROS message 169 | cv_bridge::CvImage color_right_cvi; 170 | color_right_cvi.header.stamp = r_img_msg->header.stamp; 171 | color_right_cvi.header.frame_id = r_img_msg->header.frame_id; 172 | color_right_cvi.encoding = "bgr8"; 173 | color_right_cvi.image = clahs_color_right; 174 | sensor_msgs::Image color_right_im; 175 | color_right_cvi.toImageMsg(color_right_im); 176 | right_color_pub_.publish(color_right_im, *r_info_msg); 177 | } 178 | 179 | // Update the camera model 180 | if (pub_points2_.getNumSubscribers() > 0) { 181 | image_geometry::PinholeCameraModel left_cam; 182 | left_cam.fromCameraInfo(l_info_msg); 183 | 184 | // Split clahs image in channels 185 | std::vector channels(3); 186 | split(clahs_color_left, channels); 187 | Mat r, g, b; 188 | channels[0].convertTo(b,CV_8U); 189 | channels[1].convertTo(g,CV_8U); 190 | channels[2].convertTo(r,CV_8U); 191 | 192 | std::vector channels2(3); 193 | split(l_img_color, channels2); 194 | Mat ro, go, bo; 195 | channels2[0].convertTo(bo,CV_8U); 196 | channels2[1].convertTo(go,CV_8U); 197 | channels2[2].convertTo(ro,CV_8U); 198 | 199 | // Convert the points2 to pcl 200 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 201 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 202 | fromROSMsg(*cloud_msg, *cloud); 203 | 204 | // For every point into the pointcloud, get the clahs color from left image 205 | int32_t tmp_rgb; 206 | for (uint i=0; isize(); i++) { 207 | pcl::PointXYZRGB p = cloud->points[i]; 208 | if (isfinite(p.x) && isfinite(p.y) && isfinite(p.z)) { 209 | cv::Point3d xyz(p.x, p.y, p.z); 210 | cv::Point2d pixel = left_cam.project3dToPixel(xyz); 211 | 212 | // Get the color of the corresponding pixel 213 | uint8_t pr = r.at(pixel.y, pixel.x); 214 | uint8_t pg = g.at(pixel.y, pixel.x); 215 | uint8_t pb = b.at(pixel.y, pixel.x); 216 | 217 | int32_t tmp_rgb = (pr << 16) | (pg << 8) | pb; 218 | p.rgb = *reinterpret_cast(&tmp_rgb); 219 | 220 | cloud_out->push_back(p); 221 | } 222 | } 223 | 224 | // Republish the pointcloud 225 | cloud_out->header = pcl_conversions::toPCL(cloud_msg->header); 226 | pub_points2_.publish(cloud_out); 227 | } 228 | } 229 | -------------------------------------------------------------------------------- /src/stereo_dehazer.cpp: -------------------------------------------------------------------------------- 1 | /// Copyright 2014 Miquel Massot Campos 2 | /// Systems, Robotics and Vision 3 | /// University of the Balearic Islands 4 | /// All rights reserved. 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | using namespace cv; 12 | using namespace sensor_msgs; 13 | 14 | StereoDehazer::StereoDehazer(ros::NodeHandle nh, ros::NodeHandle nhp) 15 | : nh_(nh), nhp_(nhp) { 16 | // Topic parameters 17 | string stereo_ns = nh_.resolveName("stereo"); 18 | 19 | nhp_.param("left_mono_topic", left_mono_topic_, 20 | string("/left/image_rect")); 21 | nhp_.param("right_mono_topic", right_mono_topic_, 22 | string("/right/image_rect")); 23 | 24 | nhp_.param("left_color_topic", left_color_topic_, 25 | string("/left/image_rect_color")); 26 | nhp_.param("right_color_topic", right_color_topic_, 27 | string("/right/image_rect_color")); 28 | 29 | nhp_.param("left_info_topic", left_info_topic_, 30 | string("/left/camera_info")); 31 | nhp_.param("right_info_topic", right_info_topic_, 32 | string("/right/camera_info")); 33 | 34 | nhp_.param("cloud_topic", cloud_topic_, 35 | string("/points2")); 36 | 37 | // Topics subscriptions 38 | image_transport::ImageTransport it(nh_); 39 | left_color_sub_.subscribe(it, 40 | ros::names::clean(stereo_ns + left_color_topic_), 3); 41 | right_color_sub_.subscribe(it, 42 | ros::names::clean(stereo_ns + right_color_topic_), 3); 43 | left_color_info_sub_.subscribe(nh_, 44 | ros::names::clean(stereo_ns + left_info_topic_), 3); 45 | right_color_info_sub_.subscribe(nh_, 46 | ros::names::clean(stereo_ns + right_info_topic_), 3); 47 | cloud_color_sub_.subscribe(nh_, 48 | ros::names::clean(stereo_ns + cloud_topic_), 3); 49 | 50 | left_mono_sub_.subscribe(it, 51 | ros::names::clean(stereo_ns + left_mono_topic_), 3); 52 | right_mono_sub_.subscribe(it, 53 | ros::names::clean(stereo_ns + right_mono_topic_), 3); 54 | left_mono_info_sub_.subscribe(nh_, 55 | ros::names::clean(stereo_ns + left_info_topic_), 3); 56 | right_mono_info_sub_.subscribe(nh_, 57 | ros::names::clean(stereo_ns + right_info_topic_), 3); 58 | 59 | sync_color_.reset(new SyncColor(SyncColorPolicy(5), 60 | left_color_sub_, 61 | right_color_sub_, 62 | left_color_info_sub_, 63 | right_color_info_sub_, 64 | cloud_color_sub_) ); 65 | sync_color_->registerCallback(boost::bind( 66 | &StereoDehazer::colorCb, 67 | this, _1, _2, _3, _4, _5)); 68 | 69 | sync_mono_.reset(new SyncMono(SyncMonoPolicy(5), 70 | left_mono_sub_, 71 | right_mono_sub_, 72 | left_mono_info_sub_, 73 | right_mono_info_sub_) ); 74 | sync_mono_->registerCallback(boost::bind( 75 | &StereoDehazer::monoCb, 76 | this, _1, _2, _3, _4)); 77 | 78 | // Set the image publishers before the streaming 79 | left_mono_pub_ = it.advertiseCamera( 80 | ros::names::clean(stereo_ns + "/enhanced/left/image_rect"), 1); 81 | right_mono_pub_ = it.advertiseCamera( 82 | ros::names::clean(stereo_ns + "/enhanced/right/image_rect"), 1); 83 | 84 | left_color_pub_ = it.advertiseCamera( 85 | ros::names::clean(stereo_ns + "/enhanced/left/image_rect_color"), 1); 86 | right_color_pub_ = it.advertiseCamera( 87 | ros::names::clean(stereo_ns + "/enhanced/right/image_rect_color"), 1); 88 | 89 | // Create the callback with the clouds 90 | pub_points2_ = nh.advertise< pcl::PointCloud >( 91 | ros::names::clean(stereo_ns + "/enhanced/points2"), 1); 92 | 93 | ROS_INFO("Stereo Image Enhacer Initialized!"); 94 | } 95 | 96 | /** \brief Stereo callback. This function is called when synchronized 97 | * mono image pairs are received. 98 | * @return 99 | * \param l_img left stereo image message of type sensor_msgs::Image 100 | * \param r_img right stereo image message of type sensor_msgs::Image 101 | * \param l_info left stereo info message of type sensor_msgs::CameraInfo 102 | * \param r_info right stereo info message of type sensor_msgs::CameraInfo 103 | */ 104 | void StereoDehazer::monoCb(const ImageConstPtr& l_img_msg, 105 | const ImageConstPtr& r_img_msg, 106 | const CameraInfoConstPtr& l_info_msg, 107 | const CameraInfoConstPtr& r_info_msg) { 108 | if (left_mono_pub_.getNumSubscribers() > 0) { 109 | Mat l_img_mono = cv_bridge::toCvShare(l_img_msg, 110 | image_encodings::MONO8)->image; 111 | Mat deh_mono_left = d_.dehazeGrayscale(l_img_mono); 112 | // convert OpenCV image to ROS message 113 | cv_bridge::CvImage mono_left_cvi; 114 | mono_left_cvi.header.stamp = l_img_msg->header.stamp; 115 | mono_left_cvi.header.frame_id = l_img_msg->header.frame_id; 116 | mono_left_cvi.encoding = "mono8"; 117 | mono_left_cvi.image = deh_mono_left; 118 | sensor_msgs::Image mono_left_im; 119 | mono_left_cvi.toImageMsg(mono_left_im); 120 | left_mono_pub_.publish(mono_left_im, *l_info_msg); 121 | } 122 | if (right_mono_pub_.getNumSubscribers() > 0) { 123 | Mat r_img_mono = cv_bridge::toCvShare(r_img_msg, 124 | image_encodings::MONO8)->image; 125 | Mat deh_mono_right = d_.dehazeGrayscale(r_img_mono); 126 | // convert OpenCV image to ROS message 127 | cv_bridge::CvImage mono_right_cvi; 128 | mono_right_cvi.header.stamp = r_img_msg->header.stamp; 129 | mono_right_cvi.header.frame_id = r_img_msg->header.frame_id; 130 | mono_right_cvi.encoding = "mono8"; 131 | mono_right_cvi.image = deh_mono_right; 132 | sensor_msgs::Image mono_right_im; 133 | mono_right_cvi.toImageMsg(mono_right_im); 134 | right_mono_pub_.publish(mono_right_im, *r_info_msg); 135 | } 136 | } 137 | 138 | 139 | void StereoDehazer::colorCb(const ImageConstPtr& l_img_msg, 140 | const ImageConstPtr& r_img_msg, 141 | const CameraInfoConstPtr& l_info_msg, 142 | const CameraInfoConstPtr& r_info_msg, 143 | const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { 144 | 145 | // Left image must be dehazed if exists a subscription to points2 146 | Mat l_img_color; 147 | Mat deh_color_left; 148 | if (left_color_pub_.getNumSubscribers() > 0 || pub_points2_.getNumSubscribers() > 0) { 149 | l_img_color = cv_bridge::toCvShare(l_img_msg, 150 | image_encodings::BGR8)->image; 151 | deh_color_left = d_.dehazeRGB(l_img_color); 152 | // convert OpenCV image to ROS message 153 | cv_bridge::CvImage color_left_cvi; 154 | color_left_cvi.header.stamp = l_img_msg->header.stamp; 155 | color_left_cvi.header.frame_id = l_img_msg->header.frame_id; 156 | color_left_cvi.encoding = "bgr8"; 157 | color_left_cvi.image = deh_color_left; 158 | sensor_msgs::Image color_left_im; 159 | color_left_cvi.toImageMsg(color_left_im); 160 | left_color_pub_.publish(color_left_im, *l_info_msg); 161 | } 162 | if (right_color_pub_.getNumSubscribers() > 0) { 163 | Mat r_img_color = cv_bridge::toCvShare(r_img_msg, 164 | image_encodings::BGR8)->image; 165 | Mat deh_color_right = d_.dehazeRGB(r_img_color); 166 | // convert OpenCV image to ROS message 167 | cv_bridge::CvImage color_right_cvi; 168 | color_right_cvi.header.stamp = r_img_msg->header.stamp; 169 | color_right_cvi.header.frame_id = r_img_msg->header.frame_id; 170 | color_right_cvi.encoding = "bgr8"; 171 | color_right_cvi.image = deh_color_right; 172 | sensor_msgs::Image color_right_im; 173 | color_right_cvi.toImageMsg(color_right_im); 174 | right_color_pub_.publish(color_right_im, *r_info_msg); 175 | } 176 | 177 | // Update the camera model 178 | if (pub_points2_.getNumSubscribers() > 0) { 179 | image_geometry::PinholeCameraModel left_cam; 180 | left_cam.fromCameraInfo(l_info_msg); 181 | 182 | // Split enhanced image in channels 183 | std::vector channels(3); 184 | split(deh_color_left, channels); 185 | Mat r, g, b; 186 | channels[0].convertTo(b,CV_8U); 187 | channels[1].convertTo(g,CV_8U); 188 | channels[2].convertTo(r,CV_8U); 189 | 190 | std::vector channels2(3); 191 | split(l_img_color, channels2); 192 | Mat ro, go, bo; 193 | channels2[0].convertTo(bo,CV_8U); 194 | channels2[1].convertTo(go,CV_8U); 195 | channels2[2].convertTo(ro,CV_8U); 196 | 197 | // Convert the points2 to pcl 198 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 199 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 200 | fromROSMsg(*cloud_msg, *cloud); 201 | 202 | // For every point into the pointcloud, get the dehazed color from left image 203 | int32_t tmp_rgb; 204 | for (uint i=0; isize(); i++) { 205 | pcl::PointXYZRGB p = cloud->points[i]; 206 | if (isfinite(p.x) && isfinite(p.y) && isfinite(p.z)) { 207 | cv::Point3d xyz(p.x, p.y, p.z); 208 | cv::Point2d pixel = left_cam.project3dToPixel(xyz); 209 | 210 | // Get the color of the corresponding pixel 211 | uint8_t pr = r.at(pixel.y, pixel.x); 212 | uint8_t pg = g.at(pixel.y, pixel.x); 213 | uint8_t pb = b.at(pixel.y, pixel.x); 214 | 215 | int32_t tmp_rgb = (pr << 16) | (pg << 8) | pb; 216 | p.rgb = *reinterpret_cast(&tmp_rgb); 217 | 218 | cloud_out->push_back(p); 219 | } 220 | } 221 | 222 | // Republish the pointcloud 223 | cloud_out->header = pcl_conversions::toPCL(cloud_msg->header); 224 | pub_points2_.publish(cloud_out); 225 | } 226 | } 227 | --------------------------------------------------------------------------------