├── .gitignore ├── LICENSE ├── README.md ├── flir_lepton ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── flir_lepton_auxiliary ├── CHANGELOG.rst ├── CMakeLists.txt ├── datasets │ ├── dataset05 │ └── dataset_spline ├── package.xml └── scripts │ ├── produce_dataset.py │ └── thermal_interp.py ├── flir_lepton_image_processing ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── cfg │ ├── processing_node_topics.yaml │ └── thermal_cfg.cfg ├── include │ └── flir_lepton_image_processing │ │ ├── processing_node │ │ ├── process.h │ │ └── thermal_roi_detector.h │ │ └── utils │ │ ├── blob_detection.h │ │ ├── bounding_box_detection.h │ │ ├── defines.h │ │ ├── edge_detection.h │ │ ├── message_conversions.h │ │ ├── morphological_operators.h │ │ ├── outline_discovery.h │ │ ├── parameters.h │ │ ├── roi_filters.h │ │ ├── thermal_rois_conveyor.h │ │ ├── timer.h │ │ └── visualization.h ├── launch │ └── flir_lepton_image_processing.launch ├── package.xml └── src │ ├── processing_node │ ├── CMakeLists.txt │ ├── process.cpp │ ├── process_node.cpp │ └── thermal_roi_detector.cpp │ └── utils │ ├── blob_detection.cpp │ ├── bounding_box_detection.cpp │ ├── edge_detection.cpp │ ├── message_conversions.cpp │ ├── morphological_operators.cpp │ ├── outline_discovery.cpp │ ├── parameters.cpp │ ├── roi_filters.cpp │ ├── thermal_rois_conveyor.cpp │ ├── timer.cpp │ └── visualization.cpp ├── flir_lepton_launchers ├── CHANGELOG.rst ├── CMakeLists.txt ├── launch │ └── flir_lepton.launch └── package.xml ├── flir_lepton_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── flir_lepton_image_processing │ │ ├── CandidateRoiMsg.msg │ │ ├── CandidateRoisVectorMsg.msg │ │ ├── GeneralAlertInfo.msg │ │ ├── ThermalAlert.msg │ │ └── ThermalAlertVector.msg │ └── flir_lepton_sensor │ │ ├── FlirLeptonBatchMsg.msg │ │ ├── FlirLeptonRawMsg.msg │ │ └── TemperaturesMsg.msg └── package.xml └── flir_lepton_sensor ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config └── params.yaml ├── include ├── colormaps.h ├── flir_lepton_hw_iface.h └── flir_lepton_utils.h ├── launch └── flir_lepton_sensor.launch ├── package.xml └── src ├── flir_lepton_hw_iface.cpp ├── flir_lepton_sensor.cpp └── flir_lepton_utils.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # ROS # 2 | ###################### 3 | build/ 4 | devel/ 5 | bin/ 6 | lib/ 7 | msg_gen/ 8 | srv_gen/ 9 | ROS_NOBUILD 10 | CATKIN_IGNORE 11 | bags/ 12 | 13 | *Action.msg 14 | *Feedback.msg 15 | *Goal.msg 16 | *Result.msg 17 | 18 | 19 | # Compiled source # 20 | ################### 21 | *.com 22 | *.class 23 | *.o 24 | *.so 25 | *.pyc 26 | 27 | # OS generated files # 28 | ###################### 29 | .DS_Store 30 | .DS_Store? 31 | ._* 32 | .Spotlight-V100 33 | .Trashes 34 | Icon? 35 | ehthumbs.db 36 | Thumbs.db 37 | 38 | 39 | # Packages # 40 | ############ 41 | # it's better to unpack these files and commit the raw source 42 | # git has its own built in compression methods 43 | *.7z 44 | *.dmg 45 | *.gz 46 | *.iso 47 | *.jar 48 | *.rar 49 | *.tar 50 | *.zip 51 | 52 | 53 | # General # 54 | .*.swp 55 | *.log 56 | core* 57 | tags 58 | cscope.out 59 | cscope.files 60 | 61 | 62 | # Eclipse # 63 | ############ 64 | .project 65 | 66 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2015, P.A.N.D.O.R.A. Team. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Synopsis 2 | 3 | Flir-Lepton LWIR sensor, ROS package. 4 | 5 | As this is a ROS package, ROS has to be installed onto the system. For instructions regarding installation of ROS, follow [this link](http://wiki.ros.org/indigo/Installation) 6 | A minimal installation of ROS is only required. 7 | 8 | The package has been tested under [Hydro](http://wiki.ros.org/hydro/Installation) and [Indigo](http://wiki.ros.org/indigo/Installation) releases, both armhf and x86_64 architectures. 9 | 10 | 11 | ## Packages 12 | 13 | Integrated packages: 14 | - flir_lepton_sensor: 15 | - flir_lepton_image_processing: 16 | - flir_lepton_msgs: 17 | - flir_lepton_launchers: 18 | 19 | ## Build procedure 20 | 21 | As this is a ROS package, build procedures are done using the ROS framework. 22 | 23 | First create a [catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) 24 | 25 | ```shell 26 | $ mkdir -p ~/lepton_catkin_ws/src 27 | ``` 28 | 29 | Clone this repository under the src/ directory of the previously created catkin workspace directory. 30 | 31 | ```shell 32 | $ cd ~/lepton_catkin_ws/src && git clone git@github.com:angetria/flir_lepton.git 33 | ``` 34 | 35 | Build sources. 36 | 37 | ```shell 38 | $ cd ~/lepton_catkin_ws && catkin_make 39 | ``` 40 | 41 | Don't forget to source the setup.bash file. 42 | 43 | ```shell 44 | $ source /opt/ros//setup.bash 45 | ``` 46 | 47 | Using the short name of your ROS distribution instead of . Or you can either add this line to your .bashrc 48 | 49 | No other external dependencies exists. Has been tested on **Debian** and **Ubuntu** distributions. 50 | 51 | 52 | ## How to run nodes 53 | 54 | This package consists of the following nodes: 55 | 56 | - flir_lepton_sensor node: This node handles communication and data acquisition from Flir-Lepton sensor. 57 | - flir_lepton_image_processing: This node performs image-processing, in order to extract higher level information regarding regions of interest from thermal image frames. 58 | 59 | 60 | 61 | ### Launch the standalone flir_lepton_sensor node 62 | 63 | Simply execute: 64 | 65 | ```shell 66 | $ roslaunch flir_lepton_sensor flir_lepton_sensor.launch 67 | ``` 68 | 69 | Or through the global launcher (Disable image processing node execution): 70 | 71 | ```shell 72 | $ roslaunch flir_lepton_launchers flir_lepton.launch image_processing:=false 73 | ``` 74 | 75 | The node exposes two image message topics, a temperatures topic and a batch message topic (usefull if you want to do some **COOL** image processing stuff): 76 | - /flir_lepton/image/rgb: RGB image (rainbow colorscheme). ``` sensor_msgs/Image.``` 77 | - /flir_lepton/image/gray: GrayScale image. ``` sensor_msgs/Image.``` 78 | - /flir_lepton/temperatures: Scene frame temperatures values in Celcius degrees. [flir_lepton_msgs/TemperaturesMsg](https://github.com/angetria/flir_lepton/blob/master/flir_lepton_msgs/msg/flir_lepton_sensor/TemperaturesMsg.msg) 79 | - /flir_lepton/batch: Constists of a sensor_msgs/Image (GrayScale values) plus temperatures, as above described. 80 | 81 | By default, both rgb and grayscale image topics are published. You can disable any of them through the relevant launcher. 82 | For example you can disable both rgb and gray publications by executing: 83 | 84 | ```shell 85 | $ roslaunch flir_lepton_launchers flir_lepton.launch image_processing:=false gray_image:=false rgb_image:=false 86 | ``` 87 | 88 | We measured that maximum VoSPI frames/sec, for a stable communication, is around 25fps. 89 | Though, global node publishing rate of the relevant topics is independed from VoSPI frames acquisition, as this implementation uses a thread that continuously reads through the SPI interface. 90 | You can change node publishing rate through configuration file located under flir_lepton_sensor/config/ directory, [here](https://github.com/angetria/flir_lepton/blob/master/flir_lepton_sensor/config/params.yaml). 91 | Some other usefull configuration parameters are also defined into the aforementioned configuration file: 92 | 93 | - SPI interface port. 94 | - SPI clock speed. 95 | - Publishing topic names. 96 | 97 | 98 | ### Launch flir_lepton_sensor with image processing capabilities 99 | 100 | Like already mentioned, this package integrates some image processing functionalities. In order to launch with those image processing functionalities, simply execute: 101 | 102 | ```shell 103 | $ roslaunch flir_lepton_launchers flir_lepton.launch 104 | ``` 105 | 106 | Launching with image processing capabilities is an extension. It does not overwrite the default behaviors (flir_lepton_sensor) 107 | 108 | The image processing node is used by default. 109 | 110 | 111 | ###Launch the standalone flir_lepton_image_processing node 112 | 113 | In case of an independent use of this node, for exaple using a rosbag file,simply execute: 114 | 115 | ```shell 116 | $ roslaunch flir_lepton_image_processing flir_lepton_image_processing.launch 117 | ``` 118 | 119 | By default rosbag mode is set to false. To be enabled simply execute: 120 | 121 | ```shell 122 | $ roslaunch flir_lepton_image_processing flir_lepton_image_processing.launch bag_mode:=true 123 | ``` 124 | 125 | **Dont forget to add the full path of your bag file in the launcher [here](https://github.com/angetria/flir_lepton/blob/master/flir_lepton_image_processing/launch/flir_lepton_image_processing.launch)** 126 | 127 | ## Tests 128 | 129 | This metapackage does not include any tests. Node (package) specific tests are developed under each package directory. 130 | 131 | ## Contributors 132 | 133 | - Konstaninos Panayiotou, [klpanagi@gmail.com] 134 | - Angelos Triantafyllidis, [aggelostriadafillidis@gmail.com] 135 | 136 | 137 | ## Contact 138 | 139 | Angelos Triantafyllidis, [aggelostriadafillidis@gmail.com] 140 | -------------------------------------------------------------------------------- /flir_lepton/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Changed metapackage name to flir_lepton -- platform independent 8 | * Refactored flir_lepton_sensor package + packaging 9 | * Minor changes 10 | * Apply namespaces in flir_lepton package 11 | * Explicit dependency on messages added to flir_lepton package 12 | * minor typo changes 13 | * dataset loading 14 | * Integrated hardware_interface and image_processing 15 | * Contributors: Angelos Triantafyllidis, Konstantinos Panayiotou 16 | -------------------------------------------------------------------------------- /flir_lepton/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /flir_lepton/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_lepton 4 | 0.0.1 5 | 6 | Contains sources for working with Flir-Lepton LWIR camera under ROS. 7 | 8 | 9 | Konstantinos Panayiotou 10 | 11 | Konstantinos Panayiotou 12 | Angelos Triantafyllidis 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | flir_lepton_sensor 19 | flir_lepton_image_processing 20 | flir_lepton_msgs 21 | flir_lepton_launchers 22 | flir_lepton_auxiliary 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton_auxiliary 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Changed metapackage name to flir_lepton -- platform independent 8 | * Refactored flir_lepton_sensor package + packaging 9 | * Minor changes in scripts 10 | * minor typo changes 11 | * dataset loading 12 | * Contributors: Angelos Triantafyllidis, Konstantinos Panayiotou 13 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton_auxiliary) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | 7 | catkin_package( 8 | INCLUDE_DIRS 9 | #include 10 | CATKIN_DEPENDS 11 | ) 12 | 13 | include_directories() 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/datasets/dataset05: -------------------------------------------------------------------------------- 1 | 7818 2 | 20 3 | 7842 4 | 21 5 | 7866 6 | 22 7 | 7885 8 | 23 9 | 7905 10 | 24 11 | 7926 12 | 25 13 | 7950 14 | 26 15 | 7966 16 | 27 17 | 7987 18 | 28 19 | 8009 20 | 29 21 | 8025 22 | 30 23 | 8036 24 | 31 25 | 8055 26 | 32 27 | 8074 28 | 33 29 | 8101 30 | 34 31 | 8121 32 | 35 33 | 8190 34 | 36 35 | 8213 36 | 37 37 | 8227 38 | 38 39 | 8238 40 | 39 41 | 8259 42 | 40 43 | 8285 44 | 41 45 | 8301 46 | 42 47 | 8325 48 | 43 49 | 8353 50 | 44 51 | 8374 52 | 45 53 | 8397 54 | 46 55 | 8412 56 | 47 57 | 8456 58 | 48 59 | 8465 60 | 49 61 | 8480 62 | 50 63 | 8495 64 | 51 65 | 8510 66 | 52 67 | 8525 68 | 53 69 | 8540 70 | 54 71 | 8555 72 | 55 73 | 8570 74 | 56 75 | 8585 76 | 57 77 | 8600 78 | 58 79 | 8615 80 | 59 81 | 8630 82 | 60 83 | 8645 84 | 61 85 | 8660 86 | 62 87 | 8675 88 | 63 89 | 8690 90 | 64 91 | 8705 92 | 65 93 | 8720 94 | 66 95 | 8735 96 | 67 97 | 8750 98 | 68 99 | 8765 100 | 69 101 | 8780 102 | 70 103 | 8795 104 | 71 105 | 8810 106 | 72 107 | 8825 108 | 73 109 | 8840 110 | 74 111 | 8855 112 | 75 113 | 8870 114 | 76 115 | 8885 116 | 77 117 | 8900 118 | 78 119 | 8915 120 | 79 121 | 8930 122 | 80 123 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/package.xml: -------------------------------------------------------------------------------- 1 | 2 | flir_lepton_auxiliary 3 | 0.0.1 4 | Contains auxiliary files used by the flir_lepton_rpi2 package 5 | 6 | Konstantinos Panayiotou 7 | 8 | BSD 9 | 10 | https://github.com/klpanagi/rpi_hardware_interface/issues 11 | https://github.com/klpanagi/rpi_hardware_interface.git 12 | 13 | Konstantinos Panayiotou 14 | Angelos Triantafyllidis 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/scripts/produce_dataset.py: -------------------------------------------------------------------------------- 1 | #!usr/bin/python 2 | 3 | import sys 4 | 5 | # The area of interest(pixels) to find the mean temperature 6 | x1 = 52 7 | x2 = 54 8 | y1 = 18 9 | y2 = 20 10 | 11 | # The area of neutral pixels to find the mean temperature 12 | x3 = 66 13 | x4 = 68 14 | y3 = 6 15 | y4 = 8 16 | 17 | # Make the dataset from raw pixel flir-lepton images. 18 | try: 19 | dataset = open("dataset.iemb","w") 20 | except IOError: 21 | print "dataset file could not be made" 22 | sys.exit() 23 | 24 | # Process each image raw data for all images 25 | for i in range(20,51): 26 | 27 | # The flir-lepton images to be read. 28 | fileName = \ 29 | "Input your flir-lepton images full path"\ 30 | + str(i) + ".iemb" 31 | 32 | try: 33 | data = open(fileName, "r") 34 | except IOError: 35 | print "No file was found" 36 | sys.exit() 37 | 38 | #counter = 0 39 | signal_val = [] 40 | 41 | with open(fileName, "r") as file: 42 | for line in file: 43 | #counter = counter + 1 44 | signal_val.append(line) 45 | temp = signal_val[0].split() 46 | 47 | 48 | # To read specific pixel(x,y) must give : gimp gives inverted x,y coords 49 | # average = the average temperature of the point of interest 50 | # av = the average temperature of a neutral area 51 | 52 | average = 0 53 | av = 0 54 | if i < 51: 55 | for x in range(x1,x2): 56 | for y in range(y1,y2): 57 | # for raw data images 58 | a = y * 80 + x 59 | average = average + int(temp[a]) 60 | 61 | # Find in the other array whats going on in pixel values 62 | for x in range(x3,x4): 63 | for y in range(y3,y4): 64 | b = y * 80 + x 65 | av = av + int(temp[b]) 66 | 67 | average = average / 4 68 | av = av / 4 69 | 70 | # If we check second area 71 | #else: 72 | #for x in range(24,26): 73 | #temp = signal_val[x].split() 74 | #for y in range(42,44): 75 | #a = int(temp[y]) 76 | #average = average + a 77 | 78 | #Find in the other array whats going on in pix values 79 | #for x in range(10,12): 80 | #temp = signal_val[x].split() 81 | #for y in range(65,67): 82 | #a = int(temp[y]) 83 | #av = av + a 84 | 85 | #average = average / 4 86 | #av = av / 4 87 | 88 | # Make the file with our own standards, for even numbers of rows place 89 | # the raw pixel values, for odd the degree's. 90 | degree = str(i) + "\n" 91 | raw = str(average) + "\n" 92 | 93 | dataset.write(raw) 94 | dataset.write(degree) 95 | 96 | print "The average of image", i ,"=",average 97 | 98 | print "The av of image", i ,"=",av 99 | 100 | data.close() 101 | dataset.close() 102 | -------------------------------------------------------------------------------- /flir_lepton_auxiliary/scripts/thermal_interp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # coding:utf-8 3 | 4 | __author__ = 'Angelos Triantafyllidis', 'Konstantinos Panayiotou' 5 | __maintainer__ = 'Angelos Triantafyllidis', 'Konstantinos Panayiotou' 6 | __email__ = 'aggelostriadafillidis@gmail.com', 'klpanagi@gmail.com' 7 | 8 | import pylab as pl 9 | import json 10 | import numpy as np 11 | import scipy as sp 12 | from scipy.interpolate import interp1d 13 | from scipy.interpolate import splrep 14 | from scipy.interpolate import splev 15 | import matplotlib.pyplot as plt 16 | 17 | #class FlirUtils: 18 | #def __init__(self): 19 | 20 | 21 | class Utils: 22 | def __init__(self): 23 | self.module = 'Interpolation Utilities' 24 | 25 | def normalize(self, dataset): 26 | normal_dataset = [] 27 | max_value = max(dataset) 28 | min_value = min(dataset) 29 | 30 | print '\033[32mDataset max value:\033[0m [%s]' %max_value 31 | print '\033[32mDataset min value:\033[0m [%s]' %min_value 32 | for value in dataset: 33 | norm_value = 1.0 * (value - min_value) / (max_value - min_value) 34 | #print norm_value 35 | normal_dataset.append(norm_value) 36 | 37 | return {'dataset': normal_dataset, 'max': max_value, 'min': min_value} 38 | 39 | 40 | def readDataset(fileUri): 41 | x_data = [] 42 | y_data = [] 43 | linecounter = 0 44 | 45 | with open(fileUri, "r") as file: 46 | for line in file: 47 | if (linecounter % 2) == 0: 48 | x_data.append(int(line.split('\n')[0])) 49 | else: 50 | y_data.append(int(line.split('\n')[0])) 51 | linecounter += 1 52 | return {'x_data': x_data, 'y_data': y_data} 53 | 54 | #### ---- ---- #### 55 | def linear_interpolation(x_data, y_data): 56 | max_value_x = max(x_data) 57 | min_value_x = min(x_data) 58 | f1d = interp1d(x_data, y_data) # Create 1D interpolation function 59 | x_data_new = range(min_value_x, max_value_x+1) # Max/Min values from dataset 60 | y_data_new = f1d(x_data_new) # Calculate linear interpolated temperature values 61 | return {'x_data': x_data_new, 'y_data': y_data_new} 62 | 63 | def cubicSpline_interpolation(x_data, y_data): 64 | max_value_x = max(x_data) 65 | min_value_x = min(x_data) 66 | x_data_new = range(min_value_x, max_value_x+1) # Max/Min values from dataset 67 | tck = splrep(x_data, y_data, s=0) 68 | y_data_new = splev(x_data_new, tck, der=0) 69 | return {'x_data': x_data_new, 'y_data': y_data_new} 70 | 71 | def writeData(x_data, y_data, fileUri): 72 | f = open(fileUri, "w") 73 | if len(x_data) != len(y_data): 74 | print 'Something went wrong!! y_data and x_data must be the same size' 75 | sys.exit(0) 76 | for i in range(0, len(x_data)): 77 | f.write('%d\n' % x_data[i]) 78 | f.write('%f\n' % y_data[i]) 79 | 80 | 81 | #### ---- ---- #### 82 | dataset_uri = '/home/pandora/pandora_ws/srv/rpi_hardware_interface/data/flir_lepton/dataset05.pandora' 83 | dataset = readDataset(dataset_uri) 84 | signals = dataset['x_data'] 85 | temps = dataset['y_data'] 86 | ###################################### 87 | 88 | ##### ----- ---- ###### 89 | linInterp_data = linear_interpolation(signals, temps) 90 | 91 | plt.subplot(2,1,1) 92 | plt.plot(signals, temps, 'o', linInterp_data['x_data'], linInterp_data['y_data'], '-') 93 | plt.legend(['dataset', 'linear_interp'], loc='best') 94 | plt.title('Linear (1D) interpolation') 95 | ############################################### 96 | 97 | #### ---- ---- #### 98 | splineInterp_data = cubicSpline_interpolation(signals, temps) 99 | plt.subplot(2,1,2) 100 | plt.plot(signals, temps, 'o', splineInterp_data['x_data'], splineInterp_data['y_data'], '-') 101 | plt.legend(['dataset', 'Cubic Spline'], loc='best') 102 | plt.title('Cubic-spline interpolation') 103 | 104 | 105 | #writeData(splineInterp_data['x_data'], splineInterp_data['y_data'], \ 106 | #'dataset_spline_interp.pandora') 107 | 108 | "Uncomment the following line in order to plot the interplolated data" 109 | plt.show() 110 | 111 | #diffList = {} 112 | #count = 0 113 | #for val in interp_temps: 114 | #diffList[val] = interp_temps_spline[count] 115 | #print '\033[33mRaw Signal Value:\033[0m [%s] ---> \033[31mLinear:\033[0m [%s], \033[35mCubic-Spline:\033[0m [%s]'\ 116 | #%(signals_new[count], val,interp_temps_spline[count]) 117 | #count += 1 118 | 119 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton_image_processing 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Initialized CHANGELOG.rst files 8 | * Global ros launcher ready 9 | * Fixed compilation error -- explicit 10 | * Remove unnecessary dependencies 11 | * Move all messages into flir_lepton_ros_comm package 12 | * Refactor namespaces in image_processing package 13 | * Fixed config parameters on loading thermal mode 14 | * Integrated hardware_interface and image_processing 15 | * Contributors: Angelos Triantafyllidis, Konstantinos Panayiotou 16 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton_image_processing) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | cv_bridge 7 | #image_transport 8 | dynamic_reconfigure 9 | flir_lepton_msgs 10 | roslint 11 | ) 12 | 13 | generate_dynamic_reconfigure_options( 14 | cfg/thermal_cfg.cfg 15 | ) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS 19 | roscpp 20 | cv_bridge 21 | #image_transport 22 | dynamic_reconfigure 23 | flir_lepton_msgs 24 | INCLUDE_DIRS 25 | include 26 | LIBRARIES 27 | ) 28 | 29 | include_directories( 30 | include/${PROJECT_NAME} 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | ################################ Utils library ################################# 35 | add_library(${PROJECT_NAME}_utils 36 | src/utils/blob_detection.cpp 37 | src/utils/bounding_box_detection.cpp 38 | src/utils/edge_detection.cpp 39 | src/utils/roi_filters.cpp 40 | src/utils/thermal_rois_conveyor.cpp 41 | src/utils/message_conversions.cpp 42 | src/utils/morphological_operators.cpp 43 | src/utils/outline_discovery.cpp 44 | src/utils/parameters.cpp 45 | src/utils/timer.cpp 46 | src/utils/visualization.cpp 47 | ) 48 | 49 | # Ensure all their headers get built before any targets that need them 50 | add_dependencies(${PROJECT_NAME}_utils ${catkin_EXPORTED_TARGETS}) 51 | 52 | target_link_libraries(${PROJECT_NAME}_utils 53 | ${catkin_LIBRARIES} 54 | ) 55 | 56 | add_subdirectory(src/processing_node) 57 | 58 | ################################# ROSLINT ###################################### 59 | # Execute via lint : catkin_make roslint_{PACKAGE_NAME} 60 | 61 | set(ROSLINT_CPP_OPTS 62 | "--filter=-whitespace/end_of_line, -build/include_order, -whitespace/blank_line, -whitespace/parens, -whitespace/comments, -build/include_what_you_use, -whitespace/braces, -whitespace/newline, -readability/multiline_string, -runtime/printf, -readability/streams") 63 | 64 | set(ROSLINT_CPP_OPTS 65 | "--filter=-build/include_what_you_use, -runtime/references") 66 | 67 | file(GLOB_RECURSE ${PROJECT_NAME}_LINT_SRCS 68 | RELATIVE ${PROJECT_SOURCE_DIR} 69 | 70 | include/*.h 71 | src/*.cpp 72 | ) 73 | 74 | roslint_cpp(${${PROJECT_NAME}_LINT_SRCS}) 75 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/README.md: -------------------------------------------------------------------------------- 1 | thermal_node_cfg 2 | =============== 3 | 4 | This README file contains information about the configurations of the thermal node. 5 | 6 | #Thermal detection method 7 | There are two different detection methods in thermal_node. The first one processes a binary 8 | image(values are 0 or 255) extracted from the acquired temperatures of the sensor's image. 9 | The second method processes the sensor_msgs/Image(values are 0 - 255) that is directly 10 | acquired from the thermal sensor. 11 | 12 | ##Binary image processing method 13 | Set detection method, in order to process binary image: 14 | ```bash 15 | detection_method = 0 16 | ``` 17 | **This method is preferred for flir-lepton**. 18 | 19 | ###Further configurations 20 | When detection method is 0 the most important configurations that must 21 | be set are: 22 | 23 | 1.low_temperature: This variable is the lower bound(temperature) that we want 24 | to be represented as white pixel. 25 | 26 | 2.high_temperature: This variable is the upper bound(temperature)that we want 27 | to be represented as white pixel. 28 | 29 | **Values outside of these bounds are represented as black** 30 | 31 | **Important**: 32 | low_temperature must be set to the value(temperature) of the victim(or a bit less) 33 | or to a value(temperature) higher than the environment's temperature. 34 | 35 | high_temperature must be set to the value(temperature) of the thermal source. 36 | 37 | 38 | ##Sensor Image processing method 39 | Set detection method, in order to process the sensor's image: 40 | ```bash 41 | detection_method = 1 42 | ``` 43 | **This method is not preferred** 44 | 45 | ##Other important configurations 46 | 1.min_area is a variable that checks the area of the candidate Poi that was found. 47 | If area < min_area this Poi is rejected. 48 | 49 | 2.probability_method: The are two methods for the Pois probability extraction: 50 | 51 | **Gaussian distribution**: 52 | ```bash 53 | probability_method = 0 54 | ``` 55 | This method is based on two variables: 56 | optimal_temperature: This variable defines the temperature that gives 100% probability. 57 | tolerance: The tolerance of the gaussian distribution. The higher the value, the higher 58 | the tolerance. 59 | 60 | **Logistic distribution**: 61 | ```bash 62 | probability_method = 1 63 | ``` 64 | This method is based on four variables: 65 | low_acceptable_temperature: The low acceptable temperature of this method. 66 | high_acceptable_temperature: The high acceptable temperature of this method. 67 | left_tolerance: The tolerance of the logistic distribution's curve left side. 68 | The higher the value, the lower the tolerance. 69 | right_tolerance: The tolerance of the logistic distribution's curve right side. 70 | The higher the value, the lower the tolerance. 71 | 72 | 3.Dont forget to check the image matching parameters in thermal_node_maching_variables.yaml. 73 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/cfg/processing_node_topics.yaml: -------------------------------------------------------------------------------- 1 | subscribed_topics: 2 | flir_batch_topic: /flir_lepton/batch 3 | 4 | published_topics: 5 | # The outline, center and rectangle of the thermal rois found 6 | candidate_rois_topic: /flir_lepton_image_proc/thermal_rois_body 7 | # The yaw, pitch, probability and temperature of the thermal rois found 8 | candidate_rois_alert_topic: /flir_lepton_image_proc/thermal_rois_info 9 | 10 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/cfg/thermal_cfg.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Kinect configuration 3 | 4 | PACKAGE = 'flir_lepton_image_processing' 5 | 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | # Example : gen.add("int_param", int_t, 0, "An In.....", 50, 0, 100) 11 | # name type level description def min max 12 | 13 | #-------------------------- show the thermal image ------------------------------- 14 | gen.add("show_thermal_image", bool_t, 0,"", False) 15 | 16 | gen.add("detection_method", int_t, 1, "0 for binary image-1 for sensor/Image", 0, 0, 1) 17 | #-------------- Low and High accepted Temperatures --------------------- 18 | gen.add("low_temperature", double_t, 0, "Used when detection method is 0", 27, 20, 80) 19 | gen.add("high_temperature", double_t, 0, "Used when detection method is 0", 70, 20, 80) 20 | 21 | #-------------------------- Edge detection method ------------------------------ 22 | dirm = gen.enum([ 23 | gen.const("Canny_edge_detector", int_t, 0,""), 24 | gen.const("Scharr_edge_detector", int_t, 1,""), 25 | gen.const("Sobel_edge_detector", int_t, 2,""), 26 | gen.const("Laplacian_edge_detector", int_t, 3,""), 27 | gen.const("Mixed_edge_detector", int_t, 4,"")], "") 28 | 29 | gen.add("edge_detection_method", int_t, 0, "", 0, 0, 4, edit_method=dirm) 30 | 31 | #--------------------------- canny parameters ---------------------------------- 32 | gen.add("canny_ratio", int_t, 0,"", 3, 0, 10) 33 | gen.add("canny_kernel_size", int_t, 0,"", 3, 1, 10) 34 | gen.add("canny_low_threshold", int_t, 0,"", 10, 1, 255) 35 | gen.add("canny_blur_noise_kernel_size", int_t, 0,"", 3, 1, 10) 36 | 37 | gen.add("contrast_enhance_beta", int_t, 0,"", 2, 1, 10) 38 | gen.add("contrast_enhance_alpha", int_t, 0,"", 2, 1, 10) 39 | 40 | #-------------------------- Threshold parameters ------------------------------- 41 | gen.add("denoised_edges_threshold", int_t, 0,"", 130, 0, 255) 42 | 43 | #------------------------- Blob detection parameters --------------------------- 44 | 45 | gen.add("min_threshold", int_t, 0,"", 0, 0, 5000) 46 | gen.add("max_threshold", int_t, 0,"", 200, 0, 5000) 47 | gen.add("threshold_step", int_t, 0,"", 100, 1, 100) 48 | gen.add("min_area", int_t, 0,"", 100, 1, 1000000) 49 | gen.add("max_area", int_t, 0,"", 181000, 1, 302700) 50 | gen.add("min_convexity", double_t, 0,"", 0.0, 0.0, 100.0) 51 | gen.add("max_convexity", double_t, 0,"", 100.0, 0.0, 100.0) 52 | gen.add("min_inertia_ratio", double_t, 0,"", 0.0, 0.0, 100.0) 53 | gen.add("max_circularity", double_t, 0,"", 1.0, 0.0, 1.0) 54 | gen.add("min_circularity", double_t, 0,"", 0.0, 0.0, 1.0) 55 | gen.add("filter_by_color", bool_t, 0,"", False) 56 | gen.add("filter_by_circularity", bool_t, 0,"", True) 57 | 58 | #------------------------- Bounding boxes parameters --------------------------- 59 | 60 | odm = gen.enum([ 61 | gen.const("Brushfire_bounding_box_detection", int_t, 0,""), 62 | gen.const("Raycast_bounding_box_detection", int_t, 1,"")], 63 | "") 64 | 65 | gen.add("outline_detection_method", int_t, 0, "", 0, 0, 1, edit_method=odm) 66 | 67 | gen.add("raycast_keypoint_partitions", int_t, 0, "", 32, 0, 128) 68 | 69 | #----------------------- Loose ends connection parameters ---------------------- 70 | 71 | gen.add("AB_to_MO_ratio", int_t, 0, "", 4, 0, 10) 72 | gen.add("minimum_curve_points", int_t, 0, "", 50, 0, 1000) 73 | 74 | 75 | #--------------------- Scaling from 32FC1 to 8UC1 parameters ------------------- 76 | 77 | gen.add("scale_method", int_t, 0, "", 0, 0, 1) 78 | 79 | #------------------------------- Debug ----------------------------------------- 80 | 81 | gen.add("show_find_rois", bool_t, 0,"", False) 82 | gen.add("show_find_rois_size", int_t, 0,"", 1200, 0, 3000) 83 | 84 | gen.add("show_denoise_edges", bool_t, 0,"", False) 85 | gen.add("show_denoise_edges_size", int_t, 0,"", 1200, 0, 3000) 86 | 87 | gen.add("show_connect_pairs", bool_t, 0,"", False) 88 | gen.add("show_connect_pairs_size", int_t, 0,"", 1200, 0, 3000) 89 | 90 | gen.add("show_get_shapes_clear_border", bool_t, 0,"", False) 91 | gen.add("show_get_shapes_clear_border_size", int_t, 0,"", 1200, 0, 3000) 92 | 93 | #---------------------- Probability extraction Parameters ---------------------- 94 | 95 | gen.add("probability_method", int_t, 0,"", 1, 0, 1) 96 | gen.add("min_thermal_probability", double_t, 0,"Min thermal's probabability", 0.2, 0, 1) 97 | 98 | #------------------ Gaussian Function----------------------- 99 | gen.add("optimal_temperature", double_t, 0,"Gaussian_Probability", 37, 20, 50) 100 | gen.add("tolerance", double_t, 0,"Gaussian_Probability", 10, 0, 25) 101 | 102 | #------------------ Logistic Function ---------------------- 103 | gen.add("low_acceptable_temperature", double_t, 0,"Logistic_Probability", 27, 20, 80) 104 | gen.add("high_acceptable_temperature", double_t, 0,"Logistic_Probability", 70, 20, 80) 105 | 106 | #------------------ Tolerance right and left --------------------------------- 107 | gen.add("left_tolerance", double_t, 0,"Logistic_Probability- Big value means small tolerance", 4, 1, 10) 108 | gen.add("right_tolerance", double_t, 0,"Logistic_Probability- Big value means small tolerance", 8, 1, 10) 109 | 110 | exit(gen.generate(PACKAGE, "flir_lepton_image_processing", "thermal_cfg")) 111 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/processing_node/process.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias, 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_PROCESS_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_PROCESS_H 41 | 42 | #include "processing_node/thermal_roi_detector.h" 43 | #include "utils/parameters.h" 44 | #include "utils/message_conversions.h" 45 | #include "flir_lepton_msgs/GeneralAlertInfo.h" 46 | #include "flir_lepton_msgs/ThermalAlert.h" 47 | #include "flir_lepton_msgs/ThermalAlertVector.h" 48 | #include "flir_lepton_msgs/FlirLeptonBatchMsg.h" 49 | #include "std_msgs/Float32MultiArray.h" 50 | 51 | /** 52 | @brief The namespaces for this package 53 | **/ 54 | namespace flir_lepton 55 | { 56 | namespace flir_lepton_image_processing 57 | { 58 | /** 59 | @class Process 60 | @brief Provides functionalities for locating thermal rois via 61 | analysis of a thermal image acquired by flir lepton camera. 62 | **/ 63 | class Process 64 | { 65 | private: 66 | // The ROS node handle. 67 | ros::NodeHandle nodeHandle_; 68 | 69 | // Subscriber of Flir Lepton camera sensor/temperature image. 70 | ros::Subscriber batchSubscriber_; 71 | 72 | // The name of the topic where the sensor/temperature image is acquired from. 73 | std::string batchTopic_; 74 | 75 | // Ros publisher for the candidate Rois found by process node. 76 | // Includes keypoints, outline and bounding box 77 | ros::Publisher candidateRoisPublisher_; 78 | 79 | // The name of the topic where the candidate Rois that the process node 80 | // locates are published to. 81 | std::string candidateRoisTopic_; 82 | 83 | // Ros publisher for the candidate Rois Alert message found by process node. 84 | // Includes flir_lepton_msgs/GeneralAlertInfo and temperatures 85 | ros::Publisher candidateRoisAlertPublisher_; 86 | 87 | // The name of the topic where the candidate Rois Alert message that the 88 | // process node locates are published to. 89 | std::string candidateRoisAlertTopic_; 90 | 91 | // The dynamic reconfigure (process) parameters' server 92 | dynamic_reconfigure::Server< ::flir_lepton_image_processing::thermal_cfgConfig> 93 | server; 94 | 95 | // The dynamic reconfigure (process) parameters' callback 96 | dynamic_reconfigure::Server< ::flir_lepton_image_processing::thermal_cfgConfig> 97 | ::CallbackType f; 98 | 99 | /** 100 | @brief Based on the detection method that we select, return the appropriate 101 | image to be further processed.The thermal image message is unpacked in 102 | a cv::Mat image. 103 | @param-[in] msg [const flir_lepton_msgs::FlirLeptonBatchMsg&] 104 | The batch message from flir-lepton 105 | @param[in] detection_method [const int] The detection method that we use 106 | @return cv::Mat The image to be further processed. 107 | **/ 108 | cv::Mat selectImageToProcess( 109 | const flir_lepton_msgs::FlirLeptonBatchMsg& msg, 110 | const int detection_method); 111 | 112 | /** 113 | @brief Callback for the thermal sensor/temperature image received by 114 | flir-lepton sensor. 115 | Thermal rois are then located inside this image. 116 | @param msg [const flir_lepton_msgs::FlirLeptonBatchMsg&] 117 | The input flir lepton message. 118 | @return void 119 | **/ 120 | void inputThermalImageCallback( 121 | const flir_lepton_msgs::FlirLeptonBatchMsg& msg); 122 | 123 | /** 124 | @brief Acquire the names of topics which the process node will be having 125 | transactionary affairs with. 126 | @param void 127 | @return void 128 | **/ 129 | void getTopicNames(); 130 | 131 | /** 132 | @brief The function called when a parameter is changed 133 | @param[in] config [const ::flir_lepton_image_processing::thermal_cfgConfig&] 134 | @param[in] level [const uint32_t] 135 | @return void 136 | **/ 137 | void parametersCallback( 138 | const ::flir_lepton_image_processing::thermal_cfgConfig& config, 139 | const uint32_t& level); 140 | 141 | 142 | /** 143 | @brief This function finds for each region of interest it's 144 | probability based on the keypoint's average temperature. 145 | Then fills the probability and the temperature in the RoisConveyor struct. 146 | @param[out] rois [const RoisConveyor&] The regions of interest found. 147 | @param[in] temperatures [const Float32MultiArray&] The multiArray with 148 | the temperatures of the image. 149 | @param[in] method [const int&] Denotes the probabilities extraction 150 | method. 151 | @return void 152 | **/ 153 | void findRoisProbability(RoisConveyor* rois, 154 | const std_msgs::Float32MultiArray& temperatures, const int& method); 155 | 156 | public: 157 | /** 158 | @brief Default constructor. Initiates communications, loads parameters. 159 | @return void 160 | **/ 161 | Process(void); 162 | 163 | /** 164 | @brief Default destructor 165 | @return void 166 | **/ 167 | ~Process(void); 168 | }; 169 | 170 | } // namespace flir_lepton_image_processing 171 | } // namespace flir_lepton 172 | 173 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_PROCESS_H 174 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/processing_node/thermal_roi_detector.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias, 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_THERMAL_ROI_DETECTOR_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_THERMAL_ROI_DETECTOR_H 41 | 42 | #include "utils/blob_detection.h" 43 | #include "utils/thermal_rois_conveyor.h" 44 | #include "utils/roi_filters.h" 45 | 46 | /** 47 | @brief The namespaces for this package 48 | **/ 49 | namespace flir_lepton 50 | { 51 | namespace flir_lepton_image_processing 52 | { 53 | /** 54 | @class RoiDetector 55 | @brief Provides the functionalities for detecting thermal Rois via analysis 56 | of a thermal image acquired by Flir Lepton camera. 57 | **/ 58 | class RoiDetector 59 | { 60 | public: 61 | /** 62 | @brief Finds the Rois in a thermal image in CV_8UC1 format. 63 | First, the edges of the thermal image are detected. 64 | Then, keypoints of blobs are detected in the above image. 65 | Finally, the potential Rois outline is found, along with the bounding 66 | boxes of those outlines. 67 | @param[in] thermalImage [const cv::Mat&] The thermal image in CV_8UC1 format 68 | @return RoisConveyor The struct that contains the rois found. 69 | **/ 70 | static RoisConveyor findRois(const cv::Mat& thermalImage); 71 | }; 72 | 73 | } // namespace flir_lepton_image_processing 74 | } // namespace flir_lepton 75 | 76 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_PROCESSING_NODE_THERMAL_ROI_DETECTOR_H 77 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/blob_detection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BLOB_DETECTION_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BLOB_DETECTION_H 40 | 41 | #include "utils/morphological_operators.h" 42 | 43 | /** 44 | @brief The namespaces for this package 45 | **/ 46 | namespace flir_lepton 47 | { 48 | namespace flir_lepton_image_processing 49 | { 50 | /** 51 | @class BlobDetection 52 | @brief Provides methods for blob detection 53 | **/ 54 | class BlobDetection 55 | { 56 | public: 57 | /** 58 | @brief Detects blobs in an image 59 | @param[in] inImage [const cv::Mat&] The input image 60 | @param[out] keyPointsOut [std::vector*] The ouput 61 | @return void 62 | **/ 63 | static void detectBlobs(const cv::Mat& inImage, 64 | std::vector* keyPointsOut); 65 | }; 66 | 67 | } // namespace flir_lepton_image_processing 68 | } // namespace flir_lepton 69 | 70 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BLOB_DETECTION_H 71 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/bounding_box_detection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BOUNDING_BOX_DETECTION_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BOUNDING_BOX_DETECTION_H 40 | 41 | #include "utils/morphological_operators.h" 42 | 43 | /** 44 | @brief The namespaces for this package 45 | **/ 46 | namespace flir_lepton 47 | { 48 | namespace flir_lepton_image_processing 49 | { 50 | /** 51 | @class BoundingBoxDetection 52 | @brief Provides methods for bounding box detection 53 | **/ 54 | class BoundingBoxDetection 55 | { 56 | public: 57 | /** 58 | @brief Finds rotated bounding boxes from blob outlines. 59 | The blob's area must be larger than 60 | Parameters::bounding_box_min_area_threshold. 61 | The blob and its bounding rectangle must be inside the image's limits. 62 | @param[in] inImage [const cv::Mat&] The input image 63 | @param[in] blobsOutlineVector 64 | [const std::vector >&] 65 | The outline points of the blobs 66 | @param[in] blobsArea [const std::vector&] The blobs' area 67 | @param[out] outRectangles [std::vector< std::vector >*] 68 | The rectangles of the bounding boxes 69 | @return void 70 | **/ 71 | static void findRotatedBoundingBoxesFromOutline( 72 | const cv::Mat& inImage, 73 | const std::vector >& blobsOutlineVector, 74 | const std::vector& blobsArea, 75 | std::vector< std::vector >* outRectangles); 76 | }; 77 | 78 | } // namespace flir_lepton_image_processing 79 | } // namespace flir_lepton 80 | 81 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_BOUNDING_BOX_DETECTION_H 82 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/defines.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_DEFINES_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_DEFINES_H 41 | 42 | #include 43 | // #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include "opencv2/imgproc/imgproc.hpp" 51 | 52 | #include "ros/ros.h" 53 | #include 54 | #include 55 | // #include 56 | #include "std_msgs/String.h" 57 | 58 | #include "utils/timer.h" 59 | 60 | #define DEBUG_SHOW 61 | #define DEBUG_TIME 62 | 63 | // Transforms a float number to string 64 | #define TOSTR(x) static_cast(\ 65 | (std::ostringstream() << std::dec << x)).str() 66 | 67 | // Takes the file name from an absolute path 68 | #define LPATH(x) x.substr(x.find_last_of("/") + 1 , \ 69 | x.size() - x.find_last_of("/") - 1) 70 | 71 | // Shortcut to std::string 72 | #define STR(x) std::string(x) 73 | 74 | // The string identifier of the thermal detector package. 75 | // It must comply with the sub-namespace set in the package's launchers 76 | #define PKG_NAME "flir_lepton_image_processing" 77 | 78 | /** 79 | @brief The namespaces for this package 80 | **/ 81 | namespace flir_lepton 82 | { 83 | namespace flir_lepton_image_processing 84 | { 85 | 86 | } // namespace flir_lepton_image_processing 87 | } // namespace flir_lepton 88 | 89 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_DEFINES_H 90 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/edge_detection.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_EDGE_DETECTION_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_EDGE_DETECTION_H 41 | 42 | #include "utils/outline_discovery.h" 43 | #include "utils/morphological_operators.h" 44 | 45 | #define CANNY 0 46 | #define SCHARR 1 47 | #define SOBEL 2 48 | #define LAPLACIAN 3 49 | #define MIXED 4 50 | 51 | /** 52 | @brief The namespaces for this package 53 | **/ 54 | namespace flir_lepton 55 | { 56 | namespace flir_lepton_image_processing 57 | { 58 | struct GraphNode 59 | { 60 | GraphNode(int x, int y) 61 | { 62 | this->x = x; 63 | this->y = y; 64 | } 65 | 66 | GraphNode(void) 67 | { 68 | this->x = 0; 69 | this->y = 0; 70 | } 71 | 72 | int x; 73 | int y; 74 | 75 | std::vector connections; 76 | 77 | std::vector dists; 78 | 79 | unsigned int dist; 80 | }; 81 | 82 | /** 83 | @class EdgeDetection 84 | @brief Provides methods for edge detection 85 | **/ 86 | class EdgeDetection 87 | { 88 | public: 89 | /** 90 | @brief Applies the Canny edge detector 91 | @param[in] inImage [const cv::Mat&] Input image in CV_8U depth 92 | @param[out] outImage [cv::Mat*] The processed image in CV_8U depth 93 | @return void 94 | **/ 95 | static void applyCanny(const cv::Mat& inImage, cv::Mat* outImage); 96 | 97 | /** 98 | @brief Applies the Scharr edge transform 99 | @param[in] inImage [const cv::Mat&] Input image in CV_8UC1 format 100 | @param[out] outImage [cv::Mat*] The processed image in CV_8UC1 format 101 | @return void 102 | **/ 103 | static void applyScharr(const cv::Mat& inImage, cv::Mat* outImage); 104 | 105 | /** 106 | @brief Applies the Sobel edge transform 107 | @param[in] inImage [const cv::Mat&] Input image in CV_8UC1 format 108 | @param[out] outImage [cv::Mat*] The processed image in CV_8UC1 format 109 | @return void 110 | **/ 111 | static void applySobel(const cv::Mat& inImage, cv::Mat* outImage); 112 | 113 | /** 114 | @brief Applies the Laplacian edge transform 115 | @param[in] inImage [const cv::Mat&] Input image in CV_8UC1 format 116 | @param[out] outImage [cv::Mat*] The processed image in CV_8UC1 format 117 | @return void 118 | **/ 119 | static void applyLaplacian(const cv::Mat& inImage, cv::Mat* outImage); 120 | 121 | /** 122 | @brief Applies contamination to an image of edges. 123 | It keeps only the edges that are not iteratively neighbors 124 | to the image's limits 125 | @param[in,out] inImage [cv::Mat*] Input image in CV_8UC1 format 126 | @return void 127 | **/ 128 | static void applyEdgeContamination(cv::Mat* inImage); 129 | 130 | /** 131 | @brief Takes as input a thermal image containing unsigned chars, 132 | locates the edges in it and tries to clear as much noise as possible 133 | in the edges image. 134 | It outputs a binary image that contains areas that we wish to validate 135 | as thermal rois. 136 | @param[in] inImage [const cv::Mat&] The thermal image extracted from the 137 | thermal camera, of type CV_8UC1 138 | @param[out] edges [cv::Mat*] The final denoised edges image that 139 | corresponds to the input image 140 | @return void 141 | **/ 142 | static void computeThermalEdges(const cv::Mat& inImage, cv::Mat* edges); 143 | 144 | /** 145 | @brief Connects each point of a number of pair of points with a line 146 | or an elliptic arc 147 | @param[in,out] inImage [cv::Mat*] The image whose selected points will 148 | be connected and line or the elliptic arc drawn on 149 | @param[in] pairs [const std::vector >&] 150 | The vector of pair points 151 | @param[in] method [const int&] Denotes the connection type. 152 | 0 for line, 153 | 1 for elliptic arc 154 | @return void 155 | **/ 156 | static void connectPairs(cv::Mat* inImage, 157 | const std::vector >& pairs, 158 | const int& method); 159 | 160 | /** 161 | @brief Takes an input image of edges of CV_8U depth 162 | and tries to isolate specific shapes so as to facilitate 163 | the blob detection process. 164 | 165 | The execution flow is as follows: first, all pixels connected 166 | directly or indirectly to the image's borders are eliminated, 167 | leaving behind all standalone shapes and curves. 168 | Next, all open-ended curves are contidionally closed. 169 | The reason behind this is that the open-ended curves might be 170 | edges of thermal rois that have either been smudged by the appliance of 171 | a threshold after the production of edges, or that have in the first 172 | place not been detected due to low contrast in a roi's borders. 173 | After the connection of the end-points of open-ended shapes, 174 | pruning is applied to get rid of all minor open-ended curves not 175 | closed by the connection process. The result here is a binary image 176 | featuring only closed shapes, but whose insides may be cluttered with 177 | non-zero value pixels, since the edges of what may be inside a roi 178 | are still detected and present in the current product image. 179 | What we want, based on this image, is to find only the external 180 | limits of each closed curve, because these pixels will be the 181 | outline of each blob. A method is then invoked to do just so. 182 | The end product of this method is a binary image with closed curves. 183 | Everything inside each closed curve is black and everything outside 184 | all closed curves is void. 185 | @param[in,out] img [cv::Mat*] The input image in unsigned char format 186 | @return void 187 | **/ 188 | static void denoiseEdges(cv::Mat* img); 189 | 190 | /** 191 | @brief Detects edges depending on a edge detection selection method. 192 | @param[in] inImage [const cv::Mat&] The image whose edges one wishes 193 | to detect. 194 | @param[out] outImage [cv::Mat*] The image of edges, in format CV_8UC1 195 | @param[in] detectionMethod [const int&] The edge detection method 196 | selector. 197 | @return void 198 | **/ 199 | static void detectEdges(const cv::Mat& inImage, cv::Mat* outImage, 200 | const int& detectionMethod); 201 | 202 | /** 203 | @brief Identifies in which curve a point lies on and returns the 204 | curve's two end points. 205 | @param[in] img [cv::Mat*] The input binary image 206 | @param[in] x_ [const int&] The x coordinate of the point 207 | @param[in] y_ [const int&] The y coordinate of the point 208 | @param[out] ret [std::set&] The points that represent the 209 | curve on which the point lies on 210 | @return edgePoints [std::pair*] The curve's pair 211 | of end points 212 | **/ 213 | static std::pair identifyCurveAndEndpoints( 214 | cv::Mat* img, const int& x_, const int& y_, std::set* ret); 215 | 216 | /** 217 | @brief Given an image of CV_8UC1 format, this method locates and 218 | identifies all continuous curves, along with their end-points. 219 | CAUTION: the length of each curve must exceed a certain threshold. 220 | @param[in,out] image [cv::Mat*] The image whose curves and their 221 | endpoints one wishes to locate and identify. CAUTION: image is cleared 222 | at the end of the process. 223 | @param[out] lines [std::vector >*] 224 | A set containing the indices of points that constite 225 | @param[out] endPoints [std::vector >*] 226 | A vector of endpoints. 227 | @return void 228 | **/ 229 | static void identifyCurvesAndEndpoints(cv::Mat* image, 230 | std::vector >*, 231 | std::vector >* endPoints); 232 | }; 233 | 234 | } // namespace flir_lepton_image_processing 235 | } // namespace flir_lepton 236 | 237 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_EDGE_DETECTION_H 238 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/message_conversions.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MESSAGE_CONVERSIONS_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MESSAGE_CONVERSIONS_H 41 | 42 | #include 43 | #include 44 | #include "flir_lepton_msgs/CandidateRoisVectorMsg.h" 45 | #include "utils/defines.h" 46 | #include "utils/outline_discovery.h" 47 | #include "utils/thermal_rois_conveyor.h" 48 | 49 | /** 50 | @brief The namespaces for this package 51 | **/ 52 | namespace flir_lepton 53 | { 54 | namespace flir_lepton_image_processing 55 | { 56 | /** 57 | @class MessageConversions 58 | @brief Provides methods for converting images from and to ROS messages 59 | **/ 60 | class MessageConversions 61 | { 62 | public: 63 | /** 64 | @brief Converts a cv::Mat image into a sensor_msgs::Image message 65 | @param[in] image [const cv::Mat&] The image 66 | @param[in] encoding [const std::string&] The image message's encoding 67 | @param[in] msg [const sensor_msgs::Image&] A message needed for 68 | setting the output message's header by extracting its header 69 | @return [sensor_msgs::Image] The output image message 70 | **/ 71 | static sensor_msgs::Image convertImageToMessage( 72 | const cv::Mat& image, const std::string& encoding, 73 | const sensor_msgs::Image& msg); 74 | 75 | /** 76 | @brief Constructs a flir_lepton_msgs/CandidateRoisVectorMsg 77 | message 78 | @param[in] conveyor [RoisConveyor&] A struct containing 79 | vectors of the rois keypoints, bounding rectangles' vertices 80 | and blobs' outlines. 81 | @param[out] candidateRoisVector 82 | [std::vector*] 83 | The vector containing the conveyor's rois in 84 | flir_lepton_msgs::CandidateRoisVectorMsg format 85 | @return void 86 | **/ 87 | static void createCandidateRoisVector( 88 | const RoisConveyor& conveyor, 89 | std::vector* 90 | candidateRoisVector); 91 | 92 | /** 93 | @brief Constructs a flir_lepton_msgs/CandidateRoisVectorMsg 94 | message 95 | @param[in] conveyor [RoisConveyor&] A struct containing 96 | vectors of the rois keypoints, bounding rectangles' vertices 97 | and blobs' outlines. 98 | @param[in] image [cv::Mat&] The image to be packed in the message 99 | @param[out] candidateRoisVectorMsg 100 | [flir_lepton_msgs::CandidateRoisVectorMsg*] 101 | The output message 102 | @param[in] encoding [std::string&] The image's encoding 103 | @param[in] msg [const sensor_msgs::Image&] Needed to extract 104 | its header and place it as the header of the output message 105 | @return void 106 | **/ 107 | static void createCandidateRoisVectorMessage( 108 | const RoisConveyor& conveyor, 109 | const cv::Mat& image, 110 | flir_lepton_msgs::CandidateRoisVectorMsg* 111 | candidateRoisVectorMsg, 112 | const std::string& encoding, 113 | const sensor_msgs::Image& msg); 114 | 115 | /** 116 | @brief Extracts a cv::Mat image from a ROS image message 117 | @param[in] msg [const sensor_msgs::Image&] The input ROS image 118 | message 119 | @param[out] image [cv::Mat*] The output image 120 | @param[in] encoding [const std::string&] The image encoding 121 | @return void 122 | **/ 123 | static void extractImageFromMessage( 124 | const sensor_msgs::Image& msg, cv::Mat* image, 125 | const std::string& encoding); 126 | 127 | /** 128 | @brief Extracts a cv::Mat image from a custom ROS message of type 129 | flir_lepton_msgs::CandidateRoisVectorMsg 130 | @param[in] msg [const flir_lepton_msgs::CandidateRoisVectorMsg&] 131 | The input ROS message 132 | @param[out] image [cv::Mat*] The output image 133 | @param[in] encoding [const std::string&] The image encoding 134 | @return void 135 | **/ 136 | static void extractImageFromMessageContainer( 137 | const flir_lepton_msgs::CandidateRoisVectorMsg& msg, 138 | cv::Mat* image, const std::string& encoding); 139 | 140 | /** 141 | @brief Recreates the RoisConveyor struct for the candidate rois 142 | from the flir_lepton_msgs::CandidateRoiMsg message 143 | @param[in] candidateRoisVector 144 | [const std::vector&] 145 | The input candidate rois 146 | @param[out] conveyor [RoisConveyor*] The output conveyor 147 | struct 148 | @return void 149 | **/ 150 | static void fromCandidateRoiMsgToConveyor( 151 | const std::vector& 152 | candidateRoisVector, 153 | RoisConveyor* conveyor); 154 | 155 | /** 156 | @brief Unpacks the the RoisConveyor struct for the 157 | candidate rois. 158 | @param[in] roisMsg 159 | [flir_lepton_msgs::CandidateRoisVectorMsg&] The input 160 | candidate rois message obtained through the processor node 161 | @param[out] conveyor [RoisConveyor*] The output conveyor 162 | struct 163 | @param[out] image [cv::Mat*] The output image 164 | @param[in] encoding [const std::string&] The encoding used for 165 | @return void 166 | **/ 167 | static void unpackMessage( 168 | const flir_lepton_msgs::CandidateRoisVectorMsg& roisMsg, 169 | RoisConveyor* conveyor, 170 | cv::Mat* image, 171 | const std::string& encoding); 172 | 173 | /** 174 | @brief Convert the Float32MultiArray data to cv::Mat. 175 | Its cv format will be CV_8UC1. 176 | @param[in] inArray [const std_msgs::Float32MultiArray&] 177 | The input MultiArray 178 | @return cv::Mat 179 | **/ 180 | static cv::Mat convertFloat32MultiArrayToMat( 181 | const std_msgs::Float32MultiArray& inArray); 182 | }; 183 | 184 | } // namespace flir_lepton_image_processing 185 | } // namespace flir_lepton 186 | 187 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MESSAGE_CONVERSIONS_H 188 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/morphological_operators.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MORPHOLOGICAL_OPERATORS_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MORPHOLOGICAL_OPERATORS_H 40 | 41 | #include "utils/visualization.h" 42 | 43 | /** 44 | @brief The namespaces for this package 45 | **/ 46 | namespace flir_lepton 47 | { 48 | namespace flir_lepton_image_processing 49 | { 50 | /** 51 | @class Morphology 52 | @brief Provides methods for image binary morphological operators 53 | **/ 54 | class Morphology 55 | { 56 | public: 57 | /** 58 | @brief Performs steps of closing 59 | @param img [cv::Mat*] The input image in CV_8UC1 format 60 | @param steps [const int&] Number of operator steps 61 | @param visualize [const bool&] True for step-by-step visualization 62 | @return void 63 | **/ 64 | static void closing(cv::Mat* img, const int& steps, 65 | const bool& visualize = false); 66 | 67 | /** 68 | @brief Performs steps of dilation 69 | @param img [cv::Mat*] The input image in CV_8UC1 format 70 | @param steps [const int&] Number of operator steps 71 | @param visualize [const bool&] True for step-by-step visualization 72 | @return void 73 | **/ 74 | static void dilation(cv::Mat* img, const int& steps, 75 | const bool& visualize = false); 76 | 77 | /** 78 | @brief Performs steps of dilation. Each non-zero pixel is assigned 79 | the maximum value of its non-zero neighbors. 80 | @param img [cv::Mat&*] The input image in CV_8UC1 format 81 | @param steps [const int&] Number of operator steps 82 | @param visualize [const bool&] True for step-by-step visualization 83 | @return void 84 | **/ 85 | static void dilationRelative(cv::Mat* img, const int& steps, 86 | const bool& visualize = false); 87 | 88 | /** 89 | @brief Performs steps of erosion 90 | @param img [cv::Mat&] The input image in CV_8UC1 format 91 | @param steps [const int&] Number of operator steps 92 | @param visualize [const bool&] True for step-by-step visualization 93 | @return void 94 | **/ 95 | static void erosion(cv::Mat* img, const int& steps, 96 | const bool& visualize = false); 97 | 98 | /** 99 | @brief Checks if a kernel in a specific point in an image is satisfied 100 | Caution: this method presupposes that @param center does not lie on 101 | the edges of @param img 102 | @param kernel [const char [3][3]] The kernel 103 | @param img [const cv::Mat&] The input image (uchar) 104 | @param center [const cv::Point&] The center of the kernel 105 | @return bool : True on match 106 | **/ 107 | static bool kernelCheck(const char kernel[3][3], const cv::Mat& img, 108 | const cv::Point& center); 109 | 110 | /** 111 | @brief Performs steps of opening 112 | @param img [cv::Mat*] The input image in CV_8UC1 format 113 | @param steps [const int&] Number of operator steps 114 | @param visualize [const bool&] True for step-by-step visualization 115 | @return void 116 | **/ 117 | static void opening(cv::Mat* img, const int& steps, 118 | const bool& visualize = false); 119 | 120 | /** 121 | @brief Performs steps of strict pruning (removes more stuff) 122 | Caution: This method presupposes that the input image @param img is 123 | a thinned image 124 | @param img [cv::Mat*] The input image in CV_8UC1 format 125 | @param steps [const int&] Number of operator steps 126 | @return void 127 | **/ 128 | static void pruningStrictIterative(cv::Mat* inImage, const int& steps); 129 | 130 | /** 131 | @brief Performs steps of thinning 132 | (http://homepages.inf.ed.ac.uk/rbf/HIPR2/thin.htm) 133 | @param inImage [const cv::Mat&] The input image in CV_8UC1 format 134 | @param outImage [cv::Mat*] The input image in CV_8UC1 format 135 | @param steps [const int&] Number of operator steps 136 | @param visualize [const bool&] True for step-by-step visualization 137 | @return void 138 | **/ 139 | static void thinning(const cv::Mat& inImage, cv::Mat* outImage, 140 | const int& steps, const bool& visualize = false); 141 | }; 142 | 143 | } // namespace flir_lepton_image_processing 144 | } // namespace flir_lepton 145 | 146 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_MORPHOLOGICAL_OPERATORS_H 147 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/outline_discovery.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Alexandros Philotheou 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_OUTLINE_DISCOVERY_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_OUTLINE_DISCOVERY_H 40 | 41 | #include "utils/morphological_operators.h" 42 | 43 | /** 44 | @brief The namespaces for this package 45 | **/ 46 | namespace flir_lepton 47 | { 48 | namespace flir_lepton_image_processing 49 | { 50 | /** 51 | @class OutlineDiscovery 52 | @brief Provides methods for detection of thermal roi's outlines 53 | **/ 54 | class OutlineDiscovery 55 | { 56 | public: 57 | /** 58 | @brief Implements the brushfire algorithm for one blob keypoint 59 | in order to find its outline points 60 | @param[in] inKeyPoint [const cv::KeyPoint&] The keypoint 61 | @param[in] edgesImage [cv::Mat*] The input image 62 | @param[out] blobOutlineVector [std::vector*] 63 | The output vector containing the blob's outline 64 | @param[out] blobArea [float*] The area of the blob 65 | @return void 66 | **/ 67 | static void brushfireKeypoint( 68 | const cv::KeyPoint& inKeyPoint, 69 | cv::Mat* edgesImage, 70 | std::vector* blobOutlineVector, 71 | float* blobArea); 72 | 73 | /** 74 | @brief Implements the brushfire algorithm for all blob keypoints 75 | in order to find blobs' outlines 76 | @param[in] inKeyPoints [const std::vector&] The keypoints 77 | @param[in] edgesImage [cv::Mat*] The input image 78 | @param[out] blobsOutlineVector [std::vector >*] 79 | The output vector containing the blobs' outline 80 | @param[out] blobsArea [std::vector*] The area of each blob 81 | @return void 82 | **/ 83 | static void brushfireKeypoints( 84 | const std::vector& inKeyPoints, 85 | cv::Mat* edgesImage, 86 | std::vector >* blobsOutlineVector, 87 | std::vector* blobsArea); 88 | 89 | /** 90 | @brief Implements the brushfire algorithm. Its specific purpose is 91 | to find the points between a blob's outline and its bounding box 92 | (not necessarily one of least area). 93 | @param[in] inPoint [const cv::Point2f&] The input point 94 | @param[in] inImage [cv::Mat*] The input image 95 | @param[out] visited [std::set&] The points between 96 | areas of non-zero value pixels. 97 | @return void 98 | **/ 99 | static void brushfirePoint(const cv::Point2f& inPoint, 100 | cv::Mat* inImage, 101 | std::set* visited); 102 | 103 | /** 104 | @brief Given an image with a single, compact concentration 105 | of non-zero value pixels on top zero-value ones, this method 106 | extracts the outline pixels of the concentration. 107 | @param[in] image [const cv::Mat&] The input image, in CV_8UC1 format 108 | @param[out] outline [std::vector] The outline points 109 | of the concentration of points 110 | @return void 111 | **/ 112 | static void getOutlineOfMask(const cv::Mat& image, 113 | std::vector* outline); 114 | 115 | /** 116 | @brief With an binary input image (quantized in 0 and 255 levels), 117 | this function fills closed regions, at first, and then extracts 118 | the outline of each region. Used when there is a closed region 119 | with garbage pixels with a value of 255 within it. 120 | Caution: The outline of ALL shapes is computed: if there are 121 | closed shapes inside other closed shapes, their outline is detected 122 | and included in the output image. 123 | @param[in,out] inImage [cv::Mat*] The input image 124 | @return void 125 | **/ 126 | static void getShapesClearBorder(cv::Mat* inImage); 127 | 128 | /** 129 | @brief With an binary input image (quantized in 0 and 255 levels), 130 | this function fills closed regions, at first, and then extracts the 131 | outermost outline of each region. 132 | Used when there is a closed region with garbage pixels with 133 | a value of 255 within it. 134 | Caution: Only the outermost outline of shapes is computed. If there are 135 | closed shapes inside of other closed shapes, only the latter's outline 136 | will be computed, as opposed to the getShapesClearBorder function 137 | @param[in,out] inImage [cv::Mat*] The input image 138 | @return void 139 | **/ 140 | static void getShapesClearBorderSimple(cv::Mat* inImage); 141 | 142 | /** 143 | @brief Implements a raycast algorithm for a blob keypoint in order 144 | to find its outline points. The output is a vector of connected points. 145 | @param[in] inKeyPoint [const cv::KeyPoint&] The keypoint 146 | @param[in] edgesImage [cv::Mat*] The input image 147 | @param[in] partitions [const int&] The number of directions 148 | towards which the outline of the blob will be sought, 149 | or the number of partitions in which the blob will be divided by 150 | the rays. Same deal. 151 | @param[in] findArea [const bool&] Indicates whether to calculate 152 | the area of the blob 153 | @param[out] blobOutlineVector [std::vector*] 154 | The output vector containing the blobs' (rough approximate) outline 155 | @param[out] blobArea [float*] The blob's area. Non-zero if @param 156 | findArea true 157 | @return void 158 | **/ 159 | static void raycastKeypoint( 160 | const cv::KeyPoint& inKeyPoint, 161 | cv::Mat* edgesImage, 162 | const int& partitions, 163 | const bool& findArea, 164 | std::vector* blobOutlineVector, 165 | float* blobArea); 166 | 167 | /** 168 | @brief Implements a raycast algorithm for all blob keypoints in order 169 | to find blobs' outlines. The output is a vector containing a coherent 170 | vector of points. 171 | @param[in] inKeyPoints [const std::vector&] The keypoints 172 | @param[in] edgesImage [cv::Mat*] The input image 173 | @param[in] partitions [const int&] The number of directions 174 | towards which the outline of the blob will be sought, 175 | or the number of partitions in which the blob will be divided by 176 | the rays. Same deal. 177 | @param[out] blobsOutlineVector [std::vector >*] 178 | The output vector containing the blobs' (rough approximate) outline 179 | @param[out] blobsArea [std::vector*] The area of each blob 180 | @return void 181 | **/ 182 | static void raycastKeypoints( 183 | const std::vector& inKeyPoints, 184 | cv::Mat* edgesImage, 185 | const int& partitions, 186 | std::vector >* blobsOutlineVector, 187 | std::vector* blobsArea); 188 | }; 189 | 190 | } // namespace flir_lepton_image_processing 191 | } // namespace flir_lepton 192 | 193 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_OUTLINE_DISCOVERY_H 194 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/parameters.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_PARAMETERS_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_PARAMETERS_H 41 | 42 | #include "utils/defines.h" 43 | #include 44 | #include 45 | 46 | /** 47 | @brief The namespaces for this package 48 | **/ 49 | namespace flir_lepton 50 | { 51 | namespace flir_lepton_image_processing 52 | { 53 | /** 54 | @struct Parameters 55 | @brief Provides flexibility by parameterizing variables needed by the 56 | hole detector package 57 | **/ 58 | struct Parameters 59 | { 60 | // Blob detection - specific parameters 61 | struct Blob 62 | { 63 | static int min_threshold; 64 | static int max_threshold; 65 | static int threshold_step; 66 | static int min_area; 67 | static int max_area; 68 | static double min_convexity; 69 | static double max_convexity; 70 | static double min_inertia_ratio; 71 | static double max_circularity; 72 | static double min_circularity; 73 | static bool filter_by_color; 74 | static bool filter_by_circularity; 75 | }; 76 | 77 | // Debug-specific parameters 78 | struct Debug 79 | { 80 | // Show the thermal image that arrives in the thermal node 81 | static bool show_thermal_image; 82 | 83 | // In the terminal's window, show the probabilities of candidate rois 84 | static bool show_probabilities; 85 | 86 | static bool show_find_rois; 87 | static int show_find_rois_size; 88 | 89 | static bool show_denoise_edges; 90 | static int show_denoise_edges_size; 91 | 92 | static bool show_connect_pairs; 93 | static int show_connect_pairs_size; 94 | 95 | static bool show_get_shapes_clear_border; 96 | static int show_get_shapes_clear_border_size; 97 | }; 98 | 99 | // Parameters specific to the Thermal node 100 | struct Thermal 101 | { 102 | // The thermal detection method 103 | // If set to 0 process the binary image acquired from temperatures MultiArray 104 | // If set to 1 process the sensor/Image from thermal sensor 105 | static int detection_method; 106 | 107 | // The probability extraction method 108 | // 0 for Gaussian function 109 | // 1 for Logistic function 110 | static int probability_method; 111 | static float min_thermal_probability; 112 | 113 | // Gausian variables 114 | static float optimal_temperature; 115 | static float tolerance; 116 | 117 | // Logistic variables 118 | static float low_acceptable_temperature; 119 | static float high_acceptable_temperature; 120 | 121 | static float left_tolerance; 122 | static float right_tolerance; 123 | 124 | // Low and High acceptable temperatures for thresholding 125 | static float low_temperature; 126 | static float high_temperature; 127 | }; 128 | 129 | // Thermal image parameters 130 | struct ThermalImage 131 | { 132 | // Thermal image width and height 133 | static int WIDTH; 134 | static int HEIGHT; 135 | }; 136 | 137 | // Edge detection specific parameters 138 | struct Edge 139 | { 140 | // canny parameters 141 | static int canny_ratio; 142 | static int canny_kernel_size; 143 | static int canny_low_threshold; 144 | static int canny_blur_noise_kernel_size; 145 | 146 | // The opencv edge detection method: 147 | // 0 for the Canny edge detector 148 | // 1 for the Scharr edge detector 149 | // 2 for the Sobel edge detector 150 | // 3 for the Laplacian edge detector 151 | // 4 for mixed Scharr / Sobel edge detection 152 | static int edge_detection_method; 153 | 154 | // Threshold parameters 155 | static int denoised_edges_threshold; 156 | 157 | // When mixed edge detection is selected, this toggle switch 158 | // is needed in order to shift execution from one edge detector 159 | // to the other. 160 | // 1 for the Scharr edge detector, 161 | // 2 for the Sobel edge detector 162 | static int mixed_edges_toggle_switch; 163 | }; 164 | 165 | // Image representation specific parameters 166 | struct Image 167 | { 168 | // The thermal sensor's horizontal field of view 169 | static float horizontal_field_of_view; 170 | 171 | // The thermal sensor's vertical field of view 172 | static float vertical_field_of_view; 173 | 174 | // Depth and RGB images' representation method. 175 | // 0 if image used is used as obtained from the image sensor 176 | // 1 through wavelet analysis 177 | static int image_representation_method; 178 | 179 | // Method to scale the CV_32F images to CV_8UC1 180 | static int scale_method; 181 | 182 | // Term criteria for segmentation purposes 183 | static int term_criteria_max_iterations; 184 | static double term_criteria_max_epsilon; 185 | }; 186 | 187 | 188 | 189 | // Outline discovery specific parameters 190 | struct Outline 191 | { 192 | // The detection method used to obtain the outline of a blob 193 | // 0 for detecting by means of brushfire 194 | // 1 for detecting by means of raycasting 195 | static int outline_detection_method; 196 | 197 | // When using raycast instead of brushfire to find the (approximate here) 198 | // outline of blobs, raycast_keypoint_partitions dictates the number of 199 | // rays, or equivalently, the number of partitions in which the blob is 200 | // partitioned in search of the blob's borders 201 | static int raycast_keypoint_partitions; 202 | 203 | // Loose ends connection parameters 204 | static int AB_to_MO_ratio; 205 | static int minimum_curve_points; 206 | }; 207 | }; 208 | 209 | } // namespace flir_lepton_image_processing 210 | } // namespace flir_lepton 211 | 212 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_PARAMETERS_H 213 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/roi_filters.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_ROI_FILTERS_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_ROI_FILTERS_H 40 | 41 | #include "utils/edge_detection.h" 42 | #include "utils/outline_discovery.h" 43 | #include "utils/bounding_box_detection.h" 44 | #include "utils/thermal_rois_conveyor.h" 45 | #include 46 | 47 | /** 48 | @brief The namespaces for this package 49 | **/ 50 | namespace flir_lepton 51 | { 52 | namespace flir_lepton_image_processing 53 | { 54 | /** 55 | @class RoiFilters 56 | @brief Provides methods for validating blobs 57 | **/ 58 | class RoiFilters 59 | { 60 | public: 61 | /** 62 | @brief Given a set of keypoints and an edges image, this function 63 | returns the valid keypoints and for each one, its respective, least 64 | area rotated bounding box and the points of its outline. 65 | @param[in] keyPoints [const std::vector&] 66 | The original keypoints found. 67 | @param[in] thermalImageEdges [cv::Mat*] The original thermal edges image 68 | @param[in] detectionMethod [const int&] The method by which the 69 | outline of a blob is obtained. 70 | 0 means by means of brushfire, 71 | 1 by means of raycasting 72 | @param[in,out] conveyor [RoisConveyor*] A struct that contains 73 | the final valid candidate rois 74 | @return void 75 | **/ 76 | static void validateBlobs( 77 | const std::vector& keyPoints, 78 | cv::Mat* thermalImageEdges, 79 | const int& detectionMethod, 80 | RoisConveyor* conveyor); 81 | 82 | /** 83 | @brief This functions takes as input arguments a keypoints vector of 84 | size N, a rectangles vector of size M (the rectangle is represented 85 | by its 4 vertices so that the input can be either a Rectangle or a 86 | Rotated Rectangle) and a vector with the area of each rectangle 87 | with purpose to identify the least area rectangle in which a 88 | keypoint resides. It outputs a vector of keypoints (each keypoint must 89 | reside in at least one rectangle) and a vector of rectangles (again 90 | represented by its 4 vertices). There is a one-to-one association 91 | between the keypoints and the rectangles. 92 | @param[in] inKeyPoints [const std::vector&] The key points 93 | @param[in] inRectangles [const std::vector >&] 94 | The rectangles found 95 | @param[in] inRectanglesArea [const std::vector&] The area of each 96 | rectangle 97 | @param[in] inContours [const std::vector >&] 98 | The outline of each blob found 99 | @param[out] conveyor [RoisConveyor*] The container of vector of blobs' 100 | keypoints, outlines and areas 101 | @return void 102 | **/ 103 | static void validateKeypointsToRectangles( 104 | const std::vector& inKeyPoints, 105 | const std::vector >& inRectangles, 106 | const std::vector& inRectanglesArea, 107 | const std::vector >& inContours, 108 | RoisConveyor* conveyor); 109 | }; 110 | 111 | } // namespace flir_lepton_image_processing 112 | } // namespace flir_lepton 113 | 114 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_ROI_FILTERS_H 115 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/thermal_rois_conveyor.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_THERMAL_ROIS_CONVEYOR_H 40 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_THERMAL_ROIS_CONVEYOR_H 41 | 42 | #include 43 | #include "utils/defines.h" 44 | 45 | /** 46 | @brief The namespaces for this package 47 | **/ 48 | namespace flir_lepton 49 | { 50 | namespace flir_lepton_image_processing 51 | { 52 | /** 53 | @brief The structure that represents a single thermal region of interest. 54 | @param keypoint [cv::KeyPoint] The roi's keypoint 55 | @param rectangle [std::vector] The vector of the roi's 56 | rotated bounding box vertices 57 | @param outline [std::vector] The vector of the roi's 58 | outline points 59 | **/ 60 | struct RoiConveyor 61 | { 62 | cv::KeyPoint keypoint; 63 | std::vector rectangle; 64 | std::vector outline; 65 | 66 | // For Thermal Rois these variables get filled too. 67 | float roiProbability; 68 | float roiTemperature; 69 | }; 70 | 71 | typedef boost::shared_ptr RoiConveyorPtr; 72 | typedef boost::shared_ptr RoiConveyorConstPtr; 73 | 74 | /** 75 | A struct conveying information about multiple thermal rois 76 | @param header [std_msgs::Header] Header in respect with the frame of rois 77 | @param rois [std::vector] The vector of rois 78 | **/ 79 | struct RoisConveyor 80 | { 81 | std_msgs::Header header; 82 | std::vector rois; 83 | 84 | /** 85 | @brief Returns the size of the @param rois vector 86 | @param void 87 | @return [unsigned int] The number of entries in the @param rois vector 88 | **/ 89 | unsigned int size() const 90 | { 91 | return rois.size(); 92 | } 93 | }; 94 | 95 | typedef boost::shared_ptr RoisConveyorPtr; 96 | typedef boost::shared_ptr RoisConveyorConstPtr; 97 | 98 | /** 99 | @Class RoisConveyorUtils 100 | @brief Provides methods pertinent to the RoisConveyor struct 101 | **/ 102 | class RoisConveyorUtils 103 | { 104 | public: 105 | /** 106 | @brief Appends one RoisConveyor struct to another. 107 | @param[in] src [const RoisConveyor&] The source conveyor 108 | @param[out] dst [RoisConveyor*] The destination conveyor 109 | @return void 110 | **/ 111 | static void append(const RoisConveyor& src, RoisConveyor* dst); 112 | 113 | /** 114 | @brief Appends a dummy RoisConveyor to a RoisConveyor struct 115 | @param[in] rectangleUpperLeft [const cv::Point2f&] The upper left 116 | vertex of the bounding rectangle 117 | @param[in] outlineUpperLeft [const cv::Point2f] The upper left 118 | vertex of the roi's outline 119 | @param[in] rx [const int&] The width of the rectangle 120 | @param[in] ry [const int&] The height of the rectangle 121 | @param[in] ox [const int&] The width of the outline rectangle 122 | @param[in] ry [const int&] The height of the outline rectangle 123 | @param[in,out] conveyor [RoisConveyor*] The conveyor to which the 124 | dummy RoisConveyor will be appended 125 | @return void 126 | **/ 127 | static void appendDummyConveyor( 128 | const cv::Point2f& rectangleUpperLeft, 129 | const cv::Point2f& outlineUpperLeft, 130 | const int& rx, const int& ry, 131 | const int& ox, const int& oy, 132 | RoisConveyor* conveyor); 133 | 134 | /** 135 | @brief Hollows a RoisConveyor struct, deleting every entry in it 136 | @param[in,out] conveyor [RoisConveyor*] The conveyor struct that will 137 | be cleared 138 | @return void 139 | **/ 140 | static void clear(RoisConveyor* conveyor); 141 | 142 | /** 143 | @brief Copies one RoisConveyor struct to another. If the dst conveyor 144 | is not empty, it empties it first, and then copies the src to the dst 145 | @param[in] src [const RoisConveyor&] The source struct 146 | @param[out] dst [RoisConveyor*] The destination struct 147 | @return void 148 | **/ 149 | static void copyTo(const RoisConveyor& src, RoisConveyor* dst); 150 | 151 | /** 152 | @brief Generates a vector of cv::Point2f that represents the 4 vertices 153 | of a rectangle 154 | @param[in] upperLeft [const cv::Point2f&] The upper left vertex point 155 | of the rectangle 156 | @param[in] x [const int&] The length at x direction 157 | @param[in] y [const int&] The length at y direction 158 | @param[in] intent [const int&] 0 for vertices' 159 | construction, 1 for a coherent outline construction 160 | @return std::vector A vector of four vertices 161 | of type cv::Point2f 162 | **/ 163 | static std::vector generateRectangle( 164 | const cv::Point2f& upperLeft, const int& x, const int& y, 165 | const int& intent); 166 | 167 | /** 168 | @brief Extracts the specified roi from a RoisConveyor into a new 169 | RoisConveyor struct that is returned 170 | @param[in] conveyor [const RoisConveyor&] The RoisConveyor struct 171 | @param[in] index [const int&] The index of the roi inside the conveyor 172 | @return A RoisConveyor struct that containes the index-th roi of the 173 | conveyor 174 | **/ 175 | static RoisConveyor getRoi(const RoisConveyor& conveyor, 176 | const int& index); 177 | 178 | /** 179 | @brief Given two sources of struct RoisConveyor, this function 180 | merge them into one struct. 181 | @param[in] srcA [const RoisConveyor&] The first RoisConveyor source 182 | @param[in] srcB [const RoisConveyor&] The second RoisConveyor source 183 | @param[out] dst [RoisConveyor*] The final struct 184 | **/ 185 | static void merge(const RoisConveyor& srcA, 186 | const RoisConveyor& srcB, RoisConveyor* dst); 187 | 188 | /** 189 | @brief Prints data pertaining to the contents of a RoisConveyor struct, 190 | that is, the keypoints, rectangle points and outline points of the 191 | rois it contains 192 | @param[in] conveyor [const RoisConveyor&] The conveyor 193 | @param[in] id [const int&] The identifier of a specific roi 194 | @return void 195 | **/ 196 | static void print(const RoisConveyor& conveyor, const int& id = -1); 197 | 198 | /** 199 | @brief Replaces an entire RoisConveyor struct with another 200 | @param[in] src [const RoisConveyor&] The source conveyor struct 201 | @param[out] dst [RoisConveyor*] The destination conveyor struct 202 | @return void 203 | **/ 204 | static void replace(const RoisConveyor& src, RoisConveyor* dst); 205 | 206 | /** 207 | @brief Deletes a roi from RoisConveyor struct, 208 | @param[in,out] conveyor [RoisConveyor*] The conveyor struct from which 209 | the roi will be removed 210 | @param[in] id [const int&] The index of the roi in the conveyor 211 | @return void 212 | **/ 213 | static void removeRoi(RoisConveyor* conveyor, const int& id); 214 | 215 | /** 216 | @brief Replaces a specified roi from a RoisConveyor dst struct 217 | with the roi of index srcIndex of the src RoisConveyor struct entry 218 | @param[in] src [const RoisConveyor&] The RoisConveyor source struct 219 | @param[in] srcIndex [const int&] The index of the roi inside the 220 | src conveyor that will be copied into the dst RoisConveyor struct, 221 | in the dstIndex position 222 | @param[out] dst [RoisConveyor*] The RoisConveyor destination struct 223 | @param[in] dstIndex [const int&] The index of the roi inside the 224 | dst conveyor that will be replaced by the srcIndex-th of the src 225 | RoisConveyor struct 226 | @return void 227 | **/ 228 | static void replaceRoi(const RoisConveyor& src, 229 | const int& srcIndex, RoisConveyor* dst, const int& dstIndex); 230 | 231 | /** 232 | @brief Shuffles the contents of a RoisConveyor 233 | @param[in,out] src [RoisConveyor*] The conveyor 234 | @return void 235 | **/ 236 | static void shuffle(RoisConveyor* src); 237 | }; 238 | 239 | } // namespace flir_lepton_image_processsing 240 | } // namespace flir_lepton 241 | 242 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_THERMAL_ROIS_CONVEYOR_H 243 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/timer.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_TIMER_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_TIMER_H 40 | 41 | #ifndef DEFINES 42 | #define DEFINES 43 | #define PKG_NAME "flir_lepton_image_processing" 44 | #endif // DEFINES 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | class Timer 56 | { 57 | typedef std::map Msd; 58 | typedef std::map Mfs; 59 | typedef std::map::iterator MfsIt; 60 | typedef std::map Msul; 61 | typedef std::map::iterator MsdIt; 62 | typedef std::map::const_iterator MsdCIt; 63 | typedef std::pair Psd; 64 | typedef std::pair Psul; 65 | 66 | static Msd times; 67 | static Msd max_time; 68 | static Msd min_time; 69 | static Msd mean_time; 70 | static Msd sum_time; 71 | static Msul count; 72 | static struct timeval msTime; 73 | static std::map > timer_tree; 74 | 75 | static std::string top_node; 76 | 77 | Timer(void){} 78 | 79 | static void printMsInternal(double t); 80 | static void printSecInternal(double t); 81 | static void printMinInternal(double t); 82 | static void printHoursInternal(double t); 83 | static void printLiteralInternal(double t); 84 | 85 | public: 86 | static void start(std::string timerId, std::string father = "", bool top = false); 87 | static double stop(std::string timerId); 88 | static double mean(std::string timerId); 89 | static void tick(std::string timerId); 90 | static void printMs(std::string timerId); 91 | static void printSec(std::string timerId); 92 | static void printMin(std::string timerId); 93 | static void printHours(std::string timerId); 94 | static void printLiteral(std::string timerId); 95 | static void printLiteralMean(std::string timerId, std::string identation = ""); 96 | static void printAll(void); 97 | static void printAllMeans(void); 98 | static void printAllMeansTree(void); 99 | static void printIterativeTree(std::string node, std::string identation); 100 | }; 101 | 102 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_TIMER_H 103 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/include/flir_lepton_image_processing/utils/visualization.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #ifndef FLIR_LEPTON_IMAGE_PROCESSING_UTILS_VISUALIZATION_H 39 | #define FLIR_LEPTON_IMAGE_PROCESSING_UTILS_VISUALIZATION_H 40 | 41 | #include "utils/parameters.h" 42 | #include "utils/thermal_rois_conveyor.h" 43 | 44 | /** 45 | @brief The namespaces for this package 46 | **/ 47 | namespace flir_lepton 48 | { 49 | namespace flir_lepton_image_processing 50 | { 51 | /** 52 | @class Visualization 53 | @brief Provides methods for image visualization 54 | **/ 55 | class Visualization 56 | { 57 | public: 58 | /** 59 | @brief Shows multiple images in one window 60 | @param[in] title [const std::string&] The window's title 61 | @param[in] imgs [const std::vector&] The images to be shown 62 | @param[in] titles [const std::vector&] The titles for each 63 | image 64 | @param[in] maxSize [const int&] The maximum size of the window 65 | @param[in] ms [const int&] How many seconds the showing lasts 66 | @return [cv::Mat] An image featuring multiple images 67 | **/ 68 | static cv::Mat multipleShow( 69 | const std::string& title, 70 | const std::vector& imgs, 71 | const std::vector& titles, 72 | const int& maxSize, 73 | const int& ms); 74 | 75 | /** 76 | @brief Scales an image from its original format to CV_8UC1 77 | @param[in] inImage [const cv::Mat&] The image to show 78 | @param[in] ms [const int&] How many ms the showing lasts 79 | @return void 80 | **/ 81 | static cv::Mat scaleImageForVisualization( 82 | const cv::Mat& inImage, 83 | const int& method); 84 | 85 | /** 86 | @brief Overrides the cv::imshow function. 87 | @param[in] windowTitle [const std::string&] The window title 88 | @param[in] inImage [const cv::Mat&] The image to show 89 | @param[in] ms [const int&] How many ms the showing lasts 90 | @return void 91 | **/ 92 | static void show( 93 | const std::string& windowTitle, 94 | const cv::Mat& inImage, 95 | const int& ms); 96 | 97 | /** 98 | @brief Depicts the contents of a RoisConveyor on an image 99 | @param[in] windowTitle [const std::string&] The window title 100 | @param[in] inImage [const cv::Mat&] The image to show 101 | @param[in] conveyor [const RoisConveyor&] The rois conveyor 102 | @param[in] ms [const int&] How many ms the showing lasts 103 | @param[in] msgs [const std::vector&] Message to show to 104 | each keypoint 105 | @param[in] hz [const float&] If positive holds the Hz 106 | @return [cv::Mat] The drawn image 107 | **/ 108 | static cv::Mat showRois( 109 | const std::string& windowTitle, 110 | const cv::Mat& inImage, 111 | const RoisConveyor& conveyor, 112 | const int& ms, 113 | const std::vector& msgs, 114 | const float& hz = -1); 115 | 116 | /** 117 | @brief Depicts the keypoints and bounding boxes 118 | @param[in] windowTitle [const std::string&] The window title 119 | @param[in] inImage [const cv::Mat&] The image to show 120 | @param[in] ms [const int&] How many ms the showing lasts 121 | @param[in] keypoints [const std::vector&] The keypoints 122 | @return void 123 | **/ 124 | static cv::Mat showKeypoints( 125 | const std::string& windowTitle, 126 | const cv::Mat& inImage, 127 | const int& ms, 128 | const std::vector& keypoints); 129 | 130 | /** 131 | @brief Overrides the cv::imshow function. Provides image scaling from 132 | the image's original forma to CV_8UC1 format 133 | @param[in] windowTitle [const std::string&] The window title 134 | @param[in] inImage [const cv::Mat&] The image to show 135 | @param[in] ms [const int&] How many ms the showing lasts 136 | @return void 137 | **/ 138 | static void showScaled( 139 | const std::string& windowTitle, 140 | const cv::Mat& inImage, 141 | const int& ms); 142 | }; 143 | 144 | } // namespace flir_lepton_image_processing 145 | } // namespace flir_lepton 146 | 147 | #endif // FLIR_LEPTON_IMAGE_PROCESSING_UTILS_VISUALIZATION_H 148 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/launch/flir_lepton_image_processing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 18 | 19 | 20 | 21 | 22 | 23 | 30 | 31 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_lepton_image_processing 4 | 0.1.0 5 | Contains thermal ROI detection node using Flir-Lepton sensor. 6 | 7 | Angelos Triantafyllidis 8 | 9 | BSD 10 | 11 | Angelos Triantafyllidis 12 | Alexandros Philotheou 13 | Manos Tsardoulias 14 | 15 | catkin 16 | 17 | roslint 18 | 19 | roscpp 20 | cv_bridge 21 | 22 | dynamic_reconfigure 23 | flir_lepton_msgs 24 | 25 | 26 | rostest 27 | rosunit 28 | 29 | 30 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/processing_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(${PROJECT_NAME}_process 2 | process.cpp 3 | thermal_roi_detector.cpp 4 | ) 5 | 6 | target_link_libraries(${PROJECT_NAME}_process 7 | ${PROJECT_NAME}_utils 8 | ${catkin_LIBRARIES}) 9 | 10 | add_dependencies(${PROJECT_NAME}_process 11 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 12 | ${catkin_EXPORTED_TARGETS}) 13 | 14 | add_executable(thermal_camera_node process_node.cpp) 15 | 16 | target_link_libraries(thermal_camera_node 17 | ${PROJECT_NAME}_process 18 | ${catkin_LIBRARIES}) 19 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/processing_node/process_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Angelos Triantafyllidis 36 | *********************************************************************/ 37 | 38 | #include "processing_node/process.h" 39 | 40 | /** 41 | @brief Main function of the thermal node 42 | @param argc [int] Number of input arguments 43 | @param argv [char**] The input arguments 44 | @return int : 0 for success 45 | **/ 46 | int main(int argc, char** argv) 47 | { 48 | ros::init(argc, argv, "process_node"); 49 | flir_lepton::flir_lepton_image_processing::Process process; 50 | ros::spin(); 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/processing_node/thermal_roi_detector.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias, 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #include "processing_node/thermal_roi_detector.h" 40 | 41 | /** 42 | @brief The namespaces for this package 43 | **/ 44 | namespace flir_lepton 45 | { 46 | namespace flir_lepton_image_processing 47 | { 48 | /** 49 | @brief Finds the Rois in the thermal image in CV_8UC1 format. 50 | First, the edges of the thermal image are detected. 51 | Then, keypoints of blobs are detected in the above image. 52 | Finally, the potential Rois outline is found, along with the bounding 53 | boxes of those outlines. 54 | @param[in] thermalImage [const cv::Mat&] The thermal 55 | image in CV_8UC1 format. 56 | @return RoisConveyor The struct that contains the rois found. 57 | **/ 58 | RoisConveyor RoiDetector::findRois(const cv::Mat& thermalImage) 59 | { 60 | #ifdef DEBUG_TIME 61 | Timer::start("findRois", "inputThermalImageCallback"); 62 | #endif 63 | 64 | #ifdef DEBUG_SHOW 65 | std::vector imgs; 66 | std::vector msgs; 67 | if (Parameters::Debug::show_find_rois) // Debug 68 | { 69 | std::string msg = LPATH(STR(__FILE__)) + STR(" ") + TOSTR(__LINE__); 70 | msg += " : Thermal image"; 71 | msgs.push_back(msg); 72 | cv::Mat tmp = Visualization::scaleImageForVisualization( 73 | thermalImage, Parameters::Image::scale_method); 74 | imgs.push_back(tmp); 75 | } 76 | #endif 77 | 78 | // Detect edges in the thermal image 79 | cv::Mat thermalImageEdges; 80 | EdgeDetection::computeThermalEdges(thermalImage, 81 | &thermalImageEdges); 82 | 83 | #ifdef DEBUG_SHOW 84 | if (Parameters::Debug::show_find_rois) // Debug 85 | { 86 | std::string msg = LPATH(STR(__FILE__)) + STR(" ") + TOSTR(__LINE__); 87 | msg += STR(" : Edges after denoise"); 88 | msgs.push_back(msg); 89 | cv::Mat tmp; 90 | thermalImageEdges.copyTo(tmp); 91 | imgs.push_back(tmp); 92 | } 93 | #endif 94 | 95 | // Find blobs in the edges image. Each blob is represented as 96 | // a keypoint which is the center of the blob found 97 | std::vector keyPoints; 98 | BlobDetection::detectBlobs(thermalImageEdges, &keyPoints); 99 | 100 | #ifdef DEBUG_SHOW 101 | if (Parameters::Debug::show_find_rois) // Debug 102 | { 103 | std::string msg = LPATH(STR(__FILE__)) + STR(" ") + TOSTR(__LINE__); 104 | msg += STR(" : Initial keypoints"); 105 | msgs.push_back(msg); 106 | imgs.push_back( 107 | Visualization::showKeypoints(msg, thermalImageEdges, -1, 108 | keyPoints)); 109 | } 110 | #endif 111 | 112 | // The final vectors of keypoints, rectangles and blobs' outlines. 113 | RoisConveyor conveyor; 114 | 115 | /** 116 | Get me blobs that their center point is inside the image, 117 | their bounding box is also entirely inside the image, and their area is 118 | greater than Parameters::bounding_box_min_area_threshold. 119 | Each keypoint is associated with exactly one rectangle. 120 | The end product here is a set of keypoints, a set of rectangles that 121 | enclose them and a set of the outlines of the blobs found, all tightly 122 | packed in the conveyor struct. 123 | **/ 124 | RoiFilters::validateBlobs( 125 | keyPoints, 126 | &thermalImageEdges, 127 | Parameters::Outline::outline_detection_method, 128 | &conveyor); 129 | 130 | #ifdef DEBUG_SHOW 131 | if (Parameters::Debug::show_find_rois) // Debug 132 | { 133 | std::string msg = LPATH(STR(__FILE__)) + STR(" ") + TOSTR(__LINE__); 134 | msg += STR(" : Blobs"); 135 | msgs.push_back(msg); 136 | imgs.push_back( 137 | Visualization::showRois( 138 | msg, 139 | thermalImage, 140 | conveyor, 141 | -1, 142 | std::vector())); 143 | } 144 | #endif 145 | 146 | #ifdef DEBUG_SHOW 147 | if (Parameters::Debug::show_find_rois) // Debug 148 | { 149 | // A vector of keypoints 150 | std::vector keypointsVector; 151 | 152 | for (int i = 0; i < conveyor.size(); i++) 153 | { 154 | keypointsVector.push_back(conveyor.rois[i].keypoint); 155 | } 156 | 157 | std::string msg = LPATH(STR(__FILE__)) + STR(" ") + TOSTR(__LINE__); 158 | msg += STR(" : Keypoints found from this process"); 159 | msgs.push_back(msg); 160 | imgs.push_back( 161 | Visualization::showKeypoints( 162 | msg, 163 | thermalImage, 164 | -1, 165 | keypointsVector)); 166 | } 167 | if (Parameters::Debug::show_find_rois) 168 | { 169 | Visualization::multipleShow("Thermal node", imgs, msgs, 170 | Parameters::Debug::show_find_rois_size, 1); 171 | } 172 | #endif 173 | 174 | #ifdef DEBUG_TIME 175 | Timer::tick("findRois"); 176 | #endif 177 | 178 | return conveyor; 179 | } 180 | 181 | } // namespace flir_lepton_image_processing 182 | } // namespace flir_lepton 183 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/blob_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #include "utils/blob_detection.h" 39 | 40 | /** 41 | @brief The namespaces for this package 42 | **/ 43 | namespace flir_lepton 44 | { 45 | namespace flir_lepton_image_processing 46 | { 47 | /** 48 | @brief Detects blobs in an image 49 | @param[in] inImage [const cv::Mat&] The input image 50 | @param[out] keyPointsOut [std::vector*] The ouput 51 | @return void 52 | **/ 53 | void BlobDetection::detectBlobs(const cv::Mat& inImage, 54 | std::vector* keyPointsOut) 55 | { 56 | #ifdef DEBUG_TIME 57 | Timer::start("detectBlobs", "findHoles"); 58 | #endif 59 | 60 | cv::SimpleBlobDetector::Params params; 61 | 62 | params.minThreshold = Parameters::Blob::min_threshold; // 40; 63 | params.maxThreshold = Parameters::Blob::max_threshold; // 60; 64 | params.thresholdStep = Parameters::Blob::threshold_step; 65 | 66 | params.minArea = Parameters::Blob::min_area; 67 | params.maxArea = Parameters::Blob::max_area; 68 | 69 | params.minConvexity = Parameters::Blob::min_convexity; // 0.6; 70 | params.maxConvexity = Parameters::Blob::max_convexity; 71 | 72 | params.minInertiaRatio = Parameters::Blob::min_inertia_ratio; // 0.5; 73 | 74 | params.maxCircularity = Parameters::Blob::max_circularity; 75 | params.minCircularity = Parameters::Blob::min_circularity; // 0.3; 76 | 77 | params.filterByColor = Parameters::Blob::filter_by_color; 78 | params.filterByCircularity = Parameters::Blob::filter_by_circularity; 79 | 80 | cv::SimpleBlobDetector blobDetector(params); 81 | blobDetector.create("SimpleBlob"); 82 | 83 | std::vector keyPoints; 84 | 85 | // detect blobs. store their center point 86 | blobDetector.detect(inImage, keyPoints); 87 | 88 | for (int keypointId = 0; keypointId < keyPoints.size(); keypointId++) 89 | { 90 | // if the keypoint is out of image limits, discard it 91 | if (keyPoints[keypointId].pt.x < inImage.cols && 92 | keyPoints[keypointId].pt.x >= 0 && 93 | keyPoints[keypointId].pt.y < inImage.rows && 94 | keyPoints[keypointId].pt.y >= 0) 95 | { 96 | keyPointsOut->push_back(keyPoints[keypointId]); 97 | } 98 | } 99 | 100 | #ifdef DEBUG_TIME 101 | Timer::tick("detectBlobs"); 102 | #endif 103 | } 104 | 105 | } // namespace flir_lepton_image_processing 106 | } // namespace flir_lepton 107 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/bounding_box_detection.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #include "utils/bounding_box_detection.h" 39 | 40 | /** 41 | @brief The namespaces for this package 42 | **/ 43 | namespace flir_lepton 44 | { 45 | namespace flir_lepton_image_processing 46 | { 47 | /** 48 | @brief Finds rotated bounding boxes from blob outlines. 49 | The blob's area must be larger than 50 | Parameters::bounding_box_min_area_threshold. 51 | The blob and its bounding rectangle must be inside the image's limits. 52 | @param[in] inImage [const cv::Mat&] The input image 53 | @param[in] blobsOutlineVector 54 | [const std::vector >&] 55 | The outline points of the blobs 56 | @param[in] blobsArea [const std::vector&] The blobs' area 57 | @param[out] outRectangles [std::vector< std::vector >*] 58 | The rectangles of the bounding boxes 59 | @return void 60 | **/ 61 | void BoundingBoxDetection::findRotatedBoundingBoxesFromOutline( 62 | const cv::Mat& inImage, 63 | const std::vector >& blobsOutlineVector, 64 | const std::vector& blobsArea, 65 | std::vector >* outRectangles) 66 | { 67 | #ifdef DEBUG_TIME 68 | Timer::start("findRotatedBoundingBoxesFromOutline", "validateBlobs"); 69 | #endif 70 | 71 | // Find the rotated rectangles for each blob based on its outline 72 | std::vector minRect; 73 | for (unsigned int i = 0; i < blobsOutlineVector.size(); i++) 74 | { 75 | // The area of the blob should be greater than a threshold, 76 | // so that tiny formations of pixels are not identified as blobs 77 | if (blobsArea[i] >= Parameters::Blob::min_area) 78 | { 79 | minRect.push_back(minAreaRect(cv::Mat(blobsOutlineVector[i]))); 80 | } 81 | } 82 | 83 | 84 | // For each rotated rectangle whose corresponding blob exceeds the minimum 85 | // area threshold, if its vertices reside within the image's boundaries, 86 | // store its vertices 87 | for (unsigned int i = 0; i < minRect.size(); i++) 88 | { 89 | // The for vertices of the rotated rectangle 90 | cv::Point2f rect_points[4]; 91 | minRect[i].points(rect_points); 92 | 93 | // Check if the vertices reside in the image's boundaries 94 | int numVerticesWithinImageLimits = 0; 95 | 96 | for (int j = 0; j < 4; j++) 97 | { 98 | if (rect_points[j].x < inImage.cols && 99 | rect_points[j].x >= 0 && 100 | rect_points[j].y < inImage.rows && 101 | rect_points[j].y >= 0) 102 | { 103 | numVerticesWithinImageLimits++; 104 | } 105 | } 106 | 107 | // If the rotated rectangle's edges reside outside the image's edges, 108 | // discard this rotated rectangle 109 | if (numVerticesWithinImageLimits < 4) 110 | { 111 | continue; 112 | } 113 | 114 | // If all four vertices reside within the image's boundaries, 115 | // store them in their respective position 116 | 117 | // Same as rect_points array, but vector 118 | std::vector rect_points_vector; 119 | 120 | for (int j = 0; j < 4; j++) 121 | { 122 | rect_points_vector.push_back(rect_points[j]); 123 | } 124 | 125 | // Push back the 4 vertices of rectangle i 126 | outRectangles->push_back(rect_points_vector); 127 | } 128 | 129 | #ifdef DEBUG_TIME 130 | Timer::tick("findRotatedBoundingBoxesFromOutline"); 131 | #endif 132 | } 133 | 134 | } // namespace flir_lepton_image_processing 135 | } // namespace flir_lepton 136 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/parameters.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | * Angelos Triantafyllidis 37 | *********************************************************************/ 38 | 39 | #include "utils/parameters.h" 40 | 41 | /** 42 | @brief The namespaces for this package 43 | **/ 44 | namespace flir_lepton 45 | { 46 | namespace flir_lepton_image_processing 47 | { 48 | //////////////////// Blob detection - specific parameters //////////////////// 49 | 50 | int Parameters::Blob::min_threshold = 0; 51 | int Parameters::Blob::max_threshold = 200; 52 | int Parameters::Blob::threshold_step = 100; 53 | int Parameters::Blob::min_area = 550; 54 | int Parameters::Blob::max_area = 300000; 55 | double Parameters::Blob::min_convexity = 0; 56 | double Parameters::Blob::max_convexity = 100; 57 | double Parameters::Blob::min_inertia_ratio = 0; 58 | double Parameters::Blob::max_circularity = 1.0; 59 | double Parameters::Blob::min_circularity = 0.3; 60 | bool Parameters::Blob::filter_by_color = 0; 61 | bool Parameters::Blob::filter_by_circularity = 1; 62 | 63 | 64 | ///////////////////////// Debug-specific parameters ////////////////////////// 65 | 66 | 67 | // Show the thermal image that arrives in the thermal node 68 | bool Parameters::Debug::show_thermal_image = false; 69 | 70 | // In the terminal's window, show the probabilities of candidate holes 71 | bool Parameters::Debug::show_probabilities = false; 72 | 73 | bool Parameters::Debug::show_find_rois = false; 74 | int Parameters::Debug::show_find_rois_size = 1000; 75 | 76 | bool Parameters::Debug::show_denoise_edges = false; 77 | int Parameters::Debug::show_denoise_edges_size = 900; 78 | 79 | bool Parameters::Debug::show_connect_pairs = false; 80 | int Parameters::Debug::show_connect_pairs_size = 1200; 81 | 82 | bool Parameters::Debug::show_get_shapes_clear_border = false; 83 | int Parameters::Debug::show_get_shapes_clear_border_size = 1200; 84 | 85 | ////////////////// Parameters specific to the Thermal node //////////////////// 86 | 87 | // The thermal detection method 88 | // If set to 0 process the binary image acquired from temperatures MultiArray 89 | // If set to 1 process the sensor/Image from thermal sensor 90 | int Parameters::Thermal::detection_method = 0; 91 | 92 | // The probability extraction method 93 | // 0 for Gaussian function 94 | // 1 for Logistic function 95 | int Parameters::Thermal::probability_method = 1; 96 | float Parameters::Thermal::min_thermal_probability = 0.3; 97 | 98 | // Gausian variables 99 | float Parameters::Thermal::optimal_temperature = 35; 100 | float Parameters::Thermal::tolerance = 10; 101 | 102 | // Logistic variables 103 | float Parameters::Thermal::low_acceptable_temperature = 32; 104 | float Parameters::Thermal::high_acceptable_temperature = 38; 105 | 106 | float Parameters::Thermal::left_tolerance = 4; 107 | float Parameters::Thermal::right_tolerance = 8; 108 | 109 | ///////////// Parameters of acceptable temperature for threshold ///////////// 110 | float Parameters::Thermal::low_temperature = 28; 111 | float Parameters::Thermal::high_temperature = 40; 112 | 113 | ////////////////////// Parameters of the thermal image /////////////////////// 114 | int Parameters::ThermalImage::WIDTH = 80; 115 | int Parameters::ThermalImage::HEIGHT = 60; 116 | 117 | ///////////////////// Edge detection specific parameters ///////////////////// 118 | 119 | // canny parameters 120 | int Parameters::Edge::canny_ratio = 3; 121 | int Parameters::Edge::canny_kernel_size = 3; 122 | int Parameters::Edge::canny_low_threshold = 50; 123 | int Parameters::Edge::canny_blur_noise_kernel_size = 3; 124 | 125 | // The opencv edge detection method: 126 | // 0 for the Canny edge detector 127 | // 1 for the Scharr edge detector 128 | // 2 for the Sobel edge detector 129 | // 3 for the Laplacian edge detector 130 | // 4 for mixed Scharr / Sobel edge detection 131 | int Parameters::Edge::edge_detection_method = 2; 132 | 133 | // Threshold parameters 134 | int Parameters::Edge::denoised_edges_threshold = 10; 135 | 136 | // When mixed edge detection is selected, this toggle switch 137 | // is needed in order to shift execution from one edge detector 138 | // to the other. 139 | // 1 for the Scharr edge detector, 140 | // 2 for the Sobel edge detector 141 | int Parameters::Edge::mixed_edges_toggle_switch = 1; 142 | 143 | ////////////////// Image representation specific parameters ////////////////// 144 | 145 | // The depth sensor's horizontal field of view in rads 146 | float Parameters::Image::horizontal_field_of_view = 147 | static_cast(58) / 180 * M_PI; 148 | 149 | // The depth sensor's vertical field of view in rads 150 | float Parameters::Image::vertical_field_of_view = 151 | static_cast(45) / 180 * M_PI; 152 | 153 | // Depth and RGB images' representation method. 154 | // 0 if image used is used as obtained from the image sensor 155 | // 1 through wavelet analysis 156 | int Parameters::Image::image_representation_method = 0; 157 | 158 | // Method to scale the CV_32F image to CV_8UC1 159 | int Parameters::Image::scale_method = 0; 160 | 161 | // Term criteria for segmentation purposes 162 | int Parameters::Image::term_criteria_max_iterations = 5; 163 | double Parameters::Image::term_criteria_max_epsilon = 1; 164 | 165 | 166 | /////////////////// Outline discovery specific parameters //////////////////// 167 | 168 | // The detection method used to obtain the outline of a blob 169 | // 0 for detecting by means of brushfire 170 | // 1 for detecting by means of raycasting 171 | int Parameters::Outline::outline_detection_method = 0; 172 | 173 | // When using raycast instead of brushfire to find the (approximate here) 174 | // outline of blobs, raycast_keypoint_partitions dictates the number of 175 | // rays, or equivalently, the number of partitions in which the blob is 176 | // partitioned in search of the blob's borders 177 | int Parameters::Outline::raycast_keypoint_partitions = 32; 178 | 179 | // Loose ends connection parameters 180 | int Parameters::Outline::AB_to_MO_ratio = 4; 181 | int Parameters::Outline::minimum_curve_points = 50; 182 | 183 | } // namespace flir_lepton_image_processing 184 | } // namespace flir_lepton 185 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/roi_filters.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #include "utils/roi_filters.h" 39 | 40 | /** 41 | @brief The namespaces for this package 42 | **/ 43 | namespace flir_lepton 44 | { 45 | namespace flir_lepton_image_processing 46 | { 47 | /** 48 | @brief Given a set of keypoints and an edges image, this function 49 | returns the valid keypoints and for each one, its respective, least 50 | area rotated bounding box and the points of its outline. 51 | @param[in] keyPoints [const std::vector&] 52 | The original keypoints found. 53 | @param[in] thermalImageEdges [cv::Mat*] The original thermal edges image 54 | @param[in] detectionMethod [const int&] The method by which the 55 | outline of a blob is obtained. 56 | 0 means by means of brushfire, 57 | 1 by means of raycasting 58 | @param[in,out] conveyor [RoisConveyor*] A struct that contains 59 | the final valid candidate rois 60 | @return void 61 | **/ 62 | void RoiFilters::validateBlobs( 63 | const std::vector& keyPoints, 64 | cv::Mat* thermalImageEdges, 65 | const int& detectionMethod, 66 | RoisConveyor* conveyor) 67 | { 68 | #ifdef DEBUG_TIME 69 | Timer::start("validateBlobs", "findRois"); 70 | #endif 71 | 72 | switch (detectionMethod) 73 | { 74 | // Locate the outline of blobs via brushfiring 75 | case 0: 76 | { 77 | std::vector > blobsOutlineVector; 78 | std::vector blobsArea; 79 | 80 | // Find the outline points of each keypoint 81 | OutlineDiscovery::brushfireKeypoints(keyPoints, 82 | thermalImageEdges, 83 | &blobsOutlineVector, 84 | &blobsArea); 85 | 86 | // For each outline found, find the rotated rectangle 87 | // with the least area that encloses it. 88 | cv::Mat inputThermalImageEdges; 89 | thermalImageEdges->copyTo(inputThermalImageEdges); 90 | 91 | std::vector< std::vector > rectangles; 92 | 93 | // Given the outline of the blob, find the least area 94 | // rotated bounding box that encloses it 95 | BoundingBoxDetection::findRotatedBoundingBoxesFromOutline( 96 | inputThermalImageEdges, 97 | blobsOutlineVector, 98 | blobsArea, 99 | &rectangles); 100 | 101 | // Correlate each keypoint with each rectangle found. 102 | // Keep in mind that for a blob to be a potential roi, its area 103 | // must be greater than Parameters::bounding_box_min_area_threshold 104 | validateKeypointsToRectangles( 105 | keyPoints, 106 | rectangles, 107 | blobsArea, 108 | blobsOutlineVector, 109 | conveyor); 110 | 111 | break; 112 | } 113 | // Locate the outline of blobs via raycasting 114 | case 1: 115 | { 116 | std::vector > blobsOutlineVector; 117 | std::vector blobsArea; 118 | 119 | // Find the (approximate) outline points of each keypoint 120 | OutlineDiscovery::raycastKeypoints(keyPoints, 121 | thermalImageEdges, 122 | Parameters::Outline::raycast_keypoint_partitions, 123 | &blobsOutlineVector, 124 | &blobsArea); 125 | 126 | // For each outline found, find the rotated rectangle 127 | // with the least area that encloses it. 128 | cv::Mat inputThermalImageEdges; 129 | thermalImageEdges->copyTo(inputThermalImageEdges); 130 | 131 | std::vector< std::vector > rectangles; 132 | 133 | // Given the outline of the blob, find the least area 134 | // rotated bounding box that encloses it 135 | BoundingBoxDetection::findRotatedBoundingBoxesFromOutline( 136 | inputThermalImageEdges, 137 | blobsOutlineVector, 138 | blobsArea, 139 | &rectangles); 140 | 141 | // Correlate each keypoint with each rectangle found. 142 | // Keep in mind that for a blob to be a potential roi, its area 143 | // must be greater than Parameters::bounding_box_min_area_threshold 144 | validateKeypointsToRectangles( 145 | keyPoints, 146 | rectangles, 147 | blobsArea, 148 | blobsOutlineVector, 149 | conveyor); 150 | 151 | break; 152 | } 153 | } 154 | 155 | // The end product here is a struct (conveyor) of keypoints, 156 | // a set of rectangles that enclose them and the outline of 157 | // each blob found. 158 | 159 | #ifdef DEBUG_TIME 160 | Timer::tick("validateBlobs"); 161 | #endif 162 | } 163 | 164 | 165 | 166 | /** 167 | @brief This functions takes as input arguments a keypoints vector of 168 | size N, a rectangles vector of size M (the rectangle is represented 169 | by its 4 vertices so that the input can be either a Rectangle or a 170 | Rotated Rectangle) and a vector with the area of each rectangle 171 | with purpose to identify the least area rectangle in which a 172 | keypoint resides. It outputs a vector of keypoints (each keypoint must 173 | reside in at least one rectangle) and a vector of rectangles (again 174 | represented by its 4 vertices). There is a one-to-one association 175 | between the keypoints and the rectangles. 176 | @param[in] inKeyPoints [const std::vector&] The key points 177 | @param[in] inRectangles [const std::vector >&] The 178 | rectangles found 179 | @param[in] inRectanglesArea [const std::vector&] 180 | The area of each rectangle 181 | @param[in] inContours [const std::vector >&] 182 | The outline of each blob found 183 | @param[out] conveyor [RoisConveyor*] The container of vector of blobs' 184 | keypoints, outlines and areas 185 | @return void 186 | **/ 187 | void RoiFilters::validateKeypointsToRectangles( 188 | const std::vector& inKeyPoints, 189 | const std::vector >& inRectangles, 190 | const std::vector& inRectanglesArea, 191 | const std::vector >& inContours, 192 | RoisConveyor* conveyor) 193 | { 194 | #ifdef DEBUG_TIME 195 | Timer::start("validateKeypointsToRectangles", "validateBlobs"); 196 | #endif 197 | 198 | for (int keypointId = 0; keypointId < inKeyPoints.size(); keypointId++) 199 | { 200 | std::vector keypointResidesInRectIds; 201 | 202 | // Test to see where (in which rectangle(s)) the keypoint resides. 203 | for (int rectId = 0; rectId < inRectangles.size(); rectId++) 204 | { 205 | if (cv::pointPolygonTest( 206 | inRectangles[rectId], inKeyPoints[keypointId].pt, false) > 0) 207 | { 208 | keypointResidesInRectIds.push_back(rectId); 209 | } 210 | } 211 | 212 | 213 | // If the keypoint resides in exactly one rectangle. 214 | if (keypointResidesInRectIds.size() == 1) 215 | { 216 | // A single thermal roi 217 | RoiConveyor roi; 218 | 219 | // Accumulate keypoint, rectangle and outline properties onto roi 220 | roi.keypoint = inKeyPoints[keypointId]; 221 | roi.rectangle = inRectangles[keypointResidesInRectIds[0]]; 222 | roi.outline = inContours[keypointId]; 223 | 224 | // Push roi back into the conveyor 225 | conveyor->rois.push_back(roi); 226 | } 227 | 228 | // If the keypoint resides in multiple rectangles, 229 | // choose the one with the least area. 230 | if (keypointResidesInRectIds.size() > 1) 231 | { 232 | // The minimum area of all rectangles 233 | float minRectArea = std::numeric_limits::max(); 234 | 235 | // The index of the rectangle with the least area 236 | int minAreaRectId; 237 | 238 | for (unsigned int i = 0; i < keypointResidesInRectIds.size(); i++) 239 | { 240 | if (inRectanglesArea[keypointResidesInRectIds[i]] < minRectArea) 241 | { 242 | minRectArea = inRectanglesArea[keypointResidesInRectIds[i]]; 243 | minAreaRectId = keypointResidesInRectIds[i]; 244 | } 245 | } 246 | 247 | // A single roi 248 | RoiConveyor roi; 249 | 250 | // Accumulate keypoint, rectangle and outline properties onto roi 251 | roi.keypoint = inKeyPoints[keypointId]; 252 | roi.rectangle = inRectangles[minAreaRectId]; 253 | roi.outline = inContours[keypointId]; 254 | 255 | // Push roi back into the conveyor 256 | conveyor->rois.push_back(roi); 257 | } 258 | 259 | // If the keypoint has no rectangle attached to it, 260 | // do not insert the roi it corresponds to in struct roi 261 | } 262 | 263 | #ifdef DEBUG_TIME 264 | Timer::tick("validateKeypointsToRectangles"); 265 | #endif 266 | } 267 | 268 | } // namespace flir_lepton_image_processing 269 | } // namespace flir_lepton 270 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/timer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #include "utils/timer.h" 39 | 40 | std::map Timer::times = std::map(); 41 | std::map Timer::max_time = std::map(); 42 | std::map Timer::min_time = std::map(); 43 | std::map Timer::mean_time = std::map(); 44 | std::map Timer::sum_time = std::map(); 45 | 46 | std::map Timer::count = 47 | std::map(); 48 | 49 | std::map > Timer::timer_tree = 50 | std::map >(); 51 | 52 | std::string Timer::top_node = ""; 53 | 54 | struct timeval Timer::msTime = timeval(); 55 | 56 | void Timer::printMsInternal(double t) 57 | { 58 | ROS_INFO_STREAM_NAMED(PKG_NAME, t << " ms"); 59 | } 60 | 61 | void Timer::printSecInternal(double t) 62 | { 63 | ROS_INFO_STREAM_NAMED(PKG_NAME, t / 1000.0 << " sec"); 64 | } 65 | 66 | void Timer::printMinInternal(double t) 67 | { 68 | ROS_INFO_STREAM_NAMED(PKG_NAME, t / 1000.0 / 60.0 << " minutes"); 69 | } 70 | 71 | void Timer::printHoursInternal(double t) 72 | { 73 | ROS_INFO_STREAM_NAMED(PKG_NAME, t / 1000.0 / 60.0 / 60.0 << " hours"); 74 | } 75 | 76 | void Timer::printLiteralInternal(double t) 77 | { 78 | int sec = t / 1000.0; 79 | if (sec >= 1) 80 | { 81 | t -= sec * 1000; 82 | int min = sec / 60.0; 83 | if (min >= 1) 84 | { 85 | sec -= min * 60; 86 | int hours = min / 60.0; 87 | if (hours >= 1) 88 | { 89 | min -= hours * 60; 90 | ROS_INFO_STREAM_NAMED(PKG_NAME, 91 | min << " hours " < > 117 | (timerId, std::set())); 118 | gettimeofday(&msTime, NULL); 119 | double ms = static_cast(msTime.tv_sec) * 1000 + 120 | static_cast(msTime.tv_usec) / 1000; 121 | times.insert(Psd(timerId, ms)); 122 | count.insert(Psul(timerId, 0)); 123 | mean_time.insert(Psul(timerId, 0)); 124 | sum_time.insert(Psul(timerId, 0)); 125 | max_time.insert(Psd(timerId, -1.0)); 126 | min_time.insert(Psd(timerId, 1000000000000000.0)); 127 | } 128 | else 129 | { 130 | gettimeofday(&msTime, NULL); 131 | double ms = static_cast(msTime.tv_sec) * 1000 + 132 | static_cast(msTime.tv_usec) / 1000; 133 | it->second = ms; 134 | mean_time.insert(Psul(timerId, 0)); 135 | sum_time.insert(Psul(timerId, 0)); 136 | max_time.insert(Psd(timerId, -1.0)); 137 | min_time.insert(Psd(timerId, 1000000000000000.0)); 138 | } 139 | if (father != "") 140 | { 141 | timer_tree[father].insert(timerId); 142 | } 143 | if (top) 144 | { 145 | top_node = timerId; 146 | } 147 | } 148 | 149 | double Timer::stop(std::string timerId) 150 | { 151 | MsdIt it = times.find(timerId); 152 | if (it == times.end()) 153 | { 154 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Invalid timer id : " << timerId << "\n"); 155 | return -1; 156 | } 157 | else 158 | { 159 | gettimeofday(&msTime , NULL); 160 | double ms = static_cast(msTime.tv_sec) * 1000 + 161 | static_cast(msTime.tv_usec) / 1000; 162 | return ms - it->second; 163 | } 164 | } 165 | 166 | double Timer::mean(std::string timerId) 167 | { 168 | MsdIt it = times.find(timerId); 169 | if (it == times.end()) 170 | { 171 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Invalid timer id : " << timerId << "\n"); 172 | return -1; 173 | } 174 | else 175 | { 176 | return mean_time[timerId]; 177 | } 178 | } 179 | 180 | void Timer::tick(std::string timerId) 181 | { 182 | MsdIt it = times.find(timerId); 183 | if (it == times.end()) 184 | { 185 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Invalid timer id : " << timerId << "\n"); 186 | } 187 | else 188 | { 189 | count[timerId]++; 190 | gettimeofday(&msTime , NULL); 191 | double ms = static_cast(msTime.tv_sec) * 1000 + 192 | static_cast(msTime.tv_usec) / 1000; 193 | ms -= it->second; 194 | times[timerId] = ms; 195 | sum_time[timerId] += ms; 196 | mean_time[timerId] = sum_time[timerId] / count[timerId]; 197 | if (min_time[timerId] > ms) 198 | min_time[timerId] = ms; 199 | if (max_time[timerId] < ms) 200 | max_time[timerId] = ms; 201 | } 202 | } 203 | 204 | void Timer::printMs(std::string timerId) 205 | { 206 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timer " << timerId << " : "); 207 | printMsInternal(stop(timerId)); 208 | } 209 | 210 | void Timer::printSec(std::string timerId) 211 | { 212 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timer " << timerId << " : "); 213 | printSecInternal(stop(timerId)); 214 | } 215 | 216 | void Timer::printMin(std::string timerId) 217 | { 218 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timer " << timerId << " : "); 219 | printMinInternal(stop(timerId)); 220 | } 221 | 222 | void Timer::printHours(std::string timerId) 223 | { 224 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timer " << timerId << " : "); 225 | printHoursInternal(stop(timerId)); 226 | } 227 | 228 | void Timer::printLiteral(std::string timerId) 229 | { 230 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timer " << timerId << " : "); 231 | printLiteralInternal(stop(timerId)); 232 | } 233 | 234 | void Timer::printLiteralMean(std::string timerId, std::string identation) 235 | { 236 | MsdIt it = times.find(timerId); 237 | if (it == times.end()) 238 | { 239 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Invalid timer id : " << timerId << "\n"); 240 | } 241 | else 242 | { 243 | ROS_INFO_STREAM_NAMED(PKG_NAME, identation << timerId << 244 | "' [" << times[timerId] << " - " << 245 | min_time[timerId] << " , " << mean_time[timerId] << " , " << 246 | max_time[timerId] << " - " << count[timerId] << "]"); 247 | } 248 | } 249 | 250 | void Timer::printAll(void) 251 | { 252 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timers available :"); 253 | for (MsdCIt it = times.begin() ; it != times.end() ; it++) 254 | { 255 | ROS_INFO_STREAM_NAMED(PKG_NAME, "\t"); 256 | printLiteral(it->first); 257 | } 258 | } 259 | void Timer::printAllMeans(void) 260 | { 261 | ROS_INFO_STREAM_NAMED(PKG_NAME, "Timers available :"); 262 | Mfs tms; 263 | for (MsdCIt it = mean_time.begin() ; it != mean_time.end() ; it++) 264 | { 265 | tms.insert(std::pair(it->second, it->first)); 266 | } 267 | for (MfsIt it = tms.begin() ; it != tms.end() ; it++) 268 | { 269 | ROS_INFO_STREAM_NAMED(PKG_NAME, "\t"; printLiteralMean(it->second)); 270 | } 271 | } 272 | 273 | void Timer::printAllMeansTree(void) 274 | { 275 | ROS_INFO_STREAM_NAMED(PKG_NAME, 276 | "Timers available : [curr - min , mean , max - ticks]\n"); 277 | Mfs tms; 278 | for (MsdCIt it = mean_time.begin() ; it != mean_time.end() ; it++) 279 | { 280 | tms.insert(std::pair(it->second, it->first)); 281 | } 282 | printIterativeTree(top_node, ""); 283 | } 284 | 285 | void Timer::printIterativeTree(std::string node, std::string identation) 286 | { 287 | printLiteralMean(node, identation); 288 | for (std::set::iterator it = timer_tree[node].begin() ; it != 289 | timer_tree[node].end() ; it++) 290 | { 291 | printIterativeTree(*it, identation + "|-"); 292 | } 293 | } 294 | -------------------------------------------------------------------------------- /flir_lepton_image_processing/src/utils/visualization.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Authors: Alexandros Philotheou, Manos Tsardoulias 36 | *********************************************************************/ 37 | 38 | #include "utils/defines.h" 39 | #include "utils/visualization.h" 40 | 41 | /** 42 | @brief The namespaces for this package 43 | **/ 44 | namespace flir_lepton 45 | { 46 | namespace flir_lepton_image_processing 47 | { 48 | /** 49 | @brief Shows multiple images in one window 50 | @param[in] title [const std::string&] The window's title 51 | @param[in] imgs [const std::vector&] The images to be shown 52 | @param[in] titles [const std::vector&] The titles for each 53 | image 54 | @param[in] maxSize [const int&] The maximum size of the window 55 | @param[in] ms [const int&] How many seconds the showing lasts 56 | @return [cv::Mat] An image featuring multiple images 57 | **/ 58 | cv::Mat Visualization::multipleShow( 59 | const std::string& title, 60 | const std::vector& imgs, 61 | const std::vector& titles, 62 | const int& maxSize, 63 | const int& ms) 64 | { 65 | unsigned int rows = 0, cols = 0; 66 | unsigned int sqdim = sqrt(imgs.size()); 67 | rows = sqdim; 68 | cols = imgs.size() / rows + (((imgs.size() % rows) != 0) ? 1 : 0); 69 | 70 | unsigned int winCols = (static_cast(maxSize)) / cols; 71 | float scale = (static_cast(winCols)) / imgs[0].cols; 72 | 73 | unsigned int finalRows, finalCols; 74 | finalRows = rows * (static_cast(imgs[0].rows) * scale); 75 | finalCols = winCols * cols; 76 | 77 | cv::Mat big(rows * imgs[0].rows, cols * imgs[0].cols, CV_8UC3); 78 | 79 | // Draw images 80 | for (unsigned int im = 0 ; im < imgs.size() ; im++) 81 | { 82 | unsigned int startRow, startCol; 83 | startRow = im / cols * imgs[im].rows; 84 | startCol = im % cols * imgs[im].cols; 85 | for (unsigned int i = 0 ; i < imgs[im].rows ; i++) 86 | { 87 | for (unsigned int j = 0 ; j < imgs[im].cols ; j++) 88 | { 89 | if (imgs[im].channels() == 1) 90 | { 91 | big.at(startRow + i, startCol + j)[0] = 92 | imgs[im].at(i, j); 93 | big.at(startRow + i, startCol + j)[1] = 94 | imgs[im].at(i, j); 95 | big.at(startRow + i, startCol + j)[2] = 96 | imgs[im].at(i, j); 97 | } 98 | else if (imgs[im].channels() == 3) 99 | { 100 | big.at(startRow + i, startCol + j) = 101 | imgs[im].at(i, j); 102 | } 103 | } 104 | } 105 | 106 | cv::putText(big, titles[im].c_str(), 107 | cvPoint(startCol + 10, startRow + 20), 108 | cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0, 255, 0), 1, CV_AA); 109 | } 110 | 111 | for (unsigned int i = 1 ; i <= rows - 1 ; i++) 112 | { 113 | cv::line(big, 114 | cv::Point2f(0 , i * imgs[0].rows - 1), 115 | cv::Point2f(cols * imgs[0].cols - 1, i * imgs[0].rows - 1), 116 | CV_RGB(255, 255, 255), 2, 8); 117 | } 118 | 119 | for (unsigned int i = 1 ; i <= cols - 1 ; i++) 120 | { 121 | cv::line(big, 122 | cv::Point2f(i * imgs[0].cols - 1 , 0), 123 | cv::Point2f(i * imgs[0].cols - 1, rows * imgs[0].rows), 124 | CV_RGB(255, 255, 255), 2, 8); 125 | } 126 | 127 | 128 | // Final resize 129 | cv::Mat resized(finalRows, finalCols, CV_8UC3); 130 | cv::resize(big, resized, cv::Size(finalCols, finalRows)); 131 | 132 | if (ms > 0) 133 | { 134 | show(title, resized, ms); 135 | } 136 | 137 | return resized; 138 | } 139 | 140 | 141 | 142 | /** 143 | @brief Scales an image from its original format to CV_8UC1 144 | @param[in] inImage [const cv::Mat&] The image to show 145 | @param[in] ms [const int&] How many ms the showing lasts 146 | @return void 147 | **/ 148 | cv::Mat Visualization::scaleImageForVisualization( 149 | const cv::Mat& inImage, 150 | const int& method) 151 | { 152 | cv::Mat outImage; 153 | 154 | double min; 155 | double max; 156 | cv::minMaxIdx(inImage, &min, &max); 157 | 158 | if (method == 0) 159 | { 160 | if (max != min) 161 | { 162 | outImage = (inImage - min) * 255 / (max - min); 163 | outImage = cv::abs(outImage); 164 | outImage.convertTo(outImage, CV_8UC1); 165 | } 166 | else 167 | { 168 | cv::convertScaleAbs(inImage, outImage, 255 / max); 169 | } 170 | } 171 | else if (method == 1) 172 | { 173 | cv::convertScaleAbs(inImage, outImage, 255 / max); 174 | } 175 | else if (method == 2) 176 | { 177 | cv::equalizeHist(scaleImageForVisualization(inImage, 0), outImage); 178 | } 179 | 180 | return outImage; 181 | } 182 | 183 | 184 | 185 | /** 186 | @brief Overrides the cv::imshow function. 187 | @param[in] windowTitle [const std::string&] The window title 188 | @param[in] inImage [const cv::Mat&] The image to show 189 | @param[in] ms [const int&] How many ms the showing lasts 190 | @return void 191 | **/ 192 | void Visualization::show( 193 | const std::string& windowTitle, 194 | const cv::Mat& inImage, 195 | const int& ms) 196 | { 197 | cv::imshow(windowTitle, inImage); 198 | cv::waitKey(ms); 199 | } 200 | 201 | 202 | 203 | /** 204 | @brief Depicts the contents of a RoisConveyor on an image 205 | @param[in] windowTitle [const std::string&] The window title 206 | @param[in] inImage [const cv::Mat&] The image to show 207 | @param[in] conveyor [const RoisConveyor&] The rois conveyor 208 | @param[in] ms [const int&] How many ms the showing lasts 209 | @param[in] msgs [const std::vector&] Message to show to 210 | each keypoint 211 | @param[in] hz [const float&] If positive holds the Hz 212 | @return void 213 | **/ 214 | cv::Mat Visualization::showRois( 215 | const std::string& windowTitle, 216 | const cv::Mat& inImage, 217 | const RoisConveyor& conveyor, 218 | const int& ms, 219 | const std::vector& msgs, 220 | const float& hz) 221 | { 222 | cv::Mat img; 223 | if (inImage.type() != CV_8UC1 || inImage.type() != CV_8UC3) 224 | { 225 | img = scaleImageForVisualization(inImage, 226 | Parameters::Image::scale_method); 227 | } 228 | else 229 | { 230 | inImage.copyTo(img); 231 | } 232 | 233 | // Construct a keypoints vector to feed into the cv::drawKeypoints method 234 | std::vector keypointsVector; 235 | for (int i = 0; i < conveyor.size(); i++) 236 | { 237 | keypointsVector.push_back(conveyor.rois[i].keypoint); 238 | } 239 | 240 | cv::drawKeypoints(img, keypointsVector, img, CV_RGB(0, 255, 0), 241 | cv::DrawMatchesFlags::DEFAULT); 242 | 243 | for (unsigned int i = 0; i < conveyor.size(); i++) 244 | { 245 | for (unsigned int j = 0; j < conveyor.rois[i].outline.size(); j++) 246 | { 247 | img.at(conveyor.rois[i].outline[j].y, 248 | 3 * conveyor.rois[i].outline[j].x + 2) = 0; 249 | 250 | img.at(conveyor.rois[i].outline[j].y, 251 | 3 * conveyor.rois[i].outline[j].x + 1) = 255; 252 | 253 | img.at(conveyor.rois[i].outline[j].y, 254 | 3 * conveyor.rois[i].outline[j].x + 0) = 0; 255 | } 256 | } 257 | 258 | for (int i = 0; i < conveyor.size(); i++) 259 | { 260 | for (int j = 0; j < 4; j++) 261 | { 262 | cv::line(img, conveyor.rois[i].rectangle[j], 263 | conveyor.rois[i].rectangle[(j + 1) % 4], CV_RGB(255, 0, 0), 1, 8); 264 | } 265 | 266 | if (msgs.size() == conveyor.size()) 267 | { 268 | cv::putText(img, msgs[i].c_str(), 269 | cvPoint(conveyor.rois[i].keypoint.pt.x - 20, 270 | conveyor.rois[i].keypoint.pt.y - 20), 271 | cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(255, 50, 50), 1, CV_AA); 272 | } 273 | } 274 | 275 | if (hz > 0) 276 | { 277 | cv::putText(img, 278 | (TOSTR(hz)+std::string("Hz")).c_str(), 279 | cvPoint(20, 20), 280 | cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0, 0, 255), 1, CV_AA); 281 | } 282 | 283 | if (ms >= 0) 284 | { 285 | Visualization::show(windowTitle.c_str(), img, ms); 286 | } 287 | 288 | return img; 289 | } 290 | 291 | 292 | 293 | /** 294 | @brief Depicts the keypoints and bounding boxes 295 | @param[in] windowTitle [const std::string&] The window title 296 | @param[in] inImage [const cv::Mat&] The image to show 297 | @param[in] ms [const int&] How many ms the showing lasts 298 | @param[in] keypoints [const std::vector&] The keypoints 299 | @return void 300 | **/ 301 | cv::Mat Visualization::showKeypoints( 302 | const std::string& windowTitle, 303 | const cv::Mat& inImage, 304 | const int& ms, 305 | const std::vector& keypoints) 306 | { 307 | cv::Mat img; 308 | if (inImage.depth() != CV_8U) 309 | { 310 | img = scaleImageForVisualization(inImage, Parameters::Image::scale_method); 311 | } 312 | else 313 | { 314 | inImage.copyTo(img); 315 | } 316 | 317 | cv::drawKeypoints(img, keypoints, img, CV_RGB(255, 0, 0), 318 | cv::DrawMatchesFlags::DEFAULT); 319 | 320 | if (ms >= 0) 321 | { 322 | Visualization::show(windowTitle.c_str(), img, ms); 323 | } 324 | 325 | return img; 326 | } 327 | 328 | 329 | 330 | /** 331 | @brief Overrides the cv::imshow function. Provides image scaling from 332 | the image's original forma to CV_8UC1 format 333 | @param[in] windowTitle [const std::string&] The window title 334 | @param[in] inImage [const cv::Mat&] The image to show 335 | @param[in] ms [const int&] How many ms the showing lasts 336 | @return void 337 | **/ 338 | void Visualization::showScaled( 339 | const std::string& windowTitle, 340 | const cv::Mat& inImage, 341 | const int& ms) 342 | { 343 | cv::imshow(windowTitle, 344 | scaleImageForVisualization(inImage, Parameters::Image::scale_method)); 345 | cv::waitKey(ms); 346 | } 347 | 348 | } // namespace flir_lepton_image_processing 349 | } // namespace flir_lepton 350 | -------------------------------------------------------------------------------- /flir_lepton_launchers/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton_launchers 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Changed metapackage name to flir_lepton -- platform independent 8 | * Global ros launcher ready 9 | * Contributors: Angelos Triantafyllidis, Konstantinos Panayiotou 10 | -------------------------------------------------------------------------------- /flir_lepton_launchers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton_launchers) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | -------------------------------------------------------------------------------- /flir_lepton_launchers/launch/flir_lepton.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /flir_lepton_launchers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_lepton_launchers 4 | 0.0.1 5 | Contains launchers for bring-up. 6 | 7 | Angelos Triantafyllidis 8 | 9 | BSD 10 | 11 | Angelos Triantafyllidis 12 | Konstantinos Panayiotou 13 | 14 | catkin 15 | 16 | flir_lepton_sensor 17 | flir_lepton_image_processing 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /flir_lepton_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton_ros_comm 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Changed metapackage name to flir_lepton -- platform independent 8 | * Refactored flir_lepton_sensor package + packaging 9 | * Fixed compilation error -- explicit 10 | * Move all messages into flir_lepton_ros_comm package 11 | * minor typo changes 12 | * Integrated hardware_interface and image_processing 13 | * Contributors: Angelos Triantafyllidis, Konstantinos Panayiotou 14 | -------------------------------------------------------------------------------- /flir_lepton_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | #message_runtime 7 | std_msgs 8 | sensor_msgs 9 | ) 10 | 11 | 12 | add_message_files(DIRECTORY msg/flir_lepton_sensor 13 | FILES 14 | TemperaturesMsg.msg 15 | FlirLeptonBatchMsg.msg 16 | FlirLeptonRawMsg.msg 17 | ) 18 | 19 | 20 | add_message_files(DIRECTORY msg/flir_lepton_image_processing 21 | FILES 22 | CandidateRoiMsg.msg 23 | CandidateRoisVectorMsg.msg 24 | GeneralAlertInfo.msg 25 | ThermalAlert.msg 26 | ThermalAlertVector.msg 27 | ) 28 | 29 | 30 | ## Generate added messages with any dependencies listed here 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | sensor_msgs 35 | ) 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | ## The catkin_package macro generates cmake config files for your package 41 | ## Declare things to be passed to dependent projects 42 | catkin_package( 43 | CATKIN_DEPENDS 44 | #message_generation 45 | message_runtime 46 | std_msgs 47 | sensor_msgs 48 | ) 49 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_image_processing/CandidateRoiMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # keypoint.pt.x 4 | float32 keypointX 5 | 6 | # keypoint.pt.y 7 | float32 keypointY 8 | 9 | # bounding box's vertices X coordinate 10 | float32[] verticesX 11 | 12 | # bounding box's vertices Y coordinate 13 | float32[] verticesY 14 | 15 | # blob's outline points X coordinate 16 | float32[] outlineX 17 | 18 | # blob's outline points X coordinate 19 | float32[] outlineY 20 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_image_processing/CandidateRoisVectorMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | CandidateRoiMsg[] candidateRois 4 | 5 | sensor_msgs/Image image 6 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_image_processing/GeneralAlertInfo.msg: -------------------------------------------------------------------------------- 1 | float64 yaw 2 | float64 pitch 3 | float32 probability 4 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_image_processing/ThermalAlert.msg: -------------------------------------------------------------------------------- 1 | GeneralAlertInfo info 2 | 3 | float32 temperature 4 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_image_processing/ThermalAlertVector.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ThermalAlert[] alerts 3 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_sensor/FlirLeptonBatchMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # The thermal image acquired from flir_lepton sensor 4 | sensor_msgs/Image thermalImage 5 | 6 | # The temperatures in each pixel after calibration 7 | TemperaturesMsg temperatures 8 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_sensor/FlirLeptonRawMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # The raw pixel values from the sensor 4 | std_msgs/UInt16MultiArray rawValues 5 | -------------------------------------------------------------------------------- /flir_lepton_msgs/msg/flir_lepton_sensor/TemperaturesMsg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # The temperatures in each pixel after calibration 4 | std_msgs/Float32MultiArray values 5 | -------------------------------------------------------------------------------- /flir_lepton_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flir_lepton_msgs 4 | 0.0.1 5 | Flir Lepton ros communications 6 | 7 | Konstantinos Panayiotou 8 | 9 | Konstantinos Panayiotou 10 | Angelos Triantafyllidis 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | 17 | message_generation 18 | message_runtime 19 | 20 | std_msgs 21 | sensor_msgs 22 | 23 | 24 | -------------------------------------------------------------------------------- /flir_lepton_sensor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package flir_lepton_sensor 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | Forthcoming 6 | ----------- 7 | * Initialized CHANGELOG.rst files 8 | * Changed metapackage name to flir_lepton -- platform independent 9 | * Refactored flir_lepton_sensor package + packaging 10 | * Contributors: Konstantinos Panayiotou 11 | -------------------------------------------------------------------------------- /flir_lepton_sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(flir_lepton_sensor) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | roslib 7 | sensor_msgs 8 | flir_lepton_msgs 9 | ) 10 | 11 | find_package(Boost REQUIRED COMPONENTS thread) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS 15 | include 16 | CATKIN_DEPENDS 17 | message_runtime 18 | roscpp 19 | roslib 20 | sensor_msgs 21 | flir_lepton_msgs 22 | DEPENDS system_lib 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | 31 | ################################################################################ 32 | # Flir Lepton Hardware Interface Lib # 33 | ################################################################################ 34 | 35 | add_library(${PROJECT_NAME}_utils_lib 36 | src/flir_lepton_utils.cpp 37 | ) 38 | 39 | add_library(${PROJECT_NAME}_lib 40 | src/flir_lepton_hw_iface.cpp 41 | ) 42 | 43 | target_link_libraries(${PROJECT_NAME}_lib 44 | ${catkin_LIBRARIES} 45 | ${PROJECT_NAME}_utils_lib 46 | ) 47 | 48 | add_dependencies(${PROJECT_NAME}_lib ${catkin_EXPORTED_TARGETS}) 49 | 50 | 51 | ################################################################################ 52 | # Flir Lepton Sensor Executable # 53 | ################################################################################ 54 | 55 | add_executable(${PROJECT_NAME}_node 56 | src/flir_lepton_sensor.cpp 57 | ) 58 | 59 | add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) 60 | 61 | target_link_libraries(${PROJECT_NAME}_node 62 | ${catkin_LIBRARIES} 63 | ${PROJECT_NAME}_lib 64 | ) 65 | -------------------------------------------------------------------------------- /flir_lepton_sensor/README.md: -------------------------------------------------------------------------------- 1 | ## Synopsis 2 | 3 | This package containes source code used for communication with the Flir Lepton LWIR sensor. 4 | The communication is achieved through the platform's spi port. 5 | 6 | It has been developed to be PLATFORM-Independent. 7 | 8 | ## Directories 9 | 10 | 11 | ## Tests 12 | 13 | Sensor data acquisition is handled by a thread. Using this technique, the required time for retrieving a full scene frame has been measured at 14 | approximately, [31, 50] milliseconds. 15 | 16 | 17 | ## Contributors 18 | 19 | - Konstaninos Panayiotou, **[klpanagi@gmail.com]** 20 | - Aggelos Triantafyllidis, **[angetria@gmail.com]** 21 | -------------------------------------------------------------------------------- /flir_lepton_sensor/config/params.yaml: -------------------------------------------------------------------------------- 1 | published_topics: 2 | flir_image_topic: /flir_lepton/image/gray 3 | flir_rgb_image_topic: /flir_lepton/image/rgb 4 | flir_temper_topic: /flir_lepton/temperatures 5 | flir_batch_topic: /flir_lepton/batch 6 | flir_urdf: 7 | camera_optical_frame: /flir_lepton_optical_frame 8 | iface: 9 | device_port: "/dev/spidev0.0" 10 | bits: 8 11 | speed: 20000000 12 | delay: 0 13 | packet_size: 164 14 | packets_per_frame: 60 15 | frame_rate: 25 16 | dataset_file_name: "dataset_spline" 17 | -------------------------------------------------------------------------------- /flir_lepton_sensor/include/colormaps.h: -------------------------------------------------------------------------------- 1 | #ifndef COLORMAPS_H 2 | #define COLORMAPS_H 3 | 4 | namespace colormap 5 | { 6 | 7 | const int rainbow[] = { 8 | 1, 3, 74, 0, 3, 74, 0, 3, 75, 0, 3, 75, 0, 3, 9 | 76, 0, 3, 76, 0, 3, 77, 0, 3, 79, 0, 3, 82, 0, 5, 85, 0, 7, 88, 0, 10, 91, 0, 10 | 14, 94, 0, 19, 98, 0, 22, 100, 0, 25, 103, 0, 28, 106, 0, 32, 109, 0, 35, 112, 11 | 0, 38, 116, 0, 40, 119, 0, 42, 123, 0, 45, 128, 0, 49, 133, 0, 50, 134, 0, 51, 12 | 136, 0, 52, 137, 0, 53, 139, 0, 54, 142, 0, 55, 144, 0, 56, 145, 0, 58, 149, 13 | 0, 61, 154, 0, 63, 156, 0, 65, 159, 0, 66, 161, 0, 68, 164, 0, 69, 167, 0, 71, 14 | 170, 0, 73, 174, 0, 75, 179, 0, 76, 181, 0, 78, 184, 0, 79, 187, 0, 80, 188, 15 | 0, 81, 190, 0, 84, 194, 0, 87, 198, 0, 88, 200, 0, 90, 203, 0, 92, 205, 0, 94, 16 | 207, 0, 94, 208, 0, 95, 209, 0, 96, 210, 0, 97, 211, 0, 99, 214, 0, 102, 217, 17 | 0, 103, 218, 0, 104, 219, 0, 105, 220, 0, 107, 221, 0, 109, 223, 0, 111, 223, 18 | 0, 113, 223, 0, 115, 222, 0, 117, 221, 0, 118, 220, 1, 120, 219, 1, 122, 217, 19 | 2, 124, 216, 2, 126, 214, 3, 129, 212, 3, 131, 207, 4, 132, 205, 4, 133, 202, 20 | 4, 134, 197, 5, 136, 192, 6, 138, 185, 7, 141, 178, 8, 142, 172, 10, 144, 166, 21 | 10, 144, 162, 11, 145, 158, 12, 146, 153, 13, 147, 149, 15, 149, 140, 17, 151, 22 | 132, 22, 153, 120, 25, 154, 115, 28, 156, 109, 34, 158, 101, 40, 160, 94, 45, 23 | 162, 86, 51, 164, 79, 59, 167, 69, 67, 171, 60, 72, 173, 54, 78, 175, 48, 83, 24 | 177, 43, 89, 179, 39, 93, 181, 35, 98, 183, 31, 105, 185, 26, 109, 187, 23, 25 | 113, 188, 21, 118, 189, 19, 123, 191, 17, 128, 193, 14, 134, 195, 12, 138, 26 | 196, 10, 142, 197, 8, 146, 198, 6, 151, 200, 5, 155, 201, 4, 160, 203, 3, 164, 27 | 204, 2, 169, 205, 2, 173, 206, 1, 175, 207, 1, 178, 207, 1, 184, 208, 0, 190, 28 | 210, 0, 193, 211, 0, 196, 212, 0, 199, 212, 0, 202, 213, 1, 207, 214, 2, 212, 29 | 215, 3, 215, 214, 3, 218, 214, 3, 220, 213, 3, 222, 213, 4, 224, 212, 4, 225, 30 | 212, 5, 226, 212, 5, 229, 211, 5, 232, 211, 6, 232, 211, 6, 233, 211, 6, 234, 31 | 210, 6, 235, 210, 7, 236, 209, 7, 237, 208, 8, 239, 206, 8, 241, 204, 9, 242, 32 | 203, 9, 244, 202, 10, 244, 201, 10, 245, 200, 10, 245, 199, 11, 246, 198, 11, 33 | 247, 197, 12, 248, 194, 13, 249, 191, 14, 250, 189, 14, 251, 187, 15, 251, 34 | 185, 16, 252, 183, 17, 252, 178, 18, 253, 174, 19, 253, 171, 19, 254, 168, 35 | 20, 254, 165, 21, 254, 164, 21, 255, 163, 22, 255, 161, 22, 255, 159, 23, 255, 36 | 157, 23, 255, 155, 24, 255, 149, 25, 255, 143, 27, 255, 139, 28, 255, 135, 30, 37 | 255, 131, 31, 255, 127, 32, 255, 118, 34, 255, 110, 36, 255, 104, 37, 255, 38 | 101, 38, 255, 99, 39, 255, 93, 40, 255, 88, 42, 254, 82, 43, 254, 77, 45, 254, 39 | 69, 47, 254, 62, 49, 253, 57, 50, 253, 53, 52, 252, 49, 53, 252, 45, 55, 251, 40 | 39, 57, 251, 33, 59, 251, 32, 60, 251, 31, 60, 251, 30, 61, 251, 29, 61, 251, 41 | 28, 62, 250, 27, 63, 250, 27, 65, 249, 26, 66, 249, 26, 68, 248, 25, 70, 248, 42 | 24, 73, 247, 24, 75, 247, 25, 77, 247, 25, 79, 247, 26, 81, 247, 32, 83, 247, 43 | 35, 85, 247, 38, 86, 247, 42, 88, 247, 46, 90, 247, 50, 92, 248, 55, 94, 248, 44 | 59, 96, 248, 64, 98, 248, 72, 101, 249, 81, 104, 249, 87, 106, 250, 93, 108, 45 | 250, 95, 109, 250, 98, 110, 250, 100, 111, 251, 101, 112, 251, 102, 113, 251, 46 | 109, 117, 252, 116, 121, 252, 121, 123, 253, 126, 126, 253, 130, 128, 254, 47 | 135, 131, 254, 139, 133, 254, 144, 136, 254, 151, 140, 255, 158, 144, 255, 48 | 163, 146, 255, 168, 149, 255, 173, 152, 255, 176, 153, 255, 178, 155, 255, 49 | 184, 160, 255, 191, 165, 255, 195, 168, 255, 199, 172, 255, 203, 175, 255, 50 | 207, 179, 255, 211, 182, 255, 216, 185, 255, 218, 190, 255, 220, 196, 255, 51 | 222, 200, 255, 225, 202, 255, 227, 204, 255, 230, 206, 255, 233, 208 52 | }; 53 | } 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /flir_lepton_sensor/include/flir_lepton_hw_iface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * * 3 | * * Software License Agreement (BSD License) 4 | * * 5 | * * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * * All rights reserved. 7 | * * 8 | * * Redistribution and use in source and binary forms, with or without 9 | * * modification, are permitted provided that the following conditions 10 | * * are met: 11 | * * 12 | * * * Redistributions of source code must retain the above copyright 13 | * * notice, this list of conditions and the following disclaimer. 14 | * * * Redistributions in binary form must reproduce the above 15 | * * copyright notice, this list of conditions and the following 16 | * * disclaimer in the documentation and/or other materials provided 17 | * * with the distribution. 18 | * * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * * contributors may be used to endorse or promote products derived 20 | * * from this software without specific prior written permission. 21 | * * 22 | * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * * POSSIBILITY OF SUCH DAMAGE. 34 | * * 35 | * * Author: Konstantinos Panayiotou, Agnelos Triantafyllidis, 36 | * * Tsirigotis Christos 37 | * * Maintainer: Konstantinos Panayiotou 38 | * * Email: klpanagi@gmail.com 39 | * * 40 | * *********************************************************************/ 41 | 42 | #ifndef FLIR_LEPTON_FLIR_LEPTON_HW_IFACE_H 43 | #define FLIR_LEPTON_FLIR_LEPTON_HW_IFACE_H 44 | 45 | /* ---< Containers >--- */ 46 | #include 47 | #include 48 | #include 49 | /* -------------------- */ 50 | 51 | #include 52 | #include 53 | 54 | 55 | /* ---< ROS related >--- */ 56 | #include "ros/ros.h" 57 | #include "sensor_msgs/Image.h" 58 | #include "flir_lepton_msgs/TemperaturesMsg.h" 59 | #include "flir_lepton_msgs/FlirLeptonBatchMsg.h" 60 | #include "flir_lepton_msgs/FlirLeptonRawMsg.h" 61 | #include 62 | #include 63 | /* --------------------- */ 64 | 65 | 66 | namespace flir_lepton 67 | { 68 | namespace flir_lepton_sensor 69 | { 70 | #define IMAGE_HEIGHT 60 71 | #define IMAGE_WIDTH 80 72 | #define MAX_RESTART_ATTEMPS_EXIT 5 73 | #define MAX_RESETS_ERROR 750 74 | 75 | 76 | class FlirLeptonHWIface 77 | { 78 | private: 79 | /*! 80 | * @brief Flir Lepton VoSPI struct. 81 | */ 82 | struct FlirSpi 83 | { 84 | std::string devicePort; 85 | uint8_t mode; 86 | uint8_t bits; 87 | uint32_t speed; 88 | uint16_t delay; 89 | int handler; 90 | 91 | void configSpiParams(const ros::NodeHandle& nh); 92 | }; 93 | 94 | 95 | /*! 96 | * @brief Flir Lepton data frame acquisition struct. 97 | */ 98 | struct FlirDataFrame 99 | { 100 | uint16_t packetSize; 101 | uint16_t packetsPerFrame; 102 | uint16_t packetSize16; 103 | uint16_t frameSize16; 104 | 105 | uint8_t* frameBuffer; 106 | uint16_t* cleanDataBuffer; 107 | uint16_t* frameData; // Clean flir data frame. 108 | //std::vector dataV; 109 | uint16_t minVal; 110 | uint16_t maxVal; 111 | void allocateBuffers(void); 112 | }; 113 | 114 | 115 | /* ------< Published Topics >------ */ 116 | std::string grayTopic_; 117 | std::string rgbTopic_; 118 | std::string temperTopic_; 119 | std::string batchTopic_; 120 | /* -------------------------------- */ 121 | 122 | /* -------< ROS Publishers >------- */ 123 | ros::Publisher grayPublisher_; 124 | ros::Publisher rgbPublisher_; 125 | ros::Publisher temperPublisher_; 126 | ros::Publisher batchPublisher_; 127 | ros::Publisher raw_publisher_; 128 | /* -------------------------------- */ 129 | ros::NodeHandle nh_; 130 | ros::Time now_; 131 | 132 | FlirSpi flirSpi_; // SPI interface container 133 | float vospiFps_; 134 | FlirDataFrame flirDataFrame_; 135 | uint16_t* lastFrame_; 136 | 137 | /* -----< Thermal Image Characteristics >----- */ 138 | std::string frameId_; 139 | /* ------------------------------------------- */ 140 | 141 | /* ------< Scene Temperature Values >--------- */ 142 | float frameAvgTemper_; 143 | /* ------------------------------------------- */ 144 | 145 | /* -------------< Publishing Messages >-------------- */ 146 | flir_lepton_msgs::TemperaturesMsg temperMsg_; 147 | flir_lepton_msgs::FlirLeptonBatchMsg batchMsg_; 148 | sensor_msgs::Image grayImage_; 149 | sensor_msgs::Image rgbImage_; 150 | /* -------------------------------------------------- */ 151 | 152 | // Raw sensor signal values to absolute thermal values map 153 | std::map calibMap_; 154 | std::string calibFileUri_; 155 | 156 | bool pubGray_; 157 | bool pubRgb_; 158 | 159 | // Threads 160 | boost::thread ioThread_; 161 | boost::mutex mtxLock_; 162 | 163 | 164 | /*! 165 | * @brief Loads parameters from parameter server 166 | */ 167 | void loadParameters(void); 168 | 169 | 170 | /*! 171 | * @brief Opens SPI device port for communication with flir lepton camera 172 | */ 173 | void openDevice(void); 174 | 175 | 176 | /*! 177 | * @brief Closes the SPI device communication port 178 | */ 179 | void closeDevice(void); 180 | 181 | 182 | void initThreadedIO(void); 183 | 184 | 185 | /*! 186 | * @brief Reads a frame from flir lepton thermal camera 187 | */ 188 | void readFrame(void); 189 | 190 | 191 | /*! 192 | * @brief Exports thermal signal values from an obtained VoSPI frame 193 | */ 194 | void processFrame(void); 195 | 196 | 197 | /*! 198 | * @brief Allocate the uint16_t buffer memory 199 | */ 200 | void allocateFrameData(void); 201 | 202 | 203 | void fillBatchMsg(void); 204 | 205 | 206 | float calcVoSPIfps( 207 | boost::posix_time::ptime& start, boost::posix_time::ptime& stop); 208 | 209 | 210 | public: 211 | 212 | /*! 213 | * @brief Default constructor 214 | */ 215 | FlirLeptonHWIface(const std::string& ns); 216 | 217 | 218 | /*! 219 | * @brief Default Destructor 220 | */ 221 | virtual ~FlirLeptonHWIface(); 222 | 223 | 224 | /*! 225 | * @brief Reads a thermal scene frame and publishes the image 226 | * on the relevant topic 227 | */ 228 | void run(void); 229 | 230 | }; 231 | 232 | } // namespace flir_lepton_sensor 233 | } // namespace flir_lepton 234 | 235 | #endif // FLIR_LEPTON_FLIR_LEPTON_H 236 | -------------------------------------------------------------------------------- /flir_lepton_sensor/include/flir_lepton_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * * 3 | * * Software License Agreement (BSD License) 4 | * * 5 | * * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * * All rights reserved. 7 | * * 8 | * * Redistribution and use in source and binary forms, with or without 9 | * * modification, are permitted provided that the following conditions 10 | * * are met: 11 | * * 12 | * * * Redistributions of source code must retain the above copyright 13 | * * notice, this list of conditions and the following disclaimer. 14 | * * * Redistributions in binary form must reproduce the above 15 | * * copyright notice, this list of conditions and the following 16 | * * disclaimer in the documentation and/or other materials provided 17 | * * with the distribution. 18 | * * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * * contributors may be used to endorse or promote products derived 20 | * * from this software without specific prior written permission. 21 | * * 22 | * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * * POSSIBILITY OF SUCH DAMAGE. 34 | * * 35 | * * Author: Konstantinos Panayiotou, Angelos Triantafyllidis, 36 | * * Tsirigotis Christos 37 | * * Maintainer: Konstantinos Panayiotou 38 | * * Email: klpanagi@gmail.com 39 | * 40 | * *********************************************************************/ 41 | 42 | #ifndef FLIR_LEPTON_UTILS_H 43 | #define FLIR_LEPTON_UTILS_H 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include "colormaps.h" 51 | 52 | 53 | namespace flir_lepton 54 | { 55 | namespace flir_lepton_sensor 56 | { 57 | class Utils : private boost::noncopyable 58 | { 59 | public: 60 | /*! 61 | * @brief Converts signal values to raw_image values. 62 | * @param signal signal value 63 | * @param minVal Minimun signal value captured on a thermal image frame. 64 | * @param maxVal Maximum signal value captured on a thermal image frame. 65 | * @return raw_image value. 66 | */ 67 | static uint8_t signalToImageValue( 68 | uint16_t signal, uint16_t minVal, uint16_t maxVal); 69 | 70 | 71 | /*! 72 | * @brief Convert a signal value to absolute temperature value. 73 | * @param signalValue Signal value obtained from flir-lepton sensor. 74 | */ 75 | static float signalToTemperature(uint16_t signalValue, 76 | std::map& tempMap); 77 | 78 | 79 | /*! 80 | * @brief Fill an std::map that contains the calibration dataset 81 | */ 82 | static std::map loadThermalCalibMap( 83 | const std::string& calibFileUri); 84 | 85 | static void toColormap(uint8_t val, uint8_t& red, uint8_t& green, 86 | uint8_t& blue); 87 | 88 | 89 | }; 90 | 91 | } // namespace flir_lepton_sensor 92 | } // namespace flir_lepton 93 | 94 | #endif 95 | 96 | 97 | -------------------------------------------------------------------------------- /flir_lepton_sensor/launch/flir_lepton_sensor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 15 | 16 | 17 | 18 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /flir_lepton_sensor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | flir_lepton_sensor 3 | 0.0.1 4 | This package containes sources for communicating with the FlirLepton LWIR sensor 5 | 6 | Konstantinos Panayiotou 7 | 8 | BSD 9 | 10 | https://github.com/klpanagi/rpi_hardware_interface/issues 11 | https://github.com/klpanagi/rpi_hardware_interface.git 12 | 13 | Konstantinos Panayiotou 14 | 15 | catkin 16 | message_runtime 17 | 18 | roscpp 19 | roslib 20 | std_msgs 21 | sensor_msgs 22 | flir_lepton_msgs 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /flir_lepton_sensor/src/flir_lepton_sensor.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * * 3 | * * Software License Agreement (BSD License) 4 | * * 5 | * * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * * All rights reserved. 7 | * * 8 | * * Redistribution and use in source and binary forms, with or without 9 | * * modification, are permitted provided that the following conditions 10 | * * are met: 11 | * * 12 | * * * Redistributions of source code must retain the above copyright 13 | * * notice, this list of conditions and the following disclaimer. 14 | * * * Redistributions in binary form must reproduce the above 15 | * * copyright notice, this list of conditions and the following 16 | * * disclaimer in the documentation and/or other materials provided 17 | * * with the distribution. 18 | * * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * * contributors may be used to endorse or promote products derived 20 | * * from this software without specific prior written permission. 21 | * * 22 | * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * * POSSIBILITY OF SUCH DAMAGE. 34 | * * 35 | * * Author: Konstantinos Panayiotou, Angelos Triantafyllidis, 36 | * * Tsirigotis Christos 37 | * * Maintainer: Konstantinos Panayiotou 38 | * * Email: klpanagi@gmail.com 39 | * *********************************************************************/ 40 | 41 | #include "ros/ros.h" 42 | 43 | #include "flir_lepton_hw_iface.h" 44 | 45 | using namespace flir_lepton; 46 | 47 | int main(int argc, char** argv) 48 | { 49 | ros::init(argc, argv, "flir_lepton_sensor_node"); 50 | // Register Nodehandle under the /flir_lepton_sensor namespace 51 | flir_lepton_sensor::FlirLeptonHWIface flirLepton("/flir_lepton_sensor"); 52 | 53 | int rate; 54 | ros::NodeHandle("/flir_lepton_sensor").param( 55 | "frame_rate", rate, 20); 56 | ros::Rate loop_rate(rate); 57 | 58 | while(ros::ok()) 59 | { 60 | flirLepton.run(); 61 | loop_rate.sleep(); 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /flir_lepton_sensor/src/flir_lepton_utils.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * * 3 | * * Software License Agreement (BSD License) 4 | * * 5 | * * Copyright (c) 2015, P.A.N.D.O.R.A. Team. 6 | * * All rights reserved. 7 | * * 8 | * * Redistribution and use in source and binary forms, with or without 9 | * * modification, are permitted provided that the following conditions 10 | * * are met: 11 | * * 12 | * * * Redistributions of source code must retain the above copyright 13 | * * notice, this list of conditions and the following disclaimer. 14 | * * * Redistributions in binary form must reproduce the above 15 | * * copyright notice, this list of conditions and the following 16 | * * disclaimer in the documentation and/or other materials provided 17 | * * with the distribution. 18 | * * * Neither the name of the P.A.N.D.O.R.A. Team nor the names of its 19 | * * contributors may be used to endorse or promote products derived 20 | * * from this software without specific prior written permission. 21 | * * 22 | * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * * POSSIBILITY OF SUCH DAMAGE. 34 | * * 35 | * * Author: Konstantinos Panayiotou, Angelos Triantafyllidis, 36 | * * Tsirigotis Christos 37 | * * Maintainer: Konstantinos Panayiotou 38 | * * Email: klpanagi@gmail.com 39 | * 40 | * *********************************************************************/ 41 | 42 | #include "flir_lepton_utils.h" 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace flir_lepton 49 | { 50 | namespace flir_lepton_sensor 51 | { 52 | uint8_t Utils::signalToImageValue( 53 | uint16_t signal, uint16_t minVal, uint16_t maxVal) 54 | { 55 | uint16_t imageValue = 0; 56 | uint16_t diff = 0; 57 | float scale = 0.0; 58 | diff = maxVal - minVal; 59 | scale = 255.0/diff; 60 | imageValue = (float)(signal-minVal) * scale; 61 | return (uint8_t)(imageValue); 62 | } 63 | 64 | 65 | float Utils::signalToTemperature(uint16_t signalValue, 66 | std::map& tempMap) 67 | { 68 | // The input signalValue is the keyword of the map with temperatures 69 | std::map::iterator search = tempMap.find(signalValue); 70 | float temp_celcius = 0.0; 71 | 72 | if(search != tempMap.end()) 73 | { 74 | // Pass the value from the map 75 | temp_celcius = search->second; 76 | } 77 | return temp_celcius; 78 | } 79 | 80 | 81 | std::map Utils::loadThermalCalibMap( 82 | const std::string& calibFileUri) 83 | { 84 | /* ---< Open a file input stream to read the dataset >--- */ 85 | std::ifstream file(calibFileUri.c_str()); 86 | 87 | // Evaluate existance of given filepath 88 | if(! file.good()) 89 | { 90 | ROS_FATAL("Temperature calibration dataset file [%s] does not exist!", 91 | calibFileUri.c_str()); 92 | file.close(); 93 | exit(1); 94 | } 95 | std::string line; 96 | 97 | std::map calibMap; 98 | 99 | // Value to be stored in map and its name in the file that we read 100 | float value = 0; 101 | uint16_t keyword = 0; 102 | std::string value_s; 103 | std::string keyword_s; 104 | std::istringstream ss; 105 | 106 | // If counter even -> read keyword of map from file 107 | // If counter odd -> read value of map from file 108 | int counter = 2; 109 | 110 | if(file.is_open()) 111 | { 112 | while(std::getline(file, line)) 113 | { 114 | if((counter % 2) == 0) 115 | { 116 | // Read the keyword of map and convert it to uint16_t 117 | keyword_s = line; 118 | std::istringstream ss(keyword_s); 119 | ss >> keyword; 120 | 121 | if(ss.fail()) 122 | { 123 | std::cout << "Failed to read thermal-signal dataset\n"; 124 | exit(1); 125 | } 126 | } 127 | else 128 | { 129 | value_s = line; 130 | std::istringstream ss(value_s); 131 | // Convert string to float for map 132 | ss >> value; 133 | 134 | if(ss.fail()) 135 | { 136 | std::cout << "Failed to read thermal-signal dataset\n"; 137 | exit(1); 138 | } 139 | // Fill the map with the next pair 140 | calibMap[keyword] = value; 141 | } 142 | counter++; 143 | } 144 | file.close(); 145 | } 146 | else 147 | { 148 | std::cout << "Failed to open file\n"; 149 | exit(1); 150 | } 151 | return calibMap; 152 | } 153 | 154 | void Utils::toColormap(uint8_t val, uint8_t& red, uint8_t& green, 155 | uint8_t& blue) 156 | { 157 | red = colormap::rainbow[3 * val]; 158 | green = colormap::rainbow[3 * val + 1]; 159 | blue = colormap::rainbow[3 * val + 2]; 160 | } 161 | 162 | } // namespace flir_lepton_sensor 163 | } // namespace flir_lepton 164 | --------------------------------------------------------------------------------