├── src
    └── DLoopDetector.cmake.in
├── .travis.yml
├── include
    └── DLoopDetector
    │   ├── DLoopDetector.h
    │   └── TemplatedLoopDetector.h
├── LICENSE.txt
├── README.md
├── demo
    ├── demo_brief.cpp
    └── demoDetector.h
└── CMakeLists.txt


/src/DLoopDetector.cmake.in:
--------------------------------------------------------------------------------
1 | FIND_LIBRARY(DLoopDetector_LIBRARY DLoopDetector
2 |     PATHS @CMAKE_INSTALL_PREFIX@/lib
3 | )
4 | FIND_PATH(DLoopDetector_INCLUDE_DIR DLoopDetectorConfig.cmake
5 |     PATHS @CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME@ 
6 | )
7 | SET(DLoopDetector_LIBRARIES ${DLoopDetector_LIBRARY})
8 | SET(DLoopDetector_LIBS ${DLoopDetector_LIBRARY})
9 | SET(DLoopDetector_INCLUDE_DIRS ${DLoopDetector_INCLUDE_DIR})


--------------------------------------------------------------------------------
/.travis.yml:
--------------------------------------------------------------------------------
 1 | language: cpp
 2 | os:
 3 |     - linux
 4 |     - osx
 5 |     - windows
 6 | dist:
 7 |     - bionic
 8 | osx_image:
 9 |     - xcode11.3
10 | env:
11 |     - OPENCV_BRANCH=master
12 |     - OPENCV_BRANCH=3.4
13 | cache:
14 |     directories:
15 |         - ${TRAVIS_BUILD_DIR}/opencv
16 | before_script:
17 |     - rmdir opencv || true
18 |     - if [ ! -d opencv ]; then git clone --single-branch --branch ${OPENCV_BRANCH} https://github.com/opencv/opencv.git; fi
19 |     - pushd opencv
20 |     - mkdir -p build
21 |     - cd build
22 |     - cmake .. -DBUILD_LIST=core,imgproc,imgcodecs,calib3d,highgui,features2d,flann -DBUILD_EXAMPLES=OFF -DCMAKE_INSTALL_PREFIX=../install
23 |     - cmake --build . --parallel 8 --target install --config Release
24 |     - popd
25 | script:
26 |     - mkdir -p build
27 |     - cd build
28 |     - export OPENCV_CONFIG=$(dirname $(find ${TRAVIS_BUILD_DIR}/opencv/install -name OpenCVConfig.cmake | head -n1))
29 |     - cmake .. -DBUILD_DemoBRIEF=ON -DOpenCV_DIR=${OPENCV_CONFIG}
30 |     - cmake --build . --config Release
31 | 


--------------------------------------------------------------------------------
/include/DLoopDetector/DLoopDetector.h:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * File: DLoopDetector.h
 3 |  * Date: November 2011
 4 |  * Author: Dorian Galvez-Lopez
 5 |  * Description: Generic include file for the DLoopDetector classes and
 6 |  *   the specialized loop detectors
 7 |  * License: see the LICENSE.txt file
 8 |  *
 9 |  */
10 | 
11 | /*! \mainpage DLoopDetector Library
12 |  *
13 |  * DLoopDetector library for C++:
14 |  * Loop detector for monocular image sequences based on bags of words
15 |  *
16 |  * Written by Dorian Galvez-Lopez,
17 |  * University of Zaragoza
18 |  * 
19 |  * Check my website to obtain updates: http://webdiis.unizar.es/~dorian
20 |  *
21 |  * \section requirements Requirements
22 |  * This library requires the DUtils, DUtilsCV, DVision, DBoW2 and OpenCV libraries,
23 |  * as well as the boost::dynamic_bitset class.
24 |  *
25 |  * \section citation Citation
26 |  * If you use this software in academic works, please cite:
27 |  <pre>
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |  </pre>
41 |  *
42 |  */
43 | 
44 | 
45 | #ifndef __D_T_LOOP_DETECTOR__
46 | #define __D_T_LOOP_DETECTOR__
47 | 
48 | /// Loop detector for sequences of monocular images
49 | namespace DLoopDetector
50 | {
51 | }
52 | 
53 | #include "DBoW2.h"
54 | #include "TemplatedLoopDetector.h"
55 | #include "FORB.h"
56 | #include "FBrief.h"
57 | 
58 | /// SURF64 Loop Detector
59 | typedef DLoopDetector::TemplatedLoopDetector
60 |   <FORB::TDescriptor, FORB> OrbLoopDetector;
61 | 
62 | /// BRIEF Loop Detector
63 | typedef DLoopDetector::TemplatedLoopDetector
64 |   <FBrief::TDescriptor, FBrief> BriefLoopDetector;
65 | 
66 | #endif
67 | 
68 | 


--------------------------------------------------------------------------------
/LICENSE.txt:
--------------------------------------------------------------------------------
 1 | DLoopDetector: loop detector for monocular image sequences
 2 | 
 3 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions
 8 | are met:
 9 | 1. Redistributions of source code must retain the above copyright
10 |    notice, this list of conditions and the following disclaimer.
11 | 2. Redistributions in binary form must reproduce the above copyright
12 |    notice, this list of conditions and the following disclaimer in the
13 |    documentation and/or other materials provided with the distribution.
14 | 3. The original author of the work must be notified of any 
15 |    redistribution of source code or in binary form.
16 | 4. Neither the name of copyright holders nor the names of its
17 |    contributors may be used to endorse or promote products derived
18 |    from this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
22 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23 | PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS
24 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 | POSSIBILITY OF SUCH DAMAGE.
31 | 
32 | If you use it in an academic work, please cite:
33 | 
34 |   @ARTICLE{GalvezTRO12,
35 |     author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 
36 |     journal={IEEE Transactions on Robotics},
37 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
38 |     year={2012},
39 |     month={October},
40 |     volume={28},
41 |     number={5},
42 |     pages={1188--1197},
43 |     doi={10.1109/TRO.2012.2197158},
44 |     ISSN={1552-3098}
45 |   }
46 | 
47 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | DLoopDetector
 2 | =============
 3 | 
 4 | ## Overview
 5 | 
 6 | DLoopDetector is an open source C++ library to detect loops in a sequence of images collected by a mobile robot. It implements the algorithm presented in GalvezTRO12, based on a bag-of-words database created from image local descriptors, and temporal and geometrical constraints. The current implementation includes versions to work with SURF64 and BRIEF descriptors. DLoopDetector is based on the DBoW2 library, so that it can work with any other type of descriptor with little effort.
 7 | 
 8 | DLoopDetector requires OpenCV and the `boost::dynamic_bitset` class in order to use the BRIEF version.
 9 | 
10 | DLoopDetector has been tested on several real datasets, yielding an execution time of ~9 ms to detect a loop a in a sequence with more than 19000 images (without considering the feature extraction). When BRIEF descriptors were used, the feature extraction and the loop detection were performed in 16 ms on average.
11 | 
12 | ## Citing
13 | 
14 | If you use this software in an academic work, please cite:
15 | 
16 |     @ARTICLE{GalvezTRO12,
17 |       author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.},
18 |       journal={IEEE Transactions on Robotics},
19 |       title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
20 |       year={2012},
21 |       month={October},
22 |       volume={28},
23 |       number={5},
24 |       pages={1188--1197},
25 |       doi={10.1109/TRO.2012.2197158},
26 |       ISSN={1552-3098}
27 |     }
28 | 
29 | ## Install and usage notes
30 | 
31 | DLoopDetector requires [DLib](https://github.com/dorian3d/DLib) and [DBoW2](https://github.com/dorian3d/DBoW2), which are installed automatically.
32 | 
33 | DLoopDetector requires OpenCV and the `boost::dynamic_bitset` class in order to use the BRIEF version. You can install Boost by typing:
34 | 
35 |     $ sudo apt-get install libboost-dev
36 | 
37 | 
38 | To check how to use DLoopDetector, compile the demo applications. This demo includes the DLoopDetector classes, bag-of-words vocabularies with 10^6 words to use with SURF64 or BRIEF features, a small collection of images of the Bicocca 2009-02-25b dataset of the Rawseeds FP-6 project and a demo application to find loops in these images.
39 | 
40 | Note that the demo applications require some external resources that are downloaded automatically when they are activated in CMake. The resource file is 374MB so it may take a while to download.
41 | 
42 | You can run either demo_surf or demo_brief. When the demo is running, you should see a window with the current image, a window with the trajectory of the robot and the status of the detection process in the console:
43 | ![Loop detector](http://dorian3d.github.com/other/images/dloop.png)
44 | 
45 | The main functionality of the demo is written in the `demoDetector.h` class. Check it to see how to change the parameters of the loop detector.
46 | 


--------------------------------------------------------------------------------
/demo/demo_brief.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * File: demo_brief.cpp
  3 |  * Date: November 2011
  4 |  * Author: Dorian Galvez-Lopez
  5 |  * Description: demo application of DLoopDetector
  6 |  * License: see the LICENSE.txt file
  7 |  */
  8 | 
  9 | #include <iostream>
 10 | #include <vector>
 11 | #include <string>
 12 | 
 13 | // DLoopDetector and DBoW2
 14 | #include <DBoW2/DBoW2.h> // defines BriefVocabulary
 15 | #include "DLoopDetector.h" // defines BriefLoopDetector
 16 | #include <DVision/DVision.h> // Brief
 17 | 
 18 | // OpenCV
 19 | #include <opencv2/core.hpp>
 20 | #include <opencv2/highgui.hpp>
 21 | 
 22 | #include "demoDetector.h"
 23 | 
 24 | using namespace DLoopDetector;
 25 | using namespace DBoW2;
 26 | using namespace DVision;
 27 | using namespace std;
 28 | 
 29 | // ----------------------------------------------------------------------------
 30 | 
 31 | static const char *VOC_FILE = "./resources/brief_k10L6.voc.gz";
 32 | static const char *IMAGE_DIR = "./resources/images";
 33 | static const char *POSE_FILE = "./resources/pose.txt";
 34 | static const int IMAGE_W = 640; // image size
 35 | static const int IMAGE_H = 480;
 36 | static const char *BRIEF_PATTERN_FILE = "./resources/brief_pattern.yml";
 37 | 
 38 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 39 | 
 40 | /// This functor extracts BRIEF descriptors in the required format
 41 | class BriefExtractor: public FeatureExtractor<BRIEF256::bitset>
 42 | {
 43 | public:
 44 |   /** 
 45 |    * Extracts features from an image
 46 |    * @param im image
 47 |    * @param keys keypoints extracted
 48 |    * @param descriptors descriptors extracted
 49 |    */
 50 |   void operator()(const cv::Mat &im, 
 51 |     vector<cv::KeyPoint> &keys, vector<BRIEF256::bitset> &descriptors) const override;
 52 | 
 53 |   /**
 54 |    * Creates the brief extractor with the given pattern file
 55 |    * @param pattern_file
 56 |    */
 57 |   BriefExtractor(const std::string &pattern_file);
 58 | 
 59 | private:
 60 | 
 61 |   /// BRIEF descriptor extractor
 62 |   BRIEF256 m_brief;
 63 | };
 64 | 
 65 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 66 | 
 67 | int main(int argc, char* argv[])
 68 | {
 69 |   bool show = true;
 70 |   if (argc > 1 && std::string(argv[1]) == "-noshow")
 71 |     show = false;
 72 | 
 73 |   // prepares the demo
 74 |   demoDetector<BriefVocabulary, BriefLoopDetector, FBrief::TDescriptor>
 75 |     demo(VOC_FILE, IMAGE_DIR, POSE_FILE, IMAGE_W, IMAGE_H, show);
 76 |   
 77 |   try 
 78 |   {
 79 |     // run the demo with the given functor to extract features
 80 |     BriefExtractor extractor(BRIEF_PATTERN_FILE);
 81 |     demo.run("BRIEF", extractor);
 82 |   }
 83 |   catch(const std::string &ex)
 84 |   {
 85 |     cout << "Error: " << ex << endl;
 86 |   }
 87 | 
 88 |   return 0;
 89 | }
 90 | 
 91 | // ----------------------------------------------------------------------------
 92 | 
 93 | BriefExtractor::BriefExtractor(const std::string &pattern_file)
 94 | {
 95 |   // The DVision::BRIEF extractor computes a random pattern by default when
 96 |   // the object is created.
 97 |   // We load the pattern that we used to build the vocabulary, to make
 98 |   // the descriptors compatible with the predefined vocabulary
 99 |   
100 |   // loads the pattern
101 |   cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ);
102 |   if(!fs.isOpened()) throw string("Could not open file ") + pattern_file;
103 |   
104 |   vector<int> x1, y1, x2, y2;
105 |   fs["x1"] >> x1;
106 |   fs["x2"] >> x2;
107 |   fs["y1"] >> y1;
108 |   fs["y2"] >> y2;
109 |   
110 |   m_brief.importPairs(x1, y1, x2, y2);
111 | }
112 | 
113 | // ----------------------------------------------------------------------------
114 | 
115 | void BriefExtractor::operator() (const cv::Mat &im, 
116 |   vector<cv::KeyPoint> &keys, vector<BRIEF256::bitset> &descriptors) const
117 | {
118 |   // extract FAST keypoints with opencv
119 |   const int fast_th = 20; // corner detector response threshold
120 |   cv::FAST(im, keys, fast_th, true);
121 |   
122 |   // compute their BRIEF descriptor
123 |   m_brief.compute(im, keys, descriptors);
124 | }
125 | 
126 | // ----------------------------------------------------------------------------
127 | 
128 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8)
  2 | project(DLoopDetector)
  3 | include(ExternalProject)
  4 | 
  5 | option(BUILD_DemoBRIEF  "Build demo application with BRIEF features" OFF)
  6 | 
  7 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
  8 |   set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE)
  9 |   set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release"
 10 |     "MinSizeRel" "RelWithDebInfo")
 11 | endif()
 12 | 
 13 | if(MSVC)
 14 |   add_compile_options(/W4)
 15 | else()
 16 |   add_compile_options(-Wall -Wextra -Wpedantic)
 17 | endif()
 18 | 
 19 | set(HDRS
 20 |   include/DLoopDetector/DLoopDetector.h         include/DLoopDetector/TemplatedLoopDetector.h)
 21 | 
 22 | set(DEPENDENCY_DIR ${CMAKE_CURRENT_BINARY_DIR}/dependencies)
 23 | set(DEPENDENCY_INSTALL_DIR ${DEPENDENCY_DIR}/install)
 24 | set(DEPENDENCY_CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE})
 25 | if(DEFINED OpenCV_DIR)
 26 |     set(DEPENDENCY_CMAKE_ARGS ${DEPENDENCY_CMAKE_ARGS} -DOpenCV_DIR=${OpenCV_DIR})
 27 | endif()
 28 | 
 29 | find_package(OpenCV REQUIRED)
 30 | include_directories(${OpenCV_INCLUDE_DIRS})
 31 | 
 32 | macro(GetDependency name other_dependency)
 33 |   find_package(${name} QUIET
 34 |     PATHS ${DEPENDENCY_INSTALL_DIR})
 35 |   if(${${name}_FOUND})
 36 |     message("${name} library found, using it from the system")
 37 |     include_directories(${${name}_INCLUDE_DIRS})
 38 |     add_custom_target(${name}_dep)
 39 |   else(${${name}_FOUND})
 40 |     message("${name} library not found in the system, it will be downloaded on build")
 41 |     option(DOWNLOAD_${name}_dependency "Download ${name} dependency" ON)
 42 |     if(${DOWNLOAD_${name}_dependency})
 43 |       if(NOT ${other_dependency})
 44 |         set(dependency ${other_dependency}_dep)
 45 |       endif()
 46 |       ExternalProject_Add(${name}
 47 |         PREFIX ${DEPENDENCY_DIR}
 48 |         GIT_REPOSITORY http://github.com/dorian3d/${name}
 49 |         GIT_TAG master
 50 |         INSTALL_DIR ${DEPENDENCY_INSTALL_DIR}
 51 |         CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=<INSTALL_DIR> ${DEPENDENCY_CMAKE_ARGS}
 52 |         DEPENDS ${dependency})
 53 |       add_custom_target(${name}_dep ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} DEPENDS ${name})
 54 |     else()
 55 |       message(SEND_ERROR "Please, activate DOWNLOAD_${name}_dependency option or download manually")
 56 |     endif(${DOWNLOAD_${name}_dependency})
 57 |   endif(${${name}_FOUND})
 58 | endmacro(GetDependency)
 59 | 
 60 | GetDependency(DLib "")
 61 | GetDependency(DBoW2 DLib)
 62 | add_custom_target(Dependencies ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} DEPENDS DBoW2_dep DLib_dep)
 63 | 
 64 | include_directories(include/DLoopDetector/)
 65 | 
 66 | if(BUILD_DemoBRIEF)
 67 |   add_executable(demo_brief demo/demo_brief.cpp)
 68 |   add_dependencies(demo_brief DLib_dep DBoW2_dep)
 69 |   target_link_libraries(demo_brief ${OpenCV_LIBS} ${DLib_LIBS} ${DBoW2_LIBS})
 70 |   target_include_directories(demo_brief PUBLIC ${DLib_INCLUDE_DIRS} ${DBoW2_INCLUDE_DIRS})
 71 |   set_target_properties(demo_brief PROPERTIES CXX_STANDARD 11)
 72 | endif(BUILD_DemoBRIEF)
 73 | 
 74 | #if(BUILD_DemoSURF)
 75 | #  add_executable(demo_surf demo/demo_surf.cpp)
 76 | #  target_link_libraries(demo_surf ${OpenCV_LIBS} ${DLib_LIBS} ${DBoW2_LIBS})
 77 | #endif(BUILD_DemoSURF)
 78 | 
 79 | if(BUILD_DemoBRIEF)
 80 |   set(RESOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/resources/)
 81 |   set(RESOURCE_FILE ${CMAKE_CURRENT_BINARY_DIR}/resources.tar.gz)
 82 |   if(NOT EXISTS ${RESOURCE_DIR})
 83 |     if(EXISTS ${RESOURCE_FILE})
 84 |       execute_process(COMMAND ${CMAKE_COMMAND} -E tar xzf ${RESOURCE_FILE})
 85 |     else()
 86 |       message("To run the demo application, please download \
 87 | https://drive.google.com/uc?export=download&id=1MpZwPjXDAUxKfSTpeCjG0PAUpaeWuo7D \
 88 | into ${RESOURCE_FILE} and run cmake again.")
 89 |     endif()
 90 |   endif(NOT EXISTS ${RESOURCE_DIR})
 91 | endif(BUILD_DemoBRIEF)
 92 | 
 93 | configure_file(src/DLoopDetector.cmake.in
 94 |   "${PROJECT_BINARY_DIR}/DLoopDetectorConfig.cmake" @ONLY)
 95 | 
 96 | install(DIRECTORY include/DLoopDetector DESTINATION ${CMAKE_INSTALL_PREFIX}/include)
 97 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/DLoopDetectorConfig.cmake"
 98 |   DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME})
 99 | install(FILES "${PROJECT_BINARY_DIR}/DLoopDetectorConfig.cmake" 
100 |   DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/cmake/DLoopDetector/)
101 | install(DIRECTORY ${DEPENDENCY_INSTALL_DIR}/ DESTINATION ${CMAKE_INSTALL_PREFIX} OPTIONAL)
102 | 


--------------------------------------------------------------------------------
/demo/demoDetector.h:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * File: demoDetector.h
  3 |  * Date: November 2011
  4 |  * Author: Dorian Galvez-Lopez
  5 |  * Description: demo application of DLoopDetector
  6 |  * License: see the LICENSE.txt file
  7 |  */
  8 | 
  9 | #ifndef __DEMO_DETECTOR__
 10 | #define __DEMO_DETECTOR__
 11 | 
 12 | #include <iostream>
 13 | #include <vector>
 14 | #include <string>
 15 | 
 16 | // OpenCV
 17 | #include <opencv2/core.hpp>
 18 | #include <opencv2/highgui.hpp>
 19 | 
 20 | // DLoopDetector and DBoW2
 21 | #include <DBoW2/DBoW2.h>
 22 | #include "DLoopDetector.h"
 23 | #include <DUtils/DUtils.h>
 24 | #include <DUtilsCV/DUtilsCV.h>
 25 | #include <DVision/DVision.h>
 26 | 
 27 | using namespace DLoopDetector;
 28 | using namespace DBoW2;
 29 | using namespace std;
 30 | 
 31 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 32 | 
 33 | /// Generic class to create functors to extract features
 34 | template<class TDescriptor>
 35 | class FeatureExtractor
 36 | {
 37 | public:
 38 |   /**
 39 |    * Extracts features
 40 |    * @param im image
 41 |    * @param keys keypoints extracted
 42 |    * @param descriptors descriptors extracted
 43 |    */
 44 |   virtual void operator()(const cv::Mat &im, 
 45 |     vector<cv::KeyPoint> &keys, vector<TDescriptor> &descriptors) const = 0;
 46 | };
 47 | 
 48 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 49 | 
 50 | /// @param TVocabulary vocabulary class (e.g: BriefVocabulary)
 51 | /// @param TDetector detector class (e.g: BriefLoopDetector)
 52 | /// @param TDescriptor descriptor class (e.g: bitset for Brief)
 53 | template<class TVocabulary, class TDetector, class TDescriptor>
 54 | /// Class to run the demo 
 55 | class demoDetector
 56 | {
 57 | public:
 58 | 
 59 |   /**
 60 |    * @param vocfile vocabulary file to load
 61 |    * @param imagedir directory to read images from
 62 |    * @param posefile pose file
 63 |    * @param width image width
 64 |    * @param height image height
 65 |    */
 66 |   demoDetector(const std::string &vocfile, const std::string &imagedir,
 67 |     const std::string &posefile, int width, int height, bool show);
 68 |     
 69 |   ~demoDetector(){}
 70 | 
 71 |   /**
 72 |    * Runs the demo
 73 |    * @param name demo name
 74 |    * @param extractor functor to extract features
 75 |    */
 76 |   void run(const std::string &name, 
 77 |     const FeatureExtractor<TDescriptor> &extractor);
 78 | 
 79 | protected:
 80 | 
 81 |   /**
 82 |    * Reads the robot poses from a file
 83 |    * @param filename file
 84 |    * @param xs
 85 |    * @param ys
 86 |    */
 87 |   void readPoseFile(const char *filename, std::vector<double> &xs, 
 88 |     std::vector<double> &ys) const;
 89 | 
 90 | protected:
 91 | 
 92 |   std::string m_vocfile;
 93 |   std::string m_imagedir;
 94 |   std::string m_posefile;
 95 |   int m_width;
 96 |   int m_height;
 97 |   bool m_show;
 98 | };
 99 | 
100 | // ---------------------------------------------------------------------------
101 | 
102 | template<class TVocabulary, class TDetector, class TDescriptor>
103 | demoDetector<TVocabulary, TDetector, TDescriptor>::demoDetector
104 |   (const std::string &vocfile, const std::string &imagedir,
105 |   const std::string &posefile, int width, int height, bool show)
106 |   : m_vocfile(vocfile), m_imagedir(imagedir), m_posefile(posefile),
107 |     m_width(width), m_height(height), m_show(show)
108 | {
109 | }
110 | 
111 | // ---------------------------------------------------------------------------
112 | 
113 | template<class TVocabulary, class TDetector, class TDescriptor>
114 | void demoDetector<TVocabulary, TDetector, TDescriptor>::run
115 |   (const std::string &name, const FeatureExtractor<TDescriptor> &extractor)
116 | {
117 |   cout << "DLoopDetector Demo" << endl 
118 |     << "Dorian Galvez-Lopez" << endl
119 |     << "http://doriangalvez.com" << endl << endl;
120 |   
121 |   // Set loop detector parameters
122 |   typename TDetector::Parameters params(m_height, m_width);
123 |   
124 |   // Parameters given by default are:
125 |   // use nss = true
126 |   // alpha = 0.3
127 |   // k = 3
128 |   // geom checking = GEOM_DI
129 |   // di levels = 0
130 |   
131 |   // We are going to change these values individually:
132 |   params.use_nss = true; // use normalized similarity score instead of raw score
133 |   params.alpha = 0.3; // nss threshold
134 |   params.k = 1; // a loop must be consistent with 1 previous matches
135 |   params.geom_check = GEOM_DI; // use direct index for geometrical checking
136 |   params.di_levels = 2; // use two direct index levels
137 |   
138 |   // To verify loops you can select one of the next geometrical checkings:
139 |   // GEOM_EXHAUSTIVE: correspondence points are computed by comparing all
140 |   //    the features between the two images.
141 |   // GEOM_FLANN: as above, but the comparisons are done with a Flann structure,
142 |   //    which makes them faster. However, creating the flann structure may
143 |   //    be slow.
144 |   // GEOM_DI: the direct index is used to select correspondence points between
145 |   //    those features whose vocabulary node at a certain level is the same.
146 |   //    The level at which the comparison is done is set by the parameter
147 |   //    di_levels:
148 |   //      di_levels = 0 -> features must belong to the same leaf (word).
149 |   //         This is the fastest configuration and the most restrictive one.
150 |   //      di_levels = l (l < L) -> node at level l starting from the leaves.
151 |   //         The higher l, the slower the geometrical checking, but higher
152 |   //         recall as well.
153 |   //         Here, L stands for the depth levels of the vocabulary tree.
154 |   //      di_levels = L -> the same as the exhaustive technique.
155 |   // GEOM_NONE: no geometrical checking is done.
156 |   //
157 |   // In general, with a 10^6 vocabulary, GEOM_DI with 2 <= di_levels <= 4 
158 |   // yields the best results in recall/time.
159 |   // Check the T-RO paper for more information.
160 |   //
161 |   
162 |   // Load the vocabulary to use
163 |   cout << "Loading " << name << " vocabulary..." << endl;
164 |   TVocabulary voc(m_vocfile);
165 |   
166 |   // Initiate loop detector with the vocabulary 
167 |   cout << "Processing sequence..." << endl;
168 |   TDetector detector(voc, params);
169 |   
170 |   // Process images
171 |   vector<cv::KeyPoint> keys;
172 |   vector<TDescriptor> descriptors;
173 | 
174 |   // load image filenames  
175 |   vector<string> filenames = 
176 |     DUtils::FileFunctions::Dir(m_imagedir.c_str(), ".png", true);
177 |   
178 |   // load robot poses
179 |   vector<double> xs, ys;
180 |   readPoseFile(m_posefile.c_str(), xs, ys);
181 |   
182 |   // we can allocate memory for the expected number of images
183 |   detector.allocate(filenames.size());
184 |   
185 |   // prepare visualization windows
186 |   DUtilsCV::GUI::tWinHandler win = "Current image";
187 |   DUtilsCV::GUI::tWinHandler winplot = "Trajectory";
188 |   
189 |   DUtilsCV::Drawing::Plot::Style normal_style(2); // thickness
190 |   DUtilsCV::Drawing::Plot::Style loop_style('r', 2); // color, thickness
191 |   
192 |   DUtilsCV::Drawing::Plot implot(240, 320,
193 |     - *std::max_element(xs.begin(), xs.end()),
194 |     - *std::min_element(xs.begin(), xs.end()),
195 |     *std::min_element(ys.begin(), ys.end()),
196 |     *std::max_element(ys.begin(), ys.end()), 20);
197 |   
198 |   // prepare profiler to measure times
199 |   DUtils::Profiler profiler;
200 |   
201 |   int count = 0;
202 |   
203 |   // go
204 |   for(unsigned int i = 0; i < filenames.size(); ++i)
205 |   {
206 |     cout << "Adding image " << i << ": " << filenames[i] << "... " << endl;
207 |     
208 |     // get image
209 |     cv::Mat im = cv::imread(filenames[i].c_str(), 0); // grey scale
210 |     
211 |     // show image
212 |     if(m_show)
213 |       DUtilsCV::GUI::showImage(im, true, &win, 10);
214 |     
215 |     // get features
216 |     profiler.profile("features");
217 |     extractor(im, keys, descriptors);
218 |     profiler.stop();
219 |         
220 |     // add image to the collection and check if there is some loop
221 |     DetectionResult result;
222 |     
223 |     profiler.profile("detection");
224 |     detector.detectLoop(keys, descriptors, result);
225 |     profiler.stop();
226 |     
227 |     if(result.detection())
228 |     {
229 |       cout << "- Loop found with image " << result.match << "!"
230 |         << endl;
231 |       ++count;
232 |     }
233 |     else
234 |     {
235 |       cout << "- No loop: ";
236 |       switch(result.status)
237 |       {
238 |         case CLOSE_MATCHES_ONLY:
239 |           cout << "All the images in the database are very recent" << endl;
240 |           break;
241 |           
242 |         case NO_DB_RESULTS:
243 |           cout << "There are no matches against the database (few features in"
244 |             " the image?)" << endl;
245 |           break;
246 |           
247 |         case LOW_NSS_FACTOR:
248 |           cout << "Little overlap between this image and the previous one"
249 |             << endl;
250 |           break;
251 |             
252 |         case LOW_SCORES:
253 |           cout << "No match reaches the score threshold (alpha: " <<
254 |             params.alpha << ")" << endl;
255 |           break;
256 |           
257 |         case NO_GROUPS:
258 |           cout << "Not enough close matches to create groups. "
259 |             << "Best candidate: " << result.match << endl;
260 |           break;
261 |           
262 |         case NO_TEMPORAL_CONSISTENCY:
263 |           cout << "No temporal consistency (k: " << params.k << "). "
264 |             << "Best candidate: " << result.match << endl;
265 |           break;
266 |           
267 |         case NO_GEOMETRICAL_CONSISTENCY:
268 |           cout << "No geometrical consistency. Best candidate: " 
269 |             << result.match << endl;
270 |           break;
271 |           
272 |         default:
273 |           break;
274 |       }
275 |     }
276 |     
277 |     cout << endl;
278 |     
279 |     // show trajectory
280 |     if(m_show && i > 0)
281 |     {
282 |       if(result.detection())
283 |         implot.line(-xs[i-1], ys[i-1], -xs[i], ys[i], loop_style);
284 |       else
285 |         implot.line(-xs[i-1], ys[i-1], -xs[i], ys[i], normal_style);
286 |       
287 |       DUtilsCV::GUI::showImage(implot.getImage(), true, &winplot, 10); 
288 |     }
289 |   }
290 |   
291 |   if(count == 0)
292 |   {
293 |     cout << "No loops found in this image sequence" << endl;
294 |   }
295 |   else
296 |   {
297 |     cout << count << " loops found in this image sequence!" << endl;
298 |   } 
299 | 
300 |   cout << endl << "Execution time:" << endl
301 |     << " - Feature computation: " << profiler.getMeanTime("features") * 1e3
302 |     << " ms/image" << endl
303 |     << " - Loop detection: " << profiler.getMeanTime("detection") * 1e3
304 |     << " ms/image" << endl;
305 | 
306 |   if(m_show) {
307 |     cout << endl << "Press a key to finish..." << endl;
308 |     DUtilsCV::GUI::showImage(implot.getImage(), true, &winplot, 0);
309 |   }
310 | }
311 | 
312 | // ---------------------------------------------------------------------------
313 | 
314 | template<class TVocabulary, class TDetector, class TDescriptor>
315 | void demoDetector<TVocabulary, TDetector, TDescriptor>::readPoseFile
316 |   (const char *filename, std::vector<double> &xs, std::vector<double> &ys)
317 |   const
318 | {
319 |   xs.clear();
320 |   ys.clear();
321 |   
322 |   fstream f(filename, ios::in);
323 |   
324 |   string s;
325 |   double ts, x, y, t;
326 |   while(!f.eof())
327 |   {
328 |     getline(f, s);
329 |     if(!f.eof() && !s.empty())
330 |     {
331 |       sscanf(s.c_str(), "%lf, %lf, %lf, %lf", &ts, &x, &y, &t);
332 |       xs.push_back(x);
333 |       ys.push_back(y);
334 |     }
335 |   }
336 |   
337 |   f.close();
338 | }
339 | 
340 | // ---------------------------------------------------------------------------
341 | 
342 | #endif
343 | 
344 | 


--------------------------------------------------------------------------------
/include/DLoopDetector/TemplatedLoopDetector.h:
--------------------------------------------------------------------------------
   1 | /**
   2 |  * File: TemplatedLoopDetector
   3 |  * Date: March 2011
   4 |  * Author: Dorian Galvez-Lopez
   5 |  * Description: templated loop detector
   6 |  * License: see the LICENSE.txt file
   7 |  *
   8 |  */
   9 | 
  10 | #ifndef __D_T_TEMPLATED_LOOP_DETECTOR__
  11 | #define __D_T_TEMPLATED_LOOP_DETECTOR__
  12 | 
  13 | #include <vector>
  14 | #include <numeric>
  15 | #include <fstream>
  16 | #include <string>
  17 | 
  18 | #include <opencv2/core.hpp>
  19 | #include <opencv2/features2d.hpp>
  20 | 
  21 | #include "TemplatedVocabulary.h"
  22 | #include "TemplatedDatabase.h"
  23 | #include "QueryResults.h"
  24 | #include "BowVector.h"
  25 | 
  26 | #include "DUtils.h"
  27 | #include "DUtilsCV.h"
  28 | #include "DVision.h"
  29 | 
  30 | using namespace std;
  31 | using namespace DUtils;
  32 | using namespace DBoW2;
  33 | 
  34 | namespace DLoopDetector {
  35 | 
  36 | 
  37 | /// Geometrical checking methods
  38 | enum GeometricalCheck
  39 | {
  40 |   /// Exhaustive search
  41 |   GEOM_EXHAUSTIVE,
  42 |   /// Use direct index
  43 |   GEOM_DI,
  44 |   /// Use a Flann structure
  45 |   GEOM_FLANN,
  46 |   /// Do not perform geometrical checking
  47 |   GEOM_NONE
  48 | };
  49 | 
  50 | /// Reasons for dismissing loops
  51 | enum DetectionStatus
  52 | {
  53 |   /// Loop correctly detected
  54 |   LOOP_DETECTED,
  55 |   /// All the matches are very recent
  56 |   CLOSE_MATCHES_ONLY,
  57 |   /// No matches against the database
  58 |   NO_DB_RESULTS,
  59 |   /// Score of current image against previous one too low
  60 |   LOW_NSS_FACTOR,
  61 |   /// Scores (or NS Scores) were below the alpha threshold
  62 |   LOW_SCORES,
  63 |   /// Not enough matches to create groups  
  64 |   NO_GROUPS,
  65 |   /// Not enough temporary consistent matches (k)
  66 |   NO_TEMPORAL_CONSISTENCY,
  67 |   /// The geometrical consistency failed
  68 |   NO_GEOMETRICAL_CONSISTENCY
  69 | };
  70 | 
  71 | /// Result of a detection
  72 | struct DetectionResult
  73 | {
  74 |   /// Detection status. LOOP_DETECTED iff loop detected
  75 |   DetectionStatus status;
  76 |   /// Query id
  77 |   EntryId query;
  78 |   /// Matched id if loop detected, otherwise, best candidate 
  79 |   EntryId match;
  80 |   
  81 |   /**
  82 |    * Checks if the loop was detected
  83 |    * @return true iff a loop was detected
  84 |    */
  85 |   inline bool detection() const
  86 |   {
  87 |     return status == LOOP_DETECTED;
  88 |   }
  89 | };
  90 | 
  91 | /// TDescriptor: class of descriptor
  92 | /// F: class of descriptor functions
  93 | template<class TDescriptor, class F>
  94 | /// Generic Loop detector
  95 | class TemplatedLoopDetector
  96 | {
  97 | public:
  98 |   
  99 |   /// Parameters to create a loop detector
 100 |   struct Parameters
 101 |   {
 102 |     /// Height of expected images
 103 |     int image_rows;
 104 |     /// Width of expected images
 105 |     int image_cols;
 106 |     
 107 |     // Main loop detector parameters
 108 |     
 109 |     /// Use normalized similarity score?
 110 |     bool use_nss;
 111 |     /// Alpha threshold
 112 |     float alpha;
 113 |     /// Min consistent matches to pass the temporal check
 114 |     int k;
 115 |     /// Geometrical check
 116 |     GeometricalCheck geom_check;
 117 |     /// If using direct index for geometrical checking, direct index levels
 118 |     int di_levels;
 119 |     
 120 |     // These are less deciding parameters of the system
 121 |     
 122 |     /// Distance between entries to be consider a match
 123 |     int dislocal;
 124 |     /// Max number of results from db queries to consider
 125 |     int max_db_results;
 126 |     /// Min raw score between current entry and previous one to consider a match 
 127 |     float min_nss_factor;
 128 |     /// Min number of close matches to consider some of them
 129 |     int min_matches_per_group; 
 130 |     /// Max separation between matches to consider them of the same group
 131 |     int max_intragroup_gap; 
 132 |     /// Max separation between groups of matches to consider them consistent
 133 |     int max_distance_between_groups;
 134 |     /// Max separation between two queries to consider them consistent
 135 |     int max_distance_between_queries; 
 136 |   
 137 |     // These are for the RANSAC to compute the F
 138 |     
 139 |     /// Min number of inliers when computing a fundamental matrix
 140 |     int min_Fpoints;
 141 |     /// Max number of iterations of RANSAC
 142 |     int max_ransac_iterations;
 143 |     /// Success probability of RANSAC
 144 |     double ransac_probability;
 145 |     /// Max reprojection error of fundamental matrices
 146 |     double max_reprojection_error;
 147 |     
 148 |     // This is to compute correspondences
 149 |     
 150 |     /// Max value of the neighbour-ratio of accepted correspondences
 151 |     double max_neighbor_ratio;
 152 |   
 153 |     /**
 154 |      * Creates parameters by default
 155 |      */ 
 156 |     Parameters();
 157 |     
 158 |     /**
 159 |      * Creates parameters by default
 160 |      * @param height image height
 161 |      * @param width image width
 162 |      * @param frequency set the value of some parameters according to the 
 163 |      *   expected working frequency (Hz) 
 164 |      * @param nss use normalized similarity score
 165 |      * @param _alpha alpha parameter
 166 |      * @param _k k parameter (number of temporary consistent matches)
 167 |      * @param geom type of geometrical check
 168 |      * @param dilevels direct index levels when geom == GEOM_DI
 169 |      */ 
 170 |     Parameters(int height, int width, float frequency = 1, bool nss = true,
 171 |       float _alpha = 0.3, int _k = 3, 
 172 |       GeometricalCheck geom = GEOM_DI, int dilevels = 0);
 173 |       
 174 |   private:
 175 |     /**
 176 |      * Sets some parameters according to the frequency
 177 |      * @param frequency
 178 |      */
 179 |     void set(float frequency);
 180 |   };
 181 |   
 182 | public:
 183 | 
 184 |   /**
 185 |    * Empty constructor
 186 |    */
 187 |   TemplatedLoopDetector(const Parameters &params = Parameters());
 188 | 
 189 |   /**
 190 |    * Creates a loop detector with the given parameters and with a BoW2 database 
 191 |    * with the given vocabulary
 192 |    * @param voc vocabulary
 193 |    * @param params loop detector parameters
 194 |    */
 195 |   TemplatedLoopDetector(const TemplatedVocabulary<TDescriptor, F> &voc,
 196 |     const Parameters &params = Parameters());
 197 |   
 198 |   /**
 199 |    * Creates a loop detector with a copy of the given database, but clearing
 200 |    * its contents
 201 |    * @param db database to copy
 202 |    * @param params loop detector parameters
 203 |    */
 204 |   TemplatedLoopDetector(const TemplatedDatabase<TDescriptor, F> &db,
 205 |     const Parameters &params = Parameters());
 206 | 
 207 |   /**
 208 |    * Creates a loop detector with a copy of the given database, but clearing
 209 |    * its contents
 210 |    * @param T class derived from TemplatedDatabase
 211 |    * @param db database to copy
 212 |    * @param params loop detector parameters
 213 |    */
 214 |   template<class T>
 215 |   TemplatedLoopDetector(const T &db, const Parameters &params = Parameters());
 216 | 
 217 |   /**
 218 |    * Destructor
 219 |    */
 220 |   virtual ~TemplatedLoopDetector(void);
 221 |   
 222 |   /**
 223 |    * Retrieves a reference to the database used by the loop detector
 224 |    * @return const reference to database
 225 |    */
 226 |   inline const TemplatedDatabase<TDescriptor, F>& getDatabase() const;
 227 |   
 228 |   /**
 229 |    * Retrieves a reference to the vocabulary used by the loop detector
 230 |    * @return const reference to vocabulary
 231 |    */
 232 |   inline const TemplatedVocabulary<TDescriptor, F>& getVocabulary() const;
 233 |   
 234 |   /**
 235 |    * Sets the database to use. The contents of the database and the detector
 236 |    * entries are cleared
 237 |    * @param T class derived from TemplatedDatabase
 238 |    * @param db database to copy
 239 |    */
 240 |   template<class T>
 241 |   void setDatabase(const T &db);
 242 |   
 243 |   /**
 244 |    * Sets a new DBoW2 database created from the given vocabulary
 245 |    * @param voc vocabulary to copy
 246 |    */
 247 |   void setVocabulary(const TemplatedVocabulary<TDescriptor, F>& voc);
 248 |   
 249 |   /**
 250 |    * Allocates some memory for the first entries
 251 |    * @param nentries number of expected entries
 252 |    * @param nkeys number of keypoints per image expected
 253 |    */
 254 |   void allocate(int nentries, int nkeys = 0);
 255 | 
 256 |   /**
 257 |    * Adds the given tuple <keys, descriptors, current_t> to the database
 258 |    * and returns the match if any
 259 |    * @param keys keypoints of the image
 260 |    * @param descriptors descriptors associated to the given keypoints
 261 |    * @param match (out) match or failing information
 262 |    * @return true iff there was match
 263 |    */
 264 |   bool detectLoop(const std::vector<cv::KeyPoint> &keys, 
 265 |     const std::vector<TDescriptor> &descriptors,
 266 |     DetectionResult &match);
 267 | 
 268 |   /**
 269 |    * Resets the detector and clears the database, such that the next entry
 270 |    * will be 0 again
 271 |    */
 272 |   inline void clear();
 273 | 
 274 | protected:
 275 |   
 276 |   /// Matching island
 277 |   struct tIsland
 278 |   {
 279 |     /// Island starting entry
 280 |     EntryId first;
 281 |     /// Island ending entry
 282 |     EntryId last;
 283 |     /// Island score
 284 |     double score; // score of island
 285 |     
 286 |     /// Entry of the island with the highest score
 287 |     EntryId best_entry; // id and score of the entry with the highest score
 288 |     /// Highest single score in the island
 289 |     double best_score;  // in the island
 290 |     
 291 |     /**
 292 |      * Creates an empty island
 293 |      */
 294 |     tIsland(){}
 295 |     
 296 |     /**
 297 |      * Creates an island
 298 |      * @param f first entry
 299 |      * @param l last entry
 300 |      */
 301 |     tIsland(EntryId f, EntryId l): first(f), last(l){}
 302 |     
 303 |     /**
 304 |      * Creates an island
 305 |      * @param f first entry
 306 |      * @param l last entry
 307 |      * @param s island score
 308 |      */
 309 |     tIsland(EntryId f, EntryId l, double s): first(f), last(l), score(s){}
 310 |     
 311 |     /**
 312 |      * Says whether this score is less than the score of another island
 313 |      * @param b
 314 |      * @return true iff this score < b.score
 315 |      */
 316 |     inline bool operator < (const tIsland &b) const
 317 |     {
 318 |       return this->score < b.score;
 319 |     }
 320 |     
 321 |     /**
 322 |      * Says whether this score is greater than the score of another island
 323 |      * @param b
 324 |      * @return true iff this score > b.score
 325 |      */
 326 |     inline bool operator > (const tIsland &b) const
 327 |     {
 328 |       return this->score > b.score;
 329 |     }
 330 |     
 331 |     /** 
 332 |      * Returns true iff a > b
 333 |      * This function is used to sort in descending order
 334 |      * @param a
 335 |      * @param b
 336 |      * @return a > b
 337 |      */
 338 |     static inline bool gt(const tIsland &a, const tIsland &b)
 339 |     {
 340 |       return a.score > b.score;
 341 |     }
 342 |         
 343 |     /**
 344 |      * Returns true iff entry ids of a are less then those of b.
 345 |      * Assumes there is no overlap between the islands
 346 |      * @param a
 347 |      * @param b
 348 |      * @return a.first < b.first
 349 |      */
 350 |     static inline bool ltId(const tIsland &a, const tIsland &b)
 351 |     {
 352 |       return a.first < b.first;
 353 |     }
 354 |     
 355 |     /**
 356 |      * Returns the length of the island
 357 |      * @return length of island
 358 |      */
 359 |     inline int length() const { return last - first + 1; }
 360 |     
 361 |     /**
 362 |      * Returns a printable version of the island
 363 |      * @return printable island
 364 |      */
 365 |     std::string toString() const
 366 |     {
 367 |       stringstream ss;
 368 |       ss << "[" << first << "-" << last << ": " << score << " | best: <"
 369 |         << best_entry << ": " << best_score << "> ] ";
 370 |       return ss.str();
 371 |     }
 372 |   };
 373 |   
 374 |   /// Temporal consistency window
 375 |   struct tTemporalWindow
 376 |   {
 377 |     /// Island matched in the last query
 378 |     tIsland last_matched_island;
 379 |     /// Last query id
 380 |     EntryId last_query_id;
 381 |     /// Number of consistent entries in the window
 382 |     int nentries;
 383 |     
 384 |     /**
 385 |      * Creates an empty temporal window
 386 |      */
 387 |     tTemporalWindow(): nentries(0) {}
 388 |   };
 389 |   
 390 |   
 391 | protected:
 392 |   
 393 |   /**
 394 |    * Removes from q those results whose score is lower than threshold
 395 |    * (that should be alpha * ns_factor)
 396 |    * @param q results from query
 397 |    * @param threshold min value of the resting results
 398 |    */
 399 |   void removeLowScores(QueryResults &q, double threshold) const;
 400 |   
 401 |   /**
 402 |    * Returns the islands of the given matches in ascending order of entry ids
 403 |    * @param q 
 404 |    * @param islands (out) computed islands
 405 |    */
 406 |   void computeIslands(QueryResults &q, vector<tIsland> &islands) const;
 407 |   
 408 |   /**
 409 |    * Returns the score of the island composed of the entries of q whose indices
 410 |    * are in [i_first, i_last] (both included)
 411 |    * @param q
 412 |    * @param i_first first index of q of the island
 413 |    * @param i_last last index of q of the island
 414 |    * @return island score
 415 |    */
 416 |   double calculateIslandScore(const QueryResults &q, unsigned int i_first, 
 417 |     unsigned int i_last) const;
 418 | 
 419 |   /**
 420 |    * Updates the temporal window by adding the given match <island, id>, such 
 421 |    * that the window will contain only those islands which are consistent
 422 |    * @param matched_island
 423 |    * @param entry_id
 424 |    */
 425 |   void updateTemporalWindow(const tIsland &matched_island, EntryId entry_id);
 426 |   
 427 |   /**
 428 |    * Returns the number of consistent islands in the temporal window
 429 |    * @return number of temporal consistent islands
 430 |    */
 431 |   inline int getConsistentEntries() const
 432 |   {
 433 |     return m_window.nentries;
 434 |   }
 435 |   
 436 |   /**
 437 |    * Check if an old entry is geometrically consistent (by calculating a 
 438 |    * fundamental matrix) with the given set of keys and descriptors
 439 |    * @param old_entry entry id of the stored image to check
 440 |    * @param keys current keypoints
 441 |    * @param descriptors current descriptors associated to the given keypoints
 442 |    * @param curvec feature vector of the current entry 
 443 |    */
 444 |   bool isGeometricallyConsistent_DI(EntryId old_entry, 
 445 |     const std::vector<cv::KeyPoint> &keys, 
 446 |     const std::vector<TDescriptor> &descriptors, 
 447 |     const FeatureVector &curvec) const;
 448 |   
 449 |   /**
 450 |    * Checks if an old entry is geometrically consistent (by using FLANN and 
 451 |    * computing an essential matrix by using the neighbour ratio 0.6) 
 452 |    * with the given set of keys and descriptors
 453 |    * @param old_entry entry id of the stored image to check
 454 |    * @param keys current keypoints
 455 |    * @param descriptors current descriptors
 456 |    * @param flann_structure flann structure with the descriptors of the current entry
 457 |    */
 458 |   bool isGeometricallyConsistent_Flann(EntryId old_entry,
 459 |     const std::vector<cv::KeyPoint> &keys, 
 460 |     const std::vector<TDescriptor> &descriptors,
 461 |     cv::FlannBasedMatcher &flann_structure) const;
 462 | 
 463 |   /**
 464 |    * Creates a flann structure from a set of descriptors to perform queries
 465 |    * @param descriptors
 466 |    * @param flann_structure (out) flann matcher
 467 |    */
 468 |   void getFlannStructure(const std::vector<TDescriptor> &descriptors, 
 469 |     cv::FlannBasedMatcher &flann_structure) const;
 470 | 
 471 |   /**
 472 |    * Check if an old entry is geometrically consistent (by calculating a
 473 |    * fundamental matrix from left-right correspondences) with the given set 
 474 |    * of keys and descriptors,
 475 |    * without using the direct index
 476 |    * @param old_keys keys of old entry
 477 |    * @param old_descriptors descriptors of old keys
 478 |    * @param cur_keys keys of current entry
 479 |    * @param cur_descriptors descriptors of cur keys
 480 |    */
 481 |   bool isGeometricallyConsistent_Exhaustive(
 482 |     const std::vector<cv::KeyPoint> &old_keys,
 483 |     const std::vector<TDescriptor> &old_descriptors,
 484 |     const std::vector<cv::KeyPoint> &cur_keys,
 485 |     const std::vector<TDescriptor> &cur_descriptors) const; 
 486 | 
 487 |   /**
 488 |    * Calculate the matches between the descriptors A[i_A] and the descriptors
 489 |    * B[i_B]. Applies a left-right matching without neighbour ratio 
 490 |    * @param A set A of descriptors
 491 |    * @param i_A only descriptors A[i_A] will be checked
 492 |    * @param B set B of descriptors
 493 |    * @param i_B only descriptors B[i_B] will be checked
 494 |    * @param i_match_A (out) indices of descriptors matched (s.t. A[i_match_A])
 495 |    * @param i_match_B (out) indices of descriptors matched (s.t. B[i_match_B])
 496 |    */
 497 |   void getMatches_neighratio(const std::vector<TDescriptor> &A, 
 498 |     const vector<unsigned int> &i_A, const vector<TDescriptor> &B,
 499 |     const vector<unsigned int> &i_B,
 500 |     vector<unsigned int> &i_match_A, vector<unsigned int> &i_match_B) const;
 501 | 
 502 | protected:
 503 | 
 504 |   /// Database
 505 |   // The loop detector stores its own copy of the database
 506 |   TemplatedDatabase<TDescriptor,F> *m_database;
 507 |   
 508 |   /// KeyPoints of images
 509 |   vector<vector<cv::KeyPoint> > m_image_keys;
 510 |   
 511 |   /// Descriptors of images
 512 |   vector<vector<TDescriptor> > m_image_descriptors;
 513 |   
 514 |   /// Last bow vector added to database
 515 |   BowVector m_last_bowvec;
 516 |   
 517 |   /// Temporal consistency window
 518 |   tTemporalWindow m_window;
 519 |   
 520 |   /// Parameters of loop detector
 521 |   Parameters m_params;
 522 |   
 523 |   /// To compute the fundamental matrix
 524 |   DVision::FSolver m_fsolver;
 525 |   
 526 | };
 527 | 
 528 | // --------------------------------------------------------------------------
 529 | 
 530 | template <class TDescriptor, class F> 
 531 | TemplatedLoopDetector<TDescriptor,F>::Parameters::Parameters():
 532 |   use_nss(true), alpha(0.3), k(4), geom_check(GEOM_DI), di_levels(0)
 533 | {
 534 |   set(1);
 535 | }
 536 | 
 537 | // --------------------------------------------------------------------------
 538 | 
 539 | template <class TDescriptor, class F> 
 540 | TemplatedLoopDetector<TDescriptor,F>::Parameters::Parameters
 541 |   (int height, int width, float frequency, bool nss, float _alpha, 
 542 |   int _k, GeometricalCheck geom, int dilevels)
 543 |   : image_rows(height), image_cols(width), use_nss(nss), alpha(_alpha), k(_k),
 544 |     geom_check(geom), di_levels(dilevels)
 545 | {
 546 |   set(frequency);
 547 | }
 548 | 
 549 | // --------------------------------------------------------------------------
 550 | 
 551 | template <class TDescriptor, class F> 
 552 | void TemplatedLoopDetector<TDescriptor,F>::Parameters::set(float f)
 553 | {
 554 |   dislocal = 20 * f;
 555 |   max_db_results = 50 * f;
 556 |   min_nss_factor = 0.005;
 557 |   min_matches_per_group = f;
 558 |   max_intragroup_gap = 3 * f;
 559 |   max_distance_between_groups = 3 * f;
 560 |   max_distance_between_queries = 2 * f; 
 561 | 
 562 |   min_Fpoints = 12;
 563 |   max_ransac_iterations = 500;
 564 |   ransac_probability = 0.99;
 565 |   max_reprojection_error = 2.0;
 566 |   
 567 |   max_neighbor_ratio = 0.6;
 568 | }
 569 | 
 570 | // --------------------------------------------------------------------------
 571 | 
 572 | template<class TDescriptor, class F>
 573 | TemplatedLoopDetector<TDescriptor,F>::TemplatedLoopDetector
 574 |   (const Parameters &params)
 575 |   : m_database(NULL), m_params(params)
 576 | {
 577 | }
 578 | 
 579 | // --------------------------------------------------------------------------
 580 | 
 581 | template<class TDescriptor, class F>
 582 | TemplatedLoopDetector<TDescriptor,F>::TemplatedLoopDetector
 583 |   (const TemplatedVocabulary<TDescriptor, F> &voc, const Parameters &params)
 584 |   : m_params(params) 
 585 | {
 586 |   m_database = new TemplatedDatabase<TDescriptor, F>(voc, 
 587 |     params.geom_check == GEOM_DI, params.di_levels);
 588 |   
 589 |   m_fsolver.setImageSize(params.image_cols, params.image_rows);
 590 | }
 591 | 
 592 | // --------------------------------------------------------------------------
 593 | 
 594 | template<class TDescriptor, class F>
 595 | void TemplatedLoopDetector<TDescriptor,F>::setVocabulary
 596 |   (const TemplatedVocabulary<TDescriptor, F>& voc)
 597 | {
 598 |   delete m_database;
 599 |   m_database = new TemplatedDatabase<TDescriptor, F>(voc, 
 600 |     m_params.geom_check == GEOM_DI, m_params.di_levels);
 601 | }
 602 | 
 603 | // --------------------------------------------------------------------------
 604 | 
 605 | template<class TDescriptor, class F>
 606 | TemplatedLoopDetector<TDescriptor, F>::TemplatedLoopDetector
 607 |   (const TemplatedDatabase<TDescriptor, F> &db, const Parameters &params)
 608 |   : m_params(params)
 609 | {
 610 |   m_database = new TemplatedDatabase<TDescriptor, F>(db.getVocabulary(),
 611 |     params.geom_check == GEOM_DI, params.di_levels);
 612 |   
 613 |   m_fsolver.setImageSize(params.image_cols, params.image_rows);
 614 | }
 615 | 
 616 | // --------------------------------------------------------------------------
 617 | 
 618 | template<class TDescriptor, class F>
 619 | template<class T>
 620 | TemplatedLoopDetector<TDescriptor, F>::TemplatedLoopDetector
 621 |   (const T &db, const Parameters &params)
 622 |   : m_params(params)
 623 | {
 624 |   m_database = new T(db);
 625 |   m_database->clear();
 626 |   
 627 |   m_fsolver.setImageSize(params.image_cols, params.image_rows);
 628 | }
 629 | 
 630 | // --------------------------------------------------------------------------
 631 | 
 632 | template<class TDescriptor, class F>
 633 | template<class T>
 634 | void TemplatedLoopDetector<TDescriptor, F>::setDatabase(const T &db)
 635 | {
 636 |   delete m_database;
 637 |   m_database = new T(db);
 638 |   clear();
 639 | }
 640 | 
 641 | // --------------------------------------------------------------------------
 642 | 
 643 | template<class TDescriptor, class F>
 644 | TemplatedLoopDetector<TDescriptor, F>::~TemplatedLoopDetector(void)
 645 | {
 646 |   delete m_database;
 647 |   m_database = NULL;
 648 | }
 649 | 
 650 | // --------------------------------------------------------------------------
 651 | 
 652 | template<class TDescriptor, class F>
 653 | void TemplatedLoopDetector<TDescriptor,F>::allocate
 654 |   (int nentries, int nkeys)
 655 | {
 656 |   const int sz = (const int)m_image_keys.size();
 657 |   
 658 |   if(sz < nentries)
 659 |   {
 660 |     m_image_keys.resize(nentries);
 661 |     m_image_descriptors.resize(nentries);
 662 |   }
 663 |   
 664 |   if(nkeys > 0)
 665 |   {
 666 |     for(int i = sz; i < nentries; ++i)
 667 |     {
 668 |       m_image_keys[i].reserve(nkeys);
 669 |       m_image_descriptors[i].reserve(nkeys);
 670 |     }
 671 |   }
 672 |   
 673 |   m_database->allocate(nentries, nkeys);
 674 | }
 675 | 
 676 | // --------------------------------------------------------------------------
 677 | 
 678 | template<class TDescriptor, class F>
 679 | inline const TemplatedDatabase<TDescriptor, F>& 
 680 | TemplatedLoopDetector<TDescriptor, F>::getDatabase() const
 681 | {
 682 |   return *m_database;
 683 | }
 684 | 
 685 | // --------------------------------------------------------------------------
 686 | 
 687 | template<class TDescriptor, class F>
 688 | inline const TemplatedVocabulary<TDescriptor, F>& 
 689 | TemplatedLoopDetector<TDescriptor, F>::getVocabulary() const
 690 | {
 691 |   return m_database->getVocabulary();
 692 | }
 693 | 
 694 | // --------------------------------------------------------------------------
 695 | 
 696 | template<class TDescriptor, class F>
 697 | bool TemplatedLoopDetector<TDescriptor, F>::detectLoop(
 698 |   const std::vector<cv::KeyPoint> &keys, 
 699 |   const std::vector<TDescriptor> &descriptors,
 700 |   DetectionResult &match)
 701 | {
 702 |   EntryId entry_id = m_database->size();
 703 |   match.query = entry_id;
 704 |   
 705 |   BowVector bowvec;
 706 |   FeatureVector featvec;
 707 |   
 708 |   if(m_params.geom_check == GEOM_DI)
 709 |     m_database->getVocabulary()->transform(descriptors, bowvec, featvec,
 710 |       m_params.di_levels);
 711 |   else
 712 |     m_database->getVocabulary()->transform(descriptors, bowvec);
 713 | 
 714 |   if((int)entry_id <= m_params.dislocal)
 715 |   {
 716 |     // only add the entry to the database and finish
 717 |     m_database->add(bowvec, featvec);
 718 |     match.status = CLOSE_MATCHES_ONLY;
 719 |   }
 720 |   else
 721 |   {
 722 |     int max_id = (int)entry_id - m_params.dislocal;
 723 |     
 724 |     QueryResults qret;
 725 |     m_database->query(bowvec, qret, m_params.max_db_results, max_id);
 726 | 
 727 |     // update database
 728 |     m_database->add(bowvec, featvec); // returns entry_id
 729 |     
 730 |     if(!qret.empty())
 731 |     {
 732 |       // factor to compute normalized similarity score, if necessary
 733 |       double ns_factor = 1.0;
 734 |       
 735 |       if(m_params.use_nss)
 736 |       {
 737 |         ns_factor = m_database->getVocabulary()->score(bowvec, m_last_bowvec);
 738 |       }
 739 |       
 740 |       if(!m_params.use_nss || ns_factor >= m_params.min_nss_factor)
 741 |       {
 742 |         // scores in qret must be divided by ns_factor to obtain the
 743 |         // normalized similarity score, but we can
 744 |         // speed this up by moving ns_factor to alpha's
 745 |         
 746 |         // remove those scores whose nss is lower than alpha
 747 |         // (ret is sorted in descending score order now)
 748 |         removeLowScores(qret, m_params.alpha * ns_factor);
 749 |         
 750 |         if(!qret.empty())
 751 |         {
 752 |           // the best candidate is the one with highest score by now
 753 |           match.match = qret[0].Id;
 754 |           
 755 |           // compute islands
 756 |           vector<tIsland> islands;
 757 |           computeIslands(qret, islands); 
 758 |           // this modifies qret and changes the score order
 759 |           
 760 |           // get best island
 761 |           if(!islands.empty())
 762 |           {
 763 |             const tIsland& island = 
 764 |               *std::max_element(islands.begin(), islands.end());
 765 |             
 766 |             // check temporal consistency of this island
 767 |             updateTemporalWindow(island, entry_id);
 768 |             
 769 |             // get the best candidate (maybe match)
 770 |             match.match = island.best_entry;
 771 |             
 772 |             if(getConsistentEntries() > m_params.k)
 773 |             {
 774 |               // candidate loop detected
 775 |               // check geometry
 776 |               bool detection;
 777 | 
 778 |               if(m_params.geom_check == GEOM_DI)
 779 |               {
 780 |                 // all the DI stuff is implicit in the database
 781 |                 detection = isGeometricallyConsistent_DI(island.best_entry, 
 782 |                   keys, descriptors, featvec);
 783 |               }
 784 |               else if(m_params.geom_check == GEOM_FLANN)
 785 |               {
 786 |                 cv::FlannBasedMatcher flann_structure;
 787 |                 getFlannStructure(descriptors, flann_structure);
 788 |                             
 789 |                 detection = isGeometricallyConsistent_Flann(island.best_entry, 
 790 |                   keys, descriptors, flann_structure);
 791 |               }
 792 |               else if(m_params.geom_check == GEOM_EXHAUSTIVE)
 793 |               { 
 794 |                 detection = isGeometricallyConsistent_Exhaustive(
 795 |                   m_image_keys[island.best_entry], 
 796 |                   m_image_descriptors[island.best_entry],
 797 |                   keys, descriptors);            
 798 |               }
 799 |               else // GEOM_NONE, accept the match
 800 |               {
 801 |                 detection = true;
 802 |               }
 803 |               
 804 |               if(detection)
 805 |               {
 806 |                 match.status = LOOP_DETECTED;
 807 |               }
 808 |               else
 809 |               {
 810 |                 match.status = NO_GEOMETRICAL_CONSISTENCY;
 811 |               }
 812 |               
 813 |             } // if enough temporal matches
 814 |             else
 815 |             {
 816 |               match.status = NO_TEMPORAL_CONSISTENCY;
 817 |             }
 818 |             
 819 |           } // if there is some island
 820 |           else
 821 |           {
 822 |             match.status = NO_GROUPS;
 823 |           }
 824 |         } // if !qret empty after removing low scores
 825 |         else
 826 |         {
 827 |           match.status = LOW_SCORES;
 828 |         }
 829 |       } // if (ns_factor > min normal score)
 830 |       else
 831 |       {
 832 |         match.status = LOW_NSS_FACTOR;
 833 |       }
 834 |     } // if(!qret.empty())
 835 |     else
 836 |     {
 837 |       match.status = NO_DB_RESULTS;
 838 |     }
 839 |   }
 840 | 
 841 |   // update record
 842 |   // m_image_keys and m_image_descriptors have the same length
 843 |   if(m_image_keys.size() == entry_id)
 844 |   {
 845 |     m_image_keys.push_back(keys);
 846 |     m_image_descriptors.push_back(descriptors);
 847 |   }
 848 |   else
 849 |   {
 850 |     m_image_keys[entry_id] = keys;
 851 |     m_image_descriptors[entry_id] = descriptors;
 852 |   }
 853 |   
 854 |   // store this bowvec if we are going to use it in next iteratons
 855 |   if(m_params.use_nss && (int)entry_id + 1 > m_params.dislocal)
 856 |   {
 857 |     m_last_bowvec = bowvec;
 858 |   }
 859 | 
 860 |   return match.detection();
 861 | }
 862 | 
 863 | // --------------------------------------------------------------------------
 864 | 
 865 | template<class TDescriptor, class F>
 866 | inline void TemplatedLoopDetector<TDescriptor, F>::clear()
 867 | {
 868 |   m_database->clear();
 869 |   m_window.nentries = 0;
 870 | }
 871 | 
 872 | // --------------------------------------------------------------------------
 873 | 
 874 | template<class TDescriptor, class F>
 875 | void TemplatedLoopDetector<TDescriptor, F>::computeIslands
 876 |   (QueryResults &q, vector<tIsland> &islands) const
 877 | {
 878 |   islands.clear();
 879 |   
 880 |   if(q.size() == 1)
 881 |   {
 882 |     islands.push_back(tIsland(q[0].Id, q[0].Id, calculateIslandScore(q, 0, 0)));
 883 |     islands.back().best_entry = q[0].Id;
 884 |     islands.back().best_score = q[0].Score;
 885 |   }
 886 |   else if(!q.empty())
 887 |   {
 888 |     // sort query results in ascending order of ids
 889 |     std::sort(q.begin(), q.end(), Result::ltId);
 890 |     
 891 |     // create long enough islands
 892 |     QueryResults::const_iterator dit = q.begin();
 893 |     int first_island_entry = dit->Id;
 894 |     int last_island_entry = dit->Id;
 895 |     
 896 |     // these are indices of q
 897 |     unsigned int i_first = 0;
 898 |     unsigned int i_last = 0;
 899 |     
 900 |     double best_score = dit->Score;
 901 |     EntryId best_entry = dit->Id;
 902 | 
 903 |     ++dit;
 904 |     for(unsigned int idx = 1; dit != q.end(); ++dit, ++idx)
 905 |     {
 906 |       if((int)dit->Id - last_island_entry < m_params.max_intragroup_gap)
 907 |       {
 908 |         // go on until find the end of the island
 909 |         last_island_entry = dit->Id;
 910 |         i_last = idx;
 911 |         if(dit->Score > best_score)
 912 |         {
 913 |           best_score = dit->Score;
 914 |           best_entry = dit->Id;
 915 |         }
 916 |       }
 917 |       else
 918 |       {
 919 |         // end of island reached
 920 |         int length = last_island_entry - first_island_entry + 1;
 921 |         if(length >= m_params.min_matches_per_group)
 922 |         {
 923 |           islands.push_back( tIsland(first_island_entry, last_island_entry,
 924 |             calculateIslandScore(q, i_first, i_last)) );
 925 |           
 926 |           islands.back().best_score = best_score;
 927 |           islands.back().best_entry = best_entry;
 928 |         }
 929 |         
 930 |         // prepare next island
 931 |         first_island_entry = last_island_entry = dit->Id;
 932 |         i_first = i_last = idx;
 933 |         best_score = dit->Score;
 934 |         best_entry = dit->Id;
 935 |       }
 936 |     }
 937 |     // add last island
 938 |     if(last_island_entry - first_island_entry + 1 >= 
 939 |       m_params.min_matches_per_group)
 940 |     {
 941 |       islands.push_back( tIsland(first_island_entry, last_island_entry,
 942 |         calculateIslandScore(q, i_first, i_last)) );
 943 |         
 944 |       islands.back().best_score = best_score;
 945 |       islands.back().best_entry = best_entry;
 946 |     }
 947 |   }
 948 |   
 949 | }
 950 | 
 951 | // --------------------------------------------------------------------------
 952 | 
 953 | template<class TDescriptor, class F>
 954 | double TemplatedLoopDetector<TDescriptor, F>::calculateIslandScore(
 955 |   const QueryResults &q, unsigned int i_first, unsigned int i_last) const
 956 | {
 957 |   // get the sum of the scores
 958 |   double sum = 0;
 959 |   while(i_first <= i_last) sum += q[i_first++].Score;
 960 |   return sum;
 961 | }
 962 | 
 963 | // --------------------------------------------------------------------------
 964 | 
 965 | template<class TDescriptor, class F>
 966 | void TemplatedLoopDetector<TDescriptor, F>::updateTemporalWindow
 967 |   (const tIsland &matched_island, EntryId entry_id)
 968 | {
 969 |   // if m_window.nentries > 0, island > m_window.last_matched_island and
 970 |   // entry_id > m_window.last_query_id hold
 971 |   
 972 |   if(m_window.nentries == 0 || int(entry_id - m_window.last_query_id)
 973 |     > m_params.max_distance_between_queries)
 974 |   {
 975 |     m_window.nentries = 1;
 976 |   }
 977 |   else
 978 |   {
 979 |     EntryId a1 = m_window.last_matched_island.first;
 980 |     EntryId a2 = m_window.last_matched_island.last;
 981 |     EntryId b1 = matched_island.first;
 982 |     EntryId b2 = matched_island.last;
 983 |     
 984 |     bool fit = (b1 <= a1 && a1 <= b2) || (a1 <= b1 && b1 <= a2);
 985 | 
 986 |     if(!fit)
 987 |     {
 988 |       int d1 = (int)a1 - (int)b2;
 989 |       int d2 = (int)b1 - (int)a2;
 990 |       int gap = (d1 > d2 ? d1 : d2);
 991 |       
 992 |       fit = (gap <= m_params.max_distance_between_groups);
 993 |     }
 994 |     
 995 |     if(fit) m_window.nentries++;
 996 |     else m_window.nentries = 1;
 997 |   }
 998 |   
 999 |   m_window.last_matched_island = matched_island;
1000 |   m_window.last_query_id = entry_id;
1001 | }
1002 | 
1003 | // --------------------------------------------------------------------------
1004 | 
1005 | template<class TDescriptor, class F>
1006 | bool TemplatedLoopDetector<TDescriptor, F>::isGeometricallyConsistent_DI(
1007 |   EntryId old_entry, const std::vector<cv::KeyPoint> &keys, 
1008 |   const std::vector<TDescriptor> &descriptors, 
1009 |   const FeatureVector &bowvec) const
1010 | {
1011 |   const FeatureVector &oldvec = m_database->retrieveFeatures(old_entry);
1012 |   
1013 |   // for each word in common, get the closest descriptors
1014 |   
1015 |   vector<unsigned int> i_old, i_cur;
1016 |   
1017 |   FeatureVector::const_iterator old_it, cur_it; 
1018 |   const FeatureVector::const_iterator old_end = oldvec.end();
1019 |   const FeatureVector::const_iterator cur_end = bowvec.end();
1020 |   
1021 |   old_it = oldvec.begin();
1022 |   cur_it = bowvec.begin();
1023 |   
1024 |   while(old_it != old_end && cur_it != cur_end)
1025 |   {
1026 |     if(old_it->first == cur_it->first)
1027 |     {
1028 |       // compute matches between 
1029 |       // features old_it->second of m_image_keys[old_entry] and
1030 |       // features cur_it->second of keys
1031 |       vector<unsigned int> i_old_now, i_cur_now;
1032 |       
1033 |       getMatches_neighratio(
1034 |         m_image_descriptors[old_entry], old_it->second, 
1035 |         descriptors, cur_it->second,  
1036 |         i_old_now, i_cur_now);
1037 |       
1038 |       i_old.insert(i_old.end(), i_old_now.begin(), i_old_now.end());
1039 |       i_cur.insert(i_cur.end(), i_cur_now.begin(), i_cur_now.end());
1040 |       
1041 |       // move old_it and cur_it forward
1042 |       ++old_it;
1043 |       ++cur_it;
1044 |     }
1045 |     else if(old_it->first < cur_it->first)
1046 |     {
1047 |       // move old_it forward
1048 |       old_it = oldvec.lower_bound(cur_it->first);
1049 |       // old_it = (first element >= cur_it.id)
1050 |     }
1051 |     else
1052 |     {
1053 |       // move cur_it forward
1054 |       cur_it = bowvec.lower_bound(old_it->first);
1055 |       // cur_it = (first element >= old_it.id)
1056 |     }
1057 |   }
1058 |   
1059 |   // calculate now the fundamental matrix
1060 |   if((int)i_old.size() >= m_params.min_Fpoints)
1061 |   {
1062 |     vector<cv::Point2f> old_points, cur_points;
1063 |     
1064 |     // add matches to the vectors to calculate the fundamental matrix
1065 |     vector<unsigned int>::const_iterator oit, cit;
1066 |     oit = i_old.begin();
1067 |     cit = i_cur.begin();
1068 |     
1069 |     for(; oit != i_old.end(); ++oit, ++cit)
1070 |     {
1071 |       const cv::KeyPoint &old_k = m_image_keys[old_entry][*oit];
1072 |       const cv::KeyPoint &cur_k = keys[*cit];
1073 |       
1074 |       old_points.push_back(old_k.pt);
1075 |       cur_points.push_back(cur_k.pt);
1076 |     }
1077 |   
1078 |     cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]);
1079 |     cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]);
1080 |     
1081 |     return m_fsolver.checkFundamentalMat(oldMat, curMat, 
1082 |       m_params.max_reprojection_error, m_params.min_Fpoints,
1083 |       m_params.ransac_probability, m_params.max_ransac_iterations);
1084 |   }
1085 |   
1086 |   return false;
1087 | }
1088 | 
1089 | // --------------------------------------------------------------------------
1090 | 
1091 | template<class TDescriptor, class F>
1092 | bool TemplatedLoopDetector<TDescriptor, F>::
1093 | isGeometricallyConsistent_Exhaustive(
1094 |   const std::vector<cv::KeyPoint> &old_keys,
1095 |   const std::vector<TDescriptor> &old_descriptors,
1096 |   const std::vector<cv::KeyPoint> &cur_keys,
1097 |   const std::vector<TDescriptor> &cur_descriptors) const
1098 | {
1099 |   vector<unsigned int> i_old, i_cur;
1100 |   vector<unsigned int> i_all_old, i_all_cur;
1101 |   
1102 |   i_all_old.reserve(old_keys.size());
1103 |   i_all_cur.reserve(cur_keys.size());
1104 |   
1105 |   for(unsigned int i = 0; i < old_keys.size(); ++i)
1106 |   {
1107 |     i_all_old.push_back(i);
1108 |   }
1109 |   
1110 |   for(unsigned int i = 0; i < cur_keys.size(); ++i)
1111 |   {
1112 |     i_all_cur.push_back(i);
1113 |   }
1114 |   
1115 |   getMatches_neighratio(old_descriptors, i_all_old, 
1116 |     cur_descriptors, i_all_cur,  i_old, i_cur);
1117 |   
1118 |   if((int)i_old.size() >= m_params.min_Fpoints)
1119 |   {
1120 |     // add matches to the vectors to calculate the fundamental matrix
1121 |     vector<unsigned int>::const_iterator oit, cit;
1122 |     oit = i_old.begin();
1123 |     cit = i_cur.begin();
1124 |     
1125 |     vector<cv::Point2f> old_points, cur_points;
1126 |     old_points.reserve(i_old.size());
1127 |     cur_points.reserve(i_cur.size());
1128 |     
1129 |     for(; oit != i_old.end(); ++oit, ++cit)
1130 |     {
1131 |       const cv::KeyPoint &old_k = old_keys[*oit];
1132 |       const cv::KeyPoint &cur_k = cur_keys[*cit];
1133 |       
1134 |       old_points.push_back(old_k.pt);
1135 |       cur_points.push_back(cur_k.pt);
1136 |     }
1137 |   
1138 |     cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]);
1139 |     cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]);
1140 |     
1141 |     return m_fsolver.checkFundamentalMat(oldMat, curMat, 
1142 |       m_params.max_reprojection_error, m_params.min_Fpoints,
1143 |       m_params.ransac_probability, m_params.max_ransac_iterations);
1144 |   }
1145 |   
1146 |   return false;
1147 | } 
1148 | 
1149 | // --------------------------------------------------------------------------
1150 | 
1151 | template<class TDescriptor, class F>
1152 | void TemplatedLoopDetector<TDescriptor, F>::getFlannStructure(
1153 |   const std::vector<TDescriptor> &descriptors, 
1154 |   cv::FlannBasedMatcher &flann_structure) const
1155 | {
1156 |   vector<cv::Mat> features(1);
1157 |   F::toMat32F(descriptors, features[0]);
1158 |   
1159 |   flann_structure.clear();
1160 |   flann_structure.add(features);
1161 |   flann_structure.train();
1162 | }
1163 | 
1164 | // --------------------------------------------------------------------------
1165 | 
1166 | template<class TDescriptor, class F>
1167 | bool TemplatedLoopDetector<TDescriptor, F>::isGeometricallyConsistent_Flann
1168 |   (EntryId old_entry,
1169 |   const std::vector<cv::KeyPoint> &keys, 
1170 |   const std::vector<TDescriptor> &,
1171 |   cv::FlannBasedMatcher &flann_structure) const
1172 | {
1173 |   vector<unsigned int> i_old, i_cur; // indices of correspondences
1174 |   
1175 |   const vector<cv::KeyPoint>& old_keys = m_image_keys[old_entry];
1176 |   const vector<TDescriptor>& old_descs = m_image_descriptors[old_entry];
1177 |   const vector<cv::KeyPoint>& cur_keys = keys;
1178 |   
1179 |   vector<cv::Mat> queryDescs_v(1);
1180 |   F::toMat32F(old_descs, queryDescs_v[0]);
1181 |   
1182 |   vector<vector<cv::DMatch> > matches;
1183 |   
1184 |   flann_structure.knnMatch(queryDescs_v[0], matches, 2);
1185 |   
1186 |   for(int old_idx = 0; old_idx < (int)matches.size(); ++old_idx)
1187 |   {
1188 |     if(!matches[old_idx].empty())
1189 |     {
1190 |       int cur_idx = matches[old_idx][0].trainIdx;
1191 |       float dist = matches[old_idx][0].distance;
1192 |       
1193 |       bool ok = true;
1194 |       if(matches[old_idx].size() >= 2)
1195 |       {
1196 |         float dist_ratio = dist / matches[old_idx][1].distance;
1197 |         ok = dist_ratio <= m_params.max_neighbor_ratio;
1198 |       }
1199 |       
1200 |       if(ok)
1201 |       {
1202 |         vector<unsigned int>::iterator cit =
1203 |           std::find(i_cur.begin(), i_cur.end(), cur_idx);
1204 |         
1205 |         if(cit == i_cur.end())
1206 |         {
1207 |           i_old.push_back(old_idx);
1208 |           i_cur.push_back(cur_idx);
1209 |         }
1210 |         else
1211 |         {
1212 |           int idx = i_old[ cit - i_cur.begin() ];
1213 |           if(dist < matches[idx][0].distance)
1214 |           {
1215 |             i_old[ cit - i_cur.begin() ] = old_idx;
1216 |           }
1217 |         }
1218 |       }
1219 |     }
1220 |   }
1221 |   
1222 |   if((int)i_old.size() >= m_params.min_Fpoints)
1223 |   {
1224 |     // add matches to the vectors for calculating the fundamental matrix
1225 |     vector<unsigned int>::const_iterator oit, cit;
1226 |     oit = i_old.begin();
1227 |     cit = i_cur.begin();
1228 |     
1229 |     vector<cv::Point2f> old_points, cur_points;
1230 |     old_points.reserve(i_old.size());
1231 |     cur_points.reserve(i_cur.size());
1232 |     
1233 |     for(; oit != i_old.end(); ++oit, ++cit)
1234 |     {
1235 |       const cv::KeyPoint &old_k = old_keys[*oit];
1236 |       const cv::KeyPoint &cur_k = cur_keys[*cit];
1237 |       
1238 |       old_points.push_back(old_k.pt);
1239 |       cur_points.push_back(cur_k.pt);
1240 |     }
1241 |     
1242 |     cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]);
1243 |     cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]);
1244 |     
1245 |     return m_fsolver.checkFundamentalMat(oldMat, curMat, 
1246 |       m_params.max_reprojection_error, m_params.min_Fpoints,
1247 |       m_params.ransac_probability, m_params.max_ransac_iterations);
1248 |   }
1249 |   
1250 |   return false;
1251 | }
1252 | 
1253 | // --------------------------------------------------------------------------
1254 | 
1255 | template<class TDescriptor, class F>
1256 | void TemplatedLoopDetector<TDescriptor, F>::getMatches_neighratio(
1257 |   const vector<TDescriptor> &A, const vector<unsigned int> &i_A,
1258 |   const vector<TDescriptor> &B, const vector<unsigned int> &i_B,
1259 |   vector<unsigned int> &i_match_A, vector<unsigned int> &i_match_B) const 
1260 | {
1261 |   i_match_A.resize(0);
1262 |   i_match_B.resize(0);
1263 |   i_match_A.reserve( min(i_A.size(), i_B.size()) );
1264 |   i_match_B.reserve( min(i_A.size(), i_B.size()) );
1265 |   
1266 |   vector<unsigned int>::const_iterator ait, bit;
1267 |   unsigned int i, j;
1268 |   i = 0;
1269 |   for(ait = i_A.begin(); ait != i_A.end(); ++ait, ++i)
1270 |   {
1271 |     int best_j_now = -1;
1272 |     double best_dist_1 = 1e9;
1273 |     double best_dist_2 = 1e9;
1274 |     
1275 |     j = 0;
1276 |     for(bit = i_B.begin(); bit != i_B.end(); ++bit, ++j)
1277 |     {
1278 |       double d = F::distance(A[*ait], B[*bit]);
1279 |             
1280 |       // in i
1281 |       if(d < best_dist_1)
1282 |       {
1283 |         best_j_now = j;
1284 |         best_dist_2 = best_dist_1;
1285 |         best_dist_1 = d;
1286 |       }
1287 |       else if(d < best_dist_2)
1288 |       {
1289 |         best_dist_2 = d;
1290 |       }
1291 |     }
1292 |     
1293 |     if(best_dist_1 / best_dist_2 <= m_params.max_neighbor_ratio)
1294 |     {
1295 |       unsigned int idx_B = i_B[best_j_now];
1296 |       bit = find(i_match_B.begin(), i_match_B.end(), idx_B);
1297 |       
1298 |       if(bit == i_match_B.end())
1299 |       {
1300 |         i_match_B.push_back(idx_B);
1301 |         i_match_A.push_back(*ait);
1302 |       }
1303 |       else
1304 |       {
1305 |         unsigned int idx_A = i_match_A[ bit - i_match_B.begin() ];
1306 |         double d = F::distance(A[idx_A], B[idx_B]);
1307 |         if(best_dist_1 < d)
1308 |         {
1309 |           i_match_A[ bit - i_match_B.begin() ] = *ait;
1310 |         }
1311 |       }
1312 |         
1313 |     }
1314 |   }
1315 | }
1316 | 
1317 | // --------------------------------------------------------------------------
1318 | 
1319 | template<class TDescriptor, class F>
1320 | void TemplatedLoopDetector<TDescriptor, F>::removeLowScores(QueryResults &q,
1321 |   double threshold) const
1322 | {
1323 |   // remember scores in q are in descending order now
1324 |   //QueryResults::iterator qit = 
1325 |   //  lower_bound(q.begin(), q.end(), threshold, Result::geqv);
1326 |   
1327 |   Result aux(0, threshold);
1328 |   QueryResults::iterator qit = 
1329 |     lower_bound(q.begin(), q.end(), aux, Result::geq);
1330 |   
1331 |   // qit = first element < m_alpha_minus || end
1332 |   
1333 |   if(qit != q.end())
1334 |   {
1335 |     int valid_entries = qit - q.begin();
1336 |     q.resize(valid_entries);
1337 |   }
1338 | }
1339 | 
1340 | // --------------------------------------------------------------------------
1341 | 
1342 | } // namespace DLoopDetector
1343 | 
1344 | #endif
1345 | 


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