├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── json.CMakeLists.txt.in └── vocab_download.cmake ├── include └── kimera_multi_lcd │ ├── io.h │ ├── lcd_third_party.h │ ├── loop_closure_detector.h │ ├── serializer.h │ ├── types.h │ └── utils.h ├── package.xml ├── src ├── io.cpp ├── lcd_third_party.cpp ├── loop_closure_detector.cpp ├── serializer.cpp ├── types.cpp └── utils.cpp └── test ├── test_config.h.in ├── test_lcd.cpp └── test_utils.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 88 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: true 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: 48 | - foreach 49 | - Q_FOREACH 50 | - BOOST_FOREACH 51 | IncludeCategories: 52 | - Regex: '^' 53 | Priority: 2 54 | - Regex: '^<.*\.h>' 55 | Priority: 1 56 | - Regex: '^<.*' 57 | Priority: 2 58 | - Regex: '.*' 59 | Priority: 3 60 | IndentCaseLabels: true 61 | IndentWidth: 2 62 | IndentWrappedFunctionNames: false 63 | KeepEmptyLinesAtTheStartOfBlocks: false 64 | MacroBlockBegin: '' 65 | MacroBlockEnd: '' 66 | MaxEmptyLinesToKeep: 1 67 | NamespaceIndentation: None 68 | ObjCBlockIndentWidth: 2 69 | ObjCSpaceAfterProperty: false 70 | ObjCSpaceBeforeProtocolList: false 71 | PenaltyBreakBeforeFirstCallParameter: 1 72 | PenaltyBreakComment: 300 73 | PenaltyBreakFirstLessLess: 120 74 | PenaltyBreakString: 1000 75 | PenaltyExcessCharacter: 1000000 76 | PenaltyReturnTypeOnItsOwnLine: 200 77 | PointerAlignment: Left 78 | ReflowComments: true 79 | SortIncludes: true 80 | SpaceAfterCStyleCast: false 81 | SpaceBeforeAssignmentOperators: true 82 | SpaceBeforeParens: ControlStatements 83 | SpaceInEmptyParentheses: false 84 | SpacesBeforeTrailingComments: 2 85 | SpacesInAngles: false 86 | SpacesInContainerLiterals: true 87 | SpacesInCStyleCastParentheses: false 88 | SpacesInParentheses: false 89 | SpacesInSquareBrackets: false 90 | Standard: Auto 91 | TabWidth: 8 92 | UseTab: Never 93 | ... 94 | 95 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | *.tex 3 | vocab/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(kimera_multi_lcd) 3 | 4 | add_compile_options(-Wall -Wextra) 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_CXX_EXTENSIONS OFF) 8 | 9 | find_package(catkin_simple REQUIRED) 10 | 11 | find_package(Boost REQUIRED 12 | date_time 13 | serialization 14 | thread 15 | filesystem 16 | system 17 | regex 18 | timer 19 | chrono 20 | ) 21 | 22 | find_package(GTSAM REQUIRED) 23 | find_package(opengv REQUIRED) 24 | find_package(DBoW2 REQUIRED) 25 | if(NOT TARGET DBoW2::DBoW2) 26 | add_library(DBoW2::DBoW2 INTERFACE IMPORTED) 27 | set_target_properties(DBoW2::DBoW2 PROPERTIES 28 | INTERFACE_LINK_LIBRARIES "${DBoW2_LIBRARIES}" 29 | INTERFACE_INCLUDE_DIRECTORIES "${DBoW2_INCLUDE_DIRS}") 30 | endif() 31 | find_package(PCL REQUIRED) 32 | 33 | configure_file(cmake/json.CMakeLists.txt.in json-download/CMakeLists.txt) 34 | execute_process( 35 | COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . 36 | WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/json-download" OUTPUT_QUIET 37 | ) 38 | execute_process( 39 | COMMAND "${CMAKE_COMMAND}" --build . 40 | WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/json-download" OUTPUT_QUIET 41 | ) 42 | 43 | set(JSON_BuildTests OFF CACHE INTERNAL "") 44 | set(JSON_Install OFF CACHE INTERNAL "") 45 | add_subdirectory(${CMAKE_BINARY_DIR}/json-src ${CMAKE_BINARY_DIR}/json-build) 46 | 47 | catkin_simple() 48 | 49 | 50 | cs_add_library(${PROJECT_NAME} 51 | src/loop_closure_detector.cpp 52 | src/lcd_third_party.cpp 53 | src/types.cpp 54 | src/utils.cpp 55 | src/serializer.cpp 56 | src/io.cpp 57 | ) 58 | 59 | target_link_libraries(${PROJECT_NAME} 60 | ${catkin_LIBRARIES} 61 | ${Boost_LIBRARIES} 62 | ${OpenCV_LIBRARIES} 63 | gtsam 64 | opengv 65 | DBoW2::DBoW2 66 | tbb 67 | glog 68 | nlohmann_json::nlohmann_json 69 | ) 70 | 71 | include(cmake/vocab_download.cmake) 72 | 73 | # Unit tests 74 | # set(TEST_DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/test/data") 75 | set(TEST_VOCAB_PATH "${CMAKE_CURRENT_SOURCE_DIR}/vocab") 76 | configure_file(test/test_config.h.in test/test_config.h) 77 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/test) 78 | 79 | catkin_add_gtest(${PROJECT_NAME}-test_lcd test/test_lcd.cpp) 80 | target_link_libraries(${PROJECT_NAME}-test_lcd ${PROJECT_NAME}) 81 | 82 | catkin_add_gtest(${PROJECT_NAME}-test_utils test/test_utils.cpp) 83 | target_link_libraries(${PROJECT_NAME}-test_utils ${PROJECT_NAME}) 84 | 85 | cs_install() 86 | 87 | cs_export() 88 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2023, Massachusetts Institute of Technology 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kimera-Multi-LCD 2 | 3 | Loop closure detection for centralized and distributed multirobot visual systems. 4 | This package was used in the [Kimera-Multi system](https://github.com/MIT-SPARK/Kimera-Multi). 5 | Sample usage can be found in the [distributed loop closure modules](https://github.com/MIT-SPARK/Kimera-Distributed). 6 | 7 | ## Citation 8 | 9 | - [1] Y. Chang, Y. Tian, J. P. How and L. Carlone, "Kimera-Multi: a System for Distributed Multi-Robot Metric-Semantic Simultaneous Localization and Mapping," 2021 IEEE International Conference on Robotics and Automation (ICRA), Xi'an, China, 2021, pp. 11210-11218, doi: 10.1109/ICRA48506.2021.9561090. 10 | 11 | ```bibtex 12 | @INPROCEEDINGS{chang21icra_kimeramulti, 13 | author={Chang, Yun and Tian, Yulun and How, Jonathan P. and Carlone, Luca}, 14 | booktitle={2021 IEEE International Conference on Robotics and Automation (ICRA)}, 15 | title={Kimera-Multi: a System for Distributed Multi-Robot Metric-Semantic Simultaneous Localization and Mapping}, 16 | year={2021}, 17 | volume={}, 18 | number={}, 19 | pages={11210-11218}, 20 | doi={10.1109/ICRA48506.2021.9561090} 21 | } 22 | 23 | ``` 24 | 25 | - [2] Y. Tian, Y. Chang, F. Herrera Arias, C. Nieto-Granda, J. P. How and L. Carlone, "Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems," in IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2022-2038, Aug. 2022, doi: 10.1109/TRO.2021.3137751. 26 | ```bibtex 27 | @ARTICLE{tian22tro_kimeramulti, 28 | author={Tian, Yulun and Chang, Yun and Herrera Arias, Fernando and Nieto-Granda, Carlos and How, Jonathan P. and Carlone, Luca}, 29 | journal={IEEE Transactions on Robotics}, 30 | title={Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems}, 31 | year={2022}, 32 | volume={38}, 33 | number={4}, 34 | pages={2022-2038}, 35 | doi={10.1109/TRO.2021.3137751} 36 | } 37 | 38 | ``` -------------------------------------------------------------------------------- /cmake/json.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(json_download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add( 7 | json 8 | URL https://github.com/nlohmann/json/archive/refs/tags/v3.10.5.zip 9 | # GIT_REPOSITORY "https://github.com/nlohmann/json.git" GIT_TAG "v3.9.1" GIT_SHALLOW 10 | # TRUE 11 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/json-src" 12 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/json-build" 13 | CONFIGURE_COMMAND "" 14 | BUILD_COMMAND "" 15 | INSTALL_COMMAND "" 16 | TEST_COMMAND "" 17 | ) 18 | -------------------------------------------------------------------------------- /cmake/vocab_download.cmake: -------------------------------------------------------------------------------- 1 | ### Download and unzip the vocabulary file 2 | message("[Kimera-Multi-LCD] Trying to download and unzip the vocabulary file...") 3 | if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/vocab/mit_voc.yml) 4 | message("[Kimera-Multi-LCD] Downloading vocabulary file from drive.") 5 | file(DOWNLOAD 6 | https://github.com/MIT-SPARK/kimera-multi-vocab/raw/master/vocab.zip 7 | ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip 8 | STATUS voc_download_success 9 | ) 10 | if(voc_download_success) 11 | message("[Kimera-Multi-LCD] Unzipping vocabulary file...") 12 | 13 | execute_process(COMMAND ${CMAKE_COMMAND} -E tar xzf ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip 14 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/) 15 | execute_process(COMMAND ${CMAKE_COMMAND} -E rm ${CMAKE_CURRENT_SOURCE_DIR}/vocab.zip) 16 | message("[Kimera-Multi-LCD] Complete! Check your `${Kimera-Multi-LCD}/vocab/mit_voc.yml`.") 17 | else() 18 | message(FATAL_ERROR "[Kimera-Multi-LCD] Failed to download vocabulary file. Please download manually.") 19 | endif() 20 | else() 21 | message("[Kimera-Multi-LCD] Vocabulary file exists, will not download.") 22 | endif() 23 | -------------------------------------------------------------------------------- /include/kimera_multi_lcd/io.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "kimera_multi_lcd/types.h" 15 | 16 | namespace kimera_multi_lcd { 17 | 18 | // Save BoW vectors 19 | void saveBowVectors(const std::map& bow_vectors, 20 | const std::string& filename); 21 | 22 | void saveBowVectors(const std::map& bow_vectors, 23 | const std::string& filename); 24 | 25 | // Save VLC Frames 26 | void saveVLCFrames(const std::map& vlc_frames, 27 | const std::string& filename); 28 | 29 | void saveVLCFrames(const std::map& vlc_frames, 30 | const std::string& filename); 31 | 32 | // Save BoW vectors 33 | void loadBowVectors(const std::string& filename, 34 | std::map& bow_vectors); 35 | 36 | void loadBowVectors(const std::string& filename, 37 | std::map& bow_vectors); 38 | 39 | // Save VLC Frames 40 | void loadVLCFrames(const std::string& filename, 41 | std::map& vlc_frames); 42 | 43 | void loadVLCFrames(const std::string& filename, std::map& vlc_frames); 44 | } // namespace kimera_multi_lcd 45 | -------------------------------------------------------------------------------- /include/kimera_multi_lcd/lcd_third_party.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************** 2 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 3. The original author of the work must be notified of any 14 | redistribution of source code or in binary form. 15 | 4. Neither the name of copyright holders nor the names of its 16 | contributors may be used to endorse or promote products derived 17 | from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 21 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 22 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 23 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | *********************************************************************************/ 31 | 32 | /* ---------------------------------------------------------------------------- 33 | * Copyright 2017, Massachusetts Institute of Technology, 34 | * Cambridge, MA 02139 35 | * All Rights Reserved 36 | * Authors: Luca Carlone, et al. (see THANKS for the full author list) 37 | * See LICENSE for the license information 38 | * -------------------------------------------------------------------------- */ 39 | 40 | /** 41 | * @file ThirdPartyWrapper.h 42 | * @brief Wrapper for functions of DLoopDetector (see copyright notice above). 43 | * @author Marcus Abate 44 | */ 45 | 46 | #pragma once 47 | 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | #include "kimera_multi_lcd/types.h" 55 | 56 | namespace kimera_multi_lcd { 57 | 58 | class LcdThirdPartyWrapper { 59 | public: 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | /* ------------------------------------------------------------------------ */ 63 | /** @brief Constructor: wrapper for functions of DLoopDetector that are used 64 | * in the loop-closure-detection pipeline by LoopClosureDetector. 65 | * @param[in] params The LoopClosureDetectorParams object with all relevant 66 | * parameters. 67 | */ 68 | LcdThirdPartyWrapper(const LcdTpParams& params); 69 | 70 | /* ------------------------------------------------------------------------ */ 71 | virtual ~LcdThirdPartyWrapper(); 72 | 73 | /* ------------------------------------------------------------------------ */ 74 | /** @brief Determines whether a frame meets the temoral constraint given by 75 | * a MatchIsland. 76 | * @param[in] id The frame ID of the frame being processed in the database. 77 | * @param[in] island A MatchIsland representing several potential matches. 78 | * @return True if the constraint is met, false otherwise. 79 | */ 80 | // TODO(marcus): unit tests 81 | bool checkTemporalConstraint(const size_t& id, const MatchIsland& island); 82 | 83 | /* ------------------------------------------------------------------------ */ 84 | /** @brief Computes the various islands created by a QueryResult, which is 85 | * given by the OrbDatabase. 86 | * @param[in] q A QueryResults object containing all the resulting possible 87 | * matches with a frame. 88 | * @param[out] A vector of MatchIslands, each of which is an island of 89 | * nearby possible matches with the frame being queried. 90 | */ 91 | // TODO(marcus): unit tests 92 | void computeIslands(DBoW2::QueryResults* q, 93 | std::vector* islands) const; 94 | 95 | private: 96 | /* ------------------------------------------------------------------------ */ 97 | /** @brief Compute the overall score of an island. 98 | * @param[in] q A QueryResults object containing all the possible matches 99 | * with a frame. 100 | * @param[in] start_id The frame ID that starts the island. 101 | * @param[in] end_id The frame ID that ends the island. 102 | * @reutrn The score of the island. 103 | */ 104 | double computeIslandScore(const DBoW2::QueryResults& q, 105 | const size_t& start_id, 106 | const size_t& end_id) const; 107 | 108 | private: 109 | LcdTpParams params_; 110 | int temporal_entries_; 111 | MatchIsland latest_matched_island_; 112 | size_t latest_query_id_; 113 | }; // class LcdThirdPartyWrapper 114 | 115 | } // namespace kimera_multi_lcd 116 | -------------------------------------------------------------------------------- /include/kimera_multi_lcd/loop_closure_detector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace kimera_multi_lcd { 28 | 29 | class LoopClosureDetector { 30 | public: 31 | LoopClosureDetector(); 32 | ~LoopClosureDetector(); 33 | 34 | // Load params and initialize 35 | void loadAndInitialize(const LcdParams& params); 36 | 37 | // Add new bow vector to databse 38 | void addBowVector(const RobotPoseId& id, const DBoW2::BowVector& bow_vector); 39 | 40 | /** 41 | * @brief Find loop closure against the trajectory of the specified robot 42 | * @param robot 43 | * @param vertex_query 44 | * @param bow_vector_query 45 | * @param vertex_matches 46 | * @param scores If not null, also return the corresponding vector of normalized score 47 | * (BoW score / nss_factor) 48 | * @return 49 | */ 50 | bool detectLoopWithRobot(size_t robot, 51 | const RobotPoseId& vertex_query, 52 | const DBoW2::BowVector& bow_vector_query, 53 | std::vector* vertex_matches, 54 | std::vector* scores = nullptr); 55 | 56 | /** 57 | * @brief Find loop closure against all robots in the database 58 | * @param vertex_query 59 | * @param bow_vector_query 60 | * @param vertex_matches 61 | * @param scores If not null, also return the corresponding vector of normalized score 62 | * (BoW score / nss_factor) 63 | * @return 64 | */ 65 | bool detectLoop(const RobotPoseId& vertex_query, 66 | const DBoW2::BowVector& bow_vector_query, 67 | std::vector* vertex_matches, 68 | std::vector* scores = nullptr); 69 | 70 | void computeMatchedIndices(const RobotPoseId& vertex_query, 71 | const RobotPoseId& vertex_match, 72 | std::vector* i_query, 73 | std::vector* i_match) const; 74 | /** 75 | * @brief Perform monocular RANSAC 76 | * @param vertex_query 77 | * @param vertex_match 78 | * @param inlier_query 79 | * @param inlier_match 80 | * @param R_query_match optional output that stores the estimated relative rotation 81 | * from monocular RANSAC 82 | * @return 83 | */ 84 | bool geometricVerificationNister(const RobotPoseId& vertex_query, 85 | const RobotPoseId& vertex_match, 86 | std::vector* inlier_query, 87 | std::vector* inlier_match, 88 | gtsam::Rot3* R_query_match = nullptr); 89 | /** 90 | * @brief Perform stereo RANSAC 91 | * @param vertex_query 92 | * @param vertex_match 93 | * @param inlier_query 94 | * @param inlier_match 95 | * @param T_query_match output 3D transformation from match frame to query frame 96 | * @param R_query_match_prior prior estimates on the relative rotation, e.g, computed 97 | * with mono RANSAC. Default to nullptr in which case no prior information is used. 98 | * @return 99 | */ 100 | bool recoverPose(const RobotPoseId& vertex_query, 101 | const RobotPoseId& vertex_match, 102 | std::vector* inlier_query, 103 | std::vector* inlier_match, 104 | gtsam::Pose3* T_query_match, 105 | const gtsam::Rot3* R_query_match_prior = nullptr); 106 | 107 | inline void addVLCFrame(const RobotPoseId& id, const VLCFrame& frame) { 108 | vlc_frames_[id] = frame; 109 | } 110 | 111 | bool bowExists(const RobotPoseId& id) const; 112 | 113 | // Try to find a BoW vector in the database with ID in the 114 | // range of (id-window, id-1) 115 | bool findPreviousBoWVector(const RobotPoseId& id, 116 | int window = 5, 117 | DBoW2::BowVector* previous_bow = nullptr); 118 | 119 | int numBoWForRobot(RobotId robot_id) const; 120 | 121 | // For the input robot, return the latest PoseId where the BoW is stored 122 | // If no BoW vector is found for this robot, -1 is returned instead 123 | int latestPoseIdWithBoW(RobotId robot_id) const; 124 | 125 | DBoW2::BowVector getBoWVector(const RobotPoseId& id) const; 126 | 127 | PoseBowVector getBoWVectors(const RobotId& robot_id) const; 128 | 129 | VLCFrame getVLCFrame(const RobotPoseId& id) const; 130 | 131 | std::map getVLCFrames(const RobotId& robot_id) const; 132 | 133 | inline bool frameExists(const RobotPoseId& id) const { 134 | return vlc_frames_.find(id) != vlc_frames_.end(); 135 | } 136 | 137 | inline size_t totalBoWMatches() const { return total_bow_matches_; } 138 | 139 | inline size_t getNumGeomVerificationsMono() const { 140 | return total_geom_verifications_mono_; 141 | } 142 | 143 | inline size_t getNumGeomVerifications() const { 144 | return total_geometric_verifications_; 145 | } 146 | 147 | inline const OrbVocabulary* getVocabulary() const { return &vocab_; } 148 | 149 | inline LcdParams getParams() const { return params_; } 150 | 151 | private: 152 | // Loop closure detection parameters 153 | LcdParams params_; 154 | 155 | // BOW vocab 156 | OrbVocabulary vocab_; 157 | 158 | // Track loop closure stats 159 | size_t total_bow_matches_; 160 | size_t total_geom_verifications_mono_; 161 | size_t total_geometric_verifications_; 162 | 163 | // Database of BOW vectors from each robot (trajectory) 164 | std::unordered_map bow_vectors_; 165 | std::unordered_map> db_BoW_; 166 | // Keep track of latest pose Id with BoW for each robot 167 | std::unordered_map bow_latest_pose_id_; 168 | // Map DBoW2 Entry Id to Pose Id 169 | std::unordered_map> 170 | db_EntryId_to_PoseId_; 171 | 172 | // LCD third party wrapper 173 | std::unique_ptr lcd_tp_wrapper_; 174 | 175 | // ORB extraction and matching members 176 | cv::Ptr orb_feature_matcher_; 177 | 178 | // Dictionary of VLC frames 179 | VLCFrameDict vlc_frames_; 180 | }; 181 | 182 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /include/kimera_multi_lcd/serializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace pose_graph_tools_msgs { 8 | 9 | void to_json(nlohmann::json& j, const pose_graph_tools_msgs::BowVector& bow_vector); 10 | 11 | void from_json(const nlohmann::json& j, pose_graph_tools_msgs::BowVector& bow_vector); 12 | 13 | void to_json(nlohmann::json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame); 14 | 15 | void from_json(const nlohmann::json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame); 16 | } // namespace pose_graph_tools_msgs 17 | -------------------------------------------------------------------------------- /include/kimera_multi_lcd/types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace kimera_multi_lcd { 24 | 25 | typedef size_t RobotId; 26 | typedef size_t PoseId; 27 | typedef std::pair RobotPoseId; 28 | typedef std::set RobotPoseIdSet; 29 | typedef std::vector RobotPoseIdVector; 30 | typedef std::map PoseBowVector; 31 | 32 | // Each edge in the pose graph is uniquely identified by four integers 33 | // (robot_src, frame_src, robot_dst, frame_dst) 34 | struct EdgeID { 35 | EdgeID(size_t robot_from = 0, 36 | size_t frame_from = 0, 37 | size_t robot_to = 0, 38 | size_t frame_to = 0) 39 | : robot_src(robot_from), 40 | robot_dst(robot_to), 41 | frame_src(frame_from), 42 | frame_dst(frame_to) {} 43 | bool operator==(const EdgeID& other) const { 44 | return (robot_src == other.robot_src && frame_src == other.frame_src && 45 | robot_dst == other.robot_dst && frame_dst == other.frame_dst); 46 | } 47 | RobotId robot_src; 48 | RobotId robot_dst; 49 | PoseId frame_src; 50 | PoseId frame_dst; 51 | }; 52 | 53 | // Comparator for EdgeID 54 | struct CompareEdgeID { 55 | bool operator()(const EdgeID& a, const EdgeID& b) const { 56 | // Treat edge ID as an ordered tuple 57 | const auto ta = std::make_tuple(a.robot_src, a.robot_dst, a.frame_src, a.frame_dst); 58 | const auto tb = std::make_tuple(b.robot_src, b.robot_dst, b.frame_src, b.frame_dst); 59 | return ta < tb; 60 | } 61 | }; 62 | 63 | typedef cv::Mat OrbDescriptor; 64 | typedef std::vector OrbDescriptorVec; 65 | 66 | class VLCFrame { 67 | public: 68 | VLCFrame(); 69 | VLCFrame(const size_t& robot_id, 70 | const size_t& pose_id, 71 | const std::vector& keypoints_3d, 72 | const std::vector& versors, 73 | const OrbDescriptor& descriptors_mat); 74 | VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg); 75 | size_t robot_id_; 76 | size_t pose_id_; 77 | size_t submap_id_; // ID of the submap that contains this frame (pose) 78 | std::vector keypoints_; // 3D keypoints 79 | std::vector versors_; // bearing vector 80 | OrbDescriptorVec descriptors_vec_; 81 | OrbDescriptor descriptors_mat_; 82 | gtsam::Pose3 T_submap_pose_; // 3D pose in submap frame 83 | void initializeDescriptorsVector(); 84 | void toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const; 85 | // void pruneInvalidKeypoints(); 86 | }; // class VLCFrame 87 | 88 | struct PotentialVLCEdge { 89 | public: 90 | PotentialVLCEdge() {} 91 | PotentialVLCEdge(const RobotPoseId& vertex_src, 92 | const RobotPoseId& vertex_dst, 93 | double score = 0) 94 | : vertex_src_(vertex_src), vertex_dst_(vertex_dst), score_(score) {} 95 | RobotPoseId vertex_src_; 96 | RobotPoseId vertex_dst_; 97 | double score_; 98 | bool operator==(const PotentialVLCEdge& other) { 99 | return (vertex_src_ == other.vertex_src_ && vertex_dst_ == other.vertex_dst_); 100 | } 101 | }; // struct PotentialVLCEdge 102 | 103 | struct VLCEdge { 104 | public: 105 | VLCEdge() 106 | : stamp_ns_(0), normalized_bow_score_(0), mono_inliers_(0), stereo_inliers_(0) {} 107 | VLCEdge(const RobotPoseId& vertex_src, 108 | const RobotPoseId& vertex_dst, 109 | const gtsam::Pose3 T_src_dst) 110 | : vertex_src_(vertex_src), 111 | vertex_dst_(vertex_dst), 112 | T_src_dst_(T_src_dst), 113 | stamp_ns_(0), 114 | normalized_bow_score_(0), 115 | mono_inliers_(0), 116 | stereo_inliers_(0) {} 117 | 118 | RobotPoseId vertex_src_; 119 | RobotPoseId vertex_dst_; 120 | gtsam::Pose3 T_src_dst_; 121 | // Additional optional statistics for logging 122 | uint64_t stamp_ns_; // time this loop closure is detected 123 | double normalized_bow_score_; 124 | int mono_inliers_; 125 | int stereo_inliers_; 126 | bool operator==(const VLCEdge& other) { 127 | return (vertex_src_ == other.vertex_src_ && vertex_dst_ == other.vertex_dst_ && 128 | T_src_dst_.equals(other.T_src_dst_)); 129 | } 130 | }; // struct VLCEdge 131 | 132 | struct MatchIsland { 133 | MatchIsland() 134 | : start_id_(0), end_id_(0), island_score_(0), best_id_(0), best_score_(0) {} 135 | 136 | MatchIsland(const size_t& start, const size_t& end) 137 | : start_id_(start), end_id_(end), island_score_(0), best_id_(0), best_score_(0) {} 138 | 139 | MatchIsland(const size_t& start, const size_t& end, const double& score) 140 | : start_id_(start), 141 | end_id_(end), 142 | island_score_(score), 143 | best_id_(0), 144 | best_score_(0) {} 145 | 146 | inline bool operator<(const MatchIsland& other) const { 147 | return island_score_ < other.island_score_; 148 | } 149 | 150 | inline bool operator>(const MatchIsland& other) const { 151 | return island_score_ > other.island_score_; 152 | } 153 | 154 | inline size_t size() const { return end_id_ - start_id_ + 1; } 155 | 156 | inline void clear() { 157 | start_id_ = 0; 158 | end_id_ = 0; 159 | island_score_ = 0; 160 | best_id_ = 0; 161 | best_score_ = 0; 162 | } 163 | 164 | size_t start_id_; 165 | size_t end_id_; 166 | double island_score_; 167 | size_t best_id_; 168 | double best_score_; 169 | }; // struct MatchIsland 170 | 171 | struct LcdTpParams { 172 | int max_nrFrames_between_queries_; 173 | int max_nrFrames_between_islands_; 174 | int min_temporal_matches_; 175 | int max_intraisland_gap_; 176 | int min_matches_per_island_; 177 | 178 | bool equals(const LcdTpParams& other) const { 179 | return (max_nrFrames_between_queries_ == other.max_nrFrames_between_queries_ && 180 | max_nrFrames_between_islands_ == other.max_nrFrames_between_islands_ && 181 | min_temporal_matches_ == other.min_temporal_matches_ && 182 | max_intraisland_gap_ == other.max_intraisland_gap_ && 183 | min_matches_per_island_ == other.min_matches_per_island_); 184 | } 185 | 186 | bool operator==(const LcdTpParams& other) const { return equals(other); } 187 | }; 188 | 189 | struct LcdParams { 190 | std::string vocab_path_; 191 | // Only detect interrobot loop closures 192 | bool inter_robot_only_; 193 | 194 | // Parameters for visual loop closure detection 195 | float alpha_; 196 | int dist_local_; 197 | int max_db_results_; 198 | float min_nss_factor_; 199 | LcdTpParams lcd_tp_params_; 200 | 201 | // Parameters for geometric verification 202 | double ransac_threshold_mono_; 203 | double ransac_inlier_percentage_mono_; 204 | int max_ransac_iterations_mono_; 205 | 206 | int max_ransac_iterations_; 207 | double lowe_ratio_; 208 | double ransac_threshold_; 209 | double geometric_verification_min_inlier_count_; 210 | double geometric_verification_min_inlier_percentage_; 211 | 212 | bool equals(const LcdParams& other) const { 213 | return (vocab_path_ == other.vocab_path_ && 214 | inter_robot_only_ == other.inter_robot_only_ && alpha_ == other.alpha_ && 215 | dist_local_ == other.dist_local_ && 216 | max_db_results_ == other.max_db_results_ && 217 | min_nss_factor_ == other.min_nss_factor_ && 218 | lcd_tp_params_ == other.lcd_tp_params_ && 219 | ransac_threshold_mono_ == other.ransac_threshold_mono_ && 220 | ransac_inlier_percentage_mono_ == other.ransac_inlier_percentage_mono_ && 221 | max_ransac_iterations_mono_ == other.max_ransac_iterations_mono_ && 222 | max_ransac_iterations_ == other.max_ransac_iterations_ && 223 | lowe_ratio_ == other.lowe_ratio_ && 224 | ransac_threshold_ == other.ransac_threshold_ && 225 | geometric_verification_min_inlier_count_ == 226 | other.geometric_verification_min_inlier_count_ && 227 | geometric_verification_min_inlier_percentage_ == 228 | other.geometric_verification_min_inlier_percentage_); 229 | } 230 | 231 | bool operator==(const LcdParams& other) const { return equals(other); } 232 | }; 233 | 234 | typedef std::map > VLCFrameDict; 235 | 236 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /include/kimera_multi_lcd/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include "kimera_multi_lcd/types.h" 20 | 21 | namespace kimera_multi_lcd { 22 | void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg); 23 | 24 | void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg, 25 | DBoW2::BowVector* bow_vec); 26 | 27 | void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg); 28 | void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame); 29 | 30 | void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg); 31 | void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge); 32 | 33 | // Compute the payload size in a BowQuery message 34 | size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg); 35 | 36 | // Compute the payload size of a VLC frame 37 | size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg); 38 | 39 | } // namespace kimera_multi_lcd 40 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kimera_multi_lcd 4 | 1.0.0 5 | Kimera-Multi-LCD 6 | 7 | Yun Chang 8 | 9 | BSD 10 | 11 | catkin 12 | catkin_simple 13 | 14 | DBoW2 15 | dbow2_catkin 16 | gtsam 17 | Eigen 18 | OpenCV 19 | opengv 20 | roscpp 21 | std_msgs 22 | sensor_msgs 23 | geometry_msgs 24 | nav_msgs 25 | image_transport 26 | pose_graph_tools_msgs 27 | pcl_ros 28 | rosbag 29 | cv_bridge 30 | tf 31 | rosunit 32 | rostest 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/io.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include "kimera_multi_lcd/io.h" 8 | 9 | #include 10 | 11 | #include "kimera_multi_lcd/serializer.h" 12 | #include "kimera_multi_lcd/utils.h" 13 | 14 | namespace kimera_multi_lcd { 15 | 16 | using nlohmann::json; 17 | 18 | void saveBowVectors(const std::map& bow_vectors, 19 | const std::string& filename) { 20 | std::ofstream outfile(filename); 21 | json record; 22 | record["ids"] = json::array(); 23 | record["bow_vectors"] = json::array(); 24 | for (const auto& id_bowvec : bow_vectors) { 25 | record["ids"].push_back(id_bowvec.first); 26 | record["bow_vectors"].push_back(id_bowvec.second); 27 | } 28 | outfile << record.dump(); 29 | } 30 | 31 | void saveBowVectors(const std::map& bow_vectors, 32 | const std::string& filename) { 33 | std::map pg_bow_vectors; 34 | for (const auto& id_bow : bow_vectors) { 35 | pg_bow_vectors[id_bow.first] = pose_graph_tools_msgs::BowVector(); 36 | BowVectorToMsg(id_bow.second, &pg_bow_vectors[id_bow.first]); 37 | } 38 | saveBowVectors(pg_bow_vectors, filename); 39 | } 40 | 41 | void saveVLCFrames(const std::map& vlc_frames, 42 | const std::string& filename) { 43 | std::ofstream outfile(filename); 44 | json record; 45 | record["ids"] = json::array(); 46 | record["frames"] = json::array(); 47 | for (const auto& id_frame : vlc_frames) { 48 | record["ids"].push_back(id_frame.first); 49 | record["frames"].push_back(id_frame.second); 50 | } 51 | outfile << record.dump(); 52 | } 53 | 54 | void saveVLCFrames(const std::map& vlc_frames, 55 | const std::string& filename) { 56 | std::map pg_vlc_frames; 57 | for (const auto& id_frame : vlc_frames) { 58 | pg_vlc_frames[id_frame.first] = pose_graph_tools_msgs::VLCFrameMsg(); 59 | VLCFrameToMsg(id_frame.second, &pg_vlc_frames[id_frame.first]); 60 | } 61 | saveVLCFrames(pg_vlc_frames, filename); 62 | } 63 | 64 | // Save BoW vectors 65 | void loadBowVectors(const std::string& filename, 66 | std::map& bow_vectors) { 67 | std::stringstream ss; 68 | std::ifstream infile(filename); 69 | if (!infile) { 70 | throw std::runtime_error(filename + " does not exist"); 71 | } 72 | ss << infile.rdbuf(); 73 | 74 | const auto record = json::parse(ss.str()); 75 | 76 | for (size_t i = 0; i < record.at("ids").size(); i++) { 77 | bow_vectors[record.at("ids").at(i)] = record.at("bow_vectors").at(i); 78 | } 79 | } 80 | 81 | void loadBowVectors(const std::string& filename, 82 | std::map& bow_vectors) { 83 | std::map pg_bow_vectors; 84 | loadBowVectors(filename, pg_bow_vectors); 85 | 86 | for (const auto& id_bow : pg_bow_vectors) { 87 | bow_vectors[id_bow.first] = DBoW2::BowVector(); 88 | BowVectorFromMsg(id_bow.second, &bow_vectors[id_bow.first]); 89 | } 90 | } 91 | 92 | // Save VLC Frames 93 | void loadVLCFrames(const std::string& filename, 94 | std::map& vlc_frames) { 95 | std::stringstream ss; 96 | std::ifstream infile(filename); 97 | if (!infile) { 98 | throw std::runtime_error(filename + " does not exist"); 99 | } 100 | ss << infile.rdbuf(); 101 | 102 | const auto record = json::parse(ss.str()); 103 | 104 | for (size_t i = 0; i < record.at("ids").size(); i++) { 105 | vlc_frames[record.at("ids").at(i)] = record.at("frames").at(i); 106 | } 107 | } 108 | 109 | void loadVLCFrames(const std::string& filename, 110 | std::map& vlc_frames) { 111 | std::map pg_vlc_frames; 112 | loadVLCFrames(filename, pg_vlc_frames); 113 | for (const auto& id_frame : pg_vlc_frames) { 114 | vlc_frames[id_frame.first] = VLCFrame(); 115 | VLCFrameFromMsg(id_frame.second, &vlc_frames[id_frame.first]); 116 | } 117 | } 118 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /src/lcd_third_party.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************** 2 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 3. The original author of the work must be notified of any 14 | redistribution of source code or in binary form. 15 | 4. Neither the name of copyright holders nor the names of its 16 | contributors may be used to endorse or promote products derived 17 | from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 21 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 22 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 23 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | *********************************************************************************/ 31 | 32 | /* ---------------------------------------------------------------------------- 33 | * Copyright 2017, Massachusetts Institute of Technology, 34 | * Cambridge, MA 02139 35 | * All Rights Reserved 36 | * Authors: Luca Carlone, et al. (see THANKS for the full author list) 37 | * See LICENSE for the license information 38 | * -------------------------------------------------------------------------- */ 39 | 40 | /** 41 | * @file lcd_third_party.cpp 42 | * @brief Wrapper for functions of DLoopDetector (see copyright notice above). 43 | * @author Marcus Abate 44 | */ 45 | 46 | #include "kimera_multi_lcd/lcd_third_party.h" 47 | 48 | #include 49 | #include 50 | 51 | namespace kimera_multi_lcd { 52 | 53 | /* ------------------------------------------------------------------------ */ 54 | LcdThirdPartyWrapper::LcdThirdPartyWrapper(const LcdTpParams& params) 55 | : params_(params), 56 | temporal_entries_(0), 57 | latest_matched_island_(), 58 | latest_query_id_(size_t(0)) {} 59 | 60 | /* ------------------------------------------------------------------------ */ 61 | LcdThirdPartyWrapper::~LcdThirdPartyWrapper() {} 62 | 63 | /* ------------------------------------------------------------------------ */ 64 | bool LcdThirdPartyWrapper::checkTemporalConstraint(const size_t& id, 65 | const MatchIsland& island) { 66 | // temporal_entries_ starts at zero and counts the number of 67 | if (temporal_entries_ == 0 || 68 | static_cast(id - latest_query_id_) > params_.max_nrFrames_between_queries_) { 69 | temporal_entries_ = 1; 70 | } else { 71 | int a1 = static_cast(latest_matched_island_.start_id_); 72 | int a2 = static_cast(latest_matched_island_.end_id_); 73 | int b1 = static_cast(island.start_id_); 74 | int b2 = static_cast(island.end_id_); 75 | 76 | // Check that segments (a1, a2) and (b1, b2) have some overlap 77 | bool overlap = (b1 <= a1 && a1 <= b2) || (a1 <= b1 && b1 <= a2); 78 | bool gap_is_small = false; 79 | if (!overlap) { 80 | // Compute gap between segments (one of the two is negative) 81 | int d1 = static_cast(latest_matched_island_.start_id_) - 82 | static_cast(island.end_id_); 83 | int d2 = static_cast(island.start_id_) - 84 | static_cast(latest_matched_island_.end_id_); 85 | 86 | int gap = (d1 > d2 ? d1 : d2); // Choose positive gap 87 | gap_is_small = gap <= params_.max_nrFrames_between_islands_; 88 | } 89 | 90 | if (overlap || gap_is_small) { 91 | temporal_entries_++; 92 | } else { 93 | temporal_entries_ = 1; 94 | } 95 | } 96 | 97 | latest_matched_island_ = island; 98 | latest_query_id_ = id; 99 | return temporal_entries_ > params_.min_temporal_matches_; 100 | } 101 | 102 | /* ------------------------------------------------------------------------ */ 103 | void LcdThirdPartyWrapper::computeIslands(DBoW2::QueryResults* q, 104 | std::vector* islands) const { 105 | assert(NULL != q); 106 | assert(NULL != islands); 107 | islands->clear(); 108 | 109 | // The case of one island is easy to compute and is done separately 110 | if (q->size() == 1) { 111 | const DBoW2::Result& result = (*q)[0]; 112 | const DBoW2::EntryId& result_id = result.Id; 113 | MatchIsland island(result_id, result_id, result.Score); 114 | island.best_id_ = result_id; 115 | island.best_score_ = result.Score; 116 | islands->push_back(island); 117 | } else if (!q->empty()) { 118 | // sort query results in ascending order of frame ids 119 | std::sort(q->begin(), q->end(), DBoW2::Result::ltId); 120 | 121 | // create long enough islands 122 | DBoW2::QueryResults::const_iterator dit = q->begin(); 123 | int first_island_entry = static_cast(dit->Id); 124 | int last_island_entry = static_cast(dit->Id); 125 | 126 | // these are indices of q 127 | size_t i_first = 0; 128 | size_t i_last = 0; 129 | 130 | double best_score = dit->Score; 131 | DBoW2::EntryId best_entry = dit->Id; 132 | 133 | ++dit; 134 | for (size_t idx = 1; dit != q->end(); ++dit, ++idx) { 135 | if (static_cast(dit->Id) - last_island_entry < 136 | params_.max_intraisland_gap_) { 137 | last_island_entry = dit->Id; 138 | i_last = idx; 139 | if (dit->Score > best_score) { 140 | best_score = dit->Score; 141 | best_entry = dit->Id; 142 | } 143 | } else { 144 | // end of island reached 145 | int length = last_island_entry - first_island_entry + 1; 146 | if (length >= params_.min_matches_per_island_) { 147 | MatchIsland island = MatchIsland(first_island_entry, 148 | last_island_entry, 149 | computeIslandScore(*q, i_first, i_last)); 150 | 151 | islands->push_back(island); 152 | islands->back().best_score_ = best_score; 153 | islands->back().best_id_ = best_entry; 154 | } 155 | 156 | // prepare next island 157 | first_island_entry = last_island_entry = dit->Id; 158 | i_first = i_last = idx; 159 | best_score = dit->Score; 160 | best_entry = dit->Id; 161 | } 162 | } 163 | // add last island 164 | // TODO: do we need this? why isn't it handled in prev for loop? 165 | if (last_island_entry - first_island_entry + 1 >= params_.min_matches_per_island_) { 166 | MatchIsland island = MatchIsland(first_island_entry, 167 | last_island_entry, 168 | computeIslandScore(*q, i_first, i_last)); 169 | 170 | islands->push_back(island); 171 | islands->back().best_score_ = best_score; 172 | islands->back().best_id_ = static_cast(best_entry); 173 | } 174 | } 175 | } 176 | 177 | /* ------------------------------------------------------------------------ */ 178 | double LcdThirdPartyWrapper::computeIslandScore(const DBoW2::QueryResults& q, 179 | const size_t& start_id, 180 | const size_t& end_id) const { 181 | assert(q.size() > start_id); 182 | assert(q.size() > end_id); 183 | double score_sum = 0.0; 184 | for (size_t id = start_id; id <= end_id; id++) { 185 | score_sum += q.at(id).Score; 186 | } 187 | 188 | return score_sum; 189 | } 190 | 191 | } // namespace kimera_multi_lcd 192 | -------------------------------------------------------------------------------- /src/loop_closure_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using RansacProblem = 22 | opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem; 23 | using Adapter = opengv::relative_pose::CentralRelativeAdapter; 24 | using AdapterStereo = opengv::point_cloud::PointCloudAdapter; 25 | using RansacProblemStereo = opengv::sac_problems::point_cloud::PointCloudSacProblem; 26 | using BearingVectors = 27 | std::vector>; 28 | using DMatchVec = std::vector; 29 | 30 | namespace kimera_multi_lcd { 31 | 32 | LoopClosureDetector::LoopClosureDetector() : lcd_tp_wrapper_(nullptr) { 33 | // Track stats 34 | total_bow_matches_ = 0; 35 | total_geom_verifications_mono_ = 0; 36 | total_geometric_verifications_ = 0; 37 | } 38 | 39 | LoopClosureDetector::~LoopClosureDetector() {} 40 | 41 | void LoopClosureDetector::loadAndInitialize(const LcdParams& params) { 42 | params_ = params; 43 | lcd_tp_wrapper_ = std::unique_ptr( 44 | new LcdThirdPartyWrapper(params.lcd_tp_params_)); 45 | 46 | // Initiate orb matcher 47 | orb_feature_matcher_ = cv::DescriptorMatcher::create(3); 48 | 49 | // Initialize bag-of-word database 50 | vocab_.load(params.vocab_path_); 51 | } 52 | 53 | bool LoopClosureDetector::bowExists(const kimera_multi_lcd::RobotPoseId& id) const { 54 | RobotId robot_id = id.first; 55 | PoseId pose_id = id.second; 56 | if (bow_vectors_.find(robot_id) != bow_vectors_.end() && 57 | bow_vectors_.at(robot_id).find(pose_id) != bow_vectors_.at(robot_id).end()) { 58 | return true; 59 | } 60 | return false; 61 | } 62 | 63 | int LoopClosureDetector::numBoWForRobot(RobotId robot_id) const { 64 | if (bow_vectors_.find(robot_id) != bow_vectors_.end()) { 65 | return bow_vectors_.at(robot_id).size(); 66 | } 67 | return 0; 68 | } 69 | 70 | int LoopClosureDetector::latestPoseIdWithBoW(RobotId robot_id) const { 71 | if (numBoWForRobot(robot_id) == 0) { 72 | return -1; 73 | } 74 | return bow_latest_pose_id_.at(robot_id); 75 | } 76 | 77 | bool LoopClosureDetector::findPreviousBoWVector(const RobotPoseId& id, 78 | int window, 79 | DBoW2::BowVector* previous_bow) { 80 | CHECK_GE(window, 1); 81 | RobotId robot_id = id.first; 82 | PoseId pose_id = id.second; 83 | for (size_t i = 1; i <= static_cast(window); ++i) { 84 | if (i > pose_id) break; 85 | RobotPoseId prev_id(robot_id, pose_id - i); 86 | if (bowExists(prev_id)) { 87 | if (previous_bow) { 88 | *previous_bow = getBoWVector(prev_id); 89 | } 90 | return true; 91 | } 92 | } 93 | return false; 94 | } 95 | 96 | DBoW2::BowVector LoopClosureDetector::getBoWVector( 97 | const kimera_multi_lcd::RobotPoseId& id) const { 98 | CHECK(bowExists(id)); 99 | RobotId robot_id = id.first; 100 | PoseId pose_id = id.second; 101 | return bow_vectors_.at(robot_id).at(pose_id); 102 | } 103 | 104 | PoseBowVector LoopClosureDetector::getBoWVectors(const RobotId& robot_id) const { 105 | if (!bow_vectors_.count(robot_id)) { 106 | return PoseBowVector(); 107 | } 108 | return bow_vectors_.at(robot_id); 109 | } 110 | 111 | VLCFrame LoopClosureDetector::getVLCFrame( 112 | const kimera_multi_lcd::RobotPoseId& id) const { 113 | CHECK(frameExists(id)); 114 | return vlc_frames_.at(id); 115 | } 116 | 117 | std::map LoopClosureDetector::getVLCFrames( 118 | const RobotId& robot_id) const { 119 | std::map vlc_frames; 120 | for (const auto& robot_pose_id_vlc : vlc_frames_) { 121 | if (robot_pose_id_vlc.first.first == robot_id) { 122 | vlc_frames[robot_pose_id_vlc.first.second] = robot_pose_id_vlc.second; 123 | } 124 | } 125 | return vlc_frames; 126 | } 127 | 128 | void LoopClosureDetector::addBowVector(const RobotPoseId& id, 129 | const DBoW2::BowVector& bow_vector) { 130 | const size_t robot_id = id.first; 131 | const size_t pose_id = id.second; 132 | // Skip if this BoW vector has been added 133 | if (bowExists(id)) return; 134 | if (db_BoW_.find(robot_id) == db_BoW_.end()) { 135 | db_BoW_[robot_id] = std::unique_ptr(new OrbDatabase(vocab_)); 136 | bow_vectors_[robot_id] = PoseBowVector(); 137 | db_EntryId_to_PoseId_[robot_id] = std::unordered_map(); 138 | bow_latest_pose_id_[robot_id] = pose_id; 139 | ROS_INFO("Initialized BoW for robot %lu.", robot_id); 140 | } 141 | // Add Bow vector to the robot's database 142 | DBoW2::EntryId entry_id = db_BoW_[robot_id]->add(bow_vector); 143 | // Save the raw bow vectors 144 | bow_vectors_[robot_id][pose_id] = bow_vector; 145 | db_EntryId_to_PoseId_[robot_id][entry_id] = pose_id; 146 | // Update latest pose ID with BoW 147 | if (pose_id > bow_latest_pose_id_[robot_id]) { 148 | bow_latest_pose_id_[robot_id] = pose_id; 149 | } 150 | } 151 | 152 | bool LoopClosureDetector::detectLoopWithRobot(size_t robot, 153 | const RobotPoseId& vertex_query, 154 | const DBoW2::BowVector& bow_vector_query, 155 | std::vector* vertex_matches, 156 | std::vector* scores) { 157 | assert(NULL != vertex_matches); 158 | vertex_matches->clear(); 159 | if (scores) scores->clear(); 160 | 161 | // Return false if specified robot does not exist 162 | if (db_BoW_.find(robot) == db_BoW_.end()) return false; 163 | const OrbDatabase* db = db_BoW_.at(robot).get(); 164 | 165 | // Extract robot and pose id 166 | RobotId robot_query = vertex_query.first; 167 | PoseId pose_query = vertex_query.second; 168 | 169 | // If query and database from same robot 170 | if (params_.inter_robot_only_ && robot_query == robot) return false; 171 | 172 | // Try to locate BoW of previous frame 173 | if (pose_query == 0) return false; 174 | 175 | DBoW2::BowVector bow_vec_prev; 176 | if (!findPreviousBoWVector(vertex_query, 5, &bow_vec_prev)) { 177 | ROS_WARN("Cannot find previous BoW for query vertex (%lu,%lu).", 178 | robot_query, 179 | pose_query); 180 | return false; 181 | } 182 | // Compute nss factor with the previous keyframe of the query robot 183 | double nss_factor = db->getVocabulary()->score(bow_vector_query, bow_vec_prev); 184 | if (nss_factor < params_.min_nss_factor_) return false; 185 | 186 | // Query similar keyframes based on bow 187 | DBoW2::QueryResults query_result; 188 | db->query(bow_vector_query, query_result, params_.max_db_results_); 189 | 190 | // Sort query_result in descending score. 191 | // This should be done by the query function already, 192 | // but we do it again in case that behavior changes in the future. 193 | std::sort(query_result.begin(), query_result.end(), std::greater()); 194 | 195 | // Remove low scores from the QueryResults based on nss. 196 | DBoW2::QueryResults::iterator query_it = 197 | lower_bound(query_result.begin(), 198 | query_result.end(), 199 | DBoW2::Result(0, params_.alpha_ * nss_factor), 200 | DBoW2::Result::geq); 201 | if (query_it != query_result.end()) { 202 | query_result.resize(query_it - query_result.begin()); 203 | } 204 | 205 | if (!query_result.empty()) { 206 | DBoW2::Result best_result = query_result[0]; 207 | double normalized_score = best_result.Score / nss_factor; 208 | const PoseId best_match_pose_id = db_EntryId_to_PoseId_[robot][best_result.Id]; 209 | if (robot != robot_query) { 210 | vertex_matches->push_back(std::make_pair(robot, best_match_pose_id)); 211 | if (scores) scores->push_back(normalized_score); 212 | } else { 213 | // Check dist_local param 214 | int pose_query_int = (int)pose_query; 215 | int pose_match_int = (int)best_match_pose_id; 216 | if (std::abs(pose_query_int - pose_match_int) < params_.dist_local_) return false; 217 | // Compute islands in the matches. 218 | // An island is a group of matches with close frame_ids. 219 | std::vector islands; 220 | lcd_tp_wrapper_->computeIslands(&query_result, &islands); 221 | if (!islands.empty()) { 222 | // Check for temporal constraint if it is an single robot lc 223 | // Find the best island grouping using MatchIsland sorting. 224 | const MatchIsland& best_island = 225 | *std::max_element(islands.begin(), islands.end()); 226 | 227 | // Run temporal constraint check on this best island. 228 | bool pass_temporal_constraint = 229 | lcd_tp_wrapper_->checkTemporalConstraint(pose_query, best_island); 230 | if (pass_temporal_constraint) { 231 | vertex_matches->push_back(std::make_pair(robot, best_match_pose_id)); 232 | if (scores) scores->push_back(normalized_score); 233 | } 234 | } 235 | } 236 | } 237 | if (scores) CHECK_EQ(vertex_matches->size(), scores->size()); 238 | 239 | if (!vertex_matches->empty()) { 240 | total_bow_matches_ += vertex_matches->size(); 241 | return true; 242 | } 243 | return false; 244 | } 245 | 246 | bool LoopClosureDetector::detectLoop(const RobotPoseId& vertex_query, 247 | const DBoW2::BowVector& bow_vector_query, 248 | std::vector* vertex_matches, 249 | std::vector* scores) { 250 | assert(NULL != vertex_matches); 251 | vertex_matches->clear(); 252 | if (scores) scores->clear(); 253 | // Detect loop with every robot in the database 254 | for (const auto& db : db_BoW_) { 255 | std::vector vertex_matches_with_robot; 256 | std::vector scores_with_robot; 257 | if (detectLoopWithRobot(db.first, 258 | vertex_query, 259 | bow_vector_query, 260 | &vertex_matches_with_robot, 261 | &scores_with_robot)) { 262 | vertex_matches->insert(vertex_matches->end(), 263 | vertex_matches_with_robot.begin(), 264 | vertex_matches_with_robot.end()); 265 | if (scores) 266 | scores->insert( 267 | scores->end(), scores_with_robot.begin(), scores_with_robot.end()); 268 | } 269 | } 270 | if (scores) CHECK_EQ(vertex_matches->size(), scores->size()); 271 | if (!vertex_matches->empty()) return true; 272 | return false; 273 | } 274 | 275 | void LoopClosureDetector::computeMatchedIndices( 276 | const RobotPoseId& vertex_query, 277 | const RobotPoseId& vertex_match, 278 | std::vector* i_query, 279 | std::vector* i_match) const { 280 | assert(i_query != NULL); 281 | assert(i_match != NULL); 282 | i_query->clear(); 283 | i_match->clear(); 284 | 285 | // Get two best matches between frame descriptors. 286 | std::vector matches; 287 | 288 | VLCFrame frame_query = vlc_frames_.find(vertex_query)->second; 289 | VLCFrame frame_match = vlc_frames_.find(vertex_match)->second; 290 | 291 | try { 292 | orb_feature_matcher_->knnMatch( 293 | frame_query.descriptors_mat_, frame_match.descriptors_mat_, matches, 2u); 294 | } catch (cv::Exception& e) { 295 | ROS_ERROR("Failed KnnMatch in ComputeMatchedIndices. "); 296 | } 297 | 298 | const size_t& n_matches = matches.size(); 299 | for (size_t i = 0; i < n_matches; i++) { 300 | const DMatchVec& match = matches[i]; 301 | if (match.size() < 2) continue; 302 | if (match[0].distance < params_.lowe_ratio_ * match[1].distance) { 303 | i_query->push_back(match[0].queryIdx); 304 | i_match->push_back(match[0].trainIdx); 305 | } 306 | } 307 | } 308 | 309 | bool LoopClosureDetector::geometricVerificationNister( 310 | const RobotPoseId& vertex_query, 311 | const RobotPoseId& vertex_match, 312 | std::vector* inlier_query, 313 | std::vector* inlier_match, 314 | gtsam::Rot3* R_query_match) { 315 | assert(NULL != inlier_query); 316 | assert(NULL != inlier_match); 317 | 318 | total_geom_verifications_mono_++; 319 | 320 | std::vector i_query = *inlier_query; 321 | std::vector i_match = *inlier_match; 322 | 323 | BearingVectors query_versors, match_versors; 324 | 325 | query_versors.resize(i_query.size()); 326 | match_versors.resize(i_match.size()); 327 | for (size_t i = 0; i < i_match.size(); i++) { 328 | query_versors[i] = vlc_frames_[vertex_query].versors_.at(i_query[i]); 329 | match_versors[i] = vlc_frames_[vertex_match].versors_.at(i_match[i]); 330 | } 331 | 332 | Adapter adapter(query_versors, match_versors); 333 | 334 | // Use RANSAC to solve the central-relative-pose problem. 335 | opengv::sac::Ransac ransac; 336 | 337 | ransac.sac_model_ = 338 | std::make_shared(adapter, RansacProblem::Algorithm::NISTER, true); 339 | ransac.max_iterations_ = params_.max_ransac_iterations_mono_; 340 | ransac.threshold_ = params_.ransac_threshold_mono_; 341 | 342 | // Compute transformation via RANSAC. 343 | bool ransac_success = ransac.computeModel(); 344 | if (ransac_success) { 345 | double inlier_percentage = 346 | static_cast(ransac.inliers_.size()) / query_versors.size(); 347 | 348 | if (inlier_percentage >= params_.ransac_inlier_percentage_mono_) { 349 | if (R_query_match) { 350 | opengv::transformation_t monoT_query_match = ransac.model_coefficients_; 351 | *R_query_match = gtsam::Rot3(monoT_query_match.block<3, 3>(0, 0)); 352 | } 353 | 354 | inlier_query->clear(); 355 | inlier_match->clear(); 356 | for (auto idx : ransac.inliers_) { 357 | inlier_query->push_back(i_query[idx]); 358 | inlier_match->push_back(i_match[idx]); 359 | } 360 | return true; 361 | } 362 | } 363 | return false; 364 | } 365 | 366 | bool LoopClosureDetector::recoverPose(const RobotPoseId& vertex_query, 367 | const RobotPoseId& vertex_match, 368 | std::vector* inlier_query, 369 | std::vector* inlier_match, 370 | gtsam::Pose3* T_query_match, 371 | const gtsam::Rot3* R_query_match_prior) { 372 | CHECK_NOTNULL(inlier_query); 373 | CHECK_NOTNULL(inlier_match); 374 | total_geometric_verifications_++; 375 | std::vector i_query; // input indices to stereo ransac 376 | std::vector i_match; 377 | 378 | opengv::points_t f_match, f_query; 379 | for (size_t i = 0; i < inlier_match->size(); i++) { 380 | gtsam::Vector3 point_query = 381 | vlc_frames_[vertex_query].keypoints_.at(inlier_query->at(i)); 382 | gtsam::Vector3 point_match = 383 | vlc_frames_[vertex_match].keypoints_.at(inlier_match->at(i)); 384 | if (point_query.norm() > 1e-3 && point_match.norm() > 1e-3) { 385 | f_query.push_back(point_query); 386 | f_match.push_back(point_match); 387 | i_query.push_back(inlier_query->at(i)); 388 | i_match.push_back(inlier_match->at(i)); 389 | } 390 | } 391 | 392 | if (f_query.size() < 3) { 393 | // ROS_INFO("Less than 3 putative correspondences."); 394 | return false; 395 | } 396 | 397 | AdapterStereo adapter(f_query, f_match); 398 | if (R_query_match_prior) { 399 | // Use input rotation estimate as prior 400 | adapter.setR12(R_query_match_prior->matrix()); 401 | } 402 | 403 | // Compute transform using RANSAC 3-point method (Arun). 404 | std::shared_ptr ptcloudproblem_ptr( 405 | new RansacProblemStereo(adapter, true)); 406 | opengv::sac::Ransac ransac; 407 | ransac.sac_model_ = ptcloudproblem_ptr; 408 | ransac.max_iterations_ = params_.max_ransac_iterations_; 409 | ransac.threshold_ = params_.ransac_threshold_; 410 | 411 | // Compute transformation via RANSAC. 412 | bool ransac_success = ransac.computeModel(); 413 | 414 | if (ransac_success) { 415 | if (ransac.inliers_.size() < params_.geometric_verification_min_inlier_count_) { 416 | // ROS_INFO_STREAM("Number of inlier correspondences after RANSAC " 417 | // << ransac.inliers_.size() << " is too low."); 418 | return false; 419 | } 420 | 421 | double inlier_percentage = 422 | static_cast(ransac.inliers_.size()) / f_match.size(); 423 | if (inlier_percentage < params_.geometric_verification_min_inlier_percentage_) { 424 | // ROS_INFO_STREAM("Percentage of inlier correspondences after RANSAC " 425 | // << inlier_percentage << " is too low."); 426 | return false; 427 | } 428 | 429 | opengv::transformation_t T = ransac.model_coefficients_; 430 | 431 | gtsam::Point3 estimated_translation(T(0, 3), T(1, 3), T(2, 3)); 432 | 433 | // Output is the 3D transformation from the match frame to the query frame 434 | *T_query_match = gtsam::Pose3(gtsam::Rot3(T.block<3, 3>(0, 0)), 435 | gtsam::Point3(T(0, 3), T(1, 3), T(2, 3))); 436 | 437 | // Populate inlier indices 438 | inlier_query->clear(); 439 | inlier_match->clear(); 440 | for (auto idx : ransac.inliers_) { 441 | inlier_query->push_back(i_query[idx]); 442 | inlier_match->push_back(i_match[idx]); 443 | } 444 | 445 | return true; 446 | } 447 | 448 | return false; 449 | } 450 | 451 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /src/serializer.cpp: -------------------------------------------------------------------------------- 1 | #include "kimera_multi_lcd/serializer.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | using json = nlohmann::json; 9 | 10 | namespace pcl { 11 | void to_json(json& j, const pcl::PointXYZ& point) { 12 | j = json{{"x", point.x}, {"y", point.y}, {"z", point.z}}; 13 | } 14 | 15 | void from_json(const json& j, pcl::PointXYZ& point) { 16 | point.x = j.at("x").is_null() ? std::numeric_limits::quiet_NaN() 17 | : j.at("x").get(); 18 | point.y = j.at("y").is_null() ? std::numeric_limits::quiet_NaN() 19 | : j.at("y").get(); 20 | point.z = j.at("z").is_null() ? std::numeric_limits::quiet_NaN() 21 | : j.at("z").get(); 22 | } 23 | 24 | void to_json(json& j, const pcl::PointCloud& points) { 25 | j = json::array(); 26 | for (const auto& point : points) { 27 | json point_json = point; 28 | j.push_back(point_json); 29 | } 30 | } 31 | 32 | void from_json(const json& j, pcl::PointCloud& points) { 33 | for (const auto& point : j) { 34 | points.push_back(point.get()); 35 | } 36 | } 37 | 38 | } // namespace pcl 39 | 40 | namespace pose_graph_tools_msgs { 41 | 42 | void to_json(json& j, const pose_graph_tools_msgs::BowVector& bow_vector) { 43 | j = json{{"word_ids", bow_vector.word_ids}, {"word_values", bow_vector.word_values}}; 44 | } 45 | 46 | void from_json(const json& j, pose_graph_tools_msgs::BowVector& bow_vector) { 47 | j.at("word_ids").get_to(bow_vector.word_ids); 48 | j.at("word_values").get_to(bow_vector.word_values); 49 | } 50 | 51 | void to_json(json& j, const pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) { 52 | pcl::PointCloud versors; 53 | pcl::fromROSMsg(vlc_frame.versors, versors); 54 | 55 | j = json{{"robot_id", vlc_frame.robot_id}, 56 | {"pose_id", vlc_frame.pose_id}, 57 | {"submap_id", vlc_frame.submap_id}, 58 | {"descriptors_mat", 59 | {{"height", vlc_frame.descriptors_mat.height}, 60 | {"width", vlc_frame.descriptors_mat.width}, 61 | {"encoding", vlc_frame.descriptors_mat.encoding}, 62 | {"step", vlc_frame.descriptors_mat.step}, 63 | {"data", vlc_frame.descriptors_mat.data}}}, 64 | {"versors", versors}, 65 | {"depths", vlc_frame.depths}, 66 | {"T_submap_pose", 67 | {{"x", vlc_frame.T_submap_pose.position.x}, 68 | {"y", vlc_frame.T_submap_pose.position.y}, 69 | {"z", vlc_frame.T_submap_pose.position.z}, 70 | {"qx", vlc_frame.T_submap_pose.orientation.x}, 71 | {"qy", vlc_frame.T_submap_pose.orientation.y}, 72 | {"qz", vlc_frame.T_submap_pose.orientation.z}, 73 | {"qw", vlc_frame.T_submap_pose.orientation.w}}}}; 74 | } 75 | 76 | void from_json(const json& j, pose_graph_tools_msgs::VLCFrameMsg& vlc_frame) { 77 | j.at("robot_id").get_to(vlc_frame.robot_id); 78 | j.at("pose_id").get_to(vlc_frame.pose_id); 79 | j.at("submap_id").get_to(vlc_frame.submap_id); 80 | j.at("descriptors_mat").at("height").get_to(vlc_frame.descriptors_mat.height); 81 | j.at("descriptors_mat").at("width").get_to(vlc_frame.descriptors_mat.width); 82 | j.at("descriptors_mat").at("encoding").get_to(vlc_frame.descriptors_mat.encoding); 83 | j.at("descriptors_mat").at("step").get_to(vlc_frame.descriptors_mat.step); 84 | j.at("descriptors_mat").at("data").get_to(vlc_frame.descriptors_mat.data); 85 | 86 | pcl::PointCloud versors; 87 | j.at("versors").get_to(versors); 88 | pcl::toROSMsg(versors, vlc_frame.versors); 89 | 90 | j.at("depths").get_to(vlc_frame.depths); 91 | 92 | geometry_msgs::Point& T_submap_t = vlc_frame.T_submap_pose.position; 93 | geometry_msgs::Quaternion& T_submap_R = vlc_frame.T_submap_pose.orientation; 94 | j.at("T_submap_pose").at("x").get_to(T_submap_t.x); 95 | j.at("T_submap_pose").at("y").get_to(T_submap_t.y); 96 | j.at("T_submap_pose").at("z").get_to(T_submap_t.z); 97 | j.at("T_submap_pose").at("qx").get_to(T_submap_R.x); 98 | j.at("T_submap_pose").at("qy").get_to(T_submap_R.y); 99 | j.at("T_submap_pose").at("qz").get_to(T_submap_R.z); 100 | j.at("T_submap_pose").at("qw").get_to(T_submap_R.w); 101 | } 102 | } // namespace pose_graph_tools_msgs 103 | -------------------------------------------------------------------------------- /src/types.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | #include 7 | #include 8 | 9 | #include "kimera_multi_lcd/types.h" 10 | 11 | namespace kimera_multi_lcd { 12 | 13 | VLCFrame::VLCFrame() {} 14 | 15 | VLCFrame::VLCFrame(const RobotId& robot_id, 16 | const PoseId& pose_id, 17 | const std::vector& keypoints_3d, 18 | const std::vector& versors, 19 | const OrbDescriptor& descriptors_mat) 20 | : robot_id_(robot_id), 21 | pose_id_(pose_id), 22 | keypoints_(keypoints_3d), 23 | versors_(versors), 24 | descriptors_mat_(descriptors_mat) { 25 | assert(keypoints_.size() == descriptors_mat_.size().height); 26 | initializeDescriptorsVector(); 27 | } 28 | 29 | VLCFrame::VLCFrame(const pose_graph_tools_msgs::VLCFrameMsg& msg) 30 | : robot_id_(msg.robot_id), pose_id_(msg.pose_id), submap_id_(msg.submap_id) { 31 | T_submap_pose_ = gtsam::Pose3(gtsam::Rot3(msg.T_submap_pose.orientation.w, 32 | msg.T_submap_pose.orientation.x, 33 | msg.T_submap_pose.orientation.y, 34 | msg.T_submap_pose.orientation.z), 35 | gtsam::Point3(msg.T_submap_pose.position.x, 36 | msg.T_submap_pose.position.y, 37 | msg.T_submap_pose.position.z)); 38 | 39 | // Convert versors and 3D keypoints 40 | if (!msg.versors.data.empty()) { 41 | pcl::PointCloud versors; 42 | pcl::fromROSMsg(msg.versors, versors); 43 | for (size_t i = 0; i < versors.size(); ++i) { 44 | gtsam::Vector3 v(versors[i].x, versors[i].y, versors[i].z); 45 | versors_.push_back(v); 46 | const auto depth = msg.depths[i]; 47 | if (depth < 1e-3) { 48 | // Depth is invalid for this keypoint and the 3D keypoint is set to zero. 49 | // Zero keypoints will not be used during stereo RANSAC. 50 | keypoints_.push_back(gtsam::Vector3::Zero()); 51 | } else { 52 | // Depth is valid for this keypoint. 53 | // We can recover the 3D point by multiplying with the bearing vector 54 | // See sparseStereoReconstruction function in Stereo Matcher in Kimera-VIO. 55 | keypoints_.push_back(depth * v / v(2)); 56 | } 57 | } 58 | } else { 59 | ROS_WARN("[VLCFrame] Empty versors!"); 60 | } 61 | 62 | // Convert descriptors 63 | try { 64 | sensor_msgs::ImageConstPtr ros_image_ptr( 65 | new sensor_msgs::Image(msg.descriptors_mat)); 66 | descriptors_mat_ = 67 | cv_bridge::toCvCopy(ros_image_ptr, sensor_msgs::image_encodings::TYPE_8UC1) 68 | ->image; 69 | initializeDescriptorsVector(); 70 | } catch (...) { 71 | ROS_WARN("[VLCFrame] Failed to read descriptors!"); 72 | } 73 | } 74 | 75 | void VLCFrame::toROSMessage(pose_graph_tools_msgs::VLCFrameMsg* msg) const { 76 | msg->robot_id = robot_id_; 77 | msg->pose_id = pose_id_; 78 | 79 | // Convert submap info 80 | msg->submap_id = submap_id_; 81 | geometry_msgs::Pose pose; 82 | const gtsam::Point3& position = T_submap_pose_.translation(); 83 | const gtsam::Quaternion& orientation = T_submap_pose_.rotation().toQuaternion(); 84 | pose.position.x = position.x(); 85 | pose.position.y = position.y(); 86 | pose.position.z = position.z(); 87 | pose.orientation.x = orientation.x(); 88 | pose.orientation.y = orientation.y(); 89 | pose.orientation.z = orientation.z(); 90 | pose.orientation.w = orientation.w(); 91 | msg->T_submap_pose = pose; 92 | 93 | // Convert keypoints 94 | pcl::PointCloud versors; 95 | for (size_t i = 0; i < keypoints_.size(); ++i) { 96 | // Push bearing vector 97 | gtsam::Vector3 v_ = versors_[i]; 98 | pcl::PointXYZ v(v_(0), v_(1), v_(2)); 99 | versors.push_back(v); 100 | // Push keypoint depth 101 | gtsam::Vector3 p_ = keypoints_[i]; 102 | if (p_.norm() < 1e-3) { 103 | // This 3D keypoint is not valid 104 | msg->depths.push_back(0); 105 | } else { 106 | // We have valid 3D keypoint and the depth is given by the z component 107 | // See sparseStereoReconstruction function in Stereo Matcher in 108 | // Kimera-VIO. 109 | msg->depths.push_back(p_[2]); 110 | } 111 | } 112 | pcl::toROSMsg(versors, msg->versors); 113 | 114 | // Convert descriptors 115 | assert(descriptors_mat_.type() == 116 | CV_8UC1); // check that the matrix is of type CV_8U 117 | cv_bridge::CvImage cv_img; 118 | // cv_img.header = in_msg->header; // Yulun: need to set header explicitly? 119 | cv_img.encoding = sensor_msgs::image_encodings::TYPE_8UC1; 120 | cv_img.image = descriptors_mat_; 121 | cv_img.toImageMsg(msg->descriptors_mat); 122 | } 123 | 124 | void VLCFrame::initializeDescriptorsVector() { 125 | descriptors_vec_.clear(); 126 | // Create vector of descriptors 127 | int L = descriptors_mat_.size().width; 128 | descriptors_vec_.resize(descriptors_mat_.size().height); 129 | 130 | for (size_t i = 0; i < descriptors_vec_.size(); i++) { 131 | descriptors_vec_[i] = cv::Mat(1, L, descriptors_mat_.type()); // one row only 132 | descriptors_mat_.row(i).copyTo(descriptors_vec_[i].row(0)); 133 | } 134 | } 135 | 136 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #include "kimera_multi_lcd/utils.h" 8 | 9 | #include 10 | 11 | namespace kimera_multi_lcd { 12 | 13 | void BowVectorToMsg(const DBoW2::BowVector& bow_vec, pose_graph_tools_msgs::BowVector* msg) { 14 | msg->word_ids.clear(); 15 | msg->word_values.clear(); 16 | for (auto it = bow_vec.begin(); it != bow_vec.end(); ++it) { 17 | msg->word_ids.push_back(it->first); 18 | msg->word_values.push_back(it->second); 19 | } 20 | } 21 | 22 | void BowVectorFromMsg(const pose_graph_tools_msgs::BowVector& msg, 23 | DBoW2::BowVector* bow_vec) { 24 | assert(msg.word_ids.size() == msg.word_values.size()); 25 | bow_vec->clear(); 26 | for (size_t i = 0; i < msg.word_ids.size(); ++i) { 27 | bow_vec->addWeight(msg.word_ids[i], msg.word_values[i]); 28 | } 29 | } 30 | 31 | void VLCFrameToMsg(const VLCFrame& frame, pose_graph_tools_msgs::VLCFrameMsg* msg) { 32 | frame.toROSMessage(msg); 33 | } 34 | 35 | void VLCFrameFromMsg(const pose_graph_tools_msgs::VLCFrameMsg& msg, VLCFrame* frame) { 36 | *frame = VLCFrame(msg); 37 | } 38 | 39 | void VLCEdgeToMsg(const VLCEdge& edge, pose_graph_tools_msgs::PoseGraphEdge* msg) { 40 | // Yulun: this function currently does not assign covariance! 41 | 42 | msg->robot_from = edge.vertex_src_.first; 43 | msg->key_from = edge.vertex_src_.second; 44 | msg->robot_to = edge.vertex_dst_.first; 45 | msg->key_to = edge.vertex_dst_.second; 46 | msg->type = pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE; 47 | 48 | gtsam::Pose3 pose = edge.T_src_dst_; 49 | gtsam::Quaternion quat = pose.rotation().toQuaternion(); 50 | gtsam::Point3 position = pose.translation(); 51 | 52 | msg->pose.orientation.x = quat.x(); 53 | msg->pose.orientation.y = quat.y(); 54 | msg->pose.orientation.z = quat.z(); 55 | msg->pose.orientation.w = quat.w(); 56 | 57 | msg->pose.position.x = position.x(); 58 | msg->pose.position.y = position.y(); 59 | msg->pose.position.z = position.z(); 60 | } 61 | 62 | void VLCEdgeFromMsg(const pose_graph_tools_msgs::PoseGraphEdge& msg, VLCEdge* edge) { 63 | edge->vertex_src_ = std::make_pair(msg.robot_from, msg.key_from); 64 | edge->vertex_dst_ = std::make_pair(msg.robot_to, msg.key_to); 65 | 66 | gtsam::Rot3 rotation(msg.pose.orientation.w, 67 | msg.pose.orientation.x, 68 | msg.pose.orientation.y, 69 | msg.pose.orientation.z); 70 | 71 | gtsam::Point3 position(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); 72 | 73 | gtsam::Pose3 T_src_dst(rotation, position); 74 | edge->T_src_dst_ = T_src_dst; 75 | } 76 | 77 | size_t computeBowQueryPayloadBytes(const pose_graph_tools_msgs::BowQuery& msg) { 78 | size_t bytes = 0; 79 | bytes += sizeof(msg.robot_id); 80 | bytes += sizeof(msg.pose_id); 81 | bytes += sizeof(msg.bow_vector.word_ids[0]) * msg.bow_vector.word_ids.size(); 82 | bytes += sizeof(msg.bow_vector.word_values[0]) * msg.bow_vector.word_values.size(); 83 | return bytes; 84 | } 85 | 86 | size_t computeVLCFramePayloadBytes(const pose_graph_tools_msgs::VLCFrameMsg& msg) { 87 | size_t bytes = 0; 88 | bytes += sizeof(msg.robot_id); 89 | bytes += sizeof(msg.pose_id); 90 | // descriptors 91 | bytes += sizeof(msg.descriptors_mat); 92 | bytes += sizeof(msg.descriptors_mat.data[0]) * msg.descriptors_mat.data.size(); 93 | // keypoints 94 | bytes += sizeof(msg.versors); 95 | bytes += sizeof(msg.versors.data[0]) * msg.versors.data.size(); 96 | return bytes; 97 | } 98 | 99 | } // namespace kimera_multi_lcd -------------------------------------------------------------------------------- /test/test_config.h.in: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Path to the library 4 | // #define DATASET_PATH "@TEST_DATA_PATH@" 5 | #define VOCAB_PATH "@TEST_VOCAB_PATH@" -------------------------------------------------------------------------------- /test/test_lcd.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "kimera_multi_lcd/LoopClosureDetector.h" 13 | #include "kimera_multi_lcd/types.h" 14 | #include "kimera_multi_lcd/utils.h" 15 | #include "test_config.h" 16 | 17 | namespace kimera_multi_lcd { 18 | 19 | LcdParams defaultLcdTestParams() { 20 | LcdParams params; 21 | params.vocab_path_ = std::string(VOCAB_PATH) + "/mit_voc.yml"; 22 | params.inter_robot_only_ = true; 23 | params.alpha_ = 0.3; 24 | params.dist_local_ = 90; 25 | params.max_db_results_ = 2; 26 | params.min_nss_factor_ = 0.05; 27 | 28 | params.lcd_tp_params_.max_nrFrames_between_queries_ = 2; 29 | params.lcd_tp_params_.max_nrFrames_between_islands_ = 3; 30 | params.lcd_tp_params_.min_temporal_matches_ = 3; 31 | params.lcd_tp_params_.max_intraisland_gap_ = 3; 32 | params.lcd_tp_params_.min_matches_per_island_ = 1; 33 | 34 | params.ransac_threshold_mono_ = 1.0e-06; 35 | params.ransac_inlier_percentage_mono_ = 0.01; 36 | params.max_ransac_iterations_mono_ = 1000; 37 | params.max_ransac_iterations_ = 1000; 38 | params.lowe_ratio_ = 0.9; 39 | params.ransac_threshold_ = 0.01; 40 | params.geometric_verification_min_inlier_count_ = 5; 41 | params.geometric_verification_min_inlier_percentage_ = 0.0; 42 | return params; 43 | } 44 | 45 | DBoW2::BowVector constructBowVector() { 46 | DBoW2::BowVector bow_vec; 47 | for (size_t i = 0; i < 100; i++) { 48 | bow_vec.addWeight(i, static_cast(i) * 1.0); 49 | } 50 | return bow_vec; 51 | } 52 | 53 | VLCFrame constructVLCFrame(const RobotId& robot_id, const PoseId& pose_id) { 54 | std::vector keypoints, versors; 55 | OrbDescriptor descriptors_mat = cv::Mat::eye(10, 10, CV_8UC1); 56 | for (size_t i = 0; i < 10; i++) { 57 | gtsam::Vector3 keypt(static_cast(i) * 0.1 + 0.1, 58 | static_cast(i) * 0.1 + 0.2, 59 | static_cast(i) * 0.1 + 0.3); 60 | keypoints.push_back(keypt); 61 | versors.push_back(keypt / keypt(2)); 62 | } 63 | 64 | VLCFrame frame(robot_id, pose_id, keypoints, versors, descriptors_mat); 65 | return frame; 66 | } 67 | 68 | TEST(LcdTest, LoadParams) { 69 | LoopClosureDetector lcd_test; 70 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 71 | EXPECT_EQ(lcd_test.getParams(), defaultLcdTestParams()); 72 | } 73 | 74 | TEST(LcdTest, AddBowVector) { 75 | LoopClosureDetector lcd_test; 76 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 77 | 78 | DBoW2::BowVector bow_vec = constructBowVector(); 79 | RobotPoseId id(0, 0); 80 | lcd_test.addBowVector(id, bow_vec); 81 | EXPECT_TRUE(lcd_test.bowExists(id)); 82 | } 83 | 84 | TEST(LcdTest, FindPreviousBoWVector) { 85 | LoopClosureDetector lcd_test; 86 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 87 | 88 | DBoW2::BowVector bow_vec_0 = constructBowVector(); 89 | RobotPoseId id_0(0, 0); 90 | lcd_test.addBowVector(id_0, bow_vec_0); 91 | 92 | DBoW2::BowVector bow_vec_1; 93 | bow_vec_1.addWeight(1, 1.0); 94 | bow_vec_1.addWeight(2, 1.0); 95 | RobotPoseId id_1(0, 1); 96 | lcd_test.addBowVector(id_1, bow_vec_1); 97 | 98 | DBoW2::BowVector bow_vec_out; 99 | lcd_test.findPreviousBoWVector(id_1, 2, &bow_vec_out); 100 | EXPECT_EQ(bow_vec_out, bow_vec_0); 101 | } 102 | 103 | TEST(LcdTest, DetectLoop) { 104 | LoopClosureDetector lcd_test; 105 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 106 | DBoW2::BowVector bow_vec = constructBowVector(); 107 | 108 | RobotPoseId id_0(0, 0); 109 | lcd_test.addBowVector(id_0, bow_vec); 110 | 111 | RobotPoseId id_1(1, 0); 112 | lcd_test.addBowVector(id_1, bow_vec); // To compute NSS 113 | 114 | RobotPoseId id_2(1, 1); 115 | std::vector matches; 116 | std::vector scores; 117 | bool detected_loop = 118 | lcd_test.detectLoopWithRobot(0, id_2, bow_vec, &matches, &scores); 119 | EXPECT_EQ(1, matches.size()); 120 | EXPECT_TRUE(detected_loop); 121 | } 122 | 123 | TEST(LcdTest, AddVLCFrame) { 124 | LoopClosureDetector lcd_test; 125 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 126 | 127 | VLCFrame vlc_frame = constructVLCFrame(0, 0); 128 | RobotPoseId id_0(0, 0); 129 | lcd_test.addVLCFrame(id_0, vlc_frame); 130 | 131 | EXPECT_TRUE(lcd_test.frameExists(id_0)); 132 | 133 | VLCFrame vlc_frame_out = lcd_test.getVLCFrame(id_0); 134 | EXPECT_EQ(vlc_frame_out.robot_id_, vlc_frame.robot_id_); 135 | EXPECT_EQ(vlc_frame_out.pose_id_, vlc_frame.pose_id_); 136 | EXPECT_LE((vlc_frame_out.keypoints_[0] - vlc_frame.keypoints_[0]).norm(), 1e-4); 137 | EXPECT_LE((vlc_frame_out.keypoints_[1] - vlc_frame.keypoints_[1]).norm(), 1e-4); 138 | EXPECT_LE((vlc_frame_out.versors_[0] - vlc_frame.versors_[0]).norm(), 1e-4); 139 | EXPECT_LE((vlc_frame_out.versors_[1] - vlc_frame.versors_[1]).norm(), 1e-4); 140 | EXPECT_LE(cv::norm(vlc_frame_out.descriptors_mat_ - vlc_frame.descriptors_mat_), 141 | 1e-4); 142 | for (size_t i = 0; i < vlc_frame.descriptors_mat_.size().height; ++i) { 143 | EXPECT_LE( 144 | cv::norm(vlc_frame_out.descriptors_vec_[i] - vlc_frame.descriptors_mat_.row(i)), 145 | 1e-4); 146 | } 147 | } 148 | 149 | TEST(LcdTest, ComputeMatchedIndices) { 150 | LoopClosureDetector lcd_test; 151 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 152 | 153 | VLCFrame vlc_frame_0 = constructVLCFrame(0, 0); 154 | RobotPoseId id_0(0, 0); 155 | lcd_test.addVLCFrame(id_0, vlc_frame_0); 156 | 157 | VLCFrame vlc_frame_1 = constructVLCFrame(1, 0); 158 | RobotPoseId id_1(1, 0); 159 | lcd_test.addVLCFrame(id_1, vlc_frame_1); 160 | 161 | std::vector idx_query, idx_match; 162 | lcd_test.computeMatchedIndices(id_1, id_0, &idx_query, &idx_match); 163 | EXPECT_EQ(idx_query.size(), 10); 164 | EXPECT_EQ(idx_match.size(), 10); 165 | } 166 | 167 | TEST(LcdTest, GeometricVerificationNister) { 168 | LoopClosureDetector lcd_test; 169 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 170 | 171 | VLCFrame vlc_frame_0 = constructVLCFrame(0, 0); 172 | RobotPoseId id_0(0, 0); 173 | lcd_test.addVLCFrame(id_0, vlc_frame_0); 174 | 175 | VLCFrame vlc_frame_1 = constructVLCFrame(1, 0); 176 | RobotPoseId id_1(1, 0); 177 | lcd_test.addVLCFrame(id_1, vlc_frame_1); 178 | 179 | std::vector idx_query, idx_match; 180 | lcd_test.computeMatchedIndices(id_1, id_0, &idx_query, &idx_match); 181 | 182 | gtsam::Rot3 R_1_0; 183 | bool pass_geometric_verification = 184 | lcd_test.geometricVerificationNister(id_1, id_0, &idx_query, &idx_match, &R_1_0); 185 | EXPECT_TRUE(pass_geometric_verification); 186 | EXPECT_TRUE(gtsam::assert_equal(gtsam::Rot3(), R_1_0, 1e-1)); 187 | EXPECT_EQ(idx_query.size(), 10); 188 | EXPECT_EQ(idx_match.size(), 10); 189 | } 190 | 191 | TEST(LcdTest, RecoverPose) { 192 | LoopClosureDetector lcd_test; 193 | lcd_test.loadAndInitialize(defaultLcdTestParams()); 194 | 195 | VLCFrame vlc_frame_0 = constructVLCFrame(0, 0); 196 | RobotPoseId id_0(0, 0); 197 | lcd_test.addVLCFrame(id_0, vlc_frame_0); 198 | 199 | VLCFrame vlc_frame_1 = constructVLCFrame(1, 0); 200 | RobotPoseId id_1(1, 0); 201 | lcd_test.addVLCFrame(id_1, vlc_frame_1); 202 | 203 | std::vector idx_query, idx_match; 204 | lcd_test.computeMatchedIndices(id_1, id_0, &idx_query, &idx_match); 205 | 206 | gtsam::Rot3 R_1_0; 207 | bool pass_geometric_verification = 208 | lcd_test.geometricVerificationNister(id_1, id_0, &idx_query, &idx_match, &R_1_0); 209 | 210 | gtsam::Pose3 T_1_0; 211 | bool recovered_pose = 212 | lcd_test.recoverPose(id_1, id_0, &idx_query, &idx_match, &T_1_0, &R_1_0); 213 | EXPECT_TRUE(recovered_pose); 214 | EXPECT_TRUE(gtsam::assert_equal(gtsam::Pose3(), T_1_0, 1e-2)); 215 | } 216 | 217 | } // namespace kimera_multi_lcd 218 | 219 | int main(int argc, char** argv) { 220 | ::testing::InitGoogleTest(&argc, argv); 221 | return RUN_ALL_TESTS(); 222 | } 223 | -------------------------------------------------------------------------------- /test/test_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "kimera_multi_lcd/types.h" 13 | #include "kimera_multi_lcd/utils.h" 14 | 15 | namespace kimera_multi_lcd { 16 | 17 | TEST(UtilsTest, BowVector) { 18 | DBoW2::BowVector bow_vec; 19 | pose_graph_tools::BowVector msg; 20 | 21 | bow_vec.addWeight(1, 1.0); 22 | bow_vec.addWeight(1, 2.0); 23 | bow_vec.addWeight(2, 2.5); 24 | 25 | BowVectorToMsg(bow_vec, &msg); 26 | DBoW2::BowVector bow_vec_out; 27 | BowVectorFromMsg(msg, &bow_vec_out); 28 | 29 | EXPECT_EQ(bow_vec, bow_vec_out); 30 | } 31 | 32 | TEST(UtilsTest, VLCFrameConstruction) { 33 | RobotId robot_id = 0; 34 | PoseId pose_id = 0; 35 | std::vector keypoints, versors; 36 | gtsam::Vector3 p0(0.1, 0.2, 0.3); 37 | gtsam::Vector3 p1(0.9, 0.8, 0.7); 38 | keypoints.push_back(p0); 39 | keypoints.push_back(p1); 40 | versors.push_back(p0 / p0(2)); 41 | versors.push_back(p1 / p1(2)); 42 | 43 | OrbDescriptor descriptors_mat(2, 7, CV_8UC1, 1); 44 | 45 | VLCFrame frame(robot_id, pose_id, keypoints, versors, descriptors_mat); 46 | 47 | EXPECT_EQ(frame.robot_id_, robot_id); 48 | EXPECT_EQ(frame.pose_id_, pose_id); 49 | EXPECT_LE((frame.keypoints_[0] - p0).norm(), 1e-4); 50 | EXPECT_LE((frame.keypoints_[1] - p1).norm(), 1e-4); 51 | EXPECT_LE((frame.versors_[0] - p0 / p0[2]).norm(), 1e-4); 52 | EXPECT_LE((frame.versors_[1] - p1 / p1[2]).norm(), 1e-4); 53 | EXPECT_LE(cv::norm(descriptors_mat - frame.descriptors_mat_), 1e-4); 54 | for (size_t i = 0; i < descriptors_mat.size().height; ++i) { 55 | EXPECT_LE(cv::norm(frame.descriptors_vec_[i] - descriptors_mat.row(i)), 1e-4); 56 | } 57 | } 58 | 59 | TEST(UtilsTest, VLCFrameMessage) { 60 | RobotId robot_id = 0; 61 | PoseId pose_id = 0; 62 | std::vector keypoints, versors; 63 | gtsam::Vector3 p0(0.1, 0.2, 0.3); 64 | gtsam::Vector3 p1(0.9, 0.8, 0.7); 65 | keypoints.push_back(p0); 66 | keypoints.push_back(p1); 67 | versors.push_back(p0 / p0(2)); 68 | versors.push_back(p1 / p1(2)); 69 | 70 | OrbDescriptor descriptors_mat(2, 7, CV_8UC1, 1); 71 | 72 | VLCFrame frame_in(robot_id, pose_id, keypoints, versors, descriptors_mat); 73 | VLCFrame frame; 74 | pose_graph_tools::VLCFrameMsg msg; 75 | VLCFrameToMsg(frame_in, &msg); 76 | VLCFrameFromMsg(msg, &frame); 77 | 78 | EXPECT_EQ(frame.robot_id_, robot_id); 79 | EXPECT_EQ(frame.pose_id_, pose_id); 80 | EXPECT_LE((frame.keypoints_[0] - p0).norm(), 1e-4); 81 | EXPECT_LE((frame.keypoints_[1] - p1).norm(), 1e-4); 82 | EXPECT_LE(cv::norm(descriptors_mat - frame.descriptors_mat_), 1e-4); 83 | for (size_t i = 0; i < descriptors_mat.size().height; ++i) { 84 | EXPECT_LE(cv::norm(frame.descriptors_vec_[i] - descriptors_mat.row(i)), 1e-4); 85 | } 86 | } 87 | 88 | TEST(UtilsTest, VLCEdgeMsg) { 89 | RobotPoseId vertex_src = std::make_pair(0, 0); 90 | RobotPoseId vertex_dst = std::make_pair(1, 1); 91 | gtsam::Point3 position(1.0, 2.0, 3.0); 92 | gtsam::Rot3 rotation(-0.33968, 0.11263, 0.089, 0.92952); 93 | gtsam::Pose3 T_src_dst(rotation, position); 94 | VLCEdge edge(vertex_src, vertex_dst, T_src_dst); 95 | 96 | VLCEdge edge_out; 97 | pose_graph_tools::PoseGraphEdge msg; 98 | VLCEdgeToMsg(edge, &msg); 99 | VLCEdgeFromMsg(msg, &edge_out); 100 | 101 | EXPECT_EQ(edge.vertex_src_, edge_out.vertex_src_); 102 | EXPECT_EQ(edge.vertex_dst_, edge_out.vertex_dst_); 103 | EXPECT_TRUE(edge.T_src_dst_.equals(edge_out.T_src_dst_)); 104 | } 105 | 106 | } // namespace kimera_multi_lcd 107 | 108 | int main(int argc, char **argv) { 109 | ::testing::InitGoogleTest(&argc, argv); 110 | return RUN_ALL_TESTS(); 111 | } 112 | --------------------------------------------------------------------------------