├── .gitignore ├── CMakeLists.txt ├── COPYING ├── COPYING.LESSER ├── README.md ├── data └── Vocabulary.voc ├── doc └── Doxyfile.in ├── insertLicense └── src ├── CMakeLists.txt ├── applications ├── CMakeLists.txt ├── FeatureExtractor.cpp ├── GFPLoopClosingTest.cpp ├── GenerateBoW.cpp ├── GenerateNN.cpp ├── LearnVocabularyKMeans.cpp ├── NNLoopClosingTest.cpp ├── gflip_cl.cpp └── gflip_cl_onequery.cpp ├── build_tools └── FindEigen3.cmake ├── gflip ├── CMakeLists.txt ├── gflip_engine.cpp └── gflip_engine.hpp ├── gflip_mainpage.h ├── utils ├── CMakeLists.txt ├── Convolution.h ├── Convolution.hpp ├── HierarchicalKMeansClustering.h ├── HierarchicalKMeansClustering.hpp ├── HistogramDistances.h ├── HistogramDistances.hpp ├── KMeansClustering.h ├── KMeansClustering.hpp ├── PeakFinder.h ├── PoseEstimation.cpp ├── PoseEstimation.h ├── Regression.cpp ├── Regression.h ├── SimpleMinMaxPeakFinder.cpp ├── SimpleMinMaxPeakFinder.h ├── SimplePeakFinder.cpp └── SimplePeakFinder.h └── vocabulary ├── CMakeLists.txt ├── HierarchicalKMeansClustering.h ├── HierarchicalKMeansClustering.hpp ├── KMeansClustering.h ├── KMeansClustering.hpp ├── Vocabulary.cpp └── Vocabulary.h /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeCache.txt 2 | CMakeFiles 3 | Makefile 4 | cmake_install.cmake 5 | install_manifest.txt 6 | bin 7 | lib* 8 | build 9 | *.kate-swp 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This is the main CMake configuration file, you should always do "cmake ." from here and not from subdirs 2 | 3 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8 FATAL_ERROR) 4 | 5 | PROJECT(gflip CXX C) 6 | 7 | SET(CMAKE_VERBOSE_MAKEFILE 0) 8 | 9 | SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${PROJECT_SOURCE_DIR}/src/build_tools") 10 | 11 | if(NOT CMAKE_BUILD_TYPE) 12 | set (CMAKE_BUILD_TYPE Release) 13 | endif(NOT CMAKE_BUILD_TYPE) 14 | 15 | # Set to build shared objects as option 16 | OPTION(BUILD_SHARED_LIBS "Build package with shared libraries." ON) 17 | 18 | add_subdirectory(src) 19 | 20 | #-- Add the doc target to generate the API documentation 21 | option(BUILD_DOC "Build the documentation during install phase or when typed make doc" ON) 22 | if(BUILD_DOC) 23 | FIND_PACKAGE(Doxygen) 24 | if (NOT DOXYGEN_FOUND) 25 | message(WARNING "Doxygen is needed to build the documentation. Please install it correctly") 26 | else() 27 | configure_file(${PROJECT_SOURCE_DIR}/doc/Doxyfile.in ${PROJECT_BINARY_DIR}/Doxyfile @ONLY IMMEDIATE) 28 | add_custom_target (doc COMMAND ${DOXYGEN_EXECUTABLE} ${PROJECT_BINARY_DIR}/Doxyfile SOURCES ${PROJECT_BINARY_DIR}/Doxyfile) 29 | message(STATUS ${PROJECT_BINARY_DIR}) 30 | install(CODE "execute_process(COMMAND \"${CMAKE_COMMAND}\" --build ${PROJECT_BINARY_DIR} --target doc)") 31 | message(STATUS ${PROJECT_BINARY_DIR}) 32 | install(DIRECTORY ${PROJECT_BINARY_DIR}/doc/html DESTINATION share/${PROJECT_NAME}/doc) 33 | endif() 34 | endif() 35 | 36 | # Install the licenses and the README 37 | install(FILES ${PROJECT_SOURCE_DIR}/COPYING ${PROJECT_SOURCE_DIR}/COPYING.LESSER ${PROJECT_SOURCE_DIR}/README DESTINATION share/${PROJECT_NAME}) 38 | 39 | # Install the datasets 40 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/data DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) 41 | -------------------------------------------------------------------------------- /COPYING.LESSER: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | gflip 2 | ===== 3 | 4 | GFLIP - Geometric FLIRT Phrases for Large Scale Place Recognition 5 | -------------------------------------------------------------------------------- /data/Vocabulary.voc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tipaldi/gflip/d9ef02b8ed8ce0c5b4a645fd391dcdce9709c611/data/Vocabulary.voc -------------------------------------------------------------------------------- /insertLicense: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CH='/usr/local/bin/copyright-header' 4 | 5 | SW='GFLIP' 6 | DESC="Geometrical FLIRT Phrases for Large Scale Place Recognition" 7 | HOLDER="Gian Diego Tipaldi and Luciano Spinello and Wolfram Burgard" 8 | YEAR="2012-2013" 9 | 10 | $CH --copyright-software "$SW" --copyright-software-description "$DESC" --copyright-holder "$HOLDER" --copyright-year "$YEAR" $@ 11 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ---------------------------- Checking the OS ------------------------------ 2 | IF("${CMAKE_SYSTEM}" MATCHES "Linux") 3 | MESSAGE(STATUS "Compiling on Linux") 4 | SET(LINUX 1) 5 | EXEC_PROGRAM("uname" ARGS -m OUTPUT_VARIABLE myArch) 6 | IF("${myArch}" MATCHES "x86_64") 7 | MESSAGE(STATUS " 64-bit architecture detected") 8 | SET(LINUX64 1) 9 | ENDIF("${myArch}" MATCHES "x86_64") 10 | ELSEIF(APPLE) 11 | SET(LINUX 0) 12 | MESSAGE(STATUS "Compiling on MacOSX") 13 | ELSEIF(WIN32) 14 | SET(LINUX 0) 15 | IF(CYGWIN) 16 | MESSAGE(STATUS "Compiling on CygWin") 17 | SET(CYGWIN 1) 18 | ELSE(CYGWIN) 19 | MESSAGE(FATAL_ERROR "Compiling on Windows is not possible at the moment") 20 | ENDIF(CYGWIN) 21 | ENDIF("${CMAKE_SYSTEM}" MATCHES "Linux") 22 | 23 | include(ExternalProject) 24 | 25 | # Set variables for external libraries 26 | INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src) 27 | SET(PROJECT_EXTERNAL_DIR ${PROJECT_SOURCE_DIR}/external) 28 | INCLUDE_DIRECTORIES(${PROJECT_EXTERNAL_DIR}) 29 | 30 | 31 | # Library: FLIRT 32 | set(FLIRT_INSTALL_DIR ${PROJECT_EXTERNAL_DIR}/flirtlib) 33 | ExternalProject_Add(flirt 34 | PREFIX ${FLIRT_INSTALL_DIR} 35 | SVN_REPOSITORY https://svn.openslam.org/data/svn/flirtlib/trunk 36 | SVN_TRUST_CERT 1 37 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${FLIRT_INSTALL_DIR} -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DBUILD_DOC=OFF -DBUILD_GUI=OFF 38 | INSTALL_DIR ${FLIRT_INSTALL_DIR} 39 | ) 40 | INCLUDE_DIRECTORIES(${FLIRT_INSTALL_DIR}/include/flirtlib) 41 | LINK_DIRECTORIES(${FLIRT_INSTALL_DIR}/lib/flirtlib) 42 | install(DIRECTORY ${FLIRT_INSTALL_DIR}/share/flirtlib/data DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) 43 | 44 | # Library: Boost 45 | set(Boost_USE_MULTITHREADED ON) 46 | FIND_PACKAGE(Boost 1.36.0 COMPONENTS program_options serialization) 47 | #FIND_PACKAGE(Boost COMPONENTS program_options math graph) 48 | IF(Boost_FOUND) 49 | MESSAGE(STATUS "Boost found") 50 | IF($ENV{VERBOSE}) 51 | MESSAGE(STATUS " Boost_INCLUDE_DIR = ${Boost_INCLUDE_DIR}") 52 | MESSAGE(STATUS " Boost_LINK_DIRECTORIES = ${Boost_LINK_DIRECTORIES}") 53 | MESSAGE(STATUS " Boost Version = m:${Boost_MINOR_VERSION} M:${Boost_MAJOR_VERSION} v:${Boost_VERSION}") 54 | ENDIF($ENV{VERBOSE}) 55 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR}) 56 | LINK_DIRECTORIES(${Boost_LINK_DIRECTORIES}) 57 | ELSE(Boost_FOUND) 58 | MESSAGE(FATAL_ERROR " Boost not found, it is REQUIRED to build the FLIRT.\n Boost can be found on http://www.boost.org") 59 | ENDIF(Boost_FOUND) 60 | 61 | # Library: Eigen3 62 | INCLUDE(./build_tools/FindEigen3.cmake REQUIRED) 63 | IF(EIGEN3_FOUND) 64 | MESSAGE(STATUS "Eigen3 found") 65 | IF($ENV{VERBOSE}) 66 | MESSAGE(STATUS " EIGEN3_INCLUDE_DIR = ${EIGEN3_INCLUDE_DIR}") 67 | ENDIF($ENV{VERBOSE}) 68 | INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 69 | ELSE(EIGEN3_FOUND) 70 | MESSAGE(FATAL_ERROR " Eigen3 not found, but it is REQUIRED to build FLIRT.") 71 | ENDIF(EIGEN3_FOUND) 72 | 73 | # Optimization 74 | SET(LOCAL_OPTIMIZATION_FLAGS "-O3 -march=nocona -msse4.1 -funroll-loops -ftree-vectorize -DTBB -mfpmath=both") 75 | # SET(LOCAL_OPTIMIZATION_FLAGS "-O3 -march=native -msse4.2 -msse4.1 -funroll-loops -ftree-vectorize -DTBB -mfpmath=both") 76 | SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${LOCAL_OPTIMIZATION_FLAGS}") 77 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -std=gnu++0x ${LOCAL_OPTIMIZATION_FLAGS}") 78 | 79 | #Debug (it can clash with optimization) 80 | SET(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -O0 -g3 -ggdb3 -fno-inline -Wall") 81 | SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=gnu++0x -O0 -g3 -ggdb3 -fno-inline -Wall") 82 | 83 | #Profiling 84 | #ADD_DEFINITIONS(-pg) 85 | #SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -pg") 86 | #SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pg") 87 | 88 | 89 | # RPATH Handling during building and installation phases 90 | set(CMAKE_SKIP_BUILD_RPATH FALSE) 91 | set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 92 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 93 | SET(CMAKE_INSTALL_RPATH "${FLIRT_INSTALL_DIR}/lib/flirtlib:$ORIGIN/../lib/${CMAKE_PROJECT_NAME}") 94 | # 95 | # # use, i.e. don't skip the full RPATH for the build tree 96 | # SET(CMAKE_SKIP_BUILD_RPATH FALSE) 97 | # # when building, don't use the install RPATH already 98 | # # (but later on when installing) 99 | # SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) 100 | # # set(INSTALL_RPATH_USE_LINK_PATH true) 101 | # # Set the install RPATH relative to the binaries 102 | 103 | # # add the automatically determined parts of the RPATH 104 | # # which point to directories outside the build tree to the install RPATH 105 | # SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 106 | # # the RPATH to be used when installing, but only if it's not a system directory 107 | # # LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib/${CMAKE_PROJECT_NAME}" isSystemDir) 108 | # # IF("${isSystemDir}" STREQUAL "-1") 109 | # # SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${CMAKE_PROJECT_NAME}") 110 | # # ENDIF("${isSystemDir}" STREQUAL "-1") 111 | 112 | 113 | add_subdirectory(gflip) 114 | ADD_SUBDIRECTORY(vocabulary) 115 | ADD_SUBDIRECTORY(applications) 116 | 117 | message(STATUS "INSTALL_RPATH: ${INSTALL_RPATH}") 118 | message(STATUS "CMAKE_INSTALL_RPATH: ${CMAKE_INSTALL_RPATH}") 119 | 120 | 121 | -------------------------------------------------------------------------------- /src/applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_EXECUTABLE(featureExtractor FeatureExtractor.cpp) 2 | TARGET_LINK_LIBRARIES(featureExtractor feature geometry sensorstream sensors utils boost_serialization) 3 | ADD_DEPENDENCIES(featureExtractor flirt) 4 | 5 | ADD_EXECUTABLE(learnVocabularyKMeans LearnVocabularyKMeans.cpp) 6 | TARGET_LINK_LIBRARIES(learnVocabularyKMeans vocabulary feature geometry sensorstream sensors utils boost_serialization) 7 | ADD_DEPENDENCIES(learnVocabularyKMeans flirt) 8 | #SET_SOURCE_FILES_PROPERTIES(LearnVocabularyKMeans.cpp PROPERTIES COMPILE_FLAGS "-O0 -ggdb ") 9 | 10 | ADD_EXECUTABLE(generateBoW GenerateBoW.cpp) 11 | TARGET_LINK_LIBRARIES(generateBoW vocabulary feature geometry sensorstream sensors utils boost_filesystem boost_serialization) 12 | ADD_DEPENDENCIES(generateBoW flirt) 13 | 14 | ADD_EXECUTABLE(nnLoopClosingTest NNLoopClosingTest.cpp) 15 | TARGET_LINK_LIBRARIES(nnLoopClosingTest feature geometry sensorstream sensors utils boost_filesystem boost_serialization) 16 | ADD_DEPENDENCIES(nnLoopClosingTest flirt) 17 | #SET_SOURCE_FILES_PROPERTIES(NNLoopClosingTest.cpp PROPERTIES COMPILE_FLAGS "-O0 -ggdb ") 18 | 19 | ADD_EXECUTABLE(generateNN GenerateNN.cpp) 20 | TARGET_LINK_LIBRARIES(generateNN vocabulary feature geometry sensorstream sensors utils boost_filesystem gflip boost_serialization) 21 | ADD_DEPENDENCIES(generateNN flirt) 22 | #SET_SOURCE_FILES_PROPERTIES(NNLoopClosingTest.cpp PROPERTIES COMPILE_FLAGS "-O0 -ggdb ") 23 | 24 | ADD_EXECUTABLE(GFPLoopClosingTest GFPLoopClosingTest.cpp) 25 | TARGET_LINK_LIBRARIES(GFPLoopClosingTest vocabulary feature geometry sensorstream sensors utils boost_filesystem gflip boost_serialization) 26 | ADD_DEPENDENCIES(GFPLoopClosingTest flirt) 27 | #SET_SOURCE_FILES_PROPERTIES(NNLoopClosingTest.cpp PROPERTIES COMPILE_FLAGS "-O0 -ggdb ") 28 | 29 | ADD_EXECUTABLE(gflip_cl gflip_cl.cpp) 30 | TARGET_LINK_LIBRARIES(gflip_cl gflip) 31 | 32 | ADD_EXECUTABLE(gflip_cl_onequery gflip_cl_onequery.cpp) 33 | TARGET_LINK_LIBRARIES(gflip_cl_onequery gflip) 34 | 35 | install(TARGETS featureExtractor learnVocabularyKMeans generateBoW nnLoopClosingTest generateNN GFPLoopClosingTest gflip_cl gflip_cl_onequery 36 | RUNTIME DESTINATION bin 37 | LIBRARY DESTINATION lib/flirtlib 38 | ARCHIVE DESTINATION lib/flirtlib) 39 | -------------------------------------------------------------------------------- /src/applications/FeatureExtractor.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | 53 | LogSensorStream m_sensorReference(NULL,NULL); 54 | 55 | CurvatureDetector *m_detectorCurvature = NULL; 56 | NormalBlobDetector *m_detectorNormalBlob = NULL; 57 | NormalEdgeDetector *m_detectorNormalEdge = NULL; 58 | RangeDetector *m_detectorRange = NULL; 59 | Detector* m_detector = NULL; 60 | 61 | BetaGridGenerator *m_betaGenerator = NULL; 62 | ShapeContextGenerator *m_shapeGenerator = NULL; 63 | DescriptorGenerator *m_descriptor = NULL; 64 | 65 | std::vector< std::vector > m_pointsReference; 66 | std::vector< OrientedPoint2D > m_posesReference; 67 | 68 | struct timeval detectTime, describeTime; 69 | 70 | void detectLog(){ 71 | unsigned int i = 0; 72 | unsigned int position = m_sensorReference.tell(); 73 | m_sensorReference.seek(0,END); 74 | unsigned int last = m_sensorReference.tell(); 75 | m_sensorReference.seek(0); 76 | 77 | std::string bar(50, ' '); 78 | 79 | struct timeval start, end; 80 | gettimeofday(&start, NULL); 81 | while(!m_sensorReference.end()){ 82 | bar[(m_sensorReference.tell()*50)/last] = '#'; 83 | std::cout << "\rDetecting points [" << bar << "] " << (m_sensorReference.tell()*100)/last << "%" << std::flush; 84 | const LaserReading* lreadReference = dynamic_cast(m_sensorReference.next()); 85 | if (lreadReference){ 86 | m_detector->detect(*lreadReference, m_pointsReference[i]); 87 | m_posesReference[i] = lreadReference->getLaserPose(); 88 | i++; 89 | } 90 | } 91 | gettimeofday(&end,NULL); 92 | timersub(&end,&start,&detectTime); 93 | m_sensorReference.seek(position); 94 | std::cout << "done. Time elapsed " << double(detectTime.tv_sec) + 1e-06 * double(detectTime.tv_usec) << std::endl; 95 | } 96 | 97 | void countLog(){ 98 | double flirtNum = 0.; 99 | uint count = 0; 100 | 101 | std::string bar(50, ' '); 102 | 103 | for(unsigned int i = 0; i < m_pointsReference.size(); i++){ 104 | bar[(i*50)/m_pointsReference.size()] = '#'; 105 | std::cout << "\rCounting points [" << bar << "] " << (i*100)/m_pointsReference.size() << "%" << std::flush; 106 | if(m_pointsReference[i].size()){ 107 | flirtNum += m_pointsReference[i].size(); 108 | count++; 109 | } 110 | } 111 | flirtNum=count?flirtNum/double(count):0.; 112 | std::cout << "done: Total: " << flirtNum*double(count) << ", Avg: " << flirtNum << std::endl; 113 | } 114 | 115 | void describeLog(){ 116 | unsigned int i = 0; 117 | unsigned int position = m_sensorReference.tell(); 118 | m_sensorReference.seek(0,END); 119 | unsigned int last = m_sensorReference.tell(); 120 | m_sensorReference.seek(0); 121 | 122 | std::string bar(50, ' '); 123 | 124 | struct timeval start, end; 125 | gettimeofday(&start, NULL); 126 | while(!m_sensorReference.end()){ 127 | bar[(m_sensorReference.tell()*50)/last] = '#'; 128 | std::cout << "\rDescribing points [" << bar << "] " << (m_sensorReference.tell()*100)/last << "%" << std::flush; 129 | const LaserReading* lreadReference = dynamic_cast(m_sensorReference.next()); 130 | if (lreadReference){ 131 | for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){ 132 | m_pointsReference[i][j]->setDescriptor(m_descriptor->describe(*m_pointsReference[i][j], *lreadReference)); 133 | } 134 | i++; 135 | } 136 | } 137 | gettimeofday(&end,NULL); 138 | timersub(&end,&start,&describeTime); 139 | m_sensorReference.seek(position); 140 | std::cout << "done. Time elapsed " << double(describeTime.tv_sec) + 1e-06 * double(describeTime.tv_usec) << std::endl; 141 | } 142 | 143 | void writeGspan(std::ostream& out, const std::vector >& points){ 144 | for(unsigned int i = 0; i < points.size(); i++) { 145 | out << "t # " << i << std::endl; 146 | for(unsigned int v = 0; v < points[i].size(); v++) { 147 | out << "v " << v << " a " << points[i][v]->getPosition().x << " " << points[i][v]->getPosition().y << " 0.0" << std::endl; 148 | } 149 | if(points[i].size() < 2) continue; 150 | std::vector usedVertexes(points[i].size(), false); 151 | usedVertexes[0] = true; 152 | unsigned int vertexCount = 1; 153 | while(vertexCount != points[i].size()){ 154 | unsigned int minIndex1 = 0; 155 | unsigned int minIndex2 = 0; 156 | double minDistance = 1e10; 157 | for(unsigned int j = 0; j < points[i].size(); j++) { 158 | if(!usedVertexes[j]) continue; 159 | for(unsigned int k = 0; k < points[i].size(); k++) { 160 | if(usedVertexes[k]) continue; 161 | Point2D delta = points[i][j]->getPosition() - points[i][k]->getPosition(); 162 | double currentDistance = delta * delta; 163 | if(currentDistance < minDistance){ 164 | minDistance = currentDistance; 165 | minIndex1 = j; 166 | minIndex2 = k; 167 | } 168 | } 169 | } 170 | out << "e " << minIndex1 << " " << minIndex2 << " sparse" << std::endl; 171 | usedVertexes[minIndex2] = true; 172 | vertexCount++; 173 | } 174 | } 175 | } 176 | 177 | int main(int argc, char **argv){ 178 | 179 | std::string filename(""); 180 | unsigned int scale = 5, dmst = 2, window = 3, detectorType = 0, descriptorType = 0, distanceType = 2; 181 | double baseSigma = 0.2, sigmaStep = 1.4, minPeak = 0.34, minPeakDistance = 0.001; 182 | bool useMaxRange = false, gspan = false; 183 | 184 | int i = 1; 185 | while(i < argc){ 186 | if(strncmp("-filename", argv[i], sizeof("-filename")) == 0 ){ 187 | filename = argv[++i]; 188 | i++; 189 | } else if(strncmp("-detector", argv[i], sizeof("-detector")) == 0 ){ 190 | detectorType = atoi(argv[++i]); 191 | i++; 192 | } else if(strncmp("-descriptor", argv[i], sizeof("-descriptor")) == 0 ){ 193 | descriptorType = atoi(argv[++i]); 194 | i++; 195 | } else if(strncmp("-distance", argv[i], sizeof("-distance")) == 0 ){ 196 | distanceType = atoi(argv[++i]); 197 | i++; 198 | } else if(strncmp("-baseSigma", argv[i], sizeof("-baseSigma")) == 0 ){ 199 | baseSigma = strtod(argv[++i], NULL); 200 | i++; 201 | } else if(strncmp("-sigmaStep", argv[i], sizeof("-sigmaStep")) == 0 ){ 202 | sigmaStep = strtod(argv[++i], NULL); 203 | i++; 204 | } else if(strncmp("-minPeak", argv[i], sizeof("-minPeak")) == 0 ){ 205 | minPeak = strtod(argv[++i], NULL); 206 | i++; 207 | } else if(strncmp("-minPeakDistance", argv[i], sizeof("-minPeakDistance")) == 0 ){ 208 | minPeakDistance = strtod(argv[++i], NULL); 209 | i++; 210 | } else if(strncmp("-gspan", argv[i], sizeof("-gspan")) == 0 ){ 211 | gspan = true; 212 | i++; 213 | } else { 214 | i++; 215 | } 216 | } 217 | 218 | 219 | CarmenLogWriter writer; 220 | CarmenLogReader reader; 221 | 222 | m_sensorReference = LogSensorStream(&reader, &writer); 223 | 224 | m_sensorReference.load(filename); 225 | 226 | SimpleMinMaxPeakFinder *m_peakMinMax = new SimpleMinMaxPeakFinder(minPeak, minPeakDistance); 227 | 228 | 229 | std::string detector(""); 230 | switch(detectorType){ 231 | case 0: 232 | m_detectorCurvature = new CurvatureDetector(m_peakMinMax, scale, baseSigma, sigmaStep, dmst); 233 | m_detectorCurvature->setUseMaxRange(useMaxRange); 234 | m_detector = m_detectorCurvature; 235 | detector = "curvature"; 236 | break; 237 | case 1: 238 | m_detectorNormalEdge = new NormalEdgeDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window); 239 | m_detector = m_detectorNormalEdge; 240 | detector = "edge"; 241 | break; 242 | case 2: 243 | m_detectorNormalBlob = new NormalBlobDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window); 244 | m_detector = m_detectorNormalBlob; 245 | detector = "blob"; 246 | break; 247 | case 3: 248 | m_detectorRange = new RangeDetector(m_peakMinMax, scale, baseSigma, sigmaStep); 249 | m_detector = m_detectorRange; 250 | detector = "range"; 251 | break; 252 | default: 253 | std::cerr << "Wrong detector type" << std::endl; 254 | exit(-1); 255 | } 256 | 257 | HistogramDistance *dist = NULL; 258 | 259 | std::string distance(""); 260 | switch(distanceType){ 261 | case 0: 262 | dist = new EuclideanDistance(); 263 | distance = "euclid"; 264 | break; 265 | case 1: 266 | dist = new Chi2Distance(); 267 | distance = "chi2"; 268 | break; 269 | case 2: 270 | dist = new SymmetricChi2Distance(); 271 | distance = "symchi2"; 272 | break; 273 | case 3: 274 | dist = new BatthacharyyaDistance(); 275 | distance = "batt"; 276 | break; 277 | case 4: 278 | dist = new KullbackLeiblerDistance(); 279 | distance = "kld"; 280 | break; 281 | case 5: 282 | dist = new JensenShannonDistance(); 283 | distance = "jsd"; 284 | break; 285 | default: 286 | std::cerr << "Wrong distance type" << std::endl; 287 | exit(-1); 288 | } 289 | 290 | std::string descriptor(""); 291 | switch(descriptorType){ 292 | case 0: 293 | m_betaGenerator = new BetaGridGenerator(0.02, 0.5, 4, 12); 294 | m_betaGenerator->setDistanceFunction(dist); 295 | m_descriptor = m_betaGenerator; 296 | descriptor = "beta"; 297 | break; 298 | case 1: 299 | m_shapeGenerator = new ShapeContextGenerator(0.02, 0.5, 4, 12); 300 | m_shapeGenerator->setDistanceFunction(dist); 301 | m_descriptor = m_shapeGenerator; 302 | descriptor = "shape"; 303 | break; 304 | default: 305 | std::cerr << "Wrong descriptor type" << std::endl; 306 | exit(-1); 307 | } 308 | 309 | std::cerr << "Processing file:\t" << filename << "\nDetector:\t\t" << detector << "\nDescriptor:\t\t" << descriptor << "\nDistance:\t\t" << distance << std::endl; 310 | 311 | m_sensorReference.seek(0,END); 312 | unsigned int end = m_sensorReference.tell(); 313 | m_sensorReference.seek(0,BEGIN); 314 | 315 | m_pointsReference.resize(end + 1); 316 | m_posesReference.resize(end + 1); 317 | 318 | detectLog(); 319 | 320 | countLog(); 321 | // return 1; 322 | 323 | describeLog(); 324 | 325 | std::string outfile = filename.substr(0, filename.find_last_of(".")); 326 | outfile.append(".flt"); 327 | 328 | if(gspan){ 329 | std::string gspanfile = filename.substr(0, filename.find_last_of(".")); 330 | gspanfile.append(".gspan"); 331 | std::ofstream gspanStream(gspanfile.c_str()); 332 | writeGspan(gspanStream, m_pointsReference); 333 | } 334 | 335 | std::ofstream outputStream(outfile.c_str()); 336 | // boost::archive::xml_oarchive archive(outputStream); 337 | boost::archive::binary_oarchive archive(outputStream); 338 | 339 | 340 | std::cout << "Saving the points reference " << BOOST_PP_STRINGIZE(m_pointsReference) << std::endl; 341 | archive << BOOST_SERIALIZATION_NVP(m_pointsReference); 342 | std::cout << "Saving the points generators " << BOOST_PP_STRINGIZE(m_betaGenerator->getPhiEdges()) << std::endl; 343 | if(m_betaGenerator) { 344 | 345 | archive << boost::serialization::make_nvp("phiEdges", m_betaGenerator->getPhiEdges()); 346 | archive << boost::serialization::make_nvp("rhoEdges", m_betaGenerator->getRhoEdges()); 347 | } else if(m_shapeGenerator) { 348 | archive << boost::serialization::make_nvp("phiEdges", m_shapeGenerator->getPhiEdges()); 349 | archive << boost::serialization::make_nvp("rhoEdges", m_shapeGenerator->getRhoEdges()); 350 | } 351 | std::cout << "done" << std::endl; 352 | 353 | outputStream.close(); 354 | 355 | // Uncomment for Serialization test 356 | // const InterestPoint* testPoint = m_pointsReference.back().back(); 357 | // unsigned int numPoints = 0; 358 | // for(unsigned int j = 0; j < m_pointsReference.size(); numPoints += m_pointsReference[j++].size()){ 359 | // std::cerr << numPoints << " " << m_pointsReference[j].size() << std::endl; 360 | // 361 | // } 362 | // 363 | // std::cout << "Filename: " << outfile 364 | // << ", Scans: " << m_pointsReference.size() 365 | // << ", Points: " << numPoints 366 | // << ", Avg: " << double(numPoints)/double(m_pointsReference.size()) << std::endl << std::endl; 367 | // std::cout << "Test Point: end, end" << std::endl 368 | // << testPoint->getPosition() << std::endl 369 | // << testPoint->getScale() << std::endl 370 | // << testPoint->getScaleLevel() << std::endl 371 | // << typeid(*testPoint->getDescriptor()).name() << std::endl 372 | // << testPoint->getSupport().size() << std::endl; 373 | 374 | 375 | } 376 | 377 | -------------------------------------------------------------------------------- /src/applications/GenerateBoW.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | 55 | 56 | LogSensorStream m_sensorReference(NULL,NULL); 57 | 58 | CurvatureDetector *m_detectorCurvature = NULL; 59 | NormalBlobDetector *m_detectorNormalBlob = NULL; 60 | NormalEdgeDetector *m_detectorNormalEdge = NULL; 61 | RangeDetector *m_detectorRange = NULL; 62 | Detector* m_detector = NULL; 63 | 64 | BetaGridGenerator *m_betaGenerator = NULL; 65 | ShapeContextGenerator *m_shapeGenerator = NULL; 66 | DescriptorGenerator *m_descriptor = NULL; 67 | 68 | std::vector< std::vector > m_pointsReference; 69 | std::vector< OrientedPoint2D > m_posesReference; 70 | 71 | struct timeval detectTime, describeTime, vocabularyTime; 72 | 73 | HistogramVocabulary histogramVocabulary; 74 | 75 | void help(){ 76 | std::cerr << "FLIRTLib version 0.9b - authors Gian Diego Tipaldi and Kai O. Arras" << std::endl 77 | << "Usage: generateBoW -filename [options] " << std::endl 78 | << "Options:" << std::endl 79 | << " -filename \t The logfile in CARMEN format to process (mandatory)." << std::endl 80 | << " -vocabulary \t The vocabulary file to use (default=Vocabolary.voc)." << std::endl 81 | << " -scale \t The number of scales to consider (default=5)." << std::endl 82 | << " -dmst \t The number of spanning tree for the curvature detector (deafult=2)." << std::endl 83 | << " -window \t The size of the local window for estimating the normal signal (default=3)." << std::endl 84 | << " -detector \t The type of detector to use. Available options are (default=0):" << std::endl 85 | << " \t 0 - Curvature detector;" << std::endl 86 | << " \t 1 - Normal edge detector;" << std::endl 87 | << " \t 2 - Normal blob detector;" << std::endl 88 | << " \t 3 - Range detector." << std::endl 89 | << " -descriptor \t The type of descriptor to use. Available options are (default=0):" << std::endl 90 | << " \t 0 - Beta - grid;" << std::endl 91 | << " \t 1 - Shape context." << std::endl 92 | << " -distance \t The distance function to compare the descriptors. Available options are (default=2):" << std::endl 93 | << " \t 0 - Euclidean distance;" << std::endl 94 | << " \t 1 - Chi square distance;" << std::endl 95 | << " \t 2 - Symmetric Chi square distance;" << std::endl 96 | << " \t 3 - Bhattacharyya distance;" << std::endl 97 | << " \t 4 - Kullback Leibler divergence;" << std::endl 98 | << " \t 5 - Jensen Shannon divergence." << std::endl 99 | << " -baseSigma \t The initial standard deviation for the smoothing operator (default=0.2)." << std::endl 100 | << " -sigmaStep \t The incremental step for the scales of the smoothing operator. (default=1.4)." << std::endl 101 | << " -minPeak \t The minimum value for a peak to be detected (default=0.34)." << std::endl 102 | << " -minPeakDistance \t The minimum difference with the neighbors for a peak to be detected (default=0.001)." << std::endl 103 | << std::endl 104 | << "The program generates a bag of words description for each scan present in the logfile." << std::endl 105 | << "The program writes the description in a file named .bow " << std::endl 106 | << "Each line of the file corresponds to one laser reading of the logfile." << std::endl 107 | << "The data is written in a space seprated value, following the format:" << std::endl 108 | << " <#Words> ... " << std::endl 109 | << "where #Words are the number of feature in the reading and X,Y the local position of the feature." << std::endl 110 | << std::endl; 111 | } 112 | 113 | 114 | bool pairComparator ( const std::pair& l, const std::pair& r) 115 | { return l.first > r.first; } 116 | 117 | 118 | void writePoses(){ 119 | unsigned int i = 0; 120 | unsigned int position = m_sensorReference.tell(); 121 | m_sensorReference.seek(0,END); 122 | unsigned int last = m_sensorReference.tell(); 123 | m_sensorReference.seek(0); 124 | 125 | std::string bar(50, ' '); 126 | bar[0] = '#'; 127 | unsigned int progress = 0; 128 | 129 | while(!m_sensorReference.end()){ 130 | unsigned int currentProgress = (m_sensorReference.tell()*100)/last; 131 | if (progress < currentProgress){ 132 | progress = currentProgress; 133 | bar[progress/2] = '#'; 134 | std::cout << "\rWriting poses [" << bar << "] " << (m_sensorReference.tell()*100)/last << "%" << std::flush; 135 | } 136 | const LaserReading* lreadReference = dynamic_cast(m_sensorReference.next()); 137 | if (lreadReference){ 138 | m_posesReference[i] = lreadReference->getLaserPose(); 139 | i++; 140 | } 141 | } 142 | m_sensorReference.seek(position); 143 | std::cout << " done." << std::endl; 144 | } 145 | 146 | void detectLog(){ 147 | unsigned int i = 0; 148 | unsigned int position = m_sensorReference.tell(); 149 | m_sensorReference.seek(0,END); 150 | unsigned int last = m_sensorReference.tell(); 151 | m_sensorReference.seek(0); 152 | 153 | std::string bar(50, ' '); 154 | bar[0] = '#'; 155 | unsigned int progress = 0; 156 | 157 | struct timeval start, end; 158 | gettimeofday(&start, NULL); 159 | while(!m_sensorReference.end()){ 160 | unsigned int currentProgress = (m_sensorReference.tell()*100)/last; 161 | if (progress < currentProgress){ 162 | progress = currentProgress; 163 | bar[progress/2] = '#'; 164 | std::cout << "\rDetecting points [" << bar << "] " << (m_sensorReference.tell()*100)/last << "%" << std::flush; 165 | } 166 | const LaserReading* lreadReference = dynamic_cast(m_sensorReference.next()); 167 | if (lreadReference){ 168 | m_detector->detect(*lreadReference, m_pointsReference[i]); 169 | m_posesReference[i] = lreadReference->getLaserPose(); 170 | i++; 171 | } 172 | } 173 | gettimeofday(&end,NULL); 174 | timersub(&end,&start,&detectTime); 175 | m_sensorReference.seek(position); 176 | std::cout << " done." << std::endl; 177 | } 178 | 179 | void countLog(){ 180 | double flirtNum = 0.; 181 | uint count = 0; 182 | 183 | std::string bar(50, ' '); 184 | bar[0] = '#'; 185 | 186 | unsigned int progress = 0; 187 | for(unsigned int i = 0; i < m_pointsReference.size(); i++){ 188 | unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1); 189 | if (progress < currentProgress){ 190 | progress = currentProgress; 191 | bar[progress/2] = '#'; 192 | std::cout << "\rCounting points [" << bar << "] " << progress << "%" << std::flush; 193 | } 194 | if(m_pointsReference[i].size()){ 195 | flirtNum += m_pointsReference[i].size(); 196 | count++; 197 | } 198 | } 199 | flirtNum=count?flirtNum/double(count):0.; 200 | std::cout << " done.\nFound " << flirtNum << " FLIRT features per scan." << std::endl; 201 | } 202 | 203 | void describeLog(){ 204 | unsigned int i = 0; 205 | unsigned int position = m_sensorReference.tell(); 206 | m_sensorReference.seek(0,END); 207 | unsigned int last = m_sensorReference.tell(); 208 | m_sensorReference.seek(0); 209 | 210 | std::string bar(50, ' '); 211 | bar[0] = '#'; 212 | struct timeval start, end; 213 | gettimeofday(&start, NULL); 214 | unsigned int progress = 0; 215 | 216 | while(!m_sensorReference.end()){ 217 | unsigned int currentProgress = (m_sensorReference.tell()*100)/last; 218 | if (progress < currentProgress){ 219 | progress = currentProgress; 220 | bar[progress/2] = '#'; 221 | std::cout << "\rDescribing points [" << bar << "] " << progress << "%" << std::flush; 222 | } 223 | const LaserReading* lreadReference = dynamic_cast(m_sensorReference.next()); 224 | if (lreadReference){ 225 | for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){ 226 | m_pointsReference[i][j]->setDescriptor(m_descriptor->describe(*m_pointsReference[i][j], *lreadReference)); 227 | } 228 | i++; 229 | } 230 | } 231 | gettimeofday(&end,NULL); 232 | timersub(&end,&start,&describeTime); 233 | m_sensorReference.seek(position); 234 | std::cout << " done." << std::endl; 235 | } 236 | 237 | struct WordResult { 238 | unsigned int word; 239 | OrientedPoint2D pose; 240 | }; 241 | 242 | void writeBoW(std::ofstream& out){ 243 | std::string bar(50, ' '); 244 | bar[0] = '#'; 245 | 246 | unsigned int progress = 0; 247 | struct timeval start, end; 248 | gettimeofday(&start, NULL); 249 | for(unsigned int i = 0; i < m_pointsReference.size(); i++){ 250 | unsigned int currentProgress = (i*100)/(m_pointsReference.size() - 1); 251 | if (progress < currentProgress){ 252 | progress = currentProgress; 253 | bar[progress/2] = '#'; 254 | std::cout << "\rDescribing scans [" << bar << "] " << progress << "%" << std::flush; 255 | } 256 | std::multimap signature; 257 | for(unsigned int j = 0; j < m_pointsReference[i].size(); j++){ 258 | InterestPoint * point = m_pointsReference[i][j]; 259 | OrientedPoint2D localpose = m_posesReference[i].ominus(point->getPosition()); 260 | double angle = atan2(localpose.y, localpose.x); 261 | unsigned int bestWord = 0; 262 | double bestMatch = 0.; 263 | std::vector descriptor; 264 | std::vector weights; 265 | point->getDescriptor()->getWeightedFlatDescription(descriptor, weights); 266 | HistogramFeatureWord word(descriptor, NULL, weights); 267 | for(unsigned int w = 0; w < histogramVocabulary.size(); w++) { 268 | double score = histogramVocabulary[w].sim(&word); 269 | if(score > bestMatch) { 270 | bestMatch = score; 271 | bestWord = w; 272 | } 273 | } 274 | WordResult best; best.pose = localpose; best.word = bestWord; 275 | signature.insert(std::make_pair(angle,best)); 276 | } 277 | out << m_pointsReference[i].size(); 278 | for(std::multimap::const_iterator it = signature.begin(); it != signature.end(); it++){ 279 | out << " " << it->second.word << " " << it->second.pose.x << " " << it->second.pose.y; 280 | } 281 | out <setUseMaxRange(useMaxRange); 365 | m_detector = m_detectorCurvature; 366 | detector = "curvature"; 367 | break; 368 | case 1: 369 | m_detectorNormalEdge = new NormalEdgeDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window); 370 | m_detectorNormalEdge->setUseMaxRange(useMaxRange); 371 | m_detector = m_detectorNormalEdge; 372 | detector = "edge"; 373 | break; 374 | case 2: 375 | m_detectorNormalBlob = new NormalBlobDetector(m_peakMinMax, scale, baseSigma, sigmaStep, window); 376 | m_detectorNormalBlob->setUseMaxRange(useMaxRange); 377 | m_detector = m_detectorNormalBlob; 378 | detector = "blob"; 379 | break; 380 | case 3: 381 | m_detectorRange = new RangeDetector(m_peakMinMax, scale, baseSigma, sigmaStep); 382 | m_detectorRange->setUseMaxRange(useMaxRange); 383 | m_detector = m_detectorRange; 384 | detector = "range"; 385 | break; 386 | default: 387 | std::cerr << "Wrong detector type" << std::endl; 388 | exit(-1); 389 | } 390 | 391 | HistogramDistance *dist = NULL; 392 | 393 | std::string distance(""); 394 | switch(distanceType){ 395 | case 0: 396 | dist = new EuclideanDistance(); 397 | distance = "euclid"; 398 | break; 399 | case 1: 400 | dist = new Chi2Distance(); 401 | distance = "chi2"; 402 | break; 403 | case 2: 404 | dist = new SymmetricChi2Distance(); 405 | distance = "symchi2"; 406 | break; 407 | case 3: 408 | dist = new BatthacharyyaDistance(); 409 | distance = "batt"; 410 | break; 411 | case 4: 412 | dist = new KullbackLeiblerDistance(); 413 | distance = "kld"; 414 | break; 415 | case 5: 416 | dist = new JensenShannonDistance(); 417 | distance = "jsd"; 418 | break; 419 | default: 420 | std::cerr << "Wrong distance type" << std::endl; 421 | exit(-1); 422 | } 423 | 424 | std::string descriptor(""); 425 | switch(descriptorType){ 426 | case 0: 427 | m_betaGenerator = new BetaGridGenerator(0.02, 0.5, 4, 12); 428 | m_betaGenerator->setDistanceFunction(dist); 429 | m_descriptor = m_betaGenerator; 430 | descriptor = "beta"; 431 | break; 432 | case 1: 433 | m_shapeGenerator = new ShapeContextGenerator(0.02, 0.5, 4, 12); 434 | m_shapeGenerator->setDistanceFunction(dist); 435 | m_descriptor = m_shapeGenerator; 436 | descriptor = "shape"; 437 | break; 438 | default: 439 | std::cerr << "Wrong descriptor type" << std::endl; 440 | exit(-1); 441 | } 442 | 443 | std::cerr << "Processing file:\t" << filename << "\nDetector:\t\t" << detector << "\nDescriptor:\t\t" << descriptor << "\nDistance:\t\t" << distance << "\nVocabulary:\t\t" << vocabulary << std::endl; 444 | 445 | std::ifstream vocabularyStream(vocabulary.c_str()); 446 | boost::archive::binary_iarchive vocabularyArchive(vocabularyStream); 447 | vocabularyArchive >> histogramVocabulary; 448 | 449 | m_sensorReference.seek(0,END); 450 | unsigned int end = m_sensorReference.tell(); 451 | m_sensorReference.seek(0,BEGIN); 452 | 453 | m_pointsReference.resize(end + 1); 454 | m_posesReference.resize(end + 1); 455 | 456 | writePoses(); 457 | try { 458 | std::string featureFile = filename.substr(0,filename.find_last_of('.')) + ".flt"; 459 | std::ifstream featureStream(featureFile.c_str()); 460 | boost::archive::binary_iarchive featureArchive(featureStream); 461 | std::cout << "Loading feature file " << featureFile << " ..."; 462 | featureArchive >> m_pointsReference; 463 | std::cout << " done." << std::endl; 464 | } catch(boost::archive::archive_exception& exc) { 465 | detectLog(); 466 | countLog(); 467 | describeLog(); 468 | } 469 | 470 | std::string bowFile = filename.substr(0,filename.find_last_of('.')) + ".bow"; 471 | std::ofstream bowStream(bowFile.c_str()); 472 | 473 | writeBoW(bowStream); 474 | 475 | } 476 | 477 | -------------------------------------------------------------------------------- /src/applications/LearnVocabularyKMeans.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | #include 54 | 55 | #include 56 | 57 | 58 | typedef HierarchicalKMeansClustering VocabularyClustering; 59 | typedef std::vector< std::vector< InterestPoint *> > InterestPointLog; 60 | typedef std::vector< std::vector< unsigned int> > LabelLog; 61 | 62 | struct VMeasure { 63 | double homogeneity; 64 | double completeness; 65 | }; 66 | 67 | struct LabelledPoint { 68 | typedef double value_type; 69 | double point[2]; 70 | unsigned int label; 71 | double& operator[](unsigned int i) { return point[i];} 72 | double operator[](unsigned int i) const { return point[i];} 73 | }; 74 | 75 | typedef KDTree::KDTree<2,LabelledPoint> SearchTree; 76 | typedef std::pair::const_iterator, KDTree::KDTree<2,LabelledPoint>::distance_type> SearchResult; 77 | 78 | std::ostream& operator<<(std::ostream& out, const std::vector& vec){ 79 | out << "[ "; 80 | for(std::vector::const_iterator it = vec.begin(); it != vec.end(); it++){ 81 | out << *it << " "; 82 | } 83 | out << "]" << std::endl; 84 | return out; 85 | } 86 | 87 | VMeasure testCluster(const HistogramVocabulary& clusters, const LabelLog& tree, const InterestPointLog& points, unsigned int label, double beta){ 88 | // Checking the assignment 89 | Eigen::MatrixXd assignment = Eigen::MatrixXd::Constant(clusters.size(), label, std::numeric_limits< double >::min()); 90 | for(unsigned int i = 0; i < points.size(); i++) { 91 | for(unsigned int j = 0; j < points[i].size(); j++) { 92 | LabelledPoint point, closestPoint; 93 | point[0]=points[i][j]->getPosition().x; 94 | point[1]=points[i][j]->getPosition().y; 95 | // SearchResult closest = tree.find_nearest(point); 96 | unsigned int cluster = 0, realCluster = tree[i][j]; 97 | double maxSim = 0.; 98 | std::vector description; 99 | std::vector weights; 100 | points[i][j]->getDescriptor()->getWeightedFlatDescription(description, weights); 101 | for(unsigned int w = 0; w < clusters.size(); w++) { 102 | double sim = clusters[w].sim(description, weights); 103 | if(sim > maxSim) { 104 | maxSim = sim; 105 | cluster = w; 106 | } 107 | } 108 | assignment(cluster, realCluster) = assignment(cluster, realCluster) + 1; 109 | // assignment(0, realCluster) = assignment(0, realCluster) + 1; 110 | // assignment(cluster, 0) = assignment(cluster, 0) + 1; 111 | } 112 | } 113 | 114 | // Computing the entropy based measure 115 | // std::cout << "assignment = " << assignment << std::endl; 116 | double number = assignment.sum(); 117 | // std::cout << "number = " << number << std::endl; 118 | assignment *= 1./number; 119 | Eigen::MatrixXd assignmentC = assignment.colwise().sum(); 120 | Eigen::MatrixXd assignmentK = assignment.rowwise().sum(); 121 | // std::cout << "assignment = " << assignment << std::endl; 122 | // std::cout << "assignmentK = " << assignmentK << std::endl; 123 | // std::cout << "assignmentC = " << assignmentC << std::endl; 124 | double entropyKandC = - (assignment.array() * (assignment.array().log())).sum(); 125 | double entropyK = - (assignmentK.array() * (assignmentK.array().log())).sum(); 126 | double entropyC = - (assignmentC.array() * (assignmentC.array().log())).sum(); 127 | /* std::cout << std::endl; 128 | std::cout << "entropyKC = " << entropyKandC << std::endl; 129 | std::cout << "entropyK = " << entropyK << std::endl; 130 | std::cout << "entropyC = " << entropyC << std::endl; 131 | std::cout << "entropyKC - entropyK= " << entropyKandC - entropyK << std::endl; 132 | std::cout << "entropyKC - entropyC= " << entropyKandC - entropyC << std::endl; 133 | std::cout << "entropyCgivenK = " << (entropyKandC - entropyK)/entropyC << std::endl; 134 | std::cout << "entropyKgivenC = " << (entropyKandC - entropyC)/entropyK << std::endl;*/ 135 | VMeasure result; 136 | result.homogeneity = entropyC ? 1. - (entropyKandC - entropyK)/entropyC : 1.; 137 | result.completeness = entropyK ? 1. - (entropyKandC - entropyC)/entropyK : 1.; 138 | return result; 139 | } 140 | 141 | 142 | int main(int argc, char **argv){ 143 | 144 | 145 | std::vector< std::string > filenames; 146 | std::string outfile("Vocabulary"); 147 | unsigned int maxFeatures = 1000, distanceType = 2, samplingType = 1; 148 | double beta = 1.; 149 | 150 | unsigned int vocabularyLevels = 3, vocabularySize = 5; 151 | 152 | int i = 1; 153 | while(i < argc){ 154 | if(strncmp("-maxFeatures", argv[i], sizeof("-maxFeatures")) == 0 ){ 155 | maxFeatures = atoi(argv[++i]); 156 | i++; 157 | } else if(strncmp("-outfile", argv[i], sizeof("-outfile")) == 0 ){ 158 | outfile = argv[++i]; 159 | i++; 160 | } else if(strncmp("-levels", argv[i], sizeof("-levels")) == 0 ){ 161 | vocabularyLevels = atoi(argv[++i]); 162 | i++; 163 | } else if(strncmp("-size", argv[i], sizeof("-size")) == 0 ){ 164 | vocabularySize = atoi(argv[++i]); 165 | i++; 166 | } else if(strncmp("-distanceType", argv[i], sizeof("-distanceType")) == 0 ){ 167 | distanceType = atoi(argv[++i]); 168 | i++; 169 | } else if(strncmp("-samplingType", argv[i], sizeof("-samplingType")) == 0 ){ 170 | samplingType = atoi(argv[++i]); 171 | i++; 172 | } else if(strncmp("-beta", argv[i], sizeof("-beta")) == 0 ){ 173 | beta = atof(argv[++i]); 174 | i++; 175 | } else { 176 | filenames.push_back(argv[i++]); 177 | } 178 | } 179 | 180 | 181 | HistogramDistance *dist = NULL; 182 | 183 | std::string distance(""); 184 | switch(distanceType){ 185 | case 0: 186 | dist = new EuclideanDistance(); 187 | distance = "euclid"; 188 | break; 189 | case 1: 190 | dist = new Chi2Distance(); 191 | distance = "chi2"; 192 | break; 193 | case 2: 194 | dist = new SymmetricChi2Distance(); 195 | distance = "symchi2"; 196 | break; 197 | case 3: 198 | dist = new BatthacharyyaDistance(); 199 | distance = "batt"; 200 | break; 201 | case 4: 202 | dist = new KullbackLeiblerDistance(); 203 | distance = "kld"; 204 | break; 205 | case 5: 206 | dist = new JensenShannonDistance(); 207 | distance = "jsd"; 208 | break; 209 | default: 210 | std::cerr << "Wrong distance type" << std::endl; 211 | exit(-1); 212 | } 213 | 214 | switch (samplingType){ 215 | case 0: 216 | std::cerr << "low variance sampling" << std::endl; 217 | break; 218 | case 1: 219 | std::cerr << "random uniform sampling" << std::endl; 220 | break; 221 | default: 222 | std::cerr << "Wrong sampling type" << std::endl; 223 | exit(-1); 224 | } 225 | 226 | std::cerr << "Processing files:\t"; 227 | for(std::vector::const_iterator it = filenames.begin(); it != filenames.end(); it++) { 228 | std::cerr << *it << " "; 229 | } 230 | 231 | std::cerr << "\nDistance:\t\t" << distance 232 | << "\nSize:\t\t\t" << maxFeatures 233 | << "\nLevels:\t\t\t" << vocabularyLevels 234 | << "\nSize:\t\t\t" << vocabularySize 235 | << std::endl; 236 | 237 | unsigned int fileFeatures = ceil(double(maxFeatures)/double(filenames.size())); 238 | 239 | 240 | VocabularyClustering clustering(30, 0.001, vocabularySize); 241 | boost::mt19937 rng; 242 | 243 | HistogramVocabulary currentVocabulary; 244 | currentVocabulary.reserve(maxFeatures); 245 | 246 | std::cout << "Reading files: " << std::endl; 247 | for(std::vector::const_iterator it = filenames.begin(); it != filenames.end(); it++) { 248 | std::ifstream inputStream(it->c_str()); 249 | boost::archive::binary_iarchive inputArchive(inputStream); 250 | std::vector< std::vector< InterestPoint *> > points; 251 | inputArchive >> BOOST_SERIALIZATION_NVP(points); 252 | 253 | if (samplingType == 0){ 254 | ///low variance sampling 255 | double step = double(points.size() -1) / double(fileFeatures); 256 | boost::uniform_real generator(0, step); 257 | std::cerr << "initial seed in " << step << " step= " << step << std::endl; 258 | unsigned int seed = generator(rng); 259 | for (unsigned int p = 0; p < fileFeatures; p++){ 260 | unsigned int index1 = seed + double(p)*step; 261 | while(points[index1].size() < 1){ 262 | index1 -= 1; 263 | } 264 | unsigned int max = points[index1].size() - 1; 265 | boost::uniform_int generator2(0, max); 266 | unsigned int index2 = generator2(rng); 267 | std::vector description; 268 | std::vector weights; 269 | // std::cout << "p= " << p << " i1= "<< index1 << " i2= " << index2 << " " << max << std::endl; 270 | points[index1][index2]->getDescriptor()->getWeightedFlatDescription(description, weights); 271 | // std::cout << description << std::endl; 272 | HistogramFeatureWord word(description, dist, weights); 273 | // HistogramFeatureWord word(description, dist); 274 | currentVocabulary.push_back(word); 275 | } 276 | } 277 | 278 | if (samplingType == 1){ 279 | ///uniform sampling 280 | boost::uniform_int generator(0, points.size() - 1); 281 | for(unsigned int p = 0; p < fileFeatures; p++){ 282 | unsigned int index1 = generator(rng); ///here are samples generated (this is on feature files, not on log files!) 283 | unsigned int max = points[index1].size() - 1; 284 | if(points[index1].size() < 1){ 285 | p--; 286 | continue; 287 | } 288 | boost::uniform_int generator2(0, max); 289 | unsigned int index2 = generator2(rng); 290 | std::vector description; 291 | std::vector weights; 292 | // std::cout << "p= " << p << " i1= "<< index1 << " i2= " << index2 << " " << max << std::endl; 293 | points[index1][index2]->getDescriptor()->getWeightedFlatDescription(description, weights); 294 | // std::cout << description << std::endl; 295 | HistogramFeatureWord word(description, dist, weights); 296 | currentVocabulary.push_back(word); 297 | } 298 | } 299 | 300 | 301 | 302 | for(unsigned int j = 0; j < points.size(); j++){ 303 | for(unsigned int k = 0; k < points[j].size(); k++){ 304 | delete points[j][k]; 305 | } 306 | } 307 | 308 | inputStream.close(); 309 | std::cout << *it << " " << points.size() << " | " << currentVocabulary.size() << std::endl; 310 | } 311 | 312 | KMeansClustering KMeans(30, 0.001); 313 | HistogramVocabulary clusters2(vocabularySize); 314 | KMeans.initializeClusters(currentVocabulary, clusters2); 315 | KMeans.clusterPoints(currentVocabulary, clusters2); 316 | std::ostringstream outfileK; 317 | outfileK << outfile << "_0_" << vocabularySize << "KMEANS.voc"; 318 | std::ofstream outputStreamK(outfileK.str().c_str()); 319 | boost::archive::binary_oarchive outputArchiveK(outputStreamK); 320 | outputArchiveK << BOOST_SERIALIZATION_NVP(clusters2); 321 | std::cout << "Writing Vocabulary: " << outfileK.str() << " Size = " << clusters2.size() << std::endl; 322 | 323 | 324 | 325 | double bestScore = -1; 326 | HistogramVocabulary bestVocabulary; 327 | for(unsigned int i = 0; i < vocabularyLevels; i++){ 328 | HistogramVocabulary clusters(currentVocabulary); 329 | 330 | clustering.clusterPoints< PlusPlusKmeansInitialization >(currentVocabulary, clusters, i, vocabularySize); 331 | 332 | 333 | // Test the filenames 334 | std::ifstream inputStream(filenames[0].c_str()); 335 | boost::archive::binary_iarchive inputArchive(inputStream); 336 | std::vector< std::vector< InterestPoint *> > points; 337 | inputArchive >> BOOST_SERIALIZATION_NVP(points); 338 | KDTree::KDTree<2,LabelledPoint> tree; 339 | unsigned int label = 0, features = 0; 340 | 341 | // Creating ground truth labels 342 | LabelLog labelLog(points.size()); 343 | for(unsigned int j = 0; j < points.size(); j++) { 344 | labelLog[j].resize(points[j].size()); 345 | for(unsigned int k = 0; k < points[j].size(); k++) { 346 | LabelledPoint point, closestPoint; 347 | point[0]=points[j][k]->getPosition().x; 348 | point[1]=points[j][k]->getPosition().y; 349 | SearchResult closest = tree.find_nearest(point, 0.04); 350 | features++; 351 | if(closest.first == tree.end()) { 352 | point.label = label++; 353 | tree.insert(point); 354 | labelLog[j][k] = point.label; 355 | } else { 356 | labelLog[j][k] = closest.first->label; 357 | } 358 | } 359 | } 360 | 361 | std::cout << "Testing vocabulary on " << features << " interest points" << std::endl; 362 | 363 | std::string bar(50,' '); 364 | bar[0] = '#'; 365 | unsigned int progress = 0; 366 | 367 | 368 | 369 | VMeasure result;// = testCluster(clusters, labelLog, points, label, beta); 370 | double score = 0;//(1.+ beta) * result.homogeneity * result.completeness / (beta * result.homogeneity + result.completeness); 371 | // unsigned int vocabularySize = std::numeric_limits< unsigned int >::max(); 372 | /* if(score > bestScore){ 373 | std::cout << "Vocabulary with " << clusters.size() << " words, tested on " << label << " labels, scored " << score << " c = " << result.completeness << " h = " << result.homogeneity << std::endl; 374 | bestScore = score; 375 | bestVocabulary = clusters; 376 | }*/ 377 | // currentVocabulary.swap(clusters); 378 | 379 | // std::cout << "Vocabulary with " << clusters.size() << " words, tested on " << label << " labels, scored " << score << std::endl; 380 | /* unsigned int dendit = 1; 381 | for(HistogramVocabulary::iterator it = clusters.begin(); it != clusters.end(); it++, dendit++) { 382 | // std::cout << "Merging cluster " << it->second.first << " with " << it->second.second << ", distance = " << it->first << std::endl; 383 | unsigned int currentProgress = (dendit*100)/(clusters.size()); 384 | if (progress < currentProgress){ 385 | progress = currentProgress; 386 | bar[progress/2] = '#'; 387 | std::cout << "\rTesting vocabulary [" << bar << "] " << progress << "%" << std::flush; 388 | }*/ 389 | result = testCluster(clusters, labelLog, points, label, beta); 390 | score = (1.+ beta) * result.homogeneity * result.completeness / (beta * result.homogeneity + result.completeness); 391 | std::cout << "Vocabulary with " << clusters.size() << " words, tested on " << label << " labels, scored " << score << " c = " << result.completeness << " h = " << result.homogeneity << std::endl; 392 | if(score > bestScore){ 393 | std::cout << "Improved score from " << bestScore << " to " << score << std::endl; 394 | bestScore = score; 395 | bestVocabulary = clusters; 396 | // vocabularySize = clusters.size(); 397 | }/* else if(fabs(score - bestScore) < 0.001 && clusters.size() < vocabularySize){ 398 | std::cout << "Improved size from " << bestScore << " to " << score << std::endl; 399 | bestVocabulary = clusters; 400 | // vocabularySize = clusters.size(); 401 | }*/ 402 | /* } 403 | std::cout << "\rTesting vocabulary [" << bar << "] 100% done." << std::endl;*/ 404 | 405 | // HistogramVocabulary clusters(20); 406 | // boost::mt19937 rng; 407 | // boost::uniform_smallint generator(0, maxFeatures); 408 | // for(int j = 0; j < 20; j++){ 409 | // clusters[j] = currentVocabulary[generator(rng)]; 410 | // } 411 | 412 | // clustering.clusterPoints(currentVocabulary, clusters); 413 | 414 | std::ostringstream outfileL; 415 | outfileL << outfile << "_" << i << "_" << vocabularySize << ".voc"; 416 | 417 | std::ofstream outputStreamL(outfileL.str().c_str()); 418 | // boost::archive::xml_oarchive archive(outputStream); 419 | boost::archive::binary_oarchive outputArchiveL(outputStreamL); 420 | 421 | outputArchiveL << BOOST_SERIALIZATION_NVP(clusters); 422 | 423 | std::cout << "Writing Vocabulary: " << outfileL.str() << std::endl; 424 | } 425 | std::ostringstream outfileB; 426 | outfileB << outfile << "_" << maxFeatures << "_B.voc"; 427 | 428 | std::ofstream outputStream(outfileB.str().c_str()); 429 | // boost::archive::xml_oarchive archive(outputStream); 430 | boost::archive::binary_oarchive outputArchive(outputStream); 431 | 432 | outputArchive << BOOST_SERIALIZATION_NVP(bestVocabulary); 433 | 434 | std::cout << "Writing Vocabulary: " << outfileB.str() << std::endl; 435 | } 436 | 437 | -------------------------------------------------------------------------------- /src/applications/gflip_cl.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | #include 25 | 26 | 27 | typedef struct 28 | { 29 | double meters,angle,alpha_vss; 30 | int type,kernel, kbest,bow_subtype; 31 | char bag; 32 | std::string filein ; 33 | std::string outdir; 34 | }sw_param_str; 35 | 36 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 37 | 38 | void program_info(void) 39 | { 40 | std::cout << "-i BOW input file " << std::endl; 41 | std::cout << "-t kind of matching {1: standard bow, 2: GFP geometrical flirt phrases [DEFAULT]} " << std::endl; 42 | std::cout << "-st TFIDF flavour {0: TFIDF, 1: Sublinear TFIDF scaling, 2: lenght smoothing TFIDF} (used only with -t 1)" << std::endl; 43 | std::cout << "-alpha [0,1] smoother for length (used only with -t 1 -st 2) [0.4 DEFAULT]" << std::endl; 44 | std::cout << "-k [2..N] GFP kernel size [2 DEFAULT] (used only with -t 2) " << std::endl; 45 | std::cout << "-b bag of distance words (histograms of pairwise distances) [NO DEFAULT]" << std::endl; 46 | std::cout << "-kbest [0..N] returns best k results for each query [50 DEFAULT]" << std::endl; 47 | } 48 | 49 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 50 | 51 | int parse_command_line(int argc, char **argv, sw_param_str *sw_param) 52 | { 53 | int i; 54 | 55 | sw_param -> type = 2; 56 | sw_param -> kernel = 2; 57 | sw_param -> bag = 0; 58 | sw_param -> outdir = "./"; 59 | sw_param -> kbest = 50; 60 | sw_param -> bow_subtype = 0; 61 | sw_param -> alpha_vss = 0.4; 62 | 63 | for(i=0; i filein = argv[i+1]; 68 | 69 | if(!strcmp(argv[i], "-b")) 70 | sw_param -> bag = 1; 71 | 72 | if(!strcmp(argv[i], "-t")) 73 | sw_param -> type = atoi(argv[i+1]); 74 | 75 | if(!strcmp(argv[i], "-st")) 76 | sw_param -> bow_subtype = atoi(argv[i+1]); 77 | 78 | if(!strcmp(argv[i], "-alpha")) 79 | sw_param -> alpha_vss = atof(argv[i+1]); 80 | 81 | if(!strcmp(argv[i], "-k")) 82 | sw_param -> kernel = atoi(argv[i+1]); 83 | 84 | if(!strcmp(argv[i], "--help")) 85 | { 86 | program_info(); 87 | exit(1); 88 | } 89 | 90 | 91 | if(!strcmp(argv[i], "-kbest")) 92 | sw_param -> kbest = atoi(argv[i+1]); 93 | 94 | } 95 | 96 | 97 | if(sw_param -> kbest == 1) 98 | { 99 | std::cout << "Kbest must be > 1" << std::endl; 100 | exit(1); 101 | } 102 | 103 | if(!sw_param -> filein.size()) 104 | { 105 | printf("Input filename missing\n"); 106 | exit(1); 107 | } 108 | 109 | if(sw_param -> kernel < 2) 110 | { 111 | printf("Standard BOW selected. Readjusting params\n"); 112 | sw_param -> kernel = 2; 113 | sw_param -> type = 1; 114 | } 115 | 116 | 117 | std::cout << "[PAR] BOW input filename: " << sw_param -> filein << std::endl; 118 | if(sw_param -> type == 1) 119 | { 120 | std::cout << "[PAR] Standard Bag-of-words -- NO GFP" << std::endl; 121 | std::cout << "[PAR] TF-IDF flavour: " << sw_param -> bow_subtype << std::endl; 122 | } 123 | else 124 | { 125 | std::cout << "[PAR] Geometrical FLIRT Phrases matching " << std::endl; 126 | std::cout << "[PAR] Kernel size : " << sw_param -> kernel << std::endl; 127 | } 128 | 129 | if(sw_param -> bag == 1) 130 | std::cout << "[PAR] Bag of distances " << sw_param -> alpha_vss; 131 | if(sw_param -> type == 1 && sw_param -> bow_subtype == 2) 132 | std::cout << "[PAR] Type of matching: " << sw_param -> type << std::endl; 133 | if(sw_param -> type == 1 && sw_param -> bow_subtype == 2) 134 | std::cout << "[PAR] alpha smoothing " << sw_param -> alpha_vss; 135 | 136 | std::cout << "[PAR] Kbest results : " << sw_param -> kbest << std::endl; 137 | return(1); 138 | } 139 | 140 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 141 | 142 | int main (int argc, char **argv) 143 | { 144 | sw_param_str sw_param; 145 | std::cout << std::endl; 146 | std::cout << ">> Geometrical FLIRT Phrases for Large Scale Place Recognition in 2D Range Data" << std::endl; 147 | std::cout << ">> G.D. Tipaldi, L.Spinello, W. Burgard -- Int. Conf. Robotics and Automation (ICRA) 2013" << std::endl; 148 | std::cout << ">> coded: L. Spinello 2013" << std::endl << std::endl; 149 | if(!parse_command_line(argc, argv, &sw_param)) 150 | exit(0); 151 | 152 | class gflip_engine gfp (sw_param.kernel, sw_param.kbest, sw_param.bag, sw_param.bow_subtype, sw_param.alpha_vss); 153 | 154 | int ret2 = gfp.read_wordscan_file(sw_param.filein); 155 | std::cout << "Read FLIRT word scans: " << ret2 << std::endl; 156 | 157 | std::cout << "Preparing inverted file index and TF-IDF" << std::endl; 158 | gfp.prepare( ); 159 | 160 | std::cout << "Start retreival of all scans vs all dataset " << std::endl; 161 | gfp.run_evaluation(sw_param.type); 162 | std::cout << "done." << std::endl; 163 | 164 | } 165 | 166 | -------------------------------------------------------------------------------- /src/applications/gflip_cl_onequery.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | #include 25 | 26 | 27 | typedef struct 28 | { 29 | double meters,angle,alpha_vss; 30 | int type,kernel, kbest,bow_subtype; 31 | char bag; 32 | std::string filein ; 33 | std::string outdir; 34 | }sw_param_str; 35 | 36 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 37 | 38 | void program_info(void) 39 | { 40 | std::cout << "-i BOW input file " << std::endl; 41 | std::cout << "-t kind of matching {1: standard bow, 2: GFP geometrical flirt phrases [DEFAULT]} " << std::endl; 42 | std::cout << "-st TFIDF flavour {0: TFIDF, 1: Sublinear TFIDF scaling, 2: lenght smoothing TFIDF} (used only with -t 1)" << std::endl; 43 | std::cout << "-alpha [0,1] smoother for length (used only with -t 1 -st 2) [0.4 DEFAULT]" << std::endl; 44 | std::cout << "-k [2..N] GFP kernel size [2 DEFAULT] (used only with -t 2) " << std::endl; 45 | std::cout << "-b bag of distance words (histograms of pairwise distances) [NO DEFAULT]" << std::endl; 46 | std::cout << "-kbest [0..N] returns best k results for each query [50 DEFAULT]" << std::endl; 47 | } 48 | 49 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 50 | 51 | int parse_command_line(int argc, char **argv, sw_param_str *sw_param) 52 | { 53 | int i; 54 | 55 | sw_param -> type = 2; 56 | sw_param -> kernel = 2; 57 | sw_param -> bag = 0; 58 | sw_param -> outdir = "./"; 59 | sw_param -> kbest = 50; 60 | sw_param -> bow_subtype = 0; 61 | sw_param -> alpha_vss = 0.4; 62 | 63 | for(i=0; i filein = argv[i+1]; 68 | 69 | if(!strcmp(argv[i], "-b")) 70 | sw_param -> bag = 1; 71 | 72 | if(!strcmp(argv[i], "-t")) 73 | sw_param -> type = atoi(argv[i+1]); 74 | 75 | if(!strcmp(argv[i], "-st")) 76 | sw_param -> bow_subtype = atoi(argv[i+1]); 77 | 78 | if(!strcmp(argv[i], "-alpha")) 79 | sw_param -> alpha_vss = atof(argv[i+1]); 80 | 81 | if(!strcmp(argv[i], "-k")) 82 | sw_param -> kernel = atoi(argv[i+1]); 83 | 84 | if(!strcmp(argv[i], "--help")) 85 | { 86 | program_info(); 87 | exit(1); 88 | } 89 | 90 | 91 | if(!strcmp(argv[i], "-kbest")) 92 | sw_param -> kbest = atoi(argv[i+1]); 93 | 94 | } 95 | 96 | 97 | if(sw_param -> kbest == 1) 98 | { 99 | std::cout << "Kbest must be > 1" << std::endl; 100 | exit(1); 101 | } 102 | 103 | if(!sw_param -> filein.size()) 104 | { 105 | printf("Input filename missing\n"); 106 | exit(1); 107 | } 108 | 109 | if(sw_param -> kernel < 2) 110 | { 111 | printf("Standard BOW selected. Readjusting params\n"); 112 | sw_param -> kernel = 2; 113 | sw_param -> type = 1; 114 | } 115 | 116 | 117 | std::cout << "[PAR] BOW input filename: " << sw_param -> filein << std::endl; 118 | if(sw_param -> type == 1) 119 | { 120 | std::cout << "[PAR] Standard Bag-of-words -- NO GFP" << std::endl; 121 | std::cout << "[PAR] TF-IDF flavour: " << sw_param -> bow_subtype << std::endl; 122 | } 123 | else 124 | { 125 | std::cout << "[PAR] Geometrical FLIRT Phrases matching " << std::endl; 126 | std::cout << "[PAR] Kernel size : " << sw_param -> kernel << std::endl; 127 | } 128 | 129 | if(sw_param -> bag == 1) 130 | std::cout << "[PAR] Bag of distances " << sw_param -> alpha_vss; 131 | if(sw_param -> type == 1 && sw_param -> bow_subtype == 2) 132 | std::cout << "[PAR] Type of matching: " << sw_param -> type << std::endl; 133 | if(sw_param -> type == 1 && sw_param -> bow_subtype == 2) 134 | std::cout << "[PAR] alpha smoothing " << sw_param -> alpha_vss; 135 | 136 | std::cout << "[PAR] Kbest results : " << sw_param -> kbest << std::endl; 137 | return(1); 138 | } 139 | 140 | //~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ 141 | 142 | int main (int argc, char **argv) 143 | { 144 | sw_param_str sw_param; 145 | std::cout << std::endl; 146 | std::cout << ">> Geometrical FLIRT Phrases for Large Scale Place Recognition in 2D Range Data" << std::endl; 147 | std::cout << ">> G.D. Tipaldi, L.Spinello, W. Burgard -- Int. Conf. Robotics and Automation (ICRA) 2013" << std::endl; 148 | std::cout << ">> coded: L. Spinello 2013" << std::endl << std::endl; 149 | std::cout << "** One query scan example **" << std::endl << std::endl; 150 | 151 | if(!parse_command_line(argc, argv, &sw_param)) 152 | exit(0); 153 | 154 | class gflip_engine gfp (sw_param.kernel, sw_param.kbest, sw_param.bag, sw_param.bow_subtype, sw_param.alpha_vss); 155 | 156 | int ret2 = gfp.read_wordscan_file(sw_param.filein); 157 | std::cout << "Read FLIRT word scans: " << ret2 << std::endl; 158 | 159 | std::cout << "Preparing inverted file index and TF-IDF" << std::endl; 160 | gfp.prepare( ); 161 | 162 | std::cout << "Example of querying a scan " << std::endl; 163 | std::vector query_v (26); 164 | int qry[] = {157, 153, 157, 160, 156, 165 | 158, 84, 162, 168, 162, 166 | 168, 224, 187, 134, 157, 167 | 130 ,130, 153 ,187, 130, 168 | 167, 194, 160, 130, 130,181}; 169 | query_v.assign(&qry[0], &qry[0]+26); 170 | 171 | std::vector < std::pair > *scorequery = NULL; 172 | gfp.query(sw_param.type, query_v, &scorequery); 173 | for(int ii=0;ii 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | -------------------------------------------------------------------------------- /src/gflip/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(gflip_SRCS 2 | gflip_engine.cpp 3 | ) 4 | 5 | SET(gflip_HDRS 6 | gflip_engine.hpp 7 | ) 8 | 9 | ADD_LIBRARY(gflip SHARED ${gflip_SRCS}) 10 | 11 | install(TARGETS gflip 12 | RUNTIME DESTINATION bin 13 | LIBRARY DESTINATION lib/${PROJECT_NAME} 14 | ARCHIVE DESTINATION lib/${PROJECT_NAME}) 15 | 16 | install(FILES ${gflip_HDRS} DESTINATION include/${PROJECT_NAME}/gflip) 17 | -------------------------------------------------------------------------------- /src/gflip/gflip_engine.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #include 24 | 25 | 26 | // --------------------------------------------------------- 27 | 28 | void gflip_engine::prepare(void) 29 | { 30 | cache_binomial_coeff(); 31 | 32 | if(bow_type==1) 33 | reformulate_to_bagofdistances(); 34 | 35 | build_tfidf(); 36 | 37 | 38 | //~ norms 39 | normgfp_rc_idf_sum.resize(max_bow_len); 40 | normgfp_rc_weak_match.resize(max_bow_len); 41 | for(uint i=0;i (laserscan_bow.size()); 50 | mtchgfp_min_det_idx = std::vector (laserscan_bow.size()); 51 | } 52 | 53 | 54 | // --------------------------------------------------------- 55 | 56 | void gflip_engine::cache_binomial_coeff(void) 57 | { 58 | cached_binomial_coeff.resize (DEFAULT_CACHEBINOMIAL,0); 59 | for(uint i=wgv_kernel_size-1;i(i, (double)wgv_kernel_size-1); 61 | } 62 | 63 | // --------------------------------------------------------- 64 | 65 | void gflip_engine:: reformulate_to_bagofdistances(void) 66 | { 67 | int sz = (bow_dst_end-bow_dst_start)/bow_dst_interval+1; 68 | 69 | for(uint i=0;i w_tmp; 72 | laserscan_bow[i].w.clear(); 73 | for(int j=0;j<(int)laserscan_bow[i].w_x.size()-1;j++) 74 | { 75 | double dx = laserscan_bow[i].w_x[j] - laserscan_bow[i].w_x[j+1]; 76 | double dy = laserscan_bow[i].w_y[j] - laserscan_bow[i].w_y[j+1]; 77 | double d = sqrt(dx*dx + dy*dy); 78 | int idx = (d-bow_dst_start)/bow_dst_interval; 79 | if(idx > sz) 80 | idx = sz +1; 81 | w_tmp.push_back(idx); 82 | laserscan_bow[i].w = w_tmp; 83 | //~ std::cout << d << " " << dx << " "<< dy << " "<< idx << " " << sz << std::endl; 84 | } 85 | } 86 | 87 | } 88 | 89 | // --------------------------------------------------------- 90 | 91 | double gflip_engine::norm_gfp(std::vector & query_v) 92 | { 93 | double norm2 = 0,query_v_norm=1; 94 | std::fill(normgfp_rc_idf_sum.begin(), normgfp_rc_idf_sum.end(), 0); 95 | std::fill(normgfp_rc_weak_match.begin(), normgfp_rc_weak_match.end(), 0); 96 | 97 | int min_det_idx_qry=INT_MAX; 98 | int max_det_idx_qry=-INT_MAX; 99 | int middleidx = max_bow_len/2; 100 | 101 | for(uint j=0;j max_det_idx_qry) 115 | max_det_idx_qry = middleidx+w_order_dif; 116 | } 117 | } 118 | } 119 | 120 | for(int b=min_det_idx_qry;b<=max_det_idx_qry;b++) 121 | { 122 | double combo = 0; 123 | if(normgfp_rc_weak_match[b] >= wgv_kernel_size ) 124 | combo = cached_binomial_coeff[normgfp_rc_weak_match[b]-1]; 125 | norm2 +=normgfp_rc_idf_sum[b] * combo; 126 | } 127 | //~ rounding errs 128 | if(norm2 > 0) 129 | query_v_norm = sqrt(norm2); 130 | 131 | return(query_v_norm); 132 | } 133 | 134 | 135 | 136 | // --------------------------------------------------------- 137 | 138 | void gflip_engine::matching_gfp(std::vector &query_v) 139 | { 140 | int middleidx = max_bow_len/2; 141 | std::fill(mtchgfp_used_doc_idx.begin(), mtchgfp_used_doc_idx.end(), 0); 142 | std::fill(mtchgfp_min_det_idx.begin(), mtchgfp_min_det_idx.end(), +INT_MAX); 143 | std::fill(mtchgfp_max_det_idx.begin(), mtchgfp_max_det_idx.end(), -INT_MAX); 144 | 145 | std::fill(mtchgfp_rc_weak_match.begin(), mtchgfp_rc_weak_match.end(), 0); 146 | std::fill(mtchgfp_rc_idf_sum.begin(), mtchgfp_rc_idf_sum.end(), 0); 147 | 148 | //~ query norm 149 | double query_v_norm = norm_gfp(query_v); 150 | //~ every word of the query 151 | for(uint j=0;j mtchgfp_max_det_idx[doc_idx]) 171 | mtchgfp_max_det_idx[doc_idx] = middleidx+w_order_dif; 172 | } 173 | } 174 | } 175 | 176 | int num_used_doc_idx = 0; 177 | for(uint j=0;j= wgv_kernel_size ) 199 | combo = cached_binomial_coeff[ mtchgfp_rc_weak_match[rcidx]-1 ]; 200 | score +=mtchgfp_rc_idf_sum[rcidx] * combo; 201 | } 202 | //~ normed istance 203 | score = score / (laserscan_bow[doc_idx].norm_wgv * query_v_norm); 204 | 205 | //~ avoids no go zone 206 | if( doc_idx <= start_l || doc_idx >= stop_l) 207 | scoreset[u_idx].first = 1.0 - score; 208 | 209 | u_idx++; 210 | } 211 | 212 | //~ sort the results 213 | sort(scoreset.begin(), scoreset.end(), isBettermatched); 214 | 215 | } 216 | 217 | // --------------------------------------------------------- 218 | 219 | bool isBettermatched(std::pair x, std::pair y) 220 | { 221 | return x.first < y.first; 222 | } 223 | 224 | // --------------------------------------------------------- 225 | 226 | void gflip_engine::matching_bow(std::vector &query_v ) 227 | { 228 | std::vector image_db_scores(laserscan_bow.size(),0); 229 | std::set used_doc_idx; 230 | 231 | double query_v_norm = 1, qsum = 0; 232 | std::vector query_bow = std::vector (dictionary_dimensions,0); 233 | for(uint j=0;j::iterator it=used_doc_idx.begin(); it!=used_doc_idx.end(); it++) 263 | { 264 | double score = image_db_scores[*it]/query_v_norm; 265 | 266 | scoreset[u_idx].first = 1.0; 267 | scoreset[u_idx].second = *it; 268 | 269 | //~ avoids no-go zone 270 | if( *it <= start_l || *it >= stop_l) 271 | scoreset[u_idx].first = 1.0 - score; 272 | u_idx++; 273 | } 274 | 275 | 276 | sort(scoreset.begin(), scoreset.end(), isBettermatched); 277 | 278 | } 279 | 280 | 281 | // --------------------------------------------------------- 282 | 283 | 284 | void gflip_engine::query(int dtype, std::vector &query_v, std::vector < std::pair > **scoreoutput) 285 | { 286 | //~ avoids any skip 287 | start_l = 0; 288 | stop_l = 0; 289 | //~ does the search 290 | if(dtype ==1) 291 | matching_bow(query_v); 292 | if(dtype ==2) 293 | matching_gfp(query_v); 294 | 295 | *scoreoutput = &scoreset; 296 | } 297 | 298 | // --------------------------------------------------------- 299 | 300 | 301 | 302 | void gflip_engine::run_evaluation(int dtype) 303 | { 304 | struct timeval tim_st,tim_ed; 305 | char nosave = 0; 306 | 307 | char buff[2000]; 308 | sprintf(buff,"./%s.nn", fileoutput_rootname.c_str()); 309 | FILE *f; 310 | 311 | if(!nosave) 312 | f=fopen(buff, "wt"); 313 | 314 | double dtime_avg=0; 315 | int countv=0; 316 | for(uint i=0;i query_v = laserscan_bow[i].w; 320 | 321 | //~ if 0 len 322 | if(!query_v.size()) 323 | { 324 | if(!nosave) 325 | fprintf (f, "\n"); 326 | continue; 327 | } 328 | //~ exclude the i scan 329 | start_l = i-1; 330 | stop_l = i+1; 331 | 332 | //~ match with several techs 333 | gettimeofday(&tim_st, NULL); 334 | if(dtype ==1) 335 | matching_bow(query_v); 336 | if(dtype ==2) 337 | matching_gfp(query_v); 338 | gettimeofday(&tim_ed, NULL); 339 | double dtimeqry = (tim_ed.tv_sec-tim_st.tv_sec) + (tim_ed.tv_usec-tim_st.tv_usec)/1000000.0; 340 | dtime_avg += dtimeqry; 341 | if(!nosave) 342 | { 343 | fprintf (f, "%d %d %7.7f ", kbest, (int)scoreset.size(), dtimeqry); 344 | uint minscoresetsize = std::min((int)kbest,(int)scoreset.size()); 345 | for(uint ii=0;ii maxid) 374 | { 375 | maxid = laserscan_bow[i].w[j]; 376 | maxid_idx = i; 377 | } 378 | 379 | } 380 | 381 | if((int)laserscan_bow[i].w.size() > max_bow_len) 382 | max_bow_len=laserscan_bow[i].w.size(); 383 | } 384 | //~ include last number 385 | maxid+=1; 386 | dictionary_dimensions = maxid; 387 | //~ do it large 388 | max_bow_len = (max_bow_len+1)*2; 389 | std::cout << "Detected dictionary dimension: "<< maxid << " @ "<< maxid_idx << std::endl; 390 | std::cout << "Detected max bow len : "<< max_bow_len << std::endl; 391 | 392 | tf_idf = std::vector (maxid); 393 | for(int word_id=0; word_id mxtf_val (laserscan_bow.size(),-DBL_MAX); 428 | for(int doc_id=0;doc_id<(int)laserscan_bow.size();doc_id++) 429 | { 430 | std::set used_idx; 431 | for(uint j=0;j mxtf_val [doc_id] ) 437 | mxtf_val[doc_id] = tf_idf[word_id].term_count_unnormalized[h]; 438 | } 439 | //~ printf("%d %g %d\n",doc_id,mxtf_val,mxtf_idx); 440 | } 441 | 442 | //~ normedtfidf, wtfidf 443 | for(int doc_id=0;doc_id<(int)laserscan_bow.size();doc_id++) 444 | { 445 | std::set used_idx; 446 | for(uint j=0;j::iterator word_id_iter=used_idx.begin(); word_id_iter!=used_idx.end(); word_id_iter++) 451 | for(uint h=0;h::iterator word_id_iter=used_idx.begin(); word_id_iter!=used_idx.end(); word_id_iter++) 468 | for(uint h=0;h 0.00001 && sum > 0.00001 ) 482 | { 483 | std::cout << "ERROR NORMALIZ FAIL "< 0.00001 && sum_wf > 0.00001 ) 488 | { 489 | std::cout << "ERROR WFIDF NORMALIZ FAIL "< 0.00001 && sum_vss > 0.00001 ) 494 | { 495 | std::cout << "ERROR VSSIDF NORMALIZ FAIL "< scanbow, std::vector xpos, std::vector ypos) 528 | { 529 | uint numwords = scanbow.size(); 530 | scan_bow tmpbow(scanbow.size()); 531 | 532 | for(uint i=0;i ftokens; 551 | LSL_stringtoken(filename, ftokens, "/"); 552 | fileoutput_rootname = ftokens[ftokens.size()-1]; 553 | //~ std::cout<< fileoutput_rootname < tokens; 560 | LSL_stringtoken(line, tokens, " "); 561 | 562 | if(tokens.size()) 563 | { 564 | uint numwords=atoi(tokens[0].c_str()); 565 | scan_bow tmpbow(numwords); 566 | 567 | if(tokens.size() != 1+numwords*3) 568 | { 569 | std::cout << "Error: input file must contain coordinates" << std::endl; 570 | exit(1); 571 | } 572 | 573 | for(uint i=0, a=0;i& tokens, const std::string& delimiters) 595 | { 596 | // Skip delimiters at beginning. 597 | std::string::size_type lastPos = str.find_first_not_of(delimiters, 0); 598 | // Find first "non-delimiter". 599 | std::string::size_type pos = str.find_first_of(delimiters, lastPos); 600 | 601 | while (std::string::npos != pos || std::string::npos != lastPos) 602 | { 603 | // Found a token, add it to the vector. 604 | tokens.push_back(str.substr(lastPos, pos - lastPos)); 605 | // Skip delimiters. Note the "not_of" 606 | lastPos = str.find_first_not_of(delimiters, pos); 607 | // Find next "non-delimiter" 608 | pos = str.find_first_of(delimiters, lastPos); 609 | } 610 | } 611 | 612 | -------------------------------------------------------------------------------- /src/gflip/gflip_engine.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | #ifndef GFP_NGN_H 24 | #define GFP_NGN_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | //~ Basic and bag of distances defaults 42 | #define DEFAULT_BOWDST_START 0 43 | #define DEFAULT_BOWDST_INTERVAL 0.2 44 | #define DEFAULT_BOWDST_END 15 45 | #define DEFAULT_BOWSUBTYPE 0 46 | #define DEFAULT_ALPHASMOOTH 0.4 47 | #define DEFAULT_BAGDISTANCE 0 48 | #define DEFAULT_CACHEBINOMIAL 10000 49 | 50 | /** 51 | * Contains a 2D scan represented by FLIRT words identified by their index, their (TF-IDF) weights, their norm for GFP 52 | * 53 | * @author Luciano Spinello 54 | */ 55 | 56 | class scan_bow 57 | { 58 | public: 59 | std::vector w, word_weight_unnormalized; 60 | std::vector w_x, w_y, word_weight, tfidf_w; 61 | double sum_weight, norm_wgv; 62 | scan_bow(int no) 63 | { 64 | w= std::vector (no); 65 | w_x = std::vector (no); 66 | w_y = std::vector (no); 67 | } 68 | }; 69 | 70 | 71 | 72 | /** 73 | * Caches the word orders in a scan for GFP indexing 74 | * 75 | * @author Luciano Spinello 76 | */ 77 | class tf_idf_db_ordercache 78 | { 79 | public: 80 | std::vector pos; 81 | }; 82 | 83 | 84 | /** 85 | * Contains TF-IDF weight for a FLIRT word 86 | * 87 | * @author Luciano Spinello 88 | */ 89 | class tf_idf_db 90 | { 91 | public: 92 | //~ per doc 93 | std::vector word_order; 94 | std::vector doc_id; 95 | std::vector term_count_unnormalized; 96 | std::vector tf_idf_doc_normed; 97 | std::vector ntf_idf_doc_normed; 98 | std::vector wf_idf_doc_normed; 99 | std::vector num_words; 100 | std::vector term_count; 101 | 102 | //~ per term 103 | int num_doc_containing_the_word, corpus_size; 104 | double idf; 105 | 106 | tf_idf_db() 107 | { 108 | num_doc_containing_the_word = 0; 109 | corpus_size = 0; 110 | } 111 | }; 112 | 113 | /** 114 | * Geometrical FLIRT Phrases (GFP) for matching 2D laser scans represented FLIRT words 115 | * 116 | * It includes methods for building the search index and matching 117 | * 118 | * "Geometrical FLIRT Phrases for Large Scale Place Recognition in 2D Range Data", G. D. Tipaldi, L. Spinello, W. Burgard -- Int. Conf. Robotics and Automation (ICRA) 2013 119 | * @author Luciano Spinello 120 | */ 121 | class gflip_engine 122 | { 123 | private: 124 | //~ vars 125 | std::vector laserscan_bow; 126 | std::vector < std::pair > scoreset; 127 | std::vector tf_idf; 128 | std::string fileoutput_rootname; 129 | int dictionary_dimensions, start_l, stop_l, max_bow_len, wgv_kernel_size, bow_type, bow_subtype; 130 | double anglethres, bow_dst_start, bow_dst_interval, bow_dst_end, alpha_vss; 131 | uint number_of_scans, kbest; 132 | std::vector cached_binomial_coeff, mtchgfp_rc_idf_sum, normgfp_rc_idf_sum; 133 | std::vector mtchgfp_min_det_idx, mtchgfp_max_det_idx, mtchgfp_rc_weak_match, normgfp_rc_weak_match; 134 | std::vector mtchgfp_used_doc_idx; 135 | 136 | //~ functions 137 | double norm_gfp(std::vector & query_v); 138 | void matching_bow(std::vector &query_v ); 139 | void matching_gfp(std::vector &query_v ); 140 | void voting_tfidf_weak_verificationOLD(std::vector &query_v ); 141 | void reformulate_to_bagofdistances(void); 142 | void cache_binomial_coeff(void); 143 | 144 | public: 145 | 146 | //~ functions 147 | /** 148 | * Reads file generated by FLIRTLIB in which each scan is described as a sequence of FLIRT words, represented each by a number 149 | * 150 | * @author Luciano Spinello 151 | */ 152 | int read_wordscan_file(std::string filename); 153 | 154 | /** 155 | * Inserts a scan described as a sequence of FLIRT words, represented each by a number 156 | * @param wordscan scan identified as a sequence of ids 157 | * @param xpos,ypos metric position of each word in \c wordscan 158 | * @author Luciano Spinello 159 | */ 160 | void insert_wordscan(std::vector wordscan, std::vector xpos, std::vector ypos); 161 | 162 | 163 | /** 164 | * Builds TF-IDF index for standard and weak verification matching methods 165 | * 166 | * It implements IDF, TF, TF-IDF, wordcount and improved TF-IDF models proposed in: 167 | * Gerard Salton and Christopher Buckley: "Term-weighting approaches in automatic text retrieval", Information processing & management, vol. 24, no. 5, 1988, Elsevier 168 | * @author Luciano Spinello 169 | */ 170 | void build_tfidf(void); 171 | 172 | /** 173 | * Matches all the scans in the dataset vs all the scans in the dataset 174 | * 175 | * Saves the k-best results on disk for each query along with computational time 176 | * 177 | * @param dtype kind of matching method: 1 standard bag-of-words, 2 geometrical FLIRT phrases 178 | * @author Luciano Spinello 179 | */ 180 | void run_evaluation(int dtype); 181 | 182 | 183 | /** 184 | * Matches a query scan with the dataset 185 | * 186 | * Saves the k-best results on disk for each query along with computational time 187 | * 188 | * @param dtype kind of matching method: 1 standard bag-of-words, 2 geometrical FLIRT phrases 189 | * @param query_v a query scan, composed by a vector of numbers, each indicating a FLIRT word 190 | * @param scoreoutput a pointer to a sorted vector of pairs containing . 191 | * @author Luciano Spinello 192 | */ 193 | void query(int dtype, std::vector &query_v, std::vector < std::pair > **scoreoutput); 194 | 195 | 196 | /** 197 | * Prepares indeces and cache for matching. Executed once at the beginning. 198 | * Builds TF-IDF index for the dataset and norms all vectors on the dataset. Allocates also needed memory. 199 | * Optionally it generates bag-of-distances 200 | * @author Luciano Spinello 201 | */ 202 | void prepare(void); 203 | 204 | /** 205 | * Constructor 206 | * 207 | * @param krnl Kernel size 208 | * @param kbt # of k-best results 209 | * @param bt 1 for bag-of-distances, 0 otherwise 210 | * @param bstype flavor of TF-IDF in case of standard bag-of-words: 0 standard TFIDF, 1 sublinear TFIDF scaling, 2 lenght smoothing TFIDF, see \link gflip_engine::build_tfidf\endlink 211 | * @param a_vss alpha_smoothing in case of standard bag-of-words with lenght smoothing TFIDF (0.4 default) 212 | * @author Luciano Spinello 213 | */ 214 | 215 | gflip_engine (int krnl, int kbt, int bt=DEFAULT_BAGDISTANCE, int bstype=DEFAULT_BOWSUBTYPE, double a_vss=DEFAULT_ALPHASMOOTH) 216 | { 217 | bow_type = bt; 218 | kbest = kbt; 219 | wgv_kernel_size = krnl; 220 | bow_subtype= bstype; 221 | alpha_vss = a_vss; 222 | 223 | //~ basic defaults for bag of distances 224 | bow_dst_start= DEFAULT_BOWDST_START; 225 | bow_dst_interval=DEFAULT_BOWDST_INTERVAL; 226 | bow_dst_end=DEFAULT_BOWDST_END; 227 | 228 | } 229 | 230 | }; 231 | 232 | /** 233 | * Simple string tokenizer (avoids additional dependencies) 234 | * 235 | * @author Luciano Spinello 236 | */ 237 | void LSL_stringtoken(const std::string& str, std::vector& tokens, const std::string& delimiters); 238 | bool isBettermatched(std::pair x, std::pair y); 239 | #endif 240 | -------------------------------------------------------------------------------- /src/gflip_mainpage.h: -------------------------------------------------------------------------------- 1 | /* * GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 2 | * Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 3 | * Burgard 4 | * 5 | * This file is part of GFLIP. 6 | * 7 | * GFLIP is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * GFLIP is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with GFLIP. If not, see . 19 | */ 20 | 21 | /*! \mainpage 22 | * 23 | * \section need Why do I need Geometrical FLIRT Phrases (GFLIP)? 24 | * Let's say you have a large 2D scan dataset and you need to localize, or to check for loop closing. 25 | * Usually, a brute forcing ICP scan matching one vs all solves the problem. 26 | * In case you need to do this process online, or repetitively, this may be computationally expensive. 27 | * 28 | * GFLIP comes to help in this situations. GFLIP makes use of a FLIRT bag-of-word representation, to create "phrases": combinations of words that 29 | * are robust to noise and are able to encode complicate sequential patterns. In contrast to other approaches, GFLIP exploits the fact that a scan 30 | * is a 1D manifold to create a rotation invariant scan representation. 31 | * This allows for reliable and fast retreival of 2D laser scans. Way more reliable 32 | * than standard bag-of-words approaches. 33 | * 34 | * In practice, given a scan represented by FLIRT words eg: 35 | * 36 | * \verbatim scan example: 50 12 56 87 94 52 68 \endverbatim 37 | * 38 | * GFLIP matches it with a dataset containing 39 | * \verbatim scan1: 13 12 56 8 94 52 687 40 | scan2: 4 58 66 45 33 6 1 41 | scan3: 1 223 3 42 | scan4: 45 33 6 1 12 56 43 | ... \endverbatim 44 | * 45 | * \section intro Why are GFLIPs useful? 46 | * 47 | * Place recognition, i.e., the problem of recognizing 48 | * if the robot is navigating in an already visited place, is a 49 | * fundamental problem in mobile robot navigation. Efficient 50 | * solutions to this problem are relevant for effectively localizing 51 | * robots and for creating maps in real time. Relatively few methods 52 | * have been proposed to efficiently solve this problem in very large 53 | * environments using 2D range data. In this paper, we introduce 54 | * geometrical FLIRT phrases (GFLIPs) as a novel retrieval method 55 | * for very efficient and precise place recognition. GFLIPs perform 56 | * approximate 2D range data matching, have low computational 57 | * cost, can handle complicated partial matching patterns and are 58 | * robust to noise. Experiments carried out with publicly available 59 | * datasets demonstrate that GFLIPs largely outperform state-of-theart approaches in 2D range-based place recognition in terms of 60 | * efficiency and recall. We obtain retrieval performances with more 61 | * than 85% recall at 99% precision in less than a second, even on 62 | * data sets obtained from several kilometer long runs. 63 | * 64 | * see the paper 65 | * \section install_sec Installation 66 | * Download the library. The library relies on cmake to generate the Makefiles. 67 | * 68 | * Go to the \c root directory of your project and run 69 | * \verbatim $ mkdir build 70 | $ cd build 71 | $ cmake ../ 72 | $ make \endverbatim 73 | Binaries are generated in \c build/bin and \c build/lib 74 | * 75 | * Library can be installed in your system by running 76 | * \verbatim $ make install \endverbatim 77 | * 78 | * 79 | * 80 | * The software depends on the following external libraries 81 | * \li Boost >= 1.4 (special_functions/binomial, serialization) 82 | * \li Eigen >= 3 83 | * \li FLIRT (The cmake scripts take care of downloading it for you) 84 | * 85 | * \section ex_sec Binaries 86 | * Binaries can be found in \c bin/. For testing a FLIRT words dataset and the vocabulary used for the paper have been included in \c data/ 87 | * The datasets can be found in hte data directory of the FLIRT library. 88 | * 89 | * \verbatim gflip_cl \endverbatim reads a dataset where each scan is composed of FLIRT words and retreives best matches in the dataset for benchmarking. 90 | * Many parameters can be selected from command line (kernel size, bag-of-words/distances, etc) 91 | * GFPLoopClosingTest learnVocabularyKMeans 92 | * 93 | * \verbatim gflip_cl_onequery \endverbatim same as above but with one scan. This is an example for understanding how to use it. 94 | * 95 | * \verbatim learnVocabularyKMeans \endverbatim learns a vocabulary from a set of datasets. 96 | * 97 | * \verbatim featureExtractor \endverbatim extracts the features from a dataset and writes them to a file for further use. 98 | * 99 | * \verbatim generateBoW \endverbatim generates a BoW description of the whole dataset to be used for gflip_cl and gflip_cl_onequery. 100 | * 101 | * \verbatim generateNN \endverbatim generates a nearest neighbor file to be used with nnLoopClosingTest. 102 | * 103 | * \verbatim GFPLoopClosingTest \endverbatim performs the full matching from feature extraction to BoW generation and kernelized matching. 104 | * 105 | * \section ref How to cite GFLIP 106 | * "Geometrical FLIRT Phrases for Large Scale Place Recognition in 2D Range Data", G. D. Tipaldi, L. Spinello, W. Burgard -- Int. Conf. Robotics and Automation (ICRA) 2013 107 | * 108 | * \section license License 109 | * GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 110 | * Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram Burgard 111 | * 112 | * This file is part of GFLIP. 113 | * 114 | * GFLIP is free software: you can redistribute it and/or modify 115 | * it under the terms of the GNU Lesser General Public License as published by 116 | * the Free Software Foundation, either version 3 of the License, or 117 | * (at your option) any later version. 118 | * 119 | * GFLIP is distributed in the hope that it will be useful, 120 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 121 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 122 | * GNU Lesser General Public License for more details. 123 | * 124 | * You should have received a copy of the GNU Lesser General Public License 125 | * along with GFLIP. If not, see . 126 | * 127 | */ 128 | 129 | -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(utils_SRCS 2 | Regression.cpp 3 | SimplePeakFinder.cpp 4 | SimpleMinMaxPeakFinder.cpp 5 | PoseEstimation.cpp 6 | ) 7 | 8 | ADD_LIBRARY(utils SHARED ${utils_SRCS}) 9 | -------------------------------------------------------------------------------- /src/utils/Convolution.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the FLIRTLib project 4 | * 5 | * FLIRTLib Copyright (c) 2010 Gian Diego Tipaldi and Kai O. Arras 6 | * 7 | * This software is licensed under the "Creative Commons 8 | * License (Attribution-NonCommercial-ShareAlike 3.0)" 9 | * and is copyrighted by Gian Diego Tipaldi and Kai O. Arras 10 | * 11 | * Further information on this license can be found at: 12 | * http://creativecommons.org/licenses/by-nc-sa/3.0/ 13 | * 14 | * FLIRTLib is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied 16 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 17 | * PURPOSE. 18 | * 19 | *****************************************************************/ 20 | 21 | 22 | 23 | #ifndef CONVOLUTION_H_ 24 | #define CONVOLUTION_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | /** 32 | * Simple functions for performing convolution on discrete signals. 33 | * 34 | * @author Gian Diego Tipaldi 35 | */ 36 | 37 | /** 38 | * The padding strategy for the convolution operation. 39 | * 40 | */ 41 | enum ConvolutionPadding{ 42 | ZERO, /**< The zero padding strategy. The original signal is augmented with zeros at the borders. */ 43 | SPECULAR, /**< The specular padding strategy. The original signal is replicated in a specular way at the borders. */ 44 | CIRCULAR /**< The circular padding strategy. It implements the circular convolution. */ 45 | }; 46 | 47 | /** 48 | * The size of the convolution result 49 | * 50 | */ 51 | enum ConvolutionResult{ 52 | SAME, /**< The convolution result has the same size of the original signal. */ 53 | FULL /**< The convolution result has the full size. */ 54 | }; 55 | 56 | 57 | /** 58 | * Convolve the kernel over the source, if the size of the source is bigger than the size of the kernel, the opposite otherwise. 59 | * The padding is defined according to padding and can be ZERO, SPECULAR or CIRCULAR. The size of the result is defined by resultType 60 | * and can be FULL or SAME. The parameter offset define the offset of the kernel with respect to the source. 61 | * 62 | * @param source The signal to convolve. 63 | * @param kernel The convolution kernel. If the kernel is bigger than the source kernel and source are inverted. 64 | * @param offset The offset of the convolution. It is useful to shifting the signal to the left or right. 65 | * @param padding The type of padding. See the #ConvolutionPadding enum. 66 | * @param resultType The size of the convolution result. See the #ConvolutionResult enum. 67 | */ 68 | 69 | template 70 | std::vector convolve1D(const std::vector& source, const std::vector& kernel, int offset = 0, ConvolutionPadding padding = SPECULAR, ConvolutionResult resultType = SAME); 71 | 72 | /** 73 | * Compute the Bessel kernel for smoothing 74 | * 75 | * @param sigma The standard deviation of the kernel. 76 | * @param kernelSize The size of the kernel. 77 | */ 78 | 79 | template 80 | std::vector besselKernel1D(Numeric sigma, unsigned int kernelSize); 81 | 82 | /** 83 | * Compute the Gaussian kernel for smoothing 84 | * 85 | * @param sigma The standard deviation of the kernel. 86 | * @param kernelSize The size of the kernel. 87 | */ 88 | 89 | template 90 | std::vector gaussianKernel1D(Numeric sigma, unsigned int kernelSize); 91 | 92 | /** Print the signal to a stream */ 93 | template 94 | std::ostream& operator<<(std::ostream& out, const std::vector& vector); 95 | 96 | // real implementation 97 | #include 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /src/utils/Convolution.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // 3 | // GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 4 | // Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 5 | // Burgard 6 | // 7 | // This file is part of GFLIP. 8 | // 9 | // GFLIP is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU Lesser General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // GFLIP is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU Lesser General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU Lesser General Public License 20 | // along with GFLIP. If not, see . 21 | // 22 | 23 | 24 | template 25 | std::ostream& operator<<(std::ostream& out, const std::vector& vector){ 26 | for(uint i = 0; i < vector.size(); i++){ 27 | out << vector[i] << " "; 28 | } 29 | return out; 30 | } 31 | 32 | template 33 | std::vector convolve1D(const std::vector& _source, const std::vector& _kernel, int _offset, ConvolutionPadding _padding, ConvolutionResult _resultType){ 34 | _resultType = (_padding == CIRCULAR) ? SAME : _resultType; 35 | bool inverted = (_source.size() < _kernel.size()); 36 | const std::vector& source = (inverted) ? _kernel : _source; 37 | const std::vector& kernel = (inverted) ? _source : _kernel; 38 | unsigned int size = _source.size() + (_resultType == FULL) * (_kernel.size() - 1); 39 | std::vector result(size); 40 | 41 | unsigned int i = 0; 42 | for( ; i < result.size(); i++){ 43 | result[i] = 0; 44 | 45 | int j = (int)i - (int)kernel.size() + 1 ; 46 | for(; j <= (int)i && j < _offset; j++){ 47 | Numeric pad = 0; 48 | switch(_padding) { 49 | case ZERO: 50 | pad = 0; break; 51 | case SPECULAR: 52 | pad = source[_offset - j]; break; 53 | case CIRCULAR: 54 | pad = source[source.size() + j -_offset]; break; 55 | } 56 | result[i] += pad * kernel[i - j]; 57 | } 58 | 59 | for( ; j <= (int)i && j < (int)source.size() + _offset; j++){ 60 | result[i] += source[j - _offset] * kernel[i - j]; 61 | } 62 | 63 | for( ; j <= (int)i; j++){ 64 | Numeric pad = 0; 65 | switch (_padding){ 66 | case ZERO: 67 | pad = 0; break; 68 | case SPECULAR: 69 | pad = source[2*source.size() - 1 - j + _offset]; break; 70 | case CIRCULAR: 71 | pad = source[j - _offset -source.size()]; break; 72 | } 73 | result[i] += pad * kernel[i - j]; 74 | } 75 | } 76 | /* for( ; i < result.size(); i++){ 77 | result[i] = 0; 78 | 79 | int j = (int)i - (int)kernel.size() + 1; 80 | for(; j <= (int)i; j++){ 81 | Numeric pad = 0; 82 | switch (_padding){ 83 | case ZERO: 84 | pad = 0; break; 85 | case SPECULAR: 86 | pad = source[source.size() - j]; break; 87 | case CIRCULAR: 88 | pad = source[j]; break; 89 | } 90 | result[i] += pad * kernel[i - j]; 91 | } 92 | }*/ 93 | 94 | return result; 95 | } 96 | 97 | template 98 | std::vector besselKernel1D(Numeric _sigma, unsigned int _kernelSize){ 99 | unsigned int size = (_kernelSize % 2) ? _kernelSize : _kernelSize + 1; 100 | std::vector result(size); 101 | Numeric accumulator = 0; 102 | 103 | for(unsigned int i = 0; i < size; i++){ 104 | result[i] = exp(_sigma) * boost::math::cyl_bessel_i(i - 0.5*(size-1),_sigma); 105 | accumulator += result[i]; 106 | } 107 | 108 | for(unsigned int i = 0; i < size; i++){ 109 | result[i] = result[i] / accumulator; 110 | } 111 | 112 | return result; 113 | } 114 | 115 | template 116 | std::vector gaussianKernel1D(Numeric sigma, unsigned int _kernelSize){ 117 | unsigned int size = (_kernelSize % 2) ? _kernelSize : _kernelSize + 1; 118 | std::vector result(size); 119 | Numeric accumulator = 0; 120 | if(sigma < 1e-10) sigma = 1e-6; 121 | 122 | for(unsigned int i = 0; i < size; i++){ 123 | Numeric x = i - 0.5*(size-1); 124 | result[i] = exp(-x*x/(2*sigma * sigma)); 125 | accumulator += result[i]; 126 | } 127 | 128 | for(unsigned int i = 0; i < size; i++){ 129 | result[i] = result[i] / accumulator; 130 | } 131 | 132 | return result; 133 | } 134 | -------------------------------------------------------------------------------- /src/utils/HierarchicalKMeansClustering.h: -------------------------------------------------------------------------------- 1 | /* * 2 | * GFLIP - Geometrical FLIRT Phrases for Large Scale Place Recognition 3 | * Copyright (C) 2012-2013 Gian Diego Tipaldi and Luciano Spinello and Wolfram 4 | * Burgard 5 | * 6 | * This file is part of GFLIP. 7 | * 8 | * GFLIP is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU Lesser General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * GFLIP is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public License 19 | * along with GFLIP. If not, see . 20 | */ 21 | 22 | #ifndef HIERARCHICALKMEANSLUSTERING_H_ 23 | #define HIERARCHICALKMEANSLUSTERING_H_ 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | /** 31 | * Implement the Hierarchical K-Means clustering algorithm. 32 | * 33 | * @author Gian Diego Tipaldi 34 | * 35 | */ 36 | 37 | template 38 | class HierarchicalKMeansClustering { 39 | public: 40 | /** 41 | * Default constructor. It set the maximum iterations for the clustering, the minimum cluster difference and the fanout. 42 | * 43 | */ 44 | HierarchicalKMeansClustering(unsigned int maxIterations, double minError, unsigned int fanout = 10); 45 | 46 | /** 47 | * Cluster the @param points into clusters. It initialize the centroids with the provided Initialization class. 48 | * The number of clusters is the size of @param seeds. The @param seeds are modified to hold the clusters. 49 | * 50 | */ 51 | template