├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doxygen ├── Doxyfile.in ├── DoxygenLayout.xml ├── citations │ ├── openFABMAP.bib │ └── openFABMAP.enw ├── images │ └── surf_small.jpg └── mainpage.dox.hpp ├── include ├── bowmsctrainer.hpp ├── chowliutree.hpp ├── fabmap.hpp ├── inference.hpp ├── msckd.h └── openfabmap.hpp ├── samples ├── openFABMAPcli.cpp └── settings.yml └── src ├── bowmsctrainer.cpp ├── chowliutree.cpp ├── fabmap.cpp ├── inference.cpp └── msckd.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # OpenFABMAP git ignored files 2 | 3 | # Qt Creator 4 | CMakeLists.txt.user 5 | *.autosave 6 | 7 | # Gedit/emacs 8 | *~ 9 | 10 | # Diff orig files (when not configured properly) 11 | *.orig 12 | 13 | # build directory 14 | 15 | build/ 16 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | dist: focal 4 | 5 | compiler: 6 | - gcc 7 | 8 | before_install: 9 | - sudo apt-get update -qq 10 | install: 11 | - sudo apt-get install -qq cmake build-essential libopencv-dev 12 | 13 | before_script: 14 | - mkdir build 15 | - cd build 16 | - cmake .. -DCMAKE_BUILD_TYPE=DEBUG 17 | 18 | script: make -j 2 19 | 20 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This Cmake file written by Michael Warren, 2 | # Queensland University of Technology, Australia 3 | # https://wiki.qut.edu.au/display/cyphy/Michael+Warren 4 | # Last updated 2014-11-03 5 | 6 | project(openFABMAP) 7 | 8 | cmake_minimum_required(VERSION 3.0) 9 | 10 | ## Cmake setup ################################################################# 11 | 12 | # make a lib directory in the build directory 13 | file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib) 14 | 15 | # tell cmake that the library goes in the library directory 16 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/lib) 17 | 18 | # make a binary directory in the build directory 19 | file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/bin) 20 | 21 | # tell cmake that the binaries goes in the binary directory 22 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin) 23 | 24 | # Compiler warning level /W3 (msvc) or -Wall (gcc) 25 | if(MSVC) 26 | # Force to always compile with W4 27 | if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]") 28 | string(REGEX REPLACE "/W[0-4]" "/W3" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") 29 | else() 30 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /W3") 31 | endif() 32 | elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) 33 | # Update if necessary 34 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-long-long -pedantic") 35 | endif() 36 | 37 | if(NOT CMAKE_CONFIGURATION_TYPES) 38 | if(NOT CMAKE_BUILD_TYPE) 39 | message(STATUS "Setting build type to 'Release' as none was specified.") 40 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 41 | endif() 42 | endif() 43 | 44 | 45 | ## Required Packages ########################################################### 46 | 47 | # OpenMP speedups 48 | message(STATUS "") 49 | find_package(OpenMP) 50 | if(OPENMP_FOUND) 51 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 52 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 53 | message("Found OpenMP") 54 | endif(OPENMP_FOUND) 55 | 56 | # Find OpenCV. 57 | # If it's not found, set OpenCV_DIR to the directory with OpenCVConfig.cmake 58 | if(WIN32) 59 | set(OpenCV_PATHS 60 | $ENV{OPENCV_HOME} 61 | $ENV{OPENCV_DIR}/../../ 62 | C:/opencv/ 63 | C:/OpenCV2.2/ 64 | C:/OpenCV2.3/ 65 | C:/OpenCV2.4/ 66 | ) 67 | else() # Linux 68 | set(OpenCV_PATHS 69 | $ENV{OPENCV_HOME}/build 70 | /usr/local/share/OpenCV/ 71 | /usr/share/OpenCV 72 | ) 73 | endif() 74 | find_package(OpenCV REQUIRED NO_MODULE PATHS ${OpenCV_PATHS}) 75 | 76 | message("OpenCV version: ${OpenCV_VERSION}") 77 | 78 | if(OpenCV_VERSION VERSION_LESS "2.4.0") 79 | add_definitions(-DUSENONFREE) 80 | elseif(OpenCV_VERSION VERSION_LESS "3.0.0") 81 | if("${OpenCV_LIBRARIES}" MATCHES opencv_nonfree) 82 | message("Non-free packages found. Using e.g. SIFT/SURF") 83 | add_definitions(-DUSENONFREE) 84 | endif() 85 | else() 86 | if("${OpenCV_LIBRARIES}" MATCHES opencv_xfeatures2d) 87 | message("Non-free packages found. Using e.g. SIFT/SURF") 88 | add_definitions(-DUSENONFREE) 89 | endif() 90 | endif() 91 | 92 | 93 | ## openFABMAP library ########################################################## 94 | 95 | # List sources 96 | aux_source_directory(${CMAKE_CURRENT_SOURCE_DIR}/src OPENFABMAP_FILES) 97 | 98 | # Include the headers 99 | file(GLOB OPENFABMAP_IMPL_INCS "${CMAKE_CURRENT_SOURCE_DIR}/include/*.hpp") 100 | file(GLOB OPENFABMAP_INCS "${CMAKE_CURRENT_SOURCE_DIR}/include/*.h") 101 | 102 | # tell cmake about the library 103 | add_library(openFABMAP ${OPENFABMAP_FILES} 104 | ${OPENFABMAP_IMPL_INCS} ${OPENFABMAP_INCS}) 105 | 106 | # Tell CMake where the headers are 107 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${OpenCV_INCLUDE_DIRS}) 108 | 109 | # Link against the required libraries in OpenCV >=2.2 110 | target_link_libraries(openFABMAP ${OpenCV_LIBRARIES}) 111 | 112 | ## openFABMAPcli executable #################################################### 113 | 114 | # Tell the project where settings / doxygen / readme files are (for Qt Creator) 115 | file(GLOB SETTINGS_FILE "${CMAKE_CURRENT_SOURCE_DIR}/samples/settings.yml") 116 | file(GLOB DOXY_FILES "${CMAKE_CURRENT_SOURCE_DIR}" 117 | ".travis.yml" "doxygen/*.dox" "doxygen/*.dox.*" "doxygen/*.xml") 118 | set(README_FILES "${CMAKE_CURRENT_SOURCE_DIR}/README.md") 119 | 120 | # Copy the settings file across when building (not used for now) 121 | #FILE(COPY ${SETTINGS_FILE} DESTINATION ${CMAKE_BINARY_DIR}/bin) 122 | 123 | # Tell cmake about the binary 124 | add_executable(openFABMAPcli ${CMAKE_SOURCE_DIR}/samples/openFABMAPcli.cpp 125 | ${SETTINGS_FILE} ${DOXY_FILES} ${README_FILES}) 126 | 127 | # Tell openFABMAPcli to link against its required libs 128 | target_link_libraries(openFABMAPcli openFABMAP ${OpenCV_LIBRARIES} ) 129 | 130 | 131 | ## Doxygen API documentation ################################################### 132 | 133 | # Add the 'doc' target to generate API documentation with Doxygen 134 | set(DOXYGEN_CREATE_DOCS false CACHE BOOL "Create documentation with Doxygen") 135 | if(DOXYGEN_CREATE_DOCS) 136 | find_package(Doxygen) 137 | if(DOXYGEN_FOUND) 138 | # Process and copy configuration file 139 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doxygen/Doxyfile.in 140 | ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY) 141 | # Uncomment ALL to make every time. Otherwise use "make doc" 142 | add_custom_target(doc #ALL 143 | ${DOXYGEN_EXECUTABLE} 144 | ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile 145 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 146 | COMMENT "Generating API documentation with Doxygen" VERBATIM 147 | ) 148 | else(DOXYGEN_FOUND) 149 | message(WARNING "Doxygen package not found, no documentation target") 150 | endif(DOXYGEN_FOUND) 151 | endif(DOXYGEN_CREATE_DOCS) 152 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # openFABMAP openfabmap 2 | 3 | Open Source C++ Code for the FAB-MAP Algorithm 4 | 5 | [St. Lucia Multiple Times of Day and Other Datasets](https://github.com/arrenglover/openfabmap/wiki/Datasets) 6 | 7 | See the [Wiki](https://github.com/arrenglover/openfabmap/wiki) for tips! 8 | 9 | ``` 10 | @inproceedings{ 11 | author = {Glover, A. and Maddern, W. and Warren, M. and Reid, S. and Milford, M. and Wyeth, G.}, 12 | title = {OpenFABMAP: An Open Source Toolbox for Appearance-based Loop Closure Detection}, 13 | booktitle = {The International Conference on Robotics and Automation}, 14 | address = {St Paul, Minnesota}, 15 | publisher = {IEEE}, 16 | year = {2011} 17 | } 18 | ``` 19 | 20 | OpenFABMAP [Glover et. al. 2012](http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5509547&tag=1) is an open-source, OpenCV-only dependent, version of the popular Fast Appearance-based Mapping (FAB-MAP) algorithm [Cummins & Newman 2008](http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5509547&tag=1 Glover et al. 2010). OpenFABMAP was developed from the ground-up following FAB-MAP publications. The original FAB-MAP algorithm is now also [open-source](http://www.robots.ox.ac.uk/~mjc/Software.htm) but requires alternative project dependencies. 21 | 22 | FAB-MAP is a Simultaneous Localisation and Mapping algorithm which operates solely in appearance space. FAB-MAP performs location matching between places that have been visited within the world as well as providing a measure of the probability of being at a new, previously unvisited location. Camera images form the sole input to the system, from which OpenCV's feature extraction methods are used to develop bag-of-words representations for the Bayesian comparison technique. 23 | 24 | The code has implementations of 25 | * Feature Detection, Feature Extraction, and Bag-of-words models using OpenCV 26 | * Chow-Liu tree implementation 27 | * FAB-MAP v1.0 [Cummins & Newman 2008](http://ijr.sagepub.com/content/27/6/647.short Cummins & Newman 2008) 28 | * FAB-MAP v1.0 using a Look-up-table for improved computation speed 29 | * FAB-MAP with Fast-Bailout [Cummins & Newman 2010](http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5613942) 30 | * FAB-MAP v2.0 [Cummins & Newman 2010](http://ijr.sagepub.com/content/30/9/1100.short) 31 | 32 | An overview of OpenFABMAP [Glover et. al. 2012](http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5509547&tag=1) or the original implementation/use [Glover et al. 2010](http://eprints.qut.edu.au/50317/1/glover_ICRA2012_final.pdf). 33 | 34 | As of the latest version, openFABMAP is dependent solely on [OpenCV 2.3](http://opencv.org/) or higher. The project has a [CMake](http://www.cmake.org/) build environment for general use on both Linux and Windows systems. OpenFABMAP is also designed to integrate with [ROS](http://www.ros.org/wiki/). See the [CyPhy-ROS](https://wiki.qut.edu.au/display/cyphy/cyphy+ROS+wiki+page) page for a package that has implemented openFABMAP as a ROS node. 35 | 36 | The original googlecode project page was [here](http://code.google.com/p/openfabmap/) 37 | 38 | 39 | 40 | ### Installation 41 | 42 | Linux (g++) 43 | 44 | 1. install cmake `sudo apt install cmake` 45 | 1. install opencv `sudo apt install libopencv-dev` 46 | 1. get the openFABMAP code `git clone https://github.com/arrenglover/openfabmap.git` 47 | 1. make the build directory `mkdir openfabmap/build && cd openfabmap/build` 48 | 1. use cmake to compile the makefile `cmake ..`. *note: the output will tell you which version of opencv you are using and if you are using the "non-free" modules* 49 | 1. make the project `make` 50 | 1. view/modify the settings file `gedit ../samples/settings.yml` 51 | 1. run the command line tool `bin/openFABMAPcli -s ../samples/settings.yml` 52 | 53 | OpenCV non-free for OpenCV 3.4 54 | 55 | 1. clone opencv_contrib 56 | 1. clone opencv repository 57 | 1. checkout version 3.4 58 | 1. mkdir build && cd build 59 | 1. cmake .. -DOPENCV_EXTRA_MODULES_PATH='path_to/opencv_contrib/modules' -DBUILD_opencv_xfeatures2d=ON -DOPENCV_ENABLE_NONFREE=ON 60 | 1. make 61 | 62 | Windows (Visual Studio 2008) 63 | 64 | 1. install [openCV2.3](http://opencv.willowgarage.com/wiki/) 65 | 2. install [cmake](www.cmake.org/) 66 | 3. open the cmake gui, specify the source directory (the directory this README is in), a build directory for the code, and click configure 67 | 4. you may have to specify the location of opencv2.3 in UngroupedEntries->OPENCV_PATH. 68 | 5. click configure in the cmake gui again 69 | 6. click generate 70 | 7. open the visual studio solution, for default running right-click on openFABMAPexe project and select 'Set as StartUp project'. Compile openFABMAP within Visual studio. 71 | 8. add required .dll files from openCV2.3 to your build/bin directory (respective debug versions for debug mode). 72 | 9. you also may need an extra tbb .dll due to OpenCV bug which can be downloaded [here](http://threadingbuildingblocks.org/ver.php?fid=171) 73 | 10. under openFABMAPcli->properties->Debugging->command arguments specify the path to the settings file (e.g. "-s samples\settings.yml") 74 | 11. Alter the settings file for your data 75 | 12. run exampleopenFABMAP in your build/bin directory (respective debug versions for debug mode). 76 | 77 | ### Contributors 78 | 79 | - Arren GLover 80 | - Will Maddern 81 | - Kirk MacTavish 82 | -------------------------------------------------------------------------------- /doxygen/DoxygenLayout.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | -------------------------------------------------------------------------------- /doxygen/citations/openFABMAP.bib: -------------------------------------------------------------------------------- 1 | @article{ 2 | author = {Cummins, Mark and Newman, Paul}, 3 | title = {FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance}, 4 | journal = {International Journal of Robotics Research}, 5 | volume = {27}, 6 | number = {6}, 7 | pages = {647-665}, 8 | note = {1377517}, 9 | year = {2008} 10 | } 11 | 12 | @article{ 13 | author = {Cummins, M. and Newman, P.}, 14 | title = {Accelerating FAB-MAP with concentration inequalities}, 15 | journal = {IEEE Transactions on Robotics}, 16 | volume = {26}, 17 | number = {6}, 18 | pages = {1042-1050}, 19 | year = {2010} 20 | } 21 | 22 | @article{ 23 | author = {Cummins, M. and Newman, P.}, 24 | title = {Appearance-only SLAM at large scale with FAB-MAP 2.0}, 25 | journal = {The International Journal of Robotics Research}, 26 | year = {2010} 27 | } 28 | 29 | @inproceedings{ 30 | author = {Glover, A. and Maddern, W. and Milford, M. and Wyeth, G.}, 31 | title = {FAB-MAP+ RatSLAM: Appearance-based SLAM for Multiple Times of Day}, 32 | booktitle = {The International Conference on Robotics and Automation}, 33 | address = {Anchorage, Alaska, USA}, 34 | publisher = {IEEE}, 35 | pages = {3507-3512}, 36 | year = {2010} 37 | } 38 | 39 | @inproceedings{ 40 | author = {Glover, A. and Maddern, W. and Warren, M. and Reid, S. and Milford, M. and Wyeth, G.}, 41 | title = {OpenFABMAP: An Open Source Toolbox for Appearance-based Loop Closure Detection}, 42 | booktitle = {The International Conference on Robotics and Automation}, 43 | address = {St Paul, Minnesota}, 44 | publisher = {IEEE}, 45 | year = {2011} 46 | } 47 | 48 | -------------------------------------------------------------------------------- /doxygen/citations/openFABMAP.enw: -------------------------------------------------------------------------------- 1 | %0 Journal Article 2 | %A Cummins, Mark 3 | %A Newman, Paul 4 | %D 2008 5 | %T FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance 6 | %J International Journal of Robotics Research 7 | %V 27 8 | %N 6 9 | %P 647-665 10 | %! FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance 11 | %@ 0278-3649 12 | %R http://dx.doi.org/10.1177/0278364908090961 13 | %Z 1377517 14 | 15 | 16 | 17 | %0 Journal Article 18 | %A Cummins, M. 19 | %A Newman, P. 20 | %D 2010 21 | %T Appearance-only SLAM at large scale with FAB-MAP 2.0 22 | %J The International Journal of Robotics Research 23 | %! Appearance-only SLAM at large scale with FAB-MAP 2.0 24 | %@ 0278-3649 25 | 26 | 27 | 28 | %0 Journal Article 29 | %A Cummins, M. 30 | %A Newman, P. 31 | %D 2010 32 | %T Accelerating FAB-MAP with concentration inequalities 33 | %J IEEE Transactions on Robotics 34 | %V 26 35 | %N 6 36 | %P 1042-1050 37 | %! Accelerating FAB-MAP with concentration inequalities 38 | %@ 1552-3098 39 | 40 | 41 | 42 | %0 Conference Proceedings 43 | %A Glover, A. 44 | %A Maddern, W. 45 | %A Milford, M. 46 | %A Wyeth, G. 47 | %D 2010 48 | %T FAB-MAP+ RatSLAM: Appearance-based SLAM for Multiple Times of Day 49 | %B The International Conference on Robotics and Automation 50 | %C Anchorage, Alaska, USA 51 | %I IEEE 52 | %P 3507-3512 53 | %! FAB-MAP+ RatSLAM: Appearance-based SLAM for Multiple Times of Day 54 | %@ 1050-4729 55 | 56 | 57 | 58 | %0 Conference Proceedings 59 | %A Glover, A. 60 | %A Maddern, W. 61 | %A Warren, M. 62 | %A Reid, S. 63 | %A Milford, M. 64 | %A Wyeth, G. 65 | %D 2011 66 | %T OpenFABMAP: An Open Source Toolbox for Appearance-based Loop Closure Detection 67 | %B The International Conference on Robotics and Automation 68 | %C St Paul, Minnesota 69 | %I IEEE 70 | %! OpenFABMAP: An Open Source Toolbox for Appearance-based Loop Closure Detection 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /doxygen/images/surf_small.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arrenglover/openfabmap/47fef64e2cb6407604cbce415a44a9b9e6219da8/doxygen/images/surf_small.jpg -------------------------------------------------------------------------------- /doxygen/mainpage.dox.hpp: -------------------------------------------------------------------------------- 1 | // Main Page Doxygen Documentation 2 | 3 | /// 4 | /// \file 5 | /// 6 | /// \brief TODO: Move this to README.md and translate to markdown 7 | /// 8 | 9 | namespace cv 10 | { 11 | 12 | namespace of2 13 | { 14 | 15 | /*! \mainpage OpenFABMAP 16 | * 17 | * 18 | * This is an open and modifiable code-source which implements the Fast Appearance-based 19 | * Mapping algorithm (FAB-MAP) originally developed by Mark Cummins and Paul Newman. 20 | * OpenFABMAP was designed from published FAB-MAP theory and is for personal and research 21 | * use. 22 | * 23 | * FAB-MAP is a Simultaneous Localisation and Mapping algorithm which operates in 24 | * appearance space only. FAB-MAP performs location matching between places that have 25 | * been visited within the world as well as providing a measure of the probability of 26 | * being at a new, previously unvisited location. Camera images form the sole input to 27 | * the system, from which bag-of-words models are formed through the extraction of 28 | * appearance-based (e.g. SURF) features. 29 | * 30 | * The code has implementations of 31 | * * Feature Detection and Extraction and Bag-of-words models using OpenCV 32 | * * BOWMSCTrainer, a Bag-of-Words vocabulary trainer 33 | * (Teynor & Burkhardt 2007) 34 | * * ChowLiuTree, a Chow-Liu tree implementation 35 | * (Chow & Liu 1968) 36 | * * FabMap1, the original FabMap algorithm 37 | * (Cummins & Newman 2008) 38 | * * FabMapLUT which uses a look-up table to precompute commonly used calculations for FabMap 39 | * * FabMapFBO which uses the fast bail-out speed-up for FabMap 40 | * (Cummins & Newman 2010) 41 | * * FabMap2 which is able to handle much larger environments than the earlier FabMap algorithms 42 | * (Cummins & Newman 2010) 43 | * 44 | * For an overview of OpenFABMAP see 45 | * (Glover et al. 2012). 46 | * OpenFABMAP was first used in 47 | * (Glover et al. 2010). 48 | * 49 | * As of the latest version, openFABMAP is dependent solely on OpenCV2.3 or 51 | * higher. The project is designed to integrate with OpenCV 2.3 2D feature-based methods 52 | * and storage methods. The project has a CMake build 53 | * environment for general use on both Linux and Windows systems. See the README file for 54 | * more information on compiling the code. 55 | * 56 | * OpenFABMAP is also designed to integrate with Robot 57 | * Operating System (ROS). See the CyPhy-ROS page 59 | * for a package that has implemented openFABMAP as a ROS node. 60 | * 61 | * For questions on how to modify the source to your specific implementation, bug 62 | * reporting, comments and suggestions, or if you would like to become involved in 63 | * developing the openFABMAP project beyond the current implementation, contact via 64 | * github. 65 | * 66 | * Citations Endnote 67 | * Bibtex 68 | * 69 | * \image html surf_small.jpg "SURF features used by FAB-MAP" 70 | * 71 | */ 72 | 73 | } 74 | 75 | } 76 | -------------------------------------------------------------------------------- /include/bowmsctrainer.hpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef BOWMSCTRAINER_H_ 55 | #define BOWMSCTRAINER_H_ 56 | 57 | #include 58 | #include 59 | 60 | 61 | namespace of2 { 62 | 63 | /// 64 | /// \brief A custom vocabulary training method based on: 65 | /// http://www.springerlink.com/content/d1h6j8x552532003/. 66 | /// 67 | class CV_EXPORTS BOWMSCTrainer: public cv::BOWTrainer { 68 | public: 69 | BOWMSCTrainer(double clusterSize = 0.4); 70 | virtual ~BOWMSCTrainer(); 71 | 72 | // Returns trained vocabulary (i.e. cluster centers). 73 | virtual cv::Mat cluster() const; 74 | virtual cv::Mat cluster(const cv::Mat& descriptors) const; 75 | 76 | protected: 77 | 78 | double clusterSize; 79 | 80 | }; 81 | 82 | } // namespace of2 83 | 84 | #endif /* BOWMSCTRAINER_H_ */ 85 | -------------------------------------------------------------------------------- /include/chowliutree.hpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef CHOWLIUTREE_H_ 55 | #define CHOWLIUTREE_H_ 56 | 57 | #include 58 | 59 | #include 60 | #include 61 | 62 | namespace of2 { 63 | 64 | /// 65 | /// \brief A Chow-Liu tree implementation designed for FAB-MAP. 66 | /// 67 | /// A Chow-Liu tree is required by FAB-MAP. The Chow-Liu tree provides an 68 | /// estimate of the full distribution of visual words using a minimum spanning 69 | /// tree. The tree is generated from training data. 70 | /// 71 | class CV_EXPORTS ChowLiuTree { 72 | public: 73 | ChowLiuTree(); 74 | virtual ~ChowLiuTree(); 75 | 76 | //@{ 77 | /// 78 | /// \brief You add data to the chow-liu tree before calling make. 79 | /// \param imgDescriptor A \#imgs x \#words bag of words descriptor. 80 | /// 81 | void add(const cv::Mat& imgDescriptor); 82 | /// 83 | /// \brief You add data to the chow-liu tree before calling make. 84 | /// \param imgDescriptors A vector of \#imgs x \#words bag of words descriptors. 85 | /// 86 | void add(const std::vector& imgDescriptors); 87 | //@} 88 | 89 | const std::vector& getImgDescriptors() const; 90 | 91 | /// 92 | /// \brief Builds the Chow Liu tree from the descriptors that have been added. 93 | /// \param infoThreshold Ignores word pairs whose mutual information is below this threshold. 94 | /// \return A Mat containing the 4 x |v| Chow Liu tree, 95 | /// where (0,q) is parent (p) index, (1,q) is P(q), (2,q) is P(q|p), (3,q) is P(q|~p) 96 | /// 97 | cv::Mat make(double infoThreshold = 0.0); 98 | 99 | private: 100 | std::vector imgDescriptors; 101 | cv::Mat mergedImgDescriptors; 102 | 103 | typedef struct info { 104 | float score; 105 | short word1; 106 | short word2; 107 | } info; 108 | 109 | //probabilities extracted from mergedImgDescriptors 110 | double P(int a, bool za); 111 | double JP(int a, bool za, int b, bool zb); //a & b 112 | double CP(int a, bool za, int b, bool zb); // a | b 113 | 114 | //calculating mutual inforMation of all edges 115 | void createBaseEdges(std::list& edges, double infoThreshold); 116 | double calcMutInfo(int word1, int word2); 117 | static bool sortInfoScores(const info& first, const info& second); 118 | 119 | //selecting minimum spanning egdges with maximum inforMation 120 | bool reduceEdgesToMinSpan(std::list& edges); 121 | 122 | //building the tree sctructure 123 | cv::Mat buildTree(int root_word, std::list &edges); 124 | void recAddToTree(cv::Mat &cltree, int q, int pq, 125 | std::list &remaining_edges); 126 | std::vector extractChildren(std::list &remaining_edges, int q); 127 | 128 | }; 129 | 130 | } // namespace of2 131 | 132 | #endif /* CHOWLIUTREE_H_ */ 133 | -------------------------------------------------------------------------------- /include/fabmap.hpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef FABMAP_H_ 55 | #define FABMAP_H_ 56 | 57 | #include "inference.hpp" 58 | 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | #ifdef __GNUC__ 67 | #define DEPRECATED(func) func __attribute__ ((deprecated)) 68 | #elif defined(_MSC_VER) 69 | #define DEPRECATED(func) __declspec(deprecated) func 70 | #else 71 | #pragma message("WARNING: You need to implement DEPRECATED for this compiler") 72 | #define DEPRECATED(func) func 73 | #endif 74 | 75 | namespace of2 { 76 | 77 | /// 78 | /// \brief The return data format for FabMap comparisons and localizations. 79 | /// 80 | struct CV_EXPORTS IMatch { 81 | 82 | /// Default constructor. 83 | IMatch() : 84 | queryIdx(-1), imgIdx(-1), 85 | groupSize(0), 86 | likelihood(-DBL_MAX), match(-DBL_MAX) 87 | { 88 | } 89 | /// 90 | /// \brief Initialization constructor 91 | /// \param _queryIdx Sets IMatch::queryIdx. 92 | /// \param _imgIdx Sets IMatch::imgIdx. 93 | /// \param _likelihood Sets IMatch::likelihood. 94 | /// \param _match Sets IMatch::match. 95 | /// \param _groupSize Sets IMatch::groupSize. 96 | /// 97 | IMatch(int _queryIdx, int _imgIdx, 98 | double _likelihood, double _match, 99 | unsigned int _groupSize = 0) : 100 | queryIdx(_queryIdx), imgIdx(_imgIdx), 101 | groupSize(_groupSize), 102 | likelihood(_likelihood), match(_match) 103 | { 104 | } 105 | 106 | int queryIdx; ///< Query descriptor index (descriptor being compared). 107 | int imgIdx; ///< Test descriptor index (reference descriptor being compared to). 108 | int groupSize; ///< Size of image groups being compared (for PiraMap) 109 | int placeId; ///< The index of the location (L) for the query after loop closure. 110 | 111 | double likelihood; ///< The likelihood of the descriptors coming from the same location. 112 | double match; ///< The normalized probability that the descriptors come from the same location 113 | 114 | /** 115 | * @brief IMatch operator < for match probabilities. 116 | * @param m The RHS IMatch object 117 | * @return If match probability for LHS < RHS 118 | */ 119 | bool operator<(const IMatch& m) const { 120 | return match < m.match; 121 | } 122 | 123 | }; 124 | 125 | 126 | 127 | /// 128 | /// \brief This class defines the common functionality for the FabMap derivatives. 129 | /// 130 | class CV_EXPORTS FabMap { 131 | public: 132 | 133 | /// FabMap flag options 134 | enum { 135 | MEAN_FIELD = 1, 136 | SAMPLED = 2, 137 | NAIVE_BAYES = 4, 138 | CHOW_LIU = 8, 139 | MOTION_MODEL = 16 140 | }; 141 | 142 | /// 143 | /// \brief Base FabMap constructor 144 | /// \param clTree Chow Liu tree from training 145 | /// \param PzGe Measurement probability given the generator is present 146 | /// \param PzGNe Measurement probability given the generator is absent 147 | /// \param flags Flag for options 148 | /// \param numSamples Number of samples to use for new place sampling 149 | /// 150 | FabMap(const cv::Mat& clTree, double PzGe, double PzGNe, int flags, 151 | int numSamples = 0); 152 | virtual ~FabMap(); 153 | 154 | //@{ 155 | /// Method to add training data for sampling method. 156 | virtual void addTraining(const cv::Mat& queryImgDescriptor); 157 | virtual void addTraining(const std::vector& queryImgDescriptors); 158 | //@} 159 | 160 | //@{ 161 | /// Method to add to the test data (the map). 162 | virtual void add(const cv::Mat& queryImgDescriptor); 163 | virtual void add(const std::vector& queryImgDescriptors); 164 | //@} 165 | 166 | //@{ 167 | /// Access the descriptors with a read-only reference 168 | const std::vector& getTrainingImgDescriptors() const; 169 | const std::vector& getTestImgDescriptors() const; 170 | //@} 171 | 172 | //@{ Image comparisons 173 | /// 174 | /// \brief FabMap image comparison (not full localization). 175 | /// \param queryImgDescriptor The descriptor for the query image 176 | /// \param testImgDescriptors The descriptors for the test images 177 | /// \param[out] matches Contains the match probabilities for the comparisons 178 | /// \param mask Not currently implemented? 179 | /// 180 | void compare(const cv::Mat& queryImgDescriptor, 181 | const cv::Mat& testImgDescriptors, std::vector& matches, 182 | const cv::Mat& mask = cv::Mat()); 183 | /// 184 | /// \brief FabMap image comparison (not full localization). 185 | /// \param queryImgDescriptor The descriptor for the query image 186 | /// \param testImgDescriptors The descriptors for the test images 187 | /// \param[out] matches Contains the match probabilities for the comparisons 188 | /// \param mask Not currently implemented? 189 | /// 190 | void compare(const cv::Mat& queryImgDescriptor, 191 | const std::vector& testImgDescriptors, 192 | std::vector& matches, const cv::Mat& mask = cv::Mat()); 193 | /// 194 | /// \brief FabMap image comparison (not full localization). 195 | /// \param queryImgDescriptors The descriptors for the query images 196 | /// \param testImgDescriptors The descriptors for the test images 197 | /// \param[out] matches Contains the match probabilities for the comparisons 198 | /// \param mask Not currently implemented? 199 | /// 200 | void compare(const std::vector& queryImgDescriptors, 201 | const std::vector& testImgDescriptors, 202 | std::vector& matches, const cv::Mat& mask = cv::Mat()); 203 | //@} 204 | 205 | //@{ Localization against map 206 | /// 207 | /// \brief FabMap localization against the map (deprecated, see FabMap::localize). 208 | /// \param queryImgDescriptors The descriptors for the query images 209 | /// \param[out] matches Contains the match probabilities for the comparisons 210 | /// \param addQuery Add the query to the test descriptors when done 211 | /// \param mask Not currently implemented? 212 | /// \deprecated This function has been deprecated, use FabMap::localize instead 213 | /// 214 | DEPRECATED (void compare(const std::vector& queryImgDescriptors, std::vector< 215 | IMatch>& matches, bool addQuery = false, const cv::Mat& mask = 216 | cv::Mat())); 217 | /// 218 | /// \brief FabMap localization against the map. 219 | /// \param queryImgDescriptors The descriptors for the query images 220 | /// \param[out] matches Contains the match probabilities for the comparisons 221 | /// \param addQuery Add the query to the test descriptors when done 222 | /// \param mask Not currently implemented? 223 | /// TODO: Add addThreshold that replaces addQuery, and addIfLatest that doesn't add to the latest place. 224 | /// 225 | virtual void localize(const std::vector& queryImgDescriptors, std::vector< 226 | IMatch>& matches, bool addQuery = false, const cv::Mat& mask = 227 | cv::Mat()); 228 | /// 229 | /// \brief FabMap localization against the map (deprecated, see FabMap::localize). 230 | /// \param queryImgDescriptor The descriptor for the query image 231 | /// \param[out] matches Contains the match probabilities for the comparisons 232 | /// \param addQuery Add the query to the test descriptors when done 233 | /// \param mask Not currently implemented? 234 | /// \deprecated This function has been deprecated, use FabMap::localize instead 235 | /// 236 | DEPRECATED (void compare(const cv::Mat& queryImgDescriptor, 237 | std::vector& matches, bool addQuery = false, 238 | const cv::Mat& mask = cv::Mat())); 239 | /// 240 | /// \brief FabMap localization against the map. 241 | /// \param queryImgDescriptor The descriptor for the query image 242 | /// \param[out] matches Contains the match probabilities for the comparisons 243 | /// \param addQuery Add the query to the test descriptors when done 244 | /// \param mask Not currently implemented? 245 | /// TODO: Add addThreshold that replaces addQuery, and addIfLatest that doesn't add to the latest place. 246 | /// 247 | virtual void localize(const cv::Mat& queryImgDescriptor, 248 | std::vector& matches, bool addQuery = false, 249 | const cv::Mat& mask = cv::Mat()); 250 | //@} 251 | 252 | protected: 253 | 254 | //@{ Base Image descriptor operations 255 | /// 256 | /// \brief Compares a query to test descriptors. 257 | /// It calculates the likelihood that the two descriptors came from the same location. 258 | /// \param queryImgDescriptor The descriptor for the query image 259 | /// \param queryIndex The index of the query image (for query index in matches). 260 | /// \param testImgDescriptors The image descriptors to compare against. 261 | /// \param[out] matches The result of the descriptor comparisons in IMatch::likelihood. 262 | /// 263 | void compareImgDescriptor(const cv::Mat& queryImgDescriptor, 264 | int queryIndex, const std::vector& testImgDescriptors, 265 | std::vector& matches); 266 | /// 267 | /// \brief Adds an image descriptor to the test descriptors (the map). 268 | /// Does no comparisons to the test data. 269 | /// \param queryImgDescriptor The query image descriptor that will be added. 270 | /// 271 | void addImgDescriptor(const cv::Mat& queryImgDescriptor); 272 | //@} 273 | 274 | //@{ Likelihood functions, these change with each type of FabMap 275 | /// 276 | /// \brief The getLikelihoods method is overwritten for each different FabMap 277 | /// \param queryImgDescriptor The descriptor to be compared. 278 | /// \param testImgDescriptors The descriptors to compare against. 279 | /// \param[out] matches Result of the comparison of query to test. 280 | /// 281 | virtual void getLikelihoods(const cv::Mat& queryImgDescriptor, 282 | const std::vector& testImgDescriptors, 283 | std::vector& matches) = 0; 284 | /// 285 | /// \brief Calculates the likelihood the query comes from a new place. 286 | /// \param queryImgDescriptor The descriptor from the query image. 287 | /// \return The log-likelihood the query descriptor came from a new place. 288 | /// 289 | virtual double getNewPlaceLikelihood(const cv::Mat& queryImgDescriptor); 290 | //@} 291 | 292 | /// 293 | /// \brief Turns measurement likelihoods into location probabilities. 294 | /// Also applies the motion model if in use (specified in the options flag). 295 | /// \param[in,out] matches Contains the input likelihoods, and output probabilities. 296 | /// 297 | void normaliseDistribution(std::vector& matches); 298 | 299 | //@{ Data 300 | /// Image descriptors seen in training (for sampled new location probability) 301 | std::vector trainingImgDescriptors; 302 | /// Image descriptors seen so far 303 | std::vector testImgDescriptors; 304 | /// Prior match probabilities for motion model p(L^k|Z^k-1) 305 | std::vector priormatches; 306 | /// Generator states for all locations 307 | std::vector peGL; 308 | //@} 309 | 310 | //@{ Parameters 311 | double Pnew; ///< Prior probability of entering a new location (motion model) 312 | 313 | double mBias; ///< Forward motion bias, 1 all forward, 0 all backward (motion model) 314 | double sFactor; ///< Smoothing factor for matches, applied at the end (see papers) 315 | 316 | int flags; ///< Flags for different modes 317 | int numSamples; ///< Number of samples to use for sampled new location probability 318 | //@} 319 | 320 | //@{ 321 | /// Chow Liu Tree 322 | cv::Ptr clTree; 323 | /// Inference object (uses clTree) 324 | cv::Ptr infer; 325 | //@} 326 | 327 | }; 328 | 329 | /// 330 | /// \brief The original FAB-MAP algorithm, developed based on: 331 | /// http://ijr.sagepub.com/content/27/6/647.short. 332 | /// 333 | /// See the FabMap base class for more inforMation. 334 | /// 335 | /// Note: It does not currently associate multiple measurements with a location (apply 336 | /// the loop closure). This is a 'todo'. 337 | /// 338 | class CV_EXPORTS FabMap1: public FabMap { 339 | public: 340 | FabMap1(const cv::Mat& clTree, double PzGe, double PzGNe, int flags, 341 | int numSamples = 0); 342 | virtual ~FabMap1(); 343 | protected: 344 | 345 | /// 346 | /// \brief FabMap1 implementation of likelihood comparison 347 | /// \param queryImgDescriptor The query descriptor to be compared. 348 | /// \param testImgDescriptors The reference descriptors to compare against. 349 | /// \param[out] matches The results of the comparisons in IMatch::likelihood. 350 | /// 351 | void getLikelihoods(const cv::Mat& queryImgDescriptor, 352 | const std::vector& testImgDescriptors, 353 | std::vector& matches); 354 | }; 355 | 356 | /// 357 | /// \brief A computationally faster Look-Up-Table version of the original FAB-MAP algorithm. 358 | /// 359 | /// See the FabMap base class for more inforMation. 360 | /// 361 | /// A look-up-table is used to precompute many of the reoccuring calculations. 362 | /// 363 | class CV_EXPORTS FabMapLUT: public FabMap { 364 | public: 365 | FabMapLUT(const cv::Mat& clTree, double PzGe, double PzGNe, 366 | int flags, int numSamples = 0, int precision = 6); 367 | virtual ~FabMapLUT(); 368 | protected: 369 | 370 | //FabMap look-up-table implementation of the likelihood comparison 371 | void getLikelihoods(const cv::Mat& queryImgDescriptor, const std::vector< 372 | cv::Mat>& testImgDescriptors, std::vector& matches); 373 | 374 | //precomputed data 375 | int (*table)[8]; 376 | 377 | //data precision 378 | int precision; 379 | }; 380 | 381 | /// 382 | /// \brief The Accelerated FAB-MAP algorithm, developed based on: 383 | /// http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942. 384 | /// 385 | /// See the FabMap base class for more inforMation. 386 | /// 387 | class CV_EXPORTS FabMapFBO: public FabMap { 388 | public: 389 | FabMapFBO(const cv::Mat& clTree, double PzGe, double PzGNe, int flags, 390 | int numSamples = 0, double rejectionThreshold = 1e-8, double PsGd = 391 | 1e-8, int bisectionStart = 512, int bisectionIts = 9); 392 | virtual ~FabMapFBO(); 393 | 394 | protected: 395 | 396 | //FabMap Fast Bail-out implementation of the likelihood comparison 397 | void getLikelihoods(const cv::Mat& queryImgDescriptor, const std::vector< 398 | cv::Mat>& testImgDescriptors, std::vector& matches); 399 | 400 | /// 401 | /// \brief Stucture used to determine word comparison order 402 | /// 403 | struct WordStats { 404 | WordStats() : 405 | q(0), info(0), V(0), M(0) { 406 | } 407 | 408 | WordStats(int _q, double _info) : 409 | q(_q), info(_info), V(0), M(0) { 410 | } 411 | 412 | int q; 413 | double info; 414 | mutable double V; 415 | mutable double M; 416 | 417 | bool operator<(const WordStats& w) const { 418 | return info < w.info; 419 | } 420 | 421 | }; 422 | 423 | //private fast bail-out necessary functions 424 | void setWordStatistics(const cv::Mat& queryImgDescriptor, std::multiset& wordData); 425 | double limitbisection(double v, double m); 426 | double bennettInequality(double v, double m, double delta); 427 | static bool compInfo(const WordStats& first, const WordStats& second); 428 | 429 | //parameters 430 | double PsGd; 431 | double rejectionThreshold; 432 | int bisectionStart; 433 | int bisectionIts; 434 | }; 435 | 436 | /// 437 | /// \brief The FAB-MAP2.0 algorithm, developed based on: 438 | /// http://ijr.sagepub.com/content/30/9/1100.abstract 439 | /// 440 | /// See the FabMap base class for more inforMation. 441 | /// 442 | class CV_EXPORTS FabMap2: public FabMap { 443 | public: 444 | 445 | FabMap2(const cv::Mat& clTree, double PzGe, double PzGNe, int flags); 446 | virtual ~FabMap2(); 447 | 448 | //FabMap2 builds the inverted index and requires an additional training/test 449 | //add function 450 | void addTraining(const cv::Mat& queryImgDescriptors) { 451 | FabMap::addTraining(queryImgDescriptors); 452 | } 453 | void addTraining(const std::vector& queryImgDescriptors); 454 | 455 | void add(const cv::Mat& queryImgDescriptors) { 456 | FabMap::add(queryImgDescriptors); 457 | } 458 | void add(const std::vector& queryImgDescriptors); 459 | 460 | protected: 461 | 462 | //FabMap2 implementation of the likelihood comparison 463 | void getLikelihoods(const cv::Mat& queryImgDescriptor, const std::vector< 464 | cv::Mat>& testImgDescriptors, std::vector& matches); 465 | double getNewPlaceLikelihood(const cv::Mat& queryImgDescriptor); 466 | 467 | //the likelihood function using the inverted index 468 | void getIndexLikelihoods(const cv::Mat& queryImgDescriptor, std::vector< 469 | double>& defaults, std::map >& invertedMap, 470 | std::vector& matches); 471 | void addToIndex(const cv::Mat& queryImgDescriptor, 472 | std::vector& defaults, 473 | std::map >& invertedMap); 474 | 475 | //data 476 | std::vector d1, d2, d3, d4; 477 | std::vector > children; 478 | 479 | // TODO: inverted map a std::vector? 480 | 481 | std::vector trainingDefaults; 482 | std::map > trainingInvertedMap; 483 | 484 | std::vector testDefaults; 485 | std::map > testInvertedMap; 486 | 487 | }; 488 | 489 | } // namespace of2 490 | 491 | #endif /* FABMAP_H_ */ 492 | -------------------------------------------------------------------------------- /include/inference.hpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef INFERENCE_H_ 55 | #define INFERENCE_H_ 56 | 57 | #include 58 | 59 | namespace of2 { 60 | 61 | /// 62 | /// \brief This class contains some base functionality for FabMap inference 63 | /// 64 | class CV_EXPORTS InferBase 65 | { 66 | public: 67 | InferBase (cv::Ptr _clTreePtr, 68 | const double & _PzGe, 69 | const double & _PzGNe, 70 | const bool & _naiveBayes) 71 | : PzGe(_PzGe), PzGNe(_PzGNe), 72 | naiveBayes(_naiveBayes), 73 | clTreePtr(_clTreePtr), clTree(*clTreePtr) 74 | { 75 | //check for a valid Chow-Liu tree 76 | CV_Assert(clTree.type() == CV_64FC1); 77 | cv::checkRange(clTree.row(0), false, NULL, 0, clTree.cols); 78 | cv::checkRange(clTree.row(1), false, NULL, DBL_MIN, 1); 79 | cv::checkRange(clTree.row(2), false, NULL, DBL_MIN, 1); 80 | cv::checkRange(clTree.row(3), false, NULL, DBL_MIN, 1); 81 | } 82 | 83 | //@{Chow-Liu Tree 84 | /// 85 | /// \brief Probability of a word's measurement being true (from training). 86 | /// \param q The index of the word. 87 | /// \return The probability. 88 | /// 89 | int pq(int q); 90 | /// 91 | /// \brief The probability of a particular word measurement (from training). 92 | /// \param q The index of the word. 93 | /// \param zq The measurement value. 94 | /// \return The probability. 95 | /// 96 | double Pzq(int q, bool zq); 97 | /// 98 | /// \brief The probability of a particular word measurement given the CLT parent measurement. 99 | /// \param q The index of the word. 100 | /// \param zq The measurement value. 101 | /// \param zpq The CLT parent word's measurement value. 102 | /// \return The probability. 103 | /// 104 | double PzqGzpq(int q, bool zq, bool zpq); 105 | //@} 106 | 107 | /// 108 | /// \brief Get the Chow-Liu tree 109 | /// \return A const reference to the Chow-Liu tree 110 | /// 111 | const cv::Mat & getClTree() 112 | { 113 | return clTree; 114 | } 115 | 116 | /// 117 | /// \brief Get the vocabulary size as specified by the size of the Chow-Liu Tree 118 | /// \return The vocabulary size 119 | /// 120 | const int & vocabSize() 121 | { 122 | return clTree.cols; 123 | } 124 | 125 | protected: 126 | 127 | //@{ 128 | double PzGe; ///< Probability of measurement given the generator is present 129 | double PzGNe; ///< Probability of measurement given the generator is absent 130 | //@} 131 | 132 | /// Are we using naiveBayes? 133 | bool naiveBayes; 134 | /// A pointer to the Chow-Liu tree stored in the FabMap class 135 | cv::Ptr clTreePtr; 136 | /// A reference to the pointer, for legacy access (remove this eventually) 137 | cv::Mat & clTree; 138 | }; 139 | 140 | /// 141 | /// \brief This class implements the binary inference for FabMap. 142 | /// 143 | class CV_EXPORTS InferBinary : public InferBase 144 | { 145 | public: 146 | InferBinary(cv::Ptr _clTree, 147 | const double & _PzGe, 148 | const double & _PzGNe, 149 | const bool & _naiveBayes) 150 | : InferBase(_clTree, _PzGe, _PzGNe, _naiveBayes) 151 | { 152 | } 153 | 154 | //@{ FAB-MAP Core 155 | /// 156 | /// \brief Calculates the measurement probability given the generator state. 157 | /// \param zq If the word was seen in the image. 158 | /// \param eq If the generator is present in the scene. 159 | /// \return The probability of the observation given the generator. 160 | /// 161 | double PzqGeq(bool zq, bool eq); 162 | /// 163 | /// \brief Calculates the generator probability given the past measurement (singular) at the location. 164 | /// TODO: Replace this with a saved state, can update with multiple measurements. 165 | /// \param q The index of the word in the vocabulary. 166 | /// \param Lzq If the word previously observed at this location. 167 | /// \param eq If the generator is present in the scene. 168 | /// \return The probability that the generator is present at the location 169 | /// 170 | double PeqGLzq(int q, bool Lzq, bool eq); 171 | /// 172 | /// \brief Returns the generator probability given the past measurements at the location. 173 | /// \param q The index of the word in the vocabulary. 174 | /// \param Lm The index of the location. 175 | /// \param eq If the generator is present in the scene. 176 | /// \return The probability that the generator is present at the location 177 | /// 178 | double PeqGL(const int & q, const int & Lm, bool eq); 179 | /// 180 | /// \brief Calculates the Naive-Bayes measurement probability given past measurement. 181 | /// The parent measurement is unused. 182 | /// \param q The index of the word in the vocabulary. 183 | /// \param zq If the word was present in the image. 184 | /// \param zpq If the parent word in the Chow-Liu tree was present in the image (unused). 185 | /// \param Lzq If the word previously observed at this location. 186 | /// \param newPlace If we'd like the mean-field probability of being in a new place. 187 | /// \return The measurement probability given the past measurement. 188 | /// 189 | double PzqGL(int q, bool zq, bool zpq, bool Lzq, 190 | const bool & newPlace = false); 191 | /// 192 | /// \brief Calculates the measurement probability given past and parent measurements. 193 | /// \param q The index of the word in the vocabulary. 194 | /// \param zq If the word was present in the image. 195 | /// \param zpq If the parent word in the Chow-Liu tree was present in the image. 196 | /// \param Lzq If the word previously observed at this location. 197 | /// \param newPlace If we'd like the mean-field probability of being in a new place. 198 | /// \return The measurement probability given past and parent measurements. 199 | /// 200 | double PzqGzpqL(int q, bool zq, bool zpq, bool Lzq, 201 | const bool & newPlace = false); 202 | /// 203 | /// \brief Pointer to the function that will calculate the measurement probability (Naive-Bayes or Chow-Liu Tree). 204 | /// \param q The index of the word in the vocabulary. 205 | /// \param zq If the word was present in the image. 206 | /// \param zpq If the parent word in the Chow-Liu tree was present in the image. 207 | /// \param Lzq If the word previously observed at this location. 208 | /// \return The measurement probability given past (and parent measurements for CLT). 209 | /// 210 | double PzGL(int q, bool zq, bool zpq, bool Lzq, 211 | const bool & newPlace) 212 | { 213 | return naiveBayes ? PzqGL(q, zq, zpq, Lzq, newPlace) 214 | : PzqGzpqL(q, zq, zpq, Lzq, newPlace); 215 | } 216 | //@} 217 | }; 218 | 219 | } // namespace of2 220 | 221 | #endif /* INFERENCE_H_ */ 222 | -------------------------------------------------------------------------------- /include/msckd.h: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef MSCKD_H_ 55 | #define MSCKD_H_ 56 | 57 | #include 58 | #include 59 | 60 | #include 61 | 62 | // Custom implementation of Modified Sequential Clustering 63 | class BOWMSCTrainer : public cv::BOWTrainer { 64 | public: 65 | BOWMSCTrainer(double clusterSize = 0.4, int minDescriptorsPerCluster = 2, 66 | bool shuffleDescriptors = false); 67 | virtual ~BOWMSCTrainer(); 68 | 69 | // Returns trained vocabulary (i.e. cluster centers). 70 | virtual cv::Mat cluster() const; 71 | virtual cv::Mat cluster(const cv::Mat &descriptors) const; 72 | 73 | protected: 74 | double clusterSize; 75 | int minDescriptorsPerCluster; 76 | bool shuffleDescriptors; 77 | }; 78 | 79 | #endif // MSCKD_H_ 80 | -------------------------------------------------------------------------------- /include/openfabmap.hpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #ifndef OPENFABMAP_H_ 55 | #define OPENFABMAP_H_ 56 | 57 | // All of the core modules that comprise OpenFABMAP 58 | #include "fabmap.hpp" 59 | #include "bowmsctrainer.hpp" 60 | #include "chowliutree.hpp" 61 | // TODO: Integrate MSC KD-Tree trainer 62 | //#include "msckd.h" 63 | 64 | #endif /* OPENFABMAP_H_ */ 65 | -------------------------------------------------------------------------------- /samples/openFABMAPcli.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include 55 | #include 56 | #include 57 | 58 | #if CV_MAJOR_VERSION == 2 and CV_MINOR_VERSION == 3 59 | 60 | #elif CV_MAJOR_VERSION == 2 and CV_MINOR_VERSION == 4 61 | #if USENONFREE 62 | #include 63 | #endif 64 | #elif CV_MAJOR_VERSION >= 3 65 | #ifdef USENONFREE 66 | #include 67 | #endif 68 | 69 | #if CV_MAJOR_VERSION >= 4 70 | #define CV_CAP_PROP_POS_FRAMES cv::CAP_PROP_POS_FRAMES 71 | #define CV_CAP_PROP_FRAME_COUNT cv::CAP_PROP_FRAME_COUNT 72 | #define CV_AA cv::LINE_AA 73 | #endif 74 | #endif 75 | 76 | #include 77 | #include 78 | 79 | /* 80 | openFABMAP procedural functions 81 | */ 82 | int help(void); 83 | int showFeatures(std::string trainPath, 84 | cv::Ptr &detector); 85 | int generateVocabTrainData(std::string trainPath, 86 | std::string vocabTrainDataPath, 87 | cv::Ptr &detector, 88 | cv::Ptr &extractor); 89 | int trainVocabulary(std::string vocabPath, 90 | std::string vocabTrainDataPath, 91 | double clusterRadius); 92 | 93 | int generateBOWImageDescs(std::string dataPath, 94 | std::string bowImageDescPath, 95 | std::string vocabPath, 96 | cv::Ptr &detector, 97 | cv::Ptr &extractor, 98 | int minWords); 99 | 100 | int trainChowLiuTree(std::string chowliutreePath, 101 | std::string fabmapTrainDataPath, 102 | double lowerInformationBound); 103 | 104 | int openFABMAP(std::string testPath, 105 | of2::FabMap *openFABMAP, 106 | std::string vocabPath, 107 | std::string resultsPath, 108 | bool addNewOnly); 109 | 110 | /* 111 | helper functions 112 | */ 113 | of2::FabMap *generateFABMAPInstance(cv::FileStorage &settings); 114 | cv::Ptr generateDetector(cv::FileStorage &fs); 115 | cv::Ptr generateExtractor(cv::FileStorage &fs); 116 | 117 | /* 118 | Advanced tools for keypoint manipulation. These tools are not currently in the 119 | functional code but are available for use if desired. 120 | */ 121 | void drawRichKeypoints(const cv::Mat& src, std::vector& kpts, 122 | cv::Mat& dst); 123 | void filterKeypoints(std::vector& kpts, int maxSize = 0, 124 | int maxFeatures = 0); 125 | void sortKeypoints(std::vector& keypoints); 126 | 127 | 128 | /* 129 | The openFabMapcli accepts a YML settings file, an example of which is provided. 130 | Modify options in the settings file for desired operation 131 | */ 132 | int main(int argc, char * argv[]) 133 | { 134 | //load the settings file 135 | std::string settfilename; 136 | if (argc == 1) { 137 | //assume settings in working directory 138 | settfilename = "settings.yml"; 139 | } else if (argc == 3) { 140 | if(std::string(argv[1]) != "-s") { 141 | //incorrect option 142 | return help(); 143 | } else { 144 | //settings provided as argument 145 | settfilename = std::string(argv[2]); 146 | } 147 | } else { 148 | //incorrect arguments 149 | return help(); 150 | } 151 | 152 | cv::FileStorage fs; 153 | fs.open(settfilename, cv::FileStorage::READ); 154 | if (!fs.isOpened()) { 155 | std::cerr << "Could not open settings file: " << settfilename << 156 | std::endl; 157 | return -1; 158 | } 159 | 160 | cv::Ptr detector = generateDetector(fs); 161 | if(!detector) { 162 | std::cerr << "Feature Detector error" << std::endl; 163 | return -1; 164 | } 165 | 166 | cv::Ptr extractor = generateExtractor(fs); 167 | if(!extractor) { 168 | std::cerr << "Feature Extractor error" << std::endl; 169 | return -1; 170 | } 171 | 172 | //run desired function 173 | int result = 0; 174 | std::string function = fs["Function"]; 175 | if (function == "ShowFeatures") { 176 | result = showFeatures( 177 | fs["FilePaths"]["TrainPath"], 178 | detector); 179 | 180 | } else if (function == "GenerateVocabTrainData") { 181 | result = generateVocabTrainData(fs["FilePaths"]["TrainPath"], 182 | fs["FilePaths"]["TrainFeatDesc"], 183 | detector, extractor); 184 | 185 | } else if (function == "TrainVocabulary") { 186 | result = trainVocabulary(fs["FilePaths"]["Vocabulary"], 187 | fs["FilePaths"]["TrainFeatDesc"], 188 | fs["VocabTrainOptions"]["ClusterSize"]); 189 | 190 | } else if (function == "GenerateFABMAPTrainData") { 191 | result = generateBOWImageDescs(fs["FilePaths"]["TrainPath"], 192 | fs["FilePaths"]["TrainImagDesc"], 193 | fs["FilePaths"]["Vocabulary"], detector, extractor, 194 | fs["BOWOptions"]["MinWords"]); 195 | 196 | } else if (function == "TrainChowLiuTree") { 197 | result = trainChowLiuTree(fs["FilePaths"]["ChowLiuTree"], 198 | fs["FilePaths"]["TrainImagDesc"], 199 | fs["ChowLiuOptions"]["LowerInfoBound"]); 200 | 201 | } else if (function == "GenerateFABMAPTestData") { 202 | result = generateBOWImageDescs(fs["FilePaths"]["TestPath"], 203 | fs["FilePaths"]["TestImageDesc"], 204 | fs["FilePaths"]["Vocabulary"], detector, extractor, 205 | fs["BOWOptions"]["MinWords"]); 206 | 207 | } else if (function == "RunOpenFABMAP") { 208 | std::string placeAddOption = fs["FabMapPlaceAddition"]; 209 | bool addNewOnly = (placeAddOption == "NewMaximumOnly"); 210 | of2::FabMap *fabmap = generateFABMAPInstance(fs); 211 | if(fabmap) { 212 | result = openFABMAP(fs["FilePaths"]["TestImageDesc"], fabmap, 213 | fs["FilePaths"]["Vocabulary"], 214 | fs["FilePaths"]["FabMapResults"], addNewOnly); 215 | } 216 | 217 | } else { 218 | std::cerr << "Incorrect Function Type" << std::endl; 219 | result = -1; 220 | } 221 | 222 | std::cout << "openFABMAP done" << std::endl; 223 | std::cin.sync(); std::cin.ignore(); 224 | 225 | fs.release(); 226 | return result; 227 | 228 | } 229 | 230 | /* 231 | displays the usage message 232 | */ 233 | int help(void) 234 | { 235 | std::cout << "Usage: openFABMAPexe -s settingsfile" << std::endl; 236 | return 0; 237 | } 238 | 239 | /* 240 | shows the features detected on the training video 241 | */ 242 | int showFeatures(std::string trainPath, cv::Ptr &detector) 243 | { 244 | 245 | //open the movie 246 | cv::VideoCapture movie; 247 | movie.open(trainPath); 248 | 249 | if (!movie.isOpened()) { 250 | std::cerr << trainPath << ": training movie not found" << std::endl; 251 | return -1; 252 | } 253 | 254 | std::cout << "Press Esc to Exit" << std::endl; 255 | cv::Mat frame, kptsImg; 256 | 257 | movie.read(frame); 258 | std::vector kpts; 259 | while (movie.read(frame)) { 260 | detector->detect(frame, kpts); 261 | 262 | std::cout << kpts.size() << " keypoints detected... \r"; 263 | fflush(stdout); 264 | 265 | cv::drawKeypoints(frame, kpts, kptsImg); 266 | 267 | cv::imshow("Features", kptsImg); 268 | if(cv::waitKey(5) == 27) { 269 | break; 270 | } 271 | } 272 | std::cout << std::endl; 273 | 274 | cv::destroyWindow("Features"); 275 | return 0; 276 | } 277 | 278 | /* 279 | generate the data needed to train a codebook/vocabulary for bag-of-words methods 280 | */ 281 | int generateVocabTrainData(std::string trainPath, 282 | std::string vocabTrainDataPath, 283 | cv::Ptr &detector, 284 | cv::Ptr &extractor) 285 | { 286 | 287 | //Do not overwrite any files 288 | std::ifstream checker; 289 | checker.open(vocabTrainDataPath.c_str()); 290 | if(checker.is_open()) { 291 | std::cerr << vocabTrainDataPath << ": Training Data already present" << 292 | std::endl; 293 | checker.close(); 294 | return -1; 295 | } 296 | 297 | //load training movie 298 | cv::VideoCapture movie; 299 | movie.open(trainPath); 300 | if (!movie.isOpened()) { 301 | std::cerr << trainPath << ": training movie not found" << std::endl; 302 | return -1; 303 | } 304 | 305 | //extract data 306 | std::cout << "Extracting Descriptors" << std::endl; 307 | cv::Mat vocabTrainData; 308 | cv::Mat frame, descs, feats; 309 | std::vector kpts; 310 | 311 | std::cout.setf(std::ios_base::fixed); 312 | std::cout.precision(0); 313 | 314 | while(movie.read(frame)) { 315 | 316 | //detect & extract features 317 | detector->detect(frame, kpts); 318 | extractor->compute(frame, kpts, descs); 319 | 320 | //add all descriptors to the training data 321 | vocabTrainData.push_back(descs); 322 | 323 | //show progress 324 | cv::drawKeypoints(frame, kpts, feats); 325 | cv::imshow("Training Data", feats); 326 | 327 | std::cout << 100.0*(movie.get(CV_CAP_PROP_POS_FRAMES) / 328 | movie.get(CV_CAP_PROP_FRAME_COUNT)) << "%. " << 329 | vocabTrainData.rows << " descriptors \r"; 330 | fflush(stdout); 331 | 332 | if(cv::waitKey(5) == 27) { 333 | cv::destroyWindow("Training Data"); 334 | std::cout << std::endl; 335 | return -1; 336 | } 337 | 338 | } 339 | cv::destroyWindow("Training Data"); 340 | std::cout << "Done: " << vocabTrainData.rows << " Descriptors" << std::endl; 341 | 342 | //save the training data 343 | cv::FileStorage fs; 344 | fs.open(vocabTrainDataPath, cv::FileStorage::WRITE); 345 | fs << "VocabTrainData" << vocabTrainData; 346 | fs.release(); 347 | 348 | return 0; 349 | } 350 | 351 | /* 352 | use training data to build a codebook/vocabulary 353 | */ 354 | int trainVocabulary(std::string vocabPath, 355 | std::string vocabTrainDataPath, 356 | double clusterRadius) 357 | { 358 | 359 | //ensure not overwriting a vocabulary 360 | std::ifstream checker; 361 | checker.open(vocabPath.c_str()); 362 | if(checker.is_open()) { 363 | std::cerr << vocabPath << ": Vocabulary already present" << 364 | std::endl; 365 | checker.close(); 366 | return -1; 367 | } 368 | 369 | std::cout << "Loading vocabulary training data" << std::endl; 370 | 371 | cv::FileStorage fs; 372 | 373 | //load in vocab training data 374 | fs.open(vocabTrainDataPath, cv::FileStorage::READ); 375 | cv::Mat vocabTrainData; 376 | fs["VocabTrainData"] >> vocabTrainData; 377 | if (vocabTrainData.empty()) { 378 | std::cerr << vocabTrainDataPath << ": Training Data not found" << 379 | std::endl; 380 | return -1; 381 | } 382 | fs.release(); 383 | 384 | std::cout << "Performing clustering" << std::endl; 385 | 386 | //uses Modified Sequential Clustering to train a vocabulary 387 | of2::BOWMSCTrainer trainer(clusterRadius); 388 | trainer.add(vocabTrainData); 389 | cv::Mat vocab = trainer.cluster(); 390 | 391 | //save the vocabulary 392 | std::cout << "Saving vocabulary" << std::endl; 393 | fs.open(vocabPath, cv::FileStorage::WRITE); 394 | fs << "Vocabulary" << vocab; 395 | fs.release(); 396 | 397 | return 0; 398 | } 399 | 400 | /* 401 | generate FabMap bag-of-words data : an image descriptor for each frame 402 | */ 403 | int generateBOWImageDescs(std::string dataPath, 404 | std::string bowImageDescPath, 405 | std::string vocabPath, 406 | cv::Ptr &detector, 407 | cv::Ptr &extractor, 408 | int minWords) 409 | { 410 | 411 | cv::FileStorage fs; 412 | 413 | //ensure not overwriting training data 414 | std::ifstream checker; 415 | checker.open(bowImageDescPath.c_str()); 416 | if(checker.is_open()) { 417 | std::cerr << bowImageDescPath << ": FabMap Training/Testing Data " 418 | "already present" << std::endl; 419 | checker.close(); 420 | return -1; 421 | } 422 | 423 | //load vocabulary 424 | std::cout << "Loading Vocabulary" << std::endl; 425 | fs.open(vocabPath, cv::FileStorage::READ); 426 | cv::Mat vocab; 427 | fs["Vocabulary"] >> vocab; 428 | if (vocab.empty()) { 429 | std::cerr << vocabPath << ": Vocabulary not found" << std::endl; 430 | return -1; 431 | } 432 | fs.release(); 433 | 434 | //use a FLANN matcher to generate bag-of-words representations 435 | cv::Ptr matcher = 436 | cv::DescriptorMatcher::create("FlannBased"); 437 | cv::BOWImgDescriptorExtractor bide(extractor, matcher); 438 | bide.setVocabulary(vocab); 439 | 440 | //load movie 441 | cv::VideoCapture movie; 442 | movie.open(dataPath); 443 | 444 | if(!movie.isOpened()) { 445 | std::cerr << dataPath << ": movie not found" << std::endl; 446 | return -1; 447 | } 448 | 449 | //extract image descriptors 450 | cv::Mat fabmapTrainData; 451 | std::cout << "Extracting Bag-of-words Image Descriptors" << std::endl; 452 | std::cout.setf(std::ios_base::fixed); 453 | std::cout.precision(0); 454 | 455 | std::ofstream maskw; 456 | 457 | if(minWords) { 458 | maskw.open(std::string(bowImageDescPath + "mask.txt").c_str()); 459 | } 460 | 461 | cv::Mat frame, bow; 462 | std::vector kpts; 463 | 464 | while(movie.read(frame)) { 465 | detector->detect(frame, kpts); 466 | bide.compute(frame, kpts, bow); 467 | 468 | if(minWords) { 469 | //writing a mask file 470 | if(cv::countNonZero(bow) < minWords) { 471 | //frame masked 472 | maskw << "0" << std::endl; 473 | } else { 474 | //frame accepted 475 | maskw << "1" << std::endl; 476 | fabmapTrainData.push_back(bow); 477 | } 478 | } else { 479 | fabmapTrainData.push_back(bow); 480 | } 481 | 482 | std::cout << 100.0 * (movie.get(CV_CAP_PROP_POS_FRAMES) / 483 | movie.get(CV_CAP_PROP_FRAME_COUNT)) << "% \r"; 484 | fflush(stdout); 485 | } 486 | std::cout << "Done " << std::endl; 487 | 488 | movie.release(); 489 | 490 | //save training data 491 | fs.open(bowImageDescPath, cv::FileStorage::WRITE); 492 | fs << "BOWImageDescs" << fabmapTrainData; 493 | fs.release(); 494 | 495 | return 0; 496 | } 497 | 498 | /* 499 | generate a Chow-Liu tree from FabMap Training data 500 | */ 501 | int trainChowLiuTree(std::string chowliutreePath, 502 | std::string fabmapTrainDataPath, 503 | double lowerInformationBound) 504 | { 505 | 506 | cv::FileStorage fs; 507 | 508 | //ensure not overwriting training data 509 | std::ifstream checker; 510 | checker.open(chowliutreePath.c_str()); 511 | if(checker.is_open()) { 512 | std::cerr << chowliutreePath << ": Chow-Liu Tree already present" << 513 | std::endl; 514 | checker.close(); 515 | return -1; 516 | } 517 | 518 | //load FabMap training data 519 | std::cout << "Loading FabMap Training Data" << std::endl; 520 | fs.open(fabmapTrainDataPath, cv::FileStorage::READ); 521 | cv::Mat fabmapTrainData; 522 | fs["BOWImageDescs"] >> fabmapTrainData; 523 | if (fabmapTrainData.empty()) { 524 | std::cerr << fabmapTrainDataPath << ": FabMap Training Data not found" 525 | << std::endl; 526 | return -1; 527 | } 528 | fs.release(); 529 | 530 | //generate the tree from the data 531 | std::cout << "Making Chow-Liu Tree" << std::endl; 532 | of2::ChowLiuTree tree; 533 | tree.add(fabmapTrainData); 534 | cv::Mat clTree = tree.make(lowerInformationBound); 535 | 536 | //save the resulting tree 537 | std::cout <<"Saving Chow-Liu Tree" << std::endl; 538 | fs.open(chowliutreePath, cv::FileStorage::WRITE); 539 | fs << "ChowLiuTree" << clTree; 540 | fs.release(); 541 | 542 | return 0; 543 | 544 | } 545 | 546 | 547 | /* 548 | Run FabMap on a test dataset 549 | */ 550 | int openFABMAP(std::string testPath, 551 | of2::FabMap *fabmap, 552 | std::string vocabPath, 553 | std::string resultsPath, 554 | bool addNewOnly) 555 | { 556 | 557 | cv::FileStorage fs; 558 | 559 | //ensure not overwriting results 560 | std::ifstream checker; 561 | checker.open(resultsPath.c_str()); 562 | if(checker.is_open()) { 563 | std::cerr << resultsPath << ": Results already present" << std::endl; 564 | checker.close(); 565 | return -1; 566 | } 567 | 568 | //load the vocabulary 569 | std::cout << "Loading Vocabulary" << std::endl; 570 | fs.open(vocabPath, cv::FileStorage::READ); 571 | cv::Mat vocab; 572 | fs["Vocabulary"] >> vocab; 573 | if (vocab.empty()) { 574 | std::cerr << vocabPath << ": Vocabulary not found" << std::endl; 575 | return -1; 576 | } 577 | fs.release(); 578 | 579 | //load the test data 580 | fs.open(testPath, cv::FileStorage::READ); 581 | cv::Mat testImageDescs; 582 | fs["BOWImageDescs"] >> testImageDescs; 583 | if(testImageDescs.empty()) { 584 | std::cerr << testPath << ": Test data not found" << std::endl; 585 | return -1; 586 | } 587 | fs.release(); 588 | 589 | //running openFABMAP 590 | std::cout << "Running openFABMAP" << std::endl; 591 | std::vector matches; 592 | std::vector::iterator l; 593 | 594 | 595 | 596 | cv::Mat confusion_mat(testImageDescs.rows, testImageDescs.rows, CV_64FC1); 597 | confusion_mat.setTo(0); // init to 0's 598 | 599 | 600 | if (!addNewOnly) { 601 | 602 | //automatically comparing a whole dataset 603 | fabmap->localize(testImageDescs, matches, true); 604 | 605 | for(l = matches.begin(); l != matches.end(); l++) { 606 | if(l->imgIdx < 0) { 607 | confusion_mat.at(l->queryIdx, l->queryIdx) = l->match; 608 | 609 | } else { 610 | confusion_mat.at(l->queryIdx, l->imgIdx) = l->match; 611 | } 612 | } 613 | 614 | } else { 615 | 616 | //criteria for adding locations used 617 | for(int i = 0; i < testImageDescs.rows; i++) { 618 | matches.clear(); 619 | //compare images individually 620 | fabmap->localize(testImageDescs.row(i), matches); 621 | 622 | bool new_place_max = true; 623 | for(l = matches.begin(); l != matches.end(); l++) { 624 | 625 | if(l->imgIdx < 0) { 626 | //add the new place to the confusion matrix 'diagonal' 627 | confusion_mat.at(i, (int)matches.size()-1) = l->match; 628 | 629 | } else { 630 | //add the score to the confusion matrix 631 | confusion_mat.at(i, l->imgIdx) = l->match; 632 | } 633 | 634 | //test for new location maximum 635 | if(l->match > matches.front().match) { 636 | new_place_max = false; 637 | } 638 | } 639 | 640 | if(new_place_max) { 641 | fabmap->add(testImageDescs.row(i)); 642 | } 643 | } 644 | } 645 | 646 | //save the result as plain text for ease of import to Matlab 647 | std::ofstream writer(resultsPath.c_str()); 648 | for(int i = 0; i < confusion_mat.rows; i++) { 649 | for(int j = 0; j < confusion_mat.cols; j++) { 650 | writer << confusion_mat.at(i, j) << " "; 651 | } 652 | writer << std::endl; 653 | } 654 | writer.close(); 655 | 656 | return 0; 657 | } 658 | 659 | #if CV_MAJOR_VERSION == 2 660 | /* 661 | generates a feature detector based on options in the settings file 662 | */ 663 | cv::Ptr generateDetector(cv::FileStorage &fs) { 664 | 665 | //create common feature detector and descriptor extractor 666 | std::string detectorMode = fs["FeatureOptions"]["DetectorMode"]; 667 | std::string detectorType = fs["FeatureOptions"]["DetectorType"]; 668 | cv::Ptr detector = NULL; 669 | if(detectorMode == "ADAPTIVE") { 670 | 671 | if(detectorType != "STAR" && detectorType != "SURF" && 672 | detectorType != "FAST") { 673 | std::cerr << "Adaptive Detectors only work with STAR, SURF " 674 | "and FAST" << std::endl; 675 | } else { 676 | 677 | detector = new cv::DynamicAdaptedFeatureDetector( 678 | cv::AdjusterAdapter::create(detectorType), 679 | fs["FeatureOptions"]["Adaptive"]["MinFeatures"], 680 | fs["FeatureOptions"]["Adaptive"]["MaxFeatures"], 681 | fs["FeatureOptions"]["Adaptive"]["MaxIters"]); 682 | } 683 | 684 | } else if(detectorMode == "STATIC") { 685 | if(detectorType == "STAR") { 686 | 687 | detector = new cv::StarFeatureDetector( 688 | fs["FeatureOptions"]["StarDetector"]["MaxSize"], 689 | fs["FeatureOptions"]["StarDetector"]["Response"], 690 | fs["FeatureOptions"]["StarDetector"]["LineThreshold"], 691 | fs["FeatureOptions"]["StarDetector"]["LineBinarized"], 692 | fs["FeatureOptions"]["StarDetector"]["Suppression"]); 693 | 694 | } else if(detectorType == "FAST") { 695 | 696 | detector = new cv::FastFeatureDetector( 697 | fs["FeatureOptions"]["FastDetector"]["Threshold"], 698 | (int)fs["FeatureOptions"]["FastDetector"] 699 | ["NonMaxSuppression"] > 0); 700 | } else if(detectorType == "MSER") { 701 | 702 | detector = new cv::MserFeatureDetector( 703 | fs["FeatureOptions"]["MSERDetector"]["Delta"], 704 | fs["FeatureOptions"]["MSERDetector"]["MinArea"], 705 | fs["FeatureOptions"]["MSERDetector"]["MaxArea"], 706 | fs["FeatureOptions"]["MSERDetector"]["MaxVariation"], 707 | fs["FeatureOptions"]["MSERDetector"]["MinDiversity"], 708 | fs["FeatureOptions"]["MSERDetector"]["MaxEvolution"], 709 | fs["FeatureOptions"]["MSERDetector"]["AreaThreshold"], 710 | fs["FeatureOptions"]["MSERDetector"]["MinMargin"], 711 | fs["FeatureOptions"]["MSERDetector"]["EdgeBlurSize"]); 712 | #if USENONFREE 713 | } else if(detectorType == "SURF") { 714 | 715 | #if CV_MINOR_VERSION == 4 716 | detector = new cv::SURF( 717 | fs["FeatureOptions"]["SurfDetector"]["HessianThreshold"], 718 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 719 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 720 | (int)fs["FeatureOptions"]["SurfDetector"]["Extended"] > 0, 721 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 722 | 723 | #elif CV_MINOR_VERSION == 3 724 | detector = new cv::SurfFeatureDetector( 725 | fs["FeatureOptions"]["SurfDetector"]["HessianThreshold"], 726 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 727 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 728 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 729 | #endif 730 | } else if(detectorType == "SIFT") { 731 | #if CV_MINOR_VERSION == 4 732 | detector = new cv::SIFT( 733 | fs["FeatureOptions"]["SiftDetector"]["NumFeatures"], 734 | fs["FeatureOptions"]["SiftDetector"]["NumOctaveLayers"], 735 | fs["FeatureOptions"]["SiftDetector"]["ContrastThreshold"], 736 | fs["FeatureOptions"]["SiftDetector"]["EdgeThreshold"], 737 | fs["FeatureOptions"]["SiftDetector"]["Sigma"]); 738 | #elif CV_MINOR_VERSION == 3 739 | detector = new cv::SiftFeatureDetector( 740 | fs["FeatureOptions"]["SiftDetector"]["ContrastThreshold"], 741 | fs["FeatureOptions"]["SiftDetector"]["EdgeThreshold"]); 742 | #endif 743 | #endif //USENONFREE 744 | 745 | } else { 746 | std::cerr << "Could not create detector class. Specify detector " 747 | "options in the settings file" << std::endl; 748 | } 749 | } else { 750 | std::cerr << "Could not create detector class. Specify detector " 751 | "mode (static/adaptive) in the settings file" << std::endl; 752 | } 753 | 754 | return detector; 755 | 756 | } 757 | #elif CV_MAJOR_VERSION >= 3 758 | /* 759 | generates a feature detector based on options in the settings file 760 | */ 761 | cv::Ptr generateDetector(cv::FileStorage &fs) { 762 | 763 | //create common feature detector and descriptor extractor 764 | std::string detectorType = fs["FeatureOptions"]["DetectorType"]; 765 | 766 | if(detectorType == "BRISK") { 767 | return cv::BRISK::create( 768 | fs["FeatureOptions"]["BRISK"]["Threshold"], 769 | fs["FeatureOptions"]["BRISK"]["Octaves"], 770 | fs["FeatureOptions"]["BRISK"]["PatternScale"]); 771 | } else if(detectorType == "ORB") { 772 | return cv::ORB::create( 773 | fs["FeatureOptions"]["ORB"]["nFeatures"], 774 | fs["FeatureOptions"]["ORB"]["scaleFactor"], 775 | fs["FeatureOptions"]["ORB"]["nLevels"], 776 | fs["FeatureOptions"]["ORB"]["edgeThreshold"], 777 | fs["FeatureOptions"]["ORB"]["firstLevel"], 778 | 2, cv::ORB::HARRIS_SCORE, 779 | fs["FeatureOptions"]["ORB"]["patchSize"]); 780 | 781 | } else if(detectorType == "MSER") { 782 | return cv::MSER::create( 783 | fs["FeatureOptions"]["MSERDetector"]["Delta"], 784 | fs["FeatureOptions"]["MSERDetector"]["MinArea"], 785 | fs["FeatureOptions"]["MSERDetector"]["MaxArea"], 786 | fs["FeatureOptions"]["MSERDetector"]["MaxVariation"], 787 | fs["FeatureOptions"]["MSERDetector"]["MinDiversity"], 788 | fs["FeatureOptions"]["MSERDetector"]["MaxEvolution"], 789 | fs["FeatureOptions"]["MSERDetector"]["AreaThreshold"], 790 | fs["FeatureOptions"]["MSERDetector"]["MinMargin"], 791 | fs["FeatureOptions"]["MSERDetector"]["EdgeBlurSize"]); 792 | } else if(detectorType == "FAST") { 793 | return cv::FastFeatureDetector::create( 794 | fs["FeatureOptions"]["FastDetector"]["Threshold"], 795 | (int)fs["FeatureOptions"]["FastDetector"]["NonMaxSuppression"] > 0); 796 | } else if(detectorType == "AGAST") { 797 | return cv::AgastFeatureDetector::create( 798 | fs["FeatureOptions"]["AGAST"]["Threshold"], 799 | (int)fs["FeatureOptions"]["AGAST"]["NonMaxSuppression"] > 0); 800 | #ifdef USENONFREE 801 | } else if(detectorType == "STAR") { 802 | 803 | return cv::xfeatures2d::StarDetector::create( 804 | fs["FeatureOptions"]["StarDetector"]["MaxSize"], 805 | fs["FeatureOptions"]["StarDetector"]["Response"], 806 | fs["FeatureOptions"]["StarDetector"]["LineThreshold"], 807 | fs["FeatureOptions"]["StarDetector"]["LineBinarized"], 808 | fs["FeatureOptions"]["StarDetector"]["Suppression"]); 809 | 810 | } else if(detectorType == "SURF") { 811 | return cv::xfeatures2d::SURF::create( 812 | fs["FeatureOptions"]["SurfDetector"]["HessianThreshold"], 813 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 814 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 815 | (int)fs["FeatureOptions"]["SurfDetector"]["Extended"] > 0, 816 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 817 | 818 | } else if(detectorType == "SIFT") { 819 | return cv::SIFT::create( 820 | fs["FeatureOptions"]["SiftDetector"]["NumFeatures"], 821 | fs["FeatureOptions"]["SiftDetector"]["NumOctaveLayers"], 822 | fs["FeatureOptions"]["SiftDetector"]["ContrastThreshold"], 823 | fs["FeatureOptions"]["SiftDetector"]["EdgeThreshold"], 824 | fs["FeatureOptions"]["SiftDetector"]["Sigma"]); 825 | #endif 826 | 827 | } else { 828 | std::cerr << "Could not create detector class. Specify detector " 829 | "mode (static/adaptive) in the settings file" << std::endl; 830 | } 831 | 832 | return cv::Ptr(); //return the nullptr 833 | 834 | } 835 | #endif 836 | 837 | 838 | /* 839 | generates a feature detector based on options in the settings file 840 | */ 841 | #if CV_MAJOR_VERSION == 2 842 | cv::Ptr generateExtractor(cv::FileStorage &fs) 843 | { 844 | std::string extractorType = fs["FeatureOptions"]["ExtractorType"]; 845 | cv::Ptr extractor = NULL; 846 | if(extractorType == "DUMMY") { 847 | 848 | #ifdef USENONFREE 849 | } else if(extractorType == "SIFT") { 850 | #if CV_MINOR_VERSION == 4 851 | extractor = new cv::SIFT( 852 | fs["FeatureOptions"]["SiftDetector"]["NumFeatures"], 853 | fs["FeatureOptions"]["SiftDetector"]["NumOctaveLayers"], 854 | fs["FeatureOptions"]["SiftDetector"]["ContrastThreshold"], 855 | fs["FeatureOptions"]["SiftDetector"]["EdgeThreshold"], 856 | fs["FeatureOptions"]["SiftDetector"]["Sigma"]); 857 | #elif CV_MINOR_VERSION == 3 858 | extractor = new cv::SiftDescriptorExtractor(); 859 | #endif 860 | 861 | } else if(extractorType == "SURF") { 862 | 863 | #if CV_MINOR_VERSION == 4 864 | extractor = new cv::SURF( 865 | fs["FeatureOptions"]["SurfDetector"]["HessianThreshold"], 866 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 867 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 868 | (int)fs["FeatureOptions"]["SurfDetector"]["Extended"] > 0, 869 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 870 | 871 | #elif CV_MINOR_VERSION == 3 872 | extractor = new cv::SurfDescriptorExtractor( 873 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 874 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 875 | (int)fs["FeatureOptions"]["SurfDetector"]["Extended"] > 0, 876 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 877 | #endif 878 | #endif 879 | 880 | } else { 881 | std::cerr << "Could not create Descriptor Extractor. Please specify " 882 | "extractor type in settings file" << std::endl; 883 | } 884 | 885 | return extractor; 886 | 887 | } 888 | #elif CV_MAJOR_VERSION >= 3 889 | 890 | cv::Ptr generateExtractor(cv::FileStorage &fs) 891 | { 892 | std::string extractorType = fs["FeatureOptions"]["ExtractorType"]; 893 | 894 | if(extractorType == "BRISK") { 895 | return cv::BRISK::create( 896 | fs["FeatureOptions"]["BRISK"]["Threshold"], 897 | fs["FeatureOptions"]["BRISK"]["Octaves"], 898 | fs["FeatureOptions"]["BRISK"]["PatternScale"]); 899 | } else if(extractorType == "ORB") { 900 | return cv::ORB::create( 901 | fs["FeatureOptions"]["ORB"]["nFeatures"], 902 | fs["FeatureOptions"]["ORB"]["scaleFactor"], 903 | fs["FeatureOptions"]["ORB"]["nLevels"], 904 | fs["FeatureOptions"]["ORB"]["edgeThreshold"], 905 | fs["FeatureOptions"]["ORB"]["firstLevel"], 906 | 2, cv::ORB::HARRIS_SCORE, 907 | fs["FeatureOptions"]["ORB"]["patchSize"]); 908 | #ifdef USENONFREE 909 | } else if(extractorType == "SURF") { 910 | return cv::xfeatures2d::SURF::create( 911 | fs["FeatureOptions"]["SurfDetector"]["HessianThreshold"], 912 | fs["FeatureOptions"]["SurfDetector"]["NumOctaves"], 913 | fs["FeatureOptions"]["SurfDetector"]["NumOctaveLayers"], 914 | (int)fs["FeatureOptions"]["SurfDetector"]["Extended"] > 0, 915 | (int)fs["FeatureOptions"]["SurfDetector"]["Upright"] > 0); 916 | 917 | } else if(extractorType == "SIFT") { 918 | return cv::SIFT::create( 919 | fs["FeatureOptions"]["SiftDetector"]["NumFeatures"], 920 | fs["FeatureOptions"]["SiftDetector"]["NumOctaveLayers"], 921 | fs["FeatureOptions"]["SiftDetector"]["ContrastThreshold"], 922 | fs["FeatureOptions"]["SiftDetector"]["EdgeThreshold"], 923 | fs["FeatureOptions"]["SiftDetector"]["Sigma"]); 924 | #endif 925 | 926 | } else { 927 | std::cerr << "Could not create Descriptor Extractor. Please specify " 928 | "extractor type in settings file" << std::endl; 929 | } 930 | 931 | return cv::Ptr(); 932 | 933 | } 934 | 935 | #endif 936 | 937 | 938 | 939 | /* 940 | create an instance of a FabMap class with the options given in the settings file 941 | */ 942 | of2::FabMap *generateFABMAPInstance(cv::FileStorage &settings) 943 | { 944 | 945 | cv::FileStorage fs; 946 | 947 | //load FabMap training data 948 | std::string fabmapTrainDataPath = settings["FilePaths"]["TrainImagDesc"]; 949 | std::string chowliutreePath = settings["FilePaths"]["ChowLiuTree"]; 950 | 951 | std::cout << "Loading FabMap Training Data" << std::endl; 952 | fs.open(fabmapTrainDataPath, cv::FileStorage::READ); 953 | cv::Mat fabmapTrainData; 954 | fs["BOWImageDescs"] >> fabmapTrainData; 955 | if (fabmapTrainData.empty()) { 956 | std::cerr << fabmapTrainDataPath << ": FabMap Training Data not found" 957 | << std::endl; 958 | return NULL; 959 | } 960 | fs.release(); 961 | 962 | //load a chow-liu tree 963 | std::cout << "Loading Chow-Liu Tree" << std::endl; 964 | fs.open(chowliutreePath, cv::FileStorage::READ); 965 | cv::Mat clTree; 966 | fs["ChowLiuTree"] >> clTree; 967 | if (clTree.empty()) { 968 | std::cerr << chowliutreePath << ": Chow-Liu tree not found" << 969 | std::endl; 970 | return NULL; 971 | } 972 | fs.release(); 973 | 974 | //create options flags 975 | std::string newPlaceMethod = 976 | settings["openFabMapOptions"]["NewPlaceMethod"]; 977 | std::string bayesMethod = settings["openFabMapOptions"]["BayesMethod"]; 978 | int simpleMotionModel = settings["openFabMapOptions"]["SimpleMotion"]; 979 | int options = 0; 980 | if(newPlaceMethod == "Sampled") { 981 | options |= of2::FabMap::SAMPLED; 982 | } else { 983 | options |= of2::FabMap::MEAN_FIELD; 984 | } 985 | if(bayesMethod == "ChowLiu") { 986 | options |= of2::FabMap::CHOW_LIU; 987 | } else { 988 | options |= of2::FabMap::NAIVE_BAYES; 989 | } 990 | if(simpleMotionModel) { 991 | options |= of2::FabMap::MOTION_MODEL; 992 | } 993 | 994 | of2::FabMap *fabmap; 995 | 996 | //create an instance of the desired type of FabMap 997 | std::string fabMapVersion = settings["openFabMapOptions"]["FabMapVersion"]; 998 | if(fabMapVersion == "FABMAP1") { 999 | fabmap = new of2::FabMap1(clTree, 1000 | settings["openFabMapOptions"]["PzGe"], 1001 | settings["openFabMapOptions"]["PzGne"], 1002 | options, 1003 | settings["openFabMapOptions"]["NumSamples"]); 1004 | } else if(fabMapVersion == "FABMAPLUT") { 1005 | fabmap = new of2::FabMapLUT(clTree, 1006 | settings["openFabMapOptions"]["PzGe"], 1007 | settings["openFabMapOptions"]["PzGne"], 1008 | options, 1009 | settings["openFabMapOptions"]["NumSamples"], 1010 | settings["openFabMapOptions"]["FabMapLUT"]["Precision"]); 1011 | } else if(fabMapVersion == "FABMAPFBO") { 1012 | fabmap = new of2::FabMapFBO(clTree, 1013 | settings["openFabMapOptions"]["PzGe"], 1014 | settings["openFabMapOptions"]["PzGne"], 1015 | options, 1016 | settings["openFabMapOptions"]["NumSamples"], 1017 | settings["openFabMapOptions"]["FabMapFBO"]["RejectionThreshold"], 1018 | settings["openFabMapOptions"]["FabMapFBO"]["PsGd"], 1019 | settings["openFabMapOptions"]["FabMapFBO"]["BisectionStart"], 1020 | settings["openFabMapOptions"]["FabMapFBO"]["BisectionIts"]); 1021 | } else if(fabMapVersion == "FABMAP2") { 1022 | fabmap = new of2::FabMap2(clTree, 1023 | settings["openFabMapOptions"]["PzGe"], 1024 | settings["openFabMapOptions"]["PzGne"], 1025 | options); 1026 | } else { 1027 | std::cerr << "Could not identify openFABMAPVersion from settings" 1028 | " file" << std::endl; 1029 | return NULL; 1030 | } 1031 | 1032 | //add the training data for use with the sampling method 1033 | fabmap->addTraining(fabmapTrainData); 1034 | 1035 | return fabmap; 1036 | 1037 | } 1038 | 1039 | 1040 | 1041 | /* 1042 | draws keypoints to scale with coloring proportional to feature strength 1043 | */ 1044 | void drawRichKeypoints(const cv::Mat& src, std::vector& kpts, cv::Mat& dst) { 1045 | 1046 | cv::Mat grayFrame; 1047 | cvtColor(src, grayFrame, cv::COLOR_RGB2GRAY); 1048 | cvtColor(grayFrame, dst, cv::COLOR_GRAY2RGB); 1049 | 1050 | if (kpts.size() == 0) { 1051 | return; 1052 | } 1053 | 1054 | std::vector kpts_cpy, kpts_sorted; 1055 | 1056 | kpts_cpy.insert(kpts_cpy.end(), kpts.begin(), kpts.end()); 1057 | 1058 | double maxResponse = kpts_cpy.at(0).response; 1059 | double minResponse = kpts_cpy.at(0).response; 1060 | 1061 | while (kpts_cpy.size() > 0) { 1062 | 1063 | double maxR = 0.0; 1064 | unsigned int idx = 0; 1065 | 1066 | for (unsigned int iii = 0; iii < kpts_cpy.size(); iii++) { 1067 | 1068 | if (kpts_cpy.at(iii).response > maxR) { 1069 | maxR = kpts_cpy.at(iii).response; 1070 | idx = iii; 1071 | } 1072 | 1073 | if (kpts_cpy.at(iii).response > maxResponse) { 1074 | maxResponse = kpts_cpy.at(iii).response; 1075 | } 1076 | 1077 | if (kpts_cpy.at(iii).response < minResponse) { 1078 | minResponse = kpts_cpy.at(iii).response; 1079 | } 1080 | } 1081 | 1082 | kpts_sorted.push_back(kpts_cpy.at(idx)); 1083 | kpts_cpy.erase(kpts_cpy.begin() + idx); 1084 | 1085 | } 1086 | 1087 | int thickness = 1; 1088 | cv::Point center; 1089 | cv::Scalar colour; 1090 | int red = 0, blue = 0, green = 0; 1091 | int radius; 1092 | double normalizedScore; 1093 | 1094 | if (minResponse == maxResponse) { 1095 | colour = CV_RGB(255, 0, 0); 1096 | } 1097 | 1098 | for (int iii = (int)kpts_sorted.size()-1; iii >= 0; iii--) { 1099 | 1100 | if (minResponse != maxResponse) { 1101 | normalizedScore = pow((kpts_sorted.at(iii).response - minResponse) / (maxResponse - minResponse), 0.25); 1102 | red = int(255.0 * normalizedScore); 1103 | green = int(255.0 - 255.0 * normalizedScore); 1104 | colour = CV_RGB(red, green, blue); 1105 | } 1106 | 1107 | center = kpts_sorted.at(iii).pt; 1108 | center.x *= 16; 1109 | center.y *= 16; 1110 | 1111 | radius = (int)(16.0 * ((double)(kpts_sorted.at(iii).size)/2.0)); 1112 | 1113 | if (radius > 0) { 1114 | circle(dst, center, radius, colour, thickness, CV_AA, 4); 1115 | } 1116 | 1117 | } 1118 | 1119 | } 1120 | 1121 | /* 1122 | Removes surplus features and those with invalid size 1123 | */ 1124 | void filterKeypoints(std::vector& kpts, int maxSize, int maxFeatures) { 1125 | 1126 | if (maxSize == 0) { 1127 | return; 1128 | } 1129 | 1130 | sortKeypoints(kpts); 1131 | 1132 | for (unsigned int iii = 0; iii < kpts.size(); iii++) { 1133 | 1134 | if (kpts.at(iii).size > float(maxSize)) { 1135 | kpts.erase(kpts.begin() + iii); 1136 | iii--; 1137 | } 1138 | } 1139 | 1140 | if ((maxFeatures != 0) && ((int)kpts.size() > maxFeatures)) { 1141 | kpts.erase(kpts.begin()+maxFeatures, kpts.end()); 1142 | } 1143 | 1144 | } 1145 | 1146 | /* 1147 | Sorts keypoints in descending order of response (strength) 1148 | */ 1149 | void sortKeypoints(std::vector& keypoints) { 1150 | 1151 | if (keypoints.size() <= 1) { 1152 | return; 1153 | } 1154 | 1155 | std::vector sortedKeypoints; 1156 | 1157 | // Add the first one 1158 | sortedKeypoints.push_back(keypoints.at(0)); 1159 | 1160 | for (unsigned int i = 1; i < keypoints.size(); i++) { 1161 | 1162 | unsigned int j = 0; 1163 | bool hasBeenAdded = false; 1164 | 1165 | while ((j < sortedKeypoints.size()) && (!hasBeenAdded)) { 1166 | 1167 | if (abs(keypoints.at(i).response) > abs(sortedKeypoints.at(j).response)) { 1168 | sortedKeypoints.insert(sortedKeypoints.begin() + j, keypoints.at(i)); 1169 | 1170 | hasBeenAdded = true; 1171 | } 1172 | 1173 | j++; 1174 | } 1175 | 1176 | if (!hasBeenAdded) { 1177 | sortedKeypoints.push_back(keypoints.at(i)); 1178 | } 1179 | 1180 | } 1181 | 1182 | keypoints.swap(sortedKeypoints); 1183 | 1184 | } 1185 | -------------------------------------------------------------------------------- /samples/settings.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # openFABMAPexe Settings File 4 | 5 | #--------------------------------------------------------------------------- 6 | 7 | FilePaths: 8 | #The training data video should be of disjoint, non-overlapping scenes 9 | #and should not visit the same location more than once 10 | 11 | TrainPath: "C:\\openFABMAP\\trainvideo.avi" 12 | 13 | #The test video should not overlap with the training video but should 14 | #have similar environment types. 15 | 16 | TestPath: "C:\\openFABMAP\\testvideo.avi" 17 | 18 | #All feature descriptors extracted from the training data. Used to 19 | #create the vocabulary/codebook 20 | 21 | TrainFeatDesc: "C:\\openFABMAP\\vocabularytraindata.yml" 22 | 23 | #All bag-of-words type whole image descriptors extracted from the 24 | #training data. Used to create the chow-liu tree and used in the 25 | #FabMap 'new place' likelihood calculation 26 | 27 | TrainImagDesc: "C:\\openFABMAP\\BOWtraindata.yml" 28 | 29 | #The vocabulary/codebook itself 30 | 31 | Vocabulary: "C:\\openFABMAP\\vocabulary.yml" 32 | 33 | #The Chow-Liu Tree itself 34 | 35 | ChowLiuTree: "C:\\openFABMAP\\tree.yml" 36 | 37 | #The FabMap Test 38 | 39 | TestImageDesc: "C:\\openFABMAP\\BOWtestdata.yml" 40 | 41 | #The FabMap results 42 | 43 | FabMapResults: "C:\\openFABMAP\\results.txt" 44 | 45 | #--------------------------------------------------------------------------- 46 | 47 | # openFABMAP running mode: 48 | 49 | # "ShowFeatures" 50 | # "GenerateVocabTrainData" 51 | # "TrainVocabulary" 52 | # "GenerateFABMAPTrainData" 53 | # "TrainChowLiuTree" 54 | # "GenerateFABMAPTestData" 55 | # "RunOpenFABMAP" 56 | 57 | Function: "ShowFeatures" 58 | 59 | #--------------------------------------------------------------------------- 60 | 61 | FeatureOptions: 62 | 63 | # Feature Detection Options 64 | # "FAST" 65 | # "STAR" 66 | # "SIFT" 67 | # "SURF" 68 | # "MSER" 69 | # "ORB" 70 | # "BRISK" 71 | 72 | DetectorType: "SURF" 73 | 74 | # Feature Detection Modes 75 | # "STATIC" 76 | # "ADAPTIVE" 77 | 78 | DetectorMode: "STATIC" 79 | 80 | #ADAPTIVE SETTINGS 81 | 82 | Adaptive: 83 | MinFeatures: 300 84 | MaxFeatures: 500 85 | MaxIters: 5 86 | 87 | # STATIC SETTINGS 88 | 89 | FastDetector: 90 | Threshold: 50 91 | NonMaxSuppression: 1 92 | 93 | StarDetector: 94 | MaxSize: 32 #45 95 | Response: 10 #30 96 | LineThreshold: 18 #10 97 | LineBinarized: 18 #8 98 | Suppression: 20 #5 99 | 100 | SiftDetector: 101 | EdgeThreshold: 10 102 | ContrastThreshold: 0.04 103 | 104 | #OPENCV2.4+only 105 | NumFeatures: 200 106 | NumOctaveLayers: 3 107 | Sigma: 1.6 108 | 109 | SurfDetector: 110 | HessianThreshold: 1000 #400 111 | NumOctaves: 4 112 | NumOctaveLayers: 2 113 | Upright: 1 114 | Extended: 0 115 | 116 | MSERDetector: 117 | Delta: 5 118 | MinArea: 60 119 | MaxArea: 14400 120 | MaxVariation: 0.25 121 | MinDiversity: 0.2 122 | MaxEvolution: 200 123 | AreaThreshold: 1.01 124 | MinMargin: 0.003 125 | EdgeBlurSize: 5 126 | 127 | ORB: 128 | nFeatures: 500 129 | scaleFactor: 1.2 130 | nLevels: 8 131 | edgeThreshold: 31 132 | firstLevel: 0 133 | patchSize: 31 134 | 135 | BRISK: 136 | Threshold: 30 137 | Octaves: 3 138 | PatternScale: 1.0 139 | 140 | AGAST: 141 | Threshold: 20 142 | NonMaxSuppression: 1 143 | 144 | # Descriptor Extraction Options 145 | # "SIFT" 146 | # "SURF" 147 | # "ORB" 148 | # "BRISK" 149 | 150 | ExtractorType: "SURF" 151 | #--------------------------------------------------------------------------- 152 | 153 | #An option to throw away frames with low numbers of different words. 154 | #Setting this to 0 turns off this feature 155 | 156 | BOWOptions: 157 | 158 | MinWords: 0 159 | 160 | #--------------------------------------------------------------------------- 161 | 162 | VocabTrainOptions: 163 | 164 | # a smaller clustersize increases the specificity of each codeword 165 | # and will increase the number of words in the vocabulary 166 | 167 | ClusterSize: 0.45 168 | 169 | #--------------------------------------------------------------------------- 170 | 171 | ChowLiuOptions: 172 | 173 | # a large amount of data is required to store all mutual information from 174 | # which the minimum spanning tree is created. e.g. an 8000 word codebook 175 | # requires 1.2 Gb RAM. Increasing the threshold results in information being 176 | # discarded, and a lower memory requirement. Too high a threshold may result 177 | # in necessary information being discarded and the tree not being created. 178 | #A threshold of 0 takes longer and may fail due to memory requirements 179 | 180 | LowerInfoBound: 0.0005 181 | 182 | #--------------------------------------------------------------------------- 183 | 184 | # Method to add new location to the FabMap location list 185 | # "All" 186 | # "NewMaximumOnly" 187 | 188 | FabMapPlaceAddition: "All" 189 | 190 | openFabMapOptions: 191 | 192 | #Detector Model 193 | 194 | PzGe: 0.39 195 | PzGne: 0 196 | 197 | #The method to calculate the likelihood of a 'new location' 198 | #Note, FAB-MAP2.0 cannot use mean-field 199 | # "Sampled" 200 | # "Meanfield" 201 | 202 | NewPlaceMethod: "Sampled" 203 | 204 | #if using "sampled" method how many samples from training data to use 205 | 206 | NumSamples: 3000 207 | 208 | # The option to switch off FabMap's feature dependency model (Chow-Liu tree) 209 | # in the likelihood calculations 210 | # "Naive" 211 | # "ChowLiu" 212 | 213 | BayesMethod: "ChowLiu" 214 | 215 | # The option to switch on and off the addition of a simple motion model 216 | # which assumes links between sequential additions to the query space. 217 | # 0 for False, 1 for True 218 | 219 | SimpleMotion: 0 220 | 221 | # Which version of openFABMAP to run 222 | # "FABMAP1" 223 | # "FABMAPLUT" 224 | # "FABMAPFBO" 225 | # "FABMAP2" 226 | 227 | FabMapVersion: "FABMAP2" 228 | 229 | #FabMap1: 230 | 231 | # no additional options 232 | 233 | FabMapLUT: 234 | 235 | # precision with which to store precomputed values (decimal places) 236 | 237 | Precision: 6 238 | 239 | FabMapFBO: 240 | # The loglikelihood bound beneath the best hypothesis at which other 241 | # hypotheses are always retained 242 | 243 | RejectionThreshold: 1e-6 244 | 245 | # The likelihood bound of a hypothesis 'overtaking' the current best 246 | # hypothesis, below which hypotheses are discarded. Used to compute delta 247 | 248 | PsGd: 1e-6 249 | 250 | # The largest value of delta when computing it via the bisection method 251 | 252 | BisectionStart: 512 253 | 254 | # The number of iterations to perform the bisection. Bisection accuracy = 255 | # BisectionStart / 2^BisectionIts 256 | 257 | BisectionIts: 9 258 | 259 | 260 | #FabMap2: 261 | 262 | # no additional options 263 | 264 | #--------------------------------------------------------------------------- 265 | -------------------------------------------------------------------------------- /src/bowmsctrainer.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include "bowmsctrainer.hpp" 55 | 56 | #include 57 | #include 58 | #include 59 | 60 | namespace of2 { 61 | 62 | BOWMSCTrainer::BOWMSCTrainer(double _clusterSize) : 63 | clusterSize(_clusterSize) { 64 | } 65 | 66 | BOWMSCTrainer::~BOWMSCTrainer() { 67 | } 68 | 69 | cv::Mat BOWMSCTrainer::cluster() const { 70 | CV_Assert(!descriptors.empty()); 71 | int descCount = 0; 72 | for(size_t i = 0; i < descriptors.size(); i++) 73 | descCount += descriptors[i].rows; 74 | 75 | cv::Mat mergedDescriptors(descCount, descriptors[0].cols, 76 | descriptors[0].type()); 77 | for(size_t i = 0, start = 0; i < descriptors.size(); i++) 78 | { 79 | cv::Mat submut = mergedDescriptors.rowRange((int)start, 80 | (int)(start + descriptors[i].rows)); 81 | descriptors[i].copyTo(submut); 82 | start += descriptors[i].rows; 83 | } 84 | return cluster(mergedDescriptors); 85 | } 86 | 87 | cv::Mat BOWMSCTrainer::cluster(const cv::Mat& descriptors) const { 88 | 89 | CV_Assert(!descriptors.empty()); 90 | 91 | // TODO: sort the descriptors before clustering. 92 | 93 | // Start timing 94 | int64 start_time = cv::getTickCount(); 95 | 96 | // Used for Mahalanobis distance calculation, identity covariance 97 | cv::Mat icovar = cv::Mat::eye(descriptors.cols,descriptors.cols,descriptors.type()); 98 | 99 | // Create initial centres guaranteeing a centre distance < minDist // 100 | 101 | // Loop through all the descriptors 102 | std::vector initialCentres; 103 | initialCentres.push_back(descriptors.row(0)); 104 | 105 | for (int i = 1; i < descriptors.rows; i++) 106 | { 107 | double minDist = DBL_MAX; 108 | #pragma omp parallel for if (initialCentres.size() > 100) 109 | for (int j = 0; j < (int)initialCentres.size(); j++) 110 | { 111 | // Our covariance is identity, just use the norm, it's faster. 112 | // cv::Mahalanobis(descriptors.row(i),initialCentres[j], icovar); 113 | double myDist = cv::norm(descriptors.row(i),initialCentres[j]); 114 | #pragma omp critical 115 | minDist = std::min(minDist, myDist); 116 | } 117 | // Add new cluster if outside of range 118 | if (minDist > clusterSize) 119 | initialCentres.push_back(descriptors.row(i)); 120 | 121 | // Status 122 | if ((i-1)%(descriptors.rows/10) == 0) 123 | std::cout << "." << std::flush; 124 | } 125 | // Status 126 | std::cout << "\nFinished initial clustering for " 127 | << descriptors.rows << " descriptors. " 128 | << initialCentres.size() << " initial clusters. " 129 | << std::endl; 130 | 131 | // Assign each descriptor to its closest centre // 132 | 133 | // Loop through all the descriptors again 134 | // TODO: Consider a kd-tree for this search 135 | std::vector > clusters; 136 | clusters.resize(initialCentres.size()); 137 | #pragma omp parallel for schedule(dynamic, 200) 138 | for (int i = 0; i < descriptors.rows; i++) { 139 | size_t index; double dist, minDist = DBL_MAX; 140 | for (size_t j = 0; j < initialCentres.size(); j++) { 141 | dist = cv::norm(descriptors.row(i),initialCentres[j]); 142 | if (dist < minDist) { 143 | minDist = dist; 144 | index = j; 145 | } 146 | } 147 | #pragma omp critical // Order doesn't matter here 148 | clusters[index].push_back(descriptors.row(i)); 149 | 150 | // Status (could be off because of parallelism, but a guess 151 | if ((i-1)%(descriptors.rows/10) == 0) 152 | std::cout << "." << std::flush; 153 | } 154 | // Status 155 | std::cout << "\nFinished re-assignment. " 156 | << std::endl; 157 | 158 | // Calculate the centre mean for each cluster // 159 | 160 | // Loop through all the clusters 161 | cv::Mat vocabulary; 162 | #pragma omp parallel for schedule(static, 1) ordered 163 | for (int i = 0; i < (int)clusters.size(); i++) { 164 | // TODO: Throw away small clusters 165 | // TODO: Make this configurable 166 | // TODO: Re-assign? 167 | // if (clusters[i].size() < 3) continue; 168 | 169 | cv::Mat centre = cv::Mat::zeros(1,descriptors.cols,descriptors.type()); 170 | for (std::list::iterator Ci = clusters[i].begin(); Ci != clusters[i].end(); Ci++) { 171 | centre += *Ci; 172 | } 173 | centre /= (double)clusters[i].size(); 174 | #pragma omp ordered // Ordered so it's identical to non omp. 175 | vocabulary.push_back(centre); 176 | 177 | // Status (could be off because of parallelism, but a guess 178 | if ((i-1)%(clusters.size()/10) == 0) 179 | std::cout << "." << std::flush; 180 | } 181 | 182 | // Finish timing 183 | int64 end_time = cv::getTickCount(); 184 | 185 | // Status 186 | std::cout << "\nFinished finding the mean. " 187 | << vocabulary.rows << " words. " 188 | << (end_time-start_time)/cv::getTickFrequency() << " s. " 189 | << std::endl; 190 | 191 | return vocabulary; 192 | } 193 | 194 | } 195 | 196 | -------------------------------------------------------------------------------- /src/chowliutree.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other cv::Materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include "chowliutree.hpp" 55 | 56 | #include 57 | #include 58 | 59 | namespace of2 { 60 | 61 | ChowLiuTree::ChowLiuTree() { 62 | } 63 | 64 | ChowLiuTree::~ChowLiuTree() { 65 | } 66 | 67 | void ChowLiuTree::add(const cv::Mat& imgDescriptor) { 68 | CV_Assert(!imgDescriptor.empty()); 69 | if (!imgDescriptors.empty()) { 70 | CV_Assert(imgDescriptors[0].cols == imgDescriptor.cols); 71 | CV_Assert(imgDescriptors[0].type() == imgDescriptor.type()); 72 | } 73 | 74 | imgDescriptors.push_back(imgDescriptor); 75 | 76 | } 77 | 78 | void ChowLiuTree::add(const std::vector& imgDescriptors) { 79 | for (size_t i = 0; i < imgDescriptors.size(); i++) { 80 | add(imgDescriptors[i]); 81 | } 82 | } 83 | 84 | const std::vector& ChowLiuTree::getImgDescriptors() const { 85 | return imgDescriptors; 86 | } 87 | 88 | cv::Mat ChowLiuTree::make(double infoThreshold) { 89 | CV_Assert(!imgDescriptors.empty()); 90 | 91 | unsigned int descCount = 0; 92 | for (size_t i = 0; i < imgDescriptors.size(); i++) 93 | descCount += imgDescriptors[i].rows; 94 | 95 | mergedImgDescriptors = cv::Mat(descCount, imgDescriptors[0].cols, 96 | imgDescriptors[0].type()); 97 | for (size_t i = 0, start = 0; i < imgDescriptors.size(); i++) 98 | { 99 | cv::Mat submut = mergedImgDescriptors.rowRange((int)start, 100 | (int)(start + imgDescriptors[i].rows)); 101 | imgDescriptors[i].copyTo(submut); 102 | start += imgDescriptors[i].rows; 103 | } 104 | 105 | std::list edges; 106 | createBaseEdges(edges, infoThreshold); 107 | 108 | // TODO: if it cv_asserts here they really won't know why. 109 | 110 | CV_Assert(reduceEdgesToMinSpan(edges)); 111 | 112 | return buildTree(edges.front().word1, edges); 113 | } 114 | 115 | double ChowLiuTree::P(int a, bool za) { 116 | 117 | if(za) { 118 | return (0.98 * cv::countNonZero(mergedImgDescriptors.col(a)) / 119 | mergedImgDescriptors.rows) + 0.01; 120 | } else { 121 | return 1 - ((0.98 * cv::countNonZero(mergedImgDescriptors.col(a)) / 122 | mergedImgDescriptors.rows) + 0.01); 123 | } 124 | 125 | } 126 | double ChowLiuTree::JP(int a, bool za, int b, bool zb) { 127 | 128 | double count = 0; 129 | for(int i = 0; i < mergedImgDescriptors.rows; i++) { 130 | if((mergedImgDescriptors.at(i,a) > 0) == za && 131 | (mergedImgDescriptors.at(i,b) > 0) == zb) { 132 | count++; 133 | } 134 | } 135 | return count / mergedImgDescriptors.rows; 136 | 137 | } 138 | double ChowLiuTree::CP(int a, bool za, int b, bool zb){ 139 | 140 | int count = 0, total = 0; 141 | for(int i = 0; i < mergedImgDescriptors.rows; i++) { 142 | if((mergedImgDescriptors.at(i,b) > 0) == zb) { 143 | total++; 144 | if((mergedImgDescriptors.at(i,a) > 0) == za) { 145 | count++; 146 | } 147 | } 148 | } 149 | if(total) { 150 | return (double)(0.98 * count)/total + 0.01; 151 | } else { 152 | return (za) ? 0.01 : 0.99; 153 | } 154 | } 155 | 156 | cv::Mat ChowLiuTree::buildTree(int root_word, std::list &edges) { 157 | 158 | int q = root_word; 159 | cv::Mat cltree(4, (int)edges.size()+1, CV_64F); 160 | 161 | cltree.at(0, q) = q; 162 | cltree.at(1, q) = P(q, true); 163 | cltree.at(2, q) = P(q, true); 164 | cltree.at(3, q) = P(q, true); 165 | //setting P(zq|zpq) to P(zq) gives the root node of the chow-liu 166 | //independence from a parent node. 167 | 168 | //find all children and do the same 169 | std::vector nextqs = extractChildren(edges, q); 170 | 171 | int pq = q; 172 | std::vector::iterator nextq; 173 | for(nextq = nextqs.begin(); nextq != nextqs.end(); nextq++) { 174 | recAddToTree(cltree, *nextq, pq, edges); 175 | } 176 | 177 | return cltree; 178 | 179 | 180 | } 181 | 182 | void ChowLiuTree::recAddToTree(cv::Mat &cltree, int q, int pq, 183 | std::list& remaining_edges) { 184 | 185 | cltree.at(0, q) = pq; 186 | cltree.at(1, q) = P(q, true); 187 | cltree.at(2, q) = CP(q, true, pq, true); 188 | cltree.at(3, q) = CP(q, true, pq, false); 189 | 190 | //find all children and do the same 191 | std::vector nextqs = extractChildren(remaining_edges, q); 192 | 193 | pq = q; 194 | std::vector::iterator nextq; 195 | for(nextq = nextqs.begin(); nextq != nextqs.end(); nextq++) { 196 | recAddToTree(cltree, *nextq, pq, remaining_edges); 197 | } 198 | } 199 | 200 | std::vector ChowLiuTree::extractChildren(std::list &remaining_edges, int q) { 201 | 202 | std::vector children; 203 | std::list::iterator edge = remaining_edges.begin(); 204 | 205 | while(edge != remaining_edges.end()) { 206 | if(edge->word1 == q) { 207 | children.push_back(edge->word2); 208 | edge = remaining_edges.erase(edge); 209 | continue; 210 | } 211 | if(edge->word2 == q) { 212 | children.push_back(edge->word1); 213 | edge = remaining_edges.erase(edge); 214 | continue; 215 | } 216 | edge++; 217 | } 218 | 219 | return children; 220 | } 221 | 222 | bool ChowLiuTree::sortInfoScores(const info& first, const info& second) { 223 | return first.score > second.score; 224 | } 225 | 226 | double ChowLiuTree::calcMutInfo(int word1, int word2) { 227 | double accumulation = 0; 228 | 229 | double P00 = JP(word1, false, word2, false); 230 | if(P00) accumulation += P00 * log(P00 / (P(word1, false)*P(word2, false))); 231 | 232 | double P01 = JP(word1, false, word2, true); 233 | if(P01) accumulation += P01 * log(P01 / (P(word1, false)*P(word2, true))); 234 | 235 | double P10 = JP(word1, true, word2, false); 236 | if(P10) accumulation += P10 * log(P10 / (P(word1, true)*P(word2, false))); 237 | 238 | double P11 = JP(word1, true, word2, true); 239 | if(P11) accumulation += P11 * log(P11 / (P(word1, true)*P(word2, true))); 240 | 241 | return accumulation; 242 | } 243 | 244 | void ChowLiuTree::createBaseEdges(std::list& edges, double infoThreshold) { 245 | 246 | int nWords = imgDescriptors[0].cols; 247 | 248 | #pragma omp parallel for schedule(dynamic, 500) 249 | for(int word1 = 0; word1 < nWords; word1++) { 250 | std::list threadEdges; 251 | info mutInfo; 252 | for(int word2 = word1 + 1; word2 < nWords; word2++) { 253 | mutInfo.word1 = word1; 254 | mutInfo.word2 = word2; 255 | mutInfo.score = (float)calcMutInfo(word1, word2); 256 | if(mutInfo.score >= infoThreshold) 257 | threadEdges.push_back(mutInfo); 258 | } 259 | #pragma omp critical 260 | { 261 | edges.splice(edges.end(), threadEdges); 262 | } 263 | 264 | // Status 265 | if (nWords >= 10 && (word1+1)%(nWords/10) == 0) 266 | std::cout << "." << std::flush; 267 | } 268 | edges.sort(sortInfoScores); 269 | } 270 | 271 | bool ChowLiuTree::reduceEdgesToMinSpan(std::list& edges) { 272 | 273 | std::map groups; std::map::iterator groupIt; 274 | for(int i = 0; i < imgDescriptors[0].cols; i++) groups[i] = i; 275 | int group1, group2; 276 | 277 | std::list::iterator edge = edges.begin(); 278 | while(edge != edges.end()) { 279 | if(groups[edge->word1] != groups[edge->word2]) { 280 | group1 = groups[edge->word1]; 281 | group2 = groups[edge->word2]; 282 | for(groupIt = groups.begin(); groupIt != groups.end(); groupIt++) 283 | if(groupIt->second == group2) groupIt->second = group1; 284 | edge++; 285 | } else { 286 | edge = edges.erase(edge); 287 | } 288 | } 289 | 290 | if(edges.size() != (unsigned int)imgDescriptors[0].cols - 1) { 291 | return false; 292 | } else { 293 | return true; 294 | } 295 | 296 | } 297 | 298 | } 299 | 300 | -------------------------------------------------------------------------------- /src/fabmap.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other Materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include "fabmap.hpp" 55 | 56 | /* 57 | Calculate the sum of two log likelihoods 58 | */ 59 | namespace of2 { 60 | 61 | static double logsumexp(double a, double b) { 62 | return a > b ? log(1 + exp(b - a)) + a : log(1 + exp(a - b)) + b; 63 | } 64 | 65 | FabMap::FabMap(const cv::Mat & _clTree, double _PzGe, 66 | double _PzGNe, int _flags, int _numSamples) : 67 | flags(_flags), numSamples(_numSamples), 68 | clTree(new cv::Mat(_clTree)), 69 | infer(new InferBinary(clTree, _PzGe, _PzGNe, (_flags & NAIVE_BAYES) != 0)) 70 | { 71 | CV_Assert(flags & MEAN_FIELD || flags & SAMPLED); 72 | CV_Assert(flags & NAIVE_BAYES || flags & CHOW_LIU); 73 | 74 | // TODO: Add default values for member variables 75 | Pnew = 0.9; 76 | sFactor = 0.99; 77 | mBias = 0.5; 78 | } 79 | 80 | FabMap::~FabMap() { 81 | } 82 | 83 | const std::vector& FabMap::getTrainingImgDescriptors() const { 84 | return trainingImgDescriptors; 85 | } 86 | 87 | const std::vector& FabMap::getTestImgDescriptors() const { 88 | return testImgDescriptors; 89 | } 90 | 91 | void FabMap::addTraining(const cv::Mat& queryImgDescriptor) { 92 | CV_Assert(!queryImgDescriptor.empty()); 93 | std::vector queryImgDescriptors; 94 | for (int i = 0; i < queryImgDescriptor.rows; i++) { 95 | queryImgDescriptors.push_back(queryImgDescriptor.row(i)); 96 | } 97 | addTraining(queryImgDescriptors); 98 | } 99 | 100 | void FabMap::addTraining(const std::vector& queryImgDescriptors) { 101 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 102 | CV_Assert(!queryImgDescriptors[i].empty()); 103 | CV_Assert(queryImgDescriptors[i].rows == 1); 104 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 105 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 106 | trainingImgDescriptors.push_back(queryImgDescriptors[i]); 107 | } 108 | } 109 | 110 | void FabMap::add(const cv::Mat& queryImgDescriptor) { 111 | CV_Assert(!queryImgDescriptor.empty()); 112 | std::vector queryImgDescriptors; 113 | for (int i = 0; i < queryImgDescriptor.rows; i++) { 114 | queryImgDescriptors.push_back(queryImgDescriptor.row(i)); 115 | } 116 | add(queryImgDescriptors); 117 | } 118 | 119 | void FabMap::add(const std::vector& queryImgDescriptors) { 120 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 121 | CV_Assert(!queryImgDescriptors[i].empty()); 122 | CV_Assert(queryImgDescriptors[i].rows == 1); 123 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 124 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 125 | testImgDescriptors.push_back(queryImgDescriptors[i]); 126 | } 127 | } 128 | 129 | void FabMap::compare(const cv::Mat& queryImgDescriptor, 130 | const cv::Mat& testImgDescriptor, std::vector& matches, 131 | const cv::Mat& mask) { 132 | CV_Assert(!queryImgDescriptor.empty()); 133 | std::vector queryImgDescriptors; 134 | for (int i = 0; i < queryImgDescriptor.rows; i++) { 135 | queryImgDescriptors.push_back(queryImgDescriptor.row(i)); 136 | } 137 | 138 | CV_Assert(!testImgDescriptor.empty()); 139 | std::vector _testImgDescriptors; 140 | for (int i = 0; i < testImgDescriptor.rows; i++) { 141 | _testImgDescriptors.push_back(testImgDescriptor.row(i)); 142 | } 143 | compare(queryImgDescriptors,_testImgDescriptors,matches,mask); 144 | 145 | } 146 | 147 | void FabMap::compare(const cv::Mat& queryImgDescriptor, 148 | const std::vector& _testImgDescriptors, 149 | std::vector& matches, const cv::Mat& mask) { 150 | CV_Assert(!queryImgDescriptor.empty()); 151 | std::vector queryImgDescriptors; 152 | for (int i = 0; i < queryImgDescriptor.rows; i++) { 153 | queryImgDescriptors.push_back(queryImgDescriptor.row(i)); 154 | } 155 | compare(queryImgDescriptors,_testImgDescriptors,matches,mask); 156 | } 157 | 158 | void FabMap::compare(const std::vector& queryImgDescriptors, 159 | const std::vector& _testImgDescriptors, 160 | std::vector& matches, const cv::Mat& /*mask*/) { 161 | 162 | CV_Assert(!(flags & MOTION_MODEL)); 163 | for (size_t i = 0; i < _testImgDescriptors.size(); i++) { 164 | CV_Assert(!_testImgDescriptors[i].empty()); 165 | CV_Assert(_testImgDescriptors[i].rows == 1); 166 | CV_Assert(_testImgDescriptors[i].cols == infer->vocabSize()); 167 | CV_Assert(_testImgDescriptors[i].type() == CV_32F); 168 | } 169 | 170 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 171 | CV_Assert(!queryImgDescriptors[i].empty()); 172 | CV_Assert(queryImgDescriptors[i].rows == 1); 173 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 174 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 175 | 176 | // TODO: add mask 177 | 178 | compareImgDescriptor(queryImgDescriptors[i], 179 | (int)i, _testImgDescriptors, matches); 180 | } 181 | } 182 | 183 | // DEPRECATED, USE LOCALIZE BELOW 184 | void FabMap::compare(const cv::Mat& queryImgDescriptor, 185 | std::vector& matches, bool addQuery, 186 | const cv::Mat& mask) { 187 | return localize(queryImgDescriptor,matches,addQuery,mask); 188 | } 189 | void FabMap::localize(const cv::Mat& queryImgDescriptor, 190 | std::vector& matches, bool addQuery, 191 | const cv::Mat& mask) { 192 | CV_Assert(!queryImgDescriptor.empty()); 193 | std::vector queryImgDescriptors; 194 | for (int i = 0; i < queryImgDescriptor.rows; i++) { 195 | queryImgDescriptors.push_back(queryImgDescriptor.row(i)); 196 | } 197 | //compare(queryImgDescriptors,matches,addQuery,mask); 198 | localize(queryImgDescriptors,matches,addQuery,mask); 199 | } 200 | 201 | // DEPRECATED, USE LOCALIZE BELOW 202 | void FabMap::compare(const std::vector& queryImgDescriptors, 203 | std::vector& matches, bool addQuery, const cv::Mat& mask) { 204 | return localize(queryImgDescriptors,matches,addQuery,mask); 205 | } 206 | void FabMap::localize(const std::vector& queryImgDescriptors, 207 | std::vector& matches, bool addQuery, const cv::Mat& /*mask*/) { 208 | 209 | // TODO: add first query if empty (is this necessary) 210 | 211 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 212 | CV_Assert(!queryImgDescriptors[i].empty()); 213 | CV_Assert(queryImgDescriptors[i].rows == 1); 214 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 215 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 216 | 217 | // TODO: add mask 218 | 219 | compareImgDescriptor(queryImgDescriptors[i], 220 | (int)i, testImgDescriptors, matches); 221 | if (addQuery) 222 | add(queryImgDescriptors[i]); 223 | } 224 | } 225 | 226 | void FabMap::compareImgDescriptor(const cv::Mat& queryImgDescriptor, 227 | int queryIndex, const std::vector& _testImgDescriptors, 228 | std::vector& matches) { 229 | 230 | std::vector querymatches; 231 | querymatches.push_back(IMatch(queryIndex,-1, 232 | getNewPlaceLikelihood(queryImgDescriptor),0)); 233 | getLikelihoods(queryImgDescriptor,_testImgDescriptors,querymatches); 234 | normaliseDistribution(querymatches); 235 | for (size_t j = 1; j < querymatches.size(); j++) { 236 | querymatches[j].queryIdx = queryIndex; 237 | } 238 | matches.insert(matches.end(), querymatches.begin(), querymatches.end()); 239 | } 240 | 241 | double FabMap::getNewPlaceLikelihood(const cv::Mat& queryImgDescriptor) { 242 | if (flags & MEAN_FIELD) { 243 | double logP = 0.; 244 | #pragma omp parallel for reduction(+:logP) 245 | for (int q = 0; q < infer->vocabSize(); q++) 246 | { 247 | bool zq = queryImgDescriptor.at(0,q) > 0; 248 | bool zpq = queryImgDescriptor.at(0,infer->pq(q)) > 0; 249 | logP += log(infer->PzGL(q, zq, zpq, false/*unused*/, true)); 250 | } 251 | return logP; 252 | } 253 | 254 | if (flags & SAMPLED) { 255 | CV_Assert(!trainingImgDescriptors.empty()); 256 | CV_Assert(numSamples > 0); 257 | 258 | std::vector sampledImgDescriptors; 259 | 260 | // TODO: this method can result in the same sample being added 261 | // multiple times. Is this desired? 262 | 263 | for (int i = 0; i < numSamples; i++) { 264 | int index = rand() % trainingImgDescriptors.size(); 265 | sampledImgDescriptors.push_back(trainingImgDescriptors[index]); 266 | } 267 | 268 | std::vector matches; 269 | getLikelihoods(queryImgDescriptor,sampledImgDescriptors,matches); 270 | 271 | double averageLogLikelihood = -DBL_MAX + matches.front().likelihood + 1; 272 | for (int i = 0; i < numSamples; i++) { 273 | averageLogLikelihood = 274 | logsumexp(matches[i].likelihood, averageLogLikelihood); 275 | } 276 | 277 | return averageLogLikelihood - log((double)numSamples); 278 | } 279 | return 0; 280 | } 281 | 282 | void FabMap::normaliseDistribution(std::vector& matches) { 283 | CV_Assert(!matches.empty()); 284 | 285 | if (flags & MOTION_MODEL) { 286 | 287 | matches[0].match = matches[0].likelihood + log(Pnew); 288 | 289 | if (priormatches.size() > 2) { 290 | matches[1].match = matches[1].likelihood; 291 | matches[1].match += log( 292 | (2 * (1-mBias) * priormatches[1].match + 293 | priormatches[1].match + 294 | 2 * mBias * priormatches[2].match) / 3); 295 | for (size_t i = 2; i < priormatches.size()-1; i++) { 296 | matches[i].match = matches[i].likelihood; 297 | matches[i].match += log( 298 | (2 * (1-mBias) * priormatches[i-1].match + 299 | priormatches[i].match + 300 | 2 * mBias * priormatches[i+1].match)/3); 301 | } 302 | matches[priormatches.size()-1].match = 303 | matches[priormatches.size()-1].likelihood; 304 | matches[priormatches.size()-1].match += log( 305 | (2 * (1-mBias) * priormatches[priormatches.size()-2].match + 306 | priormatches[priormatches.size()-1].match + 307 | 2 * mBias * priormatches[priormatches.size()-1].match)/3); 308 | 309 | for(size_t i = priormatches.size(); i < matches.size(); i++) { 310 | matches[i].match = matches[i].likelihood; 311 | } 312 | } else { 313 | for(size_t i = 1; i < matches.size(); i++) { 314 | matches[i].match = matches[i].likelihood; 315 | } 316 | } 317 | 318 | double logsum = -DBL_MAX + matches.front().match + 1; 319 | 320 | //calculate the normalising constant 321 | for (size_t i = 0; i < matches.size(); i++) { 322 | logsum = logsumexp(logsum, matches[i].match); 323 | } 324 | 325 | //normalise 326 | for (size_t i = 0; i < matches.size(); i++) { 327 | matches[i].match = exp(matches[i].match - logsum); 328 | } 329 | 330 | //smooth final probabilities 331 | for (size_t i = 0; i < matches.size(); i++) { 332 | matches[i].match = sFactor*matches[i].match + 333 | (1 - sFactor)/matches.size(); 334 | } 335 | 336 | //update our location priors 337 | priormatches = matches; 338 | 339 | } else { 340 | 341 | double logsum = -DBL_MAX + matches.front().likelihood + 1; 342 | 343 | for (size_t i = 0; i < matches.size(); i++) { 344 | logsum = logsumexp(logsum, matches[i].likelihood); 345 | } 346 | for (size_t i = 0; i < matches.size(); i++) { 347 | matches[i].match = exp(matches[i].likelihood - logsum); 348 | } 349 | for (size_t i = 0; i < matches.size(); i++) { 350 | matches[i].match = sFactor*matches[i].match + 351 | (1 - sFactor)/matches.size(); 352 | } 353 | } 354 | } 355 | 356 | FabMap1::FabMap1(const cv::Mat& _clTree, double _PzGe, double _PzGNe, int _flags, 357 | int _numSamples) : FabMap(_clTree, _PzGe, _PzGNe, _flags, 358 | _numSamples) { 359 | } 360 | 361 | FabMap1::~FabMap1() { 362 | } 363 | 364 | void FabMap1::getLikelihoods(const cv::Mat& queryImgDescriptor, 365 | const std::vector& testImgDescriptors, std::vector& matches) 366 | { 367 | // Preallocate matches 368 | size_t startOfNewMatches = matches.size(); 369 | matches.resize(startOfNewMatches+testImgDescriptors.size()); 370 | 371 | #pragma omp parallel for if (testImgDescriptors.size() > 100) 372 | for (int i = 0; i < (int)testImgDescriptors.size(); i++) 373 | { 374 | bool zq, zpq, Lzq; 375 | double logP = 0; 376 | for (int q = 0; q < infer->vocabSize(); q++) 377 | { 378 | zq = queryImgDescriptor.at(0,q) > 0; 379 | zpq = queryImgDescriptor.at(0,infer->pq(q)) > 0; 380 | Lzq = testImgDescriptors[i].at(0,q) > 0; 381 | logP += log(infer->PzGL(q, zq, zpq, Lzq, false)); 382 | } 383 | matches[startOfNewMatches+(size_t)i] = IMatch(0,i,logP,0); 384 | } 385 | } 386 | 387 | FabMapLUT::FabMapLUT(const cv::Mat& _clTree, double _PzGe, double _PzGNe, 388 | int _flags, int _numSamples, int _precision) : 389 | FabMap(_clTree, _PzGe, _PzGNe, _flags, _numSamples), precision(_precision) { 390 | 391 | int nWords = infer->vocabSize(); 392 | double precFactor = (double)pow(10.0, precision); 393 | 394 | table = new int[nWords][8]; 395 | 396 | for (int q = 0; q < nWords; q++) { 397 | for (unsigned char i = 0; i < 8; i++) { 398 | 399 | bool Lzq = (bool) ((i >> 2) & 0x01); 400 | bool zq = (bool) ((i >> 1) & 0x01); 401 | bool zpq = (bool) (i & 1); 402 | 403 | table[q][i] = -(int)(log(infer->PzGL(q, zq, zpq, Lzq, false)) 404 | * precFactor); 405 | } 406 | } 407 | } 408 | 409 | FabMapLUT::~FabMapLUT() { 410 | delete[] table; 411 | } 412 | 413 | void FabMapLUT::getLikelihoods(const cv::Mat& queryImgDescriptor, 414 | const std::vector& testImageDescriptors, std::vector& matches) { 415 | 416 | double precFactor = (double)pow(10.0, -precision); 417 | 418 | for (size_t i = 0; i < testImageDescriptors.size(); i++) { 419 | unsigned long long int logP = 0; 420 | for (int q = 0; q < infer->vocabSize(); q++) { 421 | logP += table[q][(queryImgDescriptor.at(0,infer->pq(q)) > 0) + 422 | ((queryImgDescriptor.at(0, q) > 0) << 1) + 423 | ((testImageDescriptors[i].at(0,q) > 0) << 2)]; 424 | } 425 | matches.push_back(IMatch(0,(int)i,-precFactor*(double)logP,0)); 426 | } 427 | } 428 | 429 | FabMapFBO::FabMapFBO(const cv::Mat& _clTree, double _PzGe, double _PzGNe, 430 | int _flags, int _numSamples, double _rejectionThreshold, 431 | double _PsGd, int _bisectionStart, int _bisectionIts) : 432 | FabMap(_clTree, _PzGe, _PzGNe, _flags, _numSamples), PsGd(_PsGd), 433 | rejectionThreshold(_rejectionThreshold), bisectionStart(_bisectionStart), 434 | bisectionIts(_bisectionIts) { 435 | } 436 | 437 | 438 | FabMapFBO::~FabMapFBO() { 439 | } 440 | 441 | void FabMapFBO::getLikelihoods(const cv::Mat& queryImgDescriptor, 442 | const std::vector& testImageDescriptors, std::vector& matches) { 443 | 444 | std::multiset wordData; 445 | setWordStatistics(queryImgDescriptor, wordData); 446 | 447 | std::vector matchIndices; 448 | std::vector querymatches; 449 | 450 | for (size_t i = 0; i < testImageDescriptors.size(); i++) { 451 | querymatches.push_back(IMatch(0,(int)i,0,0)); 452 | matchIndices.push_back((int)i); 453 | } 454 | 455 | double currBest = -DBL_MAX; 456 | double bailedOut = DBL_MAX; 457 | 458 | for (std::multiset::iterator wordIter = wordData.begin(); 459 | wordIter != wordData.end(); wordIter++) { 460 | bool zq = queryImgDescriptor.at(0,wordIter->q) > 0; 461 | bool zpq = queryImgDescriptor.at(0,infer->pq(wordIter->q)) > 0; 462 | 463 | currBest = -DBL_MAX; 464 | 465 | for (size_t i = 0; i < matchIndices.size(); i++) { 466 | bool Lzq = 467 | testImageDescriptors[matchIndices[i]].at(0,wordIter->q) > 0; 468 | querymatches[matchIndices[i]].likelihood += 469 | log(infer->PzGL(wordIter->q,zq,zpq,Lzq,false)); 470 | currBest = 471 | std::max(querymatches[matchIndices[i]].likelihood, currBest); 472 | } 473 | 474 | if (matchIndices.size() == 1) 475 | continue; 476 | 477 | double delta = std::max(limitbisection(wordIter->V, wordIter->M), 478 | -log(rejectionThreshold)); 479 | 480 | std::vector::iterator matchIter = matchIndices.begin(); 481 | while (matchIter != matchIndices.end()) { 482 | if (currBest - querymatches[*matchIter].likelihood > delta) { 483 | querymatches[*matchIter].likelihood = bailedOut; 484 | matchIter = matchIndices.erase(matchIter); 485 | } else { 486 | matchIter++; 487 | } 488 | } 489 | } 490 | 491 | for (size_t i = 0; i < querymatches.size(); i++) { 492 | if (querymatches[i].likelihood == bailedOut) { 493 | querymatches[i].likelihood = currBest + log(rejectionThreshold); 494 | } 495 | } 496 | matches.insert(matches.end(), querymatches.begin(), querymatches.end()); 497 | 498 | } 499 | 500 | void FabMapFBO::setWordStatistics(const cv::Mat& queryImgDescriptor, 501 | std::multiset& wordData) { 502 | //words are sorted according to information = -ln(P(zq|zpq)) 503 | //in non-log format this is lowest probability first 504 | for (int q = 0; q < infer->vocabSize(); q++) { 505 | wordData.insert(WordStats(q, 506 | infer->PzqGzpq(q, queryImgDescriptor.at(0,q) > 0, 507 | queryImgDescriptor.at(0,infer->pq(q)) > 0))); 508 | } 509 | 510 | double d = 0, V = 0, M = 0; 511 | bool zq, zpq; 512 | 513 | for (std::multiset::reverse_iterator wordIter = 514 | wordData.rbegin(); 515 | wordIter != wordData.rend(); wordIter++) { 516 | 517 | zq = queryImgDescriptor.at(0,wordIter->q) > 0; 518 | zpq = queryImgDescriptor.at(0,infer->pq(wordIter->q)) > 0; 519 | 520 | d = log(infer->PzGL(wordIter->q, zq, zpq, true, false)) - 521 | log(infer->PzGL(wordIter->q, zq, zpq, false, false)); 522 | 523 | V += pow(d, 2.0) * 2 * 524 | (infer->Pzq(wordIter->q, true) - pow(infer->Pzq(wordIter->q, true), 2.0)); 525 | M = std::max(M, fabs(d)); 526 | 527 | wordIter->V = V; 528 | wordIter->M = M; 529 | } 530 | } 531 | 532 | double FabMapFBO::limitbisection(double v, double m) { 533 | double midpoint, left_val, mid_val; 534 | double left = 0, right = bisectionStart; 535 | 536 | left_val = bennettInequality(v, m, left) - PsGd; 537 | 538 | for(int i = 0; i < bisectionIts; i++) { 539 | 540 | midpoint = (left + right)*0.5; 541 | mid_val = bennettInequality(v, m, midpoint)- PsGd; 542 | 543 | if(left_val * mid_val > 0) { 544 | left = midpoint; 545 | left_val = mid_val; 546 | } else { 547 | right = midpoint; 548 | } 549 | } 550 | 551 | return (right + left) * 0.5; 552 | } 553 | 554 | double FabMapFBO::bennettInequality(double v, double m, double delta) { 555 | double DMonV = delta * m / v; 556 | double f_delta = log(DMonV + sqrt(pow(DMonV, 2.0) + 1)); 557 | return exp((v / pow(m, 2.0))*(cosh(f_delta) - 1 - DMonV * f_delta)); 558 | } 559 | 560 | bool FabMapFBO::compInfo(const WordStats& first, const WordStats& second) { 561 | return first.info < second.info; 562 | } 563 | 564 | FabMap2::FabMap2(const cv::Mat& _clTree, double _PzGe, double _PzGNe, 565 | int _flags) : 566 | FabMap(_clTree, _PzGe, _PzGNe, _flags) { 567 | CV_Assert(flags & SAMPLED); 568 | 569 | children.resize(infer->vocabSize()); 570 | 571 | for (int q = 0; q < infer->vocabSize(); q++) { 572 | d1.push_back(log(infer->PzGL(q, false, false, true, false) / 573 | infer->PzGL(q, false, false, false, false))); 574 | d2.push_back(log(infer->PzGL(q, false, true, true, false) / 575 | infer->PzGL(q, false, true, false, false)) - d1[q]); 576 | d3.push_back(log(infer->PzGL(q, true, false, true, false) / 577 | infer->PzGL(q, true, false, false, false))- d1[q]); 578 | d4.push_back(log(infer->PzGL(q, true, true, true, false) / 579 | infer->PzGL(q, true, true, false, false))- d1[q]); 580 | children[infer->pq(q)].push_back(q); 581 | } 582 | 583 | } 584 | 585 | FabMap2::~FabMap2() { 586 | } 587 | 588 | 589 | void FabMap2::addTraining(const std::vector& queryImgDescriptors) { 590 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 591 | CV_Assert(!queryImgDescriptors[i].empty()); 592 | CV_Assert(queryImgDescriptors[i].rows == 1); 593 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 594 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 595 | trainingImgDescriptors.push_back(queryImgDescriptors[i]); 596 | addToIndex(queryImgDescriptors[i], trainingDefaults, trainingInvertedMap); 597 | } 598 | } 599 | 600 | 601 | void FabMap2::add(const std::vector& queryImgDescriptors) { 602 | for (size_t i = 0; i < queryImgDescriptors.size(); i++) { 603 | CV_Assert(!queryImgDescriptors[i].empty()); 604 | CV_Assert(queryImgDescriptors[i].rows == 1); 605 | CV_Assert(queryImgDescriptors[i].cols == infer->vocabSize()); 606 | CV_Assert(queryImgDescriptors[i].type() == CV_32F); 607 | testImgDescriptors.push_back(queryImgDescriptors[i]); 608 | addToIndex(queryImgDescriptors[i], testDefaults, testInvertedMap); 609 | } 610 | } 611 | 612 | void FabMap2::getLikelihoods(const cv::Mat& queryImgDescriptor, 613 | const std::vector& testImageDescriptors, std::vector& matches) { 614 | 615 | if (&testImageDescriptors == &testImgDescriptors) { 616 | getIndexLikelihoods(queryImgDescriptor, testDefaults, testInvertedMap, 617 | matches); 618 | } else { 619 | CV_Assert(!(flags & MOTION_MODEL)); 620 | std::vector defaults; 621 | std::map > invertedMap; 622 | for (size_t i = 0; i < testImageDescriptors.size(); i++) { 623 | addToIndex(testImageDescriptors[i],defaults,invertedMap); 624 | } 625 | getIndexLikelihoods(queryImgDescriptor, defaults, invertedMap, matches); 626 | } 627 | } 628 | 629 | double FabMap2::getNewPlaceLikelihood(const cv::Mat& queryImgDescriptor) { 630 | 631 | CV_Assert(!trainingImgDescriptors.empty()); 632 | 633 | std::vector matches; 634 | getIndexLikelihoods(queryImgDescriptor, trainingDefaults, 635 | trainingInvertedMap, matches); 636 | 637 | double averageLogLikelihood = -DBL_MAX + matches.front().likelihood + 1; 638 | for (size_t i = 0; i < matches.size(); i++) { 639 | averageLogLikelihood = 640 | logsumexp(matches[i].likelihood, averageLogLikelihood); 641 | } 642 | 643 | return averageLogLikelihood - log((double)trainingDefaults.size()); 644 | 645 | } 646 | 647 | void FabMap2::addToIndex(const cv::Mat& queryImgDescriptor, 648 | std::vector& defaults, 649 | std::map >& invertedMap) { 650 | defaults.push_back(0); 651 | for (int q = 0; q < infer->vocabSize(); q++) { 652 | if (queryImgDescriptor.at(0,q) > 0) { 653 | defaults.back() += d1[q]; 654 | invertedMap[q].push_back((int)defaults.size()-1); 655 | } 656 | } 657 | } 658 | 659 | void FabMap2::getIndexLikelihoods(const cv::Mat& queryImgDescriptor, 660 | std::vector& defaults, 661 | std::map >& invertedMap, 662 | std::vector& matches) { 663 | 664 | std::vector::iterator LwithI, child; 665 | 666 | std::vector likelihoods = defaults; 667 | 668 | for (int q = 0; q < infer->vocabSize(); q++) { 669 | if (queryImgDescriptor.at(0,q) > 0) { 670 | for (LwithI = invertedMap[q].begin(); 671 | LwithI != invertedMap[q].end(); LwithI++) { 672 | 673 | if (queryImgDescriptor.at(0,infer->pq(q)) > 0) { 674 | likelihoods[*LwithI] += d4[q]; 675 | } else { 676 | likelihoods[*LwithI] += d3[q]; 677 | } 678 | } 679 | for (child = children[q].begin(); child != children[q].end(); 680 | child++) { 681 | 682 | if (queryImgDescriptor.at(0,*child) == 0) { 683 | for (LwithI = invertedMap[*child].begin(); 684 | LwithI != invertedMap[*child].end(); LwithI++) { 685 | 686 | likelihoods[*LwithI] += d2[*child]; 687 | } 688 | } 689 | } 690 | } 691 | } 692 | 693 | for (size_t i = 0; i < likelihoods.size(); i++) { 694 | matches.push_back(IMatch(0,(int)i,likelihoods[i],0)); 695 | } 696 | } 697 | 698 | } // namespace of2 699 | -------------------------------------------------------------------------------- /src/inference.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other Materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include "inference.hpp" 55 | 56 | namespace of2 { 57 | 58 | int InferBase::pq(int q) { 59 | return (int)clTree.at(0,q); 60 | } 61 | 62 | double InferBase::Pzq(int q, bool zq) { 63 | return (zq) ? clTree.at(1,q) : 1 - clTree.at(1,q); 64 | } 65 | 66 | double InferBase::PzqGzpq(int q, bool zq, bool zpq) { 67 | if (zpq) { 68 | return (zq) ? clTree.at(2,q) : 1 - clTree.at(2,q); 69 | } else { 70 | return (zq) ? clTree.at(3,q) : 1 - clTree.at(3,q); 71 | } 72 | } 73 | 74 | double InferBinary::PzqGeq(bool zq, bool eq) { 75 | if (eq) { 76 | return (zq) ? PzGe : 1 - PzGe; 77 | } else { 78 | return (zq) ? PzGNe : 1 - PzGNe; 79 | } 80 | } 81 | 82 | double InferBinary::PeqGLzq(int q, bool Lzq, bool eq) { 83 | double alpha, beta; 84 | alpha = PzqGeq(Lzq, true) * Pzq(q, true); 85 | beta = PzqGeq(Lzq, false) * Pzq(q, false); 86 | 87 | if (eq) { 88 | return alpha / (alpha + beta); 89 | } else { 90 | return 1 - (alpha / (alpha + beta)); 91 | } 92 | } 93 | 94 | double InferBinary::PzqGL(int q, bool zq, bool /*zpq*/, bool Lzq, 95 | const bool & newPlace /*= false*/) 96 | { 97 | double p = (newPlace ? Pzq(q, false) : PeqGLzq(q, Lzq, false)) * PzqGeq(zq, false) + 98 | (newPlace ? Pzq(q, true) : PeqGLzq(q, Lzq, true)) * PzqGeq(zq, true); 99 | 100 | return p; 101 | } 102 | 103 | double InferBinary::PzqGzpqL(int q, bool zq, bool zpq, bool Lzq, 104 | const bool & newPlace /*= false*/) { 105 | double p; 106 | double alpha, beta; 107 | 108 | alpha = Pzq(q, zq) * PzqGeq(!zq, false) * PzqGzpq(q, !zq, zpq); 109 | beta = Pzq(q, !zq) * PzqGeq( zq, false) * PzqGzpq(q, zq, zpq); 110 | p = (newPlace ? Pzq(q, false) : PeqGLzq(q, Lzq, false)) 111 | * beta / (alpha + beta); 112 | 113 | alpha = Pzq(q, zq) * PzqGeq(!zq, true) * PzqGzpq(q, !zq, zpq); 114 | beta = Pzq(q, !zq) * PzqGeq( zq, true) * PzqGzpq(q, zq, zpq); 115 | p += (newPlace ? Pzq(q, true) : PeqGLzq(q, Lzq, true)) 116 | * beta / (alpha + beta); 117 | 118 | return p; 119 | } 120 | 121 | } // namespace of2 122 | -------------------------------------------------------------------------------- /src/msckd.cpp: -------------------------------------------------------------------------------- 1 | /*////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this 6 | // license. If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // This file originates from the openFABMAP project: 10 | // [http://code.google.com/p/openfabmap/] -or- 11 | // [https://github.com/arrenglover/openfabmap] 12 | // 13 | // For published work which uses all or part of OpenFABMAP, please cite: 14 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=6224843] 15 | // 16 | // Original Algorithm by Mark Cummins and Paul Newman: 17 | // [http://ijr.sagepub.com/content/27/6/647.short] 18 | // [http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=5613942] 19 | // [http://ijr.sagepub.com/content/30/9/1100.abstract] 20 | // 21 | // License Agreement 22 | // 23 | // Copyright (C) 2012 Arren Glover [aj.glover@qut.edu.au] and 24 | // Will Maddern [w.maddern@qut.edu.au], all rights reserved. 25 | // 26 | // 27 | // Redistribution and use in source and binary forms, with or without 28 | // modification, are permitted provided that the following conditions are met: 29 | // 30 | // * Redistribution's of source code must retain the above copyright notice, 31 | // this list of conditions and the following disclaimer. 32 | // 33 | // * Redistribution's in binary form must reproduce the above copyright notice, 34 | // this list of conditions and the following disclaimer in the documentation 35 | // and/or other materials provided with the distribution. 36 | // 37 | // * The name of the copyright holders may not be used to endorse or promote 38 | // products derived from this software without specific prior written 39 | /// permission. 40 | // 41 | // This software is provided by the copyright holders and contributors "as is" 42 | // and any express or implied warranties, including, but not limited to, the 43 | // implied warranties of merchantability and fitness for a particular purpose 44 | // are disclaimed. In no event shall the Intel Corporation or contributors be 45 | // liable for any direct, indirect, incidental, special, exemplary, or 46 | // consequential damages (including, but not limited to, procurement of 47 | // substitute goods or services; loss of use, data, or profits; or business 48 | // interruption) however caused and on any theory of liability, whether in 49 | // contract, strict liability,or tort (including negligence or otherwise) 50 | // arising in any way out of the use of this software, even if advised of the 51 | // possibility of such damage. 52 | //////////////////////////////////////////////////////////////////////////////*/ 53 | 54 | #include "msckd.h" 55 | #include 56 | #include 57 | 58 | // Custom implementation of Modified Sequential Clustering 59 | BOWMSCTrainer::BOWMSCTrainer(double _clusterSize, int _minDescriptorsPerCluster, 60 | bool _shuffleDescriptors) 61 | : clusterSize(_clusterSize), 62 | minDescriptorsPerCluster(_minDescriptorsPerCluster), 63 | shuffleDescriptors(_shuffleDescriptors) {} 64 | 65 | BOWMSCTrainer::~BOWMSCTrainer() {} 66 | 67 | cv::Mat BOWMSCTrainer::cluster() const { 68 | CV_Assert(!descriptors.empty()); 69 | int descCount = 0; 70 | for (size_t i = 0; i < descriptors.size(); i++) 71 | descCount += descriptors[i].rows; 72 | 73 | cv::Mat mergedDescriptors(descCount, descriptors[0].cols, 74 | descriptors[0].type()); 75 | for (size_t i = 0, start = 0; i < descriptors.size(); i++) { 76 | cv::Mat submut = mergedDescriptors.rowRange( 77 | (int)start, (int)(start + descriptors[i].rows)); 78 | descriptors[i].copyTo(submut); 79 | start += descriptors[i].rows; 80 | } 81 | return cluster(mergedDescriptors); 82 | } 83 | 84 | cv::Mat BOWMSCTrainer::cluster(const cv::Mat &descriptors) const { 85 | 86 | CV_Assert(!descriptors.empty()); 87 | 88 | // shuffle descriptors 89 | cv::Mat sortedDescriptors; 90 | if (shuffleDescriptors) { 91 | std::vector sortedDescriptorVec; 92 | for (int i = 0; i < descriptors.rows; i++) { 93 | sortedDescriptorVec.push_back(descriptors.row(i)); 94 | } 95 | std::random_shuffle(sortedDescriptorVec.begin(), sortedDescriptorVec.end()); 96 | for (size_t i = 0; i < sortedDescriptorVec.size(); i++) { 97 | sortedDescriptors.push_back(sortedDescriptorVec[i]); 98 | } 99 | } else { 100 | sortedDescriptors = descriptors; 101 | } 102 | 103 | // assign initial centres 104 | cv::FlannBasedMatcher matcher; 105 | cv::Mat initialCentres; 106 | initialCentres.push_back(sortedDescriptors.row(0)); 107 | for (int i = 1; i < sortedDescriptors.rows; i++) { 108 | std::vector matches; 109 | matcher.match(sortedDescriptors.row(i), initialCentres, matches); 110 | if (matches.front().distance > clusterSize) { 111 | initialCentres.push_back(sortedDescriptors.row(i)); 112 | } 113 | } 114 | 115 | // assign descriptors to initial centres 116 | std::vector > clusters; 117 | clusters.resize(initialCentres.rows); 118 | std::vector matches; 119 | matcher.match(sortedDescriptors, initialCentres, matches); 120 | for (std::vector::iterator matchIter = matches.begin(); 121 | matchIter != matches.end(); matchIter++) { 122 | clusters[matchIter->trainIdx].push_back( 123 | sortedDescriptors.row(matchIter->queryIdx)); 124 | } 125 | 126 | // throw away small clusters 127 | std::vector > bigClusters; 128 | for (std::vector >::iterator clusterIter = 129 | clusters.begin(); 130 | clusterIter != clusters.end(); clusterIter++) { 131 | if (clusterIter->size() > (size_t)minDescriptorsPerCluster) { 132 | bigClusters.push_back(*clusterIter); 133 | } 134 | } 135 | 136 | // average per-cluster descriptors to find new centres 137 | cv::Mat vocabulary; 138 | cv::Mat centre = cv::Mat::zeros(1, descriptors.cols, descriptors.type()); 139 | for (size_t i = 0; i < bigClusters.size(); i++) { 140 | centre.setTo(0); 141 | for (std::list::iterator clusterIter = bigClusters[i].begin(); 142 | clusterIter != bigClusters[i].end(); clusterIter++) { 143 | centre += *clusterIter; 144 | } 145 | centre /= (double)bigClusters[i].size(); 146 | vocabulary.push_back(centre); 147 | } 148 | 149 | return vocabulary; 150 | } 151 | --------------------------------------------------------------------------------