├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── htmap │ ├── HTMap.h │ ├── bayes │ └── BayesFilter.h │ ├── imgdesc │ ├── GlobalDescriptor.h │ ├── KeypointDescriptor.h │ ├── KeypointDetector.h │ └── ldb.h │ ├── lc │ └── LoopCloser.h │ ├── map │ ├── HighLevelMap.h │ └── Location.h │ └── util │ ├── Image.h │ ├── Params.h │ ├── Statistics.h │ └── Util.h ├── launch └── htmap.launch ├── matlab ├── PR.m ├── PR_curve.m ├── PR_curves.m ├── computeDistance.m ├── computeGPSDist.m ├── computeTotalDistanceGPS.m ├── generateGroundTruth.m ├── get_files.m ├── gtruths │ ├── groundtruth_CC.mat │ ├── groundtruth_KITTI00.mat │ ├── groundtruth_KITTI05.mat │ ├── groundtruth_KITTI06.mat │ ├── groundtruth_NC.mat │ └── groundtruth_StLucia.mat ├── haversine.m ├── load_loops_data.m ├── plot_path.m ├── plot_times.m ├── plot_times_bars.m ├── plot_topmap.m └── sparsity.m ├── package.xml ├── ros ├── README.md └── ros.def └── src ├── HTMap.cpp ├── bayes └── BayesFilter.cpp ├── imgdesc ├── GlobalDescriptor.cpp ├── KeypointDescriptor.cpp ├── KeypointDetector.cpp └── ldb.cpp ├── lc └── LoopCloser.cpp ├── map ├── HighLevelMap.cpp └── Location.cpp ├── nodes └── HTMap_node.cpp └── util ├── Params.cpp ├── Statistics.cpp └── Util.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(htmap) 3 | 4 | # Catkin dependencies 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | std_msgs 8 | sensor_msgs 9 | image_transport 10 | cv_bridge 11 | geometry_msgs 12 | obindex 13 | ) 14 | 15 | # System dependencies 16 | find_package(OpenCV REQUIRED) 17 | find_package(Boost REQUIRED COMPONENTS system filesystem) 18 | find_package(OpenMP REQUIRED) 19 | if(OPENMP_FOUND) 20 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 22 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 23 | endif() 24 | 25 | # Defining the package 26 | catkin_package( 27 | INCLUDE_DIRS include 28 | LIBRARIES keypoint_description_htmap #htmap 29 | CATKIN_DEPENDS 30 | roscpp 31 | std_msgs 32 | sensor_msgs 33 | image_transport 34 | cv_bridge 35 | geometry_msgs 36 | obindex 37 | DEPENDS OpenCV Boost 38 | ) 39 | 40 | ########### 41 | ## Build ## 42 | ########### 43 | 44 | # Including directories. 45 | include_directories( 46 | include 47 | ${catkin_INCLUDE_DIRS} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${Boost_INCLUDE_DIRS}) 50 | 51 | # Description Library 52 | add_library(description_htmap 53 | src/imgdesc/KeypointDetector.cpp 54 | src/imgdesc/KeypointDescriptor.cpp 55 | src/imgdesc/GlobalDescriptor.cpp 56 | src/imgdesc/ldb.cpp) 57 | target_link_libraries(description_htmap ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 58 | 59 | # HTMap Library 60 | set(HTMAP_SRCS 61 | src/bayes/BayesFilter.cpp 62 | src/util/Params.cpp 63 | src/util/Statistics.cpp 64 | src/util/Util.cpp 65 | src/map/Location.cpp 66 | src/map/HighLevelMap.cpp 67 | src/lc/LoopCloser.cpp 68 | src/HTMap.cpp 69 | ) 70 | add_library(htmap ${HTMAP_SRCS}) 71 | target_link_libraries(htmap description_htmap ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) 72 | 73 | # HTMap Node 74 | add_executable(htmap_node src/nodes/HTMap_node.cpp) 75 | target_link_libraries(htmap_node htmap) 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # HTMap: Hierarchical Topological Mapping 2 | 3 | HTMap is an appearance-based approach for topological mapping based on a hierarchical decomposition of the environment. In HTMap, images with similar visual properties are grouped together in nodes, which are represented by means of an average global descriptor (PHOG) and an index of binary features based on an incremental bag-of-binary-words approach ([OBIndex](http://github.com/emiliofidalgo/obindex)). Each image is represented by means of a global descriptor and a set of local features, and this information is used in a two-level loop closure approach, where, first, global descriptors are employed to obtain the most likely nodes of the map and then binary image features are used to retrieve the most likely images inside these nodes. This hierarchical scheme allows us to reduce the search space when recognizing places, maintaining high accuracy when creating a map. The algorithm can be used to detect loop closures in a unknown environment, without the need of a training stage, as it is usual in BoW schemes. 4 | 5 | This repository contains the code used in our IEEE-TRO [publication](http://ieeexplore.ieee.org/document/7938750/) called *Hierarchical Place Recognition for Topological Mapping*, but adapted to be used with OpenCV 3.x. Given the differentes between OpenCV versions, it might be difficult to reproduce *exactly* the same results of our paper. This code has been released for illustration purposes. 6 | 7 | HTMap is released as a ROS package, and relies on [OpenCV 3.x](http://opencv.org), [OBIndex](http://github.com/emiliofidalgo/obindex) and [Boost](http://www.boost.org) libraries. Note that HTMap is research code. The authors are not responsible for any errors it may contain. **Use it at your own risk!** 8 | 9 | # Conditions of use 10 | 11 | HTMap is distributed under the terms of the [GPL3 License](http://github.com/emiliofidalgo/htmap/blob/master/LICENSE). 12 | 13 | # Related publication 14 | 15 | The details of the algorithm are explained in the [following publication](http://ieeexplore.ieee.org/document/7938750/): 16 | 17 | **Hierarchical Place Recognition for Topological Mapping**
18 | Emilio Garcia-Fidalgo and Alberto Ortiz
19 | IEEE Transactions on Robotics, Vol. 33, No. 5, Pgs. 1061-1074 (Oct. 2017)
20 | 21 | If you use this code, please cite: 22 | ``` 23 | @article{Garcia-Fidalgo2017, 24 | author={Emilio Garcia-Fidalgo and Alberto Ortiz}, 25 | journal={IEEE Transactions on Robotics}, 26 | title={Hierarchical Place Recognition for Topological Mapping}, 27 | year={2017}, 28 | volume={33}, 29 | number={5}, 30 | pages={1061-1074}, 31 | doi={10.1109/TRO.2017.2704598}, 32 | month={Oct} 33 | } 34 | ``` 35 | 36 | # Singularity Container 37 | 38 | Find the Singularity image definition files and instructions provided in [ros](ros) folder 39 | 40 | # Installation 41 | 42 | 1. First of all, you need to install [OBIndex](http://github.com/emiliofidalgo/obindex) dependencies: 43 | ``` 44 | sudo apt-get install libflann-dev libboost-system-dev libboost-filesystem-dev 45 | ``` 46 | 47 | 2. Clone OBIndex in your workspace and change the current branch to *kinetic*: 48 | ``` 49 | cd ~/your_workspace/src 50 | git clone http://github.com/emiliofidalgo/obindex.git 51 | cd obindex 52 | git checkout kinetic 53 | ``` 54 | 55 | 3. Clone HTMap in your workspace: 56 | ``` 57 | cd ~/your_workspace/src 58 | git clone http://github.com/emiliofidalgo/htmap.git 59 | ``` 60 | 61 | 4. Compile the packages using, as usual, the `catkin_make` command: 62 | ``` 63 | cd .. 64 | catkin_make -DCMAKE_BUILD_TYPE=Release 65 | ``` 66 | 67 | 5. As a final step, HTMap requires a writable directory where it stores results and serializes some data. Therefore, we recommend to create one for this purpose at this point: 68 | ``` 69 | mkdir -p ~/Desktop/htmap 70 | ``` 71 | 72 | # Usage 73 | 74 | - To run HTMap, use the provided launch file: 75 | ``` 76 | roslaunch htmap htmap.launch image_dir:="/image/dir" working_dir:="~/Desktop/htmap" 77 | ``` 78 | where *image_dir* is a directory containing the sequence of images to be processed and *working_dir* is the directory where the algorithm will store their results and serialization data. This might be the directory we created during the installation procedure. 79 | 80 | There exist several execution parameters that can be configured when running HTMap. See next section for a description of each of them. 81 | 82 | - During the execution, screen will show information about the algorithm, indicating when a loop closure is found. 83 | 84 | - After a succesful execution, HTMap generates, as output, several text files. These files are stored in the *working_dir/results* directory. To speed up the mapping process, some of them are empty, since the corresponding parts of the code are commented. They can be uncommented if needed. The most important files are: 85 | 86 | - *htmap_loops_ninliers.txt*, which is a binary matrix where entry (*i, j*) is 1 if image *i* and image *j* can be considered as the same place, and 0 otherwise. Therefore, the *i-th* row represents the possible loop closures for image *i*. The name *ninliers* refers to the value of the parameter used as the minimum number of inliers to accept a loop. See the paper for further details. 87 | 88 | - *htmap_img2loc_ninliers.txt*, where each line contains the identifier of each image and the location to which that image was assigned during the process. 89 | 90 | - We provide several Matlab scripts to process the resulting files. Along with these scripts, we also provide ground truth files for the datasets used in the paper. If you want to use your own dataset, you should generate your own ground truth file using the same format. 91 | 92 | - In order to obtain a precision / recall value, open Matlab and select the provided *matlab* directory as your working directory. Next, use the following sequence of commands, adapting, of course, the paths (*CC* stands for the *City Centre* dataset): 93 | ``` 94 | loop_file = '~/Desktop/htmap/results/htmap_loops_20.txt'; 95 | gt_file = 'gtruths/groundtruth_CC.mat'; 96 | PR(loop_file, gt_file, true); 97 | ``` 98 | 99 | - You can also generate a precision / recall curve after obtaining a set of precision / recall values. In our original paper, these curves were generated modifying the minimum number of inliers to accept a loop. To do that: 100 | - Set the parameter *batch* to *true*. 101 | - Set the parameter *inliers_begin* to the number of inliers that you want to use at the first execution of HTMap. 102 | - Set the parameter *inliers_end* to the number of inliers where the process will stop. 103 | - Set the parameter *inliers_step* to the number of inliers to increment between each execution of HTMap. 104 | 105 | 106 | - Running HTMap under these conditions will execute the algorithm several times modifying the number of inliers and storing the corresponding files in the *results* directory. The precision / recall curve can be obtained using the following sequence of commands, adapting, again, the paths (*CC* stands for the *City Centre* dataset): 107 | ``` 108 | res_dir = '~/Desktop/htmap/results/'; 109 | gt_file = 'gtruths/groundtruth_CC.mat'; 110 | PR_curve(res_dir, gt_file, true); 111 | ``` 112 | 113 | - Contact the authors for more information about the other figures in the article. 114 | 115 | # Parameters 116 | 117 | - `image_dir`: Directory where the input images are stored. The images should be named consecutively using the same number of digits. For instance: *image00001.jpg*, *image00002.jpg*, and so on. 118 | 119 | - `working_dir`: Directory where HTMap operates and where the final results will be stored. **This directory should be writable!**. 120 | 121 | - `detector`: Local feature detector method. Options: *FAST*, *BRISK*, *SIFT*, *SURF*, *ORB* and *STAR*. Default: *FAST*. 122 | 123 | - `max_total_kps`: Number of features to find in each input image. Default: *1000*. 124 | 125 | - `descriptor`: Local feature descriptor method. Options: *FREAK*, *BRIEF*, *BRISK*, *ORB* and *LDB*. Default: *LDB*. 126 | 127 | - `gdescriptor`: Holistic method used to describe an image as a whole. Options: *PHOG*, *WI-SIFT*, *WI-SURF*, *WI-LDB* and *Brief-Gist*. Default: *PHOG*. 128 | 129 | - `load_feats`: If *true*, image keypoints and descriptions are loaded from disk, from a previous execution. Otherwise, images are described. 130 | 131 | - `match_method`: Method used to match between two images. Options: *ratio* and *crosscheck*. Default: *ratio*. 132 | 133 | - `match_ratio`: If `match_method` is *ratio*, nearest neighbour distance ratio to match descriptors. Usually, a value of 0.8 is a good option here. 134 | 135 | - `locLC_thresh`: Threshold to select locations according to their similarity with the current image during the coarse loop closure stage. Default: 0.65. 136 | 137 | - `imageLC_min_inliers`: Minimum number of inliers to consider a loop closure candidate as valid. Default: 45. 138 | 139 | - `imageLC_disc_recent`: Number of recent frames discarded to avoid loop closures with images close in time. This is highly dependant of the environment and should be configured accordingly. 140 | 141 | - `imageLC_tloop`: Minimum Bayes Filter sum of probabilities to consider an image as a possible candidate. Default: 0.8. 142 | 143 | - `loc_max_images`: Maximum number of images per location. Default: 500. 144 | 145 | - `max_sim_newnode`: Similarity to consider a new node. Default: 0.20. 146 | 147 | - `batch`: If *true*, the algorithm is executed several times according to the configuration indicated by the *inliers_* parameters. Otherwise, it is executed only once. 148 | 149 | - `inliers_begin`: If `batch` is *true*, the number of inliers to be used at the first execution of HTMap. 150 | 151 | - `inliers_end`: If `batch` is *true*, the number of inliers where the process will stop. 152 | 153 | - `inliers_step`: If `batch` is *true*, the number of inliers to increment between each execution of HTMap. 154 | 155 | # Contact 156 | 157 | If you have problems or questions using this code, please contact the author (emilio.garcia@uib.es). [Feature requests](http://github.com/emiliofidalgo/htmap/issues) and [contributions](http://github.com/emiliofidalgo/htmap/pulls) are totally welcome. 158 | -------------------------------------------------------------------------------- /include/htmap/HTMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef HTMAP_H 21 | #define HTMAP_H 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "htmap/lc/LoopCloser.h" 30 | #include "htmap/map/HighLevelMap.h" 31 | #include "htmap/util/Image.h" 32 | #include "htmap/util/Statistics.h" 33 | #include "htmap/util/Params.h" 34 | 35 | #define SSTR( x ) dynamic_cast< std::ostringstream & >( \ 36 | ( std::ostringstream() << std::dec << x ) ).str() 37 | 38 | namespace htmap 39 | { 40 | 41 | class HTMap 42 | { 43 | public: 44 | HTMap(const ros::NodeHandle nh); 45 | ~HTMap(); 46 | 47 | void process(); 48 | void processBatch(); 49 | 50 | private: 51 | // ROS 52 | ros::NodeHandle _nh; 53 | 54 | // Parameters 55 | Params* _params; 56 | 57 | // Statistics 58 | Statistics* _st; 59 | 60 | void describeImages(std::vector& images); 61 | void map(const std::vector& images, HighLevelMap& map, LoopCloser& _lc); 62 | bool isNewLocation(const Image& img, HighLevelMap& map); 63 | }; 64 | 65 | } 66 | 67 | #endif // HTMAP_H 68 | -------------------------------------------------------------------------------- /include/htmap/bayes/BayesFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _BAYESFILTER_H_ 21 | #define _BAYESFILTER_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "htmap/util/Params.h" 31 | #include "htmap/util/Statistics.h" 32 | 33 | namespace htmap 34 | { 35 | 36 | enum BayesTransitionModel 37 | { 38 | BAYES_TRANS_GAUSS, 39 | BAYES_TRANS_UNIFORM 40 | }; 41 | 42 | struct BayesFilterParams 43 | { 44 | BayesFilterParams() : 45 | trans_model(BAYES_TRANS_GAUSS), 46 | min_hyp(20) 47 | {} 48 | 49 | BayesTransitionModel trans_model; 50 | int min_hyp; 51 | }; 52 | 53 | struct BayesFilterResult 54 | { 55 | BayesFilterResult(const int id, const double p) : 56 | elem_id(id), 57 | score(p) 58 | { 59 | } 60 | 61 | bool operator<(const BayesFilterResult& other) const { return score > other.score; } 62 | 63 | int elem_id; 64 | double score; 65 | }; 66 | 67 | 68 | class BayesFilter 69 | { 70 | public: 71 | BayesFilter(const BayesFilterParams params = BayesFilterParams()); 72 | virtual ~BayesFilter(); 73 | 74 | void addElement(const int elem_id); 75 | std::vector* getElements(); 76 | void predict(const unsigned curr_img, std::vector* prior); 77 | void update(const unsigned curr_img, const std::map& lk, std::vector* prior); 78 | void getMostProbablyElements(std::vector& elems); 79 | unsigned numElems(); 80 | 81 | protected: 82 | unsigned _total_elements; 83 | BayesTransitionModel _trans_model; 84 | unsigned _min_hyp; 85 | std::vector _probability; 86 | std::map _index_to_element; 87 | std::map _element_to_index; 88 | std::vector* _elements; 89 | Statistics* _st; 90 | Params* _params; 91 | }; 92 | 93 | } 94 | 95 | #endif /* BAYESFILTER_H_ */ 96 | -------------------------------------------------------------------------------- /include/htmap/imgdesc/GlobalDescriptor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _GLOBALDESCRIPTOR_H_ 21 | #define _GLOBALDESCRIPTOR_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include "htmap/util/Util.h" 27 | #include "ldb.h" 28 | 29 | namespace htmap 30 | { 31 | 32 | enum GlobalDescriptorType 33 | { 34 | GDESCRIPTOR_WISIFT, 35 | GDESCRIPTOR_WISURF, 36 | GDESCRIPTOR_BRIEFGIST, 37 | GDESCRIPTOR_WILDB, 38 | GDESCRIPTOR_PHOG 39 | }; 40 | 41 | // --- 42 | // General parameters for global descriptors 43 | // --- 44 | struct GlobalDescriptorParams 45 | { 46 | GlobalDescriptorParams() 47 | {} 48 | }; 49 | 50 | // --- 51 | // Abstract global descriptor class. 52 | // --- 53 | class GlobalDescriptor 54 | { 55 | public: 56 | GlobalDescriptor(const GlobalDescriptorType& type, const int nbytes) : 57 | _type(type), 58 | _desc_size(nbytes) 59 | {} 60 | virtual ~GlobalDescriptor() 61 | {} 62 | 63 | inline int getDescSize() { return _desc_size; } 64 | inline GlobalDescriptorType getType() { return _type; } 65 | static double dist(const cv::Mat& a, const cv::Mat& b, GlobalDescriptor* desc); 66 | static double dist(const cv::Mat& a, const cv::Mat& b, const cv::Mat& icovar); 67 | 68 | static GlobalDescriptor* create(const std::string& name, const GlobalDescriptorParams& params); 69 | 70 | virtual void parseParameters(const GlobalDescriptorParams& params) = 0; 71 | virtual void describe(const cv::Mat& image, cv::Mat& desc) = 0; 72 | 73 | protected: 74 | GlobalDescriptorType _type; 75 | int _desc_size; 76 | }; 77 | 78 | // --- 79 | // WI-SIFT descriptor class. 80 | // --- 81 | class WISIFTDescriptor : public GlobalDescriptor 82 | { 83 | public: 84 | WISIFTDescriptor(const GlobalDescriptorParams& params) : 85 | GlobalDescriptor(GDESCRIPTOR_WISIFT, 256) 86 | { 87 | parseParameters(params); 88 | } 89 | 90 | void parseParameters(const GlobalDescriptorParams& params) 91 | { 92 | } 93 | 94 | void describe(const cv::Mat& image, cv::Mat& desc); 95 | }; 96 | 97 | // --- 98 | // WI-SURF descriptor class. 99 | // --- 100 | class WISURFDescriptor : public GlobalDescriptor 101 | { 102 | public: 103 | WISURFDescriptor(const GlobalDescriptorParams& params) : 104 | GlobalDescriptor(GDESCRIPTOR_WISURF, 256) 105 | { 106 | parseParameters(params); 107 | } 108 | 109 | void parseParameters(const GlobalDescriptorParams& params) 110 | { 111 | } 112 | 113 | void describe(const cv::Mat& image, cv::Mat& desc); 114 | }; 115 | 116 | // --- 117 | // BRIEF-Gist descriptor class. 118 | // --- 119 | class BRIEFGistDescriptor : public GlobalDescriptor 120 | { 121 | public: 122 | BRIEFGistDescriptor(const GlobalDescriptorParams& params) : 123 | GlobalDescriptor(GDESCRIPTOR_BRIEFGIST, 64) 124 | { 125 | parseParameters(params); 126 | } 127 | 128 | void parseParameters(const GlobalDescriptorParams& params) 129 | { 130 | } 131 | 132 | void describe(const cv::Mat& image, cv::Mat& desc); 133 | }; 134 | 135 | // --- 136 | // WI-LDB descriptor class. 137 | // --- 138 | class WILDBDescriptor : public GlobalDescriptor 139 | { 140 | public: 141 | WILDBDescriptor(const GlobalDescriptorParams& params) : 142 | GlobalDescriptor(GDESCRIPTOR_WILDB, 64) 143 | { 144 | parseParameters(params); 145 | } 146 | 147 | void parseParameters(const GlobalDescriptorParams& params) 148 | { 149 | } 150 | 151 | void describe(const cv::Mat& image, cv::Mat& desc); 152 | 153 | private: 154 | LdbDescriptorExtractor _ldb; 155 | }; 156 | 157 | // --- 158 | // PHOG descriptor class. 159 | // --- 160 | class PHOGDescriptor : public GlobalDescriptor 161 | { 162 | public: 163 | PHOGDescriptor(const GlobalDescriptorParams& params) : 164 | GlobalDescriptor(GDESCRIPTOR_PHOG, 420) // 20 Bins (18 deg per bin), and L = 3 165 | { 166 | parseParameters(params); 167 | } 168 | 169 | void parseParameters(const GlobalDescriptorParams& params) 170 | { 171 | } 172 | 173 | void describe(const cv::Mat& image, cv::Mat& desc); 174 | 175 | private: 176 | void getHistogram(const cv::Mat& edges, const cv::Mat& ors, const cv::Mat& mag, int startX, int startY, int width, int height, cv::Mat& hist); 177 | }; 178 | 179 | } 180 | 181 | #endif /* GLOBALDESCRIPTOR_H_ */ 182 | -------------------------------------------------------------------------------- /include/htmap/imgdesc/KeypointDescriptor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef KEYPOINTDESCRIPTOR_H_ 21 | #define KEYPOINTDESCRIPTOR_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include "ldb.h" 27 | 28 | namespace htmap 29 | { 30 | 31 | enum KeypointDescriptorType 32 | { 33 | DESCRIPTOR_BRIEF, 34 | DESCRIPTOR_ORB, 35 | DESCRIPTOR_BRISK, 36 | DESCRIPTOR_FREAK, 37 | DESCRIPTOR_SIFT, 38 | DESCRIPTOR_SURF, 39 | DESCRIPTOR_LDB 40 | }; 41 | 42 | // --- 43 | // General parameters for keypoint descriptors. 44 | // --- 45 | typedef struct _KeypointDescriptorParams 46 | { 47 | _KeypointDescriptorParams() : 48 | _type(DESCRIPTOR_FREAK), 49 | _freak_ori_norm(true), 50 | _freak_scale_norm(true), 51 | _freak_pattern_scale(22.0f), 52 | _freak_octaves(4), 53 | _brisk_thresh(30), 54 | _brisk_octaves(3), 55 | _surf_extended(1) 56 | {} 57 | 58 | // Keypoint descriptor params. 59 | KeypointDescriptorType _type; 60 | 61 | // FREAK params 62 | bool _freak_ori_norm; 63 | bool _freak_scale_norm; 64 | float _freak_pattern_scale; 65 | int _freak_octaves; 66 | 67 | // BRISK params 68 | int _brisk_thresh; 69 | int _brisk_octaves; 70 | 71 | // BRIEF params 72 | 73 | // ORB params 74 | 75 | // SIFT params 76 | 77 | // SURF params 78 | int _surf_extended; 79 | 80 | // LDB params 81 | 82 | } KeypointDescriptorParams; 83 | 84 | // --- 85 | // Abstract keypoint descriptor class. 86 | // --- 87 | class KeypointDescriptor 88 | { 89 | public: 90 | KeypointDescriptor(const KeypointDescriptorType& type, const int nbytes) : 91 | _type(type), 92 | _desc_size(nbytes) {} 93 | virtual ~KeypointDescriptor() {} 94 | 95 | inline int getDescSize() { return _desc_size; } 96 | inline KeypointDescriptorType getType() { return _type; } 97 | 98 | static KeypointDescriptor* create(const std::string& name, const KeypointDescriptorParams& params); 99 | 100 | virtual void parseParameters(const KeypointDescriptorParams& params) = 0; 101 | virtual void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) = 0; 102 | 103 | protected: 104 | KeypointDescriptorType _type; 105 | int _desc_size; 106 | }; 107 | 108 | // --- 109 | // FREAK keypoint descriptor class. 110 | // --- 111 | class FREAKKeypointDescriptor : public KeypointDescriptor 112 | { 113 | public: 114 | FREAKKeypointDescriptor(const KeypointDescriptorParams& params) : 115 | KeypointDescriptor(DESCRIPTOR_FREAK, 64), 116 | _ori_norm(true), 117 | _scale_norm(true), 118 | _pattern_scale(22.0f), 119 | _octaves(4) 120 | { 121 | parseParameters(params); 122 | _descriptor = cv::xfeatures2d::FREAK::create(_ori_norm, 123 | _scale_norm, 124 | _pattern_scale, 125 | _octaves); 126 | } 127 | 128 | void parseParameters(const KeypointDescriptorParams& params) 129 | { 130 | _type = params._type; 131 | _ori_norm = params._freak_ori_norm; 132 | _scale_norm = params._freak_scale_norm; 133 | _pattern_scale = params._freak_pattern_scale; 134 | _octaves = params._freak_octaves; 135 | } 136 | 137 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 138 | 139 | protected: 140 | bool _ori_norm; 141 | bool _scale_norm; 142 | float _pattern_scale; 143 | int _octaves; 144 | cv::Ptr _descriptor; 145 | }; 146 | 147 | // --- 148 | // BRISK keypoint descriptor class. 149 | // --- 150 | class BRISKKeypointDescriptor : public KeypointDescriptor 151 | { 152 | public: 153 | BRISKKeypointDescriptor(const KeypointDescriptorParams& params) : 154 | KeypointDescriptor(DESCRIPTOR_BRISK, 64), 155 | _thresh(30), 156 | _octaves(3) 157 | { 158 | parseParameters(params); 159 | _descriptor = cv::BRISK::create(_thresh, _octaves); 160 | } 161 | 162 | void parseParameters(const KeypointDescriptorParams& params) 163 | { 164 | _type = params._type; 165 | _thresh = params._brisk_thresh; 166 | _octaves = params._brisk_octaves; 167 | } 168 | 169 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 170 | 171 | protected: 172 | int _thresh; 173 | int _octaves; 174 | cv::Ptr _descriptor; 175 | }; 176 | 177 | // --- 178 | // BRIEF keypoint descriptor class. 179 | // --- 180 | class BRIEFKeypointDescriptor : public KeypointDescriptor 181 | { 182 | public: 183 | BRIEFKeypointDescriptor(const KeypointDescriptorParams& params) : 184 | KeypointDescriptor(DESCRIPTOR_BRIEF, 32) 185 | { 186 | parseParameters(params); 187 | _descriptor = cv::xfeatures2d::BriefDescriptorExtractor::create(); 188 | } 189 | 190 | void parseParameters(const KeypointDescriptorParams& params) 191 | { 192 | _type = params._type; 193 | } 194 | 195 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 196 | 197 | protected: 198 | cv::Ptr _descriptor; 199 | }; 200 | 201 | // --- 202 | // ORB keypoint descriptor class. 203 | // --- 204 | class ORBKeypointDescriptor : public KeypointDescriptor 205 | { 206 | public: 207 | ORBKeypointDescriptor(const KeypointDescriptorParams& params) : 208 | KeypointDescriptor(DESCRIPTOR_ORB, 32) 209 | { 210 | parseParameters(params); 211 | _descriptor = cv::ORB::create(); 212 | } 213 | 214 | void parseParameters(const KeypointDescriptorParams& params) 215 | { 216 | _type = params._type; 217 | } 218 | 219 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 220 | 221 | protected: 222 | cv::Ptr _descriptor; 223 | }; 224 | 225 | // --- 226 | // SIFT keypoint descriptor class. 227 | // --- 228 | class SIFTKeypointDescriptor : public KeypointDescriptor 229 | { 230 | public: 231 | SIFTKeypointDescriptor(const KeypointDescriptorParams& params) : 232 | KeypointDescriptor(DESCRIPTOR_SIFT, 128) 233 | { 234 | parseParameters(params); 235 | _descriptor = cv::xfeatures2d::SIFT::create(); 236 | } 237 | 238 | void parseParameters(const KeypointDescriptorParams& params) 239 | { 240 | _type = params._type; 241 | } 242 | 243 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 244 | 245 | protected: 246 | cv::Ptr _descriptor; 247 | }; 248 | 249 | // --- 250 | // SURF keypoint descriptor class. 251 | // --- 252 | class SURFKeypointDescriptor : public KeypointDescriptor 253 | { 254 | public: 255 | SURFKeypointDescriptor(const KeypointDescriptorParams& params) : 256 | KeypointDescriptor(DESCRIPTOR_SURF, 128), 257 | _extended(1) 258 | { 259 | parseParameters(params); 260 | if (!_extended) 261 | { 262 | _desc_size = 64; 263 | } 264 | 265 | _descriptor = cv::xfeatures2d::SURF::create(100, 4, 3, _extended); 266 | } 267 | 268 | void parseParameters(const KeypointDescriptorParams& params) 269 | { 270 | _type = params._type; 271 | _extended = params._surf_extended; 272 | } 273 | 274 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 275 | 276 | protected: 277 | int _extended; 278 | cv::Ptr _descriptor; 279 | }; 280 | 281 | // --- 282 | // LDB keypoint descriptor class. 283 | // --- 284 | class LDBKeypointDescriptor : public KeypointDescriptor 285 | { 286 | public: 287 | LDBKeypointDescriptor(const KeypointDescriptorParams& params) : 288 | KeypointDescriptor(DESCRIPTOR_LDB, 32) 289 | { 290 | parseParameters(params); 291 | } 292 | 293 | void parseParameters(const KeypointDescriptorParams& params) 294 | { 295 | _type = params._type; 296 | } 297 | 298 | void describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs); 299 | 300 | protected: 301 | LdbDescriptorExtractor _ldb; 302 | }; 303 | 304 | } 305 | 306 | #endif /* KEYPOINTDESCRIPTOR_H_ */ 307 | -------------------------------------------------------------------------------- /include/htmap/imgdesc/KeypointDetector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef KEYPOINTDETECTOR_H_ 21 | #define KEYPOINTDETECTOR_H_ 22 | 23 | #include 24 | #include 25 | 26 | namespace htmap 27 | { 28 | 29 | enum KeypointDetectorType 30 | { 31 | DETECTOR_FAST, 32 | DETECTOR_ORB, 33 | DETECTOR_BRISK, 34 | DETECTOR_SIFT, 35 | DETECTOR_SURF, 36 | DETECTOR_STAR 37 | }; 38 | 39 | // --- 40 | // General parameters for keypoint detectors. 41 | // --- 42 | typedef struct _KeypointDetectorParams 43 | { 44 | _KeypointDetectorParams() : 45 | _type(DETECTOR_SIFT), 46 | _fast_thresh(10), 47 | _fast_nonmaxSup(true), 48 | _brisk_thresh(30), 49 | _brisk_octaves(3), 50 | _sift_nfeats(0), 51 | _sift_noctaves(3), 52 | _sift_contrastThresh(0.04), 53 | _sift_edgeThresh(10), 54 | _sift_sigma(1.6), 55 | _surf_hessianThresh(300.0), 56 | _surf_noctaves(4), 57 | _surf_nlayers(2), 58 | _surf_upright(0), 59 | _orb_nfeats(500), 60 | _orb_scalefactor(1.2), 61 | _orb_nlevels(8), 62 | _orb_patchsize(31), 63 | _star_maxsize(45), 64 | _star_responsethresh(30), 65 | _star_linethreshproj(10), 66 | _star_linethreshbin(8), 67 | _star_suppressnonmaxsize(5) 68 | {} 69 | 70 | // Keypoint detector params. 71 | KeypointDetectorType _type; 72 | 73 | // FAST params 74 | int _fast_thresh; 75 | bool _fast_nonmaxSup; 76 | 77 | // BRISK 78 | int _brisk_thresh; 79 | int _brisk_octaves; 80 | 81 | // SIFT 82 | int _sift_nfeats; 83 | int _sift_noctaves; 84 | double _sift_contrastThresh; 85 | double _sift_edgeThresh; 86 | double _sift_sigma; 87 | 88 | // SURF 89 | double _surf_hessianThresh; 90 | int _surf_noctaves; 91 | int _surf_nlayers; 92 | int _surf_upright; 93 | 94 | // ORB 95 | int _orb_nfeats; 96 | double _orb_scalefactor; 97 | int _orb_nlevels; 98 | int _orb_patchsize; 99 | 100 | // STAR 101 | int _star_maxsize; 102 | int _star_responsethresh; 103 | int _star_linethreshproj; 104 | int _star_linethreshbin; 105 | int _star_suppressnonmaxsize; 106 | } KeypointDetectorParams; 107 | 108 | // --- 109 | // Abstract keypoint detector class. 110 | // --- 111 | class KeypointDetector 112 | { 113 | public: 114 | KeypointDetector(const KeypointDetectorType type) : _type(type) {} 115 | virtual ~KeypointDetector() {} 116 | 117 | static KeypointDetector* create(const std::string& name, const KeypointDetectorParams& params); 118 | 119 | virtual void parseParameters(const KeypointDetectorParams& params) = 0; 120 | virtual void detect(const cv::Mat& image, std::vector& kps) = 0; 121 | 122 | cv::Ptr _detector; 123 | 124 | protected: 125 | KeypointDetectorType _type; 126 | }; 127 | 128 | // --- 129 | // FAST keypoint detector class. 130 | // --- 131 | class FASTKeypointDetector : public KeypointDetector 132 | { 133 | public: 134 | FASTKeypointDetector(const KeypointDetectorParams& params) : 135 | KeypointDetector(DETECTOR_FAST), 136 | _thresh(10), 137 | _nonmaxSup(true) 138 | { 139 | parseParameters(params); 140 | _detector = cv::FastFeatureDetector::create(_thresh, _nonmaxSup); 141 | } 142 | 143 | void parseParameters(const KeypointDetectorParams& params) 144 | { 145 | _type = params._type; 146 | _thresh = params._fast_thresh; 147 | _nonmaxSup = params._fast_nonmaxSup; 148 | } 149 | 150 | void detect(const cv::Mat& image, std::vector& kps); 151 | 152 | int _thresh; 153 | bool _nonmaxSup; 154 | }; 155 | 156 | // --- 157 | // BRISK keypoint detector class. 158 | // --- 159 | class BRISKKeypointDetector : public KeypointDetector 160 | { 161 | public: 162 | BRISKKeypointDetector(const KeypointDetectorParams& params) : 163 | KeypointDetector(DETECTOR_BRISK), 164 | _thresh(30), 165 | _octaves(3) 166 | { 167 | parseParameters(params); 168 | _detector = cv::BRISK::create(_thresh, _octaves); 169 | } 170 | 171 | void parseParameters(const KeypointDetectorParams& params) 172 | { 173 | _type = params._type; 174 | _thresh = params._brisk_thresh; 175 | _octaves = params._brisk_octaves; 176 | } 177 | 178 | void detect(const cv::Mat& image, std::vector& kps); 179 | 180 | int _thresh; 181 | int _octaves; 182 | }; 183 | 184 | // --- 185 | // SIFT keypoint detector class. 186 | // --- 187 | class SIFTKeypointDetector : public KeypointDetector 188 | { 189 | public: 190 | SIFTKeypointDetector(const KeypointDetectorParams& params) : 191 | KeypointDetector(DETECTOR_SIFT), 192 | _nfeats(0), 193 | _noctaves(3), 194 | _contrastThresh(0.04), 195 | _edgeThresh(10.0), 196 | _sigma(1.6) 197 | { 198 | parseParameters(params); 199 | _detector = cv::xfeatures2d::SIFT::create(_nfeats, 200 | _noctaves, 201 | _contrastThresh, 202 | _edgeThresh, 203 | _sigma); 204 | } 205 | 206 | void parseParameters(const KeypointDetectorParams& params) 207 | { 208 | _nfeats = params._sift_nfeats; 209 | _noctaves = params._sift_noctaves; 210 | _edgeThresh = params._sift_edgeThresh; 211 | _contrastThresh = params._sift_contrastThresh; 212 | _sigma = params._sift_sigma; 213 | _type = params._type; 214 | } 215 | 216 | void detect(const cv::Mat& image, std::vector& kps); 217 | 218 | int _nfeats; 219 | int _noctaves; 220 | double _contrastThresh; 221 | double _edgeThresh; 222 | double _sigma; 223 | }; 224 | 225 | // --- 226 | // SURF keypoint detector class. 227 | // --- 228 | class SURFKeypointDetector : public KeypointDetector 229 | { 230 | public: 231 | SURFKeypointDetector(const KeypointDetectorParams& params) : 232 | KeypointDetector(DETECTOR_SURF), 233 | _hessianThresh(300.0), 234 | _noctaves(4), 235 | _nlayers(2), 236 | _upright(0) 237 | { 238 | parseParameters(params); 239 | _detector = cv::xfeatures2d::SURF::create(_hessianThresh, 240 | _noctaves, 241 | _nlayers, 242 | false, 243 | _upright); 244 | } 245 | 246 | void parseParameters(const KeypointDetectorParams& params) 247 | { 248 | _hessianThresh = params._surf_hessianThresh; 249 | _noctaves = params._surf_noctaves; 250 | _nlayers = params._surf_nlayers; 251 | _upright = params._surf_upright; 252 | } 253 | 254 | void detect(const cv::Mat& image, std::vector& kps); 255 | 256 | double _hessianThresh; 257 | int _noctaves; 258 | int _nlayers; 259 | int _upright; 260 | }; 261 | 262 | // --- 263 | // ORB keypoint detector class. 264 | // --- 265 | class ORBKeypointDetector : public KeypointDetector 266 | { 267 | public: 268 | ORBKeypointDetector(const KeypointDetectorParams& params) : 269 | KeypointDetector(DETECTOR_ORB), 270 | _nfeats(500), 271 | _scalefactor(1.2f), 272 | _nlevels(8), 273 | _patchsize(31) 274 | { 275 | parseParameters(params); 276 | _detector = cv::ORB::create(_nfeats, 277 | _scalefactor, 278 | _nlevels, 279 | 31, 280 | 0, 281 | 2, 282 | cv::ORB::HARRIS_SCORE, 283 | _patchsize); 284 | } 285 | 286 | void parseParameters(const KeypointDetectorParams& params) 287 | { 288 | _nfeats = params._orb_nfeats; 289 | _scalefactor = params._orb_scalefactor; 290 | _nlevels = params._orb_nlevels; 291 | _patchsize = params._orb_patchsize; 292 | } 293 | 294 | void detect(const cv::Mat& image, std::vector& kps); 295 | 296 | int _nfeats; 297 | double _scalefactor; 298 | int _nlevels; 299 | int _patchsize; 300 | }; 301 | 302 | // --- 303 | // STAR keypoint detector class. 304 | // --- 305 | class StarKeypointDetector : public KeypointDetector 306 | { 307 | public: 308 | StarKeypointDetector(const KeypointDetectorParams& params) : 309 | KeypointDetector(DETECTOR_STAR), 310 | _maxsize(45), 311 | _responsethresh(30), 312 | _linethreshproj(10), 313 | _linethreshbin(8), 314 | _suppressnonmaxsize(5) 315 | { 316 | parseParameters(params); 317 | _detector = cv::xfeatures2d::StarDetector::create(_maxsize, 318 | _responsethresh, 319 | _linethreshproj, 320 | _linethreshbin, 321 | _suppressnonmaxsize); 322 | } 323 | 324 | void parseParameters(const KeypointDetectorParams& params) 325 | { 326 | _maxsize = params._star_maxsize; 327 | _responsethresh = params._star_responsethresh; 328 | _linethreshproj = params._star_linethreshproj; 329 | _linethreshbin = params._star_linethreshbin; 330 | _suppressnonmaxsize = params._star_suppressnonmaxsize; 331 | } 332 | 333 | void detect(const cv::Mat& image, std::vector& kps); 334 | 335 | int _maxsize; 336 | int _responsethresh; 337 | int _linethreshproj; 338 | int _linethreshbin; 339 | int _suppressnonmaxsize; 340 | }; 341 | 342 | } 343 | 344 | #endif /* KEYPOINTDETECTOR_H_ */ 345 | 346 | -------------------------------------------------------------------------------- /include/htmap/imgdesc/ldb.h: -------------------------------------------------------------------------------- 1 | /* 2 | LDB.h 3 | Created on: Apr 10, 2013 4 | Author: xinyang 5 | 6 | LDB - Local Difference Binary 7 | Reference implementation of 8 | [1] Xin Yang and Kwang-Ting(Tim) Cheng. LDB: An Ultra-Fast Feature for 9 | Scalable Augmened Reality on Mobile Device. In Proceedings of 10 | the IEEE International Symposium on Mixed and Augmented Reality(ISMAR2012). 11 | 12 | Copyright (C) 2012 The Learning-Based Multimedia, University of California, Santa Barbara 13 | Xin Yang, Kwang-Ting(Tim) Cheng. 14 | 15 | This file is part of LDB. 16 | 17 | LDB is free software: you can redistribute it and/or modify 18 | it under the terms of the GNU General Public License as published by 19 | the Free Software Foundation, either version 3 of the License, or 20 | (at your option) any later version. 21 | 22 | LDB is distributed in the hope that it will be useful, 23 | but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | GNU General Public License for more details. 26 | 27 | You should have received a copy of the GNU General Public License 28 | along with LDB. If not, see . 29 | */ 30 | 31 | #ifndef LDB_H_ 32 | #define LDB_H_ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | class LDB 41 | { 42 | public: 43 | 44 | int kBytes; 45 | 46 | LDB(int _bytes = 32, int _nlevels = 3, int _patchSize = 48); 47 | ~LDB(); 48 | 49 | // returns the descriptor size in bytes 50 | int descriptorSize() const; 51 | // returns the descriptor type 52 | int descriptorType() const; 53 | 54 | // Compute the LDB features and descriptors on an image 55 | void compute( const cv::Mat& image, std::vector& keypoints, cv::Mat& descriptors ) const; 56 | 57 | protected: 58 | 59 | int nfeatures; 60 | double scaleFactor; 61 | int nlevels; 62 | int firstLevel; 63 | int patchSize; 64 | 65 | }; 66 | 67 | typedef LDB LdbDescriptorExtractor; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /include/htmap/lc/LoopCloser.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _LOOPCLOSER_H_ 21 | #define _LOOPCLOSER_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | #include "htmap/bayes/BayesFilter.h" 29 | #include "htmap/map/HighLevelMap.h" 30 | #include "htmap/util/Image.h" 31 | #include "htmap/util/Params.h" 32 | 33 | namespace htmap 34 | { 35 | 36 | class LoopCloser 37 | { 38 | public: 39 | LoopCloser(); 40 | ~LoopCloser(); 41 | 42 | bool process(const Image& image, HighLevelMap& hmap, unsigned& loop_loc, unsigned& loop_img); 43 | 44 | private: 45 | BayesFilter _filter; 46 | Params* _params; 47 | Statistics* _st; 48 | std::queue _buffer; 49 | std::vector _prior; 50 | 51 | void computeLikelihood(const Image& image, HighLevelMap& hmap, std::map& lik, double& llc_time, double& ilc_time); 52 | }; 53 | 54 | } 55 | 56 | #endif /* _LOOPCLOSER_H_ */ 57 | -------------------------------------------------------------------------------- /include/htmap/map/HighLevelMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _HLEVELMAP_H_ 21 | #define _HLEVELMAP_H_ 22 | 23 | #include 24 | 25 | #include "htmap/map/Location.h" 26 | 27 | namespace htmap 28 | { 29 | 30 | const double max_weight = std::numeric_limits::infinity(); 31 | 32 | struct neighbor 33 | { 34 | int target; 35 | double weight; 36 | neighbor(int arg_target, double arg_weight) : 37 | target(arg_target), weight(arg_weight) 38 | {} 39 | }; 40 | 41 | struct Graph 42 | { 43 | std::vector > adjlist; 44 | 45 | void addNode() 46 | { 47 | std::vector node; 48 | adjlist.push_back(node); 49 | } 50 | }; 51 | 52 | class HighLevelMap 53 | { 54 | public: 55 | HighLevelMap(); 56 | 57 | unsigned addLocation(Location* loc); 58 | void addImageToLocation(const unsigned idloc, const int id, const std::string image_filename); 59 | void setActive(const unsigned& act); 60 | void linkLocations(const unsigned& a, const unsigned& b, const double& weight); 61 | unsigned numLocations(); 62 | Location* getActiveLocation(); 63 | 64 | std::vector locations; 65 | unsigned active; 66 | Graph graph; 67 | std::map im2loc; 68 | }; 69 | 70 | } 71 | 72 | #endif /* _HLEVELMAP_H_ */ 73 | -------------------------------------------------------------------------------- /include/htmap/map/Location.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _LOCATION_H_ 21 | #define _LOCATION_H_ 22 | 23 | #include 24 | 25 | #include "htmap/imgdesc/GlobalDescriptor.h" 26 | #include "htmap/util/Image.h" 27 | #include "htmap/util/Params.h" 28 | #include "htmap/util/Util.h" 29 | #include "obindex/BinaryIndex.h" 30 | 31 | namespace vi = obindex; 32 | 33 | namespace htmap 34 | { 35 | 36 | struct LocationEntry 37 | { 38 | LocationEntry(const unsigned& id, const std::string& image_filename) : 39 | filename(image_filename), 40 | image_id(id) 41 | {} 42 | 43 | std::string filename; 44 | unsigned image_id; 45 | }; 46 | 47 | class Location 48 | { 49 | public: 50 | Location(const int& max_images, const int& feat_per_img, const int& desc_bytes); 51 | ~Location(); 52 | 53 | void addImage(const int& id, const std::string& image_filename); 54 | void searchImages(const cv::Mat& dscs, std::map& image_matches, double match_ratio); 55 | unsigned numImages(); 56 | 57 | std::vector images; 58 | 59 | // Global included in the image. 60 | cv::Mat gdescriptors; 61 | 62 | // Representative of the location. 63 | cv::Mat_ desc; 64 | 65 | // Inverse Covariance Matrix. 66 | cv::Mat_ invcov; 67 | 68 | // True if the location has been initialized. 69 | bool init; 70 | 71 | // Location identifier inside a map. It should be assigned externally. 72 | unsigned id; 73 | 74 | // Binary index that stores the features of the images inside this node. 75 | vi::BinaryIndex* _bindex; 76 | 77 | private: 78 | void initializeLocation(const int& id, const std::string& image_filename); 79 | void updateGlobalDescription(); 80 | 81 | // Class for accessing parameters. 82 | Params* _params; 83 | }; 84 | 85 | } 86 | 87 | #endif /* _LOCATION_H_ */ 88 | -------------------------------------------------------------------------------- /include/htmap/util/Image.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef IMAGEHAMAP_H 21 | #define IMAGEHAMAP_H 22 | 23 | #include 24 | 25 | namespace htmap 26 | { 27 | 28 | struct Image 29 | { 30 | unsigned image_id; 31 | std::string image_filename; 32 | cv::Mat image; 33 | std::vector kps; 34 | cv::Mat dscs; 35 | cv::Mat gdsc; 36 | 37 | void save(const std::string& file) 38 | { 39 | cv::FileStorage fs(file, cv::FileStorage::WRITE); 40 | fs << "image_id" << static_cast(image_id); 41 | fs << "image_filename" << image_filename; 42 | cv::write(fs, "kps", kps); 43 | fs << "dscs" << dscs; 44 | fs << "gdsc" << gdsc; 45 | fs.release(); 46 | } 47 | 48 | void load(const std::string& file) 49 | { 50 | cv::FileStorage fs(file, cv::FileStorage::READ); 51 | image_id = static_cast((int)fs["image_id"]); 52 | image_filename = (std::string)fs["image_filename"]; 53 | image = cv::imread(image_filename); 54 | kps.clear(); 55 | cv::read(fs["kps"], kps); 56 | fs["dscs"] >> dscs; 57 | fs["gdsc"] >> gdsc; 58 | fs.release(); 59 | } 60 | 61 | static void loadGlobalDesc(const std::string& file, cv::Mat& desc) 62 | { 63 | cv::FileStorage fs(file, cv::FileStorage::READ); 64 | fs["gdsc"] >> desc; 65 | fs.release(); 66 | } 67 | 68 | static void loadKeypointDesc(const std::string& file, std::vector& kps, cv::Mat& desc) 69 | { 70 | cv::FileStorage fs(file, cv::FileStorage::READ); 71 | kps.clear(); 72 | cv::read(fs["kps"], kps); 73 | fs["dscs"] >> desc; 74 | fs.release(); 75 | } 76 | 77 | static void loadAllDesc(const std::string& file, cv::Mat& gdesc, std::vector& kps, cv::Mat& desc) 78 | { 79 | cv::FileStorage fs(file, cv::FileStorage::READ); 80 | fs["gdsc"] >> gdesc; 81 | kps.clear(); 82 | cv::read(fs["kps"], kps); 83 | fs["dscs"] >> desc; 84 | fs.release(); 85 | } 86 | 87 | static void loadImage(const std::string& file, cv::Mat& img) 88 | { 89 | cv::FileStorage fs(file, cv::FileStorage::READ); 90 | std::string image_filename = (std::string)fs["image_filename"]; 91 | img = cv::imread(image_filename); 92 | fs.release(); 93 | } 94 | }; 95 | 96 | } 97 | 98 | #endif // IMAGEHAMAP_H 99 | -------------------------------------------------------------------------------- /include/htmap/util/Params.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _PARAMS_H 21 | #define _PARAMS_H 22 | 23 | #include 24 | 25 | #include "htmap/imgdesc/GlobalDescriptor.h" 26 | #include "htmap/imgdesc/KeypointDescriptor.h" 27 | #include "htmap/imgdesc/KeypointDetector.h" 28 | #include "htmap/util/Statistics.h" 29 | #include "htmap/util/Util.h" 30 | 31 | namespace htmap 32 | { 33 | 34 | class Params 35 | { 36 | public: 37 | // Global Parameters 38 | std::string dir_images; 39 | bool load_features; 40 | std::string dir_results; 41 | std::vector filenames; 42 | std::vector images; 43 | unsigned nimages; 44 | int match_method; 45 | double match_ratio; 46 | double locLC_thresh; 47 | int imageLC_min_inliers; 48 | int imageLC_disc_recent; 49 | double imageLC_tloop; 50 | int loc_max_images; 51 | int max_total_kps; 52 | double max_sim_newnode; 53 | 54 | // Batch parameters 55 | bool batch; 56 | int inliers_begin; 57 | int inliers_end; 58 | int inliers_step; 59 | 60 | // Keypoint Detector parameters 61 | std::string detector_name; 62 | KeypointDetector* detector; 63 | KeypointDetectorParams det_params; 64 | 65 | // Keypoint Descriptor parameters 66 | std::string descriptor_name; 67 | KeypointDescriptor* descriptor; 68 | KeypointDescriptorParams des_params; 69 | 70 | // Global Descriptor parameters 71 | std::string gdescriptor_name; 72 | GlobalDescriptor* gdescriptor; 73 | GlobalDescriptorParams gdes_params; 74 | 75 | // Public functions. 76 | static Params* getInstance(); 77 | void readParams(const ros::NodeHandle& nh); 78 | 79 | protected: 80 | // Protected constructor. Singleton class. 81 | Params() : 82 | nimages(0), 83 | detector(0), 84 | descriptor(0), 85 | gdescriptor(0), 86 | load_features(false), 87 | match_method(1), 88 | match_ratio(0.8), 89 | locLC_thresh(0.6), 90 | imageLC_min_inliers(50), 91 | imageLC_disc_recent(70), 92 | imageLC_tloop(0.75), 93 | loc_max_images(300), 94 | max_total_kps(4000), 95 | max_sim_newnode(0.20), 96 | batch(false), 97 | inliers_begin(20), 98 | inliers_end(200), 99 | inliers_step(15) 100 | { 101 | } 102 | 103 | ~Params() 104 | { 105 | delete detector; 106 | delete descriptor; 107 | delete gdescriptor; 108 | delete _instance; 109 | } 110 | 111 | Params(const Params &); 112 | Params& operator=(const Params &); 113 | 114 | private: 115 | // Single instance. 116 | static Params* _instance; 117 | }; 118 | 119 | } 120 | #endif /* _PARAMS_H */ 121 | -------------------------------------------------------------------------------- /include/htmap/util/Statistics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef _STATISTICS_H 21 | #define _STATISTICS_H 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #define SSTR( x ) dynamic_cast< std::ostringstream & >( \ 33 | ( std::ostringstream() << std::dec << x ) ).str() 34 | 35 | namespace htmap 36 | { 37 | 38 | class Statistics 39 | { 40 | public: 41 | // Public members. 42 | cv::Mat_ loops; 43 | cv::Mat_ prior; 44 | cv::Mat_ likelihood; 45 | cv::Mat_ posterior; 46 | std::vector locations; 47 | std::vector > desc_times; 48 | std::vector > lc_times; 49 | 50 | // Public functions. 51 | static Statistics* getInstance(); 52 | void init(const int nimages); 53 | void registerLoop(const int a, const int b); 54 | void registerPrior(const int imid, const std::vector& _prior); 55 | void registerLikelihood(const int imid, const std::vector& _lik); 56 | void registerPosterior(const int imid, const std::vector& _post); 57 | void registerImageToLocation(const int imid, const unsigned loc); 58 | void registerDescTimes(double gdesc, double det, double desc); 59 | void registerLCTimes(std::vector& values); 60 | void writeResults(const std::string& dir, int inliers); 61 | 62 | protected: 63 | // Protected constructor. Singleton class. 64 | Statistics() 65 | { 66 | } 67 | 68 | ~Statistics() 69 | { delete _instance; } 70 | 71 | Statistics(const Statistics &); 72 | Statistics& operator=(const Statistics &); 73 | 74 | private: 75 | // Single instance. 76 | static Statistics* _instance; 77 | 78 | // Mutex for description times 79 | boost::mutex mlock; 80 | }; 81 | 82 | } 83 | 84 | #endif /* _STATISTICS_H */ 85 | -------------------------------------------------------------------------------- /include/htmap/util/Util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #ifndef UTIL_HAMAP_H 21 | #define UTIL_HAMAP_H 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "htmap/util/Image.h" 31 | 32 | namespace htmap 33 | { 34 | void ratioMatching(const Image& query, const Image& train, const double& ratio, std::vector& matches); 35 | int checkEpipolarGeometry(const Image& query, const Image& train, const std::vector& matches); 36 | double distEuclidean(const cv::Mat& a, const cv::Mat& b); 37 | double distMahalanobis(const cv::Mat& a, const cv::Mat& b, const cv::Mat& icovar); 38 | int distHamming(const uchar* a, const uchar* b, const int n); 39 | double distChiSquare(const cv::Mat& a, const cv::Mat& b); 40 | double distBhattacharyya(const cv::Mat& a, const cv::Mat& b); 41 | void getFilenames(const std::string& directory, std::vector& filenames, bool images = true); 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /launch/htmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /matlab/PR.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function [precision, recall, results] = PR(loop_file, gt_file, load_file, gt_neigh) 20 | % Given a result loop matrix and a ground truth matrix, this function 21 | % computes the Precision/Recall values. 22 | 23 | if nargin < 4 24 | gt_neigh = 10; 25 | end 26 | 27 | % Loading loops and ground truth files. 28 | if load_file == 1 29 | loops = load_loops_data(loop_file); 30 | else 31 | loops = loop_file; 32 | end 33 | gtruth = load(gt_file); 34 | 35 | % Defining general counters. 36 | TP = 0; % True Positives. 37 | FP = 0; % False Positives. 38 | TN = 0; % True Negatives. 39 | FN = 0; % False Negatives. 40 | 41 | loop_size = size(loops); 42 | gt_size = size(gtruth.truth); 43 | for i=1:loop_size(1) 44 | ones_index = find(loops(i, :)); 45 | loop_closed = numel(ones_index) > 0; % Is a loop detected ? 46 | loop_candidate = 0; % Which is the loop candidate ? 47 | gt_loop_closed = 0; % Is there a loop in the range indicated range according to the GT file ?. 48 | gt_nloops = numel(find(gtruth.truth(i, :))); % Number of loops in the whole GT row. 49 | if loop_closed 50 | loop_candidate = ones_index(1); 51 | % Selecting the range to search in the ground truth file. 52 | ind1 = loop_candidate - gt_neigh; 53 | if ind1 < 1 54 | ind1 = 1; 55 | end 56 | ind2 = loop_candidate + (gt_neigh + 1); 57 | if ind2 > gt_size(2) 58 | ind2 = gt_size(2); 59 | end 60 | gt_value = gtruth.truth(i, ind1:ind2); % Ground truth range around to the loop closure candidate. 61 | gt_loop_closed = numel(find(gt_value)) > 0; 62 | end 63 | 64 | % Taking a decision about this image. 65 | if loop_closed && gt_loop_closed 66 | TP = TP + 1; 67 | results(i) = 0; 68 | elseif loop_closed && (gt_nloops == 0 || ~gt_loop_closed) 69 | FP = FP + 1; 70 | results(i) = 1; 71 | elseif ~loop_closed && gt_nloops == 0 72 | TN = TN + 1; 73 | results(i) = 2; 74 | elseif ~loop_closed && gt_nloops > 0 75 | FN = FN + 1; 76 | results(i) = 3; 77 | end 78 | 79 | % Printing results to the command line. 80 | % if loop_closed 81 | % disp(['Image: ', int2str(i), ' Loop with: ', int2str(loop_candidate)]); 82 | % else 83 | % disp(['Image: ', int2str(i), ' No Loop detected']); 84 | % end 85 | end 86 | 87 | % Printing final results. 88 | disp(['TP: ', int2str(TP)]); 89 | disp(['FP: ', int2str(FP)]); 90 | disp(['TN: ', int2str(TN)]); 91 | disp(['FN: ', int2str(FN)]); 92 | disp(['-----']); 93 | 94 | % Computing the Precision/Recall final values. 95 | precision = TP / (TP + FP); 96 | recall = TP / (TP + FN); 97 | 98 | disp(['Precision: ', num2str(precision)]); 99 | disp(['Recall: ', num2str(recall)]); 100 | 101 | disp(['-----']); 102 | end -------------------------------------------------------------------------------- /matlab/PR_curve.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function PR_curve(directory, gt_file, gt_neigh) 20 | files = get_files(directory, '.txt'); 21 | Ps = [1.0]; 22 | Rs = [0.0]; 23 | max_pr = 0.0; 24 | max_re = 0.0; 25 | max_file = ''; 26 | % PRE = []; 27 | % REC = []; 28 | TEP = {'None'}; 29 | for i=1:length(files) 30 | file = char(files(i)); 31 | [P, R] = PR(file, gt_file, 1, gt_neigh); 32 | Ps = [Ps, P]; 33 | Rs = [Rs, R]; 34 | 35 | % PRE = [PRE, P]; 36 | % REC = [REC, R]; 37 | TEP{i + 1} = file; 38 | 39 | if P > max_pr || (P == max_pr && R > max_re) 40 | max_pr = P; 41 | max_re = R; 42 | max_file = file; 43 | end 44 | end 45 | 46 | [Rs, I] = sort(Rs); 47 | Ps_a = Ps; 48 | T_a = TEP; 49 | for i=1:numel(I) 50 | Ps_a(i) = Ps(I(i)); 51 | T_a{i} = TEP{I(i)}; 52 | end 53 | Ps = Ps_a; 54 | TEP = T_a; 55 | 56 | disp(['Final results']); 57 | 58 | for i=1:length(files) 59 | fprintf('Pr %f, Re %f, File: %s\n', Ps(i), Rs(i), TEP{i}); 60 | end 61 | 62 | % disp('--'); 63 | % for i=1:length(PRE) 64 | % fprintf('Pr: %f, Re: %f, file: %s\n', PRE(i), REC(i), TEP{i}); 65 | % %disp(['Pr: ',num2str(PRE(i)),', Re: ',num2str(REC(i)),' T: ',TEP{i}]); 66 | % end 67 | % disp('--'); 68 | 69 | fprintf('MAX Pr with MAX Recall: Pr %f, Re %f, File: %s\n', max_pr, max_re, max_file); 70 | 71 | lab_size = 26; 72 | ax_size = 20; 73 | leg_size = 20; 74 | line_size = 3.0; 75 | figure(); 76 | hold on; 77 | set(gca, 'FontSize', ax_size); 78 | xlabel('Recall', 'FontSize', lab_size); 79 | ylabel('Precision', 'FontSize', lab_size); 80 | xlim([0., 1.02]); 81 | ylim([0., 1.02]); 82 | grid('off'); 83 | plot(Rs, Ps, 'r'); 84 | hold off 85 | end -------------------------------------------------------------------------------- /matlab/PR_curves.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function pr_curves 20 | % Function to plot PR curves. 21 | lab_size = 26; 22 | ax_size = 20; 23 | leg_size = 20; 24 | line_size = 3.0; 25 | 26 | % FABMAP PR/RE 27 | FABMAP.CITYCENTRE.pr = [1.0000, 1.00, 0.9733, 0.9582, 0.9416, 0.9304, 0.9248, 0.9144, 0.9002, 0.8889, 0.8778, 0.8668, 0.8532, 0.8431, 0.8232, 0.8028, 0.7902, 0.7668, 0.7421, 0.7072, 0.6672]; 28 | FABMAP.CITYCENTRE.re = [0.0000, 0.385, 0.5868, 0.6154, 0.6373, 0.6516, 0.6661, 0.6775, 0.6903, 0.6995, 0.7234, 0.7271, 0.7399, 0.7435, 0.7514, 0.7579, 0.7813, 0.7958, 0.8057, 0.8221, 0.8389]; 29 | FABMAP.NEWCOLLEGE.pr = [1.0000, 1.00, 1.000, 0.8907, 0.8555, 0.8436, 0.8179, 0.8030, 0.7780, 0.7540, 0.7177, 0.6992, 0.6726, 0.6484, 0.6232, 0.6088, 0.5829, 0.5650, 0.5396, 0.5251, 0.5078, 0.4871]; 30 | FABMAP.NEWCOLLEGE.re = [0.0000, 0.48, 0.519, 0.6707, 0.7022, 0.7312, 0.7506, 0.7835, 0.7971, 0.8093, 0.8182, 0.8300, 0.8399, 0.8448, 0.8473, 0.8568, 0.8614, 0.8756, 0.8850, 0.8875, 0.8900, 0.9023]; 31 | FABMAP.KITTI00.pr = [1.0000, 1.0000, 0.9953, 0.9955, 0.9958, 0.9959, 0.9960, 0.9960, 0.9961, 0.9961, 0.9961, 0.9962, 0.9962, 0.9943, 0.9944, 0.9944, 0.9944, 0.9944, 0.9945, 0.9945, 0.9945, 0.9946, 0.9928, 0.9928, 0.9928, 0.9928, 0.9928, 0.9929, 0.9929, 0.9912, 0.9912, 0.9913, 0.9896, 0.9896, 0.9862, 0.9845, 0.9812, 0.9747, 0.9698, 0.9572, 0.9345, 0.8345]; 32 | FABMAP.KITTI00.re = [0.0000, 0.4921, 0.5330, 0.5635, 0.5990, 0.6168, 0.6282, 0.6383, 0.6447, 0.6510, 0.6548, 0.6624, 0.6662, 0.6696, 0.6722, 0.6773, 0.6811, 0.6823, 0.6861, 0.6900, 0.6912, 0.6963, 0.7001, 0.7027, 0.7027, 0.7027, 0.7052, 0.7078, 0.7103, 0.7141, 0.7154, 0.7230, 0.7243, 0.7243, 0.7277, 0.7290, 0.7299, 0.7350, 0.7376, 0.7401, 0.7462, 0.7589]; 33 | FABMAP.KITTI05.pr = [1.0000, 1.0000, 0.9939, 0.9940, 0.9941, 0.9945, 0.9946, 0.9947, 0.9947, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9949, 0.9949, 0.9949, 0.9949, 0.9950, 0.9950, 0.9951, 0.9952, 0.9952, 0.9952, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9955, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9956, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9957, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9958, 0.9917, 0.9917, 0.9958, 0.9958, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9917, 0.9877, 0.9877, 0.9877, 0.9877, 0.9877, 0.9877, 0.9877, 0.9877, 0.9877, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9918, 0.9878, 0.9878, 0.9878, 0.9878, 0.9878, 0.9878, 0.9878, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9879, 0.9880, 0.9880, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9841, 0.9880, 0.9880, 0.9880, 0.9802, 0.9802, 0.9802, 0.9802, 0.9802, 0.9802, 0.9802, 0.9841, 0.9841, 0.9841, 0.9841, 0.9727, 0.9727, 0.9765, 0.9765, 0.9765, 0.9765, 0.9765, 0.9765, 0.9765, 0.9765, 0.9765, 0.9803, 0.9803, 0.9803, 0.9803, 0.9803, 0.9803, 0.9803, 0.9728, 0.9728, 0.9728, 0.9728, 0.9691, 0.9691, 0.9691, 0.9691, 0.9691, 0.9729, 0.9582, 0.9582, 0.9618, 0.9618, 0.9618, 0.9618, 0.9655, 0.9655, 0.9655, 0.9655, 0.9692, 0.9476, 0.9476, 0.9511, 0.9511, 0.9547, 0.9547, 0.9547, 0.9583, 0.9373, 0.9407, 0.9407, 0.9442, 0.9173, 0.9206, 0.9239, 0.9273, 0.9307, 0.9341, 0.8767, 0.8858, 0.8951, 0.9014, 0.9078, 0.9110, 0.8658, 0.8196, 0.8382, 0.8548, 0.7222, 0.7647, 0.7975]; 34 | FABMAP.KITTI05.re = [0.0000, 0.3215, 0.3410, 0.3430, 0.3514, 0.3784, 0.3825, 0.3909, 0.3929, 0.3971, 0.3992, 0.3992, 0.3992, 0.3992, 0.3992, 0.3992, 0.4012, 0.4012, 0.4033, 0.4033, 0.4054, 0.4096, 0.4116, 0.4179, 0.4200, 0.4304, 0.4345, 0.4345, 0.4366, 0.4366, 0.4387, 0.4387, 0.4387, 0.4387, 0.4387, 0.4407, 0.4407, 0.4407, 0.4407, 0.4407, 0.4407, 0.4428, 0.4449, 0.4470, 0.4470, 0.4491, 0.4491, 0.4491, 0.4491, 0.4491, 0.4491, 0.4491, 0.4491, 0.4491, 0.4511, 0.4511, 0.4532, 0.4532, 0.4532, 0.4532, 0.4553, 0.4574, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4595, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4615, 0.4636, 0.4657, 0.4657, 0.4657, 0.4657, 0.4657, 0.4657, 0.4657, 0.4657, 0.4678, 0.4678, 0.4699, 0.4699, 0.4699, 0.4699, 0.4719, 0.4719, 0.4719, 0.4740, 0.4740, 0.4740, 0.4740, 0.4740, 0.4740, 0.4740, 0.4761, 0.4761, 0.4782, 0.4782, 0.4782, 0.4782, 0.4782, 0.4782, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4802, 0.4823, 0.4823, 0.4823, 0.4844, 0.4844, 0.4844, 0.4844, 0.4865, 0.4865, 0.4865, 0.4865, 0.4865, 0.4865, 0.4865, 0.4865, 0.4865, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4886, 0.4906, 0.4906, 0.4906, 0.4906, 0.4906, 0.4906, 0.4927, 0.4948, 0.4948, 0.4948, 0.4948, 0.4948, 0.4948, 0.4948, 0.4948, 0.4969, 0.4969, 0.4969, 0.4969, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.4990, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5010, 0.5031, 0.5031, 0.5031, 0.5031, 0.5052, 0.5052, 0.5052, 0.5073, 0.5073, 0.5073, 0.5073, 0.5073, 0.5094, 0.5094, 0.5094, 0.5094, 0.5094, 0.5094, 0.5094, 0.5114, 0.5114, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5135, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5156, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5177, 0.5198, 0.5198, 0.5198, 0.5198, 0.5218, 0.5218, 0.5218, 0.5218, 0.5218, 0.5218, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5239, 0.5260, 0.5260, 0.5260, 0.5260, 0.5260, 0.5260, 0.5260, 0.5260, 0.5281, 0.5281, 0.5281, 0.5281, 0.5301, 0.5301, 0.5301, 0.5301, 0.5301, 0.5301, 0.5322, 0.5322, 0.5322, 0.5322, 0.5322, 0.5322, 0.5364, 0.5385, 0.5385, 0.5385, 0.5405, 0.5405, 0.5405]; 35 | FABMAP.KITTI06.pr = [1.0000, 1.0000, 0.9935, 0.9936, 0.9937, 0.9939, 0.9939, 0.9940, 0.9941, 0.9941, 0.9941, 0.9941, 0.9942, 0.9942, 0.9942, 0.9943, 0.9943, 0.9943, 0.9944, 0.9944, 0.9945, 0.9946, 0.9946, 0.9947, 0.9947, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9948, 0.9949, 0.9949, 0.9949, 0.9949, 0.9949, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9950, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9951, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9952, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9953, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9954, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9954, 0.9954, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9864, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9909, 0.9865, 0.9865, 0.9865, 0.9865, 0.9865, 0.9822, 0.9822, 0.9822, 0.9866, 0.9866, 0.9866, 0.9866, 0.9866, 0.9866, 0.9866, 0.9866, 0.9866, 0.9652, 0.9652, 0.9652, 0.9694, 0.9694, 0.9694, 0.9694, 0.9694, 0.9694, 0.9694, 0.9737, 0.9737, 0.9737, 0.9737, 0.9737, 0.9737, 0.9737, 0.9737, 0.9737, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9780, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9823, 0.9612, 0.9612, 0.9612, 0.9612, 0.9654, 0.9654, 0.9654, 0.9295, 0.9333, 0.9333, 0.9372, 0.9412, 0.9412, 0.9451, 0.9451, 0.9492, 0.9492, 0.9492, 0.9532, 0.9532, 0.9532, 0.9573, 0.9573, 0.9573, 0.9573, 0.9146, 0.9184, 0.9221, 0.9259, 0.8794, 0.8863, 0.8933, 0.8968, 0.9040, 0.9113, 0.7965, 0.8165, 0.8346, 0.8470, 0.8598, 0.8697, 0.7729]; 36 | FABMAP.KITTI06.re = [0.0000, 0.5534, 0.5651, 0.5799, 0.5874, 0.6059, 0.6097, 0.6208, 0.6245, 0.6245, 0.6245, 0.6283, 0.6320, 0.6320, 0.6357, 0.6431, 0.6431, 0.6506, 0.6543, 0.6580, 0.6766, 0.6840, 0.6877, 0.6914, 0.6989, 0.7063, 0.7063, 0.7063, 0.7063, 0.7063, 0.7063, 0.7063, 0.7063, 0.7212, 0.7212, 0.7212, 0.7212, 0.7249, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7361, 0.7398, 0.7398, 0.7398, 0.7435, 0.7435, 0.7472, 0.7472, 0.7472, 0.7509, 0.7509, 0.7509, 0.7509, 0.7509, 0.7509, 0.7509, 0.7509, 0.7509, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7546, 0.7584, 0.7584, 0.7584, 0.7584, 0.7584, 0.7584, 0.7621, 0.7621, 0.7621, 0.7621, 0.7658, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7695, 0.7732, 0.7732, 0.7770, 0.7770, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7844, 0.7881, 0.7881, 0.7881, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7918, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7955, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.7993, 0.8030, 0.8030, 0.8030, 0.8030, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8067, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8104, 0.8141, 0.8141, 0.8141, 0.8178, 0.8178, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8216, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8253, 0.8290, 0.8290, 0.8290, 0.8290, 0.8290, 0.8290, 0.8290, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8327, 0.8364, 0.8364, 0.8364, 0.8364, 0.8401, 0.8401, 0.8401, 0.8401, 0.8401, 0.8401, 0.8439, 0.8439, 0.8439, 0.8439, 0.8439, 0.8439, 0.8476]; 37 | FABMAP.STALUCIA.pr = [1.0000, 1.0000, 0.9984, 0.9988, 0.9989, 0.9990, 0.9990, 0.9991, 0.9992, 0.9992, 0.9992, 0.9993, 0.9979, 0.9973, 0.9955, 0.9912, 0.9903, 0.9871, 0.9824, 0.9737, 0.9568, 0.9098]; 38 | FABMAP.STALUCIA.re = [0.0000, 0.1854, 0.1943, 0.2510, 0.2818, 0.3040, 0.3231, 0.3462, 0.3683, 0.3865, 0.4047, 0.4232, 0.4426, 0.4607, 0.4758, 0.4885, 0.5026, 0.5177, 0.5325, 0.5476, 0.5731, 0.6221]; 39 | 40 | % SeqSLAM PR/RE 41 | SEQSLAM.CITYCENTRE.pr = [1.0, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9929, 0.9887, 0.9325, 0.7294]; 42 | SEQSLAM.CITYCENTRE.re = [0.0, 0.0428, 0.0873, 0.1515, 0.2353, 0.2763, 0.3333, 0.3904, 0.4456, 0.5312, 0.5847, 0.6417, 0.6898, 0.7522, 0.7772, 0.7926, 0.8261]; 43 | SEQSLAM.NEWCOLLEGE.pr = [1.0, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9868, 0.9510, 0.8114, 0.4776]; 44 | SEQSLAM.NEWCOLLEGE.re = [0.0, 0.0024, 0.0605, 0.0896, 0.1380, 0.1816, 0.2179, 0.2857, 0.3705, 0.4407, 0.4939, 0.5463, 0.5711, 0.6101, 0.6829]; 45 | SEQSLAM.KITTI00.pr = [1.0, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9947, 0.9814, 0.9047, 0.7018, 0.3288]; 46 | SEQSLAM.KITTI00.re = [0.0, 0.0089, 0.0456, 0.1065, 0.1965, 0.2687, 0.3561, 0.4563, 0.5387, 0.6210, 0.6705, 0.7116, 0.7357, 0.7598, 0.8021, 0.8778]; 47 | SEQSLAM.KITTI05.pr = [1.0, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9864, 0.9453, 0.8969, 0.6732, 0.2767]; 48 | SEQSLAM.KITTI05.re = [0.0, 0.0229, 0.0707, 0.1497, 0.2328, 0.2661, 0.3181, 0.3493, 0.3929, 0.4137, 0.4532, 0.5031, 0.5992, 0.6645, 0.7545]; 49 | SEQSLAM.KITTI06.pr = [1.0, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9841, 0.8767, 0.6797, 0.5204]; 50 | SEQSLAM.KITTI06.re = [0.0, 0.0446, 0.1970, 0.2714, 0.3643, 0.4610, 0.5242, 0.5836, 0.6468, 0.6914, 0.7538, 0.8125, 0.8821]; 51 | SEQSLAM.STALUCIA.pr = [1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9956, 0.9268, 0.6152]; 52 | SEQSLAM.STALUCIA.re = [0.0018, 0.0051, 0.0100, 0.0205, 0.0466, 0.0857, 0.1313, 0.1832, 0.2321, 0.2792, 0.3206, 0.3687, 0.4156, 0.4690, 0.5329, 0.6269]; 53 | 54 | HAMAP.NEWCOLLEGE.pr = [1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9862, 0.9824, 0.9750, 0.9352, 0.8828]; 55 | HAMAP.NEWCOLLEGE.re = [0.0000, 0.5351, 0.6150, 0.6344, 0.6780, 0.7361, 0.8107, 0.8111, 0.8499, 0.8741, 0.9089]; 56 | HAMAP.KITTI05.pr = [1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9973, 0.9920, 0.9841, 0.9718, 0.9554, 0.9112, 0.8817]; 57 | HAMAP.KITTI05.re = [0.0000, 0.6819, 0.7152, 0.7235, 0.7588, 0.7651, 0.7729, 0.7750, 0.7929, 0.8075, 0.8159, 0.8264]; 58 | HAMAP.KITTI06.pr = [1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9971, 0.9849, 0.9762, 0.9628]; 59 | HAMAP.KITTI06.re = [0.0000, 0.8587, 0.9219, 0.9480, 0.9480, 0.9703, 0.9734, 0.9812, 0.9888, 0.9923]; 60 | HAMAP.KITTI00.pr = [1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 1.0000, 0.9986, 0.9959, 0.9907, 0.9580]; 61 | HAMAP.KITTI00.re = [0.0000, 0.8542, 0.8771, 0.8872, 0.8885, 0.9024, 0.9124, 0.9364, 0.9501, 0.9604]; 62 | HAMAP.CITYCENTRE.pr = [1.00, 1.0000, 0.998, 0.964, 0.901, 0.864]; 63 | HAMAP.CITYCENTRE.re = [0.00, 0.7968, 0.821, 0.861, 0.903, 0.934]; 64 | HAMAP.STALUCIA.pr = [1.0000, 1.0000, 1.0000, 1.0000, 0.9999, 0.9997, 0.9992, 0.9957, 0.97, 0.95, 0.93]; 65 | HAMAP.STALUCIA.re = [0.0000, 0.6526, 0.6673, 0.7011, 0.7180, 0.7639, 0.7773, 0.7799, 0.82, 0.85, 0.87]; 66 | 67 | % CityCentre results 68 | figure(); 69 | hold on; 70 | plot_common(); 71 | plot_PR(FABMAP.CITYCENTRE.pr, FABMAP.CITYCENTRE.re, 'r'); 72 | plot_PR(SEQSLAM.CITYCENTRE.pr, SEQSLAM.CITYCENTRE.re, 'b'); 73 | plot_PR(HAMAP.CITYCENTRE.pr, HAMAP.CITYCENTRE.re, 'k'); 74 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 75 | set(l, 'FontSize', leg_size); 76 | title('City Center'); 77 | 78 | %print -djpeg -r300 curvepr_cc 79 | hold off; 80 | 81 | % NewCollege results 82 | figure(); 83 | hold on; 84 | plot_common(); 85 | plot_PR(FABMAP.NEWCOLLEGE.pr, FABMAP.NEWCOLLEGE.re, 'r'); 86 | plot_PR(SEQSLAM.NEWCOLLEGE.pr, SEQSLAM.NEWCOLLEGE.re, 'b'); 87 | plot_PR(HAMAP.NEWCOLLEGE.pr, HAMAP.NEWCOLLEGE.re, 'k'); 88 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 89 | set(l, 'FontSize', leg_size); 90 | title('New College'); 91 | 92 | %print -djpeg -r300 curvepr_nc 93 | hold off; 94 | 95 | % KITTI00 results 96 | figure(); 97 | hold on; 98 | plot_common(); 99 | plot_PR(FABMAP.KITTI00.pr, FABMAP.KITTI00.re, 'r'); 100 | plot_PR(SEQSLAM.KITTI00.pr, SEQSLAM.KITTI00.re, 'b'); 101 | plot_PR(HAMAP.KITTI00.pr, HAMAP.KITTI00.re, 'k'); 102 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 103 | set(l, 'FontSize', leg_size); 104 | title('KITTI 00'); 105 | 106 | %print -djpeg -r300 curvepr_kitti00 107 | hold off; 108 | 109 | % KITTI05 results 110 | figure(); 111 | hold on; 112 | plot_common(); 113 | plot_PR(FABMAP.KITTI05.pr, FABMAP.KITTI05.re, 'r'); 114 | plot_PR(SEQSLAM.KITTI05.pr, SEQSLAM.KITTI05.re, 'b'); 115 | plot_PR(HAMAP.KITTI05.pr, HAMAP.KITTI05.re, 'k'); 116 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 117 | set(l, 'FontSize', leg_size); 118 | title('KITTI 05'); 119 | 120 | %print -djpeg -r300 curvepr_kitti05 121 | hold off; 122 | 123 | % KITTI06 results 124 | figure(); 125 | hold on; 126 | plot_common(); 127 | plot_PR(FABMAP.KITTI06.pr, FABMAP.KITTI06.re, 'r'); 128 | plot_PR(SEQSLAM.KITTI06.pr, SEQSLAM.KITTI06.re, 'b'); 129 | plot_PR(HAMAP.KITTI06.pr, HAMAP.KITTI06.re, 'k'); 130 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 131 | set(l, 'FontSize', leg_size); 132 | title('KITTI 06'); 133 | 134 | %print -djpeg -r300 curvepr_kitti06 135 | hold off; 136 | 137 | % StaLucia results 138 | figure(); 139 | hold on; 140 | plot_common(); 141 | plot_PR(FABMAP.STALUCIA.pr, FABMAP.STALUCIA.re, 'r'); 142 | plot_PR(SEQSLAM.STALUCIA.pr, SEQSLAM.STALUCIA.re, 'b'); 143 | plot_PR(HAMAP.STALUCIA.pr, HAMAP.STALUCIA.re, 'k'); 144 | l = legend('FABMAPv2', 'SeqSLAM', 'HTMap', 'Location', 'SouthWest'); 145 | set(l, 'FontSize', leg_size); 146 | title('St. Lucia'); 147 | 148 | %print -djpeg -r300 curvepr_stlucia 149 | hold off; 150 | 151 | function plot_common() 152 | set(gca, 'FontSize', ax_size); 153 | xlabel('Recall', 'FontSize', lab_size); 154 | ylabel('Precision', 'FontSize', lab_size); 155 | xlim([0., 1.02]); 156 | ylim([0., 1.02]); 157 | grid('off'); 158 | end 159 | 160 | function plot_PR(pr, re, format) 161 | plot(re, pr, 'Color', format, 'LineWidth', line_size); 162 | end 163 | end 164 | -------------------------------------------------------------------------------- /matlab/computeDistance.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function computeDistance(infile) 20 | poses = load(infile); 21 | poses = poses.p; 22 | [r, c] = size(poses); 23 | total_distance = 0.0; 24 | for i=3:2:r 25 | x = poses(i,1); 26 | y = poses(i,2); 27 | distance = sqrt((x - poses(i - 2, 1))^2 + (y - poses(i - 2, 2))^2); 28 | total_distance = total_distance + distance; 29 | end 30 | 31 | disp(num2str(total_distance)); 32 | end -------------------------------------------------------------------------------- /matlab/computeGPSDist.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function distances = computeGPSDist(lat, long) 20 | [r, c] = size(long); 21 | distances = zeros(r, r); 22 | for i=1:r 23 | disp(['Processing image ', num2str(i)]); 24 | parfor j=1:i 25 | lon1 = long(i,1); 26 | lat1 = lat(i,1); 27 | lon2 = long(j,1); 28 | lat2 = lat(j,1); 29 | [km, nmi, mi] = haversine([lat1 lon1], [lat2 lon2]); 30 | dist_m = km * 1000.0; 31 | distances(i,j) = dist_m; 32 | end 33 | end 34 | end -------------------------------------------------------------------------------- /matlab/computeTotalDistanceGPS.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function computeTotalDistanceGPS(infile) 20 | poses = load(infile); 21 | [r, c] = size(poses.fGPS); 22 | total_distance = 0.0; 23 | for i=2:r 24 | lat1 = poses.fGPS(i - 1, 1); 25 | lon1 = poses.fGPS(i - 1, 2); 26 | lat2 = poses.fGPS(i, 1); 27 | lon2 = poses.fGPS(i, 2); 28 | [km, nmi, mi] = haversine([lat1 lon1], [lat2 lon2]); 29 | dist_m = km * 1000.0; 30 | total_distance = total_distance + dist_m; 31 | end 32 | disp(num2str(total_distance)); 33 | end -------------------------------------------------------------------------------- /matlab/generateGroundTruth.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function loops = generateGroundTruth(dist_matrix, max_dist, remove_prev) 20 | loops = (dist_matrix < max_dist); 21 | [r, c] = size(loops); 22 | for i=1:r 23 | if i > remove_prev 24 | loops(i, i-remove_prev:end) = false; 25 | else 26 | loops(i, :) = false; 27 | end 28 | end 29 | end -------------------------------------------------------------------------------- /matlab/get_files.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function l = get_files(dirName, ext) 20 | % This function lists all files included in dirName and filter them 21 | % according to the ext extension. 22 | 23 | dirData = dir(strcat(dirName, '*loops*.txt')); %# Get the data for the current directory 24 | dirIndex = [dirData.isdir]; %# Find the index for directories 25 | fileList = {dirData(~dirIndex).name}'; %'# Get a list of the files 26 | if ~isempty(fileList) 27 | fileList = cellfun(@(x) fullfile(dirName,x),... %# Prepend path to files 28 | fileList,'UniformOutput',false); 29 | end 30 | 31 | % Filtering files. 32 | l = []; 33 | for i=1:length(fileList) 34 | [path, name, xt] = fileparts(char(fileList(i))); 35 | if strcmp(xt, ext) 36 | l = [l; fileList(i)]; 37 | end 38 | end 39 | end -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_CC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_CC.mat -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_KITTI00.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_KITTI00.mat -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_KITTI05.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_KITTI05.mat -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_KITTI06.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_KITTI06.mat -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_NC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_NC.mat -------------------------------------------------------------------------------- /matlab/gtruths/groundtruth_StLucia.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/emiliofidalgo/htmap/70f364171a2f0260dc0e6e3f99a33b407efc0426/matlab/gtruths/groundtruth_StLucia.mat -------------------------------------------------------------------------------- /matlab/haversine.m: -------------------------------------------------------------------------------- 1 | function [km nmi mi] = haversine(loc1, loc2) 2 | % HAVERSINE Compute distance between locations using Haversine formula 3 | % KM = HAVERSINE(LOC1, LOC2) returns the distance KM in km between 4 | % locations LOC1 and LOC2 using the Haversine formula. LOC1 and LOC2 are 5 | % latitude and longitude coordinates that can be expressed as either 6 | % strings representing degrees, minutes, and seconds (suffixed with 7 | % N/S/E/W), or numeric arrays representing decimal degrees (where 8 | % negative indicates West/South). 9 | % 10 | % [KM, NMI, MI] = HAVERSINE(LOC1, LOC2) returns the computed distance in 11 | % kilometers (KM), nautical miles (NMI), and miles (MI). 12 | % 13 | % Examples 14 | % haversine('53 08 50N, 001 50 58W', '52 12 16N, 000 08 26E') returns 15 | % 170.2547 16 | % haversine([53.1472 -1.8494], '52 12.16N, 000 08.26E') returns 17 | % 170.2508 18 | % haversine([53.1472 -1.8494], [52.2044 0.1406]) returns 170.2563 19 | % 20 | % Inputs 21 | % LOC must be either a string specifying the location in degrees, 22 | % minutes and seconds, or a 2-valued numeric array specifying the 23 | % location in decimal degrees. If providing a string, the latitude 24 | % and longitude must be separated by a comma. 25 | % 26 | % The first element indicates the latitude while the second is the 27 | % longitude. 28 | % 29 | % Notes 30 | % The Haversine formula is used to calculate the great-circle 31 | % distance between two points, which is the shortest distance over 32 | % the earth's surface. 33 | % 34 | % This program was created using equations found on the website 35 | % http://www.movable-type.co.uk/scripts/latlong.html 36 | 37 | % Created by Josiah Renfree 38 | % May 27, 2010 39 | 40 | 41 | %% Check user inputs 42 | 43 | % If two inputs are given, display error 44 | if ~isequal(nargin, 2) 45 | error('User must supply two location inputs') 46 | 47 | % If two inputs are given, handle data 48 | else 49 | 50 | locs = {loc1 loc2}; % Combine inputs to make checking easier 51 | 52 | % Cycle through to check both inputs 53 | for i = 1:length(locs) 54 | 55 | % Check inputs and convert to decimal if needed 56 | if ischar(locs{i}) 57 | 58 | % Parse lat and long info from current input 59 | temp = regexp(locs{i}, ',', 'split'); 60 | lat = temp{1}; lon = temp{2}; 61 | clear temp 62 | locs{i} = []; % Remove string to make room for array 63 | 64 | % Obtain degrees, minutes, seconds, and hemisphere 65 | temp = regexp(lat, '(\d+)\D+(\d+)\D+(\d+)(\w?)', 'tokens'); 66 | temp = temp{1}; 67 | 68 | % Calculate latitude in decimal degrees 69 | locs{i}(1) = str2double(temp{1}) + str2double(temp{2})/60 + ... 70 | str2double(temp{3})/3600; 71 | 72 | % Make sure hemisphere was given 73 | if isempty(temp{4}) 74 | error('No hemisphere given') 75 | 76 | % If latitude is south, make decimal negative 77 | elseif strcmpi(temp{4}, 'S') 78 | locs{i}(1) = -locs{i}(1); 79 | end 80 | 81 | clear temp 82 | 83 | % Obtain degrees, minutes, seconds, and hemisphere 84 | temp = regexp(lon, '(\d+)\D+(\d+)\D+(\d+)(\w?)', 'tokens'); 85 | temp = temp{1}; 86 | 87 | % Calculate longitude in decimal degrees 88 | locs{i}(2) = str2double(temp{1}) + str2double(temp{2})/60 + ... 89 | str2double(temp{3})/3600; 90 | 91 | % Make sure hemisphere was given 92 | if isempty(temp{4}) 93 | error('No hemisphere given') 94 | 95 | % If longitude is west, make decimal negative 96 | elseif strcmpi(temp{4}, 'W') 97 | locs{i}(2) = -locs{i}(2); 98 | end 99 | 100 | clear temp lat lon 101 | end 102 | end 103 | end 104 | 105 | % Check that both cells are a 2-valued array 106 | if any(cellfun(@(x) ~isequal(length(x),2), locs)) 107 | error('Incorrect number of input coordinates') 108 | end 109 | 110 | % Convert all decimal degrees to radians 111 | locs = cellfun(@(x) x .* pi./180, locs, 'UniformOutput', 0); 112 | 113 | 114 | %% Begin calculation 115 | 116 | R = 6371; % Earth's radius in km 117 | delta_lat = locs{2}(1) - locs{1}(1); % difference in latitude 118 | delta_lon = locs{2}(2) - locs{1}(2); % difference in longitude 119 | a = sin(delta_lat/2)^2 + cos(locs{1}(1)) * cos(locs{2}(1)) * ... 120 | sin(delta_lon/2)^2; 121 | c = 2 * atan2(sqrt(a), sqrt(1-a)); 122 | km = R * c; % distance in km 123 | 124 | 125 | %% Convert result to nautical miles and miles 126 | 127 | nmi = km * 0.539956803; % nautical miles 128 | mi = km * 0.621371192; % miles -------------------------------------------------------------------------------- /matlab/load_loops_data.m: -------------------------------------------------------------------------------- 1 | function loops_mat = load_loops_data(loops_filepath) 2 | loops_file = fopen(loops_filepath); 3 | lines = {}; 4 | while true 5 | line = fgetl(loops_file); 6 | if ~ischar( line ); break; end 7 | lines{end+1, 1} = line; 8 | end 9 | fclose(loops_file); 10 | 11 | loops_mat = zeros(size(lines, 1)); 12 | for i=1:size(lines, 1) 13 | tokens = split(lines{i}); 14 | for j=1:size(tokens) 15 | token = tokens{j}; 16 | if isempty(token) 17 | continue 18 | else 19 | token = str2num(token) + 1; 20 | loops_mat(i, token) = 1; 21 | end 22 | end 23 | end 24 | end 25 | -------------------------------------------------------------------------------- /matlab/plot_path.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function plot_path(loops_file, coords_file) 20 | loops = load(loops_file); 21 | %loops = loops.truth; 22 | coords = load(coords_file); 23 | % For CC and NC 24 | coords = coords.GPS(1:2:end, :); 25 | % For KITTIS 26 | %c = coords.p; 27 | % For KITTI 06 28 | %c(:, 1) = coords.p(:, 2); 29 | %c(:, 2) = -coords.p(:, 1); 30 | % For KITTIs 31 | %coords = c; 32 | % For St. Lucia 33 | %coords = coords.fGPS(1:end, :); 34 | nimages = length(loops); 35 | figure(); 36 | hold on; 37 | axis equal 38 | for i=2:nimages 39 | [loops_index] = find(loops(i, :) > 0); 40 | if numel(loops_index > 0)% && results(i) ~= 1 41 | plot([coords(loops_index(1), 1), coords(i, 1)], [coords(loops_index(1), 2), coords(i, 2)], 'g-', 'LineWidth', 1); 42 | plot([coords(i - 1, 1), coords(i, 1)], [coords(i - 1, 2), coords(i, 2)], 'r-', 'LineWidth', 3); 43 | %plot(coords(loops_index(1), 1), coords(loops_index(1), 2), 'r-', 'LineWidth', 2); 44 | plot([coords(max(loops_index(1) - 1, 1), 1), coords(loops_index(1), 1)], [coords(max(loops_index(1) - 1, 1), 2), coords(loops_index(1), 2)], 'r-', 'LineWidth', 3); 45 | else 46 | plot([coords(i - 1, 1), coords(i, 1)], [coords(i - 1, 2), coords(i, 2)], 'k-', 'LineWidth', 3); 47 | end 48 | %pause(0.005); 49 | end 50 | hold off; 51 | axis off; 52 | 53 | % print -dpng -r300 image 54 | end 55 | -------------------------------------------------------------------------------- /matlab/plot_times.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function plot_times(dtimes_file, lc_file) 20 | 21 | % Function to plot PR curves. 22 | lab_size = 26; 23 | ax_size = 20; 24 | leg_size = 14; 25 | %line_size = 3.0; 26 | 27 | figure; 28 | hold on; 29 | set(gca, 'FontSize', ax_size); 30 | xlabel('# Images', 'FontSize', lab_size); 31 | ylabel('Execution Time (ms)', 'FontSize', lab_size); 32 | %xlim([0., 1.02]); 33 | %ylim([0., 1.02]); 34 | grid('off'); 35 | 36 | dtimes = load(dtimes_file); 37 | lc = load(lc_file); 38 | 39 | disp('----'); 40 | 41 | % % FAST Times 42 | % m = mean(dtimes(:, 1)*1000); 43 | % s = std(dtimes(:, 1)*1000); 44 | % mx = max(dtimes(:, 1)*1000); 45 | % mn = min(dtimes(:, 1)*1000); 46 | % disp(['FAST: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 47 | % sz = size(dtimes(:, 1)); 48 | % plot(1:1:sz(1), dtimes(:, 1) * 1000, 'r'); 49 | % 50 | % % LDB Times 51 | % m = mean(dtimes(:, 2)*1000); 52 | % s = std(dtimes(:, 2)*1000); 53 | % mx = max(dtimes(:, 2)*1000); 54 | % mn = min(dtimes(:, 2)*1000); 55 | % disp(['LDB: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 56 | % sz = size(dtimes(:, 2)); 57 | % plot(1:1:sz(1), dtimes(:, 2) * 1000, 'b'); 58 | % 59 | % % PHOG Times 60 | % m = mean(dtimes(:, 3)*1000); 61 | % s = std(dtimes(:, 3)*1000); 62 | % mx = max(dtimes(:, 3)*1000); 63 | % mn = min(dtimes(:, 3)*1000); 64 | % disp(['PHOG: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 65 | % sz = size(dtimes(:, 3)); 66 | % plot(1:1:sz(1), dtimes(:, 3) * 1000, 'g'); 67 | % 68 | % title('Description Times'); 69 | % l = legend('FAST detection', 'LDB description', 'PHOG description', 'Location', 'NorthEast'); 70 | % set(l, 'FontSize', leg_size); 71 | % hold off; 72 | % print -djpeg -r300 exectimes_desc 73 | 74 | figure; 75 | hold on; 76 | set(gca, 'FontSize', ax_size); 77 | xlabel('# Images', 'FontSize', lab_size); 78 | ylabel('Execution Time (ms)', 'FontSize', lab_size); 79 | %xlim([0., 1.02]); 80 | %ylim([0., 1.02]); 81 | grid('off'); 82 | 83 | % Bayes Predict Times 84 | m = mean(lc(:, 1) * 1000); 85 | s = std(lc(:, 1)* 1000); 86 | mx = max(lc(:, 1)* 1000); 87 | mn = min(lc(:, 1)* 1000); 88 | disp(['Bayes Predict: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 89 | 90 | % Bayes Update Times 91 | m = mean(lc(:, 2)* 1000); 92 | s = std(lc(:, 2)* 1000); 93 | mx = max(lc(:, 2)* 1000); 94 | mn = min(lc(:, 2)* 1000); 95 | disp(['Bayes Update: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 96 | 97 | % LLC 98 | m = mean(lc(:, 3)* 1000); 99 | s = std(lc(:, 3)* 1000); 100 | mx = max(lc(:, 3)* 1000); 101 | mn = min(lc(:, 3)* 1000); 102 | disp(['LLC: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 103 | sz = size(lc(:, 3)); 104 | plot(1:1:sz(1), lc(:, 3) * 1000, 'r'); 105 | 106 | % ILC 107 | m = mean(lc(:, 4)* 1000); 108 | s = std(lc(:, 4)* 1000); 109 | mx = max(lc(:, 4)* 1000); 110 | mn = min(lc(:, 4)* 1000); 111 | disp(['ILC: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 112 | sz = size(lc(:, 4)); 113 | plot(1:1:sz(1), lc(:, 4) * 1000, 'b'); 114 | 115 | % Epipolar Geom 116 | m = mean(lc(:, 5)* 1000); 117 | s = std(lc(:, 5)* 1000); 118 | mx = max(lc(:, 5)* 1000); 119 | mn = min(lc(:, 5)* 1000); 120 | disp(['Ep. Geometry: ', num2str(m), ' ', num2str(s), ' ' num2str(mx), ' ' num2str(mn)]); 121 | sz = size(lc(:, 5)); 122 | plot(1:1:sz(1), lc(:, 5) * 1000, 'g'); 123 | 124 | sz = size(lc(:, 2)); 125 | plot(1:1:sz(1), (lc(:, 1) + lc(:, 2)) * 1000, 'k'); 126 | 127 | title('Loop Closure Times'); 128 | l = legend('Location Likelihood', 'Image Likelihood', 'Epipolar Analysis', 'Bayes (Predict + Update)', 'Location', 'NorthWest'); 129 | set(l, 'FontSize', leg_size); 130 | hold off; 131 | % print -djpeg -r300 exectimes_lc 132 | 133 | disp('----'); 134 | end -------------------------------------------------------------------------------- /matlab/plot_times_bars.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function plot_times_bars(dtimes_file, lc_file, t, show_legend) 20 | 21 | % Function to plot PR curves. 22 | lab_size = 26; 23 | ax_size = 20; 24 | leg_size = 17; 25 | 26 | dtimes = load(dtimes_file); 27 | lc = load(lc_file); 28 | 29 | % Compute DTIMES averages 30 | fast = dtimes(:, 1) * 1000; 31 | ldb = dtimes(:, 2) * 1000; 32 | phog = dtimes(:, 3) * 1000; 33 | fast_t = []; 34 | ldb_t = []; 35 | phog_t = []; 36 | [r, ~] = size(fast); 37 | for i=1:100:r 38 | % FAST 39 | if i + 100 < r 40 | subc = fast(i:i+100); 41 | else 42 | subc = fast(i:end); 43 | end 44 | m = mean(subc); 45 | fast_t = [fast_t, m]; 46 | 47 | % LDB 48 | if i + 100 < r 49 | subc = ldb(i:i+100); 50 | else 51 | subc = ldb(i:end); 52 | end 53 | m = mean(subc); 54 | ldb_t = [ldb_t, m]; 55 | 56 | % PHOG 57 | if i + 100 < r 58 | subc = phog(i:i+100); 59 | else 60 | subc = phog(i:end); 61 | end 62 | m = mean(subc); 63 | phog_t = [phog_t, m]; 64 | end 65 | 66 | % Compute LC averages 67 | bpred = lc(:, 1) * 1000; 68 | bupd = lc(:, 2) * 1000; 69 | llc = lc(:, 3) * 1000; 70 | ilc = lc(:, 4) * 1000; 71 | epgeom = lc(:, 5) * 1000; 72 | 73 | bpred_t = []; 74 | bupd_t = []; 75 | llc_t = []; 76 | ilc_t = []; 77 | epgeom_t = []; 78 | [r, ~] = size(bpred); 79 | for i=1:100:r 80 | % BPRED 81 | if i + 100 < r 82 | subc = bpred(i:i+100); 83 | else 84 | subc = bpred(i:end); 85 | end 86 | m = mean(subc); 87 | bpred_t = [bpred_t, m]; 88 | 89 | % BUPD 90 | if i + 100 < r 91 | subc = bupd(i:i+100); 92 | else 93 | subc = bupd(i:end); 94 | end 95 | m = mean(subc); 96 | bupd_t = [bupd_t, m]; 97 | 98 | % LLC 99 | if i + 100 < r 100 | subc = llc(i:i+100); 101 | else 102 | subc = llc(i:end); 103 | end 104 | m = mean(subc); 105 | llc_t = [llc_t, m]; 106 | 107 | % ILC 108 | if i + 100 < r 109 | subc = ilc(i:i+100); 110 | else 111 | subc = ilc(i:end); 112 | end 113 | m = mean(subc); 114 | ilc_t = [ilc_t, m]; 115 | 116 | % EP GEOM 117 | if i + 100 < r 118 | subc = epgeom(i:i+100); 119 | else 120 | subc = epgeom(i:end); 121 | end 122 | m = mean(subc); 123 | epgeom_t = [epgeom_t, m]; 124 | end 125 | 126 | % Adding differences in sizes 127 | nels = size(fast_t, 2) - size(bpred_t, 2); 128 | for i=1:nels 129 | bpred_t = [0.0, bpred_t]; 130 | bupd_t = [0.0, bupd_t]; 131 | llc_t = [0.0, llc_t]; 132 | ilc_t = [0.0, ilc_t]; 133 | epgeom_t = [0.0, epgeom_t]; 134 | end 135 | 136 | fast_t = fast_t'; 137 | ldb_t = ldb_t'; 138 | phog_t = phog_t'; 139 | bpred_t = bpred_t'; 140 | bupd_t = bupd_t'; 141 | llc_t = llc_t'; 142 | ilc_t = ilc_t'; 143 | epgeom_t = epgeom_t'; 144 | 145 | figure; 146 | set(gca, 'FontSize', ax_size); 147 | bar((1:size(fast_t,1)), [fast_t ldb_t phog_t [bpred_t + bupd_t] llc_t ilc_t epgeom_t], 'stacked'); 148 | axis tight 149 | if show_legend 150 | l = legend('FAST detection', 'LDB description', 'PHOG description', 'Bayes (Predict + Update)', 'Location Likelihood', 'Image Likelihood', 'Epipolar Analysis', 'Location', 'Northwest'); 151 | set(l, 'FontSize', leg_size); 152 | end 153 | title(t); 154 | 155 | xlabel('# of frame (10^2)'); 156 | ylabel('Average Exec. Time (ms)'); 157 | 158 | % print -dpng -r300 exectimes 159 | 160 | % Computing global averages 161 | disp('----'); 162 | 163 | total = 0.0; 164 | 165 | m = mean(fast); 166 | total = total + m; 167 | s = std(fast); 168 | disp(['Fast Detection: ', num2str(m), ' (', num2str(s), ')']); 169 | 170 | m = mean(ldb); 171 | total = total + m; 172 | s = std(ldb); 173 | disp(['LDB Description: ', num2str(m), ' (', num2str(s), ')']); 174 | 175 | m = mean(phog); 176 | total = total + m; 177 | s = std(phog); 178 | disp(['PHOG Description: ', num2str(m), ' (', num2str(s), ')']); 179 | 180 | m = mean(bpred); 181 | total = total + m; 182 | s = std(bpred); 183 | disp(['Bayes Predict: ', num2str(m), ' (', num2str(s), ')']); 184 | 185 | m = mean(bupd); 186 | total = total + m; 187 | s = std(bupd); 188 | disp(['Bayes Update: ', num2str(m), ' (', num2str(s), ')']); 189 | 190 | m = mean(llc); 191 | total = total + m; 192 | s = std(llc); 193 | disp(['Location Likelihood: ', num2str(m), ' (', num2str(s), ')']); 194 | 195 | m = mean(ilc); 196 | total = total + m; 197 | s = std(ilc); 198 | disp(['Image Likelihood: ', num2str(m), ' (', num2str(s), ')']); 199 | 200 | m = mean(epgeom); 201 | total = total + m; 202 | s = std(epgeom); 203 | disp(['Epipolar Analysis: ', num2str(m), ' (', num2str(s), ')']); 204 | 205 | disp('----'); 206 | disp(['Average Execution Time: ', num2str(total)]); 207 | disp('----'); 208 | end -------------------------------------------------------------------------------- /matlab/plot_topmap.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function plot_topmap(tmap_file, coords_file) 20 | tmap = load(tmap_file); 21 | coords = load(coords_file); 22 | % For CC and NC 23 | %coords = coords.GPS(1:2:end, :); 24 | % For KITTIs 25 | c = coords.p; 26 | % For KITTI 06 27 | %c(:, 1) = coords.p(:, 2); 28 | %c(:, 2) = -coords.p(:, 1); 29 | % For KITTIs 30 | coords = c; 31 | % For St. Lucia 32 | %coords = coords.fGPS(1:end, :); 33 | nimages = length(tmap); 34 | 35 | total_nodes = max(tmap(:,2)) + 1; 36 | 37 | %cmap = prism(total_nodes); 38 | cmap = rand(total_nodes, 3); 39 | 40 | figure() 41 | hold on; 42 | for i=2:nimages 43 | plot([coords(i - 1, 1), coords(i, 1)], [coords(i - 1, 2), coords(i, 2)], 'k-', 'LineWidth', 3, 'Color', cmap(tmap(i,2) + 1, :)); 44 | %pause(0.005); 45 | end 46 | 47 | hold off 48 | axis off 49 | 50 | % print -dpng -r300 image 51 | end 52 | -------------------------------------------------------------------------------- /matlab/sparsity.m: -------------------------------------------------------------------------------- 1 | % 2 | % This file is part of htmap. 3 | % 4 | % Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | % 6 | % htmap is free software: you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation, either version 3 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % htmap is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with htmap. If not, see . 18 | 19 | function sparsity 20 | % Function to plot sparsity plots. 21 | lab_size = 26; 22 | ax_size = 20; 23 | leg_size = 20; 24 | line_size = 3.0; 25 | 26 | %CC.nodes = [317, 253, 100, 63, 39, 19, 14, 0]; 27 | %CC.recall = [0.08, 0.11, 0.32, 0.34, 0.36, 0.35, 0.36, 0]; 28 | CC.nodes = [310, 253, 143, 100, 63, 45, 39, 28, 24, 18, 0]; 29 | CC.recall = [0.45, 0.52, 0.61, 0.67, 0.68, 0.69, 0.71, 0.713, 0.724, 0.7325, 0]; 30 | NC.nodes = [234, 146, 104, 74, 56, 33, 24, 18, 10, 9, 7, 0]; 31 | NC.recall = [0.48, 0.55, 0.62, 0.65, 0.64, 0.65 0.65, 0.65, 0.68, 0.54, 0.47, 0]; 32 | 33 | figure(); 34 | hold on; 35 | plot_sparsity(CC.recall, CC.nodes, 'b'); 36 | plot_sparsity(NC.recall, NC.nodes, 'r'); 37 | plot_common(); 38 | l = legend('City Center', 'New College', 'Location', 'SouthEast'); 39 | set(l, 'FontSize', leg_size) 40 | %title('Sparsity'); 41 | 42 | % print -dpng -r300 sparsity 43 | hold off; 44 | 45 | function plot_common() 46 | xlabel('# Locations', 'FontSize', lab_size); 47 | ylabel('Recall', 'FontSize', lab_size); 48 | %xlim([0., 100]); 49 | ylim([0., 1.02]); 50 | grid('off'); 51 | set(gca, 'FontSize', ax_size); 52 | end 53 | 54 | function plot_sparsity(pr, re, format) 55 | plot(re, pr, 'Color', format, 'LineWidth', line_size); 56 | end 57 | end 58 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | htmap 3 | 0.1.0 4 | HTMAP: Hierarchical Topological Mapping 5 | 6 | Emilio Garcia-Fidalgo 7 | Emilio Garcia-Fidalgo 8 | GPLv3 9 | http://wiki.ros.org/htmap 10 | 11 | catkin 12 | 13 | roscpp 14 | std_msgs 15 | sensor_msgs 16 | image_transport 17 | cv_bridge 18 | geometry_msgs 19 | obindex 20 | 21 | roscpp 22 | rospy 23 | std_msgs 24 | sensor_msgs 25 | image_transport 26 | cv_bridge 27 | geometry_msgs 28 | obindex 29 | 30 | -------------------------------------------------------------------------------- /ros/README.md: -------------------------------------------------------------------------------- 1 | # Singularity Container for HTMap 2 | 3 | Singularity container for HTMap. Contains ros with opencv and boost. 4 | 5 | ## Usage 6 | Use `singularity build ros.sif ros.def` to get the singularity image file. 7 | 8 | Once the .sif is built, you will need to use the container for [installing](https://github.com/emiliofidalgo/htmap#installation) `htmap` and `obindex`: 9 | 10 | ``` 11 | singularity exec ros.sif catkin_make -DCMAKE_BUILD_TYPE=Release 12 | ``` 13 | 14 | Then to [execute](https://github.com/emiliofidalgo/htmap#installation) htmap, source the : 15 | 16 | ``` 17 | singularity shell ros.sif 18 | > . devel/setup.sh 19 | > roslaunch htmap htmap.launch image_dir:="/image/dir" working_dir:="~/Desktop/htmap" other_options:="value" 20 | ``` 21 | -------------------------------------------------------------------------------- /ros/ros.def: -------------------------------------------------------------------------------- 1 | Bootstrap: docker 2 | From: ros:kinetic-ros-core-xenial 3 | 4 | %post 5 | 6 | apt update 7 | 8 | apt install -y build-essential 9 | apt install -y libflann-dev libboost-system-dev libboost-filesystem-dev 10 | apt -y install ros-kinetic-opencv3 ros-kinetic-cv-bridge ros-kinetic-image-transport 11 | 12 | %environment 13 | echo "sourcing ros kinetic setup.sh file" 14 | . /opt/ros/kinetic/setup.sh 15 | -------------------------------------------------------------------------------- /src/HTMap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/HTMap.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | HTMap::HTMap(const ros::NodeHandle nh) 26 | : _nh(nh), 27 | _params(0), 28 | _st(0) 29 | { 30 | ROS_INFO("Initializing node"); 31 | 32 | _st = Statistics::getInstance(); 33 | 34 | ROS_INFO("Reading parameters ..."); 35 | _params = Params::getInstance(); 36 | _params->readParams(nh); 37 | ROS_INFO("Parameters read"); 38 | 39 | if (_params->batch) 40 | { 41 | processBatch(); 42 | } 43 | else 44 | { 45 | process(); 46 | } 47 | } 48 | 49 | HTMap::~HTMap() 50 | { 51 | } 52 | 53 | void HTMap::process() 54 | { 55 | // Preparing the resulting directory 56 | ROS_INFO("Preparing results directory ..."); 57 | boost::filesystem::path res_dir = _params->dir_results + "results/"; 58 | boost::filesystem::remove_all(res_dir); 59 | boost::filesystem::create_directory(res_dir); 60 | ROS_INFO("Results directory ready"); 61 | 62 | ROS_INFO("Mapping ..."); 63 | 64 | // Variables to control the mapping time 65 | double init_time, end_time; 66 | init_time = omp_get_wtime(); 67 | 68 | // STEP 1 ---- Image Description ---- 69 | ROS_INFO("--- Image description ---"); 70 | _params->images.resize(_params->nimages, ""); 71 | describeImages(_params->images); 72 | // ---- END Image Description ---- 73 | 74 | // STEP 2 ---- Mapping ---- 75 | ROS_INFO("--- Mapping ---"); 76 | HighLevelMap hmap; 77 | LoopCloser lc; 78 | map(_params->images, hmap, lc); 79 | // ---- END Mapping ---- 80 | 81 | // STEP 3 ---- Writing results ---- 82 | ROS_INFO("--- Writing results ---"); 83 | _st->writeResults(_params->dir_results + "results/", _params->imageLC_min_inliers); 84 | // ---- END Results ---- 85 | 86 | end_time = omp_get_wtime(); 87 | ROS_INFO("Total time: %.5f seconds", end_time - init_time); 88 | ROS_INFO("Process finished"); 89 | } 90 | 91 | void HTMap::processBatch() 92 | { 93 | // Preparing the resulting directory 94 | ROS_INFO("Preparing results directory ..."); 95 | boost::filesystem::path res_dir = _params->dir_results + "results/"; 96 | boost::filesystem::remove_all(res_dir); 97 | boost::filesystem::create_directory(res_dir); 98 | ROS_INFO("Results directory ready"); 99 | 100 | ROS_INFO("Mapping ..."); 101 | 102 | // STEP 1 ---- Image Description ---- 103 | ROS_INFO("--- Image description ---"); 104 | _params->images.resize(_params->nimages, ""); 105 | describeImages(_params->images); 106 | // ---- END Image Description ---- 107 | 108 | for (int curr_inl = _params->inliers_begin; curr_inl < _params->inliers_end; curr_inl += _params->inliers_step) 109 | { 110 | // Updating information 111 | _params->imageLC_min_inliers = curr_inl; 112 | _st->init(_params->nimages); 113 | 114 | // STEP 2 ---- Mapping ---- 115 | ROS_INFO("--- Mapping ---"); 116 | HighLevelMap hmap; 117 | LoopCloser lc; 118 | map(_params->images, hmap, lc); 119 | // ---- END Mapping ---- 120 | 121 | // STEP 3 ---- Writing results ---- 122 | ROS_INFO("--- Writing results ---"); 123 | _st->writeResults(_params->dir_results + "results/", _params->imageLC_min_inliers); 124 | } 125 | // ---- END Results ---- 126 | 127 | ROS_INFO("Process finished"); 128 | } 129 | 130 | void HTMap::describeImages(std::vector& images) 131 | { 132 | if (_params->load_features) 133 | { 134 | images.clear(); 135 | ROS_INFO("Images will be loaded from disk."); 136 | getFilenames(_params->dir_results + "images/", images, false); 137 | } 138 | else 139 | { 140 | // Preparing the resulting directory for images. 141 | ROS_INFO("Preparing image directory ..."); 142 | boost::filesystem::path res_imgs_dir = _params->dir_results + "images/"; 143 | boost::filesystem::remove_all(res_imgs_dir); 144 | boost::filesystem::create_directory(res_imgs_dir); 145 | ROS_INFO("Image directory ready"); 146 | 147 | #pragma omp parallel for 148 | for (unsigned image_ind = 0; image_ind < _params->nimages; image_ind++) 149 | { 150 | ROS_INFO("Describing image %i", image_ind); 151 | cv::Mat image = cv::imread(_params->filenames[image_ind]); 152 | 153 | std::string image_filename = _params->dir_results + "images/image%06d.bmp"; 154 | char name[500]; 155 | sprintf(name, image_filename.c_str(), image_ind); 156 | cv::imwrite(name, image); 157 | 158 | // Detecting and describing key points. 159 | std::vector kps; 160 | cv::Mat dsc; 161 | cv::Mat gdsc; 162 | double init_time = omp_get_wtime(); 163 | _params->detector->detect(image, kps); 164 | double end_time = omp_get_wtime(); 165 | double ftime = end_time - init_time; 166 | cv::KeyPointsFilter::retainBest(kps, _params->max_total_kps); 167 | 168 | init_time = omp_get_wtime(); 169 | _params->descriptor->describe(image, kps, dsc); 170 | end_time = omp_get_wtime(); 171 | double ltime = end_time - init_time; 172 | 173 | init_time = omp_get_wtime(); 174 | _params->gdescriptor->describe(image, gdsc); 175 | end_time = omp_get_wtime(); 176 | double ptime = end_time - init_time; 177 | gdsc.convertTo(gdsc, CV_64F); 178 | 179 | _st->registerDescTimes(ptime, ftime, ltime); 180 | 181 | ROS_INFO("%lu keypoints found.", kps.size()); 182 | 183 | Image curr_image; 184 | curr_image.image_id = image_ind; 185 | curr_image.image_filename = std::string(name); 186 | curr_image.image = image; 187 | curr_image.kps = kps; 188 | curr_image.dscs = dsc; 189 | curr_image.gdsc = gdsc; 190 | 191 | std::string yfilename = _params->dir_results + "images/" + "image%06d.yml"; 192 | sprintf(name, yfilename.c_str(), image_ind); 193 | curr_image.save(name); 194 | 195 | images[image_ind] = std::string(name); 196 | } 197 | } 198 | } 199 | 200 | void HTMap::map(const std::vector& images, HighLevelMap& map, LoopCloser& _lc) 201 | { 202 | // Initializing the first location. 203 | ROS_INFO("Adding first image to location 0."); 204 | Location* loc0 = new Location(_params->loc_max_images, _params->max_total_kps, _params->descriptor->getDescSize()); 205 | 206 | // Adding the loc0 as the first location of the map. 207 | ROS_INFO("Adding location 0 to the map."); 208 | map.addLocation(loc0); 209 | map.setActive(0); 210 | 211 | // Load the first image. 212 | Image img0; 213 | img0.load(images[0]); 214 | unsigned l1,l2; 215 | _lc.process(img0, map, l1, l2); 216 | map.addImageToLocation(0, 0, images[0]); 217 | _st->registerImageToLocation(0, 0); 218 | 219 | // Processing the remaining images. 220 | for (unsigned img_idx = 1; img_idx < _params->nimages; img_idx++) 221 | { 222 | ROS_INFO("Processing image %u", img_idx); 223 | 224 | // Load the current image. 225 | Image img; 226 | img.load(images[img_idx]); 227 | 228 | // Loop Closure ? 229 | unsigned loop_loc; 230 | unsigned loop_img; 231 | bool lclosure; 232 | lclosure = _lc.process(img, map, loop_loc, loop_img); 233 | 234 | if (lclosure) 235 | { 236 | ROS_INFO("-----> Loop detected: %u --> (N %u, I %u)", img_idx, loop_loc, loop_img); 237 | 238 | // Registering the loop. 239 | _st->registerLoop(img_idx, loop_img); 240 | 241 | if (loop_loc != map.active) 242 | { 243 | // Changing active node. 244 | ROS_INFO("Changing active location to %u", loop_loc); 245 | map.linkLocations(map.active, loop_loc, 1.0); 246 | map.setActive(loop_loc); 247 | } 248 | } 249 | else 250 | { 251 | if (isNewLocation(img, map)) 252 | { 253 | // New location. 254 | Location* loc = new Location(_params->loc_max_images, _params->max_total_kps, _params->descriptor->getDescSize()); 255 | map.addLocation(loc); 256 | map.linkLocations(map.active, loc->id, 1.0); 257 | map.setActive(loc->id); 258 | ROS_INFO("Location %u added to the map.", loc->id); 259 | } 260 | } 261 | 262 | // Adding the image to the current active node. 263 | unsigned curr_lid = map.getActiveLocation()->id; 264 | map.addImageToLocation(curr_lid, img_idx, images[img_idx]); 265 | _st->registerImageToLocation(img_idx, curr_lid); 266 | } 267 | } 268 | 269 | bool HTMap::isNewLocation(const Image& img, HighLevelMap& map) 270 | { 271 | // Get the current active location in the map. 272 | Location* loc = map.getActiveLocation(); 273 | 274 | // If we achive the maximum number of images in a node. 275 | if (loc->numImages() > _params->loc_max_images) 276 | { 277 | return true; 278 | } 279 | 280 | double dist = GlobalDescriptor::dist(loc->desc, img.gdsc, _params->gdescriptor); 281 | //ROS_INFO("Distance %f", dist); 282 | if (dist > _params->max_sim_newnode) 283 | { 284 | return true; 285 | } 286 | else 287 | { 288 | return false; 289 | } 290 | } 291 | 292 | } 293 | -------------------------------------------------------------------------------- /src/bayes/BayesFilter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/bayes/BayesFilter.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | BayesFilter::BayesFilter(const BayesFilterParams params) : 26 | _total_elements(0), 27 | _trans_model(params.trans_model), 28 | _min_hyp(params.min_hyp), 29 | _elements(0) 30 | { 31 | _probability.reserve(150000); 32 | _st = Statistics::getInstance(); 33 | _elements = new std::vector; 34 | _params = Params::getInstance(); 35 | } 36 | 37 | BayesFilter::~BayesFilter() 38 | { 39 | } 40 | 41 | void BayesFilter::addElement(const int elem_id) 42 | { 43 | _elements->push_back(elem_id); 44 | _element_to_index[elem_id] = _total_elements; 45 | _index_to_element[_total_elements] = elem_id; 46 | _total_elements++; 47 | if (_probability.size()) 48 | { 49 | _probability.push_back(0.0); 50 | } 51 | else 52 | { 53 | _probability.push_back(1.0); 54 | } 55 | } 56 | 57 | std::vector* BayesFilter::getElements() 58 | { 59 | return _elements; 60 | } 61 | 62 | void BayesFilter::predict(const unsigned curr_img, std::vector* prior) 63 | { 64 | if (_trans_model == BAYES_TRANS_GAUSS && _total_elements > 8) 65 | { 66 | prior->at(0) += _probability[0] * 0.2; 67 | prior->at(1) += _probability[0] * 0.2; 68 | prior->at(2) += _probability[0] * 0.2; 69 | prior->at(3) += _probability[0] * 0.2; 70 | prior->at(4) += _probability[0] * 0.2; 71 | 72 | prior->at(0) += _probability[1] * 0.165; 73 | prior->at(1) += _probability[1] * 0.165; 74 | prior->at(2) += _probability[1] * 0.165; 75 | prior->at(3) += _probability[1] * 0.165; 76 | prior->at(4) += _probability[1] * 0.165; 77 | prior->at(5) += _probability[1] * 0.165; 78 | 79 | prior->at(0) += _probability[2] * 0.143; 80 | prior->at(1) += _probability[2] * 0.143; 81 | prior->at(2) += _probability[2] * 0.143; 82 | prior->at(3) += _probability[2] * 0.143; 83 | prior->at(4) += _probability[2] * 0.143; 84 | prior->at(5) += _probability[2] * 0.143; 85 | prior->at(6) += _probability[2] * 0.143; 86 | 87 | prior->at(0) += _probability[3] * 0.125; 88 | prior->at(1) += _probability[3] * 0.125; 89 | prior->at(2) += _probability[3] * 0.125; 90 | prior->at(3) += _probability[3] * 0.125; 91 | prior->at(4) += _probability[3] * 0.125; 92 | prior->at(5) += _probability[3] * 0.125; 93 | prior->at(6) += _probability[3] * 0.125; 94 | prior->at(7) += _probability[3] * 0.125; 95 | 96 | for (size_t loc_id = 4; loc_id < _total_elements - 4; loc_id++) 97 | { 98 | if (_total_elements > 9) 99 | { 100 | prior->at(loc_id - 4) += _probability[loc_id] * 0.01; 101 | prior->at(loc_id - 3) += _probability[loc_id] * 0.01; 102 | prior->at(loc_id - 2) += _probability[loc_id] * 0.06; 103 | prior->at(loc_id - 1) += _probability[loc_id] * 0.19; 104 | prior->at(loc_id) += _probability[loc_id] * 0.36; 105 | prior->at(loc_id + 1) += _probability[loc_id] * 0.19; 106 | prior->at(loc_id + 2) += _probability[loc_id] * 0.06; 107 | prior->at(loc_id + 3) += _probability[loc_id] * 0.01; 108 | prior->at(loc_id + 4) += _probability[loc_id] * 0.01; 109 | } 110 | else 111 | { 112 | prior->at(loc_id - 4) += _probability[loc_id] * 0.02; 113 | prior->at(loc_id - 3) += _probability[loc_id] * 0.02; 114 | prior->at(loc_id - 2) += _probability[loc_id] * 0.07; 115 | prior->at(loc_id - 1) += _probability[loc_id] * 0.2; 116 | prior->at(loc_id) += _probability[loc_id] * 0.38; 117 | prior->at(loc_id + 1) += _probability[loc_id] * 0.2; 118 | prior->at(loc_id + 2) += _probability[loc_id] * 0.07; 119 | prior->at(loc_id + 3) += _probability[loc_id] * 0.02; 120 | prior->at(loc_id + 4) += _probability[loc_id] * 0.02; 121 | } 122 | } 123 | 124 | prior->at(_total_elements - 1) += _probability[_total_elements - 4] * 0.125; 125 | prior->at(_total_elements - 2) += _probability[_total_elements - 4] * 0.125; 126 | prior->at(_total_elements - 3) += _probability[_total_elements - 4] * 0.125; 127 | prior->at(_total_elements - 4) += _probability[_total_elements - 4] * 0.125; 128 | prior->at(_total_elements - 5) += _probability[_total_elements - 4] * 0.125; 129 | prior->at(_total_elements - 6) += _probability[_total_elements - 4] * 0.125; 130 | prior->at(_total_elements - 7) += _probability[_total_elements - 4] * 0.125; 131 | prior->at(_total_elements - 8) += _probability[_total_elements - 4] * 0.125; 132 | 133 | prior->at(_total_elements - 1) += _probability[_total_elements - 3] * 0.143; 134 | prior->at(_total_elements - 2) += _probability[_total_elements - 3] * 0.143; 135 | prior->at(_total_elements - 3) += _probability[_total_elements - 3] * 0.143; 136 | prior->at(_total_elements - 4) += _probability[_total_elements - 3] * 0.143; 137 | prior->at(_total_elements - 5) += _probability[_total_elements - 3] * 0.143; 138 | prior->at(_total_elements - 6) += _probability[_total_elements - 3] * 0.143; 139 | prior->at(_total_elements - 7) += _probability[_total_elements - 3] * 0.143; 140 | 141 | prior->at(_total_elements - 1) += _probability[_total_elements - 2] * 0.165; 142 | prior->at(_total_elements - 2) += _probability[_total_elements - 2] * 0.165; 143 | prior->at(_total_elements - 3) += _probability[_total_elements - 2] * 0.165; 144 | prior->at(_total_elements - 4) += _probability[_total_elements - 2] * 0.165; 145 | prior->at(_total_elements - 5) += _probability[_total_elements - 2] * 0.165; 146 | prior->at(_total_elements - 6) += _probability[_total_elements - 2] * 0.165; 147 | 148 | prior->at(_total_elements - 1) += _probability[_total_elements - 1] * 0.2; 149 | prior->at(_total_elements - 2) += _probability[_total_elements - 1] * 0.2; 150 | prior->at(_total_elements - 3) += _probability[_total_elements - 1] * 0.2; 151 | prior->at(_total_elements - 4) += _probability[_total_elements - 1] * 0.2; 152 | prior->at(_total_elements - 5) += _probability[_total_elements - 1] * 0.2; 153 | } 154 | else 155 | { 156 | double prob = 1.0 / _total_elements; 157 | for (int i = 0; i < _total_elements; i++) 158 | { 159 | prior->at(i) = prob; 160 | } 161 | } 162 | 163 | // Uncomment this line if you want to save the full Bayes filter info 164 | // _st->registerPrior(curr_img, prior); 165 | } 166 | 167 | void BayesFilter::update(const unsigned curr_img, const std::map& lk, std::vector* prior) 168 | { 169 | // Creating a vector for mean and stdev computation. 170 | std::vector scores; 171 | std::map::const_iterator itr; 172 | for(itr = lk.begin(); itr != lk.end(); itr++) 173 | { 174 | scores.push_back((*itr).second); 175 | } 176 | 177 | // Mean computation. 178 | double sum = std::accumulate(scores.begin(), scores.end(), 0.0); 179 | double mean = sum / scores.size(); 180 | 181 | // Stdev computation. 182 | double accum = 0.0; 183 | for (size_t score_ind = 0; score_ind < scores.size(); score_ind++) 184 | { 185 | accum += (scores[score_ind] - mean) * (scores[score_ind] - mean); 186 | } 187 | double stdev = sqrt(accum / (scores.size() - 1)); 188 | 189 | // Setting the limit for scores. 190 | double limit = mean + stdev; 191 | 192 | std::vector likelihood(_total_elements, 1.0); 193 | for(itr = lk.begin(); itr != lk.end(); itr++) 194 | { 195 | int index = _element_to_index[(*itr).first]; 196 | double score = (*itr).second; 197 | 198 | //likelihood[index] = score; 199 | 200 | if (score > limit) 201 | { 202 | likelihood[index] = (score - stdev) / mean; 203 | } 204 | } 205 | 206 | // Uncomment this line if you want to save the full Bayes filter info 207 | // _st->registerLikelihood(curr_img, likelihood); 208 | 209 | sum = 0.0; 210 | for (unsigned image_id = 0; image_id < _total_elements; image_id++) 211 | { 212 | //_probability[image_id] = likelihood[image_id] * prior[image_id]; 213 | _probability[image_id] = likelihood[image_id] * prior->at(image_id); 214 | prior->at(image_id) = 0.0; 215 | sum += _probability[image_id]; 216 | } 217 | 218 | // Normalizing the final result. 219 | for (unsigned image_id = 0; image_id < _total_elements; image_id++) 220 | { 221 | _probability[image_id] /= sum; 222 | } 223 | 224 | // Uncomment this line if you want to save the full Bayes filter info 225 | // _st->registerPosterior(curr_img, _probability); 226 | } 227 | 228 | void BayesFilter::getMostProbablyElements(std::vector& elems) 229 | { 230 | if (_total_elements > _min_hyp) 231 | { 232 | for (unsigned el_index = 0; el_index < _total_elements; el_index++) 233 | { 234 | BayesFilterResult elind(_index_to_element[el_index], _probability[el_index]); 235 | elems.push_back(elind); 236 | } 237 | 238 | std::sort(elems.begin(), elems.end()); 239 | } 240 | } 241 | 242 | unsigned BayesFilter::numElems() 243 | { 244 | return _total_elements; 245 | } 246 | 247 | } 248 | -------------------------------------------------------------------------------- /src/imgdesc/GlobalDescriptor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/imgdesc/GlobalDescriptor.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | void WISIFTDescriptor::describe(const cv::Mat& image, cv::Mat& desc) 26 | { 27 | int width = image.cols; 28 | int midpoint = static_cast(width / 2); 29 | 30 | // Obtaining left and right patches. 31 | cv::Mat lmat = image.colRange(0, midpoint); 32 | cv::Mat rmat = image.colRange(midpoint, width); 33 | cv::Mat lpatch, rpatch; 34 | cv::resize(lmat, lpatch, cv::Size(128, 128)); 35 | cv::resize(rmat, rpatch, cv::Size(128, 128)); 36 | 37 | // Describing patches using SURF 38 | cv::KeyPoint kp; 39 | kp.pt.x = lpatch.cols / 2; 40 | kp.pt.y = lpatch.rows / 2; 41 | kp.size = 25; 42 | kp.response = 20000.0; 43 | kp.angle = 200.0; 44 | std::vector kps; 45 | kps.push_back(kp); 46 | 47 | cv::Mat descp_l, descp_r; 48 | cv::xfeatures2d::SiftDescriptorExtractor extractor; 49 | extractor.compute(lpatch, kps, descp_l); 50 | extractor.compute(rpatch, kps, descp_r); 51 | 52 | // Merging the descriptors 53 | int ncomps = extractor.descriptorSize(); 54 | desc = cv::Mat::zeros(1, ncomps * 2, CV_32F); 55 | descp_l.copyTo(desc.colRange(0, ncomps)); 56 | descp_r.copyTo(desc.colRange(ncomps, ncomps * 2)); 57 | } 58 | 59 | void WISURFDescriptor::describe(const cv::Mat& image, cv::Mat& desc) 60 | { 61 | int width = image.cols; 62 | int midpoint = static_cast(width / 2); 63 | 64 | // Obtaining left and right patches. 65 | cv::Mat lmat = image.colRange(0, midpoint); 66 | cv::Mat rmat = image.colRange(midpoint, width); 67 | cv::Mat lpatch, rpatch; 68 | cv::resize(lmat, lpatch, cv::Size(128, 128)); 69 | cv::resize(rmat, rpatch, cv::Size(128, 128)); 70 | 71 | // Describing patches using SURF 72 | cv::KeyPoint kp; 73 | kp.pt.x = lpatch.cols / 2; 74 | kp.pt.y = lpatch.rows / 2; 75 | kp.size = 25; 76 | kp.response = 20000.0; 77 | kp.angle = 200.0; 78 | std::vector kps; 79 | kps.push_back(kp); 80 | 81 | cv::Mat descp_l, descp_r; 82 | cv::Ptr extractor = cv::xfeatures2d::SURF::create(5000.0, 4, 3, true, true); 83 | extractor->compute(lpatch, kps, descp_l); 84 | extractor->compute(rpatch, kps, descp_r); 85 | 86 | // Merging the descriptors 87 | int ncomps = extractor->descriptorSize(); 88 | desc = cv::Mat::zeros(1, ncomps * 2, CV_32F); 89 | descp_l.copyTo(desc.colRange(0, ncomps)); 90 | descp_r.copyTo(desc.colRange(ncomps, ncomps * 2)); 91 | } 92 | 93 | void BRIEFGistDescriptor::describe(const cv::Mat& image, cv::Mat& desc) 94 | { 95 | int width = image.cols; 96 | int midpoint = static_cast(width / 2); 97 | int desc_size = int(_desc_size / 2); 98 | 99 | // Obtaining left and right patches. 100 | cv::Mat lmat = image.colRange(0, midpoint); 101 | cv::Mat rmat = image.colRange(midpoint, width); 102 | cv::Mat lpatch, rpatch; 103 | cv::resize(lmat, lpatch, cv::Size(60, 60)); 104 | cv::resize(rmat, rpatch, cv::Size(60, 60)); 105 | 106 | // Defining patches using BRIEF 107 | cv::KeyPoint kp; 108 | kp.pt.x = lpatch.cols / 2; 109 | kp.pt.y = lpatch.rows / 2; 110 | kp.size = 1; 111 | std::vector kps; 112 | kps.push_back(kp); 113 | 114 | // Describing patches using BRIEF 115 | cv::Mat descp_l, descp_r; 116 | cv::xfeatures2d::BriefDescriptorExtractor extractor; 117 | extractor.compute(lpatch, kps, descp_l); 118 | extractor.compute(rpatch, kps, descp_r); 119 | 120 | // Merging the descriptors 121 | desc = cv::Mat::zeros(1, desc_size * 2, CV_8U); 122 | descp_l.copyTo(desc.colRange(0, desc_size)); 123 | descp_r.copyTo(desc.colRange(desc_size, desc_size * 2)); 124 | } 125 | 126 | void WILDBDescriptor::describe(const cv::Mat& image, cv::Mat& desc) 127 | { 128 | int width = image.cols; 129 | int midpoint = static_cast(width / 2); 130 | int desc_size = int(_desc_size / 2); 131 | 132 | // Obtaining left and right patches. 133 | cv::Mat lmat = image.colRange(0, midpoint); 134 | cv::Mat rmat = image.colRange(midpoint, width); 135 | cv::Mat lpatch, rpatch; 136 | cv::resize(lmat, lpatch, cv::Size(60, 60)); 137 | cv::resize(rmat, rpatch, cv::Size(60, 60)); 138 | 139 | // Defining patches using LDB 140 | cv::KeyPoint kp; 141 | kp.pt.x = lpatch.cols / 2; 142 | kp.pt.y = lpatch.rows / 2; 143 | kp.size = 1; 144 | std::vector kps; 145 | kps.push_back(kp); 146 | 147 | // Describing patches using BRIEF 148 | cv::Mat descp_l, descp_r; 149 | _ldb.compute(lpatch, kps, descp_l); 150 | _ldb.compute(rpatch, kps, descp_r); 151 | 152 | // Merging the descriptors 153 | desc = cv::Mat::zeros(1, desc_size * 2, CV_8U); 154 | descp_l.copyTo(desc.colRange(0, desc_size)); 155 | descp_r.copyTo(desc.colRange(desc_size, desc_size * 2)); 156 | } 157 | 158 | void PHOGDescriptor::getHistogram(const cv::Mat& edges, const cv::Mat& ors, const cv::Mat& mag, int startX, int startY, int width, int height, cv::Mat& hist) 159 | { 160 | // Find and increment the right bin/s 161 | for (int x = startX; x < startX + height; x++) 162 | { 163 | for (int y = startY; y < startY + width; y++) 164 | { 165 | if (edges.at(x,y) > 0) 166 | { 167 | int bin = (int)std::floor(ors.at(x, y)); 168 | hist.at(0, bin) = hist.at(0, bin) + mag.at(x, y); 169 | } 170 | } 171 | } 172 | } 173 | 174 | void PHOGDescriptor::describe(const cv::Mat& image, cv::Mat& desc) 175 | { 176 | int nbins = 60; // 20 bins as default. 177 | 178 | _desc_size = nbins + 4 * nbins + 16 * nbins; 179 | 180 | cv::Mat img = image; 181 | if (img.channels() > 1) 182 | { 183 | // Convert the image to grayscale 184 | cv::cvtColor(img, img, CV_BGR2GRAY); 185 | } 186 | 187 | // Mean and Standard Deviation 188 | cv::Scalar cvMean; 189 | cv::Scalar cvStddev; 190 | cv::meanStdDev(img, cvMean, cvStddev); 191 | double mean = cvMean(0); 192 | 193 | // Apply Canny Edge Detector 194 | cv::Mat edges; 195 | // Reduce noise with a kernel 3x3 196 | cv::blur(img, edges, cv::Size(3,3)); 197 | // Canny detector 198 | cv::Canny(edges, edges, 0.66 * mean, 1.33 * mean); 199 | 200 | // Computing the gradients. 201 | // Generate grad_x and grad_y 202 | cv::Mat grad_x, grad_y; 203 | 204 | // Gradient X 205 | cv::Sobel(img, grad_x, CV_32F, 1, 0, 3); 206 | 207 | // Gradient Y 208 | cv::Sobel(img, grad_y, CV_32F, 0, 1, 3); 209 | 210 | // Total Gradient (approximate) 211 | cv::Mat grad_m = cv::abs(grad_x) + cv::abs(grad_y); 212 | 213 | // Computing orientations 214 | cv::Mat grad_o; 215 | cv::phase(grad_x, grad_y, grad_o, true); 216 | 217 | // Quantizing orientations into bins. 218 | double w = 360.0 / (double)nbins; 219 | grad_o = grad_o / w; 220 | 221 | // Creating the descriptor. 222 | desc = cv::Mat::zeros(1, nbins + 4 * nbins + 16 * nbins, CV_32F); 223 | int width = image.cols; 224 | int height = image.rows; 225 | 226 | // Level 0 227 | cv::Mat chist = desc.colRange(0, nbins); 228 | getHistogram(edges, grad_o, grad_m, 0, 0, width, height, chist); 229 | 230 | // Level 1 231 | chist = desc.colRange(nbins, 2 * nbins); 232 | getHistogram(edges, grad_o, grad_m, 0, 0, width / 2, height / 2, chist); 233 | 234 | chist = desc.colRange(2 * nbins, 3 * nbins); 235 | getHistogram(edges, grad_o, grad_m, 0, width / 2, width / 2, height / 2, chist); 236 | 237 | chist = desc.colRange(3 * nbins, 4 * nbins); 238 | getHistogram(edges, grad_o, grad_m, height / 2, 0, width / 2, height / 2, chist); 239 | 240 | chist = desc.colRange(4 * nbins, 5 * nbins); 241 | getHistogram(edges, grad_o, grad_m, height / 2, width / 2, width / 2, height / 2, chist); 242 | 243 | // Level 2 244 | int wstep = width / 4; 245 | int hstep = height / 4; 246 | int binPos = 5; // Next free section in the histogram 247 | for (int i = 0; i < 4; i++) 248 | { 249 | for (int j = 0; j < 4; j++) 250 | { 251 | chist = desc.colRange(binPos * nbins, (binPos + 1) * nbins); 252 | getHistogram(edges, grad_o, grad_m, i * hstep, j * wstep, wstep, hstep, chist); 253 | binPos++; 254 | } 255 | } 256 | 257 | // Normalizing the histogram. 258 | cv::Mat_ sumMat; 259 | cv::reduce(desc, sumMat, 1, CV_REDUCE_SUM); 260 | float sum = sumMat.at(0, 0); 261 | desc = desc / sum; 262 | } 263 | 264 | double GlobalDescriptor::dist(const cv::Mat& a, const cv::Mat& b, GlobalDescriptor *desc) 265 | { 266 | double response; 267 | if (desc->getType() == GDESCRIPTOR_WISIFT || desc->getType() == GDESCRIPTOR_WISURF) 268 | { 269 | response = distEuclidean(a, b); 270 | } 271 | else if (desc->getType() == GDESCRIPTOR_PHOG) 272 | { 273 | //response = distBhattacharyya(a, b); 274 | response = distChiSquare(a, b); 275 | } 276 | else 277 | { 278 | response = double(distHamming(a.ptr(0), b.ptr(0), desc->getDescSize())); 279 | } 280 | return response; 281 | } 282 | 283 | double GlobalDescriptor::dist(const cv::Mat& a, const cv::Mat& b, const cv::Mat& icovar) 284 | { 285 | return distMahalanobis(a, b, icovar); 286 | } 287 | 288 | GlobalDescriptor* GlobalDescriptor::create(const std::string& name, const GlobalDescriptorParams& params) 289 | { 290 | GlobalDescriptor* desc = 0; 291 | 292 | if (name == "WI-SIFT") 293 | { 294 | desc = new WISIFTDescriptor(params); 295 | } 296 | else if (name == "WI-SURF") 297 | { 298 | desc = new WISURFDescriptor(params); 299 | } 300 | else if (name == "BRIEF-Gist") 301 | { 302 | desc = new BRIEFGistDescriptor(params); 303 | } 304 | else if (name == "WI-LDB") 305 | { 306 | desc = new WILDBDescriptor(params); 307 | } 308 | else if (name == "PHOG") 309 | { 310 | desc = new PHOGDescriptor(params); 311 | } 312 | 313 | return desc; 314 | } 315 | 316 | } 317 | -------------------------------------------------------------------------------- /src/imgdesc/KeypointDescriptor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/imgdesc/KeypointDescriptor.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | void FREAKKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 26 | { 27 | _descriptor->compute(image, kps, descs); 28 | } 29 | 30 | void BRISKKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 31 | { 32 | _descriptor->compute(image, kps, descs); 33 | } 34 | 35 | void BRIEFKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 36 | { 37 | _descriptor->compute(image, kps, descs); 38 | } 39 | 40 | void ORBKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 41 | { 42 | _descriptor->compute(image, kps, descs); 43 | } 44 | 45 | void SIFTKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 46 | { 47 | _descriptor->compute(image, kps, descs); 48 | } 49 | 50 | void SURFKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 51 | { 52 | _descriptor->compute(image, kps, descs); 53 | } 54 | 55 | void LDBKeypointDescriptor::describe(const cv::Mat& image, std::vector& kps, cv::Mat& descs) 56 | { 57 | _ldb.compute(image, kps, descs); 58 | } 59 | 60 | KeypointDescriptor* KeypointDescriptor::create(const std::string& name, const KeypointDescriptorParams& params) 61 | { 62 | KeypointDescriptor* desc = 0; 63 | 64 | if (name == "FREAK") 65 | { 66 | desc = new FREAKKeypointDescriptor(params); 67 | } 68 | else if (name == "BRISK") 69 | { 70 | desc = new BRISKKeypointDescriptor(params); 71 | } 72 | else if (name == "BRIEF") 73 | { 74 | desc = new BRIEFKeypointDescriptor(params); 75 | } 76 | else if (name == "ORB") 77 | { 78 | desc = new ORBKeypointDescriptor(params); 79 | } 80 | else if (name == "SIFT") 81 | { 82 | desc = new SIFTKeypointDescriptor(params); 83 | } 84 | else if (name == "SURF") 85 | { 86 | desc = new SURFKeypointDescriptor(params); 87 | } 88 | else if (name == "LDB") 89 | { 90 | desc = new LDBKeypointDescriptor(params); 91 | } 92 | 93 | return desc; 94 | } 95 | 96 | } 97 | -------------------------------------------------------------------------------- /src/imgdesc/KeypointDetector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/imgdesc/KeypointDetector.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | void FASTKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 26 | { 27 | _detector->detect(image, kps); 28 | } 29 | 30 | void BRISKKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 31 | { 32 | _detector->detect(image, kps); 33 | } 34 | 35 | void SIFTKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 36 | { 37 | _detector->detect(image, kps); 38 | } 39 | 40 | void SURFKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 41 | { 42 | _detector->detect(image, kps); 43 | } 44 | 45 | void ORBKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 46 | { 47 | _detector->detect(image, kps); 48 | } 49 | 50 | void StarKeypointDetector::detect(const cv::Mat& image, std::vector& kps) 51 | { 52 | _detector->detect(image, kps); 53 | } 54 | 55 | KeypointDetector* KeypointDetector::create(const std::string& name, const KeypointDetectorParams& params) 56 | { 57 | KeypointDetector* detector = 0; 58 | 59 | if (name == "FAST") 60 | { 61 | detector = new FASTKeypointDetector(params); 62 | } 63 | else if (name == "BRISK") 64 | { 65 | detector = new BRISKKeypointDetector(params); 66 | } 67 | else if (name == "SIFT") 68 | { 69 | detector = new SIFTKeypointDetector(params); 70 | } 71 | else if (name == "SURF") 72 | { 73 | detector = new SURFKeypointDetector(params); 74 | } 75 | else if (name == "ORB") 76 | { 77 | detector = new ORBKeypointDetector(params); 78 | } 79 | else if (name == "STAR") 80 | { 81 | detector = new StarKeypointDetector(params); 82 | } 83 | 84 | return detector; 85 | } 86 | 87 | } 88 | -------------------------------------------------------------------------------- /src/imgdesc/ldb.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | LDB.cpp 3 | Created on: Apr 10, 2013 4 | Author: xinyang 5 | 6 | LDB - Local Difference Binary 7 | Reference implementation of 8 | [1] Xin Yang and Kwang-Ting(Tim) Cheng. LDB: An Ultra-Fast Feature for 9 | Scalable Augmened Reality on Mobile Device. In Proceedings of 10 | IEEE International Symposium on Mixed and Augmented Reality(ISMAR2012). 11 | 12 | Copyright (C) 2012 The Learning-Based Multimedia, University of California, 13 | Santa Barbara, Xin Yang, Kwang-Ting(Tim) Cheng. 14 | 15 | This file is part of LDB. 16 | 17 | LDB is free software: you can redistribute it and/or modify 18 | it under the terms of the GNU General Public License as published by 19 | the Free Software Foundation, either version 3 of the License, or 20 | (at your option) any later version. 21 | 22 | LDB is distributed in the hope that it will be useful, 23 | but WITHOUT ANY WARRANTY; without even the implied warranty of 24 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | GNU General Public License for more details. 26 | 27 | You should have received a copy of the GNU General Public License 28 | along with LDB. If not, see . 29 | */ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "htmap/imgdesc/ldb.h" 37 | 38 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 39 | std::vector > > rotated_X_; 40 | std::vector > > rotated_Y_; 41 | 42 | bool generateCoordFlag = false; 43 | std::vector > coordinates2by2_; 44 | std::vector > coordinates3by3_; 45 | std::vector > coordinates4by4_; 46 | std::vector > coordinates5by5_; 47 | std::vector randomSequence_; 48 | 49 | /** the end of a row in a circular patch */ 50 | std::vector u_max_; 51 | 52 | //#define TRAINING 53 | 54 | int n_levels = 0; 55 | 56 | #define total_bits 1386 57 | #ifdef TRAINING 58 | #define selected_bits 1384 59 | #else 60 | #define selected_bits 256 61 | #endif 62 | 63 | #define LEVEL5 64 | 65 | static int bit_pattern_31_[selected_bits * 4]; //number of tests * 4 (x1,y1,x2,y2) 66 | static int bit_pattern_256_[selected_bits] = 67 | { //trained based on Liberty dataset 68 | 3, 4, 11, 12, 13, 34, 47, 48, 69 | 50, 56, 58, 59, 62, 71, 78, 79, 70 | 101, 102, 109, 119, 122, 125, 126, 135, 71 | 140, 147, 160, 175, 181, 182, 187, 193, 72 | 199, 215, 220, 224, 227, 232, 234, 235, 73 | 244, 246, 260, 261, 263, 273, 274, 280, 74 | 285, 286, 288, 299, 300, 310, 327, 330, 75 | 331, 343, 346, 353, 360, 361, 362, 365, 76 | 386, 389, 398, 401, 411, 412, 415, 423, 77 | 424, 427, 428, 430, 432, 433, 436, 448, 78 | 451, 454, 463, 464, 469, 477, 478, 480, 79 | 495, 512, 517, 520, 541, 550, 553, 556, 80 | 566, 571, 581, 585, 587, 592, 596, 609, 81 | 610, 611, 614, 615, 640, 655, 657, 664, 82 | 670, 672, 673, 684, 685, 699, 703, 705, 83 | 706, 718, 720, 721, 733, 734, 737, 742, 84 | 744, 745, 748, 749, 752, 764, 766, 774, 85 | 775, 778, 779, 781, 789, 794, 801, 804, 86 | 808, 815, 816, 817, 827, 828, 835, 838, 87 | 841, 844, 846, 850, 853, 855, 868, 879, 88 | 885, 896, 899, 907, 914, 922, 924, 925, 89 | 934, 937, 938, 940, 943, 953, 954, 958, 90 | 968, 969, 970, 973, 990, 991, 994, 1000, 91 | 1003, 1005, 1007, 1020, 1027, 1029, 1030, 1033, 92 | 1036, 1037, 1038, 1042, 1044, 1048, 1052, 1068, 93 | 1072, 1078, 1098, 1100, 1117, 1125, 1126, 1132, 94 | 1134, 1135, 1140, 1144, 1149, 1150, 1153, 1159, 95 | 1163, 1168, 1171, 1172, 1175, 1183, 1187, 1192, 96 | 1195, 1198, 1200, 1202, 1207, 1212, 1215, 1228, 97 | 1247, 1255, 1262, 1265, 1278, 1283, 1286, 1289, 98 | 1290, 1305, 1311, 1312, 1314, 1321, 1328, 1331, 99 | 1334, 1336, 1343, 1350, 1351, 1354, 1356, 1373 100 | }; 101 | 102 | static float IC_Angle(const cv::Mat& image, const int half_k, cv::Point2f pt, 103 | const std::vector & u_max) 104 | { 105 | int m_01 = 0, m_10 = 0; 106 | 107 | const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x)); 108 | 109 | // Treat the center line differently, v=0 110 | for (int u = -half_k; u <= half_k; ++u) 111 | m_10 += u * center[u]; 112 | 113 | // Go line by line in the circular patch 114 | int step = (int)image.step1(); 115 | for (int v = 1; v <= half_k; ++v) 116 | { 117 | // Proceed over the two lines 118 | int v_sum = 0; 119 | int d = u_max[v]; 120 | for (int u = -d; u <= d; ++u) 121 | { 122 | int val_plus = center[u + v*step], val_minus = center[u - v*step]; 123 | v_sum += (val_plus - val_minus); 124 | m_10 += u * (val_plus + val_minus); 125 | } 126 | m_01 += v * v_sum; 127 | } 128 | 129 | return cv::fastAtan2((float)m_01, (float)m_10); 130 | } 131 | 132 | static void generateRotatedPatterns(const int& patch_size, 133 | const int& kNumAngles, 134 | std::vector > >& rotatedX, 135 | std::vector > >& rotatedY) 136 | { 137 | int win_offset = (patch_size-1)/2; 138 | for (int i = 0; i < kNumAngles; i++) 139 | { 140 | std::vector > mappedX, mappedY; 141 | for(int m = 0; m < patch_size; m++){ 142 | std::vector one_row(patch_size); 143 | mappedX.push_back(one_row); 144 | mappedY.push_back(one_row); 145 | } 146 | float descriptor_dir = (float)(i * 2* CV_PI/ 30); 147 | float sin_dir = sin(descriptor_dir); 148 | float cos_dir = cos(descriptor_dir); 149 | int a, b; a = 0; 150 | for(int m = win_offset; m >= -win_offset; m--, a++){ 151 | b = 0; 152 | for(int n = -win_offset; n <= win_offset; n++, b++){ 153 | float pixel_x = n*cos_dir + m*sin_dir; 154 | float pixel_y = -n*sin_dir + m*cos_dir; 155 | 156 | int x = cvRound(pixel_x); 157 | int y = cvRound(pixel_y); 158 | 159 | mappedX[a][b] = x; 160 | mappedY[a][b] = y; 161 | } 162 | } 163 | rotatedX.push_back(mappedX); 164 | rotatedY.push_back(mappedY); 165 | } 166 | } 167 | 168 | /** computer the grid coordinates for computing LDB*/ 169 | void computeCoordinates(std::vector >& coordinates, int step, int patch_size) 170 | { 171 | int win_size = step - 1; 172 | int m = 0; 173 | for(int i = -patch_size/2 + 1; i < patch_size/2 - 2; i+= step){ 174 | for(int j = -patch_size/2 + 1; j < patch_size/2 - 2; j+=step, m++){ 175 | std::vector coord; 176 | int x1 = j; int y1 = i; 177 | int x2 = j+win_size; int y2 = i; 178 | int x3 = j; int y3 = i+win_size; 179 | int x4 = j+win_size; int y4 = i+win_size; 180 | 181 | int x5 = j+win_size/2; int y5 = i; 182 | int x6 = j+win_size; int y6 = i+win_size/2; 183 | int x7 = j+win_size/2; int y7 = i+win_size; 184 | int x8 = j; int y8 = i+win_size/2; 185 | 186 | int x9 = j+win_size/2; int y9 = i+win_size/2; 187 | 188 | coord.push_back(x1); coord.push_back(y1); 189 | coord.push_back(x2); coord.push_back(y2); 190 | coord.push_back(x3); coord.push_back(y3); 191 | coord.push_back(x4); coord.push_back(y4); 192 | 193 | coord.push_back(x5); coord.push_back(y5); 194 | coord.push_back(x6); coord.push_back(y6); 195 | coord.push_back(x7); coord.push_back(y7); 196 | coord.push_back(x8); coord.push_back(y8); 197 | 198 | coord.push_back(x9); coord.push_back(y9); 199 | 200 | coordinates.push_back(coord); 201 | } 202 | } 203 | } 204 | 205 | /** generate random sequence size of 256 for computing LDP*/ 206 | void generateRandSequence(std::vector& randomSequence) 207 | { 208 | srand(time(NULL)); 209 | std::set visited; 210 | int count = 0; 211 | do{ 212 | int randNum = rand() % total_bits; 213 | if(visited.find(randNum) == visited.end()){ 214 | visited.insert(randNum); 215 | count++; 216 | } 217 | }while(count < selected_bits); 218 | for(std::set::const_iterator it = visited.begin(); it != visited.end(); it++) 219 | randomSequence.push_back(*it); 220 | } 221 | 222 | static inline int angle2Wedge(const int& kNumAngles, float angle) 223 | { 224 | static float scale = float(kNumAngles) / 360.0f; 225 | return std::min(int(std::floor(angle * scale)), kNumAngles - 1); 226 | } 227 | 228 | inline void rotatedIntegralImage(double descriptor_dir, 229 | const cv::KeyPoint& kpt, 230 | const cv::Mat& img, 231 | const int& patch_size, 232 | cv::Mat& win_integral_image) 233 | { 234 | 235 | //* Nearest neighbour version (faster) */ 236 | descriptor_dir *= (float)(CV_PI/180); 237 | float sin_dir = sin(descriptor_dir); 238 | float cos_dir = cos(descriptor_dir); 239 | float win_offset = (int)(patch_size/2); 240 | cv::Mat win(patch_size, patch_size, CV_8U); 241 | //******************************************************// 242 | // faster version: xin yang @ 2012-07-05 11:22am 243 | //******************************************************// 244 | float start_x = kpt.pt.x - win_offset*cos_dir + win_offset*sin_dir; 245 | float start_y = kpt.pt.y - win_offset*sin_dir - win_offset*cos_dir; 246 | 247 | for(int i = 0; i < patch_size; i++, start_x -= sin_dir, start_y += cos_dir){ 248 | float pixel_x = start_x; 249 | float pixel_y = start_y; 250 | for(int j = 0; j < patch_size; j++, pixel_x += cos_dir, pixel_y += sin_dir){ 251 | int x = (int)(pixel_x + 0.5); 252 | int y = (int)(pixel_y + 0.5); 253 | //int x = std::min(std::max(cvRound(pixel_x), 0), img.cols-1); 254 | //int y = std::min(std::max(cvRound(pixel_y), 0), img.rows-1); 255 | win.at(i, j) = img.at(y, x); 256 | } 257 | } 258 | cv::integral(win, win_integral_image, CV_32S); 259 | } 260 | 261 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 262 | 263 | static void computeLdbDescriptor(const cv::KeyPoint& kpt, const cv::Mat& img, const cv::Mat& sum, unsigned char * desc, const int& patch_size, 264 | const std::vector >& coordinates2by2_, const std::vector >& coordinates3by3_, 265 | const std::vector >& coordinates4by4_, const std::vector >& coordinates5by5_, 266 | const std::vector& randSequence) 267 | { 268 | #ifdef TRAINING 269 | for(int i = 0; i < randSequence.size(); i++) 270 | bit_pattern_256_[i] = randSequence[i]; 271 | #endif 272 | 273 | // Compute the pointer to the center of the feature 274 | int img_y = (int)(kpt.pt.y + 0.5); 275 | int img_x = (int)(kpt.pt.x + 0.5); 276 | const int * center = reinterpret_cast (sum.ptr(img_y)) + img_x; 277 | 278 | int sum2by2[4], sum3by3[9], sum4by4[16], sum5by5[25]; 279 | int dx2by2[4], dx3by3[9], dx4by4[16], dx5by5[25]; 280 | int dy2by2[4], dy3by3[9], dy4by4[16], dy5by5[25]; 281 | int dxy2by2[4], dxy3by3[9], dxy4by4[16], dxy5by5[25]; 282 | 283 | int sum2by2_size = 4, sum3by3_size = 9, sum4by4_size = 16, sum5by5_size = 25; 284 | 285 | int /*patch_size = PATCH_SIZE; int */offset = patch_size/2; 286 | cv::Mat win_integral_image(patch_size+1, patch_size+1, CV_32S); 287 | 288 | if(kpt.angle != -1) 289 | rotatedIntegralImage(kpt.angle, kpt, img, patch_size, win_integral_image); 290 | 291 | for(int i = 0; i < sum2by2_size; i++){ 292 | if(kpt.angle == -1){ 293 | int a = coordinates2by2_[i][1] * sum.cols + coordinates2by2_[i][0]; 294 | int b = coordinates2by2_[i][3] * sum.cols + coordinates2by2_[i][2]; 295 | int c = coordinates2by2_[i][5] * sum.cols + coordinates2by2_[i][4]; 296 | int d = coordinates2by2_[i][7] * sum.cols + coordinates2by2_[i][6]; 297 | 298 | sum2by2[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b); 299 | 300 | int e = coordinates2by2_[i][9] * sum.cols + coordinates2by2_[i][8]; 301 | int f = coordinates2by2_[i][11] * sum.cols + coordinates2by2_[i][10]; 302 | int g = coordinates2by2_[i][13] * sum.cols + coordinates2by2_[i][12]; 303 | int h = coordinates2by2_[i][15] * sum.cols + coordinates2by2_[i][14]; 304 | 305 | dx2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c)); 306 | dy2by2[i] = sum2by2[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h)); 307 | } 308 | else{ 309 | sum2by2[i] = win_integral_image.at(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset) 310 | + win_integral_image.at(coordinates2by2_[i][7]+offset, coordinates2by2_[i][6]+offset) 311 | - win_integral_image.at(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset) 312 | - win_integral_image.at(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset); 313 | 314 | dx2by2[i] = sum2by2[i] 315 | - 2*(win_integral_image.at(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset) 316 | + win_integral_image.at(coordinates2by2_[i][13]+offset, coordinates2by2_[i][12]+offset) 317 | - win_integral_image.at(coordinates2by2_[i][9]+offset, coordinates2by2_[i][8]+offset) 318 | - win_integral_image.at(coordinates2by2_[i][5]+offset, coordinates2by2_[i][4]+offset)); 319 | 320 | dy2by2[i] = sum2by2[i] 321 | - 2*(win_integral_image.at(coordinates2by2_[i][1]+offset, coordinates2by2_[i][0]+offset) 322 | + win_integral_image.at(coordinates2by2_[i][11]+offset, coordinates2by2_[i][10]+offset) 323 | - win_integral_image.at(coordinates2by2_[i][3]+offset, coordinates2by2_[i][2]+offset) 324 | - win_integral_image.at(coordinates2by2_[i][15]+offset, coordinates2by2_[i][14]+offset)); 325 | } 326 | } 327 | 328 | for(int i = 0; i < sum3by3_size; i++){ 329 | if(kpt.angle == -1){ 330 | int a = coordinates3by3_[i][1] * sum.cols + coordinates3by3_[i][0]; 331 | int b = coordinates3by3_[i][3] * sum.cols + coordinates3by3_[i][2]; 332 | int c = coordinates3by3_[i][5] * sum.cols + coordinates3by3_[i][4]; 333 | int d = coordinates3by3_[i][7] * sum.cols + coordinates3by3_[i][6]; 334 | sum3by3[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b); 335 | 336 | int e = coordinates3by3_[i][9] * sum.cols + coordinates3by3_[i][8]; 337 | int f = coordinates3by3_[i][11] * sum.cols + coordinates3by3_[i][10]; 338 | int g = coordinates3by3_[i][13] * sum.cols + coordinates3by3_[i][12]; 339 | int h = coordinates3by3_[i][15] * sum.cols + coordinates3by3_[i][14]; 340 | 341 | dx3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c)); 342 | dy3by3[i] = sum3by3[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h));} 343 | else{ 344 | sum3by3[i] = win_integral_image.at(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset) 345 | + win_integral_image.at(coordinates3by3_[i][7]+offset, coordinates3by3_[i][6]+offset) 346 | - win_integral_image.at(coordinates3by3_[i][3]+offset, coordinates3by3_[i][2]+offset) 347 | - win_integral_image.at(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset); 348 | 349 | dx3by3[i] = sum3by3[i] 350 | - 2*(win_integral_image.at(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset) 351 | + win_integral_image.at(coordinates3by3_[i][13]+offset, coordinates3by3_[i][12]+offset) 352 | - win_integral_image.at(coordinates3by3_[i][9]+offset, coordinates3by3_[i][8]+offset) 353 | - win_integral_image.at(coordinates3by3_[i][5]+offset, coordinates3by3_[i][4]+offset)); 354 | 355 | dy3by3[i] = sum3by3[i] 356 | - 2*(win_integral_image.at(coordinates3by3_[i][1]+offset, coordinates3by3_[i][0]+offset) 357 | + win_integral_image.at(coordinates3by3_[i][11]+offset, coordinates3by3_[i][10]+offset) 358 | - win_integral_image.at(coordinates3by3_[i][3]+offset, coordinates3by3_[i][2]+offset) 359 | - win_integral_image.at(coordinates3by3_[i][15]+offset, coordinates3by3_[i][14]+offset)); 360 | } 361 | } 362 | 363 | for(int i = 0; i < sum4by4_size; i++){ 364 | if(kpt.angle == -1){ 365 | int a = coordinates4by4_[i][1] * sum.cols + coordinates4by4_[i][0]; 366 | int b = coordinates4by4_[i][3] * sum.cols + coordinates4by4_[i][2]; 367 | int c = coordinates4by4_[i][5] * sum.cols + coordinates4by4_[i][4]; 368 | int d = coordinates4by4_[i][7] * sum.cols + coordinates4by4_[i][6]; 369 | sum4by4[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b); 370 | 371 | int e = coordinates4by4_[i][9] * sum.cols + coordinates4by4_[i][8]; 372 | int f = coordinates4by4_[i][11] * sum.cols + coordinates4by4_[i][10]; 373 | int g = coordinates4by4_[i][13] * sum.cols + coordinates4by4_[i][12]; 374 | int h = coordinates4by4_[i][15] * sum.cols + coordinates4by4_[i][14]; 375 | 376 | dx4by4[i] = sum4by4[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c)); 377 | dy4by4[i] = sum4by4[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h)); 378 | } 379 | else{ 380 | sum4by4[i] = win_integral_image.at(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset) 381 | + win_integral_image.at(coordinates4by4_[i][7]+offset, coordinates4by4_[i][6]+offset) 382 | - win_integral_image.at(coordinates4by4_[i][3]+offset, coordinates4by4_[i][2]+offset) 383 | - win_integral_image.at(coordinates4by4_[i][5]+offset, coordinates4by4_[i][4]+offset); 384 | 385 | dx4by4[i] = sum4by4[i] 386 | - 2*(win_integral_image.at(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset) 387 | + win_integral_image.at(coordinates4by4_[i][13]+offset, coordinates4by4_[i][12]+offset) 388 | - win_integral_image.at(coordinates4by4_[i][9]+offset, coordinates4by4_[i][8]+offset) 389 | - win_integral_image.at(coordinates4by4_[i][5]+offset, coordinates4by4_[i][4]+offset)); 390 | 391 | dy4by4[i] = sum4by4[i] 392 | - 2*(win_integral_image.at(coordinates4by4_[i][1]+offset, coordinates4by4_[i][0]+offset) 393 | + win_integral_image.at(coordinates4by4_[i][11]+offset, coordinates4by4_[i][10]+offset) 394 | - win_integral_image.at(coordinates4by4_[i][3]+offset, coordinates4by4_[i][2]+offset) 395 | - win_integral_image.at(coordinates4by4_[i][15]+offset, coordinates4by4_[i][14]+offset)); 396 | } 397 | } 398 | #ifdef LEVEL5 399 | for(int i = 0; i < sum5by5_size; i++){ 400 | if(kpt.angle == -1){ 401 | int a = coordinates5by5_[i][1] * sum.cols + coordinates5by5_[i][0]; 402 | int b = coordinates5by5_[i][3] * sum.cols + coordinates5by5_[i][2]; 403 | int c = coordinates5by5_[i][5] * sum.cols + coordinates5by5_[i][4]; 404 | int d = coordinates5by5_[i][7] * sum.cols + coordinates5by5_[i][6]; 405 | sum5by5[i] = *(center + a) + *(center + d) - *(center + c) - *(center + b); 406 | 407 | int e = coordinates5by5_[i][9] * sum.cols + coordinates5by5_[i][8]; 408 | int f = coordinates5by5_[i][11] * sum.cols + coordinates5by5_[i][10]; 409 | int g = coordinates5by5_[i][13] * sum.cols + coordinates5by5_[i][12]; 410 | int h = coordinates5by5_[i][15] * sum.cols + coordinates5by5_[i][14]; 411 | 412 | dx5by5[i] = sum5by5[i] - 2*(*(center + a) + *(center + g) - *(center + e) - *(center + c)); 413 | dy5by5[i] = sum5by5[i] - 2*(*(center + a) + *(center + f) - *(center + b) - *(center + h)); 414 | } 415 | else{ 416 | sum5by5[i] = win_integral_image.at(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset) 417 | + win_integral_image.at(coordinates5by5_[i][7]+offset, coordinates5by5_[i][6]+offset) 418 | - win_integral_image.at(coordinates5by5_[i][3]+offset, coordinates5by5_[i][2]+offset) 419 | - win_integral_image.at(coordinates5by5_[i][5]+offset, coordinates5by5_[i][4]+offset); 420 | 421 | dx5by5[i] = sum5by5[i] 422 | - 2*(win_integral_image.at(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset) 423 | + win_integral_image.at(coordinates5by5_[i][13]+offset, coordinates5by5_[i][12]+offset) 424 | - win_integral_image.at(coordinates5by5_[i][9]+offset, coordinates5by5_[i][8]+offset) 425 | - win_integral_image.at(coordinates5by5_[i][5]+offset, coordinates5by5_[i][4]+offset)); 426 | 427 | dy5by5[i] = sum5by5[i] 428 | - 2*(win_integral_image.at(coordinates5by5_[i][1]+offset, coordinates5by5_[i][0]+offset) 429 | + win_integral_image.at(coordinates5by5_[i][11]+offset, coordinates5by5_[i][10]+offset) 430 | - win_integral_image.at(coordinates5by5_[i][3]+offset, coordinates5by5_[i][2]+offset) 431 | - win_integral_image.at(coordinates5by5_[i][15]+offset, coordinates5by5_[i][14]+offset)); 432 | } 433 | } 434 | #endif 435 | int pt = 0; 436 | int entire_pt = 0; 437 | uchar desc_bitstring[selected_bits]; 438 | static const uchar score[] = {1 << 0, 1 << 1, 1 << 2, 1 << 3, 1 << 4, 1 << 5, 1 << 6, 1 << 7}; 439 | 440 | for(int i = 0; i < sum2by2_size; i++){ 441 | int sum1 = sum2by2[i]; 442 | int dx1 = dx2by2[i]; 443 | int dy1 = dy2by2[i]; 444 | for(int j = i+1; j < sum2by2_size; j++){ 445 | if(bit_pattern_256_[pt] == entire_pt){ 446 | int sum2 = sum2by2[j]; 447 | int idx = pt % 8; 448 | desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0; 449 | pt++; 450 | } 451 | entire_pt++; 452 | if(bit_pattern_256_[pt] == entire_pt){ 453 | int dx2 = dx2by2[j]; 454 | int idx = pt % 8; 455 | desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0; 456 | pt++; 457 | } 458 | entire_pt++; 459 | if(bit_pattern_256_[pt] == entire_pt){ 460 | int dy2 = dy2by2[j]; 461 | int idx = pt % 8; 462 | desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0; 463 | pt++; 464 | } 465 | entire_pt++; 466 | } 467 | } 468 | for(int i = 0; i < sum3by3_size; i++){ 469 | int sum1 = sum3by3[i]; 470 | int dx1 = dx3by3[i]; 471 | int dy1 = dy3by3[i]; 472 | for(int j = i+1; j < sum3by3_size; j++){ 473 | if(bit_pattern_256_[pt] == entire_pt){ 474 | int sum2 = sum3by3[j]; 475 | int idx = pt % 8; 476 | desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0; 477 | pt++; 478 | } 479 | entire_pt++; 480 | if(bit_pattern_256_[pt] == entire_pt){ 481 | int dx2 = dx3by3[j]; 482 | int idx = pt % 8; 483 | desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0; 484 | pt++; 485 | } 486 | entire_pt++; 487 | if(bit_pattern_256_[pt] == entire_pt){ 488 | int dy2 = dy3by3[j]; 489 | int idx = pt % 8; 490 | desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0; 491 | pt++; 492 | } 493 | entire_pt++; 494 | } 495 | } 496 | for(int i = 0; i < sum4by4_size; i++){ 497 | int sum1 = sum4by4[i]; 498 | int dx1 = dx4by4[i]; 499 | int dy1 = dy4by4[i]; 500 | for(int j = i+1; j < sum4by4_size; j++){ 501 | if(bit_pattern_256_[pt] == entire_pt){ 502 | int sum2 = sum4by4[j]; 503 | int idx = pt % 8; 504 | desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0; 505 | pt++; 506 | } 507 | entire_pt++; 508 | if(bit_pattern_256_[pt] == entire_pt){ 509 | int dx2 = dx4by4[j]; 510 | int idx = pt % 8; 511 | desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0; 512 | pt++; 513 | } 514 | entire_pt++; 515 | if(bit_pattern_256_[pt] == entire_pt){ 516 | int dy2 = dy4by4[j]; 517 | int idx = pt % 8; 518 | desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0; 519 | pt++; 520 | } 521 | entire_pt++; 522 | } 523 | } 524 | #ifdef LEVEL5 525 | for(int i = 0; i < sum5by5_size; i++){ 526 | int sum1 = sum5by5[i]; 527 | int dx1 = dx5by5[i]; 528 | int dy1 = dy5by5[i]; 529 | for(int j = i+1; j < sum5by5_size; j++){ 530 | if(bit_pattern_256_[pt] == entire_pt){ 531 | int sum2 = sum5by5[j]; 532 | int idx = pt % 8; 533 | desc_bitstring[pt] = (sum1 > sum2) ? score[idx] : 0; 534 | pt++; 535 | } 536 | entire_pt++; 537 | if(bit_pattern_256_[pt] == entire_pt){ 538 | int dx2 = dx5by5[j]; 539 | int idx = pt % 8; 540 | desc_bitstring[pt] = (dx1 > dx2) ? score[idx] : 0; 541 | pt++; 542 | } 543 | entire_pt++; 544 | if(bit_pattern_256_[pt] == entire_pt){ 545 | int dy2 = dy5by5[j]; 546 | int idx = pt % 8; 547 | desc_bitstring[pt] = (dy1 > dy2) ? score[idx] : 0; 548 | pt++; 549 | } 550 | entire_pt++; 551 | } 552 | } 553 | #endif 554 | for (int i = 0, j = 0; i < selected_bits/8; i++, j += 8) 555 | { 556 | desc[i] = (desc_bitstring[j]) | (desc_bitstring[j+1]) 557 | | (desc_bitstring[j+2]) | (desc_bitstring[j+3]) 558 | | (desc_bitstring[j+4]) | (desc_bitstring[j+5]) 559 | | (desc_bitstring[j+6]) | (desc_bitstring[j+7]); 560 | } 561 | } 562 | 563 | static void computeOrientation(const cv::Mat& image, std::vector& keypoints, 564 | int halfPatchSize, const std::vector& umax) 565 | { 566 | // Process each keypoint 567 | for (std::vector::iterator keypoint = keypoints.begin(), 568 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 569 | { 570 | keypoint->angle = IC_Angle(image, halfPatchSize, keypoint->pt, umax); 571 | } 572 | } 573 | 574 | /** Compute the LDB decriptors 575 | * @param image the image to compute the features and descriptors on 576 | * @param integral_image the integral image of the image (can be empty, but the computation will be slower) 577 | * @param level the scale at which we compute the orientation 578 | * @param keypoints the keypoints to use 579 | * @param descriptors the resulting descriptors 580 | */ 581 | static void computeDescriptors(const cv::Mat& image, const cv::Mat& integral_image, 582 | const int& patchSize, std::vector& keypoints, cv::Mat& descriptors, int dsize) 583 | { 584 | //convert to grayscale if more than one color 585 | CV_Assert(image.type() == CV_8UC1); 586 | //create the descriptor mat, keypoints.size() rows, BYTES cols 587 | descriptors = cv::Mat::zeros((int)keypoints.size(), dsize, CV_8UC1); 588 | 589 | for (size_t i = 0; i < keypoints.size(); i++) 590 | computeLdbDescriptor(keypoints[i], image, integral_image, descriptors.ptr((int)i), 591 | patchSize, coordinates2by2_, coordinates3by3_, coordinates4by4_, coordinates5by5_,randomSequence_); 592 | } 593 | 594 | static void initializeLdbPattern(const int& nlevels, const int& patchSize) 595 | { 596 | if(!generateCoordFlag){ 597 | int patch_size = patchSize; 598 | rotated_X_.clear(); 599 | rotated_Y_.clear(); 600 | generateRotatedPatterns(patch_size, 30, rotated_X_, rotated_Y_); 601 | 602 | computeCoordinates(coordinates2by2_, (int)(patch_size/2), patch_size); 603 | computeCoordinates(coordinates3by3_, (int)(patch_size/3), patch_size); 604 | computeCoordinates(coordinates4by4_, (int)(patch_size/4), patch_size); 605 | computeCoordinates(coordinates5by5_, (int)(patch_size/5), patch_size); 606 | 607 | generateCoordFlag = true; 608 | } 609 | 610 | if(randomSequence_.empty()) 611 | generateRandSequence(randomSequence_); 612 | 613 | n_levels = nlevels; 614 | } 615 | 616 | static inline float getScale(int level, int firstLevel, double scaleFactor) 617 | { 618 | return (float)std::pow(scaleFactor, (double)(level - firstLevel)); 619 | } 620 | 621 | /** Constructor 622 | * @param detector_params parameters to use 623 | */ 624 | LDB::LDB(int _bytes, int _nlevels, int _patchSize) 625 | { 626 | scaleFactor = 1.2; 627 | patchSize = 48; 628 | firstLevel = 0; 629 | kBytes = (int)(selected_bits/8); 630 | _nlevels = 3; 631 | initializeLdbPattern(_nlevels, patchSize); 632 | } 633 | 634 | LDB::~LDB() 635 | { 636 | } 637 | 638 | int LDB::descriptorSize() const 639 | { 640 | return kBytes; 641 | } 642 | 643 | int LDB::descriptorType() const 644 | { 645 | return CV_8U; 646 | } 647 | 648 | void LDB::compute( const cv::Mat& _image, std::vector& _keypoints, cv::Mat& _descriptors) const 649 | { 650 | if(_image.empty() ) 651 | return; 652 | 653 | cv::Mat image; 654 | _image.copyTo(image); 655 | 656 | //ROI handling 657 | int halfPatchSize = patchSize / 2; 658 | int border = halfPatchSize*1.415 + 1; 659 | 660 | if( image.type() != CV_8UC1 ) 661 | cvtColor(image, image, CV_BGR2GRAY); 662 | 663 | int levelsNum = 0; 664 | for( size_t i = 0; i < _keypoints.size(); i++ ) 665 | levelsNum = std::max(levelsNum, std::max(_keypoints[i].octave, 0)); 666 | levelsNum++; 667 | 668 | // Pre-compute the scale pyramids 669 | std::vector imagePyramid(levelsNum); 670 | for (int level = 0; level < levelsNum; ++level) 671 | { 672 | float scale = 1/getScale(level, firstLevel, scaleFactor); 673 | cv::Size sz(cvRound(image.cols*scale), cvRound(image.rows*scale)); 674 | cv::Size wholeSize(sz.width + border*2, sz.height + border*2); 675 | cv::Mat temp(wholeSize, image.type()), masktemp; 676 | imagePyramid[level] = temp(cv::Rect(border, border, sz.width, sz.height)); 677 | 678 | // Compute the resized image 679 | if( level != firstLevel ) 680 | { 681 | if( level < firstLevel ) 682 | cv::resize(image, imagePyramid[level], sz, 0, 0, cv::INTER_LINEAR); 683 | else 684 | cv::resize(imagePyramid[level-1], imagePyramid[level], sz, 0, 0, cv::INTER_LINEAR); 685 | 686 | cv::copyMakeBorder(imagePyramid[level], temp, border, border, border, border, 687 | cv::BORDER_REFLECT_101+cv::BORDER_ISOLATED); 688 | } 689 | else 690 | cv::copyMakeBorder(image, temp, border, border, border, border, 691 | cv::BORDER_REFLECT_101); 692 | } 693 | 694 | // Pre-compute the keypoints (we keep the best over all scales, so this has to be done beforehand 695 | std::vector < std::vector > allKeypoints; 696 | 697 | // Remove keypoints very close to the border 698 | //KeyPointsFilter::runByImageBorder(_keypoints, image.size(), edgeThreshold); 699 | 700 | // Cluster the input keypoints depending on the level they were computed at 701 | allKeypoints.resize(levelsNum); 702 | for (std::vector::iterator keypoint = _keypoints.begin(), 703 | keypointEnd = _keypoints.end(); keypoint != keypointEnd; ++keypoint) 704 | allKeypoints[keypoint->octave].push_back(*keypoint); 705 | 706 | // Make sure we rescale the coordinates 707 | for (int level = 0; level < levelsNum; ++level) 708 | { 709 | if (level == firstLevel) 710 | continue; 711 | 712 | std::vector & keypoints = allKeypoints[level]; 713 | float scale = 1/getScale(level, firstLevel, scaleFactor); 714 | for (std::vector::iterator keypoint = keypoints.begin(), 715 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 716 | keypoint->pt *= scale; 717 | } 718 | 719 | cv::Mat descriptors; 720 | std::vector pattern; 721 | 722 | int nkeypoints = 0; 723 | for (int level = 0; level < levelsNum; ++level){ 724 | std::vector& keypoints = allKeypoints[level]; 725 | cv::Mat& workingMat = imagePyramid[level]; 726 | if(keypoints.size() > 1) 727 | cv::KeyPointsFilter::runByImageBorder(keypoints, workingMat.size(), border); 728 | 729 | nkeypoints += keypoints.size(); 730 | } 731 | if( nkeypoints == 0 ) 732 | _descriptors.release(); 733 | else 734 | { 735 | _descriptors.create(nkeypoints, descriptorSize(), CV_8U); 736 | descriptors = _descriptors; 737 | } 738 | 739 | _keypoints.clear(); 740 | int offset = 0; 741 | for (int level = 0; level < levelsNum; ++level) 742 | { 743 | // preprocess the resized image 744 | cv::Mat& workingMat = imagePyramid[level]; 745 | // Get the features and compute their orientation 746 | std::vector& keypoints = allKeypoints[level]; 747 | if(keypoints.size() > 1) 748 | cv::KeyPointsFilter::runByImageBorder(keypoints, workingMat.size(), border); 749 | int nkeypoints = (int)keypoints.size(); 750 | 751 | // Compute the descriptors 752 | cv::Mat desc; 753 | if (!descriptors.empty()) 754 | { 755 | desc = descriptors.rowRange(offset, offset + nkeypoints); 756 | } 757 | 758 | offset += nkeypoints; 759 | //boxFilter(working_mat, working_mat, working_mat.depth(), Size(5,5), Point(-1,-1), true, BORDER_REFLECT_101); 760 | cv::GaussianBlur(workingMat, workingMat, cv::Size(7, 7), 2, 2, cv::BORDER_REFLECT_101); 761 | cv::Mat integral_image; 762 | cv::integral(workingMat, integral_image, CV_32S); 763 | computeDescriptors(workingMat, integral_image, patchSize, keypoints, desc, descriptorSize()); 764 | 765 | // Copy to the output data 766 | if (level != firstLevel) 767 | { 768 | float scale = getScale(level, firstLevel, scaleFactor); 769 | for (std::vector::iterator keypoint = keypoints.begin(), 770 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 771 | keypoint->pt *= scale; 772 | } 773 | // And add the keypoints to the output 774 | _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end()); 775 | } 776 | 777 | } 778 | 779 | -------------------------------------------------------------------------------- /src/lc/LoopCloser.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/lc/LoopCloser.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | LoopCloser::LoopCloser() 26 | { 27 | _params = Params::getInstance(); 28 | _st = Statistics::getInstance(); 29 | _prior.resize(_params->nimages, 0.0); 30 | } 31 | 32 | LoopCloser::~LoopCloser() 33 | { 34 | } 35 | 36 | bool LoopCloser::process(const Image& image, HighLevelMap& hmap, unsigned& loop_loc, unsigned& loop_img) 37 | { 38 | loop_loc = 0; 39 | loop_img = 0; 40 | bool response = false; 41 | 42 | std::vector lc_times; 43 | double init_time, end_time; 44 | 45 | // Inserting new hypotheses in the filter. 46 | if (_buffer.size() == _params->imageLC_disc_recent) 47 | { 48 | _filter.addElement(_buffer.front()); 49 | _buffer.pop(); 50 | } 51 | _buffer.push(int(image.image_id)); 52 | 53 | // Executing the Bayes filter. 54 | if (_filter.numElems() > 0) 55 | { 56 | // Predict 57 | // std::vector prior; 58 | init_time = omp_get_wtime(); 59 | _filter.predict(image.image_id, &_prior); 60 | end_time = omp_get_wtime(); 61 | lc_times.push_back(end_time - init_time); 62 | 63 | // Computing the likelihood (LLC and ILC). 64 | std::map lk; 65 | double llct, ilct; 66 | computeLikelihood(image, hmap, lk, llct, ilct); 67 | 68 | // Update the filter. 69 | init_time = omp_get_wtime(); 70 | _filter.update(image.image_id, lk, &_prior); 71 | end_time = omp_get_wtime(); 72 | lc_times.push_back(end_time - init_time); 73 | 74 | // Adding likelihood times. 75 | lc_times.push_back(llct); 76 | lc_times.push_back(ilct); 77 | 78 | // Obtaining best candidate for loop closing. 79 | std::vector results; 80 | _filter.getMostProbablyElements(results); 81 | if (results.size()) 82 | { 83 | BayesFilterResult bcand = results[0]; 84 | //if (bcand.score > _params->imageLC_tloop) 85 | //{ 86 | // Loading the previous image. 87 | Image prev_img; 88 | prev_img.load(_params->images[bcand.elem_id]); 89 | 90 | // Matching images and filtering them using NNDR. 91 | init_time = omp_get_wtime(); 92 | std::vector feat_matches; 93 | ratioMatching(image, prev_img, _params->match_ratio, feat_matches); 94 | 95 | // Performing the epipolar check. 96 | int num_inliers = checkEpipolarGeometry(image, prev_img, feat_matches); 97 | end_time = omp_get_wtime(); 98 | lc_times.push_back(end_time - init_time); 99 | 100 | ROS_INFO("Image Candidate: %i, Score %f, Matchings %lu, Inliers %i", bcand.elem_id, bcand.score, feat_matches.size(), num_inliers); 101 | 102 | // Taking a decision about the loop closing. 103 | if (num_inliers > _params->imageLC_min_inliers) 104 | { 105 | loop_img = unsigned(bcand.elem_id); 106 | loop_loc = hmap.im2loc[bcand.elem_id]; 107 | response = true; 108 | } 109 | //} 110 | } 111 | } 112 | 113 | _st->registerLCTimes(lc_times); 114 | 115 | return response; 116 | } 117 | 118 | void LoopCloser::computeLikelihood(const Image& image, HighLevelMap& hmap, std::map& lik, double& llc_time, double& ilc_time) 119 | { 120 | lik.clear(); 121 | 122 | double init_time = omp_get_wtime(); 123 | 124 | // --- LOCATION LOOP CLOSURE (LLC) --- 125 | // ----------------------------------- 126 | 127 | // Vector to store the distances to the locations. 128 | cv::Mat_ loc_dists = cv::Mat::zeros(1, hmap.numLocations(), CV_64F); 129 | // Vector to store if the correspondent location has a score higher than T. 130 | std::vector loc_search(hmap.numLocations(), false); 131 | loc_search[hmap.active] = true; // We always search in the current node. 132 | 133 | // Computing the distances to all locations stored in the map. 134 | #pragma omp parallel for 135 | for (unsigned loc_idx = 0; loc_idx < hmap.numLocations(); loc_idx++) 136 | { 137 | //loc_dists(0, loc_idx) = GlobalDescriptor::dist(hmap.locations[loc_idx]->desc, image.gdsc, hmap.locations[loc_idx]->invcov); 138 | loc_dists(0, loc_idx) = GlobalDescriptor::dist(hmap.locations[loc_idx]->desc, image.gdsc, _params->gdescriptor); 139 | } 140 | 141 | // Computing the minimum and maximum value. 142 | cv::Mat_ minMat, maxMat; 143 | cv::reduce(loc_dists, minMat, 1, CV_REDUCE_MIN); 144 | cv::reduce(loc_dists, maxMat, 1, CV_REDUCE_MAX); 145 | double minv = minMat(0, 0); 146 | double maxv = maxMat(0, 0); 147 | 148 | // Converting the distances to similarities. 149 | if (hmap.numLocations() > 1) 150 | { 151 | #pragma omp parallel for 152 | for (unsigned loc_idx = 0; loc_idx < hmap.numLocations(); loc_idx++) 153 | { 154 | loc_dists(0, loc_idx) = 1.0 - ((loc_dists(0, loc_idx) - minv) / (maxv - minv)); 155 | if (loc_dists(0, loc_idx) > _params->locLC_thresh) 156 | { 157 | loc_search[loc_idx] = true; 158 | } 159 | } 160 | } 161 | else 162 | { 163 | loc_dists(0, 0) = 1.0; 164 | } 165 | 166 | double end_time = omp_get_wtime(); 167 | llc_time = end_time - init_time; 168 | 169 | init_time = omp_get_wtime(); 170 | 171 | // --- IMAGE LOOP CLOSURE (ILC) --- 172 | // -------------------------------- 173 | 174 | // Cache for searching for results in nodes. 175 | std::map > loc_imscores; 176 | 177 | // For each image that needs a likelihood to be updated in the filter. 178 | std::vector* elems = _filter.getElements(); 179 | for (unsigned elem_idx = 0; elem_idx < elems->size(); elem_idx++) 180 | { 181 | int image_id = elems->at(elem_idx); 182 | unsigned imloc = hmap.im2loc[image_id]; 183 | 184 | if (loc_search[imloc]) 185 | { 186 | // Higher than the threshold. 187 | // If we have not searched images in the node, we search and store the result. 188 | if (loc_imscores.count(imloc) == 0) 189 | { 190 | // Search for similar images. 191 | std::map image_matches; 192 | hmap.locations[imloc]->searchImages(image.dscs, image_matches, _params->match_ratio); 193 | loc_imscores[imloc] = image_matches; 194 | } 195 | 196 | // Establishing the likelihood. 197 | lik[image_id] = loc_dists(0, imloc) * loc_imscores[imloc][image_id]; 198 | // std::cout << "Likelihood for image " << image_id << " Node " << imloc << ": " << loc_dists(0, imloc) << " * " << loc_imscores[imloc][image_id] << ": " << lik[image_id] << std::endl; 199 | } 200 | else 201 | { 202 | // We do not need to search in the location. 203 | lik[image_id] = 0; 204 | // std::cout << "Likelihood for image " << image_id << " Node " << imloc << ": " << 0 << std::endl; 205 | } 206 | } 207 | 208 | end_time = omp_get_wtime(); 209 | ilc_time = end_time - init_time; 210 | } 211 | 212 | } 213 | -------------------------------------------------------------------------------- /src/map/HighLevelMap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/map/HighLevelMap.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | HighLevelMap::HighLevelMap() : 26 | active(-1) 27 | { 28 | } 29 | 30 | unsigned HighLevelMap::addLocation(Location* loc) 31 | { 32 | locations.push_back(loc); 33 | graph.addNode(); 34 | loc->id = locations.size() - 1; 35 | 36 | return loc->id; 37 | } 38 | 39 | void HighLevelMap::addImageToLocation(const unsigned idloc, const int id, const std::string image_filename) 40 | { 41 | locations[idloc]->addImage(id, image_filename); 42 | im2loc[id] = idloc; 43 | } 44 | 45 | void HighLevelMap::setActive(const unsigned& act) 46 | { 47 | active = act; 48 | } 49 | 50 | void HighLevelMap::linkLocations(const unsigned& a, const unsigned& b, const double& weight) 51 | { 52 | graph.adjlist[a].push_back(neighbor(b, weight)); 53 | graph.adjlist[b].push_back(neighbor(a, weight)); 54 | } 55 | 56 | unsigned HighLevelMap::numLocations() 57 | { 58 | return locations.size(); 59 | } 60 | 61 | Location* HighLevelMap::getActiveLocation() 62 | { 63 | return locations[active]; 64 | } 65 | 66 | } 67 | -------------------------------------------------------------------------------- /src/map/Location.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/map/Location.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | Location::Location(const int& max_images, const int& feat_per_img, const int& desc_bytes) : 26 | init(false), 27 | id(0), 28 | _bindex(0), 29 | _params(0) 30 | { 31 | vi::BinaryIndexParams bparams; 32 | bparams.max_descriptors = (max_images * feat_per_img * 3); 33 | bparams.desc_bytes = desc_bytes; 34 | _bindex = new vi::BinaryIndex(bparams); 35 | _params = Params::getInstance(); 36 | } 37 | 38 | Location::~Location() 39 | { 40 | delete _bindex; 41 | } 42 | 43 | void Location::addImage(const int& id, const std::string& image_filename) 44 | { 45 | if (!init) 46 | { 47 | initializeLocation(id, image_filename); 48 | init = true; 49 | } 50 | else 51 | { 52 | Image curr_img; 53 | curr_img.load(image_filename); 54 | 55 | LocationEntry locent(id, image_filename); 56 | images.push_back(locent); 57 | 58 | if (images.size() < _params->loc_max_images) 59 | { 60 | 61 | // Updating the representative of the location 62 | // Copying the descriptor 63 | gdescriptors.push_back(curr_img.gdsc); 64 | 65 | // Computing the mean and the inv cov matrix. 66 | updateGlobalDescription(); 67 | 68 | // Matching the images 69 | std::vector > matches_feats; 70 | _bindex->search(curr_img.dscs, matches_feats, 2); 71 | 72 | // Filtering matches 73 | std::vector matches; 74 | for (unsigned m = 0; m < matches_feats.size(); m++) 75 | { 76 | if (matches_feats[m][0].distance < matches_feats[m][1].distance * 0.8) 77 | { 78 | matches.push_back(matches_feats[m][0]); 79 | } 80 | } 81 | 82 | // Updating the index 83 | _bindex->update(int(images.size() - 1), curr_img.kps, curr_img.dscs, matches); 84 | } 85 | else 86 | { 87 | // Matching the images 88 | std::vector > matches_feats; 89 | _bindex->search(curr_img.dscs, matches_feats, 2); 90 | 91 | // Filtering matches 92 | std::vector matches; 93 | for (unsigned m = 0; m < matches_feats.size(); m++) 94 | { 95 | if (matches_feats[m][0].distance < matches_feats[m][1].distance * 0.8) 96 | { 97 | matches.push_back(matches_feats[m][0]); 98 | } 99 | } 100 | 101 | _bindex->addToInvertedIndex(int(images.size() - 1), curr_img.kps, matches); 102 | } 103 | } 104 | } 105 | 106 | void Location::searchImages(const cv::Mat& dscs, std::map& image_matches, double match_ratio) 107 | { 108 | 109 | // Matching the current descriptors against the index 110 | std::vector > matches_feats; 111 | _bindex->search(dscs, matches_feats, 2); 112 | 113 | // Filtering the matchings 114 | std::vector matches; 115 | for (unsigned m = 0; m < matches_feats.size(); m++) 116 | { 117 | if (matches_feats[m][0].distance < matches_feats[m][1].distance * match_ratio) 118 | { 119 | matches.push_back(matches_feats[m][0]); 120 | } 121 | } 122 | 123 | // Getting similar images according to the index and filtering the results. 124 | std::vector imm; 125 | _bindex->getSimilarImages(dscs, matches, imm); 126 | 127 | #pragma omp parallel for 128 | for (unsigned i = 0; i < imm.size(); i++) 129 | { 130 | // Translating the index to the real image identifier. 131 | imm[i].image_id = images[imm[i].image_id].image_id; 132 | 133 | #pragma omp critical(IMMATCH) 134 | { 135 | image_matches[imm[i].image_id] = imm[i].score; 136 | } 137 | } 138 | } 139 | 140 | void Location::initializeLocation(const int& id, const std::string& image_filename) 141 | { 142 | LocationEntry locent(id, image_filename); 143 | 144 | images.push_back(locent); 145 | 146 | // Loading the image. 147 | Image img; 148 | img.load(image_filename); 149 | 150 | // Coyping the descriptor 151 | img.gdsc.copyTo(gdescriptors); 152 | int dsize = _params->gdescriptor->getDescSize(); 153 | desc = cv::Mat::zeros(1, dsize, CV_64F); 154 | invcov = cv::Mat::zeros(dsize, dsize, CV_64F); 155 | 156 | // Computing the mean and the inv cov matrix. 157 | updateGlobalDescription(); 158 | 159 | // Adding the image to the index. 160 | _bindex->add(0, img.kps, img.dscs); 161 | } 162 | 163 | void Location::updateGlobalDescription() 164 | { 165 | int dsize = _params->gdescriptor->getDescSize(); 166 | for (int i = 0; i < dsize; i++) 167 | { 168 | cv::Mat col = gdescriptors.col(i); 169 | 170 | cv::Scalar meanV, stdevV; 171 | cv::meanStdDev(col, meanV, stdevV); 172 | 173 | double mean = (double)meanV[0]; 174 | double stde = (double)stdevV[0]; 175 | 176 | desc(0, i) = mean; 177 | if (stde > 0) 178 | { 179 | invcov(i, i) = 1.0 / stde; 180 | } 181 | } 182 | } 183 | 184 | unsigned Location::numImages() 185 | { 186 | return images.size(); 187 | } 188 | 189 | } 190 | -------------------------------------------------------------------------------- /src/nodes/HTMap_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/HTMap.h" 21 | 22 | int main(int argc, char** argv) 23 | { 24 | ros::init(argc, argv, "htmap"); 25 | ros::NodeHandle nh_private("~"); 26 | htmap::HTMap htmap(nh_private); 27 | ros::spin(); 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /src/util/Params.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/util/Params.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | Params* Params::_instance = 0; 26 | 27 | Params* Params::getInstance() 28 | { 29 | if (_instance == 0) 30 | { 31 | _instance = new Params; 32 | } 33 | return _instance; 34 | } 35 | 36 | void Params::readParams(const ros::NodeHandle& nh) 37 | { 38 | nh.param("images_dir", dir_images, ""); 39 | ROS_INFO("[Params] Image directory: %s", dir_images.c_str()); 40 | 41 | nh.param("load_feats", load_features, false); 42 | ROS_INFO("[Params] Load features: %i", load_features ? 1 : 0); 43 | 44 | getFilenames(dir_images, filenames); 45 | nimages = filenames.size(); 46 | ROS_INFO("[Params] %u images found", nimages); 47 | 48 | nh.param("working_dir", dir_results, ""); 49 | ROS_INFO("[Params] Working directory: %s", dir_results.c_str()); 50 | 51 | // Configuring the statistics directory. 52 | Statistics* st = htmap::Statistics::getInstance(); 53 | st->init(nimages); 54 | 55 | std::string match_method_st; 56 | nh.param("match_method", match_method_st, "ratio"); 57 | if (match_method_st == "ratio") 58 | match_method = 1; 59 | else if (match_method_st == "crosscheck") 60 | match_method = 0; 61 | ROS_INFO("[Params] Matching Method %s", match_method_st.c_str()); 62 | 63 | nh.param("match_ratio", match_ratio, 0.8); 64 | ROS_INFO("[Params] Matching Ratio %f", match_ratio); 65 | 66 | nh.param("detector", detector_name, "FAST"); 67 | ROS_INFO("[Params] Keypoint Detector: %s", detector_name.c_str()); 68 | detector = KeypointDetector::create(detector_name, det_params); 69 | 70 | nh.param("max_total_kps", max_total_kps, 1000); 71 | ROS_INFO("[Params] Total Features: %i", max_total_kps); 72 | 73 | nh.param("descriptor", descriptor_name, "BRIEF"); 74 | ROS_INFO("[Params] Keypoint Descriptor: %s", descriptor_name.c_str()); 75 | descriptor = KeypointDescriptor::create(descriptor_name, des_params); 76 | 77 | nh.param("gdescriptor", gdescriptor_name, "WI-SIFT"); 78 | ROS_INFO("[Params] Global Descriptor: %s", gdescriptor_name.c_str()); 79 | gdescriptor = GlobalDescriptor::create(gdescriptor_name, gdes_params); 80 | 81 | nh.param("locLC_thresh", locLC_thresh, 0.6); 82 | ROS_INFO("[Params] Location LC Threshold: %f", locLC_thresh); 83 | 84 | nh.param("imageLC_min_inliers", imageLC_min_inliers, 50); 85 | ROS_INFO("[Params] Image LC Minimum Number of Inliers: %i", imageLC_min_inliers); 86 | 87 | nh.param("imageLC_tloop", imageLC_tloop, 0.75); 88 | ROS_INFO("[Params] Image LC Minimum Threshold for LC: %f", imageLC_tloop); 89 | 90 | nh.param("imageLC_disc_recent", imageLC_disc_recent, 70); 91 | ROS_INFO("[Params] Number of Recent Images Discarded: %i", imageLC_disc_recent); 92 | 93 | nh.param("loc_max_images", loc_max_images, 300); 94 | ROS_INFO("[Params] Max Images per Location: %i", loc_max_images); 95 | 96 | nh.param("max_sim_newnode", max_sim_newnode, 0.20); 97 | ROS_INFO("[Params] Max Similarity for a New Node: %f", max_sim_newnode); 98 | 99 | nh.param("batch", batch, false); 100 | ROS_INFO("[Params] Batch Process: %i", batch ? 1 : 0); 101 | 102 | nh.param("inliers_begin", inliers_begin, 20); 103 | ROS_INFO("[Params] Batch Inliers Begin: %i", inliers_begin); 104 | 105 | nh.param("inliers_end", inliers_end, 200); 106 | ROS_INFO("[Params] Batch Inliers End: %i", inliers_end); 107 | 108 | nh.param("inliers_step", inliers_step, 15); 109 | ROS_INFO("[Params] Batch Inliers Step: %i", inliers_step); 110 | } 111 | 112 | } 113 | -------------------------------------------------------------------------------- /src/util/Statistics.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | 21 | #include "htmap/util/Statistics.h" 22 | 23 | namespace htmap 24 | { 25 | 26 | Statistics* Statistics::_instance = 0; 27 | 28 | Statistics* Statistics::getInstance() 29 | { 30 | if (_instance == 0) 31 | { 32 | _instance = new Statistics; 33 | } 34 | return _instance; 35 | } 36 | 37 | void Statistics::init(const int nimages) 38 | { 39 | loops = cv::Mat::zeros(nimages, nimages, CV_32S); 40 | prior = cv::Mat::zeros(nimages, nimages, CV_64F); 41 | likelihood = cv::Mat::zeros(nimages, nimages, CV_64F); 42 | posterior = cv::Mat::zeros(nimages, nimages, CV_64F); 43 | locations.resize(nimages, 0); 44 | } 45 | 46 | void Statistics::registerLoop(const int a, const int b) 47 | { 48 | loops(a, b) = 1; 49 | } 50 | 51 | void Statistics::registerPrior(const int imid, const std::vector& _prior) 52 | { 53 | for (int i = 0; i < _prior.size(); i++) 54 | { 55 | prior(imid, i) = _prior[i]; 56 | } 57 | } 58 | 59 | void Statistics::registerLikelihood(const int imid, const std::vector& _lik) 60 | { 61 | for (int i = 0; i < _lik.size(); i++) 62 | { 63 | likelihood(imid, i) = _lik[i]; 64 | } 65 | } 66 | 67 | void Statistics::registerPosterior(const int imid, const std::vector& _post) 68 | { 69 | for (int i = 0; i < _post.size(); i++) 70 | { 71 | posterior(imid, i) = _post[i]; 72 | } 73 | } 74 | 75 | void Statistics::registerImageToLocation(const int imid, const unsigned loc) 76 | { 77 | locations[imid] = loc; 78 | } 79 | 80 | void Statistics::registerDescTimes(double gdesc, double det, double desc) 81 | { 82 | boost::mutex::scoped_lock lock(mlock); 83 | 84 | std::vector times; 85 | times.push_back(gdesc); 86 | times.push_back(det); 87 | times.push_back(desc); 88 | 89 | desc_times.push_back(times); 90 | } 91 | 92 | void Statistics::registerLCTimes(std::vector& values) 93 | { 94 | lc_times.push_back(values); 95 | } 96 | 97 | void Statistics::writeResults(const std::string& dir, int inliers) 98 | { 99 | std::string loops_results_filename = dir + "htmap_loops_" + SSTR(inliers) + ".txt"; 100 | std::string prior_results_filename = dir + "htmap_prior_" + SSTR(inliers) + ".txt"; 101 | std::string likelihood_results_filename = dir + "htmap_likelihood_" + SSTR(inliers) + ".txt"; 102 | std::string posterior_results_filename = dir + "htmap_posterior_" + SSTR(inliers) + ".txt"; 103 | std::string img2loc_results_filename = dir + "htmap_img2loc_" + SSTR(inliers) + ".txt"; 104 | 105 | std::ofstream loops_file, prior_file, lik_file, post_file, im2loc_file; 106 | 107 | loops_file.open(loops_results_filename.c_str(), std::ios::out | std::ios::trunc); 108 | prior_file.open(prior_results_filename.c_str(), std::ios::out | std::ios::trunc); 109 | lik_file.open(likelihood_results_filename.c_str(), std::ios::out | std::ios::trunc); 110 | post_file.open(posterior_results_filename.c_str(), std::ios::out | std::ios::trunc); 111 | im2loc_file.open(img2loc_results_filename.c_str(), std::ios::out | std::ios::trunc); 112 | 113 | for (int i = 0; i < loops.rows; i++) 114 | { 115 | for (int j = 0; j < loops.cols; j++) 116 | { 117 | if(loops(i, j) == 1) { 118 | loops_file << j << "\t"; 119 | } 120 | // Uncomment these lines if you want to save the full Bayes filter info 121 | 122 | //prior_file << prior(i, j) << "\t"; 123 | //lik_file << likelihood(i, j) << "\t"; 124 | //post_file << posterior(i, j) << "\t"; 125 | } 126 | im2loc_file << i << "\t" << locations[i] << std::endl; 127 | 128 | loops_file << std::endl; 129 | // Uncomment these lines if you want to save the full Bayes filter info 130 | 131 | //prior_file << std::endl; 132 | //lik_file << std::endl; 133 | //post_file << std::endl; 134 | } 135 | 136 | loops_file.close(); 137 | prior_file.close(); 138 | lik_file.close(); 139 | post_file.close(); 140 | im2loc_file.close(); 141 | 142 | // Description times 143 | std::string dtimes_results_filename = dir + "htmap_dtimes_" + SSTR(inliers) + ".txt"; 144 | std::ofstream dtimes_file; 145 | dtimes_file.open(dtimes_results_filename.c_str(), std::ios::out | std::ios::trunc); 146 | for (unsigned i = 0; i < desc_times.size(); i++) 147 | { 148 | for (unsigned j = 0; j < desc_times[i].size(); j++) 149 | { 150 | dtimes_file << desc_times[i][j] << "\t"; 151 | } 152 | 153 | dtimes_file << std::endl; 154 | } 155 | 156 | dtimes_file.close(); 157 | 158 | // LC times 159 | std::string lc_results_filename = dir + "htmap_lc_" + SSTR(inliers) + ".txt"; 160 | std::ofstream lc_file; 161 | lc_file.open(lc_results_filename.c_str(), std::ios::out | std::ios::trunc); 162 | for (unsigned i = 0; i < lc_times.size(); i++) 163 | { 164 | for (unsigned j = 0; j < lc_times[i].size(); j++) 165 | { 166 | lc_file << lc_times[i][j] << "\t"; 167 | } 168 | 169 | lc_file << std::endl; 170 | } 171 | 172 | lc_file.close(); 173 | } 174 | 175 | } 176 | -------------------------------------------------------------------------------- /src/util/Util.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of htmap. 3 | * 4 | * Copyright (C) 2018 Emilio Garcia-Fidalgo (University of the Balearic Islands) 5 | * 6 | * htmap is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * htmap is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with htmap. If not, see . 18 | */ 19 | 20 | #include "htmap/util/Util.h" 21 | 22 | namespace htmap 23 | { 24 | 25 | static const uchar popCountTable[] = 26 | { 27 | 0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4, 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 28 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 29 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 30 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 31 | 1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5, 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 32 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 33 | 2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6, 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 34 | 3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7, 4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8 35 | }; 36 | 37 | void ratioMatching(const Image& query, const Image& train, const double& ratio, std::vector& matches) 38 | { 39 | matches.clear(); 40 | cv::BFMatcher matcher(cv::NORM_HAMMING); 41 | 42 | // Matching descriptors. 43 | std::vector > matches12; 44 | matcher.knnMatch(query.dscs, train.dscs, matches12, 2); 45 | 46 | // Filtering the resulting matchings according to the given ratio. 47 | for (unsigned m = 0; m < matches12.size(); m++) 48 | { 49 | if (matches12[m][0].distance <= matches12[m][1].distance * ratio) 50 | { 51 | matches.push_back(matches12[m][0]); 52 | } 53 | } 54 | } 55 | 56 | int checkEpipolarGeometry(const Image& query, const Image& train, const std::vector& matches) 57 | { 58 | // Convert matchings into Point2f. 59 | std::vector points1, points2; 60 | for (std::vector::const_iterator it = matches.begin(); it != matches.end(); it++) 61 | { 62 | // Get the position of query keypoints 63 | float x = query.kps[it->queryIdx].pt.x; 64 | float y = query.kps[it->queryIdx].pt.y; 65 | points1.push_back(cv::Point2f(x,y)); 66 | 67 | // Get the position of train keypoints 68 | x = train.kps[it->trainIdx].pt.x; 69 | y = train.kps[it->trainIdx].pt.y; 70 | points2.push_back(cv::Point2f(x,y)); 71 | } 72 | 73 | // Computing the fundamental matrix. 74 | std::vector inliers(points1.size(), 0); 75 | if (points1.size() > 7) 76 | { 77 | cv::findFundamentalMat( 78 | cv::Mat(points1), cv::Mat(points2), // Matching points 79 | CV_FM_RANSAC, // RANSAC method 80 | 3.0, // Distance to epipolar line 81 | 0.985, // Confidence probability 82 | inliers); // Match status (inlier or outlier) 83 | } 84 | 85 | // Extract the surviving (inliers) matches 86 | std::vector::const_iterator it = inliers.begin(); 87 | int total_inliers = 0; 88 | for (; it != inliers.end(); ++it) 89 | { 90 | if (*it) total_inliers++; 91 | } 92 | 93 | return total_inliers; 94 | } 95 | 96 | double distEuclidean(const cv::Mat& a, const cv::Mat& b) 97 | { 98 | return cv::norm(a, b, cv::NORM_L2); 99 | } 100 | 101 | double distMahalanobis(const cv::Mat& a, const cv::Mat& b, const cv::Mat& icovar) 102 | { 103 | return cv::Mahalanobis(a, b, icovar); 104 | } 105 | 106 | int distHamming(const uchar* a, const uchar* b, const int n) 107 | { 108 | int i = 0; 109 | int result = 0; 110 | 111 | for( ; i <= n - 4; i += 4 ) 112 | { 113 | result += popCountTable[a[i] ^ b[i]] + 114 | popCountTable[a[i+1] ^ b[i+1]] + 115 | popCountTable[a[i+2] ^ b[i+2]] + 116 | popCountTable[a[i+3] ^ b[i+3]]; 117 | } 118 | 119 | for( ; i < n; i++ ) 120 | { 121 | result += popCountTable[a[i] ^ b[i]]; 122 | } 123 | 124 | return result; 125 | } 126 | 127 | void getFilenames(const std::string& directory, std::vector& filenames, bool images) 128 | { 129 | using namespace boost::filesystem; 130 | 131 | filenames.clear(); 132 | path dir(directory); 133 | 134 | // Retrieving, sorting and filtering filenames. 135 | std::vector entries; 136 | copy(directory_iterator(dir), directory_iterator(), back_inserter(entries)); 137 | sort(entries.begin(), entries.end()); 138 | for (std::vector::const_iterator it(entries.begin()); it != entries.end(); ++it) 139 | { 140 | std::string ext = it->extension().c_str(); 141 | std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); 142 | 143 | if (images) 144 | { 145 | if (ext == ".png" || ext == ".jpg" || ext == ".ppm") 146 | { 147 | filenames.push_back(it->string()); 148 | } 149 | } 150 | else 151 | { 152 | if (ext == ".yml") 153 | { 154 | filenames.push_back(it->string()); 155 | } 156 | } 157 | } 158 | } 159 | 160 | double distChiSquare(const cv::Mat& a, const cv::Mat& b) 161 | { 162 | int sz = a.cols; 163 | cv::Mat_ chsvals = cv::Mat::zeros(1, sz, CV_64F); 164 | 165 | #pragma omp parallel for 166 | for (int i = 0; i < sz; i++) 167 | { 168 | double hi = a.at(0, i); 169 | double hj = b.at(0, i); 170 | chsvals(0, i) = ((hi - hj) * (hi - hj)) / (hi + hj); 171 | } 172 | 173 | cv::Mat sumMat; 174 | cv::reduce(chsvals, sumMat, 1, CV_REDUCE_SUM); 175 | return sumMat.at(0, 0); 176 | } 177 | 178 | double distBhattacharyya(const cv::Mat& a, const cv::Mat& b) 179 | { 180 | int sz = a.cols; 181 | cv::Mat_ vals = cv::Mat::zeros(1, sz, CV_64F); 182 | 183 | #pragma omp parallel for 184 | for (int i = 0; i < sz; i++) 185 | { 186 | double hi = a.at(0, i); 187 | double hj = b.at(0, i); 188 | vals(0, i) = sqrt(hi * hj); 189 | } 190 | 191 | cv::Mat sumMat; 192 | cv::reduce(vals, sumMat, 1, CV_REDUCE_SUM); 193 | return sqrt(1.0 - sumMat.at(0, 0)); 194 | } 195 | 196 | } 197 | --------------------------------------------------------------------------------