├── .gitignore
├── package.xml
├── include
    └── libhaloc
    │   ├── state.h
    │   ├── publisher.h
    │   └── hash.h
├── LICENSE.md
├── CMakeLists.txt
├── src
    ├── publisher.cpp
    └── hash.cpp
├── README.md
└── examples
    └── lip6indoor_dataset.cpp


/.gitignore:
--------------------------------------------------------------------------------
1 | data/
2 | launch/evaluation.launch
3 | *.jpg
4 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>libhaloc</name>
 4 |   <version>0.0.1</version>
 5 |   <description>
 6 |     Hash-Based Loop Closing
 7 |   </description>
 8 |   <author email="pl.negre@uib.cat">Pep Lluis Negre</author>
 9 |   <maintainer email="pl.negre@uib.cat">Pep Lluis Negre</maintainer>
10 |   <license>BSD</license>
11 |   <url type="website">https://github.com/srv/haloc</url>
12 |   <url type="bugtracker">https://github.com/srv/haloc/issues</url>
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 | 
16 |   <build_depend>roscpp</build_depend>
17 |   <build_depend>roslib</build_depend>
18 |   <build_depend>tf</build_depend>
19 |   <build_depend>cv_bridge</build_depend>
20 |   <build_depend>image_geometry</build_depend>
21 |   <build_depend>std_msgs</build_depend>
22 |   <build_depend>cmake_modules</build_depend>
23 | 
24 |   <run_depend>roscpp</run_depend>
25 |   <run_depend>roslib</run_depend>
26 |   <run_depend>tf</run_depend>
27 |   <run_depend>cv_bridge</run_depend>
28 |   <run_depend>image_geometry</run_depend>
29 |   <run_depend>std_msgs</run_depend>
30 | 
31 | </package>
32 | 


--------------------------------------------------------------------------------
/include/libhaloc/state.h:
--------------------------------------------------------------------------------
 1 | //  Copyright (c) 2017 Universitat de les Illes Balears
 2 | //  This file is part of LIBHALOC.
 3 | //
 4 | //  LIBHALOC is free software: you can redistribute it and/or modify
 5 | //  it under the terms of the GNU General Public License as published by
 6 | //  the Free Software Foundation, either version 3 of the License, or
 7 | //  (at your option) any later version.
 8 | //
 9 | //  LIBHALOC is distributed in the hope that it will be useful,
10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | //  GNU General Public License for more details.
13 | //
14 | //  You should have received a copy of the GNU General Public License
15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
16 | 
17 | #ifndef LIBHALOC_INCLUDE_LIBHALOC_STATE_H_
18 | #define LIBHALOC_INCLUDE_LIBHALOC_STATE_H_
19 | 
20 | #include <Eigen/Eigen>
21 | #include <Eigen/Dense>
22 | 
23 | #include <vector>
24 | 
25 | #include <opencv2/core/core.hpp>
26 | #include <opencv2/features2d/features2d.hpp>
27 | 
28 | namespace haloc {
29 | 
30 | /**
31 |  * @brief      Struct to save operational variables after every hash
32 |  *             computation.
33 |  */
34 | struct State {
35 |   /**
36 |    * @brief      Default constructor.
37 |    */
38 |   State();
39 | 
40 |   void Clear() {
41 |     bucketed_kp.clear();
42 |     unbucketed_kp.clear();
43 |     num_kp_per_bucket.clear();
44 |   }
45 | 
46 |   // State variables
47 |   std::vector<cv::KeyPoint> bucketed_kp;    //!> Stores the bucketed keypoints
48 |   std::vector<cv::KeyPoint> unbucketed_kp;  //!> Stores the discarded keypoints when bucketing
49 |   std::vector<int> num_kp_per_bucket;       //!> The number of keypoints per bucket
50 | };
51 | 
52 | }  // namespace haloc
53 | 
54 | #endif  // LIBHALOC_INCLUDE_LIBHALOC_STATE_H_
55 | 


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


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(libhaloc)
 3 | 
 4 | # C++11 support
 5 | include(CheckCXXCompilerFlag)
 6 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
 7 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
 8 | if(COMPILER_SUPPORTS_CXX11)
 9 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
10 | elseif(COMPILER_SUPPORTS_CXX0X)
11 |   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
12 | else()
13 |   message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
14 | endif()
15 | 
16 | find_package(catkin REQUIRED COMPONENTS
17 |     roscpp
18 |     roslib
19 |     tf
20 |     cv_bridge
21 |     image_geometry
22 |     std_msgs
23 |     cmake_modules)
24 | 
25 | # Dependencies - Eigen:
26 | find_package(Eigen3 REQUIRED)
27 | include_directories(${EIGEN3_INCLUDE_DIRS})
28 | 
29 | # Dependencies - OpenCV:
30 | find_package(OpenCV REQUIRED)
31 | if (${OpenCV_VERSION} MATCHES "3.3.1")
32 |   foreach(__cvcomponent ${OpenCV_LIB_COMPONENTS})
33 |     set (__original_cvcomponent ${__cvcomponent})
34 |     if(NOT __cvcomponent MATCHES "^opencv_")
35 |       set(__cvcomponent opencv_${__cvcomponent})
36 |     endif()
37 |     if (TARGET ${__cvcomponent})
38 |       set_target_properties(${__cvcomponent} PROPERTIES
39 |           MAP_IMPORTED_CONFIG_DEBUG ""
40 |           MAP_IMPORTED_CONFIG_RELEASE ""
41 |           MAP_IMPORTED_CONFIG_RELWITHDEBINFO ""
42 |           MAP_IMPORTED_CONFIG_MINSIZEREL ""
43 |       )
44 |     endif()
45 |   endforeach(__cvcomponent)
46 | endif()
47 | include_directories(${OpenCV_INCLUDE_DIRS})
48 | link_directories(${OpenCV_LIBRARY_DIRS})
49 | 
50 | catkin_package(INCLUDE_DIRS include LIBRARIES haloc)
51 | 
52 | # Add the Image Hashing library
53 | add_library(haloc
54 |             src/hash.cpp
55 |             src/publisher.cpp)
56 | target_link_libraries(haloc
57 |     ${Boost_LIBRARIES}
58 |     ${EIGEN3_LIBRARIES}
59 |     ${OpenCV_LIBRARIES}
60 |     ${catkin_LIBRARIES})
61 | 
62 | # Include directories
63 | include_directories(${catkin_INCLUDE_DIRS} include)
64 | 
65 | # Add examples
66 | add_executable(lip6indoor_dataset
67 |   examples/lip6indoor_dataset.cpp)
68 | target_link_libraries(lip6indoor_dataset
69 |   ${catkin_LIBRARIES}
70 |   ${OpenCV_LIBRARIES}
71 |   haloc)
72 | 
73 | 


--------------------------------------------------------------------------------
/include/libhaloc/publisher.h:
--------------------------------------------------------------------------------
 1 | //  Copyright (c) 2017 Universitat de les Illes Balears
 2 | //  This file is part of LIBHALOC.
 3 | //
 4 | //  LIBHALOC is free software: you can redistribute it and/or modify
 5 | //  it under the terms of the GNU General Public License as published by
 6 | //  the Free Software Foundation, either version 3 of the License, or
 7 | //  (at your option) any later version.
 8 | //
 9 | //  LIBHALOC is distributed in the hope that it will be useful,
10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | //  GNU General Public License for more details.
13 | //
14 | //  You should have received a copy of the GNU General Public License
15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
16 | 
17 | #ifndef LIBHALOC_INCLUDE_LIBHALOC_PUBLISHER_H_
18 | #define LIBHALOC_INCLUDE_LIBHALOC_PUBLISHER_H_
19 | 
20 | #include <ros/ros.h>
21 | #include <sensor_msgs/Image.h>
22 | #include <cv_bridge/cv_bridge.h>
23 | #include <std_msgs/String.h>
24 | 
25 | #include <vector>
26 | 
27 | #include "libhaloc/state.h"
28 | 
29 | #include <opencv2/core/core.hpp>
30 | #include <opencv2/features2d/features2d.hpp>
31 | 
32 | namespace haloc {
33 | 
34 | class Publisher {
35 |  public:
36 |   /**
37 |    * @brief      Empty class constructor.
38 |    */
39 |   Publisher();
40 | 
41 |   /**
42 |    * @brief      Publishes the bucketed image.
43 |    *
44 |    * @param[in]  state        The state obtained after a hash computation.
45 |    * @param[in]  img          The original image.
46 |    * @param[in]  bucket_rows  The bucket rows
47 |    * @param[in]  bucket_cols  The bucket cols
48 |    */
49 |   void PublishBucketedImage(const State& state, const cv::Mat& img,
50 |     const int& bucket_rows, const int& bucket_cols);
51 | 
52 |   /**
53 |    * @brief      Publishes the bucketed info
54 |    *
55 |    * @param[in]  state        The state obtained after a hash computation.
56 |    * @param[in]  max_feat     The maximum number of features per bucket
57 |    */
58 |   void PublishBucketedInfo(const State& state, const int& max_feat);
59 | 
60 |  protected:
61 |   /**
62 |    * @brief      Returns a debug image with the bucketed keypoints.
63 |    *
64 |    * @param[in]  state        The state obtained after a hash computation.
65 |    * @param[in]  img          The original image.
66 |    * @param[in]  bucket_rows  The bucket rows
67 |    * @param[in]  bucket_cols  The bucket cols
68 |    *
69 |    * @return     The bucketed image.
70 |    */
71 |   cv::Mat BuildBucketedImage(const State& state, const cv::Mat& img,
72 |     const int& bucket_rows, const int& bucket_cols);
73 | 
74 |  private:
75 | 
76 |   // The ROS publishers
77 |   ros::Publisher pub_bucketed_img_;
78 |   ros::Publisher pub_bucketed_info_;
79 | 
80 | };
81 | 
82 | }  // namespace haloc
83 | 
84 | #endif  // LIBHALOC_INCLUDE_LIBHALOC_PUBLISHER_H_
85 | 


--------------------------------------------------------------------------------
/src/publisher.cpp:
--------------------------------------------------------------------------------
 1 | //  Copyright (c) 2017 Universitat de les Illes Balears
 2 | //  This file is part of LIBHALOC.
 3 | //
 4 | //  LIBHALOC is free software: you can redistribute it and/or modify
 5 | //  it under the terms of the GNU General Public License as published by
 6 | //  the Free Software Foundation, either version 3 of the License, or
 7 | //  (at your option) any later version.
 8 | //
 9 | //  LIBHALOC is distributed in the hope that it will be useful,
10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 | //  GNU General Public License for more details.
13 | //
14 | //  You should have received a copy of the GNU General Public License
15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
16 | 
17 | #include <ros/ros.h>
18 | 
19 | #include "libhaloc/publisher.h"
20 | 
21 | haloc::State::State() {}
22 | 
23 | haloc::Publisher::Publisher() {
24 |   ros::NodeHandle nhp("~");
25 |   pub_bucketed_img_  = nhp.advertise<sensor_msgs::Image>("bucketed_image", 2, true);
26 |   pub_bucketed_info_ = nhp.advertise<std_msgs::String>("bucketed_info", 2, true);
27 | }
28 | 
29 | void haloc::Publisher::PublishBucketedImage(const State& state,
30 |     const cv::Mat& img, const int& bucket_rows, const int& bucket_cols) {
31 |   cv::Mat bucketed_img = BuildBucketedImage(state, img, bucket_rows,
32 |     bucket_cols);
33 |   cv_bridge::CvImage ros_image;
34 |   ros_image.image = bucketed_img.clone();
35 |   ros_image.header.stamp = ros::Time::now();
36 |   ros_image.encoding = "bgr8";
37 |   pub_bucketed_img_.publish(ros_image.toImageMsg());
38 | }
39 | 
40 | void haloc::Publisher::PublishBucketedInfo(const State& state,
41 |     const int& max_feat) {
42 |   std::stringstream info;
43 |   info << std::endl;
44 |   for (uint i=0; i < state.num_kp_per_bucket.size(); ++i) {
45 |     info << "Bucket " << i+1 << ": " << state.num_kp_per_bucket[i] << "/" <<
46 |       max_feat << std::endl;
47 |   }
48 |   std_msgs::String msg;
49 |   msg.data = info.str();
50 |   pub_bucketed_info_.publish(msg);
51 | }
52 | 
53 | cv::Mat haloc::Publisher::BuildBucketedImage(const State& state,
54 |     const cv::Mat& img, const int& bucket_rows, const int& bucket_cols) {
55 |   cv::Mat out_img = img;
56 | 
57 |   float bucket_width  = img.cols / bucket_cols;
58 |   float bucket_height = img.rows / bucket_rows;
59 | 
60 |   // Draw the bucket lines
61 |   int h_point = static_cast<int>(bucket_width);
62 |   while (h_point < img.cols) {
63 |     cv::line(out_img, cv::Point(h_point, 0), cv::Point(h_point, img.rows), cv::Scalar(47, 47, 47), 2, 8);
64 |     h_point += static_cast<int>(bucket_width);
65 |   }
66 |   int v_point = static_cast<int>(bucket_height);
67 |   while (v_point < img.rows) {
68 |     cv::line(out_img, cv::Point(0, v_point), cv::Point(img.cols, v_point), cv::Scalar(47, 47, 47), 2, 8);
69 |     v_point += static_cast<int>(bucket_height);
70 |   }
71 | 
72 |   // Draw bucket and unbucket keypoints
73 |   cv::drawKeypoints(out_img, state.bucketed_kp, out_img, cv::Scalar(0, 255, 0), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
74 |   cv::drawKeypoints(out_img, state.unbucketed_kp, out_img, cv::Scalar(0, 0, 255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
75 | 
76 |   return out_img;
77 | }
78 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | libhaloc
  2 | =============
  3 | 
  4 | ROS library for HAsh-based LOop Closure detection. This library provides the tools for loop closing detection based on image hashing. Image hashing consists of representing every image with a small vector (hash). Then the hash of image A can be compared with the hash of image B in a super fast way in order to determine if images are similar.
  5 | 
  6 | The image hashing implemented in this library is based on SIFT features so, since SIFT is invariant to scale and rotation, the image hashing will be also invariant to these properties.
  7 | 
  8 | The library works for both mono and stereo cameras and provides a transformation (2d for mono and 3d for stereo) when loop closures are found.
  9 | 
 10 | ## Related paper
 11 | 
 12 | [Autonomous Robots][paper]
 13 | 
 14 | CITATION:
 15 | ```bash
 16 | @Article{Negre Carrasco2015,
 17 |    author="Negre Carrasco, Pep Lluis
 18 |    and Bonin-Font, Francisco
 19 |    and Oliver-Codina, Gabriel",
 20 |    title="Global image signature for visual loop-closure detection",
 21 |    journal="Autonomous Robots",
 22 |    year="2015",
 23 |    pages="1--15",
 24 |    issn="1573-7527",
 25 |    doi="10.1007/s10514-015-9522-4",
 26 |    url="http://dx.doi.org/10.1007/s10514-015-9522-4"
 27 | }
 28 | ```
 29 | 
 30 | ## How to prepare your SLAM node
 31 | 
 32 | 
 33 | Modify your CMakeLists.txt as follows:
 34 | 
 35 | ```bash
 36 | # Add this line before catkin_package()
 37 | find_package(libhaloc REQUIRED)
 38 | 
 39 | # Include the libhaloc directories
 40 | include_directories(
 41 |   ${catkin_INCLUDE_DIRS}
 42 |   ${libhaloc_INCLUDE_DIRS}
 43 |   ...
 44 |   )
 45 | 
 46 | # Link your executable with libhaloc
 47 | target_link_libraries(your_executable_here
 48 |   ${catkin_LIBRARIES}
 49 |   ${libhaloc_LIBRARIES}
 50 |   ...
 51 |   )
 52 | ```
 53 | 
 54 | Include the header in your .cpp file:
 55 | ```bash
 56 | #include <libhaloc/lc.h>
 57 | ```
 58 | 
 59 | ## How to call the library
 60 | 
 61 | 
 62 | You can use libhaloc in two different ways:
 63 | - To generate the image hashes and then, use your own techniques to compare this hashes and retrieve the best candidates to close loop.
 64 | - To use the full power of libhaloc to search loop closing candidates and compute the homogeneous transformation (if any).
 65 | 
 66 | ### To use libhaloc as the first option:
 67 | 
 68 | * Declare your haloc object:
 69 | ```bash
 70 | haloc::Hash haloc_;
 71 | ```
 72 | 
 73 | * Init your haloc object (only once):
 74 | ```bash
 75 | if (!hash_.isInitialized())
 76 |    hash_.init(c_cluster_.getSift());
 77 | ```
 78 | 
 79 | * Then, for every new image you process, extract its descriptor matrix (SIFT)
 80 | ```bash
 81 | cv::Mat sift;
 82 | // Use opencv to extract the SIFT matrix and then:
 83 | vector<float> current_hash = hash_.getHash(sift);
 84 | ```
 85 | 
 86 | * Now, you can store all the hashes into a table and compare it:
 87 | ```bash
 88 | std::vector<float> matches;
 89 | for (uint i=0; i<my_table.size(); i++)
 90 | {
 91 |    float m = hash_.match(current_hash, my_table[i]);
 92 |    matches.push_back(matches);
 93 | }
 94 | ```
 95 | 
 96 | * Finally, sort the vector of matches from smallest to largest. Take the first 1, 2, 3 or 4 smallest values as candidates to close loop.
 97 | 
 98 | 
 99 | ### To use libhaloc as the second option:
100 | 
101 | 
102 | 1) Declare your haloc object (this object is different from the option 1!!!):
103 | ```bash
104 | haloc::LoopClosure haloc_;
105 | ```
106 | 
107 | 2) Set the haloc parameters and initialize (only once):
108 | ```bash
109 | haloc::LoopClosure::Params params;
110 | params.work_dir = whatever;
111 | .
112 | .
113 | .
114 | haloc_.setParams(params);
115 | haloc_.init();
116 | ```
117 | 
118 | 4) Then, for every image, call the function setNode() to store the image properties and getLoopClosure() to get any possible loop closure between the last image and any previous images.
119 | ```bash
120 | // Mono version
121 | int loop_closure_with; 	// <- Will contain the index of the image that closes loop with the last inserted (-1 if none).
122 | tf::Transform trans;    // <- Will contain the transformation of the loop closure (if any).
123 | haloc_.setNode(img);
124 | bool valid = haloc_.getLoopClosure(loop_closure_with, trans);
125 | 
126 | // Stereo version
127 | int loop_closure_with; 	// <- Will contain the index of the image that closes loop with the last inserted (-1 if none).
128 | tf::Transform trans; 	// <- Will contain the transformation of the loop closure (if any).
129 | haloc_.setNode(img_left, img_right);
130 | bool valid = haloc_.getLoopClosure(loop_closure_with, trans);
131 | ```
132 | 
133 | In both cases, if valid is true, then a loop closure has been found.
134 | 
135 | 
136 | ## Example
137 | 
138 | 
139 | Check [this][stereo_slam] integration for a 3D Stereo Slam.
140 | 
141 | 
142 | ## Most Important Parameters
143 | 
144 | 
145 | * `work_dir` - Directory where the library will save the image information (must be writable!).
146 | * `desc_matching_type` - Can be CROSSCHECK or RATIO.
147 | * `desc_thresh_ratio` - Descriptor threshold for crosscheck matching (typically between 0.7-0.9) or ratio for ratio matching (typically between 0.6-0.8).
148 | * `min_neighbor` - Minimum number of neighbors that will be skipped for the loop closure (typically between 5-20, but depends on the frame rate).
149 | * `n_candidates` - Get the n first candidates of the hash matching (typically 2, 3, or 4).
150 | * `min_matches` - Minimum number of descriptor matches to consider a matching as possible loop closure (>12).
151 | * `min_inliers` - Minimum number of inliers to consider a matching as possible loop closure (>12).
152 | 
153 | 
154 | [stereo_slam]: https://github.com/srv/stereo_slam
155 | [paper]: http://link.springer.com/article/10.1007/s10514-015-9522-4
156 | 


--------------------------------------------------------------------------------
/examples/lip6indoor_dataset.cpp:
--------------------------------------------------------------------------------
  1 | //  Copyright (c) 2017 Universitat de les Illes Balears
  2 | //  This file is part of LIBHALOC.
  3 | //
  4 | //  LIBHALOC is free software: you can redistribute it and/or modify
  5 | //  it under the terms of the GNU General Public License as published by
  6 | //  the Free Software Foundation, either version 3 of the License, or
  7 | //  (at your option) any later version.
  8 | //
  9 | //  LIBHALOC is distributed in the hope that it will be useful,
 10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12 | //  GNU General Public License for more details.
 13 | //
 14 | //  You should have received a copy of the GNU General Public License
 15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
 16 | 
 17 | #include <fstream>
 18 | 
 19 | #include <boost/filesystem.hpp>
 20 | 
 21 | #include <opencv2/opencv.hpp>
 22 | 
 23 | #include "libhaloc/hash.h"
 24 | 
 25 | namespace fs = boost::filesystem;
 26 | 
 27 | /**
 28 |  * @brief      Shows the program usage.
 29 |  *
 30 |  * @param[in]  name  The program name
 31 |  */
 32 | static void ShowUsage(const std::string& name) {
 33 |   std::cerr << "Usage: " << name << " image_directory" << std::endl;
 34 | }
 35 | 
 36 | /**
 37 |  * @brief      Main entry point
 38 |  *
 39 |  * @param[in]  argc  The argc
 40 |  * @param      argv  The argv
 41 |  *
 42 |  * @return     0
 43 |  */
 44 | int main(int argc, char** argv) {
 45 |   // Parse arguments
 46 |   std::string img_dir = "";
 47 |   if (argc != 1 && argc != 2) {
 48 |     ShowUsage(argv[0]);
 49 |     return 0;
 50 |   }
 51 |   if (argc == 2) {
 52 |     img_dir = argv[1];
 53 |   } else {
 54 |     ShowUsage(argv[0]);
 55 |     return 0;
 56 |   }
 57 | 
 58 |   // Init ROS
 59 |   ros::init(argc, argv, "lip6indoor_dataset");
 60 | 
 61 |   // Init feature extractor
 62 |   cv::Ptr<cv::Feature2D> feat(new cv::Feature2D());
 63 |   feat = cv::KAZE::create();
 64 | 
 65 |   // Hash object
 66 |   haloc::Hash haloc;
 67 | 
 68 |   // Set params
 69 |   haloc::Hash::Params params;
 70 |   params.max_desc = 100;
 71 |   haloc.SetParams(params);
 72 | 
 73 |   // Sort directory of images
 74 |   typedef std::vector<fs::path> vec;
 75 |   vec v;
 76 |   copy(fs::directory_iterator(img_dir), fs::directory_iterator(),
 77 |     back_inserter(v));
 78 |   sort(v.begin(), v.end());
 79 |   vec::const_iterator it(v.begin());
 80 | 
 81 |   ROS_INFO_STREAM("Processing directory: " << img_dir << " with " <<
 82 |     v.size() << " images.");
 83 | 
 84 |   // Operational variables
 85 |   int img_idx = 0;
 86 |   int discard_window = 10;
 87 |   std::map<int, std::vector<float> > hash_table;
 88 | 
 89 |   // Loop over the images
 90 |   while (it != v.end()) {
 91 |     // Check if the directory entry is an directory.
 92 |     if (fs::is_directory(*it)) {
 93 |       it++;
 94 |       continue;
 95 |     }
 96 | 
 97 |     // Open the image
 98 |     std::string filename = it->filename().string();
 99 |     std::string path = img_dir + "/" + filename;
100 |     cv::Mat img = cv::imread(path, CV_LOAD_IMAGE_COLOR);
101 |     ROS_INFO_STREAM("Reading image " << filename);
102 | 
103 |     // Extract keypoints and descriptors
104 |     cv::Mat desc;
105 |     std::vector<cv::KeyPoint> kp;
106 |     feat->detectAndCompute(img, cv::noArray(), kp, desc);
107 |     ROS_INFO_STREAM("Number of features: " << kp.size());
108 | 
109 |     // Compute the hash
110 |     std::vector<float> hash = haloc.GetHash(kp, desc, img.size());
111 |     hash_table.insert(std::pair<int, std::vector<float> >(img_idx, hash));
112 | 
113 |     // Log
114 |     haloc.PublishState(img);
115 |     haloc::State state = haloc.GetState();
116 |     ROS_INFO_STREAM("Number of features after bucketing: " <<
117 |       state.bucketed_kp.size());
118 | 
119 |     img_idx++;
120 |     it++;
121 |     std::cout << std::endl;
122 |   }
123 | 
124 |   // Find loop closings
125 |   ROS_INFO("Generating the output matrix...");
126 |   for (float eps=0.6; eps <= 1.0; eps=eps+0.02) {
127 |     std::stringstream hist_file, output_file;
128 |     hist_file << "/tmp/histogram" << eps << ".csv";
129 |     output_file << "/tmp/output" << eps << ".csv";
130 |     std::fstream hist(hist_file.str().c_str(), std::ios::out);
131 |     std::fstream output(output_file.str().c_str(), std::ios::out);
132 |     hist << std::fixed << std::setprecision(10);
133 |     output << std::fixed << std::setprecision(10);
134 | 
135 |     ROS_INFO_STREAM("Processing eps: " << eps);
136 | 
137 |     int great_3 = 0;
138 |     int great_4 = 0;
139 |     int great_5 = 0;
140 |     int great_6 = 0;
141 | 
142 |     for (uint i=0; i < hash_table.size(); ++i) {
143 |       for (uint j=0; j < hash_table.size(); ++j) {
144 |         int dist_original = 0;
145 |         int dist = 0;
146 |         int neighbourhood = abs(i-j);
147 |         if (neighbourhood > 20 && j < i) {
148 |           dist_original = haloc.CalcDist(hash_table[i], hash_table[j], eps);
149 |           if (dist_original > 3) great_3++;
150 |           if (dist_original > 4) great_4++;
151 |           if (dist_original > 5) great_5++;
152 |           if (dist_original > 6) great_6++;
153 | 
154 |           if (dist_original < 4) {
155 |             dist = 0;
156 |           } else {
157 |             dist = 1;
158 |           }
159 |         }
160 | 
161 |         // Log
162 |         hist << dist_original << ", ";
163 | 
164 |         // Log
165 |         if (j != hash_table.size()-1) {
166 |           output << dist << ", ";
167 |         } else {
168 |           output << dist << std::endl;
169 |         }
170 |       }
171 |     }
172 |     hist.close();
173 |     output.close();
174 | 
175 |     ROS_INFO_STREAM("  >3: " << great_3);
176 |     ROS_INFO_STREAM("  >4: " << great_4);
177 |     ROS_INFO_STREAM("  >5: " << great_5);
178 |     ROS_INFO_STREAM("  >6: " << great_6);
179 | 
180 |   }
181 | 
182 |   ROS_INFO_STREAM("Finished!");
183 | 
184 |   ros::shutdown();
185 |   return 0;
186 | }
187 | 


--------------------------------------------------------------------------------
/include/libhaloc/hash.h:
--------------------------------------------------------------------------------
  1 | //  Copyright (c) 2017 Universitat de les Illes Balears
  2 | //  This file is part of LIBHALOC.
  3 | //
  4 | //  LIBHALOC is free software: you can redistribute it and/or modify
  5 | //  it under the terms of the GNU General Public License as published by
  6 | //  the Free Software Foundation, either version 3 of the License, or
  7 | //  (at your option) any later version.
  8 | //
  9 | //  LIBHALOC is distributed in the hope that it will be useful,
 10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12 | //  GNU General Public License for more details.
 13 | //
 14 | //  You should have received a copy of the GNU General Public License
 15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
 16 | 
 17 | #ifndef LIBHALOC_INCLUDE_LIBHALOC_HASH_H_
 18 | #define LIBHALOC_INCLUDE_LIBHALOC_HASH_H_
 19 | 
 20 | #include <Eigen/Eigen>
 21 | #include <Eigen/Dense>
 22 | 
 23 | #include <vector>
 24 | #include <utility>
 25 | #include <numeric>
 26 | 
 27 | #include "libhaloc/publisher.h"
 28 | 
 29 | #include <opencv2/core/core.hpp>
 30 | #include <opencv2/features2d/features2d.hpp>
 31 | 
 32 | namespace haloc {
 33 | 
 34 | class Hash {
 35 |  public:
 36 |   /**
 37 |    * @brief      Struct for class parameters
 38 |    */
 39 |   struct Params {
 40 |     /**
 41 |      * @brief      Default constructor
 42 |      */
 43 |     Params();
 44 | 
 45 |     // Class parameters
 46 |     int bucket_rows;             //!> Number of horizontal divisions for the descriptors bucketing
 47 |     int bucket_cols;             //!> Number of vertical divisions for the descriptors bucketing
 48 |     int max_desc;                //!> Maximum number of descriptors per image
 49 |     int num_proj;                //!> Number of projections required
 50 | 
 51 |     // Default values
 52 |     static const int             DEFAULT_BUCKET_ROWS = 3;
 53 |     static const int             DEFAULT_BUCKET_COLS = 4;
 54 |     static const int             DEFAULT_MAX_DESC = 100;
 55 |     static const int             DEFAULT_NUM_PROJ = 2;
 56 |   };
 57 | 
 58 |   /**
 59 |    * @brief      Empty class constructor.
 60 |    */
 61 |   Hash();
 62 | 
 63 |   /**
 64 |    * @brief      Sets the parameters.
 65 |    *
 66 |    * @param[in]  params  The parameters.
 67 |    */
 68 |   inline void SetParams(const Params& params) {
 69 |     params_ = params; initialized_ = false;}
 70 | 
 71 |   /**
 72 |    * @brief      Returns the parameters.
 73 |    *
 74 |    * @return     The parameters.
 75 |    */
 76 |   inline Params GetParams() const {return params_;}
 77 | 
 78 |   /**
 79 |    * @brief      Determines if class is initialized.
 80 |    *
 81 |    * @return     True if initialized, False otherwise.
 82 |    */
 83 |   inline bool IsInitialized() const {return initialized_;}
 84 | 
 85 |   /**
 86 |    * @brief      Gets the state.
 87 |    *
 88 |    * @return     The state.
 89 |    */
 90 |   inline State GetState() const {return state_;}
 91 | 
 92 |   /**
 93 |    * @brief      Bucket the features and compute a hash for every bucket.
 94 |    *
 95 |    * @param[in]  kp        The keypoints vector.
 96 |    * @param[in]  desc      The descriptors.
 97 |    * @param[in]  img_size  The image size.
 98 |    *
 99 |    * @return     The bucketed hash.
100 |    */
101 |   std::vector<float> GetHash(const std::vector<cv::KeyPoint>& kp,
102 |     const cv::Mat& desc, const cv::Size& img_size);
103 | 
104 |   /**
105 |    * @brief      Compute the distance between 2 hashes.
106 |    *
107 |    * @param[in]  hash_1  The hash 1.
108 |    * @param[in]  hash_2  The hash 2.
109 |    *
110 |    * @return     Distance: the number of buckets seeing the same view.
111 |    */
112 |   int CalcDist(const std::vector<float>& hash_1,
113 |     const std::vector<float>& hash_2, float eps);
114 | 
115 |   /**
116 |    * @brief      Publishes the state and debug variables. Must be called after a
117 |    *             hash computation.
118 |    *
119 |    * @param[in]  img   The original image
120 |    */
121 |   void PublishState(const cv::Mat& img);
122 | 
123 |  protected:
124 |   /**
125 |    * @brief      Init the class.
126 |    *
127 |    * @param[in]  img_size     The image size.
128 |    * @param[in]  num_feat     The number of features for the input image.
129 |    * @param[in]  desc_length  The descriptor length
130 |    */
131 |   void Init(const cv::Size& img_size, const int& num_feat,
132 |     const int& desc_length);
133 | 
134 |   /**
135 |    * @brief      Compute the combinations required for the match calculation
136 |    */
137 |   void InitCombinations();
138 | 
139 |   /**
140 |    * @brief      Initializes the random vectors for projections.
141 |    *
142 |    * @param[in]  size  The size.
143 |    */
144 |   void InitProjections(const int& size);
145 | 
146 |   /**
147 |    * @brief      Calculates a random vector.
148 |    *
149 |    * @param[in]  size  The size.
150 |    * @param[in]  seed  The seed.
151 |    *
152 |    * @return     The random vector.
153 |    */
154 |   std::vector<float> ComputeRandomVector(const int& size, int seed);
155 | 
156 |   /**
157 |    * @brief      Makes a vector unitary.
158 |    *
159 |    * @param[in]  x     The input vector.
160 |    *
161 |    * @return     The output unit vector.
162 |    */
163 |   std::vector<float> UnitVector(const std::vector<float>& x);
164 | 
165 |   /**
166 |    * @brief      Bucket the descriptors.
167 |    *
168 |    * @param[in]  kp    The keypoint vector.
169 |    * @param[in]  desc  The descriptors.
170 |    *
171 |    * @return     The bucketed descriptors.
172 |    */
173 |   std::vector<cv::Mat> BucketDescriptors(const std::vector<cv::KeyPoint>& kp,
174 |     const cv::Mat& desc);
175 | 
176 |   /**
177 |    * @brief      Compute the hash by projecting the descriptors.
178 |    *
179 |    * @param[in]  desc  The descriptors.
180 |    *
181 |    * @return     The hash.
182 |    */
183 |   std::vector<float> ProjectDescriptors(const cv::Mat& desc);
184 | 
185 |  private:
186 |   // Properties
187 |   Params params_;                        //!> Stores parameters
188 |   State state_;                          //!> Stores the state after every hash computation
189 |   cv::Size img_size_;                    //!> Image size (only needed for bucketing)
190 |   int desc_length_;                      //!> The length of the descriptors used
191 |   std::vector< std::vector<float> > r_;  //!> Vector of random values
192 |   bool initialized_;                     //!> True when class has been initialized
193 |   std::vector< std::vector< std::pair<int, int> > > comb_;  //!> Combinations for the match
194 |   Publisher pub_;                        //!> The publisher for debugging purposes
195 | };
196 | 
197 | }  // namespace haloc
198 | 
199 | #endif  // LIBHALOC_INCLUDE_LIBHALOC_HASH_H_
200 | 


--------------------------------------------------------------------------------
/src/hash.cpp:
--------------------------------------------------------------------------------
  1 | //  Copyright (c) 2017 Universitat de les Illes Balears
  2 | //  This file is part of LIBHALOC.
  3 | //
  4 | //  LIBHALOC is free software: you can redistribute it and/or modify
  5 | //  it under the terms of the GNU General Public License as published by
  6 | //  the Free Software Foundation, either version 3 of the License, or
  7 | //  (at your option) any later version.
  8 | //
  9 | //  LIBHALOC is distributed in the hope that it will be useful,
 10 | //  but WITHOUT ANY WARRANTY; without even the implied warranty of
 11 | //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 12 | //  GNU General Public License for more details.
 13 | //
 14 | //  You should have received a copy of the GNU General Public License
 15 | //  along with LIBHALOC. If not, see <http://www.gnu.org/licenses/>.
 16 | 
 17 | #include <ros/ros.h>
 18 | 
 19 | #include "libhaloc/hash.h"
 20 | 
 21 | #include <opencv2/core/eigen.hpp>
 22 | 
 23 | haloc::Hash::Params::Params() :
 24 |   bucket_rows(DEFAULT_BUCKET_ROWS), bucket_cols(DEFAULT_BUCKET_COLS),
 25 |   max_desc(DEFAULT_MAX_DESC), num_proj(DEFAULT_NUM_PROJ)
 26 | {}
 27 | 
 28 | haloc::Hash::Hash() : initialized_(false) {}
 29 | 
 30 | std::vector<float> haloc::Hash::GetHash(
 31 |     const std::vector<cv::KeyPoint>& kp, const cv::Mat& desc,
 32 |     const cv::Size& img_size) {
 33 |   // Initialize first time
 34 |   if (!IsInitialized()) Init(img_size, kp.size(), desc.cols);
 35 |   state_.Clear();
 36 | 
 37 |   // Initialize output
 38 |   std::vector<float> hash;
 39 | 
 40 |   // The maximum number of features per bucket
 41 |   int max_features_x_bucket = static_cast<int>(
 42 |     floor(params_.max_desc/(params_.bucket_cols*params_.bucket_rows)));
 43 | 
 44 |   // Bucket descriptors
 45 |   std::vector<cv::Mat> bucket_desc = BucketDescriptors(kp, desc);
 46 | 
 47 |   // Get a hash for every bucket
 48 |   const int min_feat = static_cast<int>(0.7 * max_features_x_bucket);
 49 |   for (uint i=0; i < bucket_desc.size(); ++i) {
 50 |     std::vector<float> bucketed_hash;
 51 |     if (bucket_desc[i].rows >= min_feat) {
 52 |       bucketed_hash = ProjectDescriptors(bucket_desc[i]);
 53 |     } else {
 54 |       for (uint i=0; i < desc.cols*params_.num_proj; ++i)
 55 |         bucketed_hash.push_back(0.0);
 56 |     }
 57 |     hash.insert(hash.end(), bucketed_hash.begin(), bucketed_hash.end());
 58 |   }
 59 |   return hash;
 60 | }
 61 | 
 62 | int haloc::Hash::CalcDist(const std::vector<float>& hash_a,
 63 |     const std::vector<float>& hash_b, float eps) {
 64 |   // Init
 65 |   const int num_buckets = params_.bucket_cols*params_.bucket_rows;
 66 |   // const float eps = 0.6;
 67 |   int num_buckets_overlap = 0;
 68 | 
 69 |   // Compute the distance
 70 |   for (uint i=0; i < comb_.size(); ++i) {
 71 |     int comb_overlap = 0;
 72 |     for (uint j=0; j < num_buckets; ++j) {
 73 |       int idx_a = comb_[i][j].first  * params_.num_proj * desc_length_;
 74 |       int idx_b = comb_[i][j].second * params_.num_proj * desc_length_;
 75 | 
 76 |       // Check if buckets are empty
 77 |       std::vector<float>::const_iterator a_first = hash_a.begin() + idx_a;
 78 |       std::vector<float>::const_iterator a_last = hash_a.begin() + idx_a +
 79 |         desc_length_*params_.num_proj;
 80 |       float sum_a = std::accumulate(a_first, a_last, 0.0);
 81 |       if (sum_a == 0.0) continue;
 82 | 
 83 |       std::vector<float>::const_iterator b_first = hash_b.begin() + idx_b;
 84 |       std::vector<float>::const_iterator b_last = hash_b.begin() + idx_b +
 85 |         desc_length_*params_.num_proj;
 86 |       float sum_b = std::accumulate(b_first, b_last, 0.0);
 87 |       if (sum_b == 0.0) continue;
 88 | 
 89 |       float proj_sum = 0.0;
 90 |       for (uint k=0; k < desc_length_*params_.num_proj; ++k) {
 91 |         proj_sum += fabs(hash_a[idx_a+k] - hash_b[idx_b+k]);
 92 |       }
 93 |       if (proj_sum <= eps) comb_overlap++;
 94 |     }
 95 |     if (comb_overlap > num_buckets_overlap) {
 96 |       num_buckets_overlap = comb_overlap;
 97 |     }
 98 |   }
 99 |   return num_buckets_overlap;
100 | }
101 | 
102 | void haloc::Hash::PublishState(const cv::Mat& img) {
103 |   // The bucketed image
104 |   pub_.PublishBucketedImage(state_, img, params_.bucket_rows,
105 |     params_.bucket_cols);
106 | 
107 |   // The bucketed info
108 |   int max_features_x_bucket = static_cast<int>(
109 |     floor(params_.max_desc/(params_.bucket_cols*params_.bucket_rows)));
110 |   pub_.PublishBucketedInfo(state_, max_features_x_bucket);
111 | }
112 | 
113 | void haloc::Hash::Init(const cv::Size& img_size, const int& num_feat,
114 |     const int& desc_length) {
115 |   InitProjections(params_.max_desc);
116 |   InitCombinations();
117 |   img_size_ = img_size;
118 |   desc_length_ = desc_length;
119 | 
120 |   // Sanity check
121 |   if (params_.max_desc < num_feat * 0.7) {
122 |     ROS_WARN_STREAM("[Haloc:] WARNING -> Please setup the maximum number " <<
123 |       "descriptors correctly. The current image has " << num_feat << " and " <<
124 |       "the max_desc param is " << params_.max_desc << ". The parameter for " <<
125 |       "the maximum number of descriptors must be smaller than the number" <<
126 |       "of real features in the images.");
127 |   }
128 | 
129 |   initialized_ = true;
130 | }
131 | 
132 | void haloc::Hash::InitCombinations() {
133 |   comb_.clear();
134 |   int num_buckets = params_.bucket_cols*params_.bucket_rows;
135 |   int second_idx_shift = 0;
136 |   for (uint i=0; i < num_buckets; ++i) {
137 |     std::vector< std::pair<int, int> > combinations_row;
138 |     for (uint j=0; j < num_buckets; ++j) {
139 |       int second_idx = j + second_idx_shift;
140 |       if (second_idx > num_buckets-1) {
141 |         second_idx = second_idx - num_buckets;
142 |       }
143 |       combinations_row.push_back(std::make_pair(j, second_idx));
144 |     }
145 |     comb_.push_back(combinations_row);
146 |     second_idx_shift++;
147 |   }
148 | }
149 | 
150 | void haloc::Hash::InitProjections(const int& desc_size) {
151 |   // Initializations
152 |   int seed = time(NULL);
153 |   r_.clear();
154 | 
155 |   // The maximum number of features per bucket
156 |   int max_features_x_bucket = static_cast<int>(
157 |     floor(params_.max_desc/(params_.bucket_cols*params_.bucket_rows)));
158 | 
159 |   // The size of the descriptors may vary...
160 |   // But, we limit the number of descriptors per bucket.
161 |   int v_size = max_features_x_bucket;
162 | 
163 |   // We will generate N-orthogonal vectors creating a linear system of type Ax=b
164 |   // Generate a first random vector
165 |   std::vector<float> r = ComputeRandomVector(v_size, seed);
166 |   r_.push_back(UnitVector(r));
167 | 
168 |   // Generate the set of orthogonal vectors
169 |   for (uint i=1; i < params_.num_proj; i++) {
170 |     // Generate a random vector of the correct size
171 |     std::vector<float> new_v = ComputeRandomVector(v_size - i, seed + i);
172 | 
173 |     // Get the right terms (b)
174 |     Eigen::VectorXf b(r_.size());
175 |     for (uint n=0; n < r_.size(); n++) {
176 |       std::vector<float> cur_v = r_[n];
177 |       float sum = 0.0;
178 |       for (uint m=0; m < new_v.size(); m++)
179 |         sum += new_v[m]*cur_v[m];
180 |       b(n) = -sum;
181 |     }
182 | 
183 |     // Get the matrix of equations (A)
184 |     Eigen::MatrixXf A(i, i);
185 |     for (uint n=0; n < r_.size(); n++) {
186 |       uint k = 0;
187 |       for (uint m=r_[n].size()-i; m < r_[n].size(); m++) {
188 |         A(n, k) = r_[n][m];
189 |         k++;
190 |       }
191 |     }
192 | 
193 |     // Apply the solver
194 |     Eigen::VectorXf x = A.colPivHouseholderQr().solve(b);
195 | 
196 |     // Add the solutions to the new vector
197 |     for (uint n=0; n < r_.size(); n++)
198 |       new_v.push_back(x(n));
199 |     new_v = UnitVector(new_v);
200 | 
201 |     // Push the new vector
202 |     r_.push_back(new_v);
203 |   }
204 | }
205 | 
206 | std::vector<float> haloc::Hash::ComputeRandomVector(const int& size, int seed) {
207 |   std::vector<float> h;
208 |   for (int i=0; i < size; i++)
209 |     h.push_back((float)rand()/RAND_MAX);
210 |   return h;
211 | }
212 | 
213 | std::vector<float> haloc::Hash::UnitVector(const std::vector<float>& x) {
214 |   // Compute the norm
215 |   float sum = 0.0;
216 |   for (uint i=0; i < x.size(); i++)
217 |     sum += pow(x[i], 2.0);
218 |   float x_norm = sqrt(sum);
219 | 
220 |   // x^ = x/|x|
221 |   std::vector<float> out;
222 |   for (uint i=0; i < x.size(); i++)
223 |     out.push_back(x[i] / x_norm);
224 | 
225 |   return out;
226 | }
227 | 
228 | std::vector<cv::Mat> haloc::Hash::BucketDescriptors(
229 |     const std::vector<cv::KeyPoint>& kp, const cv::Mat& desc) {
230 |   // Find max values
231 |   float u_max = 0;
232 |   float v_max = 0;
233 |   std::vector<cv::KeyPoint> kp_in = kp;
234 |   for (std::vector<cv::KeyPoint>::iterator it = kp_in.begin();
235 |       it != kp_in.end(); it++) {
236 |     if (it->pt.x > u_max) u_max = it->pt.x;
237 |     if (it->pt.y > v_max) v_max = it->pt.y;
238 |   }
239 | 
240 |   // Compute width and height of the buckets
241 |   float bucket_width  = img_size_.width / params_.bucket_cols;
242 |   float bucket_height = img_size_.height / params_.bucket_rows;
243 | 
244 |   // Allocate number of buckets needed
245 |   std::vector<cv::Mat> desc_buckets(params_.bucket_cols*params_.bucket_rows);
246 |   std::vector< std::vector<cv::KeyPoint> > kp_buckets(
247 |     params_.bucket_cols*params_.bucket_rows);
248 | 
249 |   // Assign descriptors to their buckets
250 |   for (uint i=0; i < kp_in.size(); ++i) {
251 |     int u = static_cast<int>(floor(kp_in[i].pt.x/bucket_width));
252 |     int v = static_cast<int>(floor(kp_in[i].pt.y/bucket_height));
253 | 
254 |     desc_buckets[v*params_.bucket_cols+u].push_back(desc.row(i));
255 |     kp_buckets[v*params_.bucket_cols+u].push_back(kp_in[i]);
256 |   }
257 | 
258 |   // The maximum number of features per bucket
259 |   int max_features_x_bucket = static_cast<int>(
260 |     floor(params_.max_desc/(params_.bucket_cols*params_.bucket_rows)));
261 | 
262 |   // Select the best keypoints for each bucket
263 |   std::vector<cv::Mat> out_desc(params_.bucket_cols*params_.bucket_rows);
264 |   for (int i=0; i < params_.bucket_cols*params_.bucket_rows; ++i) {
265 |     // Sort keypoints by response
266 |     std::vector<cv::KeyPoint> cur_kps = kp_buckets[i];
267 |     std::vector<int> index(cur_kps.size(), 0);
268 |     for (uint j=0; j<index.size(); j++) {
269 |       index[j] = j;
270 |     }
271 |     std::sort(index.begin(), index.end(), [&](const int& a, const int& b) {
272 |       return (cur_kps[a].response > cur_kps[b].response);
273 |     });
274 | 
275 |     // Add up to max_features_x_bucket features from this bucket
276 |     int k = 0;
277 |     int num_kp = 0;
278 |     for (uint j=0; j < cur_kps.size(); ++j) {
279 |       if (k < max_features_x_bucket) {
280 |         out_desc[i].push_back(desc_buckets[i].row(index[j]));
281 |         state_.bucketed_kp.push_back(kp_buckets[i][index[j]]);
282 |         num_kp++;
283 |       } else {
284 |         state_.unbucketed_kp.push_back(kp_buckets[i][index[j]]);
285 |       }
286 |       k++;
287 |     }
288 |     state_.num_kp_per_bucket.push_back(num_kp);
289 |   }
290 | 
291 |   return out_desc;
292 | }
293 | 
294 | std::vector<float> haloc::Hash::ProjectDescriptors(const cv::Mat& desc) {
295 |   // Initialize first time
296 |   if (!IsInitialized()) Init(cv::Size(0, 0), desc.rows, desc.cols);
297 | 
298 |   // Initialize output
299 |   std::vector<float> hash;
300 | 
301 |   // Sanity checks
302 |   if (desc.rows == 0) {
303 |     ROS_ERROR("[Haloc:] ERROR -> Descriptor matrix is empty.");
304 |     return hash;
305 |   }
306 | 
307 |   if (desc.rows > r_[0].size()) {
308 |     ROS_ERROR_STREAM("[Haloc:] ERROR -> The number of descriptors is " <<
309 |       "larger than the size of the projection vector. This should not happen.");
310 |     return hash;
311 |   }
312 | 
313 |   // Project the descriptors
314 |   for (uint i=0; i < r_.size(); i++) {
315 |     for (int n=0; n < desc.cols; n++) {
316 |       float desc_sum = 0.0;
317 |       for (uint m=0; m < desc.rows; m++) {
318 |         float projected = r_[i][m]*desc.at<float>(m, n);
319 |         float projected_normalized = (projected + 1.0) / 2.0;
320 |         desc_sum += projected_normalized;
321 |       }
322 |       hash.push_back(desc_sum / static_cast<float>(desc.rows));
323 |     }
324 |   }
325 |   return hash;
326 | }
327 | 


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