├── config
    ├── dead_pixel.png
    ├── mean_compensation.png
    └── seekthermal_ros.yaml
├── README.md
├── launch
    └── seekthermal_ros.launch
├── src
    ├── seekthermal_ros_node.cpp
    └── seekthermal_ros.cpp
├── msg
    └── ThermalImage.msg
├── .gitignore
├── package.xml
├── LICENSE
├── include
    └── seekthermal_ros
    │   └── seekthermal_ros.h
└── CMakeLists.txt


/config/dead_pixel.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ethz-asl/seekthermal_ros/HEAD/config/dead_pixel.png


--------------------------------------------------------------------------------
/config/mean_compensation.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/ethz-asl/seekthermal_ros/HEAD/config/mean_compensation.png


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # seekthermal_ros
2 | ROS node for Seek thermal imaging devices
3 | 
4 | ## Depedencies
5 | [libseekthermal](https://github.com/ethz-asl/libseekthermal)
6 | 


--------------------------------------------------------------------------------
/launch/seekthermal_ros.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 | 	<node pkg="seekthermal_ros" type="seekthermal_ros_node" name="seekthermalRosNode" output="screen">
3 | 		<rosparam file="$(find seekthermal_ros)/config/seekthermal_ros.yaml" command="load" />
4 | 	</node> 
5 | </launch>
6 | 


--------------------------------------------------------------------------------
/src/seekthermal_ros_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <seekthermal_ros/seekthermal_ros.h>
 2 | 
 3 | using namespace seekthermal_ros;
 4 | 
 5 | int main(int argc, char** argv)
 6 | {
 7 |   ros::init(argc, argv, "seekthermal_ros_node");
 8 |   ros::NodeHandle nh("~");
 9 | 
10 |   SeekthermalRos seekthermalRos(nh);
11 | 
12 |   ros::spin();
13 |   return 0;
14 | }
15 | 


--------------------------------------------------------------------------------
/msg/ThermalImage.msg:
--------------------------------------------------------------------------------
 1 | uint32 height
 2 | uint32 width
 3 | 
 4 | # float32 image, raw data
 5 | float32[] data_raw # actual matrix data, size is (height*width) col1, col2, ...
 6 | 
 7 | # uint8 image, dead pixel mask
 8 | uint8[] data_mask # actual matrix data, size is (height*width) col1, col2, ...
 9 | 
10 | # images
11 | sensor_msgs/Image image_gray
12 | sensor_msgs/Image image_colored
13 | 


--------------------------------------------------------------------------------
/config/seekthermal_ros.yaml:
--------------------------------------------------------------------------------
 1 | thermal_image_topic_name: image_raw
 2 | thermal_image_raw_topic_name: thermal_image_raw
 3 | colored_thermal_image_topic_name: image_colored
 4 | camera_frame_id: thermal_camera
 5 | camera_name: thermal_camera
 6 | camera_info_url: file://${ROS_HOME}/camera_info/${NAME}.yaml
 7 | interpolate_dead_pixels: true
 8 | calibrate_dead_pixels: false
 9 | mean_compensation: true
10 | calibrate_mean_compensation: false
11 | denoise: true
12 | show_debug_images: false
13 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Compiled Object files
 2 | *.slo
 3 | *.lo
 4 | *.o
 5 | *.obj
 6 | 
 7 | # Precompiled Headers
 8 | *.gch
 9 | *.pch
10 | 
11 | # Compiled Dynamic libraries
12 | *.so
13 | *.dylib
14 | *.dll
15 | 
16 | # Fortran module files
17 | *.mod
18 | 
19 | # Compiled Static libraries
20 | *.lai
21 | *.la
22 | *.a
23 | *.lib
24 | 
25 | # Executables
26 | *.exe
27 | *.out
28 | *.app
29 | 
30 | # tmp files
31 | *~
32 | 
33 | # qt
34 | /build
35 | *CMakeLists.txt.user
36 | 
37 | # CLion
38 | *.idea/
39 | 
40 | 
41 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>seekthermal_ros</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The seekthermal_ros package</description>
 6 | 
 7 |   <maintainer email="dominic.jud@mavt.ethz.ch">Dominic Jud</maintainer>
 8 | 
 9 |   <license>BSD</license>
10 |   
11 |   <author email="dominic.jud@mavt.ethz.ch">Dominic Jud</author>
12 | 
13 |   <buildtool_depend>catkin</buildtool_depend>
14 |   <build_depend>roscpp</build_depend>
15 |   <build_depend>image_transport</build_depend>
16 |   <build_depend>cv_bridge</build_depend>
17 |   <build_depend>sensor_msgs</build_depend>
18 |   <build_depend>camera_info_manager</build_depend>
19 |   <build_depend>image_geometry</build_depend>
20 |   <build_depend>cmake_modules</build_depend>
21 |   <build_depend>std_msgs</build_depend>
22 |   <build_depend>message_generation</build_depend> 
23 |   
24 |   <run_depend>roscpp</run_depend>
25 |   <run_depend>image_transport</run_depend>
26 |   <run_depend>cv_bridge</run_depend>
27 |   <run_depend>sensor_msgs</run_depend>
28 |   <run_depend>camera_info_manager</run_depend>
29 |   <run_depend>image_geometry</run_depend>
30 |   <run_depend>std_msgs</run_depend>
31 |   <run_depend>message_runtime</run_depend>
32 | </package>
33 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2015, ETHZ ASL
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without
 5 | modification, are permitted provided that the following conditions are met:
 6 | 
 7 | * Redistributions of source code must retain the above copyright notice, this
 8 |   list of conditions and the following disclaimer.
 9 | 
10 | * Redistributions in binary form must reproduce the above copyright notice,
11 |   this list of conditions and the following disclaimer in the documentation
12 |   and/or other materials provided with the distribution.
13 | 
14 | * Neither the name of seekthermal_ros nor the names of its
15 |   contributors may be used to endorse or promote products derived from
16 |   this software without specific prior written permission.
17 | 
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | 
29 | 


--------------------------------------------------------------------------------
/include/seekthermal_ros/seekthermal_ros.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | // libseekthermal
 4 | #include <seekthermal/seekthermal.h>
 5 | #include <seekthermal/command/application.h>
 6 | #include <seekthermal/usb/context.h>
 7 | 
 8 | // ros
 9 | #include <ros/ros.h>
10 | #include <image_transport/image_transport.h>
11 | #include <sensor_msgs/Image.h>
12 | #include <sensor_msgs/CameraInfo.h>
13 | #include <camera_info_manager/camera_info_manager.h>
14 | #include <image_geometry/pinhole_camera_model.h>
15 | 
16 | // opencv
17 | #include <opencv2/highgui/highgui.hpp>
18 | #include <opencv2/photo/photo.hpp>
19 | 
20 | // boost
21 | #include <boost/thread/thread.hpp>
22 | #include <boost/thread/mutex.hpp>
23 | #include <boost/bind.hpp>
24 | #include <boost/thread/condition_variable.hpp>
25 | 
26 | #include <queue>
27 | 
28 | // thermal image msg
29 | #include <seekthermal_ros/ThermalImage.h>
30 | 
31 | using namespace cv;
32 | using namespace SeekThermal;
33 | 
34 | namespace seekthermal_ros {
35 | 
36 | class SeekthermalRos
37 | {
38 | public:
39 |   /*!
40 |    * Constructor.
41 |    */
42 |   SeekthermalRos(ros::NodeHandle nh);
43 | 
44 |   /*!
45 |    * Destructor.
46 |    */
47 |   virtual ~SeekthermalRos();
48 | 
49 | private:
50 |   void captureThermalImages(const Pointer<Device>& device);
51 |   void publishingThermalImages();
52 |   Mat convertFromGrayToColor(Mat &image);
53 | 
54 |   ros::NodeHandle nh_;
55 |   image_transport::ImageTransport it_;
56 |   image_transport::CameraPublisher thermal_image_publisher_;
57 |   image_transport::Publisher colored_thermal_image_publisher_;
58 |   ros::Publisher thermal_image_raw_publisher_;
59 | 
60 |   boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
61 | 
62 |   std::string device_adress_;
63 | 
64 |   std::string thermal_image_topic_name_;
65 |   std::string thermal_image_raw_topic_name_;
66 |   std::string colored_thermal_image_topic_name_;
67 | 
68 |   std::string camera_name_;
69 |   std::string camera_frame_id_;
70 |   std::string camera_info_url_;
71 | 
72 |   std::string package_path_;
73 | 
74 |   Pointer<Device> device_;
75 | 
76 |   Frame last_calibration_frame_;
77 |   bool use_inpaint_;
78 |   bool calibrate_dead_pixels_;
79 |   bool mean_compensation_;
80 |   bool calibrate_mean_compensation_;
81 |   bool denoise_;
82 |   bool show_debug_images_;
83 | 
84 |   enum State {CALIBRATE_DEAD_PIXEL, LOAD_DEAD_PIXEL, CALIBRATE_MEAN, LOAD_MEAN, RUN};
85 |   State state_;
86 | 
87 |   cv::Mat inpaint_mask_;
88 |   cv::Mat mean_compensation_image_;
89 | 
90 |   std::queue<Pointer<Frame>> frame_queue_;
91 |   mutable boost::mutex mutex_;
92 |   boost::condition_variable condition_variable_;
93 | 
94 | };
95 | 
96 | } /* namespace */
97 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(seekthermal_ros)
  3 | 
  4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
  5 | 
  6 | ## Find catkin macros and libraries
  7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  8 | ## is used, also find other catkin packages
  9 | find_package(catkin REQUIRED COMPONENTS
 10 |   roscpp
 11 |   std_msgs
 12 |   genmsg
 13 |   image_transport
 14 |   cv_bridge
 15 |   sensor_msgs
 16 |   camera_info_manager
 17 |   image_geometry
 18 | )
 19 | 
 20 | ## Generate messages in the 'msg' folder
 21 | add_message_files(
 22 |   DIRECTORY
 23 |   msg
 24 |   FILES
 25 |   ThermalImage.msg
 26 | )
 27 | 
 28 | ## Generate added messages and services with any dependencies listed here
 29 | generate_messages(
 30 |   DEPENDENCIES
 31 |   std_msgs
 32 |   sensor_msgs
 33 | )
 34 | 
 35 | ## System dependencies are found with CMake's conventions
 36 | find_package(cmake_modules REQUIRED)
 37 | find_package(OpenCV REQUIRED)
 38 | find_package(PkgConfig)
 39 | pkg_check_modules(SEEKTHERMAL libseekthermal) 
 40 | find_package(Boost REQUIRED)
 41 | 
 42 | ## Uncomment this if the package has a setup.py. This macro ensures
 43 | ## modules and global scripts declared therein get installed
 44 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 45 | # catkin_python_setup()
 46 | 
 47 | 
 48 | ###################################
 49 | ## catkin specific configuration ##
 50 | ###################################
 51 | ## The catkin_package macro generates cmake config files for your package
 52 | ## Declare things to be passed to dependent projects
 53 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 54 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 55 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 56 | ## DEPENDS: system dependencies of this project that dependent projects also need
 57 | catkin_package(
 58 | #  INCLUDE_DIRS include
 59 | #  LIBRARIES thermal_hotspot_inspection
 60 |   CATKIN_DEPENDS roscpp std_msgs genmsg image_transport cv_bridge sensor_msgs camera_info_manager image_geometry
 61 | #  DEPENDS system_libs
 62 |   CATKIN_DEPENDS message_runtime
 63 | )
 64 | 
 65 | ###########
 66 | ## Build ##
 67 | ###########
 68 | 
 69 | ## Specify additional locations of header files
 70 | ## Your package locations should be listed before other locations
 71 | include_directories(include)
 72 | include_directories(
 73 |   ${catkin_INCLUDE_DIRS}
 74 |   ${SEEKTHERMAL_INCLUDE_DIRS}
 75 |   ${Boost_INCLUDE_DIRS}
 76 | )
 77 | 
 78 | ## Declare a cpp library
 79 | add_library(seekthermal_ros
 80 |   src/seekthermal_ros.cpp
 81 | )
 82 | 
 83 | target_link_libraries(seekthermal_ros
 84 |   ${catkin_LIBRARIES}
 85 |   ${OpenCV_LIBS}
 86 |   ${SEEKTHERMAL_LIBRARIES}
 87 |   ${Boost_LIBRARIES}
 88 | )
 89 | add_dependencies(seekthermal_ros 
 90 |     seekthermal_ros_generate_messages_cpp
 91 | )
 92 | 
 93 | ## Declare a cpp executable
 94 | add_executable(seekthermal_ros_node src/seekthermal_ros_node.cpp)
 95 | 
 96 | ## Specify libraries to link a library or executable target against
 97 | target_link_libraries(seekthermal_ros_node
 98 |   seekthermal_ros
 99 |   ${catkin_LIBRARIES}
100 |   ${OpenCV_LIBS}
101 | )
102 | 
103 | 


--------------------------------------------------------------------------------
/src/seekthermal_ros.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | 
  3 | #include <seekthermal_ros/seekthermal_ros.h>
  4 | 
  5 | #include <cv_bridge/cv_bridge.h>
  6 | #include <sensor_msgs/image_encodings.h>
  7 | 
  8 | #include <ros/package.h>
  9 | 
 10 | #define DEG2RAD 0.01745329
 11 | 
 12 | namespace seekthermal_ros {
 13 | 
 14 | SeekthermalRos::SeekthermalRos(ros::NodeHandle nh): nh_(nh), it_(nh)
 15 | {
 16 |   //Get parameters from configuration file
 17 |   nh_.getParam("thermal_image_topic_name", thermal_image_topic_name_);
 18 |   nh_.getParam("thermal_image_raw_topic_name", thermal_image_raw_topic_name_);
 19 |   nh_.getParam("colored_thermal_image_topic_name", colored_thermal_image_topic_name_);
 20 |   nh_.getParam("device_adress", device_adress_);
 21 |   nh_.getParam("interpolate_dead_pixels", use_inpaint_);
 22 |   nh_.getParam("calibrate_dead_pixels", calibrate_dead_pixels_);
 23 |   nh_.getParam("mean_compensation", mean_compensation_);
 24 |   nh_.getParam("calibrate_mean_compensation", calibrate_mean_compensation_);
 25 |   nh_.getParam("denoise", denoise_);
 26 |   nh_.getParam("show_debug_images", show_debug_images_);
 27 |   nh_.getParam("camera_frame_id", camera_frame_id_);
 28 |   nh_.getParam("camera_name", camera_name_);
 29 |   nh_.getParam("camera_info_url", camera_info_url_);
 30 | 
 31 |   //Create a publisher for the raw thermal image
 32 |   thermal_image_publisher_ = it_.advertiseCamera("/" + camera_name_ + "/" + thermal_image_topic_name_, 1);
 33 |   colored_thermal_image_publisher_ = it_.advertise("/" + camera_name_ + "/" + colored_thermal_image_topic_name_, 1);
 34 |   thermal_image_raw_publisher_ =
 35 |           nh_.advertise<seekthermal_ros::ThermalImage>("/" + camera_name_ + "/" + thermal_image_raw_topic_name_, 1);
 36 | 
 37 |   //nh_.getParam("image_width", image_width_);
 38 |   //nh_.getParam("image_height", image_height_);
 39 |   cinfo_.reset(new camera_info_manager::CameraInfoManager(nh_, camera_name_, camera_info_url_));
 40 |   // check for default camera info
 41 |   if (camera_info_url_.size() == 0)
 42 |   {
 43 |     cinfo_->setCameraName(camera_name_);
 44 |     sensor_msgs::CameraInfo camera_info;
 45 |     camera_info.header.frame_id = camera_frame_id_;
 46 |     //camera_info.width = image_width_;
 47 |     //camera_info.height = image_height_;
 48 |     cinfo_->setCameraInfo(camera_info);
 49 |   }
 50 | 
 51 |   //Find all connected devices
 52 |   std::list<Pointer<Device>> devices = discoverDevices();
 53 | 
 54 |   SeekThermal::Usb::Context context;
 55 |   Pointer<Interface> interface;
 56 | 
 57 |   //choose first device in list if no device was specified in the configuration file
 58 |   if (device_adress_.empty())
 59 |   {
 60 |     if (!devices.empty())
 61 |     {
 62 |       for (std::list<Pointer<Device> >::const_iterator it = devices.begin(); it != devices.end(); ++it)
 63 |       {
 64 |         ROS_INFO_STREAM("Found " << (*it)->getInterface()->getName() << " " << (*it)->getInterface()->getAddress());
 65 |       }
 66 | 
 67 |       ROS_INFO_STREAM("Trying to open first device: " << devices.front()->getInterface()->getAddress());
 68 |       interface = context.getInterface(devices.front()->getInterface()->getAddress());
 69 |     }
 70 |     else
 71 |     {
 72 |       ROS_ERROR_STREAM("No devices found and no device specified in config file");
 73 |       ros::shutdown();
 74 |     }
 75 |   }
 76 |   else
 77 |   {
 78 |     ROS_INFO_STREAM("Trying to open device at " << device_adress_);
 79 |     interface = context.getInterface(device_adress_);
 80 |   }
 81 | 
 82 |   device_ = interface->discoverDevice();
 83 | 
 84 |   if (!device_.isNull()) {
 85 |     interface->setTimeout(1);
 86 |     device_->setInterface(interface);
 87 |     device_->connect();
 88 |     device_->initialize();
 89 |     ROS_INFO_STREAM("Device initialized!");
 90 |   }
 91 |   else
 92 |   {
 93 |     ROS_ERROR_STREAM("Failed to open device");
 94 |     ros::shutdown();
 95 |   }
 96 | 
 97 |   if (calibrate_mean_compensation_)
 98 |   {
 99 |     state_ = CALIBRATE_MEAN;
100 |   }
101 |   else
102 |   {
103 |     state_ = LOAD_MEAN;
104 |   }
105 | 
106 |   package_path_ = ros::package::getPath("seekthermal_ros");
107 | 
108 |   boost::thread* publishing_thread = new boost::thread(boost::bind(&SeekthermalRos::publishingThermalImages, this));
109 | 
110 |   captureThermalImages(device_);
111 | }
112 | 
113 | SeekthermalRos::~SeekthermalRos()
114 | {
115 |   device_->disconnect();
116 | }
117 | 
118 | void SeekthermalRos::captureThermalImages(const Pointer<Device>& device)
119 | {
120 |   while(ros::ok())
121 |   {
122 |     if (thermal_image_publisher_.getNumSubscribers()>0 ||
123 |             colored_thermal_image_publisher_.getNumSubscribers()>0 ||
124 |             thermal_image_raw_publisher_.getNumSubscribers()>0)
125 |     {
126 |       Pointer<Frame> frame = new Frame();
127 |       device->capture(*frame);
128 |       
129 |       boost::mutex::scoped_lock lock(mutex_);
130 |       frame_queue_.push(frame);
131 |         
132 |       if (frame_queue_.size()>10)
133 |       {
134 |         frame_queue_.pop();
135 |       }
136 |       lock.unlock();
137 |       condition_variable_.notify_one();
138 |     }
139 |     else
140 |       ros::Duration(0.1).sleep();
141 |   }
142 | }
143 | 
144 | void SeekthermalRos::publishingThermalImages()
145 | {
146 |   static int seq_counter = 0;
147 |   static int dead_pixel_counter = 0;
148 |   static size_t frameId = 0;
149 | 
150 |   std::vector<Frame> frame_vector;
151 | 
152 |   static Frame meanFrame;
153 |   static Frame varianceFrame;
154 | 
155 |   while(ros::ok())
156 |   {
157 |     boost::mutex::scoped_lock lock(mutex_);
158 |     while(frame_queue_.empty())
159 |     {
160 |       condition_variable_.wait(lock);
161 |     }
162 | 
163 |     Pointer<Frame> frame = frame_queue_.front();
164 |     frame_queue_.pop();
165 | 
166 |     if (frame->getType() == Frame::typeNormal) {
167 |       //ROS_INFO_STREAM("Normal Frame");
168 |       size_t width = frame->getWidth();
169 |       size_t height = frame->getHeight();
170 | 
171 |       Frame frame_t = *frame;
172 | 
173 |       //subtract most recent calibration frame
174 |       if (!last_calibration_frame_.isEmpty())
175 |         *frame -= last_calibration_frame_;
176 | 
177 |       static Frame mean_comp;
178 | 
179 |       // raw data
180 |       Mat cvImage_raw = Mat(frame->getHeight(), frame->getWidth(), CV_32FC1);
181 |       for (size_t x = 0; x < frame->getWidth(); ++x) {
182 |         for (size_t y = 0; y < frame->getHeight(); ++y) {
183 |           float value = (*frame)(x, y);
184 |           cvImage_raw.at<float>(y, x) = value;
185 |         }
186 |       }
187 | 
188 |       // normalize
189 |       // TODO min max?
190 |       frame->normalize(-1300,0);
191 | 
192 |       if (show_debug_images_)
193 |       {
194 |         Mat cvImage_normalized = Mat(frame->getHeight(), frame->getWidth(), CV_8UC3);
195 |         for (size_t x = 0; x < frame->getWidth(); ++x)
196 |           for (size_t y = 0; y < frame->getHeight(); ++y) {
197 |             float value = (*frame)(x, y)*255.0;
198 |             cvImage_normalized.at<Vec3b>(y,x)[0] = value;
199 |             cvImage_normalized.at<Vec3b>(y,x)[1] = value;
200 |             cvImage_normalized.at<Vec3b>(y,x)[2] = value;
201 |           }
202 | 
203 |         cv::imshow("normalized", cvImage_normalized);
204 |         cv::waitKey(10);
205 |       }
206 | 
207 | 
208 |       switch (state_)
209 |       {
210 |         case CALIBRATE_MEAN:
211 |           //collect frames for calibrating the mean
212 |           if (frame_vector.size() < 20)
213 |           {
214 |             ROS_INFO_STREAM("mean");
215 |             frame_vector.push_back(*frame);
216 |           }
217 |           else
218 |           {
219 |             mean_comp = Frame(frame->getWidth(), frame->getHeight(), Frame::typeNormal);
220 |             for(std::vector<int>::size_type i = 0; i != frame_vector.size(); i++)
221 |             {
222 |               mean_comp += frame_vector[i];
223 |             }
224 |             mean_comp *= 1.0/(float)frame_vector.size();
225 |             float overall_mean = 0;
226 |             for (size_t x = 0; x < width; ++x)
227 |               for (size_t y = 0; y < height; ++y) {
228 |                 float value = (mean_comp)(x, y);
229 |                 overall_mean += value;
230 |               }
231 |             overall_mean /= width*height;
232 | 
233 |             mean_comp -= overall_mean;
234 | 
235 |             mean_compensation_image_ = Mat(height, width, CV_8UC1);
236 |             for (size_t x = 0; x < width; ++x)
237 |               for (size_t y = 0; y < height; ++y)
238 |               {
239 |                 float value = (mean_comp)(x, y)*255;
240 |                 mean_compensation_image_.at<uchar>(y,x) = value+256/2;
241 |               }
242 | 
243 |             std::vector<int> image_pref;
244 |             image_pref.push_back(CV_IMWRITE_PNG_COMPRESSION);
245 |             image_pref.push_back(0);
246 | 
247 |             cv::imwrite(package_path_ + "/config/mean_compensation.png", mean_compensation_image_, image_pref);
248 | 
249 |             frame_vector.clear();
250 |             if (calibrate_dead_pixels_)
251 |             {
252 |               state_ = CALIBRATE_DEAD_PIXEL;
253 |             }
254 |             else
255 |             {
256 |               state_ = LOAD_DEAD_PIXEL;
257 |             }
258 |           }
259 |           break;
260 | 
261 | 
262 |           //Load an already existing mean compensation image
263 |         case LOAD_MEAN:
264 |           mean_compensation_image_ = imread(package_path_ + "/config/mean_compensation.png", CV_LOAD_IMAGE_GRAYSCALE);
265 | 
266 |           if(! mean_compensation_image_.data )                              // Check for invalid input
267 |           {
268 |             ROS_ERROR_STREAM("No mean calibration file found. Forced calibration.");
269 |             state_ = CALIBRATE_MEAN;
270 |           }
271 |           else if (calibrate_dead_pixels_)
272 |           {
273 |             state_ = CALIBRATE_DEAD_PIXEL;
274 |           }
275 |           else
276 |           {
277 |             state_ = LOAD_DEAD_PIXEL;
278 |           }
279 |           break;
280 | 
281 |           // find pixels that are dead by calculating the variance of each pixel
282 |         case CALIBRATE_DEAD_PIXEL:
283 |           if (frame_vector.size() < 50)
284 |           {
285 |             ROS_INFO_STREAM("dead pixel");
286 |             frame_vector.push_back(*frame);
287 |           }
288 |           else
289 |           {
290 |             Frame mean = Frame(frame->getWidth(), frame->getHeight(), Frame::typeNormal);
291 |             for(std::vector<int>::size_type i = 0; i != frame_vector.size(); i++)
292 |             {
293 |               mean += frame_vector[i];
294 |             }
295 |             mean *= 1.0/(float)frame_vector.size();
296 | 
297 |             Frame temp = Frame(frame->getWidth(), frame->getHeight(), Frame::typeNormal);
298 |             for(std::vector<int>::size_type i = 0; i != frame_vector.size(); i++)
299 |             {
300 |               temp += (mean-frame_vector[i])*(mean-frame_vector[i]);
301 |             }
302 |             Frame variance = temp*(1.0/(float)frame_vector.size());
303 | 
304 |             dead_pixel_counter++;
305 | 
306 |             Mat variance_mat = Mat(height, width, CV_8UC1);
307 |             for (size_t x = 0; x < width; ++x)
308 |               for (size_t y = 0; y < height; ++y) {
309 |                 float value = (variance)(x, y)*255.0;
310 |                 variance_mat.at<uchar>(y,x) = value;
311 |               }
312 | 
313 |             inpaint_mask_ = variance_mat<0.00001;
314 | 
315 |             std::vector<int> image_pref;
316 |             image_pref.push_back(CV_IMWRITE_PNG_COMPRESSION);
317 |             image_pref.push_back(0);
318 | 
319 |             cv::imwrite(package_path_ + "/config/dead_pixel.png", inpaint_mask_, image_pref);
320 | 
321 |             frame_vector.clear();
322 | 
323 |             state_ = RUN;
324 |           }
325 | 
326 |           break;
327 | 
328 |           //Load calibration image for dead pixels
329 |         case LOAD_DEAD_PIXEL:
330 |           inpaint_mask_ = imread(package_path_ + "/config/dead_pixel.png", CV_LOAD_IMAGE_GRAYSCALE);
331 | 
332 |           if(! inpaint_mask_.data )                              // Check for invalid input
333 |           {
334 |             ROS_ERROR_STREAM("No dead pixel calibration file found. Forced calibration.");
335 |             state_ = CALIBRATE_DEAD_PIXEL;
336 |           }
337 |           else
338 |           {
339 |             state_ = RUN;
340 |           }
341 |           break;
342 | 
343 |         case RUN:
344 |           // raw data
345 |           // build raw image msg
346 |           seekthermal_ros::ThermalImage msgThermalImage;
347 |           msgThermalImage.height = cvImage_raw.rows;
348 |           msgThermalImage.width = cvImage_raw.cols;
349 |           for (int x = 0; x < cvImage_raw.cols; ++x) {
350 |             for (int y = 0; y < cvImage_raw.rows; ++y) {
351 |               msgThermalImage.data_raw.push_back(cvImage_raw.at<float>(y, x));
352 |               msgThermalImage.data_mask.push_back(inpaint_mask_.at<uchar>(y, x));
353 |             }
354 |           }
355 | 
356 |           Mat cvImage = Mat(frame->getHeight(), frame->getWidth(), CV_8UC1);
357 |           for (size_t x = 0; x < frame->getWidth(); ++x)
358 |             for (size_t y = 0; y < frame->getHeight(); ++y) {
359 |               float value = (*frame)(x, y)*255.0;
360 |               cvImage.at<uchar>(y,x) = value;
361 |             }
362 | 
363 |           if (mean_compensation_)
364 |           {
365 |             for (size_t x = 0; x < cvImage.cols; ++x)
366 |               for (size_t y = 0; y < cvImage.rows; ++y)
367 |               {
368 |                 float value = cvImage.at<uchar>(y,x) - mean_compensation_image_.at<uchar>(y,x) - 256/2;
369 |                 if (value > -1)
370 |                   cvImage.at<uchar>(y,x) = -1;
371 |                 else
372 |                   cvImage.at<uchar>(y,x) = value;
373 |               }
374 | 
375 |             if (show_debug_images_)
376 |             {
377 |               cv::imshow("mean_comp", cvImage);
378 |               cv::waitKey(10);
379 |             }
380 |           }
381 | 
382 |           cv::Mat cvImage_inpainted = Mat(height, width, CV_8UC1);
383 | 
384 |           if (use_inpaint_)
385 |           {
386 |             cv::inpaint(cvImage, inpaint_mask_, cvImage_inpainted, 1, cv::INPAINT_NS);
387 |             cvImage_inpainted.copyTo(cvImage);
388 |             if (show_debug_images_)
389 |             {
390 |               cv::imshow("inpainted frame", cvImage_inpainted);
391 |               cv::waitKey(10);
392 |             }
393 |           }
394 | 
395 |           cv::Mat cvImage_denoised = Mat(height, width, CV_8UC1);
396 | 
397 |           if (denoise_)
398 |           {
399 |             cv::fastNlMeansDenoising(cvImage, cvImage_denoised, 5, 5, 31);
400 | 
401 |             cvImage_denoised.copyTo(cvImage);
402 | 
403 |             if (show_debug_images_)
404 |             {
405 |               cv::Point center(cvImage_denoised.cols / 2, cvImage_denoised.rows / 2);
406 |               int rectSize = 9;
407 |               cv::rectangle(cvImage_denoised,
408 |                             cv::Rect(center.x-rectSize/2, center.y-rectSize/2, rectSize, rectSize),
409 |                             cv::Scalar(0,0,0), 1);
410 |               cv::imshow("denoised frame", cvImage_denoised);
411 |               cv::waitKey(10);
412 |             }
413 |           }
414 | 
415 |           cv::Mat cvImage_colored = Mat(height, width, CV_8UC3);
416 |           cvImage_colored = convertFromGrayToColor(cvImage);
417 | 
418 | 
419 |           std_msgs::Header header;
420 |           header.seq = seq_counter;
421 |           header.frame_id = camera_frame_id_;
422 |           //header.stamp = ros::Time(frame.getTimestamp());
423 |           cv_bridge::CvImage *cv_ptr_colored = new cv_bridge::CvImage(header, sensor_msgs::image_encodings::BGR8, cvImage_colored);
424 |           cv_bridge::CvImage *cv_ptr = new cv_bridge::CvImage(header, sensor_msgs::image_encodings::MONO8, cvImage);
425 | 
426 |           // grab the camera info
427 |           sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
428 |           ci->header.seq = seq_counter++;
429 |           ci->header.frame_id = camera_frame_id_;
430 |           //ci->header.stamp = frame.getTimestamp();
431 |           ci->height = frame->getHeight();
432 |           ci->width = frame->getWidth();
433 | 
434 |           // publish the image
435 |           thermal_image_publisher_.publish(*cv_ptr->toImageMsg(), *ci);
436 |           colored_thermal_image_publisher_.publish(*cv_ptr_colored->toImageMsg());
437 | 
438 |           // publish raw image
439 |           msgThermalImage.image_gray = *cv_ptr->toImageMsg();
440 |           msgThermalImage.image_colored = *cv_ptr_colored->toImageMsg();
441 |           thermal_image_raw_publisher_.publish(msgThermalImage);
442 |       }
443 |     }
444 |     else if (frame->getType() == Frame::typeCalibration)
445 |     {
446 |       //ROS_INFO_STREAM("Calibration Frame");
447 |       last_calibration_frame_ = *frame;
448 |     }
449 |   }
450 | }
451 | 
452 | Mat SeekthermalRos::convertFromGrayToColor(Mat &image)
453 | {
454 |   Mat cvImage_colored = Mat(image.rows, image.cols, CV_8UC3);
455 |   for (size_t x = 0; x < image.cols; ++x)
456 |     for (size_t y = 0; y < image.rows; ++y) {
457 |       float value = image.at<uchar>(y,x);
458 |       float r = (sin((value / 255.0 * 360.0 - 120.0 > 0 ? value / 255.0 * 360.0 - 120.0 : 0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
459 |       float g = (sin((value / 255.0 * 360.0 + 60.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
460 |       float b = (sin((value / 255.0 * 360.0 + 140.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
461 |       cvImage_colored.at<Vec3b>(y,x)[0] = r;
462 |       cvImage_colored.at<Vec3b>(y,x)[1] = g;
463 |       cvImage_colored.at<Vec3b>(y,x)[2] = b;
464 |     }
465 |   return cvImage_colored;
466 | }
467 | 
468 | } /* namespace */
469 | 


--------------------------------------------------------------------------------