├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── README.md ├── include └── kimera_distributed │ ├── DistributedLoopClosure.h │ ├── DistributedLoopClosureRos.h │ ├── Keyframe.h │ ├── Submap.h │ ├── SubmapAtlas.h │ ├── configs.h │ └── utils.h ├── launch ├── dpgo.launch ├── kimera_distributed.launch ├── kimera_distributed_loop_closure_ros.launch ├── mit_rosbag.launch ├── remote_topic_manager.launch └── remote_topic_manager_test.launch ├── logs ├── kimera0 │ └── .gitignore ├── kimera1 │ └── .gitignore ├── kimera10 │ └── .gitignore ├── kimera2 │ └── .gitignore ├── kimera3 │ └── .gitignore ├── kimera4 │ └── .gitignore ├── kimera5 │ └── .gitignore ├── kimera6 │ └── .gitignore ├── kimera7 │ └── .gitignore ├── kimera8 │ └── .gitignore └── kimera9 │ └── .gitignore ├── package.xml ├── params ├── persistent_hosts.yaml ├── robot_names.yaml ├── rtm_input_topics_robot.yaml └── visual_loopclosure_Jackal.yaml ├── rviz └── single_machine.rviz ├── src ├── DistributedLoopClosure.cpp ├── DistributedLoopClosureNode.cpp ├── DistributedLoopClosureRos.cpp ├── Submap.cpp ├── SubmapAtlas.cpp └── utils.cpp ├── srv ├── addLoopClosure.srv └── requestSharedLoopClosures.srv └── test ├── test_submap.cpp ├── test_submap_atlas.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 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(kimera_distributed) 3 | 4 | SET(CMAKE_BUILD_TYPE Release) 5 | 6 | find_package(catkin_simple REQUIRED) 7 | 8 | find_package(Boost REQUIRED 9 | date_time 10 | serialization 11 | thread 12 | filesystem 13 | system 14 | regex 15 | timer 16 | chrono 17 | ) 18 | 19 | find_package(GTSAM REQUIRED) 20 | find_package(DBoW2 REQUIRED) 21 | if(NOT TARGET DBoW2::DBoW2) 22 | add_library(DBoW2::DBoW2 INTERFACE IMPORTED) 23 | set_target_properties(DBoW2::DBoW2 PROPERTIES 24 | INTERFACE_LINK_LIBRARIES "${DBoW2_LIBRARIES}" 25 | INTERFACE_INCLUDE_DIRECTORIES "${DBoW2_INCLUDE_DIRS}") 26 | endif() 27 | 28 | catkin_simple() 29 | 30 | 31 | cs_add_library(${PROJECT_NAME} 32 | src/Submap.cpp 33 | src/SubmapAtlas.cpp 34 | src/DistributedLoopClosure.cpp 35 | src/DistributedLoopClosureRos.cpp 36 | src/utils.cpp 37 | ) 38 | 39 | target_link_libraries(${PROJECT_NAME} 40 | ${catkin_LIBRARIES} 41 | ${Boost_LIBRARIES} 42 | ${OpenCV_LIBRARIES} 43 | gtsam 44 | DBoW2::DBoW2 45 | ) 46 | 47 | cs_add_executable(kimera_distributed_loop_closure_node 48 | src/DistributedLoopClosureNode.cpp) 49 | target_link_libraries(kimera_distributed_loop_closure_node ${PROJECT_NAME}) 50 | 51 | # Unit tests 52 | include_directories(${CMAKE_CURRENT_BINARY_DIR}/test) 53 | 54 | catkin_add_gtest(${PROJECT_NAME}-test_utils test/test_utils.cpp) 55 | target_link_libraries(${PROJECT_NAME}-test_utils ${PROJECT_NAME}) 56 | 57 | catkin_add_gtest(${PROJECT_NAME}-test_submap test/test_submap.cpp) 58 | target_link_libraries(${PROJECT_NAME}-test_submap ${PROJECT_NAME}) 59 | 60 | catkin_add_gtest(${PROJECT_NAME}-test_submap_atlas test/test_submap_atlas.cpp) 61 | target_link_libraries(${PROJECT_NAME}-test_submap_atlas ${PROJECT_NAME}) 62 | 63 | 64 | cs_install() 65 | 66 | cs_export() 67 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kimera-Distributed 2 | 3 | Kimera-Distributed consists of the distributed front-end module for [Kimera-Multi](https://github.com/MIT-SPARK/Kimera-Multi). 4 | Please refer to the Kimera-Multi landing page for installation instructions and examples. 5 | 6 | ## Citation 7 | If you found Kimera-Multi to be useful, we would really appreciate if you could cite our work: 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 | ``` -------------------------------------------------------------------------------- /include/kimera_distributed/DistributedLoopClosure.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 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 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "kimera_distributed/SubmapAtlas.h" 28 | #include "kimera_distributed/configs.h" 29 | #include "kimera_distributed/utils.h" 30 | 31 | namespace lcd = kimera_multi_lcd; 32 | 33 | namespace kimera_distributed { 34 | 35 | class DistributedLoopClosure { 36 | public: 37 | DistributedLoopClosure(); 38 | ~DistributedLoopClosure(); 39 | 40 | void initialize(const DistributedLoopClosureConfig& config); 41 | 42 | protected: 43 | DistributedLoopClosureConfig config_; 44 | std::atomic should_shutdown_{false}; 45 | 46 | std::vector received_bow_bytes_; 47 | std::vector received_vlc_bytes_; 48 | 49 | // Submap Atlas 50 | std::unique_ptr submap_atlas_; 51 | // Yulun: commented out the mutex as only one thread is accessing submap_atlas_ 52 | // std::mutex submap_atlas_mutex_; 53 | 54 | // Bag of words vectors 55 | std::unordered_map 56 | bow_latest_; // Latest BoW received from each robot 57 | std::unordered_map> bow_received_; 58 | std::vector 59 | bow_msgs_; // New BoW messages that need to be processed 60 | std::mutex bow_msgs_mutex_; 61 | 62 | // Loop closure detector 63 | std::shared_ptr lcd_; 64 | std::mutex lcd_mutex_; 65 | 66 | // Loop closures 67 | int num_inter_robot_loops_; 68 | gtsam::NonlinearFactorGraph keyframe_loop_closures_; 69 | gtsam::NonlinearFactorGraph submap_loop_closures_; 70 | // New loop closures to be synchronized with other robots 71 | std::map, lcd::CompareEdgeID> 72 | submap_loop_closures_queue_; 73 | // Edge IDs of all established loop closures 74 | std::set submap_loop_closures_ids_; 75 | 76 | // Data structures for offline mode 77 | gtsam::NonlinearFactorGraph offline_keyframe_loop_closures_; 78 | std::map offline_robot_pose_msg_; 79 | 80 | // List of potential loop closures 81 | // that require to request VLC frames 82 | std::mutex vlc_service_mutex_; 83 | std::unordered_map> candidate_lc_; 84 | std::mutex candidate_lc_mutex_; 85 | std::queue queued_lc_; 86 | 87 | // Keep track of BoW vectors requested by other robots 88 | std::map> requested_bows_; 89 | std::mutex requested_bows_mutex_; 90 | 91 | // Keep track of VLC frames requested by other robots 92 | std::map> requested_frames_; 93 | std::mutex requested_frames_mutex_; 94 | 95 | // Record if robot is currently connected 96 | std::map robot_connected_; 97 | 98 | // Record if loop closure publisher is initialized 99 | std::map loop_pub_initialized_; 100 | 101 | // Number of updates received from back-end 102 | int backend_update_count_; 103 | 104 | // Pose corrector 105 | gtsam::Pose3 T_world_dpgo_; 106 | 107 | // For incremental publishing 108 | size_t last_get_submap_idx_; 109 | size_t last_get_lc_idx_; 110 | 111 | // Threads 112 | std::unique_ptr detection_thread_; 113 | std::unique_ptr verification_thread_; 114 | std::unique_ptr comms_thread_; 115 | 116 | // Logging 117 | std::ofstream odometry_file_; // log received odometry poses from VIO 118 | std::ofstream loop_closure_file_; // log inter-robot loop closures 119 | std::ofstream lcd_log_file_; // log loop closure statistics 120 | size_t bow_backlog_, 121 | vlc_backlog_; // Current backlog (number of missing BoW and VLC frames) 122 | std::vector num_loops_with_robot_; 123 | 124 | protected: 125 | /** 126 | * @brief Compute the transformation T_world_odom 127 | */ 128 | gtsam::Pose3 getOdomInWorldFrame() const; 129 | /** 130 | * @brief Compute the transformation T_world_KF 131 | * KF denotes the coordinate of the latest keyframe in the submap atlas 132 | * This function is currently only used for debugging 133 | */ 134 | gtsam::Pose3 getLatestKFInWorldFrame() const; 135 | /** 136 | * @brief Compute the transformation T_odom_KF 137 | * KF denotes the coordinate of the latest keyframe in the submap atlas 138 | * This function is currently only used for debugging 139 | */ 140 | gtsam::Pose3 getLatestKFInOdomFrame() const; 141 | /** 142 | * Callback to process bag of word vectors received from robots 143 | */ 144 | void processBow(const pose_graph_tools_msgs::BowQueriesConstPtr& query_msg); 145 | 146 | /** 147 | * @brief Subscribe to incremental pose graph of this robot published by VIO 148 | * @param msg 149 | */ 150 | bool processLocalPoseGraph(const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg); 151 | 152 | /** 153 | * Callback to process internal VLC frames 154 | */ 155 | void processInternalVLC(const pose_graph_tools_msgs::VLCFramesConstPtr& msg); 156 | 157 | /** 158 | * @brief Callback to receive optimized submap poses from dpgo 159 | * @param msg 160 | */ 161 | void processOptimizedPath(const nav_msgs::PathConstPtr& msg); 162 | 163 | /** 164 | * @brief Detect loop closure using BoW vectors 165 | */ 166 | void detectLoopSpin(); 167 | 168 | /** 169 | * @brief Detect loop closure using the input BoW vector 170 | * @param vertex_query the unique ID associated with the frame of this BoW vector 171 | * @param bow_vec query bag of words vector 172 | */ 173 | void detectLoop(const lcd::RobotPoseId& vertex_query, 174 | const DBoW2::BowVector& bow_vec); 175 | 176 | /** 177 | * Perform geometric verification 178 | */ 179 | void verifyLoopSpin(); 180 | 181 | /** 182 | * Get bow vectors to query 183 | */ 184 | bool queryBowVectorsRequest(lcd::RobotId& selected_robot_id, 185 | std::set& bow_vectors); 186 | 187 | /** 188 | * Get bow vectors to publish 189 | */ 190 | bool queryBowVectorsPublish(lcd::RobotId& selected_robot_id, 191 | lcd::RobotPoseIdSet& bow_vectors); 192 | 193 | /** 194 | * Get VLC frames to request 195 | */ 196 | void queryFramesRequest(lcd::RobotPoseIdSet& local_vertex_ids, 197 | lcd::RobotId& selected_robot_id, 198 | lcd::RobotPoseIdSet& selected_vertex_ids) const; 199 | 200 | /** 201 | * Get VLC frames to publish 202 | */ 203 | void queryFramesPublish(lcd::RobotId& selected_robot_id, 204 | lcd::RobotPoseIdSet& selected_vertex_ids); 205 | 206 | /** 207 | * Get sparse pose graph 208 | */ 209 | pose_graph_tools_msgs::PoseGraph getSubmapPoseGraph(bool incremental = false); 210 | 211 | /** 212 | * Update the candidate list and verification queue 213 | */ 214 | size_t updateCandidateList(); 215 | 216 | /** 217 | * Save the Bow vectors from this session 218 | */ 219 | void saveBowVectors(const std::string& filepath) const; 220 | 221 | /** 222 | * Save the VLC frames from this session 223 | */ 224 | void saveVLCFrames(const std::string& filepath) const; 225 | 226 | /** 227 | * Load Bow vectors into this session (! caution !) 228 | */ 229 | void loadBowVectors(size_t robot_id, const std::string& bow_json); 230 | 231 | /** 232 | * Load VLC frames into this session (! caution !) 233 | */ 234 | void loadVLCFrames(size_t robot_id, const std::string& vlc_json); 235 | 236 | /** 237 | * @brief Create log files 238 | */ 239 | void createLogFiles(); 240 | /** 241 | * @brief Close all log files 242 | */ 243 | void closeLogFiles(); 244 | /** 245 | * @brief Log latest LCD statistics 246 | */ 247 | void logLcdStat(); 248 | /** 249 | * @brief Log a keyframe pose in odometry frame 250 | * @param symbol_frame 251 | * @param T_odom_frame 252 | * @param timestamp in ns 253 | */ 254 | void logOdometryPose(const gtsam::Symbol& symbol_frame, 255 | const gtsam::Pose3& T_odom_frame, 256 | uint64_t ts); 257 | /** 258 | * @brief Log a loop closure to file 259 | * @param keyframe_edge The loop closure between two keyframes 260 | */ 261 | void logLoopClosure(const lcd::VLCEdge& keyframe_edge); 262 | /** 263 | * @brief Compute the latest pose estimates to the world frame 264 | * @brief nodes Pointer of Values type to populate 265 | */ 266 | void computePosesInWorldFrame(gtsam::Values::shared_ptr nodes) const; 267 | /** 268 | * @brief Save the latest pose estimates to the world frame 269 | * @brief filename Output file 270 | */ 271 | void savePosesToFile(const std::string& filename, const gtsam::Values& nodes) const; 272 | /** 273 | * @brief Save the current submap atlas to file. 274 | * This function saves two file: 275 | * 1) kimera_distributed_keyframes.csv: the poses of all keyframes in their respective 276 | * submaps 2) kimera_distributed_submaps.csv: the poses of all submaps in the robot's 277 | * odometry frame 278 | * @param directory Output directory 279 | */ 280 | void saveSubmapAtlas(const std::string& directory) const; 281 | /** 282 | * @brief Load keyframe poses from a file 283 | * @param pose_file 284 | */ 285 | void loadOdometryFromFile(const std::string& pose_file); 286 | /** 287 | * @brief Load loop closures (between keyframes from file) 288 | * @param lc_file 289 | */ 290 | void loadLoopClosuresFromFile(const std::string& lc_file); 291 | /** 292 | * @brief Process loop closures loaded from file 293 | */ 294 | void processOfflineLoopClosures(); 295 | }; 296 | 297 | } // namespace kimera_distributed 298 | -------------------------------------------------------------------------------- /include/kimera_distributed/DistributedLoopClosureRos.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 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "kimera_distributed/DistributedLoopClosure.h" 21 | #include "kimera_distributed/utils.h" 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace lcd = kimera_multi_lcd; 36 | 37 | namespace kimera_distributed { 38 | 39 | class DistributedLoopClosureRos : DistributedLoopClosure { 40 | public: 41 | DistributedLoopClosureRos(const ros::NodeHandle& n); 42 | ~DistributedLoopClosureRos(); 43 | 44 | private: 45 | ros::NodeHandle nh_; 46 | std::atomic should_shutdown_{false}; 47 | 48 | // ROS subscriber 49 | ros::Subscriber local_pg_sub_; 50 | ros::Subscriber internal_vlc_sub_; 51 | std::vector bow_sub_; 52 | std::vector bow_requests_sub_; 53 | std::vector vlc_requests_sub_; 54 | std::vector vlc_responses_sub_; 55 | std::vector loop_sub_; 56 | std::vector loop_ack_sub_; 57 | ros::Subscriber dpgo_sub_; 58 | ros::Subscriber connectivity_sub_; 59 | ros::Subscriber dpgo_frame_corrector_sub_; 60 | 61 | // ROS publisher 62 | ros::Publisher vlc_responses_pub_; 63 | ros::Publisher vlc_requests_pub_; 64 | ros::Publisher pose_graph_pub_; 65 | ros::Publisher bow_requests_pub_; 66 | ros::Publisher bow_response_pub_; 67 | ros::Publisher loop_pub_; 68 | ros::Publisher loop_ack_pub_; 69 | ros::Publisher optimized_nodes_pub_; 70 | ros::Publisher optimized_path_pub_; 71 | ros::Publisher dpgo_frame_corrector_pub_; 72 | 73 | // ROS service 74 | ros::ServiceServer pose_graph_request_server_; 75 | 76 | // Timer 77 | ros::Timer log_timer_; 78 | ros::Timer tf_timer_; 79 | ros::Time start_time_; 80 | ros::Time next_loop_sync_time_; 81 | ros::Time next_latest_bow_pub_time_; 82 | 83 | // TF broadcaster from world to robot's odom frame 84 | tf::TransformBroadcaster tf_broadcaster_; 85 | 86 | private: 87 | std::string latest_kf_frame_id_; 88 | std::string odom_frame_id_; 89 | std::string world_frame_id_; 90 | 91 | 92 | /** 93 | * @brief Run place recognition / loop detection spin 94 | */ 95 | void runDetection(); 96 | 97 | /** 98 | * Run Geometric verification spin 99 | */ 100 | void runVerification(); 101 | 102 | /** 103 | * Run Comms spin to send requests 104 | */ 105 | void runComms(); 106 | 107 | /** 108 | * Callback to process bag of word vectors received from robots 109 | */ 110 | void bowCallback(const pose_graph_tools_msgs::BowQueriesConstPtr& query_msg); 111 | 112 | /** 113 | * @brief Subscribe to incremental pose graph of this robot published by VIO 114 | * @param msg 115 | */ 116 | void localPoseGraphCallback(const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg); 117 | 118 | /** 119 | * Callback to process the VLC responses to our requests 120 | */ 121 | void vlcResponsesCallback(const pose_graph_tools_msgs::VLCFramesConstPtr& msg); 122 | 123 | /** 124 | * Callback to process internal VLC frames 125 | */ 126 | void internalVLCCallback(const pose_graph_tools_msgs::VLCFramesConstPtr& msg); 127 | 128 | /** 129 | * @brief Callback to process the BoW requests from other robots 130 | * @param msg 131 | */ 132 | void bowRequestsCallback(const pose_graph_tools_msgs::BowRequestsConstPtr& msg); 133 | 134 | /** 135 | * Callback to process the VLC requests from other robots 136 | */ 137 | void vlcRequestsCallback(const pose_graph_tools_msgs::VLCRequestsConstPtr& msg); 138 | 139 | /** 140 | * Callback to process new inter-robot loop closures 141 | */ 142 | void loopClosureCallback(const pose_graph_tools_msgs::LoopClosuresConstPtr& msg); 143 | 144 | /** 145 | * Callback to process new inter-robot loop closures 146 | */ 147 | void loopAcknowledgementCallback( 148 | const pose_graph_tools_msgs::LoopClosuresAckConstPtr& msg); 149 | 150 | /** 151 | * @brief Callback to receive optimized submap poses from dpgo 152 | * @param msg 153 | */ 154 | void dpgoCallback(const nav_msgs::PathConstPtr& msg); 155 | 156 | /** 157 | * @brief Publish optimized nodes 158 | * @param msg 159 | */ 160 | void publishOptimizedNodesAndPath(const gtsam::Values& nodes); 161 | 162 | /** 163 | * @brief Subscribe to T_world_dpgo (default to identity) 164 | * @param msg 165 | */ 166 | void dpgoFrameCorrectionCallback(const geometry_msgs::Pose::ConstPtr& msg); 167 | 168 | /** 169 | * @brief Callback to timer used for periodically logging 170 | */ 171 | void logTimerCallback(const ros::TimerEvent& event); 172 | 173 | /** 174 | * @brief Publish world to dpgo frame based on first robot 175 | */ 176 | void publishWorldToDpgoCorrection(); 177 | 178 | /** 179 | * @brief Callback to timer used for periodically publishing TF 180 | */ 181 | void tfTimerCallback(const ros::TimerEvent& event); 182 | 183 | /** 184 | * @brief Subscriber callback that listens to the list of currently connected robots 185 | */ 186 | void connectivityCallback(const std_msgs::UInt16MultiArrayConstPtr& msg); 187 | 188 | /** 189 | * @brief Send submap-level pose graph for distributed optimization 190 | * @param request 191 | * @param response 192 | * @return 193 | */ 194 | bool requestPoseGraphCallback(pose_graph_tools_msgs::PoseGraphQuery::Request& request, 195 | pose_graph_tools_msgs::PoseGraphQuery::Response& response); 196 | 197 | /** 198 | * Initialize loop closures 199 | */ 200 | void initializeLoopPublishers(); 201 | 202 | /** 203 | * Publish queued loop closures to be synchronized with other robots 204 | */ 205 | void publishQueuedLoops(); 206 | 207 | /** 208 | * Publish VLC requests 209 | */ 210 | void processVLCRequests(const size_t& robot_id, 211 | const lcd::RobotPoseIdSet& vertex_ids); 212 | 213 | /** 214 | * Publish VLC Frame requests from other robots 215 | */ 216 | void publishVLCRequests(const size_t& robot_id, 217 | const lcd::RobotPoseIdSet& vertex_ids); 218 | 219 | /** 220 | * Request local VLC Frames from Kimera-VIO-ROS 221 | */ 222 | bool requestVLCFrameService(const lcd::RobotPoseIdSet& vertex_ids); 223 | 224 | /** 225 | * @brief Request BoW vectors 226 | */ 227 | void requestBowVectors(); 228 | 229 | /** 230 | * @brief Publish BoW vectors requested by other robots 231 | */ 232 | void publishBowVectors(); 233 | 234 | /** 235 | * @brief Publish the latest BoW vector of this robot 236 | */ 237 | void publishLatestBowVector(); 238 | 239 | /** 240 | * Check and submit VLC requests 241 | */ 242 | void requestFrames(); 243 | 244 | /** 245 | * @brief Publish VLC frames requested by other robots 246 | */ 247 | void publishFrames(); 248 | 249 | void publishSubmapOfflineInfo(); 250 | 251 | /** 252 | * Randomly sleep from (min_sec, max_sec) seconds 253 | */ 254 | void randomSleep(double min_sec, double max_sec); 255 | 256 | /** 257 | * @brief Publish TF between world and odom 258 | */ 259 | void publishOdomToWorld(); 260 | 261 | /** 262 | * @brief Publish TF between world and latest keyframe 263 | */ 264 | void publishLatestKFToWorld(); 265 | 266 | /** 267 | * @brief Publish TF between odom and latest keyframe 268 | */ 269 | void publishLatestKFToOdom(); 270 | 271 | /** 272 | * @brief Save VLC frames and BoW vectors 273 | */ 274 | void save(); 275 | }; 276 | 277 | } // namespace kimera_distributed -------------------------------------------------------------------------------- /include/kimera_distributed/Keyframe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #ifndef KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_KEYFRAME_H_ 8 | #define KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_KEYFRAME_H_ 9 | 10 | #include 11 | #include 12 | 13 | namespace kimera_distributed { 14 | 15 | // Forward declaration 16 | class Submap; 17 | 18 | class Keyframe { 19 | public: 20 | /** 21 | * @brief Constructor 22 | * @param keyframe_id unique ID of this keyframe 23 | * @param submap pointer to the submap that contains this keyframe 24 | */ 25 | Keyframe(int id, uint64_t stamp) : id_(id), stamp_(stamp) {} 26 | /** 27 | * @brief Get keyframe ID 28 | * @return 29 | */ 30 | int id() const { return id_; } 31 | /** 32 | * @brief Get timestamp in ns 33 | * @return 34 | */ 35 | uint64_t stamp() const { return stamp_; } 36 | /** 37 | * @brief Get submap 38 | * @return 39 | */ 40 | std::shared_ptr getSubmap() const { return submap_; } 41 | /** 42 | * @brief Set submap 43 | * @param submap 44 | */ 45 | void setSubmap(const std::shared_ptr& submap) { submap_ = submap; } 46 | /** 47 | * @brief Get the pose of this frame in the submap reference frame 48 | * @return 49 | */ 50 | gtsam::Pose3 getPoseInSubmapFrame() const { return T_submap_keyframe_; } 51 | /** 52 | * @brief Set the pose of this keyframe in the submap frame 53 | * @param T_submap_keyframe 54 | */ 55 | void setPoseInSubmapFrame(const gtsam::Pose3& T_submap_keyframe) { 56 | T_submap_keyframe_ = T_submap_keyframe; 57 | } 58 | /** 59 | * @brief Get the pose of this keyframe in the odometry frame 60 | * @return 61 | */ 62 | gtsam::Pose3 getPoseInOdomFrame() const { return T_odom_keyframe_; } 63 | /** 64 | * @brief Set pose of this keyframe in the odometry frame 65 | * @param T_odom_keyframe 66 | */ 67 | void setPoseInOdomFrame(const gtsam::Pose3& T_odom_keyframe) { 68 | T_odom_keyframe_ = T_odom_keyframe; 69 | } 70 | 71 | private: 72 | const int id_; // unique id associated with this keyframe 73 | const uint64_t stamp_; // const timestamp of this keyframe in ns 74 | std::shared_ptr submap_; // pointer to the submap that contains this KF 75 | gtsam::Pose3 T_odom_keyframe_; // pose of this keyframe in the odometry frame 76 | gtsam::Pose3 T_submap_keyframe_; // pose of this keyframe in the submap frame 77 | }; 78 | 79 | } // namespace kimera_distributed 80 | 81 | #endif // KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_KEYFRAME_H_ 82 | -------------------------------------------------------------------------------- /include/kimera_distributed/Submap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #ifndef KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAP_H_ 8 | #define KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAP_H_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "kimera_distributed/Keyframe.h" 15 | 16 | namespace kimera_distributed { 17 | 18 | class Submap { 19 | public: 20 | /** 21 | * @brief Constructor 22 | * @param id 23 | */ 24 | Submap(int id, uint64_t stamp); 25 | /** 26 | * @brief Get ID of this submap 27 | * @return 28 | */ 29 | int id() const; 30 | /** 31 | * @brief Get timestamp of this submap 32 | * @return 33 | */ 34 | uint64_t stamp() const; 35 | /** 36 | * @brief Get number of keyframes in this submap 37 | * @return 38 | */ 39 | int numKeyframes() const; 40 | /** 41 | * @brief Get the cumulative distance traversed in this submap 42 | * @return 43 | */ 44 | double distance() const; 45 | /** 46 | * @brief Get pose of this submap in the odometry frame 47 | * @return 48 | */ 49 | gtsam::Pose3 getPoseInOdomFrame() const; 50 | /** 51 | * @brief Set pose of this submap in the odometry frame 52 | * @param T_odom_submap 53 | */ 54 | void setPoseInOdomFrame(const gtsam::Pose3& T_odom_submap); 55 | /** 56 | * @brief Get pose of this submap in the world frame 57 | * @return 58 | */ 59 | gtsam::Pose3 getPoseInWorldFrame() const; 60 | /** 61 | * @brief Set pose of this submap in the world frame 62 | * @param T_world_submap 63 | */ 64 | void setPoseInWorldFrame(const gtsam::Pose3& T_world_submap); 65 | /** 66 | * @brief Add a new keyframe to this submap 67 | * @param keyframe 68 | */ 69 | void addKeyframe(const std::shared_ptr& keyframe); 70 | /** 71 | * @brief Get the keyframe (nullptr if keyframe does not exist) 72 | * @param keyframe_id 73 | * @return 74 | */ 75 | std::shared_ptr getKeyframe(int keyframe_id) const; 76 | /** 77 | * @brief Get the set of Keyframe IDs in this submap 78 | * @return 79 | */ 80 | std::unordered_set getKeyframeIDs() const; 81 | 82 | private: 83 | const int id_; // unique id of this submap 84 | uint64_t stamp_; 85 | std::unordered_map> 86 | keyframes_; // keyframes that belong to this submap 87 | gtsam::Pose3 88 | T_odom_submap_; // the pose of this submap in the odometry frame (Kimera-VIO) 89 | gtsam::Pose3 90 | T_world_submap_; // the pose of this submap if the world frame (might not exist) 91 | double distance_; // cumulative distance traveled in this submap 92 | }; 93 | 94 | } // namespace kimera_distributed 95 | 96 | #endif // KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAP_H_ 97 | -------------------------------------------------------------------------------- /include/kimera_distributed/SubmapAtlas.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #ifndef KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAPATLAS_H_ 8 | #define KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAPATLAS_H_ 9 | 10 | #include "kimera_distributed/Submap.h" 11 | 12 | namespace kimera_distributed { 13 | 14 | class SubmapAtlas { 15 | public: 16 | /** 17 | * @brief Parameters 18 | */ 19 | class Parameters { 20 | public: 21 | Parameters() : max_submap_size(10), max_submap_distance(1.0) {} 22 | int max_submap_size; // maximum number of keyframes in a given submap 23 | double max_submap_distance; // maximum cumulative distance in a given submap 24 | }; 25 | /** 26 | * @brief Constructor 27 | * @param params 28 | */ 29 | SubmapAtlas(const Parameters& params); 30 | /** 31 | * @brief Get parameters 32 | * @return 33 | */ 34 | Parameters params() const; 35 | /** 36 | * @brief Get number of keyframes 37 | * @return 38 | */ 39 | int numKeyframes() const; 40 | /** 41 | * @brief Get number of submaps 42 | * @return 43 | */ 44 | int numSubmaps() const; 45 | /** 46 | * @brief Create a new keyframe with the given id and pose in the odometry frame. 47 | * Furthermore, the newly created keyframe is added to a submap. 48 | * @param keyframe_id 49 | * @param T_odom_keyframe pose of this keyframe in the odometry frame 50 | * @return 51 | */ 52 | std::shared_ptr createKeyframe(int keyframe_id, 53 | const gtsam::Pose3& T_odom_keyframe, 54 | const uint64_t& timestamp); 55 | /** 56 | * @brief Check if keyframe with specified ID exists. 57 | * @param keyframe_id 58 | * @return 59 | */ 60 | bool hasKeyframe(int keyframe_id) const; 61 | /** 62 | * @brief Get keyframe (return nullptr if not found) 63 | * @param keyframe_id 64 | * @return 65 | */ 66 | std::shared_ptr getKeyframe(int keyframe_id); 67 | /** 68 | * @brief Get keyframe based on the query timestamp 69 | * @param timestamp desired timestamp in ns 70 | * @param tolerance in ns (default to 1 sec) 71 | * @return keyframe or nullptr if not found 72 | */ 73 | std::shared_ptr getKeyframeFromStamp(const uint64_t& timestamp, 74 | const uint64_t& tolNs = 1000000000); 75 | /** 76 | * @brief Get latest keyframe (nullptr) if submap atlas is empty 77 | * @return 78 | */ 79 | std::shared_ptr getLatestKeyframe(); 80 | /** 81 | * @brief Create a new submap with the given id and pose in the odometry frame. 82 | * @param submap_id 83 | * @param T_odom_submap 84 | * @param timestamp 85 | * @return 86 | */ 87 | std::shared_ptr createSubmap(int submap_id, 88 | const gtsam::Pose3& T_odom_submap, 89 | const uint64_t& timestamp); 90 | /** 91 | * @brief Check if a submap already exists. 92 | * @param submap_id 93 | * @return 94 | */ 95 | bool hasSubmap(int submap_id) const; 96 | /** 97 | * @brief Get submap (return nullptr if not found) 98 | * @param submap_id 99 | * @return 100 | */ 101 | std::shared_ptr getSubmap(int submap_id); 102 | /** 103 | * @brief Return the latest submap (null if the submap atlas is empty) 104 | * @return 105 | */ 106 | std::shared_ptr getLatestSubmap(); 107 | 108 | private: 109 | Parameters params_; 110 | std::unordered_map> keyframes_; 111 | std::unordered_map> submaps_; 112 | /** 113 | * @brief Check if a new submap should be created 114 | * @return 115 | */ 116 | bool shouldCreateNewSubmap(); 117 | }; 118 | 119 | } // namespace kimera_distributed 120 | 121 | #endif // KIMERA_DISTRIBUTED_INCLUDE_KIMERA_DISTRIBUTED_SUBMAPATLAS_H_ 122 | -------------------------------------------------------------------------------- /include/kimera_distributed/configs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace kimera_distributed { 16 | 17 | struct DistributedLoopClosureConfig { 18 | size_t my_id_; 19 | size_t num_robots_; 20 | std::string frame_id_; 21 | bool log_output_; 22 | std::string log_output_dir_; 23 | 24 | // Offline mode 25 | bool run_offline_; 26 | std::string offline_dir_; 27 | 28 | // LCD Params 29 | kimera_multi_lcd::LcdParams lcd_params_; 30 | 31 | // Submap Params 32 | SubmapAtlas::Parameters submap_params_; 33 | 34 | // Parameters controlling communication due to VLC request/response 35 | int bow_batch_size_; // Maximum number of Bow vectors per robot to request in one 36 | // batch 37 | int vlc_batch_size_; // Maximum number of VLC frames per robot to request in one 38 | // batch 39 | int loop_batch_size_; // Maximum number of loop closures to synchronize in one batch 40 | int comm_sleep_time_; // Sleep time of communication thread 41 | int loop_sync_sleep_time_; // Sleep time of loop synchronization 42 | int detection_batch_size_; // Maximum number of loop detection to perform in one 43 | // batch 44 | int bow_skip_num_; // Request every bow_skip_num_ bow vectors 45 | 46 | // Robot names 47 | std::map robot_names_; 48 | }; 49 | 50 | } // namespace kimera_distributed -------------------------------------------------------------------------------- /include/kimera_distributed/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 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace kimera_distributed { 23 | 24 | const std::map robot_id_to_prefix = { 25 | {0, 'a'}, 26 | {1, 'b'}, 27 | {2, 'c'}, 28 | {3, 'd'}, 29 | {4, 'e'}, 30 | {5, 'f'}, 31 | {6, 'g'}, 32 | {7, 'h'}, 33 | {8, 'i'}, 34 | {9, 'j'} 35 | }; 36 | 37 | const std::map robot_prefix_to_id = { 38 | {'a', 0}, 39 | {'b', 1}, 40 | {'c', 2}, 41 | {'d', 3}, 42 | {'e', 4}, 43 | {'f', 5}, 44 | {'g', 6}, 45 | {'h', 7}, 46 | {'i', 8}, 47 | {'j', 9} 48 | }; 49 | 50 | gtsam::Pose3 RosPoseToGtsam(const geometry_msgs::Pose& transform); 51 | geometry_msgs::Pose GtsamPoseToRos(const gtsam::Pose3& transform); 52 | void GtsamPoseToRosTf(const gtsam::Pose3& pose, geometry_msgs::Transform* tf); 53 | 54 | // Convert gtsam posegaph to PoseGraph msg 55 | pose_graph_tools_msgs::PoseGraph GtsamGraphToRos( 56 | const gtsam::NonlinearFactorGraph& factors, 57 | const gtsam::Values& values, 58 | const gtsam::Vector& gnc_weights = Eigen::VectorXd::Zero(0)); 59 | 60 | // Convert vector of gtsam poses to path msg 61 | nav_msgs::Path GtsamPoseTrajectoryToPath(const std::vector& gtsam_poses); 62 | 63 | /** 64 | * @brief Check if the input factor graph contains a Pose3 between factor with 65 | * the specified src and dst vertices 66 | * @param nfg 67 | * @param symbol_src 68 | * @param symbol_dst 69 | * @return 70 | */ 71 | bool hasBetweenFactor(const gtsam::NonlinearFactorGraph& nfg, 72 | const gtsam::Symbol& symbol_src, 73 | const gtsam::Symbol& symbol_dst); 74 | } // namespace kimera_distributed 75 | -------------------------------------------------------------------------------- /launch/dpgo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /launch/kimera_distributed.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /launch/kimera_distributed_loop_closure_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /launch/mit_rosbag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /launch/remote_topic_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /launch/remote_topic_manager_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | - /foo 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /logs/kimera0/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera1/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera10/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera2/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera3/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera4/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera5/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera6/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera7/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera8/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /logs/kimera9/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kimera_distributed 4 | 1.0.0 5 | Kimera Distributed 6 | 7 | Yulun Tian 8 | Yun Chang 9 | 10 | BSD 11 | 12 | catkin 13 | catkin_simple 14 | 15 | glog_catkin 16 | kimera_multi_lcd 17 | DBoW2 18 | gtsam 19 | Eigen 20 | OpenCV 21 | roscpp 22 | std_msgs 23 | sensor_msgs 24 | geometry_msgs 25 | nav_msgs 26 | message_filters 27 | image_transport 28 | pcl_ros 29 | rosbag 30 | cv_bridge 31 | tf 32 | pose_graph_tools_msgs 33 | pose_graph_tools_ros 34 | message_runtime 35 | rosunit 36 | rostest 37 | 38 | 39 | -------------------------------------------------------------------------------- /params/persistent_hosts.yaml: -------------------------------------------------------------------------------- 1 | # Hosts (each specified by hostname and enet port) that have persistent communication across disconnections 2 | - robot0: {hostname: "10.31.128.97", port: 11411} 3 | - robot1: {hostname: "10.29.255.104", port: 11412} 4 | - robot2: {hostname: "10.29.245.232", port: 11413} 5 | - robot3: {hostname: "192.168.10.133", port: 11414} 6 | - robot4: {hostname: "192.168.10.143", port: 11415} 7 | - robot5: {hostname: "192.168.10.153", port: 11416} 8 | - robot6: {hostname: "192.168.10.163", port: 11417} 9 | - robot7: {hostname: "192.168.10.173", port: 11418} 10 | - robot8: {hostname: "10.29.175.68", port: 11419} 11 | -------------------------------------------------------------------------------- /params/robot_names.yaml: -------------------------------------------------------------------------------- 1 | robot0_name: acl_jackal 2 | robot1_name: acl_jackal2 3 | robot2_name: sparkal1 4 | robot3_name: sparkal2 5 | robot4_name: hathor 6 | robot5_name: thoth 7 | robot6_name: apis 8 | robot7_name: sobek 9 | robot8_name: enyo 10 | -------------------------------------------------------------------------------- /params/rtm_input_topics_robot.yaml: -------------------------------------------------------------------------------- 1 | # ROS messages to send to others 2 | - /$(arg robot_name)/kimera_vio_ros/bow_query: {queue_size: 20, reliable: true, persistent: false} 3 | - /$(arg robot_name)/kimera_distributed/bow_requests: {queue_size: 1, reliable: true} 4 | - /$(arg robot_name)/kimera_distributed/vlc_responses: {queue_size: 1, reliable: true} 5 | - /$(arg robot_name)/kimera_distributed/vlc_requests: {queue_size: 1, reliable: true} 6 | - /$(arg robot_name)/kimera_distributed/loop_closures: {queue_size: 20, reliable: true} 7 | - /$(arg robot_name)/kimera_distributed/loop_ack: {queue_size: 20, reliable: true} 8 | - /$(arg robot_name)/kimera_distributed/pose_corrector: {queue_size: 1, reliable: true} 9 | - /$(arg robot_name)/dpgo_ros_node/lifting_matrix: {queue_size: 1, reliable: true} 10 | - /$(arg robot_name)/dpgo_ros_node/status: {queue_size: 1, reliable: true} 11 | - /$(arg robot_name)/dpgo_ros_node/command: {queue_size: 20, reliable: true} 12 | - /$(arg robot_name)/dpgo_ros_node/anchor: {queue_size: 1, reliable: true} 13 | - /$(arg robot_name)/dpgo_ros_node/public_poses: {queue_size: 10, reliable: true} 14 | - /$(arg robot_name)/dpgo_ros_node/public_measurements: {queue_size: 10, reliable: true} 15 | - /$(arg robot_name)/dpgo_ros_node/measurement_weights: {queue_size: 10, reliable: true} 16 | - /$(arg robot_name)/dpgo_ros_node/path: {queue_size: 1, reliable: true} 17 | - /$(arg robot_name)/kimera_distributed/optimized_path: {queue_size: 1, reliable: true} 18 | - /$(arg robot_name)/dpgo_ros_node/loop_closures: {queue_size: 1, reliable: true} 19 | # - /$(arg robot_name)/kimera_pgmo/optimized_mesh: {queue_size: 1, reliable: false} 20 | - /$(arg robot_name)/landmarks: {queue_size: 1, reliable: false} 21 | - /$(arg robot_name)/test_ping: {queue_size: 1, reliable: false} 22 | - /$(arg robot_name)/test_ack: {queue_size: 1, reliable: false} 23 | -------------------------------------------------------------------------------- /params/visual_loopclosure_Jackal.yaml: -------------------------------------------------------------------------------- 1 | # Yaml configuration for visual loop closure parameters 2 | 3 | detect_interrobot_only: true 4 | 5 | # Matcher parameters (DBoW2) 6 | # alpha: 0.5 7 | dist_local: 90 8 | max_db_results: 2 9 | min_nss_factor: 0.05 10 | lowe_ratio: 0.8 11 | min_temporal_matches: 3 12 | min_matches_per_island: 1 13 | max_intraisland_gap: 3 14 | max_nrFrames_between_islands: 3 15 | max_nrFrames_between_queries: 2 16 | 17 | # Mono RANSAC 18 | ransac_threshold_mono: 0.00001 19 | ransac_inlier_percentage_mono: 0.01 20 | max_ransac_iterations_mono: 1000 21 | 22 | # Stereo RANSAC 23 | max_ransac_iterations: 1000 24 | ransac_threshold: 0.3 25 | geometric_verification_min_inlier_count: 5 26 | geometric_verification_min_inlier_percentage: 0.0 27 | -------------------------------------------------------------------------------- /rviz/single_machine.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5617647171020508 10 | Tree Height: 728 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: false 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: false 55 | - Class: rviz/Marker 56 | Enabled: true 57 | Marker Topic: /kimera_pgmo/posegraph_viewer/loop_edges 58 | Name: Loop Closures 59 | Namespaces: 60 | {} 61 | Queue Size: 100 62 | Value: true 63 | - Alpha: 1 64 | Buffer Length: 1 65 | Class: rviz/Path 66 | Color: 17; 14; 235 67 | Enabled: true 68 | Head Diameter: 0.30000001192092896 69 | Head Length: 0.20000000298023224 70 | Length: 0.30000001192092896 71 | Line Style: Lines 72 | Line Width: 0.029999999329447746 73 | Name: Path0 74 | Offset: 75 | X: 0 76 | Y: 0 77 | Z: 0 78 | Pose Color: 255; 85; 255 79 | Pose Style: None 80 | Radius: 0.029999999329447746 81 | Shaft Diameter: 0.10000000149011612 82 | Shaft Length: 0.10000000149011612 83 | Topic: /acl_jackal/dpgo_ros_node/path 84 | Unreliable: false 85 | Value: true 86 | - Alpha: 1 87 | Buffer Length: 1 88 | Class: rviz/Path 89 | Color: 25; 255; 0 90 | Enabled: true 91 | Head Diameter: 0.30000001192092896 92 | Head Length: 0.20000000298023224 93 | Length: 0.30000001192092896 94 | Line Style: Lines 95 | Line Width: 0.029999999329447746 96 | Name: Path1 97 | Offset: 98 | X: 0 99 | Y: 0 100 | Z: 0 101 | Pose Color: 255; 85; 255 102 | Pose Style: None 103 | Radius: 0.029999999329447746 104 | Shaft Diameter: 0.10000000149011612 105 | Shaft Length: 0.10000000149011612 106 | Topic: /acl_jackal2/dpgo_ros_node/path 107 | Unreliable: false 108 | Value: true 109 | - Alpha: 1 110 | Buffer Length: 1 111 | Class: rviz/Path 112 | Color: 156; 8; 240 113 | Enabled: true 114 | Head Diameter: 0.30000001192092896 115 | Head Length: 0.20000000298023224 116 | Length: 0.30000001192092896 117 | Line Style: Lines 118 | Line Width: 0.029999999329447746 119 | Name: Path2 120 | Offset: 121 | X: 0 122 | Y: 0 123 | Z: 0 124 | Pose Color: 255; 85; 255 125 | Pose Style: None 126 | Radius: 0.029999999329447746 127 | Shaft Diameter: 0.10000000149011612 128 | Shaft Length: 0.10000000149011612 129 | Topic: /sparkal1/dpgo_ros_node/path 130 | Unreliable: false 131 | Value: true 132 | - Alpha: 1 133 | Buffer Length: 1 134 | Class: rviz/Path 135 | Color: 252; 233; 79 136 | Enabled: true 137 | Head Diameter: 0.30000001192092896 138 | Head Length: 0.20000000298023224 139 | Length: 0.30000001192092896 140 | Line Style: Lines 141 | Line Width: 0.029999999329447746 142 | Name: Path3 143 | Offset: 144 | X: 0 145 | Y: 0 146 | Z: 0 147 | Pose Color: 255; 85; 255 148 | Pose Style: None 149 | Radius: 0.029999999329447746 150 | Shaft Diameter: 0.10000000149011612 151 | Shaft Length: 0.10000000149011612 152 | Topic: /sparkal2/dpgo_ros_node/path 153 | Unreliable: false 154 | Value: true 155 | - Alpha: 1 156 | Buffer Length: 1 157 | Class: rviz/Path 158 | Color: 239; 41; 41 159 | Enabled: true 160 | Head Diameter: 0.30000001192092896 161 | Head Length: 0.20000000298023224 162 | Length: 0.30000001192092896 163 | Line Style: Lines 164 | Line Width: 0.029999999329447746 165 | Name: Path4 166 | Offset: 167 | X: 0 168 | Y: 0 169 | Z: 0 170 | Pose Color: 255; 85; 255 171 | Pose Style: None 172 | Radius: 0.029999999329447746 173 | Shaft Diameter: 0.10000000149011612 174 | Shaft Length: 0.10000000149011612 175 | Topic: /hathor/dpgo_ros_node/path 176 | Unreliable: false 177 | Value: true 178 | - Alpha: 1 179 | Buffer Length: 1 180 | Class: rviz/Path 181 | Color: 245; 121; 0 182 | Enabled: true 183 | Head Diameter: 0.30000001192092896 184 | Head Length: 0.20000000298023224 185 | Length: 0.30000001192092896 186 | Line Style: Lines 187 | Line Width: 0.029999999329447746 188 | Name: Path5 189 | Offset: 190 | X: 0 191 | Y: 0 192 | Z: 0 193 | Pose Color: 255; 85; 255 194 | Pose Style: None 195 | Radius: 0.029999999329447746 196 | Shaft Diameter: 0.10000000149011612 197 | Shaft Length: 0.10000000149011612 198 | Topic: /thoth/dpgo_ros_node/path 199 | Unreliable: false 200 | Value: true 201 | - Alpha: 1 202 | Buffer Length: 1 203 | Class: rviz/Path 204 | Color: 243; 9; 170 205 | Enabled: true 206 | Head Diameter: 0.30000001192092896 207 | Head Length: 0.20000000298023224 208 | Length: 0.30000001192092896 209 | Line Style: Lines 210 | Line Width: 0.029999999329447746 211 | Name: Path6 212 | Offset: 213 | X: 0 214 | Y: 0 215 | Z: 0 216 | Pose Color: 255; 85; 255 217 | Pose Style: None 218 | Radius: 0.029999999329447746 219 | Shaft Diameter: 0.10000000149011612 220 | Shaft Length: 0.10000000149011612 221 | Topic: /apis/kimera_vio_ros/optimized_trajectory 222 | Unreliable: false 223 | Value: true 224 | - Alpha: 1 225 | Buffer Length: 1 226 | Class: rviz/Path 227 | Color: 193; 125; 17 228 | Enabled: true 229 | Head Diameter: 0.30000001192092896 230 | Head Length: 0.20000000298023224 231 | Length: 0.30000001192092896 232 | Line Style: Lines 233 | Line Width: 0.029999999329447746 234 | Name: Path7 235 | Offset: 236 | X: 0 237 | Y: 0 238 | Z: 0 239 | Pose Color: 255; 85; 255 240 | Pose Style: None 241 | Radius: 0.029999999329447746 242 | Shaft Diameter: 0.10000000149011612 243 | Shaft Length: 0.10000000149011612 244 | Topic: /sobek/dpgo_ros_node/path 245 | Unreliable: false 246 | Value: true 247 | - Class: rviz/Image 248 | Enabled: false 249 | Image Topic: /robot0/kimera_vio_ros/kimera_vio_ros_node/feature_tracks 250 | Max Value: 1 251 | Median window: 5 252 | Min Value: 0 253 | Name: Image0 254 | Normalize Range: true 255 | Queue Size: 2 256 | Transport Hint: compressed 257 | Unreliable: false 258 | Value: false 259 | - Class: rviz/Image 260 | Enabled: false 261 | Image Topic: /robot1/kimera_vio_ros/kimera_vio_ros_node/feature_tracks 262 | Max Value: 1 263 | Median window: 5 264 | Min Value: 0 265 | Name: Image1 266 | Normalize Range: true 267 | Queue Size: 2 268 | Transport Hint: compressed 269 | Unreliable: false 270 | Value: false 271 | - Class: rviz/Image 272 | Enabled: false 273 | Image Topic: /robot2/kimera_vio_ros/kimera_vio_ros_node/feature_tracks 274 | Max Value: 1 275 | Median window: 5 276 | Min Value: 0 277 | Name: Image2 278 | Normalize Range: true 279 | Queue Size: 2 280 | Transport Hint: compressed 281 | Unreliable: false 282 | Value: false 283 | - Class: rviz/Image 284 | Enabled: false 285 | Image Topic: /robot3/kimera_vio_ros/kimera_vio_ros_node/feature_tracks 286 | Max Value: 1 287 | Median window: 5 288 | Min Value: 0 289 | Name: Image3 290 | Normalize Range: true 291 | Queue Size: 2 292 | Transport Hint: compressed 293 | Unreliable: false 294 | Value: false 295 | - Class: rviz_mesh_plugin/TriangleMesh 296 | Display Type: 297 | Faces Alpha: 1 298 | Faces Color: 0; 255; 0 299 | Value: Vertex Color 300 | Enabled: true 301 | Mesh Buffer Size: 1 302 | Name: TriangleMesh0 303 | Show Normals: 304 | Normals Alpha: 1 305 | Normals Color: 255; 0; 255 306 | Normals Scaling Factor: 0.10000000149011612 307 | Value: false 308 | Show Wireframe: 309 | Value: false 310 | Wireframe Alpha: 1 311 | Wireframe Color: 0; 0; 0 312 | Topic: /acl_jackal/kimera_pgmo/optimized_mesh 313 | Value: true 314 | - Class: rviz_mesh_plugin/TriangleMesh 315 | Display Type: 316 | Faces Alpha: 1 317 | Faces Color: 0; 255; 0 318 | Value: Vertex Color 319 | Enabled: true 320 | Mesh Buffer Size: 1 321 | Name: TriangleMesh1 322 | Show Normals: 323 | Normals Alpha: 1 324 | Normals Color: 255; 0; 255 325 | Normals Scaling Factor: 0.10000000149011612 326 | Value: false 327 | Show Wireframe: 328 | Value: false 329 | Wireframe Alpha: 1 330 | Wireframe Color: 0; 0; 0 331 | Topic: /acl_jackal2/kimera_pgmo/optimized_mesh 332 | Value: true 333 | - Class: rviz_mesh_plugin/TriangleMesh 334 | Display Type: 335 | Faces Alpha: 1 336 | Faces Color: 0; 255; 0 337 | Value: Vertex Color 338 | Enabled: true 339 | Mesh Buffer Size: 1 340 | Name: TriangleMesh2 341 | Show Normals: 342 | Normals Alpha: 1 343 | Normals Color: 255; 0; 255 344 | Normals Scaling Factor: 0.10000000149011612 345 | Value: false 346 | Show Wireframe: 347 | Value: false 348 | Wireframe Alpha: 1 349 | Wireframe Color: 0; 0; 0 350 | Topic: /sparkal1/kimera_pgmo/optimized_mesh 351 | Value: true 352 | - Class: rviz_mesh_plugin/TriangleMesh 353 | Display Type: 354 | Faces Alpha: 1 355 | Faces Color: 0; 255; 0 356 | Value: Vertex Color 357 | Enabled: true 358 | Mesh Buffer Size: 1 359 | Name: TriangleMesh3 360 | Show Normals: 361 | Normals Alpha: 1 362 | Normals Color: 255; 0; 255 363 | Normals Scaling Factor: 0.10000000149011612 364 | Value: false 365 | Show Wireframe: 366 | Value: false 367 | Wireframe Alpha: 1 368 | Wireframe Color: 0; 0; 0 369 | Topic: /sparkal2/kimera_pgmo/optimized_mesh 370 | Value: true 371 | - Class: rviz_mesh_plugin/TriangleMesh 372 | Display Type: 373 | Faces Alpha: 1 374 | Faces Color: 0; 255; 0 375 | Value: Vertex Color 376 | Enabled: true 377 | Mesh Buffer Size: 1 378 | Name: TriangleMesh4 379 | Show Normals: 380 | Normals Alpha: 1 381 | Normals Color: 255; 0; 255 382 | Normals Scaling Factor: 0.10000000149011612 383 | Value: false 384 | Show Wireframe: 385 | Value: false 386 | Wireframe Alpha: 1 387 | Wireframe Color: 0; 0; 0 388 | Topic: /hathor/kimera_pgmo/optimized_mesh 389 | Value: true 390 | - Class: rviz_mesh_plugin/TriangleMesh 391 | Display Type: 392 | Faces Alpha: 1 393 | Faces Color: 0; 255; 0 394 | Value: Vertex Color 395 | Enabled: true 396 | Mesh Buffer Size: 1 397 | Name: TriangleMesh5 398 | Show Normals: 399 | Normals Alpha: 1 400 | Normals Color: 255; 0; 255 401 | Normals Scaling Factor: 0.10000000149011612 402 | Value: false 403 | Show Wireframe: 404 | Value: false 405 | Wireframe Alpha: 1 406 | Wireframe Color: 0; 0; 0 407 | Topic: /thoth/kimera_pgmo/optimized_mesh 408 | Value: true 409 | - Class: rviz_mesh_plugin/TriangleMesh 410 | Display Type: 411 | Faces Alpha: 1 412 | Faces Color: 0; 255; 0 413 | Value: Vertex Color 414 | Enabled: true 415 | Mesh Buffer Size: 1 416 | Name: TriangleMesh6 417 | Show Normals: 418 | Normals Alpha: 1 419 | Normals Color: 255; 0; 255 420 | Normals Scaling Factor: 0.10000000149011612 421 | Value: false 422 | Show Wireframe: 423 | Value: false 424 | Wireframe Alpha: 1 425 | Wireframe Color: 0; 0; 0 426 | Topic: /apis/kimera_pgmo/optimized_mesh 427 | Value: true 428 | - Class: rviz_mesh_plugin/TriangleMesh 429 | Display Type: 430 | Faces Alpha: 1 431 | Faces Color: 0; 255; 0 432 | Value: Vertex Color 433 | Enabled: true 434 | Mesh Buffer Size: 1 435 | Name: TriangleMesh7 436 | Show Normals: 437 | Normals Alpha: 1 438 | Normals Color: 255; 0; 255 439 | Normals Scaling Factor: 0.10000000149011612 440 | Value: false 441 | Show Wireframe: 442 | Value: false 443 | Wireframe Alpha: 1 444 | Wireframe Color: 0; 0; 0 445 | Topic: /sobek/kimera_pgmo/optimized_mesh 446 | Value: true 447 | - Class: voxblox_rviz_plugin/VoxbloxMesh 448 | Enabled: false 449 | Name: VoxbloxMesh0 450 | Topic: /robot0/kimera_semantics_node/mesh 451 | Unreliable: false 452 | Value: false 453 | - Class: voxblox_rviz_plugin/VoxbloxMesh 454 | Enabled: false 455 | Name: VoxbloxMesh1 456 | Topic: /robot1/kimera_semantics_node/mesh 457 | Unreliable: false 458 | Value: false 459 | - Class: voxblox_rviz_plugin/VoxbloxMesh 460 | Enabled: false 461 | Name: VoxbloxMesh2 462 | Topic: /robot2/kimera_semantics_node/mesh 463 | Unreliable: false 464 | Value: false 465 | - Class: voxblox_rviz_plugin/VoxbloxMesh 466 | Enabled: false 467 | Name: VoxbloxMesh3 468 | Topic: /robot3/kimera_semantics_node/mesh 469 | Unreliable: false 470 | Value: false 471 | Enabled: true 472 | Global Options: 473 | Background Color: 255; 255; 255 474 | Default Light: true 475 | Fixed Frame: world 476 | Frame Rate: 30 477 | Name: root 478 | Tools: 479 | - Class: rviz/Interact 480 | Hide Inactive Objects: true 481 | - Class: rviz/MoveCamera 482 | - Class: rviz/Select 483 | - Class: rviz/FocusCamera 484 | - Class: rviz/Measure 485 | - Class: rviz/SetInitialPose 486 | Theta std deviation: 0.2617993950843811 487 | Topic: /initialpose 488 | X std deviation: 0.5 489 | Y std deviation: 0.5 490 | - Class: rviz/SetGoal 491 | Topic: /move_base_simple/goal 492 | - Class: rviz/PublishPoint 493 | Single click: true 494 | Topic: /clicked_point 495 | Value: true 496 | Views: 497 | Current: 498 | Class: rviz/Orbit 499 | Distance: 54.73565673828125 500 | Enable Stereo Rendering: 501 | Stereo Eye Separation: 0.05999999865889549 502 | Stereo Focal Distance: 1 503 | Swap Stereo Eyes: false 504 | Value: false 505 | Focal Point: 506 | X: 0 507 | Y: 0 508 | Z: 0 509 | Focal Shape Fixed Size: false 510 | Focal Shape Size: 0.05000000074505806 511 | Invert Z Axis: false 512 | Name: Current View 513 | Near Clip Distance: 0.009999999776482582 514 | Pitch: 1.3753981590270996 515 | Target Frame: 516 | Value: Orbit (rviz) 517 | Yaw: 3.3053998947143555 518 | Saved: ~ 519 | Window Geometry: 520 | Displays: 521 | collapsed: false 522 | Height: 1025 523 | Hide Left Dock: false 524 | Hide Right Dock: true 525 | Image0: 526 | collapsed: false 527 | Image1: 528 | collapsed: false 529 | Image2: 530 | collapsed: false 531 | Image3: 532 | collapsed: false 533 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c0049006d006100670065003000000001bd0000006f0000001600fffffffb0000000c0049006d006100670065003100000001f9000000850000001600fffffffb0000000c0049006d0061006700650032000000024e000000a50000001600fffffffb0000000c0049006d006100670065003300000002d0000000d00000001600fffffffb0000000c0049006d00610067006500340000000479000000970000000000000000fb0000000c0049006d00610067006500350000000503000000a30000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 534 | Selection: 535 | collapsed: false 536 | Time: 537 | collapsed: false 538 | Tool Properties: 539 | collapsed: false 540 | Views: 541 | collapsed: true 542 | Width: 1853 543 | X: 67 544 | Y: 27 545 | -------------------------------------------------------------------------------- /src/DistributedLoopClosure.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include "kimera_distributed/DistributedLoopClosure.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace kimera_distributed { 26 | 27 | DistributedLoopClosure::DistributedLoopClosure() 28 | : lcd_(new lcd::LoopClosureDetector), 29 | num_inter_robot_loops_(0), 30 | backend_update_count_(0), 31 | last_get_submap_idx_(0), 32 | last_get_lc_idx_(0), 33 | bow_backlog_(0), 34 | vlc_backlog_(0) {} 35 | 36 | DistributedLoopClosure::~DistributedLoopClosure() { 37 | should_shutdown_ = true; 38 | 39 | if (detection_thread_) { 40 | detection_thread_->join(); 41 | detection_thread_.reset(); 42 | } 43 | 44 | if (verification_thread_) { 45 | verification_thread_->join(); 46 | verification_thread_.reset(); 47 | } 48 | 49 | if (comms_thread_) { 50 | comms_thread_->join(); 51 | comms_thread_.reset(); 52 | } 53 | } 54 | 55 | void DistributedLoopClosure::initialize(const DistributedLoopClosureConfig& config) { 56 | config_ = config; 57 | num_loops_with_robot_.assign(config_.num_robots_, 0); 58 | 59 | // Used for logging 60 | received_bow_bytes_.clear(); 61 | received_vlc_bytes_.clear(); 62 | 63 | // Load robot names and initialize candidate lc queues 64 | for (size_t id = 0; id < config_.num_robots_; id++) { 65 | candidate_lc_[id] = std::vector{}; 66 | loop_pub_initialized_[id] = false; 67 | } 68 | 69 | // Initialize LCD 70 | lcd_->loadAndInitialize(config_.lcd_params_); 71 | 72 | // Initialize submap atlas 73 | submap_atlas_.reset(new SubmapAtlas(config_.submap_params_)); 74 | 75 | if (config_.run_offline_) { 76 | for (size_t id = config.my_id_; id < config.num_robots_; ++id) { 77 | loadBowVectors( 78 | id, 79 | config.offline_dir_ + "robot_" + std::to_string(id) + "_bow_vectors.json"); 80 | loadVLCFrames( 81 | id, config.offline_dir_ + "robot_" + std::to_string(id) + "_vlc_frames.json"); 82 | } 83 | 84 | // Load odometry 85 | loadOdometryFromFile(config_.offline_dir_ + "odometry_poses.csv"); 86 | // Load original loop closures between keyframes 87 | loadLoopClosuresFromFile(config_.offline_dir_ + "loop_closures.csv"); 88 | } else { 89 | // Initialize log files to record keyframe poses and loop closures. 90 | if (config_.log_output_) { 91 | createLogFiles(); 92 | } 93 | } 94 | 95 | // Initially assume all robots are connected 96 | for (size_t robot_id = 0; robot_id < config_.num_robots_; ++robot_id) { 97 | robot_connected_[robot_id] = true; 98 | } 99 | } 100 | 101 | void DistributedLoopClosure::processBow( 102 | const pose_graph_tools_msgs::BowQueriesConstPtr& query_msg) { 103 | for (const auto& msg : query_msg->queries) { 104 | lcd::RobotId robot_id = msg.robot_id; 105 | lcd::PoseId pose_id = msg.pose_id; 106 | // This robot is responsible for detecting loop closures with others with a larger 107 | // ID 108 | CHECK_GE(robot_id, config_.my_id_); 109 | lcd::RobotPoseId vertex_query(robot_id, pose_id); 110 | if (bow_received_[robot_id].find(pose_id) != bow_received_[robot_id].end()) { 111 | // Skip if this vector has been received before 112 | continue; 113 | } 114 | bow_latest_[robot_id] = std::max(bow_latest_[robot_id], pose_id); 115 | bow_received_[robot_id].emplace(pose_id); 116 | { // start of BoW critical section 117 | std::unique_lock bow_lock(bow_msgs_mutex_); 118 | bow_msgs_.push_back(msg); 119 | } // end BoW critical section 120 | } 121 | } 122 | 123 | bool DistributedLoopClosure::processLocalPoseGraph( 124 | const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg) { 125 | // Parse odometry edges and create new keyframes in the submap atlas 126 | std::vector local_loop_closures; 127 | const uint64_t ts = msg->header.stamp.toNSec(); 128 | 129 | bool incremental_pub = true; 130 | if (submap_atlas_->numSubmaps() == 0 && !msg->nodes.empty()) { 131 | // Create the first keyframe 132 | // Start submap critical section 133 | // std::unique_lock submap_lock(submap_atlas_mutex_); 134 | 135 | gtsam::Rot3 init_rotation(msg->nodes[0].pose.orientation.w, 136 | msg->nodes[0].pose.orientation.x, 137 | msg->nodes[0].pose.orientation.y, 138 | msg->nodes[0].pose.orientation.z); 139 | gtsam::Point3 init_position(msg->nodes[0].pose.position.x, 140 | msg->nodes[0].pose.position.y, 141 | msg->nodes[0].pose.position.z); 142 | gtsam::Pose3 init_pose(init_rotation, init_position); 143 | 144 | if (config_.my_id_ == 0) { 145 | // Implicit assumption: first pose of the first robot in DPGO is set to identity 146 | T_world_dpgo_ = init_pose; 147 | } 148 | 149 | submap_atlas_->createKeyframe(0, init_pose, ts); 150 | logOdometryPose( 151 | gtsam::Symbol(robot_id_to_prefix.at(config_.my_id_), 0), init_pose, ts); 152 | incremental_pub = false; 153 | } 154 | 155 | // Extract timestamps of each new pose node 156 | std::map node_timestamps; 157 | for (const auto& pg_node : msg->nodes) { 158 | node_timestamps[(int)pg_node.key] = pg_node.header.stamp.toNSec(); 159 | } 160 | 161 | for (const auto& pg_edge : msg->edges) { 162 | if (pg_edge.robot_from == config_.my_id_ && pg_edge.robot_to == config_.my_id_ && 163 | pg_edge.type == pose_graph_tools_msgs::PoseGraphEdge::ODOM) { 164 | int frame_src = (int)pg_edge.key_from; 165 | int frame_dst = (int)pg_edge.key_to; 166 | CHECK_EQ(frame_src + 1, frame_dst); 167 | if (submap_atlas_->hasKeyframe(frame_src) && 168 | !submap_atlas_->hasKeyframe(frame_dst)) { 169 | // Start submap critical section 170 | // std::unique_lock submap_lock(submap_atlas_mutex_); 171 | // Check that the next keyframe has the expected id 172 | int expected_frame_id = submap_atlas_->numKeyframes(); 173 | if (frame_dst != expected_frame_id) { 174 | LOG(ERROR) << "Received unexpected keyframe! (received " << frame_dst 175 | << ", expected " << expected_frame_id << ")"; 176 | } 177 | 178 | // Use odometry to initialize the next keyframe 179 | lcd::VLCEdge keyframe_odometry; 180 | kimera_multi_lcd::VLCEdgeFromMsg(pg_edge, &keyframe_odometry); 181 | const auto T_src_dst = keyframe_odometry.T_src_dst_; 182 | const auto T_odom_src = 183 | submap_atlas_->getKeyframe(frame_src)->getPoseInOdomFrame(); 184 | const auto T_odom_dst = T_odom_src * T_src_dst; 185 | uint64_t node_ts = ts; 186 | if (node_timestamps.find(frame_dst) != node_timestamps.end()) 187 | node_ts = node_timestamps[frame_dst]; 188 | submap_atlas_->createKeyframe(frame_dst, T_odom_dst, node_ts); 189 | 190 | // Save keyframe pose to file 191 | gtsam::Symbol symbol_dst(robot_id_to_prefix.at(config_.my_id_), frame_dst); 192 | logOdometryPose(symbol_dst, T_odom_dst, node_ts); 193 | } 194 | } else if (pg_edge.robot_from == config_.my_id_ && 195 | pg_edge.robot_to == config_.my_id_ && 196 | pg_edge.type == pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE) { 197 | local_loop_closures.push_back(pg_edge); 198 | } 199 | } 200 | 201 | // Parse intra-robot loop closures 202 | for (const auto& pg_edge : local_loop_closures) { 203 | // Read loop closure between the keyframes 204 | lcd::VLCEdge keyframe_loop_closure; 205 | kimera_multi_lcd::VLCEdgeFromMsg(pg_edge, &keyframe_loop_closure); 206 | logLoopClosure(keyframe_loop_closure); 207 | const auto T_f1_f2 = keyframe_loop_closure.T_src_dst_; 208 | static const gtsam::SharedNoiseModel& noise = 209 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 210 | 211 | { 212 | // Start submap critical section 213 | // std::unique_lock submap_lock(submap_atlas_mutex_); 214 | // Convert the loop closure to between the corresponding submaps 215 | const auto keyframe_src = submap_atlas_->getKeyframe(pg_edge.key_from); 216 | const auto keyframe_dst = submap_atlas_->getKeyframe(pg_edge.key_to); 217 | if (!keyframe_src || !keyframe_dst) { 218 | LOG(ERROR) << "Received intra loop closure but keyframe does not exist!"; 219 | continue; 220 | } 221 | const auto submap_src = CHECK_NOTNULL(keyframe_src->getSubmap()); 222 | const auto submap_dst = CHECK_NOTNULL(keyframe_dst->getSubmap()); 223 | gtsam::Symbol from_key(robot_id_to_prefix.at(config_.my_id_), submap_src->id()); 224 | gtsam::Symbol to_key(robot_id_to_prefix.at(config_.my_id_), submap_dst->id()); 225 | // Skip this loop closure if two submaps are identical or consecutive 226 | if (std::abs(submap_src->id() - submap_dst->id()) <= 1) continue; 227 | // Skip this loop closure if a loop closure already exists between the two submaps 228 | if (hasBetweenFactor(submap_loop_closures_, from_key, to_key)) continue; 229 | const auto T_s1_f1 = keyframe_src->getPoseInSubmapFrame(); 230 | const auto T_s2_f2 = keyframe_dst->getPoseInSubmapFrame(); 231 | const auto T_s1_s2 = T_s1_f1 * T_f1_f2 * (T_s2_f2.inverse()); 232 | // Convert the loop closure to between the corresponding submaps 233 | submap_loop_closures_.add( 234 | gtsam::BetweenFactor(from_key, to_key, T_s1_s2, noise)); 235 | } 236 | } 237 | return incremental_pub; 238 | } 239 | 240 | void DistributedLoopClosure::processOptimizedPath(const nav_msgs::PathConstPtr& msg) { 241 | if (msg->poses.empty()) { 242 | return; 243 | } 244 | // Store the optimized poses from dpgo in the submap atlas 245 | for (int submap_id = 0; submap_id < msg->poses.size(); ++submap_id) { 246 | const auto T_dpgo_submap = RosPoseToGtsam(msg->poses[submap_id].pose); 247 | const auto T_world_submap = T_world_dpgo_.compose(T_dpgo_submap); 248 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 249 | submap->setPoseInWorldFrame(T_world_submap); 250 | } 251 | // Update the new (unoptimized) poses in the submap atlas by propagating odometry 252 | for (int submap_id = msg->poses.size(); submap_id < submap_atlas_->numSubmaps(); 253 | ++submap_id) { 254 | const auto submap_curr = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 255 | const auto submap_prev = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id - 1)); 256 | const auto T_odom_curr = submap_curr->getPoseInOdomFrame(); 257 | const auto T_odom_prev = submap_prev->getPoseInOdomFrame(); 258 | const auto T_prev_curr = T_odom_prev.inverse() * T_odom_curr; 259 | const auto T_world_curr = submap_prev->getPoseInWorldFrame() * T_prev_curr; 260 | submap_curr->setPoseInWorldFrame(T_world_curr); 261 | } 262 | backend_update_count_++; 263 | LOG(INFO) << "Received DPGO updates (current count: " << backend_update_count_ 264 | << ")."; 265 | } 266 | 267 | void DistributedLoopClosure::computePosesInWorldFrame( 268 | gtsam::Values::shared_ptr nodes) const { 269 | nodes->clear(); 270 | // Using the optimized submap poses from dpgo, recover optimized poses for the 271 | // original VIO keyframes 272 | for (int submap_id = 0; submap_id < submap_atlas_->numSubmaps(); ++submap_id) { 273 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 274 | const auto T_world_submap = submap->getPoseInWorldFrame(); 275 | for (const int keyframe_id : submap->getKeyframeIDs()) { 276 | const auto keyframe = CHECK_NOTNULL(submap->getKeyframe(keyframe_id)); 277 | const auto T_submap_keyframe = keyframe->getPoseInSubmapFrame(); 278 | const auto T_world_keyframe = T_world_submap * T_submap_keyframe; 279 | nodes->insert(keyframe_id, T_world_keyframe); 280 | } 281 | } 282 | } 283 | 284 | void DistributedLoopClosure::savePosesToFile(const std::string& filename, 285 | const gtsam::Values& nodes) const { 286 | std::ofstream file; 287 | file.open(filename); 288 | if (!file.is_open()) { 289 | LOG(ERROR) << "Error opening log file: " << filename; 290 | return; 291 | } 292 | file << std::fixed << std::setprecision(8); 293 | file << "ns,pose_index,qx,qy,qz,qw,tx,ty,tz\n"; 294 | 295 | for (const auto& key_pose : nodes) { 296 | gtsam::Quaternion quat = 297 | nodes.at(key_pose.key).rotation().toQuaternion(); 298 | gtsam::Point3 point = nodes.at(key_pose.key).translation(); 299 | const auto keyframe = submap_atlas_->getKeyframe(key_pose.key); 300 | file << keyframe->stamp() << ","; 301 | file << keyframe->id() << ","; 302 | file << quat.x() << ","; 303 | file << quat.y() << ","; 304 | file << quat.z() << ","; 305 | file << quat.w() << ","; 306 | file << point.x() << ","; 307 | file << point.y() << ","; 308 | file << point.z() << "\n"; 309 | } 310 | file.close(); 311 | } 312 | 313 | void DistributedLoopClosure::saveSubmapAtlas(const std::string& directory) const { 314 | // Store keyframes in their respective submap frames 315 | std::string keyframe_path = directory + "kimera_distributed_keyframes.csv"; 316 | std::ofstream keyframe_file; 317 | keyframe_file.open(keyframe_path); 318 | if (!keyframe_file.is_open()) { 319 | LOG(ERROR) << "Error opening log file: " << keyframe_path; 320 | return; 321 | } 322 | keyframe_file << std::fixed << std::setprecision(8); 323 | keyframe_file << "keyframe_stamp_ns,keyframe_id,submap_id,qx,qy,qz,qw,tx,ty,tz\n"; 324 | 325 | for (int submap_id = 0; submap_id < submap_atlas_->numSubmaps(); ++submap_id) { 326 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 327 | for (const int keyframe_id : submap->getKeyframeIDs()) { 328 | const auto keyframe = CHECK_NOTNULL(submap->getKeyframe(keyframe_id)); 329 | const auto T_submap_keyframe = keyframe->getPoseInSubmapFrame(); 330 | // Save to log 331 | gtsam::Quaternion quat = T_submap_keyframe.rotation().toQuaternion(); 332 | gtsam::Point3 point = T_submap_keyframe.translation(); 333 | keyframe_file << keyframe->stamp() << ","; 334 | keyframe_file << keyframe_id << ","; 335 | keyframe_file << submap_id << ","; 336 | keyframe_file << quat.x() << ","; 337 | keyframe_file << quat.y() << ","; 338 | keyframe_file << quat.z() << ","; 339 | keyframe_file << quat.w() << ","; 340 | keyframe_file << point.x() << ","; 341 | keyframe_file << point.y() << ","; 342 | keyframe_file << point.z() << "\n"; 343 | } 344 | } 345 | keyframe_file.close(); 346 | 347 | // Store submaps in the robot's local odometry frame 348 | std::string submap_path = directory + "kimera_distributed_submaps.csv"; 349 | std::ofstream submap_file; 350 | submap_file.open(submap_path); 351 | if (!submap_file.is_open()) { 352 | LOG(ERROR) << "Error opening log file: " << submap_path; 353 | return; 354 | } 355 | submap_file << std::fixed << std::setprecision(8); 356 | submap_file << "submap_stamp_ns,submap_id,qx,qy,qz,qw,tx,ty,tz\n"; 357 | for (int submap_id = 0; submap_id < submap_atlas_->numSubmaps(); ++submap_id) { 358 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 359 | gtsam::Quaternion quat = submap->getPoseInOdomFrame().rotation().toQuaternion(); 360 | gtsam::Point3 point = submap->getPoseInOdomFrame().translation(); 361 | submap_file << submap->stamp() << ","; 362 | submap_file << submap_id << ","; 363 | submap_file << quat.x() << ","; 364 | submap_file << quat.y() << ","; 365 | submap_file << quat.z() << ","; 366 | submap_file << quat.w() << ","; 367 | submap_file << point.x() << ","; 368 | submap_file << point.y() << ","; 369 | submap_file << point.z() << "\n"; 370 | } 371 | submap_file.close(); 372 | } 373 | 374 | bool DistributedLoopClosure::queryBowVectorsRequest( 375 | lcd::RobotId& selected_robot_id, 376 | std::set& bow_vectors) { 377 | // Form BoW vectors that are missing from each robot 378 | std::map> missing_bow_vectors; 379 | for (lcd::RobotId robot_id = config_.my_id_; robot_id < config_.num_robots_; 380 | ++robot_id) { 381 | const auto& received_pose_ids = bow_received_.at(robot_id); 382 | const lcd::PoseId latest_pose_id = bow_latest_.at(robot_id); 383 | for (lcd::PoseId pose_id = 0; pose_id < latest_pose_id; 384 | pose_id += config_.bow_skip_num_) { 385 | if (received_pose_ids.find(pose_id) == received_pose_ids.end()) { 386 | if (robot_id == config_.my_id_) { 387 | // Missing BoW from myself. 388 | // This should not happen we should receive BoW directly from VIO 389 | LOG(ERROR) << "Robot " << robot_id << " cannot find BoW of itself! Missing " 390 | << pose_id << " (latest = " << latest_pose_id << ")."; 391 | } else { 392 | // Push to missing bow_vectors 393 | if (missing_bow_vectors.find(robot_id) == missing_bow_vectors.end()) 394 | missing_bow_vectors[robot_id] = std::set(); 395 | missing_bow_vectors[robot_id].emplace(pose_id); 396 | } 397 | } 398 | } 399 | } 400 | 401 | // Select robot with largest queue size 402 | bow_backlog_ = 0; 403 | selected_robot_id = 0; 404 | size_t selected_queue_size = 0; 405 | for (const auto& it : missing_bow_vectors) { 406 | lcd::RobotId robot_id = it.first; 407 | size_t robot_queue_size = it.second.size(); 408 | bow_backlog_ += robot_queue_size; 409 | LOG(WARNING) << "Missing " << robot_queue_size << " bow vectors from robot " 410 | << robot_id << "."; 411 | if (robot_connected_.at(robot_id) && robot_queue_size >= selected_queue_size) { 412 | selected_queue_size = robot_queue_size; 413 | selected_robot_id = robot_id; 414 | } 415 | } 416 | if (selected_queue_size == 0) { 417 | return false; 418 | } 419 | 420 | bow_vectors = missing_bow_vectors[selected_robot_id]; 421 | return true; 422 | } 423 | 424 | bool DistributedLoopClosure::queryBowVectorsPublish(lcd::RobotId& selected_robot_id, 425 | lcd::RobotPoseIdSet& bow_vectors) { 426 | std::unique_lock requested_bows_lock(requested_bows_mutex_); 427 | 428 | // Select the robot with the largest queue size 429 | selected_robot_id = 0; 430 | size_t selected_queue_size = 0; 431 | for (const auto& it : requested_bows_) { 432 | lcd::RobotId robot_id = it.first; 433 | size_t robot_queue_size = it.second.size(); 434 | if (robot_connected_[robot_id] && robot_queue_size >= selected_queue_size) { 435 | selected_robot_id = robot_id; 436 | selected_queue_size = robot_queue_size; 437 | } 438 | } 439 | 440 | if (selected_queue_size == 0) { 441 | return false; 442 | } 443 | 444 | std::set& requested_bows_from_robot = 445 | requested_bows_.at(selected_robot_id); 446 | auto it = requested_bows_from_robot.begin(); 447 | while (true) { 448 | if (bow_vectors.size() >= config_.bow_batch_size_) { 449 | break; 450 | } 451 | 452 | if (it == requested_bows_from_robot.end()) { 453 | break; 454 | } 455 | 456 | lcd::PoseId pose_id = *it; 457 | it = requested_bows_from_robot.erase( 458 | it); // remove the current ID and proceed to next one 459 | lcd::RobotPoseId requested_robot_pose_id(config_.my_id_, pose_id); 460 | if (!lcd_->bowExists(requested_robot_pose_id)) { 461 | LOG(ERROR) << "Requested BoW of frame " << pose_id << " does not exist!"; 462 | continue; 463 | } 464 | bow_vectors.insert(requested_robot_pose_id); 465 | } 466 | return true; 467 | } 468 | 469 | void DistributedLoopClosure::queryFramesRequest( 470 | lcd::RobotPoseIdSet& local_vertex_ids, 471 | lcd::RobotId& selected_robot_id, 472 | lcd::RobotPoseIdSet& selected_vertex_ids) const { 473 | std::unordered_map vertex_ids_map; 474 | for (const auto robot_queue : candidate_lc_) { 475 | // Form list of vertex ids that needs to be requested 476 | for (const auto& cand : robot_queue.second) { 477 | if (!lcd_->frameExists(cand.vertex_src_)) { 478 | const size_t& robot_id = cand.vertex_src_.first; 479 | if (vertex_ids_map.count(robot_id) == 0) { 480 | vertex_ids_map[robot_id] = lcd::RobotPoseIdSet(); 481 | } 482 | vertex_ids_map.at(robot_id).emplace(cand.vertex_src_); 483 | } 484 | if (!lcd_->frameExists(cand.vertex_dst_)) { 485 | const size_t& robot_id = cand.vertex_dst_.first; 486 | if (vertex_ids_map.count(robot_id) == 0) { 487 | vertex_ids_map[robot_id] = lcd::RobotPoseIdSet(); 488 | } 489 | vertex_ids_map.at(robot_id).emplace(cand.vertex_dst_); 490 | } 491 | } 492 | } 493 | 494 | // Process missing VLC frames of myself 495 | if (vertex_ids_map.count(config_.my_id_) && 496 | !vertex_ids_map.at(config_.my_id_).empty()) { 497 | local_vertex_ids = vertex_ids_map.at(config_.my_id_); 498 | } 499 | 500 | // Select a peer robot with most missing frames to send request 501 | selected_robot_id = 0; 502 | size_t selected_queue_size = 0; 503 | for (const auto& it : vertex_ids_map) { 504 | lcd::RobotId robot_id = it.first; 505 | size_t robot_queue_size = it.second.size(); 506 | if (robot_id == config_.my_id_ || !robot_connected_.at(robot_id)) { 507 | continue; 508 | } 509 | if (robot_queue_size >= selected_queue_size) { 510 | selected_queue_size = robot_queue_size; 511 | selected_robot_id = robot_id; 512 | } 513 | } 514 | 515 | if (selected_queue_size > 0) { 516 | selected_vertex_ids = vertex_ids_map.at(selected_robot_id); 517 | } 518 | } 519 | 520 | void DistributedLoopClosure::queryFramesPublish( 521 | lcd::RobotId& selected_robot_id, 522 | lcd::RobotPoseIdSet& selected_vertex_ids) { 523 | std::unique_lock requested_frames_lock(requested_frames_mutex_); 524 | 525 | // Select the robot with the largest queue size 526 | selected_robot_id = 0; 527 | size_t selected_queue_size = 0; 528 | for (const auto& it : requested_frames_) { 529 | lcd::RobotId robot_id = it.first; 530 | size_t robot_queue_size = it.second.size(); 531 | if (robot_connected_[robot_id] && robot_queue_size >= selected_queue_size) { 532 | selected_robot_id = robot_id; 533 | selected_queue_size = robot_queue_size; 534 | } 535 | } 536 | // ROS_INFO("Maximum num of VLC waiting: %zu (robot %zu).", selected_queue_size, 537 | // selected_robot_id); 538 | 539 | if (selected_queue_size > 0) { 540 | // Send VLC frames to the selected robot 541 | std::set& requested_frames_from_robot = 542 | requested_frames_.at(selected_robot_id); 543 | auto it = requested_frames_from_robot.begin(); 544 | while (true) { 545 | if (selected_vertex_ids.size() >= config_.vlc_batch_size_) { 546 | break; 547 | } 548 | if (it == requested_frames_from_robot.end()) { 549 | break; 550 | } 551 | lcd::RobotPoseId vertex_id(config_.my_id_, *it); 552 | if (lcd_->frameExists(vertex_id)) { 553 | selected_vertex_ids.insert(vertex_id); 554 | } 555 | it = requested_frames_from_robot.erase( 556 | it); // remove the current ID and proceed to next one 557 | } 558 | } 559 | } 560 | 561 | void DistributedLoopClosure::detectLoopSpin() { 562 | std::unique_lock bow_lock(bow_msgs_mutex_); 563 | auto it = bow_msgs_.begin(); 564 | int num_detection_performed = 0; 565 | 566 | while (num_detection_performed < config_.detection_batch_size_) { 567 | if (it == bow_msgs_.end()) { 568 | break; 569 | } 570 | const pose_graph_tools_msgs::BowQuery msg = *it; 571 | lcd::RobotId query_robot = msg.robot_id; 572 | lcd::PoseId query_pose = msg.pose_id; 573 | lcd::RobotPoseId query_vertex(query_robot, query_pose); 574 | DBoW2::BowVector bow_vec; 575 | kimera_multi_lcd::BowVectorFromMsg(msg.bow_vector, &bow_vec); 576 | 577 | if (query_pose <= 2 * config_.bow_skip_num_) { 578 | // We cannot detect loop for the very first few frames 579 | // Remove and proceed to next message 580 | LOG(INFO) << "Received initial BoW from robot " << query_robot << " (pose " 581 | << query_pose << ")."; 582 | lcd_->addBowVector(query_vertex, bow_vec); 583 | it = bow_msgs_.erase(it); 584 | } else if (!lcd_->findPreviousBoWVector(query_vertex)) { 585 | // We cannot detect loop since the BoW of previous frame is missing 586 | // (Recall we need that to compute nss factor) 587 | // In this case we skip the message and try again later 588 | // ROS_WARN("Cannot detect loop for (%zu,%zu) because previous BoW is missing.", 589 | // query_robot, query_pose); 590 | ++it; 591 | } else { 592 | // We are ready to detect loop for this query message 593 | // ROS_INFO("Detect loop for (%zu,%zu).", query_robot, query_pose); 594 | detectLoop(query_vertex, bow_vec); 595 | num_detection_performed++; 596 | // Inter-robot queries will count as communication payloads 597 | if (query_robot != config_.my_id_) { 598 | received_bow_bytes_.push_back( 599 | kimera_multi_lcd::computeBowQueryPayloadBytes(msg)); 600 | } 601 | lcd_->addBowVector(query_vertex, bow_vec); 602 | it = bow_msgs_.erase(it); // Erase this message and move on to next one 603 | } 604 | } 605 | // if (!bow_msgs_.empty()) { 606 | // ROS_INFO("Performed %d loop detection (%zu pending).", num_detection_performed, 607 | // bow_msgs_.size()); 608 | //} 609 | } 610 | 611 | void DistributedLoopClosure::detectLoop(const lcd::RobotPoseId& vertex_query, 612 | const DBoW2::BowVector& bow_vec) { 613 | const lcd::RobotId robot_query = vertex_query.first; 614 | const lcd::PoseId pose_query = vertex_query.second; 615 | std::vector vertex_matches; 616 | std::vector match_scores; 617 | { // start lcd critical section 618 | std::unique_lock lcd_lock(lcd_mutex_); 619 | 620 | // Incoming bow vector is from my trajectory 621 | // Detect loop closures with all robots in the database 622 | // (including myself if inter_robot_only is set to false) 623 | if (robot_query == config_.my_id_) { 624 | if (lcd_->detectLoop(vertex_query, bow_vec, &vertex_matches, &match_scores)) { 625 | for (size_t i = 0; i < vertex_matches.size(); ++i) { 626 | lcd::PotentialVLCEdge potential_edge( 627 | vertex_query, vertex_matches[i], match_scores[i]); 628 | 629 | { // start candidate critical section. Add to candidate for request 630 | std::unique_lock candidate_lock(candidate_lc_mutex_); 631 | candidate_lc_.at(robot_query).push_back(potential_edge); 632 | } // end candidate critical section 633 | } 634 | } 635 | } 636 | 637 | // Incoming bow vector is from another robot 638 | // Detect loop closures ONLY with my trajectory 639 | if (robot_query != config_.my_id_) { 640 | if (lcd_->detectLoopWithRobot( 641 | config_.my_id_, vertex_query, bow_vec, &vertex_matches, &match_scores)) { 642 | for (size_t i = 0; i < vertex_matches.size(); ++i) { 643 | lcd::PotentialVLCEdge potential_edge( 644 | vertex_query, vertex_matches[i], match_scores[i]); 645 | 646 | { 647 | // start candidate critical section. Add to candidate for request 648 | std::unique_lock candidate_lock(candidate_lc_mutex_); 649 | candidate_lc_.at(robot_query).push_back(potential_edge); 650 | } // end candidate critical section 651 | } 652 | } 653 | } 654 | } // end lcd critical section 655 | } 656 | 657 | void DistributedLoopClosure::verifyLoopSpin() { 658 | while (queued_lc_.size() > 0) { 659 | // Attempt to detect a single loop closure 660 | lcd::PotentialVLCEdge potential_edge = queued_lc_.front(); 661 | const auto& vertex_query = potential_edge.vertex_src_; 662 | const auto& vertex_match = potential_edge.vertex_dst_; 663 | const double match_score = potential_edge.score_; 664 | 665 | { // start lcd critical section 666 | std::unique_lock lcd_lock(lcd_mutex_); 667 | // Both frames should already exist locally 668 | CHECK(lcd_->frameExists(vertex_query)); 669 | CHECK(lcd_->frameExists(vertex_match)); 670 | 671 | // Find correspondences between frames. 672 | std::vector i_query, i_match; 673 | lcd_->computeMatchedIndices(vertex_query, vertex_match, &i_query, &i_match); 674 | assert(i_query.size() == i_match.size()); 675 | 676 | // Geometric verification 677 | gtsam::Rot3 monoR_query_match; 678 | gtsam::Pose3 T_query_match; 679 | // Perform monocular RANSAC 680 | if (lcd_->geometricVerificationNister( 681 | vertex_query, vertex_match, &i_query, &i_match, &monoR_query_match)) { 682 | size_t mono_inliers_count = i_query.size(); 683 | 684 | // Perform stereo RANSAC, using relative rotation estimate from mono RANSAC as 685 | // prior 686 | if (lcd_->recoverPose(vertex_query, 687 | vertex_match, 688 | &i_query, 689 | &i_match, 690 | &T_query_match, 691 | &monoR_query_match)) { 692 | size_t stereo_inliers_count = i_query.size(); 693 | // Get loop closure between keyframes (for logging purpose) 694 | lcd::VLCEdge keyframe_edge(vertex_query, vertex_match, T_query_match); 695 | keyframe_edge.normalized_bow_score_ = match_score; 696 | keyframe_edge.mono_inliers_ = mono_inliers_count; 697 | keyframe_edge.stereo_inliers_ = stereo_inliers_count; 698 | keyframe_edge.stamp_ns_ = ros::Time::now().toNSec(); 699 | const auto frame1 = lcd_->getVLCFrame(vertex_query); 700 | const auto frame2 = lcd_->getVLCFrame(vertex_match); 701 | gtsam::Symbol keyframe_from(robot_id_to_prefix.at(frame1.robot_id_), 702 | frame1.pose_id_); 703 | gtsam::Symbol keyframe_to(robot_id_to_prefix.at(frame2.robot_id_), 704 | frame2.pose_id_); 705 | static const gtsam::SharedNoiseModel& noise = 706 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 707 | 708 | // Log loop closure between keyframes 709 | keyframe_loop_closures_.add(gtsam::BetweenFactor( 710 | keyframe_from, keyframe_to, T_query_match, noise)); 711 | logLoopClosure(keyframe_edge); 712 | num_inter_robot_loops_++; 713 | lcd::RobotId other_robot = 0; 714 | if (vertex_query.first == config_.my_id_) { 715 | other_robot = vertex_match.first; 716 | } else { 717 | other_robot = vertex_query.first; 718 | } 719 | num_loops_with_robot_[other_robot] += 1; 720 | 721 | // Get loop closure between the corresponding two submaps 722 | const auto T_s1_f1 = frame1.T_submap_pose_; 723 | const auto T_s2_f2 = frame2.T_submap_pose_; 724 | const auto T_f1_f2 = T_query_match; 725 | const auto T_s1_s2 = T_s1_f1 * T_f1_f2 * (T_s2_f2.inverse()); 726 | gtsam::Symbol submap_from(robot_id_to_prefix.at(frame1.robot_id_), 727 | frame1.submap_id_); 728 | gtsam::Symbol submap_to(robot_id_to_prefix.at(frame2.robot_id_), 729 | frame2.submap_id_); 730 | 731 | // Skip intra loop connecting identical or consecutive submaps from the same 732 | // robot 733 | const int robot_id_from = frame1.robot_id_; 734 | const int robot_id_to = frame2.robot_id_; 735 | const int submap_id_from = frame1.submap_id_; 736 | const int submap_id_to = frame2.submap_id_; 737 | if (robot_id_from == robot_id_to && abs(submap_id_from - submap_id_to) <= 1) { 738 | continue; 739 | } 740 | 741 | // Add this loop closure if no loop closure exists between the two submaps 742 | // This ensures that there is at most one loop closure between every pair of 743 | // submaps 744 | lcd::EdgeID submap_edge_id( 745 | frame1.robot_id_, frame1.submap_id_, frame2.robot_id_, frame2.submap_id_); 746 | bool loop_exists = (submap_loop_closures_ids_.find(submap_edge_id) != 747 | submap_loop_closures_ids_.end()); 748 | if (!loop_exists) { 749 | submap_loop_closures_ids_.emplace(submap_edge_id); 750 | // Add new loop to queue for synchronization with other robots 751 | submap_loop_closures_queue_[submap_edge_id] = 752 | gtsam::BetweenFactor( 753 | submap_from, submap_to, T_s1_s2, noise); 754 | // submap_loop_closures_.add(gtsam::BetweenFactor( 755 | // submap_from, submap_to, T_s1_s2, noise)); 756 | } 757 | LOG(INFO) << "Verified loop (" << vertex_query.first << "," 758 | << vertex_query.second << ")-(" << vertex_match.first << "," 759 | << vertex_match.second << "). Normalized BoW score: " << match_score 760 | << ". Mono inliers : " << mono_inliers_count 761 | << ". Stereo inliers : " << stereo_inliers_count << "."; 762 | } 763 | } 764 | } // end lcd critical section 765 | queued_lc_.pop(); 766 | } 767 | } 768 | 769 | pose_graph_tools_msgs::PoseGraph DistributedLoopClosure::getSubmapPoseGraph( 770 | bool incremental) { 771 | // Start submap critical section 772 | // std::unique_lock submap_lock(submap_atlas_mutex_); 773 | // Fill in submap-level loop closures 774 | pose_graph_tools_msgs::PoseGraph out_graph; 775 | 776 | if (submap_atlas_->numSubmaps() == 0) { 777 | return out_graph; 778 | } 779 | 780 | if (incremental) { 781 | gtsam::NonlinearFactorGraph new_loop_closures( 782 | submap_loop_closures_.begin() + last_get_lc_idx_, submap_loop_closures_.end()); 783 | out_graph = GtsamGraphToRos(new_loop_closures, gtsam::Values()); 784 | last_get_lc_idx_ = submap_loop_closures_.size(); 785 | } else { 786 | out_graph = GtsamGraphToRos(submap_loop_closures_, gtsam::Values()); 787 | } 788 | const std::string robot_name = config_.robot_names_.at(config_.my_id_); 789 | 790 | // Fill in submap-level odometry 791 | size_t start_idx = incremental ? last_get_submap_idx_ : 0; 792 | size_t end_idx = submap_atlas_->numSubmaps() - 1; 793 | for (int submap_id = start_idx; submap_id < end_idx; ++submap_id) { 794 | pose_graph_tools_msgs::PoseGraphEdge edge; 795 | const auto submap_src = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 796 | const auto submap_dst = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id + 1)); 797 | const auto T_odom_src = submap_src->getPoseInOdomFrame(); 798 | const auto T_odom_dst = submap_dst->getPoseInOdomFrame(); 799 | const auto T_src_dst = (T_odom_src.inverse()) * T_odom_dst; 800 | edge.robot_from = config_.my_id_; 801 | edge.robot_to = config_.my_id_; 802 | edge.key_from = submap_src->id(); 803 | edge.key_to = submap_dst->id(); 804 | edge.type = pose_graph_tools_msgs::PoseGraphEdge::ODOM; 805 | edge.pose = GtsamPoseToRos(T_src_dst); 806 | edge.header.stamp.fromNSec(submap_dst->stamp()); 807 | // TODO(yun) add frame id param 808 | edge.header.frame_id = config_.frame_id_; 809 | out_graph.edges.push_back(edge); 810 | } 811 | 812 | // Fill in submap nodes 813 | start_idx = (incremental) ? start_idx + 1 : start_idx; 814 | for (int submap_id = start_idx; submap_id < end_idx + 1; ++submap_id) { 815 | pose_graph_tools_msgs::PoseGraphNode node; 816 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 817 | node.robot_id = config_.my_id_; 818 | node.key = submap->id(); 819 | node.header.stamp.fromNSec(submap->stamp()); 820 | node.header.frame_id = config_.frame_id_; 821 | node.pose = GtsamPoseToRos(submap->getPoseInOdomFrame()); 822 | out_graph.nodes.push_back(node); 823 | out_graph.header.stamp.fromNSec(submap->stamp()); 824 | out_graph.header.frame_id = config_.frame_id_; 825 | } 826 | 827 | if (incremental) { 828 | last_get_submap_idx_ = end_idx; 829 | } 830 | 831 | return out_graph; 832 | } 833 | 834 | void DistributedLoopClosure::processInternalVLC( 835 | const pose_graph_tools_msgs::VLCFramesConstPtr& msg) { 836 | for (const auto& frame_msg : msg->frames) { 837 | lcd::VLCFrame frame; 838 | kimera_multi_lcd::VLCFrameFromMsg(frame_msg, &frame); 839 | if (frame.robot_id_ != config_.my_id_) { 840 | continue; 841 | } 842 | 843 | // Fill in submap information for this keyframe 844 | const auto keyframe = submap_atlas_->getKeyframe(frame.pose_id_); 845 | if (!keyframe) { 846 | LOG(WARNING) << "Received internal VLC frame " << frame.pose_id_ 847 | << " but submap info is not found."; 848 | continue; 849 | } 850 | frame.submap_id_ = CHECK_NOTNULL(keyframe->getSubmap())->id(); 851 | frame.T_submap_pose_ = keyframe->getPoseInSubmapFrame(); 852 | 853 | // Store new frame 854 | lcd::RobotPoseId vertex_id(frame.robot_id_, frame.pose_id_); 855 | { // start lcd critical section 856 | std::unique_lock lcd_lock(lcd_mutex_); 857 | lcd_->addVLCFrame(vertex_id, frame); 858 | } // end lcd critical section 859 | } 860 | } 861 | 862 | size_t DistributedLoopClosure::updateCandidateList() { 863 | // return total number of candidates still missing VLC frames 864 | size_t total_candidates = 0; 865 | size_t ready_candidates = 0; 866 | // Recompute backlog on missing VLC frames 867 | vlc_backlog_ = 0; 868 | // start candidate list critical section 869 | std::unique_lock candidate_lock(candidate_lc_mutex_); 870 | for (const auto& robot_queue : candidate_lc_) { 871 | // Create new vector of candidates still missing VLC frames 872 | std::vector unresolved_candidates; 873 | for (const auto& candidate : robot_queue.second) { 874 | if (!lcd_->frameExists(candidate.vertex_src_)) { 875 | vlc_backlog_++; 876 | } 877 | if (!lcd_->frameExists(candidate.vertex_dst_)) { 878 | vlc_backlog_++; 879 | } 880 | if (lcd_->frameExists(candidate.vertex_src_) && 881 | lcd_->frameExists(candidate.vertex_dst_)) { 882 | queued_lc_.push(candidate); 883 | ready_candidates++; 884 | } else { 885 | unresolved_candidates.push_back(candidate); 886 | total_candidates++; 887 | } 888 | } 889 | // Update candidate list 890 | candidate_lc_[robot_queue.first] = unresolved_candidates; 891 | } 892 | LOG(INFO) << "Loop closure candidates ready for verification: " << ready_candidates 893 | << ", waiting for frames: " << total_candidates; 894 | return total_candidates; 895 | } 896 | 897 | gtsam::Pose3 DistributedLoopClosure::getOdomInWorldFrame() const { 898 | gtsam::Pose3 T_world_odom = gtsam::Pose3(); 899 | const auto keyframe = submap_atlas_->getLatestKeyframe(); 900 | if (keyframe) { 901 | const auto submap = keyframe->getSubmap(); 902 | CHECK_NOTNULL(submap); 903 | const gtsam::Pose3 T_odom_kf = keyframe->getPoseInOdomFrame(); 904 | const gtsam::Pose3 T_submap_kf = keyframe->getPoseInSubmapFrame(); 905 | const gtsam::Pose3 T_world_submap = submap->getPoseInWorldFrame(); 906 | const gtsam::Pose3 T_world_kf = T_world_submap * T_submap_kf; 907 | T_world_odom = T_world_kf * (T_odom_kf.inverse()); 908 | } 909 | return T_world_odom; 910 | } 911 | 912 | gtsam::Pose3 DistributedLoopClosure::getLatestKFInWorldFrame() const { 913 | gtsam::Pose3 T_world_kf = gtsam::Pose3(); 914 | const auto keyframe = submap_atlas_->getLatestKeyframe(); 915 | if (keyframe) { 916 | const auto submap = keyframe->getSubmap(); 917 | CHECK_NOTNULL(submap); 918 | const gtsam::Pose3 T_submap_kf = keyframe->getPoseInSubmapFrame(); 919 | const gtsam::Pose3 T_world_submap = submap->getPoseInWorldFrame(); 920 | T_world_kf = T_world_submap * T_submap_kf; 921 | } 922 | return T_world_kf; 923 | } 924 | 925 | gtsam::Pose3 DistributedLoopClosure::getLatestKFInOdomFrame() const { 926 | gtsam::Pose3 T_odom_kf = gtsam::Pose3(); 927 | const auto keyframe = submap_atlas_->getLatestKeyframe(); 928 | if (keyframe) { 929 | T_odom_kf = keyframe->getPoseInOdomFrame(); 930 | } 931 | return T_odom_kf; 932 | } 933 | 934 | void DistributedLoopClosure::saveBowVectors(const std::string& filepath) const { 935 | for (const auto& robot_pose_id : bow_latest_) { 936 | std::string bow_vector_save = filepath + "/robot_" + 937 | std::to_string(robot_pose_id.first) + 938 | "_bow_vectors.json"; 939 | LOG(INFO) << "Saving loop closure BoWs to " << bow_vector_save; 940 | lcd::saveBowVectors(lcd_->getBoWVectors(robot_pose_id.first), bow_vector_save); 941 | } 942 | } 943 | 944 | void DistributedLoopClosure::saveVLCFrames(const std::string& filepath) const { 945 | for (const auto& robot_pose_id : bow_latest_) { 946 | std::string vlc_frames_save = 947 | filepath + "/robot_" + std::to_string(robot_pose_id.first) + "_vlc_frames.json"; 948 | LOG(INFO) << "Saving loop closure Frames to " << vlc_frames_save; 949 | lcd::saveVLCFrames(lcd_->getVLCFrames(robot_pose_id.first), vlc_frames_save); 950 | } 951 | } 952 | 953 | void DistributedLoopClosure::loadBowVectors(size_t robot_id, 954 | const std::string& bow_json) { 955 | ROS_INFO_STREAM("Loading BoW vectors from " << bow_json); 956 | std::map bow_vectors; 957 | lcd::loadBowVectors(bow_json, bow_vectors); 958 | ROS_INFO_STREAM("Loaded " << bow_vectors.size() << " BoW vectors."); 959 | bow_latest_[robot_id] = 0; 960 | bow_received_[robot_id] = std::unordered_set(); 961 | for (const auto& id_bow : bow_vectors) { 962 | lcd::RobotPoseId id(robot_id, id_bow.first); 963 | lcd_->addBowVector(id, id_bow.second); 964 | bow_latest_[robot_id] = id_bow.first; 965 | bow_received_[robot_id].insert(id_bow.first); 966 | } 967 | } 968 | 969 | void DistributedLoopClosure::loadVLCFrames(size_t robot_id, 970 | const std::string& vlc_json) { 971 | ROS_INFO_STREAM("Loading VLC frames from " << vlc_json); 972 | std::map vlc_frames; 973 | lcd::loadVLCFrames(vlc_json, vlc_frames); 974 | ROS_INFO_STREAM("Loaded " << vlc_frames.size() << " VLC frames."); 975 | 976 | for (const auto& id_vlc : vlc_frames) { 977 | lcd::RobotPoseId id(robot_id, id_vlc.first); 978 | lcd_->addVLCFrame(id, id_vlc.second); 979 | } 980 | } 981 | 982 | void DistributedLoopClosure::createLogFiles() { 983 | std::string pose_file_path = config_.log_output_dir_ + "odometry_poses.csv"; 984 | std::string inter_lc_file_path = config_.log_output_dir_ + "loop_closures.csv"; 985 | std::string lcd_file_path = config_.log_output_dir_ + "lcd_log.csv"; 986 | 987 | odometry_file_.open(pose_file_path); 988 | if (!odometry_file_.is_open()) 989 | LOG(ERROR) << "Error opening log file: " << pose_file_path; 990 | odometry_file_ << std::fixed << std::setprecision(15); 991 | odometry_file_ << "stamp_ns,robot_index,pose_index,qx,qy,qz,qw,tx,ty,tz\n"; 992 | odometry_file_.flush(); 993 | 994 | loop_closure_file_.open(inter_lc_file_path); 995 | if (!loop_closure_file_.is_open()) 996 | LOG(ERROR) << "Error opening log file: " << inter_lc_file_path; 997 | loop_closure_file_ << std::fixed << std::setprecision(15); 998 | loop_closure_file_ << "robot1,pose1,robot2,pose2,qx,qy,qz,qw,tx,ty,tz,norm_bow_score," 999 | "mono_inliers,stereo_inliers,stamp_ns\n"; 1000 | loop_closure_file_.flush(); 1001 | 1002 | lcd_log_file_.open(lcd_file_path); 1003 | if (!lcd_log_file_.is_open()) { 1004 | LOG(ERROR) << "Error opening log file: " << lcd_file_path; 1005 | } 1006 | lcd_log_file_ << std::fixed << std::setprecision(15); 1007 | lcd_log_file_ << "stamp_ns, bow_matches, mono_verifications, stereo_verifications, " 1008 | "num_loop_closures, bow_bytes, vlc_bytes, bow_backlog, vlc_backlog, " 1009 | "num_loops_with_robots\n"; 1010 | lcd_log_file_.flush(); 1011 | } 1012 | 1013 | void DistributedLoopClosure::closeLogFiles() { 1014 | if (odometry_file_.is_open()) odometry_file_.close(); 1015 | if (loop_closure_file_.is_open()) loop_closure_file_.close(); 1016 | if (lcd_log_file_.is_open()) { 1017 | lcd_log_file_.close(); 1018 | } 1019 | } 1020 | 1021 | void DistributedLoopClosure::logLcdStat() { 1022 | if (lcd_log_file_.is_open()) { 1023 | // auto elapsed_sec = (ros::Time::now() - start_time_).toSec(); 1024 | lcd_log_file_ << ros::Time::now().toNSec() << ","; 1025 | lcd_log_file_ << lcd_->totalBoWMatches() << ","; 1026 | lcd_log_file_ << lcd_->getNumGeomVerificationsMono() << ","; 1027 | lcd_log_file_ << lcd_->getNumGeomVerifications() << ","; 1028 | lcd_log_file_ << keyframe_loop_closures_.size() << ","; 1029 | lcd_log_file_ << std::accumulate( 1030 | received_bow_bytes_.begin(), received_bow_bytes_.end(), 0) 1031 | << ","; 1032 | lcd_log_file_ << std::accumulate( 1033 | received_vlc_bytes_.begin(), received_vlc_bytes_.end(), 0) 1034 | << ","; 1035 | lcd_log_file_ << bow_backlog_ << ","; 1036 | lcd_log_file_ << vlc_backlog_ << ","; 1037 | for (size_t robot_id = 0; robot_id < config_.num_robots_; ++robot_id) { 1038 | lcd_log_file_ << num_loops_with_robot_[robot_id] << ","; 1039 | } 1040 | lcd_log_file_ << "\n"; 1041 | lcd_log_file_.flush(); 1042 | } 1043 | } 1044 | 1045 | void DistributedLoopClosure::logOdometryPose(const gtsam::Symbol& symbol_frame, 1046 | const gtsam::Pose3& T_odom_frame, 1047 | uint64_t ts) { 1048 | if (odometry_file_.is_open()) { 1049 | gtsam::Quaternion quat = T_odom_frame.rotation().toQuaternion(); 1050 | gtsam::Point3 point = T_odom_frame.translation(); 1051 | const uint32_t robot_id = robot_prefix_to_id.at(symbol_frame.chr()); 1052 | const uint32_t frame_id = symbol_frame.index(); 1053 | odometry_file_ << ts << ","; 1054 | odometry_file_ << robot_id << ","; 1055 | odometry_file_ << frame_id << ","; 1056 | odometry_file_ << quat.x() << ","; 1057 | odometry_file_ << quat.y() << ","; 1058 | odometry_file_ << quat.z() << ","; 1059 | odometry_file_ << quat.w() << ","; 1060 | odometry_file_ << point.x() << ","; 1061 | odometry_file_ << point.y() << ","; 1062 | odometry_file_ << point.z() << "\n"; 1063 | odometry_file_.flush(); 1064 | } 1065 | } 1066 | 1067 | void DistributedLoopClosure::logLoopClosure(const lcd::VLCEdge& keyframe_edge) { 1068 | if (loop_closure_file_.is_open()) { 1069 | gtsam::Quaternion quat = keyframe_edge.T_src_dst_.rotation().toQuaternion(); 1070 | gtsam::Point3 point = keyframe_edge.T_src_dst_.translation(); 1071 | const lcd::RobotId robot_src = keyframe_edge.vertex_src_.first; 1072 | const lcd::PoseId frame_src = keyframe_edge.vertex_src_.second; 1073 | const lcd::RobotId robot_dst = keyframe_edge.vertex_dst_.first; 1074 | const lcd::PoseId frame_dst = keyframe_edge.vertex_dst_.second; 1075 | loop_closure_file_ << robot_src << ","; 1076 | loop_closure_file_ << frame_src << ","; 1077 | loop_closure_file_ << robot_dst << ","; 1078 | loop_closure_file_ << frame_dst << ","; 1079 | loop_closure_file_ << quat.x() << ","; 1080 | loop_closure_file_ << quat.y() << ","; 1081 | loop_closure_file_ << quat.z() << ","; 1082 | loop_closure_file_ << quat.w() << ","; 1083 | loop_closure_file_ << point.x() << ","; 1084 | loop_closure_file_ << point.y() << ","; 1085 | loop_closure_file_ << point.z() << ","; 1086 | loop_closure_file_ << keyframe_edge.normalized_bow_score_ << ","; 1087 | loop_closure_file_ << keyframe_edge.mono_inliers_ << ","; 1088 | loop_closure_file_ << keyframe_edge.stereo_inliers_ << ","; 1089 | loop_closure_file_ << keyframe_edge.stamp_ns_ << "\n"; 1090 | loop_closure_file_.flush(); 1091 | } 1092 | } 1093 | 1094 | void DistributedLoopClosure::loadOdometryFromFile(const std::string& pose_file) { 1095 | std::ifstream infile(pose_file); 1096 | if (!infile.is_open()) { 1097 | LOG(ERROR) << "Could not open specified file!"; 1098 | ros::shutdown(); 1099 | } 1100 | 1101 | size_t num_poses_read = 0; 1102 | 1103 | // Scalars that will be filled 1104 | uint64_t stamp_ns; 1105 | uint32_t robot_id; 1106 | uint32_t frame_id; 1107 | double qx, qy, qz, qw; 1108 | double tx, ty, tz; 1109 | 1110 | std::string line; 1111 | std::string token; 1112 | 1113 | // Skip first line (headers) 1114 | std::getline(infile, line); 1115 | 1116 | // Iterate over remaining lines 1117 | while (std::getline(infile, line)) { 1118 | std::istringstream ss(line); 1119 | 1120 | std::getline(ss, token, ','); 1121 | stamp_ns = std::stoull(token); 1122 | std::getline(ss, token, ','); 1123 | robot_id = std::stoi(token); 1124 | std::getline(ss, token, ','); 1125 | frame_id = std::stoi(token); 1126 | 1127 | std::getline(ss, token, ','); 1128 | qx = std::stod(token); 1129 | std::getline(ss, token, ','); 1130 | qy = std::stod(token); 1131 | std::getline(ss, token, ','); 1132 | qz = std::stod(token); 1133 | std::getline(ss, token, ','); 1134 | qw = std::stod(token); 1135 | 1136 | std::getline(ss, token, ','); 1137 | tx = std::stod(token); 1138 | std::getline(ss, token, ','); 1139 | ty = std::stod(token); 1140 | std::getline(ss, token, ','); 1141 | tz = std::stod(token); 1142 | 1143 | // Add this keyframe to submap atlas 1144 | CHECK_EQ(robot_id, config_.my_id_); 1145 | gtsam::Pose3 T_odom_keyframe; 1146 | T_odom_keyframe = 1147 | gtsam::Pose3(gtsam::Rot3(qw, qx, qy, qz), gtsam::Point3(tx, ty, tz)); 1148 | if (submap_atlas_->hasKeyframe(frame_id)) continue; 1149 | if (frame_id != submap_atlas_->numKeyframes()) { 1150 | LOG(ERROR) << "Received out of ordered keyframe. Expected id:" 1151 | << submap_atlas_->numKeyframes() << ", received=" << frame_id; 1152 | } 1153 | submap_atlas_->createKeyframe(frame_id, T_odom_keyframe, stamp_ns); 1154 | num_poses_read++; 1155 | } 1156 | infile.close(); 1157 | LOG(INFO) << "Loaded " << num_poses_read << " from " << pose_file; 1158 | } 1159 | 1160 | void DistributedLoopClosure::loadLoopClosuresFromFile(const std::string& lc_file) { 1161 | std::ifstream infile(lc_file); 1162 | if (!infile.is_open()) { 1163 | LOG(ERROR) << "Could not open specified file!"; 1164 | ros::shutdown(); 1165 | } 1166 | size_t num_measurements_read = 0; 1167 | 1168 | // Scalars that will be filled 1169 | uint32_t robot_from, robot_to, pose_from, pose_to; 1170 | double qx, qy, qz, qw; 1171 | double tx, ty, tz; 1172 | 1173 | std::string line; 1174 | std::string token; 1175 | 1176 | // Skip first line (headers) 1177 | std::getline(infile, line); 1178 | 1179 | // Iterate over remaining lines 1180 | while (std::getline(infile, line)) { 1181 | std::istringstream ss(line); 1182 | 1183 | std::getline(ss, token, ','); 1184 | robot_from = std::stoi(token); 1185 | std::getline(ss, token, ','); 1186 | pose_from = std::stoi(token); 1187 | std::getline(ss, token, ','); 1188 | robot_to = std::stoi(token); 1189 | std::getline(ss, token, ','); 1190 | pose_to = std::stoi(token); 1191 | 1192 | std::getline(ss, token, ','); 1193 | qx = std::stod(token); 1194 | std::getline(ss, token, ','); 1195 | qy = std::stod(token); 1196 | std::getline(ss, token, ','); 1197 | qz = std::stod(token); 1198 | std::getline(ss, token, ','); 1199 | qw = std::stod(token); 1200 | 1201 | std::getline(ss, token, ','); 1202 | tx = std::stod(token); 1203 | std::getline(ss, token, ','); 1204 | ty = std::stod(token); 1205 | std::getline(ss, token, ','); 1206 | tz = std::stod(token); 1207 | 1208 | gtsam::Symbol keyframe_from(robot_id_to_prefix.at(robot_from), pose_from); 1209 | gtsam::Symbol keyframe_to(robot_id_to_prefix.at(robot_to), pose_to); 1210 | gtsam::Pose3 T_f1_f2; 1211 | T_f1_f2 = gtsam::Pose3(gtsam::Rot3(qw, qx, qy, qz), gtsam::Point3(tx, ty, tz)); 1212 | static const gtsam::SharedNoiseModel& noise = 1213 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 1214 | offline_keyframe_loop_closures_.add( 1215 | gtsam::BetweenFactor(keyframe_from, keyframe_to, T_f1_f2, noise)); 1216 | num_measurements_read++; 1217 | } 1218 | infile.close(); 1219 | LOG(INFO) << "Loaded " << num_measurements_read << " loop closures from " << lc_file; 1220 | } 1221 | 1222 | void DistributedLoopClosure::processOfflineLoopClosures() { 1223 | gtsam::NonlinearFactorGraph remaining_loop_closures_; 1224 | size_t num_loops_processed = 0; 1225 | for (const auto& factor : offline_keyframe_loop_closures_) { 1226 | gtsam::Symbol front(factor->front()); 1227 | gtsam::Symbol back(factor->back()); 1228 | lcd::PoseId pose_from = front.index(); 1229 | lcd::PoseId pose_to = back.index(); 1230 | lcd::RobotId robot_from = robot_prefix_to_id.at(front.chr()); 1231 | lcd::RobotId robot_to = robot_prefix_to_id.at(back.chr()); 1232 | lcd::RobotPoseId vertex_from(robot_from, pose_from); 1233 | lcd::RobotPoseId vertex_to(robot_to, pose_to); 1234 | if (offline_robot_pose_msg_.find(vertex_from) != offline_robot_pose_msg_.end() && 1235 | offline_robot_pose_msg_.find(vertex_to) != offline_robot_pose_msg_.end()) { 1236 | num_loops_processed++; 1237 | const auto& msg_from = offline_robot_pose_msg_.at(vertex_from); 1238 | const auto& msg_to = offline_robot_pose_msg_.at(vertex_to); 1239 | const int submap_id_from = (int)msg_from.submap_id; 1240 | const int submap_id_to = (int)msg_to.submap_id; 1241 | const lcd::EdgeID submap_edge_id( 1242 | robot_from, submap_id_from, robot_to, submap_id_to); 1243 | // Skip if loop closure connects identical or consecutive submaps of same robots 1244 | if (robot_from == robot_to && abs(submap_id_from - submap_id_to) <= 1) { 1245 | LOG(WARNING) << "Skip intra loop connecting submap " << submap_id_from 1246 | << " and " << submap_id_to << "."; 1247 | continue; 1248 | } 1249 | bool loop_exist = (submap_loop_closures_ids_.find(submap_edge_id) != 1250 | submap_loop_closures_ids_.end()); 1251 | if (!loop_exist) { 1252 | const gtsam::BetweenFactor& factor_pose3 = 1253 | *boost::dynamic_pointer_cast>(factor); 1254 | submap_loop_closures_ids_.emplace(submap_edge_id); 1255 | // Extract relative transformation between submaps 1256 | gtsam::Pose3 T_s1_f1 = RosPoseToGtsam(msg_from.T_submap_pose); 1257 | gtsam::Pose3 T_s2_f2 = RosPoseToGtsam(msg_to.T_submap_pose); 1258 | gtsam::Pose3 T_f1_f2 = factor_pose3.measured(); 1259 | gtsam::Pose3 T_s1_s2 = T_s1_f1 * T_f1_f2 * (T_s2_f2.inverse()); 1260 | // Add new loop to queue for synchronization with other robots 1261 | gtsam::Symbol submap_from(robot_id_to_prefix.at(robot_from), submap_id_from); 1262 | gtsam::Symbol submap_to(robot_id_to_prefix.at(robot_to), submap_id_to); 1263 | static const gtsam::SharedNoiseModel& noise = 1264 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 1265 | // submap_loop_closures_queue_[submap_edge_id] = 1266 | // gtsam::BetweenFactor( 1267 | // submap_from, submap_to, T_s1_s2, noise); 1268 | submap_loop_closures_.add( 1269 | gtsam::BetweenFactor(submap_from, submap_to, T_s1_s2, noise)); 1270 | } 1271 | } else { 1272 | // Keyframe info is not yet available 1273 | remaining_loop_closures_.add(factor); 1274 | } 1275 | } 1276 | LOG(INFO) << "Processed " << num_loops_processed 1277 | << " offline loop closures, remaining " << remaining_loop_closures_.size() 1278 | << "."; 1279 | offline_keyframe_loop_closures_ = remaining_loop_closures_; 1280 | } 1281 | 1282 | } // namespace kimera_distributed -------------------------------------------------------------------------------- /src/DistributedLoopClosureNode.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu), Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | using namespace kimera_distributed; 11 | 12 | int main(int argc, char** argv) { 13 | ros::init(argc, argv, "kimera_distributed_loop_closure_node"); 14 | ros::NodeHandle nh; 15 | 16 | DistributedLoopClosureRos dlcd(nh); 17 | 18 | ros::spin(); 19 | 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /src/DistributedLoopClosureRos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #include "kimera_distributed/DistributedLoopClosureRos.h" 8 | #include "kimera_distributed/configs.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace kimera_distributed { 28 | 29 | DistributedLoopClosureRos::DistributedLoopClosureRos(const ros::NodeHandle& n) 30 | : nh_(n) { 31 | DistributedLoopClosureConfig config; 32 | int my_id_int = -1; 33 | int num_robots_int = -1; 34 | ros::param::get("~robot_id", my_id_int); 35 | ros::param::get("~num_robots", num_robots_int); 36 | ros::param::get("~frame_id", config.frame_id_); 37 | assert(my_id_int >= 0); 38 | assert(num_robots_int > 0); 39 | config.my_id_ = my_id_int; 40 | config.num_robots_ = num_robots_int; 41 | 42 | // Path to log outputs 43 | config.log_output_ = ros::param::get("~log_output_path", config.log_output_dir_); 44 | ros::param::get("~run_offline", config.run_offline_); 45 | 46 | if (config.run_offline_) { 47 | if (!ros::param::get("~offline_dir", config.offline_dir_)) { 48 | ROS_ERROR("Offline directory is missing!"); 49 | ros::shutdown(); 50 | } 51 | } 52 | 53 | // Visual place recognition params 54 | ros::param::get("~alpha", config.lcd_params_.alpha_); 55 | ros::param::get("~dist_local", config.lcd_params_.dist_local_); 56 | ros::param::get("~max_db_results", config.lcd_params_.max_db_results_); 57 | ros::param::get("~min_nss_factor", config.lcd_params_.min_nss_factor_); 58 | 59 | // Lcd Third Party Wrapper Params 60 | ros::param::get("~max_nrFrames_between_islands", 61 | config.lcd_params_.lcd_tp_params_.max_nrFrames_between_islands_); 62 | ros::param::get("~max_nrFrames_between_queries", 63 | config.lcd_params_.lcd_tp_params_.max_nrFrames_between_queries_); 64 | ros::param::get("~max_intraisland_gap", 65 | config.lcd_params_.lcd_tp_params_.max_intraisland_gap_); 66 | ros::param::get("~min_matches_per_island", 67 | config.lcd_params_.lcd_tp_params_.min_matches_per_island_); 68 | ros::param::get("~min_temporal_matches", 69 | config.lcd_params_.lcd_tp_params_.min_temporal_matches_); 70 | 71 | // Geometric verification params 72 | ros::param::get("~ransac_threshold_mono", config.lcd_params_.ransac_threshold_mono_); 73 | ros::param::get("~ransac_inlier_percentage_mono", 74 | config.lcd_params_.ransac_inlier_percentage_mono_); 75 | ros::param::get("~max_ransac_iterations_mono", 76 | config.lcd_params_.max_ransac_iterations_mono_); 77 | ros::param::get("~lowe_ratio", config.lcd_params_.lowe_ratio_); 78 | ros::param::get("~max_ransac_iterations", config.lcd_params_.max_ransac_iterations_); 79 | ros::param::get("~ransac_threshold", config.lcd_params_.ransac_threshold_); 80 | ros::param::get("~geometric_verification_min_inlier_count", 81 | config.lcd_params_.geometric_verification_min_inlier_count_); 82 | ros::param::get("~geometric_verification_min_inlier_percentage", 83 | config.lcd_params_.geometric_verification_min_inlier_percentage_); 84 | ros::param::get("~detect_interrobot_only", config.lcd_params_.inter_robot_only_); 85 | 86 | ros::param::get("~vocabulary_path", config.lcd_params_.vocab_path_); 87 | 88 | ros::param::get("~detection_batch_size", config.detection_batch_size_); 89 | ros::param::get("~bow_skip_num", config.bow_skip_num_); 90 | // Load parameters controlling VLC communication 91 | ros::param::get("~bow_batch_size", config.bow_batch_size_); 92 | ros::param::get("~vlc_batch_size", config.vlc_batch_size_); 93 | ros::param::get("~loop_batch_size", config.loop_batch_size_); 94 | ros::param::get("~comm_sleep_time", config.comm_sleep_time_); 95 | ros::param::get("~loop_sync_sleep_time", config.loop_sync_sleep_time_); 96 | 97 | // TF 98 | if (!ros::param::get("~latest_kf_frame_id", latest_kf_frame_id_)) { 99 | ROS_ERROR("Latest KF frame ID is missing!"); 100 | ros::shutdown(); 101 | } 102 | if (!ros::param::get("~odom_frame_id", odom_frame_id_)) { 103 | ROS_ERROR("Odometry frame ID is missing!"); 104 | ros::shutdown(); 105 | } 106 | if (!ros::param::get("~world_frame_id", world_frame_id_)) { 107 | ROS_ERROR("World frame ID is missing!"); 108 | ros::shutdown(); 109 | } 110 | 111 | // Load robot names and initialize candidate lc queues 112 | for (size_t id = 0; id < config.num_robots_; id++) { 113 | std::string robot_name = "kimera" + std::to_string(id); 114 | ros::param::get("~robot" + std::to_string(id) + "_name", robot_name); 115 | config.robot_names_[id] = robot_name; 116 | } 117 | 118 | ros::param::get("~max_submap_size", config.submap_params_.max_submap_size); 119 | ros::param::get("~max_submap_distance", config.submap_params_.max_submap_distance); 120 | 121 | initialize(config); 122 | 123 | // Subscriber 124 | std::string topic = "/" + config_.robot_names_[config_.my_id_] + 125 | "/kimera_vio_ros/pose_graph_incremental"; 126 | local_pg_sub_ = nh_.subscribe( 127 | topic, 1000, &DistributedLoopClosureRos::localPoseGraphCallback, this); 128 | 129 | std::string internal_vlc_topic = 130 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_vio_ros/vlc_frames"; 131 | internal_vlc_sub_ = nh_.subscribe( 132 | internal_vlc_topic, 1000, &DistributedLoopClosureRos::internalVLCCallback, this); 133 | 134 | std::string dpgo_topic = 135 | "/" + config_.robot_names_[config_.my_id_] + "/dpgo_ros_node/path"; 136 | dpgo_sub_ = 137 | nh_.subscribe(dpgo_topic, 3, &DistributedLoopClosureRos::dpgoCallback, this); 138 | 139 | std::string connectivity_topic = 140 | "/" + config_.robot_names_[config_.my_id_] + "/connected_peer_ids"; 141 | connectivity_sub_ = nh_.subscribe( 142 | connectivity_topic, 5, &DistributedLoopClosureRos::connectivityCallback, this); 143 | 144 | for (size_t id = 0; id < config_.num_robots_; ++id) { 145 | if (id < config_.my_id_) { 146 | std::string vlc_req_topic = 147 | "/" + config_.robot_names_[id] + "/kimera_distributed/vlc_requests"; 148 | ros::Subscriber vlc_req_sub = nh_.subscribe( 149 | vlc_req_topic, 1, &DistributedLoopClosureRos::vlcRequestsCallback, this); 150 | vlc_requests_sub_.push_back(vlc_req_sub); 151 | 152 | std::string bow_req_topic = 153 | "/" + config_.robot_names_[id] + "/kimera_distributed/bow_requests"; 154 | ros::Subscriber bow_req_sub = nh_.subscribe( 155 | bow_req_topic, 1, &DistributedLoopClosureRos::bowRequestsCallback, this); 156 | bow_requests_sub_.push_back(bow_req_sub); 157 | 158 | std::string loop_topic = 159 | "/" + config_.robot_names_[id] + "/kimera_distributed/loop_closures"; 160 | ros::Subscriber loop_sub = nh_.subscribe( 161 | loop_topic, 100, &DistributedLoopClosureRos::loopClosureCallback, this); 162 | loop_sub_.push_back(loop_sub); 163 | } 164 | 165 | if (id >= config_.my_id_) { 166 | std::string bow_topic = 167 | "/" + config_.robot_names_[id] + "/kimera_vio_ros/bow_query"; 168 | ros::Subscriber bow_sub = 169 | nh_.subscribe(bow_topic, 1000, &DistributedLoopClosureRos::bowCallback, this); 170 | bow_sub_.push_back(bow_sub); 171 | bow_latest_[id] = 0; 172 | bow_received_[id] = std::unordered_set(); 173 | } 174 | 175 | if (id > config_.my_id_) { 176 | std::string resp_topic = 177 | "/" + config_.robot_names_[id] + "/kimera_distributed/vlc_responses"; 178 | ros::Subscriber resp_sub = nh_.subscribe( 179 | resp_topic, 10, &DistributedLoopClosureRos::vlcResponsesCallback, this); 180 | vlc_responses_sub_.push_back(resp_sub); 181 | 182 | std::string ack_topic = 183 | "/" + config_.robot_names_[id] + "/kimera_distributed/loop_ack"; 184 | ros::Subscriber ack_sub = 185 | nh_.subscribe(ack_topic, 186 | 100, 187 | &DistributedLoopClosureRos::loopAcknowledgementCallback, 188 | this); 189 | loop_ack_sub_.push_back(ack_sub); 190 | } 191 | } 192 | 193 | if (config_.my_id_ > 0) { 194 | std::string pose_corrector_topic = 195 | "/" + config_.robot_names_[0] + "/kimera_distributed/pose_corrector"; 196 | dpgo_frame_corrector_sub_ = 197 | nh_.subscribe(pose_corrector_topic, 198 | 1, 199 | &DistributedLoopClosureRos::dpgoFrameCorrectionCallback, 200 | this); 201 | } 202 | 203 | // Publisher 204 | std::string bow_response_topic = 205 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_vio_ros/bow_query"; 206 | bow_response_pub_ = 207 | nh_.advertise(bow_response_topic, 1000, true); 208 | 209 | std::string bow_request_topic = 210 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/bow_requests"; 211 | bow_requests_pub_ = 212 | nh_.advertise(bow_request_topic, 100, true); 213 | 214 | std::string pose_graph_topic = "/" + config_.robot_names_[config_.my_id_] + 215 | "/kimera_distributed/pose_graph_incremental"; 216 | pose_graph_pub_ = 217 | nh_.advertise(pose_graph_topic, 1000, true); 218 | 219 | std::string resp_topic = 220 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/vlc_responses"; 221 | vlc_responses_pub_ = nh_.advertise(resp_topic, 10, true); 222 | 223 | std::string req_topic = 224 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/vlc_requests"; 225 | vlc_requests_pub_ = nh_.advertise(req_topic, 10, true); 226 | 227 | std::string loop_topic = 228 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/loop_closures"; 229 | loop_pub_ = nh_.advertise(loop_topic, 100, true); 230 | 231 | std::string ack_topic = 232 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/loop_ack"; 233 | loop_ack_pub_ = 234 | nh_.advertise(ack_topic, 100, true); 235 | 236 | std::string optimized_nodes_topic = "/" + config_.robot_names_[config_.my_id_] + 237 | "/kimera_distributed/optimized_nodes"; 238 | optimized_nodes_pub_ = 239 | nh_.advertise(optimized_nodes_topic, 1, true); 240 | std::string optimized_path_topic = 241 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_distributed/optimized_path"; 242 | optimized_path_pub_ = nh_.advertise(optimized_path_topic, 1, true); 243 | 244 | if (config_.my_id_ == 0) { 245 | std::string pose_corrector_topic = "/" + config_.robot_names_[config_.my_id_] + 246 | "/kimera_distributed/pose_corrector"; 247 | dpgo_frame_corrector_pub_ = 248 | nh_.advertise(pose_corrector_topic, 1, true); 249 | } 250 | 251 | // ROS service 252 | pose_graph_request_server_ = nh_.advertiseService( 253 | "request_pose_graph", &DistributedLoopClosureRos::requestPoseGraphCallback, this); 254 | 255 | log_timer_ = nh_.createTimer( 256 | ros::Duration(10.0), &DistributedLoopClosureRos::logTimerCallback, this); 257 | 258 | tf_timer_ = nh_.createTimer( 259 | ros::Duration(1.0), &DistributedLoopClosureRos::tfTimerCallback, this); 260 | 261 | ROS_INFO_STREAM( 262 | "Distributed Kimera node initialized (ID = " 263 | << config_.my_id_ << "). \n" 264 | << "Parameters: \n" 265 | << "alpha = " << config_.lcd_params_.alpha_ << "\n" 266 | << "dist_local = " << config_.lcd_params_.dist_local_ << "\n" 267 | << "max_db_results = " << config_.lcd_params_.max_db_results_ << "\n" 268 | << "min_nss_factor = " << config_.lcd_params_.min_nss_factor_ << "\n" 269 | << "lowe_ratio = " << config_.lcd_params_.lowe_ratio_ << "\n" 270 | << "max_nrFrames_between_queries = " 271 | << config_.lcd_params_.lcd_tp_params_.max_nrFrames_between_queries_ << "\n" 272 | << "max_nrFrames_between_islands = " 273 | << config_.lcd_params_.lcd_tp_params_.max_nrFrames_between_islands_ << "\n" 274 | << "max_intraisland_gap = " 275 | << config_.lcd_params_.lcd_tp_params_.max_intraisland_gap_ << "\n" 276 | << "min_matches_per_island = " 277 | << config_.lcd_params_.lcd_tp_params_.min_matches_per_island_ << "\n" 278 | << "min_temporal_matches = " 279 | << config_.lcd_params_.lcd_tp_params_.min_temporal_matches_ << "\n" 280 | << "max_ransac_iterations = " << config_.lcd_params_.max_ransac_iterations_ 281 | << "\n" 282 | << "mono ransac threshold = " << config_.lcd_params_.ransac_threshold_mono_ 283 | << "\n" 284 | << "mono ransac max iterations = " 285 | << config_.lcd_params_.max_ransac_iterations_mono_ << "\n" 286 | << "mono ransac min inlier percentage = " 287 | << config_.lcd_params_.ransac_inlier_percentage_mono_ << "\n" 288 | << "ransac_threshold = " << config_.lcd_params_.ransac_threshold_ << "\n" 289 | << "geometric_verification_min_inlier_count = " 290 | << config_.lcd_params_.geometric_verification_min_inlier_count_ << "\n" 291 | << "geometric_verification_min_inlier_percentage = " 292 | << config_.lcd_params_.geometric_verification_min_inlier_percentage_ << "\n" 293 | << "interrobot loop closure only = " << config_.lcd_params_.inter_robot_only_ 294 | << "\n" 295 | << "maximum batch size to request BoW vectors = " << config_.bow_batch_size_ 296 | << "\n" 297 | << "maximum batch size to request VLC frames = " << config_.vlc_batch_size_ 298 | << "\n" 299 | << "Communication thread sleep time = " << config_.comm_sleep_time_ << "\n" 300 | << "maximum submap size = " << config_.submap_params_.max_submap_size << "\n" 301 | << "maximum submap distance = " << config_.submap_params_.max_submap_distance 302 | << "\n" 303 | << "loop detection batch size = " << config_.detection_batch_size_ << "\n" 304 | << "loop synchronization batch size = " << config_.loop_batch_size_ << "\n" 305 | << "loop synchronization sleep time = " << config_.loop_sync_sleep_time_ << "\n" 306 | << "BoW vector skip num = " << config_.bow_skip_num_ << "\n"); 307 | 308 | if (config_.run_offline_) { 309 | // publish submap poses 310 | for (int count = 0; count < 3; ++count) { 311 | publishSubmapOfflineInfo(); 312 | } 313 | processOfflineLoopClosures(); 314 | } 315 | 316 | // Start loop detection thread 317 | detection_thread_.reset( 318 | new std::thread(&DistributedLoopClosureRos::runDetection, this)); 319 | ROS_INFO("Robot %zu started loop detection / place recognition thread.", 320 | config_.my_id_); 321 | 322 | // Start verification thread 323 | verification_thread_.reset( 324 | new std::thread(&DistributedLoopClosureRos::runVerification, this)); 325 | ROS_INFO("Robot %zu started loop verification thread.", config_.my_id_); 326 | 327 | // Start comms thread 328 | comms_thread_.reset(new std::thread(&DistributedLoopClosureRos::runComms, this)); 329 | ROS_INFO("Robot %zu started communication thread.", config_.my_id_); 330 | 331 | start_time_ = ros::Time::now(); 332 | next_loop_sync_time_ = ros::Time::now(); 333 | next_latest_bow_pub_time_ = ros::Time::now(); 334 | } 335 | 336 | DistributedLoopClosureRos::~DistributedLoopClosureRos() { 337 | if (config_.log_output_) { 338 | save(); 339 | } 340 | ROS_INFO("Shutting down DistributedLoopClosureRos process on robot %zu...", 341 | config_.my_id_); 342 | } 343 | 344 | void DistributedLoopClosureRos::bowCallback( 345 | const pose_graph_tools_msgs::BowQueriesConstPtr& query_msg) { 346 | processBow(query_msg); 347 | } 348 | 349 | void DistributedLoopClosureRos::localPoseGraphCallback( 350 | const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg) { 351 | bool incremental_pub = processLocalPoseGraph(msg); 352 | 353 | // Publish sparsified pose graph 354 | pose_graph_tools_msgs::PoseGraph sparse_pose_graph = getSubmapPoseGraph(incremental_pub); 355 | if (!sparse_pose_graph.edges.empty() || !sparse_pose_graph.nodes.empty()) { 356 | pose_graph_pub_.publish(sparse_pose_graph); 357 | } 358 | } 359 | 360 | void DistributedLoopClosureRos::connectivityCallback( 361 | const std_msgs::UInt16MultiArrayConstPtr& msg) { 362 | std::set connected_ids(msg->data.begin(), msg->data.end()); 363 | for (unsigned robot_id = 0; robot_id < config_.num_robots_; ++robot_id) { 364 | if (robot_id == config_.my_id_) { 365 | robot_connected_[robot_id] = true; 366 | } else if (connected_ids.find(robot_id) != connected_ids.end()) { 367 | robot_connected_[robot_id] = true; 368 | } else { 369 | // ROS_WARN("Robot %u is disconnected.", robot_id); 370 | robot_connected_[robot_id] = false; 371 | } 372 | } 373 | } 374 | 375 | void DistributedLoopClosureRos::dpgoCallback(const nav_msgs::PathConstPtr& msg) { 376 | processOptimizedPath(msg); 377 | gtsam::Values::shared_ptr nodes_ptr(new gtsam::Values); 378 | computePosesInWorldFrame(nodes_ptr); 379 | if (config_.run_offline_) { 380 | saveSubmapAtlas(config_.log_output_dir_); 381 | auto elapsed_time = ros::Time::now() - start_time_; 382 | int elapsed_sec = int(elapsed_time.toSec()); 383 | std::string file_path = config_.log_output_dir_ + "kimera_distributed_poses_" + 384 | std::to_string(elapsed_sec) + ".csv"; 385 | savePosesToFile(file_path, *nodes_ptr); 386 | } 387 | publishOptimizedNodesAndPath(*nodes_ptr); 388 | } 389 | 390 | void DistributedLoopClosureRos::publishOptimizedNodesAndPath( 391 | const gtsam::Values& nodes) { 392 | pose_graph_tools_msgs::PoseGraph nodes_msg; 393 | nav_msgs::Path path_msg; 394 | for (const auto& key_pose : nodes) { 395 | pose_graph_tools_msgs::PoseGraphNode node_msg; 396 | gtsam::Symbol key_symb(key_pose.key); 397 | node_msg.key = key_symb.index(); 398 | node_msg.robot_id = config_.my_id_; 399 | node_msg.pose = GtsamPoseToRos(nodes.at(node_msg.key)); 400 | const auto keyframe = submap_atlas_->getKeyframe(key_pose.key); 401 | node_msg.header.stamp.fromNSec(keyframe->stamp()); 402 | nodes_msg.nodes.push_back(node_msg); 403 | 404 | geometry_msgs::PoseStamped pose_stamped; 405 | pose_stamped.pose = node_msg.pose; 406 | pose_stamped.header = node_msg.header; 407 | path_msg.poses.push_back(pose_stamped); 408 | path_msg.header = node_msg.header; 409 | } 410 | path_msg.header.frame_id = world_frame_id_; 411 | optimized_nodes_pub_.publish(nodes_msg); 412 | optimized_path_pub_.publish(path_msg); 413 | } 414 | 415 | void DistributedLoopClosureRos::dpgoFrameCorrectionCallback( 416 | const geometry_msgs::Pose::ConstPtr& msg) { 417 | T_world_dpgo_ = RosPoseToGtsam(*msg); 418 | } 419 | 420 | void DistributedLoopClosureRos::logTimerCallback(const ros::TimerEvent& event) { 421 | if (!config_.log_output_) return; 422 | if (config_.run_offline_) return; 423 | logLcdStat(); 424 | // Save latest submap atlas 425 | saveSubmapAtlas(config_.log_output_dir_); 426 | // Save latest trajectory estimates in the world frame 427 | if (backend_update_count_ > 0) { 428 | auto elapsed_time = ros::Time::now() - start_time_; 429 | int elapsed_sec = int(elapsed_time.toSec()); 430 | std::string file_path = config_.log_output_dir_ + "kimera_distributed_poses_" + 431 | std::to_string(elapsed_sec) + ".csv"; 432 | gtsam::Values::shared_ptr nodes_ptr(new gtsam::Values); 433 | computePosesInWorldFrame(nodes_ptr); 434 | savePosesToFile(file_path, *nodes_ptr); 435 | } 436 | } 437 | 438 | void DistributedLoopClosureRos::publishWorldToDpgoCorrection() { 439 | if (config_.my_id_ != 0) { 440 | return; 441 | } 442 | 443 | dpgo_frame_corrector_pub_.publish(GtsamPoseToRos(T_world_dpgo_)); 444 | } 445 | 446 | void DistributedLoopClosureRos::publishOdomToWorld() { 447 | geometry_msgs::TransformStamped tf_world_odom; 448 | tf_world_odom.header.stamp = ros::Time::now(); 449 | tf_world_odom.header.frame_id = world_frame_id_; 450 | tf_world_odom.child_frame_id = odom_frame_id_; 451 | 452 | const gtsam::Pose3 T_world_odom = getOdomInWorldFrame(); 453 | GtsamPoseToRosTf(T_world_odom, &tf_world_odom.transform); 454 | tf_broadcaster_.sendTransform(tf_world_odom); 455 | 456 | } 457 | 458 | void DistributedLoopClosureRos::publishLatestKFToWorld() { 459 | geometry_msgs::TransformStamped tf_world_base; 460 | tf_world_base.header.stamp = ros::Time::now(); 461 | tf_world_base.header.frame_id = world_frame_id_; 462 | tf_world_base.child_frame_id = latest_kf_frame_id_; 463 | 464 | const gtsam::Pose3 T_world_base = getLatestKFInWorldFrame(); 465 | GtsamPoseToRosTf(T_world_base, &tf_world_base.transform); 466 | tf_broadcaster_.sendTransform(tf_world_base); 467 | } 468 | 469 | void DistributedLoopClosureRos::publishLatestKFToOdom() { 470 | geometry_msgs::TransformStamped tf_odom_base; 471 | tf_odom_base.header.stamp = ros::Time::now(); 472 | tf_odom_base.header.frame_id = odom_frame_id_; 473 | tf_odom_base.child_frame_id = latest_kf_frame_id_; 474 | 475 | const gtsam::Pose3 T_odom_base = getLatestKFInOdomFrame(); 476 | GtsamPoseToRosTf(T_odom_base, &tf_odom_base.transform); 477 | tf_broadcaster_.sendTransform(tf_odom_base); 478 | } 479 | 480 | void DistributedLoopClosureRos::tfTimerCallback(const ros::TimerEvent& event) { 481 | publishWorldToDpgoCorrection(); 482 | publishOdomToWorld(); 483 | publishLatestKFToOdom(); // Currently for debugging 484 | } 485 | 486 | void DistributedLoopClosureRos::runDetection() { 487 | while (ros::ok() && !should_shutdown_) { 488 | if (!bow_msgs_.empty()) { 489 | detectLoopSpin(); 490 | } 491 | ros::Duration(1.0).sleep(); 492 | } 493 | } 494 | 495 | void DistributedLoopClosureRos::runVerification() { 496 | ros::WallRate r(1); 497 | while (ros::ok() && !should_shutdown_) { 498 | if (queued_lc_.empty()) { 499 | r.sleep(); 500 | } else { 501 | verifyLoopSpin(); 502 | } 503 | } 504 | } 505 | 506 | void DistributedLoopClosureRos::runComms() { 507 | while (ros::ok() && !should_shutdown_) { 508 | // Request missing Bow vectors from other robots 509 | requestBowVectors(); 510 | 511 | // Request VLC frames from other robots 512 | size_t total_candidates = updateCandidateList(); 513 | if (total_candidates > 0) { 514 | requestFrames(); 515 | } 516 | 517 | // Publish BoW vectors requested by other robots 518 | publishBowVectors(); 519 | 520 | // Publish VLC frames requested by other robots 521 | publishFrames(); 522 | 523 | // Publish new inter-robot loop closures, if any 524 | if (ros::Time::now().toSec() > next_loop_sync_time_.toSec()) { 525 | initializeLoopPublishers(); 526 | publishQueuedLoops(); 527 | 528 | // Generate a random sleep time 529 | std::random_device rd; 530 | std::mt19937 gen(rd()); 531 | std::uniform_real_distribution distribution( 532 | 0.5 * config_.loop_sync_sleep_time_, 1.5 * config_.loop_sync_sleep_time_); 533 | double sleep_time = distribution(gen); 534 | next_loop_sync_time_ += ros::Duration(sleep_time); 535 | } 536 | 537 | // Once a while publish latest BoW vector 538 | // This is needed for other robots to request potentially missing BoW 539 | if (ros::Time::now().toSec() > next_latest_bow_pub_time_.toSec()) { 540 | publishLatestBowVector(); 541 | next_latest_bow_pub_time_ += ros::Duration(30); // TODO: make into parameter 542 | } 543 | 544 | // Print stats 545 | ROS_INFO_STREAM("Total inter-robot loop closures: " << num_inter_robot_loops_); 546 | 547 | double avg_sleep_time = (double)config_.comm_sleep_time_; 548 | double min_sleep_time = 0.5 * avg_sleep_time; 549 | double max_sleep_time = 1.5 * avg_sleep_time; 550 | randomSleep(min_sleep_time, max_sleep_time); 551 | } 552 | } 553 | 554 | void DistributedLoopClosureRos::requestBowVectors() { 555 | // Form BoW vectors that are missing from each robot 556 | lcd::RobotId robot_id_to_query; 557 | std::set missing_bow_vectors; 558 | if (!queryBowVectorsRequest(robot_id_to_query, missing_bow_vectors)) { 559 | return; 560 | } 561 | 562 | // Publish BoW request to selected robot 563 | pose_graph_tools_msgs::BowRequests msg; 564 | msg.source_robot_id = config_.my_id_; 565 | msg.destination_robot_id = robot_id_to_query; 566 | for (const auto& pose_id : missing_bow_vectors) { 567 | if (msg.pose_ids.size() >= config_.bow_batch_size_) break; 568 | msg.pose_ids.push_back(pose_id); 569 | } 570 | ROS_WARN("Processing %lu BoW requests to robot %lu.", 571 | msg.pose_ids.size(), 572 | robot_id_to_query); 573 | bow_requests_pub_.publish(msg); 574 | } 575 | 576 | void DistributedLoopClosureRos::publishBowVectors() { 577 | lcd::RobotId robot_id_to_publish; 578 | lcd::RobotPoseIdSet bow_vectors_to_publish; 579 | if (!queryBowVectorsPublish(robot_id_to_publish, bow_vectors_to_publish)) { 580 | return; 581 | } 582 | // Send BoW vectors to selected robot 583 | pose_graph_tools_msgs::BowQueries msg; 584 | msg.destination_robot_id = robot_id_to_publish; 585 | for (const auto& robot_pose_id : bow_vectors_to_publish) { 586 | pose_graph_tools_msgs::BowQuery query_msg; 587 | query_msg.robot_id = robot_pose_id.first; 588 | query_msg.pose_id = robot_pose_id.second; 589 | kimera_multi_lcd::BowVectorToMsg(lcd_->getBoWVector(robot_pose_id), 590 | &(query_msg.bow_vector)); 591 | msg.queries.push_back(query_msg); 592 | } 593 | bow_response_pub_.publish(msg); 594 | ROS_INFO("Published %zu BoWs to robot %zu (%zu waiting).", 595 | msg.queries.size(), 596 | robot_id_to_publish, 597 | requested_bows_[robot_id_to_publish].size()); 598 | } 599 | 600 | void DistributedLoopClosureRos::publishLatestBowVector() { 601 | int pose_id = lcd_->latestPoseIdWithBoW(config_.my_id_); 602 | if (pose_id != -1) { 603 | lcd::RobotPoseId latest_id(config_.my_id_, pose_id); 604 | pose_graph_tools_msgs::BowQuery query_msg; 605 | query_msg.robot_id = config_.my_id_; 606 | query_msg.pose_id = pose_id; 607 | kimera_multi_lcd::BowVectorToMsg(lcd_->getBoWVector(latest_id), 608 | &(query_msg.bow_vector)); 609 | 610 | pose_graph_tools_msgs::BowQueries msg; 611 | msg.queries.push_back(query_msg); 612 | for (lcd::RobotId robot_id = 0; robot_id < config_.my_id_; ++robot_id) { 613 | msg.destination_robot_id = robot_id; 614 | bow_response_pub_.publish(msg); 615 | } 616 | } 617 | ROS_INFO("Published latest BoW vector."); 618 | } 619 | 620 | void DistributedLoopClosureRos::requestFrames() { 621 | lcd::RobotPoseIdSet my_vertex_ids, target_vertex_ids; 622 | lcd::RobotId target_robot_id; 623 | queryFramesRequest(my_vertex_ids, target_robot_id, target_vertex_ids); 624 | 625 | // Process missing VLC frames of myself 626 | if (my_vertex_ids.size() > 0) { 627 | processVLCRequests(config_.my_id_, my_vertex_ids); 628 | } 629 | 630 | if (target_vertex_ids.size() > 0) { 631 | processVLCRequests(target_robot_id, target_vertex_ids); 632 | } 633 | } 634 | 635 | void DistributedLoopClosureRos::publishFrames() { 636 | lcd::RobotId target_robot_id; 637 | lcd::RobotPoseIdSet target_vertex_ids; 638 | queryFramesPublish(target_robot_id, target_vertex_ids); 639 | 640 | if (target_vertex_ids.size() > 0) { 641 | // Send VLC frames to the selected robot 642 | pose_graph_tools_msgs::VLCFrames frames_msg; 643 | frames_msg.destination_robot_id = target_robot_id; 644 | for (const auto& vertex_id : target_vertex_ids) { 645 | pose_graph_tools_msgs::VLCFrameMsg vlc_msg; 646 | kimera_multi_lcd::VLCFrameToMsg(lcd_->getVLCFrame(vertex_id), &vlc_msg); 647 | frames_msg.frames.push_back(vlc_msg); 648 | } 649 | vlc_responses_pub_.publish(frames_msg); 650 | ROS_INFO("Published %zu frames to robot %zu (%zu frames waiting).", 651 | frames_msg.frames.size(), 652 | target_robot_id, 653 | requested_frames_[target_robot_id].size()); 654 | } 655 | } 656 | 657 | bool DistributedLoopClosureRos::requestPoseGraphCallback( 658 | pose_graph_tools_msgs::PoseGraphQuery::Request& request, 659 | pose_graph_tools_msgs::PoseGraphQuery::Response& response) { 660 | CHECK_EQ(request.robot_id, config_.my_id_); 661 | response.pose_graph = getSubmapPoseGraph(); 662 | 663 | if (config_.run_offline_) { 664 | if (!offline_keyframe_loop_closures_.empty()) { 665 | ROS_WARN( 666 | "[requestPoseGraphCallback] %zu offline loop closures not yet processed.", 667 | offline_keyframe_loop_closures_.size()); 668 | return false; 669 | } 670 | if (!submap_loop_closures_queue_.empty()) { 671 | ROS_WARN("[requestPoseGraphCallback] %zu loop closures to be synchronized.", 672 | submap_loop_closures_queue_.size()); 673 | return false; 674 | } 675 | } 676 | 677 | return true; 678 | } 679 | 680 | void DistributedLoopClosureRos::initializeLoopPublishers() { 681 | // Publish empty loops and acks 682 | pose_graph_tools_msgs::LoopClosures loop_msg; 683 | pose_graph_tools_msgs::LoopClosuresAck ack_msg; 684 | loop_msg.publishing_robot_id = config_.my_id_; 685 | ack_msg.publishing_robot_id = config_.my_id_; 686 | for (lcd::RobotId robot_id = 0; robot_id < config_.num_robots_; ++robot_id) { 687 | if (robot_id != config_.my_id_ && !loop_pub_initialized_[robot_id] && 688 | robot_connected_[robot_id]) { 689 | loop_msg.destination_robot_id = robot_id; 690 | loop_pub_.publish(loop_msg); 691 | 692 | ack_msg.destination_robot_id = robot_id; 693 | loop_ack_pub_.publish(ack_msg); 694 | 695 | loop_pub_initialized_[robot_id] = true; 696 | } 697 | } 698 | } 699 | 700 | void DistributedLoopClosureRos::publishQueuedLoops() { 701 | std::map robot_queue_sizes; 702 | std::map msg_map; 703 | auto it = submap_loop_closures_queue_.begin(); 704 | lcd::RobotId other_robot = 0; 705 | while (it != submap_loop_closures_queue_.end()) { 706 | const lcd::EdgeID& edge_id = it->first; 707 | const auto& factor = it->second; 708 | if (edge_id.robot_src == config_.my_id_) { 709 | other_robot = edge_id.robot_dst; 710 | } else { 711 | other_robot = edge_id.robot_src; 712 | } 713 | if (other_robot == config_.my_id_) { 714 | // This is a intra-robot loop closure and no need to synchronize 715 | submap_loop_closures_.add(factor); 716 | it = submap_loop_closures_queue_.erase(it); 717 | } else { 718 | // This is a inter-robot loop closure 719 | if (robot_queue_sizes.find(other_robot) == robot_queue_sizes.end()) { 720 | robot_queue_sizes[other_robot] = 0; 721 | pose_graph_tools_msgs::LoopClosures msg; 722 | msg.publishing_robot_id = config_.my_id_; 723 | msg.destination_robot_id = other_robot; 724 | msg_map[other_robot] = msg; 725 | } 726 | robot_queue_sizes[other_robot]++; 727 | if (msg_map[other_robot].edges.size() < config_.loop_batch_size_) { 728 | pose_graph_tools_msgs::PoseGraphEdge edge_msg; 729 | edge_msg.robot_from = edge_id.robot_src; 730 | edge_msg.robot_to = edge_id.robot_dst; 731 | edge_msg.key_from = edge_id.frame_src; 732 | edge_msg.key_to = edge_id.frame_dst; 733 | edge_msg.pose = GtsamPoseToRos(factor.measured()); 734 | // TODO: write covariance 735 | msg_map[other_robot].edges.push_back(edge_msg); 736 | } 737 | ++it; 738 | } 739 | } 740 | 741 | // Select the connected robot with largest queue size to synchronize 742 | lcd::RobotId selected_robot_id = 0; 743 | size_t selected_queue_size = 0; 744 | for (auto& it : robot_queue_sizes) { 745 | lcd::RobotId robot_id = it.first; 746 | size_t queue_size = it.second; 747 | if (robot_connected_[robot_id] && queue_size >= selected_queue_size) { 748 | selected_robot_id = robot_id; 749 | selected_queue_size = queue_size; 750 | } 751 | } 752 | if (selected_queue_size > 0) { 753 | ROS_WARN("Published %zu loops to robot %zu.", 754 | msg_map[selected_robot_id].edges.size(), 755 | selected_robot_id); 756 | loop_pub_.publish(msg_map[selected_robot_id]); 757 | } 758 | } 759 | 760 | void DistributedLoopClosureRos::loopClosureCallback( 761 | const pose_graph_tools_msgs::LoopClosuresConstPtr& msg) { 762 | if (msg->destination_robot_id != config_.my_id_) { 763 | return; 764 | } 765 | size_t loops_added = 0; 766 | pose_graph_tools_msgs::LoopClosuresAck ack_msg; 767 | ack_msg.publishing_robot_id = config_.my_id_; 768 | ack_msg.destination_robot_id = msg->publishing_robot_id; 769 | for (const auto& edge : msg->edges) { 770 | const lcd::EdgeID submap_edge_id( 771 | edge.robot_from, edge.key_from, edge.robot_to, edge.key_to); 772 | // For each incoming loop closure, only add locally if does not exist 773 | bool edge_exists = (submap_loop_closures_ids_.find(submap_edge_id) != 774 | submap_loop_closures_ids_.end()); 775 | if (!edge_exists) { 776 | loops_added++; 777 | submap_loop_closures_ids_.emplace(submap_edge_id); 778 | gtsam::Symbol submap_from(robot_id_to_prefix.at(edge.robot_from), edge.key_from); 779 | gtsam::Symbol submap_to(robot_id_to_prefix.at(edge.robot_to), edge.key_to); 780 | const auto pose = RosPoseToGtsam(edge.pose); 781 | // TODO: read covariance from message 782 | static const gtsam::SharedNoiseModel& noise = 783 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 784 | submap_loop_closures_.add( 785 | gtsam::BetweenFactor(submap_from, submap_to, pose, noise)); 786 | } 787 | // Always acknowledge all received loops 788 | ack_msg.robot_src.push_back(edge.robot_from); 789 | ack_msg.robot_dst.push_back(edge.robot_to); 790 | ack_msg.frame_src.push_back(edge.key_from); 791 | ack_msg.frame_dst.push_back(edge.key_to); 792 | } 793 | ROS_WARN("Received %zu loop closures from robot %i (%zu new).", 794 | msg->edges.size(), 795 | msg->publishing_robot_id, 796 | loops_added); 797 | loop_ack_pub_.publish(ack_msg); 798 | } 799 | 800 | void DistributedLoopClosureRos::loopAcknowledgementCallback( 801 | const pose_graph_tools_msgs::LoopClosuresAckConstPtr& msg) { 802 | if (msg->destination_robot_id != config_.my_id_) { 803 | return; 804 | } 805 | size_t loops_acked = 0; 806 | for (size_t i = 0; i < msg->robot_src.size(); ++i) { 807 | const lcd::EdgeID edge_id( 808 | msg->robot_src[i], msg->frame_src[i], msg->robot_dst[i], msg->frame_dst[i]); 809 | if (submap_loop_closures_queue_.find(edge_id) != 810 | submap_loop_closures_queue_.end()) { 811 | loops_acked++; 812 | // Move acknowledged loop closure from queue to factor graph 813 | // which will be shared with the back-end 814 | submap_loop_closures_.add(submap_loop_closures_queue_.at(edge_id)); 815 | submap_loop_closures_queue_.erase(edge_id); 816 | } 817 | } 818 | if (loops_acked > 0) { 819 | ROS_WARN( 820 | "Received %zu loop acks from robot %i.", loops_acked, msg->publishing_robot_id); 821 | } 822 | } 823 | 824 | void DistributedLoopClosureRos::processVLCRequests( 825 | const size_t& robot_id, 826 | const lcd::RobotPoseIdSet& vertex_ids) { 827 | if (vertex_ids.size() == 0) { 828 | return; 829 | } 830 | 831 | // ROS_INFO("Processing %lu VLC requests to robot %lu.", vertex_ids.size(), robot_id); 832 | if (robot_id == config_.my_id_) { 833 | // Directly request from Kimera-VIO-ROS 834 | { // start vlc service critical section 835 | std::unique_lock service_lock(vlc_service_mutex_); 836 | if (!requestVLCFrameService(vertex_ids)) { 837 | ROS_ERROR("Failed to retrieve local VLC frames on robot %zu.", config_.my_id_); 838 | } 839 | } 840 | } else { 841 | publishVLCRequests(robot_id, vertex_ids); 842 | } 843 | } 844 | 845 | void DistributedLoopClosureRos::publishVLCRequests( 846 | const size_t& robot_id, 847 | const lcd::RobotPoseIdSet& vertex_ids) { 848 | // Create requests msg 849 | pose_graph_tools_msgs::VLCRequests requests_msg; 850 | requests_msg.header.stamp = ros::Time::now(); 851 | requests_msg.source_robot_id = config_.my_id_; 852 | requests_msg.destination_robot_id = robot_id; 853 | for (const auto& vertex_id : vertex_ids) { 854 | // Do not request frame that already exists locally 855 | if (lcd_->frameExists(vertex_id)) { 856 | continue; 857 | } 858 | // Stop if reached batch size 859 | if (requests_msg.pose_ids.size() >= config_.vlc_batch_size_) { 860 | break; 861 | } 862 | 863 | // Double check robot id 864 | assert(robot_id == vertex_id.first); 865 | 866 | requests_msg.pose_ids.push_back(vertex_id.second); 867 | } 868 | 869 | vlc_requests_pub_.publish(requests_msg); 870 | ROS_INFO("Published %lu VLC requests to robot %lu.", 871 | requests_msg.pose_ids.size(), 872 | robot_id); 873 | } 874 | 875 | bool DistributedLoopClosureRos::requestVLCFrameService( 876 | const lcd::RobotPoseIdSet& vertex_ids) { 877 | ROS_WARN("Requesting %zu local VLC frames from Kimera-VIO.", vertex_ids.size()); 878 | 879 | // Request local VLC frames 880 | // Populate requested pose ids in ROS service query 881 | pose_graph_tools_msgs::VLCFrameQuery query; 882 | std::string service_name = 883 | "/" + config_.robot_names_[config_.my_id_] + "/kimera_vio_ros/vlc_frame_query"; 884 | query.request.robot_id = config_.my_id_; 885 | 886 | // Populate the pose ids to request 887 | for (const auto& vertex_id : vertex_ids) { 888 | // Do not request frame that already exists locally 889 | if (lcd_->frameExists(vertex_id)) { 890 | continue; 891 | } 892 | // Stop if reaching batch size 893 | if (query.request.pose_ids.size() >= config_.vlc_batch_size_) { 894 | break; 895 | } 896 | // We can only request via service local frames 897 | // Frames from other robots have to be requested by publisher 898 | assert(vertex_id.first == config_.my_id_); 899 | query.request.pose_ids.push_back(vertex_id.second); 900 | } 901 | 902 | // Call ROS service 903 | if (!ros::service::waitForService(service_name, ros::Duration(5.0))) { 904 | ROS_ERROR_STREAM("ROS service " << service_name << " does not exist!"); 905 | return false; 906 | } 907 | if (!ros::service::call(service_name, query)) { 908 | ROS_ERROR_STREAM("Could not query VLC frame!"); 909 | return false; 910 | } 911 | 912 | // Parse response 913 | for (const auto& frame_msg : query.response.frames) { 914 | lcd::VLCFrame frame; 915 | kimera_multi_lcd::VLCFrameFromMsg(frame_msg, &frame); 916 | assert(frame.robot_id_ == my_id_); 917 | lcd::RobotPoseId vertex_id(frame.robot_id_, frame.pose_id_); 918 | { // start lcd critical section 919 | std::unique_lock lcd_lock(lcd_mutex_); 920 | // Fill in submap information for this keyframe 921 | const auto keyframe = submap_atlas_->getKeyframe(frame.pose_id_); 922 | if (!keyframe) { 923 | ROS_WARN_STREAM("Received VLC frame " << frame.pose_id_ 924 | << " does not exist in submap atlas."); 925 | continue; 926 | } 927 | frame.submap_id_ = CHECK_NOTNULL(keyframe->getSubmap())->id(); 928 | frame.T_submap_pose_ = keyframe->getPoseInSubmapFrame(); 929 | lcd_->addVLCFrame(vertex_id, frame); 930 | } // end lcd critical section 931 | } 932 | return true; 933 | } 934 | 935 | void DistributedLoopClosureRos::vlcResponsesCallback( 936 | const pose_graph_tools_msgs::VLCFramesConstPtr& msg) { 937 | for (const auto& frame_msg : msg->frames) { 938 | lcd::VLCFrame frame; 939 | kimera_multi_lcd::VLCFrameFromMsg(frame_msg, &frame); 940 | lcd::RobotPoseId vertex_id(frame.robot_id_, frame.pose_id_); 941 | { // start lcd critical section 942 | std::unique_lock lcd_lock(lcd_mutex_); 943 | lcd_->addVLCFrame(vertex_id, frame); 944 | } // end lcd critical section 945 | // Inter-robot request will be counted as communication 946 | if (frame.robot_id_ != config_.my_id_) { 947 | received_vlc_bytes_.push_back( 948 | kimera_multi_lcd::computeVLCFramePayloadBytes(frame_msg)); 949 | } 950 | if (config_.run_offline_) { 951 | offline_robot_pose_msg_[vertex_id] = frame_msg; 952 | } 953 | } 954 | // ROS_INFO("Received %d VLC frames. ", msg->frames.size()); 955 | if (config_.run_offline_) { 956 | processOfflineLoopClosures(); 957 | } 958 | } 959 | 960 | void DistributedLoopClosureRos::internalVLCCallback( 961 | const pose_graph_tools_msgs::VLCFramesConstPtr& msg) { 962 | processInternalVLC(msg); 963 | } 964 | 965 | void DistributedLoopClosureRos::bowRequestsCallback( 966 | const pose_graph_tools_msgs::BowRequestsConstPtr& msg) { 967 | if (msg->destination_robot_id != config_.my_id_) return; 968 | if (msg->source_robot_id == config_.my_id_) { 969 | ROS_ERROR("Received BoW requests from myself!"); 970 | return; 971 | } 972 | // Push requested Bow Frame IDs to be transmitted later 973 | std::unique_lock requested_bows_lock(requested_bows_mutex_); 974 | if (requested_bows_.find(msg->source_robot_id) == requested_bows_.end()) 975 | requested_bows_[msg->source_robot_id] = std::set(); 976 | for (const auto& pose_id : msg->pose_ids) { 977 | requested_bows_[msg->source_robot_id].emplace(pose_id); 978 | } 979 | } 980 | 981 | void DistributedLoopClosureRos::vlcRequestsCallback( 982 | const pose_graph_tools_msgs::VLCRequestsConstPtr& msg) { 983 | if (msg->destination_robot_id != config_.my_id_) { 984 | return; 985 | } 986 | 987 | if (msg->source_robot_id == config_.my_id_) { 988 | ROS_ERROR("Received VLC requests from myself!"); 989 | return; 990 | } 991 | 992 | if (msg->pose_ids.empty()) { 993 | return; 994 | } 995 | 996 | // Find the vlc frames that we are missing 997 | lcd::RobotPoseIdSet missing_vertex_ids; 998 | for (const auto& pose_id : msg->pose_ids) { 999 | lcd::RobotPoseId vertex_id(config_.my_id_, pose_id); 1000 | if (!lcd_->frameExists(vertex_id)) { 1001 | missing_vertex_ids.emplace(vertex_id); 1002 | } 1003 | } 1004 | 1005 | if (!missing_vertex_ids.empty()) { // start vlc service critical section 1006 | std::unique_lock service_lock(vlc_service_mutex_); 1007 | if (!requestVLCFrameService(missing_vertex_ids)) { 1008 | ROS_ERROR_STREAM("Failed to retrieve local VLC frames on robot " 1009 | << config_.my_id_); 1010 | } 1011 | } 1012 | 1013 | // Push requested VLC frame IDs to queue to be transmitted later 1014 | std::unique_lock requested_frames_lock(requested_frames_mutex_); 1015 | if (requested_frames_.find(msg->source_robot_id) == requested_frames_.end()) 1016 | requested_frames_[msg->source_robot_id] = std::set(); 1017 | for (const auto& pose_id : msg->pose_ids) { 1018 | requested_frames_[msg->source_robot_id].emplace(pose_id); 1019 | } 1020 | } 1021 | 1022 | void DistributedLoopClosureRos::randomSleep(double min_sec, double max_sec) { 1023 | CHECK(min_sec < max_sec); 1024 | CHECK(min_sec > 0); 1025 | if (max_sec < 1e-3) return; 1026 | std::random_device rd; 1027 | std::mt19937 gen(rd()); 1028 | std::uniform_real_distribution distribution(min_sec, max_sec); 1029 | double sleep_time = distribution(gen); 1030 | // ROS_INFO("Sleep %f sec...", sleep_time); 1031 | ros::Duration(sleep_time).sleep(); 1032 | } 1033 | 1034 | void DistributedLoopClosureRos::publishSubmapOfflineInfo() { 1035 | pose_graph_tools_msgs::VLCFrames msg; 1036 | // Fill in keyframe poses in submaps 1037 | for (int submap_id = 0; submap_id < submap_atlas_->numSubmaps(); ++submap_id) { 1038 | const auto submap = CHECK_NOTNULL(submap_atlas_->getSubmap(submap_id)); 1039 | for (const int keyframe_id : submap->getKeyframeIDs()) { 1040 | const auto keyframe = CHECK_NOTNULL(submap->getKeyframe(keyframe_id)); 1041 | const auto T_submap_keyframe = keyframe->getPoseInSubmapFrame(); 1042 | pose_graph_tools_msgs::VLCFrameMsg frame_msg; 1043 | frame_msg.robot_id = config_.my_id_; 1044 | frame_msg.pose_id = keyframe_id; 1045 | frame_msg.submap_id = submap_id; 1046 | frame_msg.T_submap_pose = GtsamPoseToRos(T_submap_keyframe); 1047 | lcd::RobotPoseId vertex_id(config_.my_id_, keyframe_id); 1048 | offline_robot_pose_msg_[vertex_id] = frame_msg; 1049 | msg.frames.push_back(frame_msg); 1050 | } 1051 | } 1052 | for (lcd::RobotId robot_id = 0; robot_id < config_.my_id_; ++robot_id) { 1053 | msg.destination_robot_id = robot_id; 1054 | vlc_responses_pub_.publish(msg); 1055 | ros::Duration(1).sleep(); 1056 | } 1057 | } 1058 | 1059 | void DistributedLoopClosureRos::save() { 1060 | saveBowVectors(config_.log_output_dir_); 1061 | saveVLCFrames(config_.log_output_dir_); 1062 | } 1063 | } // namespace kimera_distributed 1064 | -------------------------------------------------------------------------------- /src/Submap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | 7 | #include "kimera_distributed/Submap.h" 8 | 9 | namespace kimera_distributed { 10 | 11 | Submap::Submap(int id, uint64_t stamp) : id_(id), stamp_(stamp), distance_(0) {} 12 | 13 | int Submap::id() const { return id_; } 14 | 15 | uint64_t Submap::stamp() const { return stamp_; } 16 | 17 | int Submap::numKeyframes() const { return (int)keyframes_.size(); } 18 | 19 | double Submap::distance() const { return distance_; } 20 | 21 | gtsam::Pose3 Submap::getPoseInOdomFrame() const { return T_odom_submap_; } 22 | 23 | void Submap::setPoseInOdomFrame(const gtsam::Pose3& T_odom_submap) { 24 | T_odom_submap_ = T_odom_submap; 25 | } 26 | 27 | gtsam::Pose3 Submap::getPoseInWorldFrame() const { return T_world_submap_; } 28 | 29 | void Submap::setPoseInWorldFrame(const gtsam::Pose3& T_world_submap) { 30 | T_world_submap_ = T_world_submap; 31 | } 32 | 33 | void Submap::addKeyframe(const std::shared_ptr& keyframe) { 34 | keyframes_.emplace(keyframe->id(), keyframe); 35 | // Update distance 36 | const auto keyframe_prev = getKeyframe(keyframe->id() - 1); 37 | if (keyframe_prev) { 38 | const auto T_submap_prev = keyframe_prev->getPoseInSubmapFrame(); 39 | const auto T_submap_curr = keyframe->getPoseInSubmapFrame(); 40 | const auto T_prev_curr = (T_submap_prev.inverse()) * T_submap_curr; 41 | distance_ += T_prev_curr.translation().norm(); 42 | } 43 | } 44 | 45 | std::shared_ptr Submap::getKeyframe(int keyframe_id) const { 46 | const auto it = keyframes_.find(keyframe_id); 47 | if (it == keyframes_.end()) { 48 | return nullptr; 49 | } else { 50 | return it->second; 51 | } 52 | } 53 | 54 | std::unordered_set Submap::getKeyframeIDs() const { 55 | std::unordered_set keyframe_ids; 56 | for (const auto& it : keyframes_) keyframe_ids.emplace(it.first); 57 | return keyframe_ids; 58 | } 59 | 60 | } // namespace kimera_distributed 61 | -------------------------------------------------------------------------------- /src/SubmapAtlas.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yulun on 3/19/22. 3 | // 4 | 5 | #include "kimera_distributed/SubmapAtlas.h" 6 | #include 7 | #include 8 | 9 | namespace kimera_distributed { 10 | 11 | SubmapAtlas::SubmapAtlas(const Parameters& params) : params_(params) {} 12 | 13 | SubmapAtlas::Parameters SubmapAtlas::params() const { return params_; } 14 | 15 | int SubmapAtlas::numKeyframes() const { return (int)keyframes_.size(); } 16 | 17 | int SubmapAtlas::numSubmaps() const { return (int)submaps_.size(); } 18 | 19 | std::shared_ptr SubmapAtlas::createKeyframe( 20 | int keyframe_id, 21 | const gtsam::Pose3& T_odom_keyframe, 22 | const uint64_t& timestamp) { 23 | if (hasKeyframe(keyframe_id)) { 24 | ROS_WARN_STREAM("Keyframe " << keyframe_id << " already exists!"); 25 | return getKeyframe(keyframe_id); 26 | } 27 | 28 | auto keyframe = std::make_shared(keyframe_id, timestamp); 29 | keyframe->setPoseInOdomFrame(T_odom_keyframe); 30 | keyframes_.emplace(keyframe_id, keyframe); 31 | 32 | // Create a new submap if needed 33 | if (shouldCreateNewSubmap()) { 34 | // Create a new submap with the same pose as the keyframe in the odom frame 35 | int new_submap_id = numSubmaps(); 36 | createSubmap(new_submap_id, T_odom_keyframe, timestamp); 37 | } 38 | 39 | // Add this keyframe to the submap 40 | auto submap = getLatestSubmap(); 41 | const auto& T_odom_submap = submap->getPoseInOdomFrame(); 42 | gtsam::Pose3 T_submap_keyframe = T_odom_submap.inverse() * T_odom_keyframe; 43 | keyframe->setPoseInSubmapFrame(T_submap_keyframe); 44 | keyframe->setSubmap(submap); 45 | submap->addKeyframe(keyframe); 46 | 47 | return keyframe; 48 | } 49 | 50 | bool SubmapAtlas::hasKeyframe(int keyframe_id) const { 51 | return keyframes_.find(keyframe_id) != keyframes_.end(); 52 | } 53 | 54 | std::shared_ptr SubmapAtlas::getKeyframe(int keyframe_id) { 55 | const auto& it = keyframes_.find(keyframe_id); 56 | if (it == keyframes_.end()) 57 | return nullptr; 58 | else 59 | return it->second; 60 | } 61 | 62 | std::shared_ptr SubmapAtlas::getKeyframeFromStamp(const uint64_t& timestamp, 63 | const uint64_t& tolNs) { 64 | int best_keyframe_id = 0; 65 | uint64_t best_stamp_diff = 2 * tolNs; 66 | for (const auto& it: keyframes_) { 67 | const auto keyframe_id = it.first; 68 | const uint64_t keyframe_stamp = it.second->stamp(); 69 | const uint64_t stamp_diff = (keyframe_stamp < timestamp) ? (timestamp-keyframe_stamp) : (keyframe_stamp-timestamp); 70 | if (stamp_diff <= best_stamp_diff) { 71 | best_stamp_diff = stamp_diff; 72 | best_keyframe_id = keyframe_id; 73 | } 74 | } 75 | if (best_stamp_diff > tolNs) { 76 | return nullptr; 77 | } else { 78 | return getKeyframe(best_keyframe_id); 79 | } 80 | } 81 | 82 | std::shared_ptr SubmapAtlas::getLatestKeyframe() { 83 | if (keyframes_.empty()) 84 | return nullptr; 85 | else { 86 | int keyframe_id = numKeyframes() - 1; 87 | CHECK_GE(keyframe_id, 0); 88 | return CHECK_NOTNULL(getKeyframe(keyframe_id)); 89 | } 90 | } 91 | 92 | std::shared_ptr SubmapAtlas::createSubmap(int submap_id, 93 | const gtsam::Pose3& T_odom_submap, 94 | const uint64_t& timestamp) { 95 | CHECK(!hasSubmap(submap_id)); 96 | auto submap = std::make_shared(submap_id, timestamp); 97 | submap->setPoseInOdomFrame(T_odom_submap); 98 | submaps_.emplace(submap_id, submap); 99 | 100 | if (submap_id == 0) { 101 | // Initialize world pose of the first submap 102 | submap->setPoseInWorldFrame(T_odom_submap); 103 | } else { 104 | auto prev_submap = CHECK_NOTNULL(getSubmap(submap_id - 1)); 105 | // Initialize the world pose by propagating odometry 106 | auto T_world_prev = prev_submap->getPoseInWorldFrame(); 107 | auto T_odom_prev = prev_submap->getPoseInOdomFrame(); 108 | auto T_prev_curr = T_odom_prev.inverse() * T_odom_submap; 109 | auto T_world_curr = T_world_prev * T_prev_curr; 110 | submap->setPoseInWorldFrame(T_world_curr); 111 | } 112 | 113 | return submap; 114 | } 115 | 116 | bool SubmapAtlas::hasSubmap(int submap_id) const { 117 | return submaps_.find(submap_id) != submaps_.end(); 118 | } 119 | 120 | std::shared_ptr SubmapAtlas::getSubmap(int submap_id) { 121 | const auto& it = submaps_.find(submap_id); 122 | if (it == submaps_.end()) 123 | return nullptr; 124 | else 125 | return it->second; 126 | } 127 | 128 | std::shared_ptr SubmapAtlas::getLatestSubmap() { 129 | if (submaps_.empty()) 130 | return nullptr; 131 | else { 132 | int submap_id = numSubmaps() - 1; 133 | CHECK_GE(submap_id, 0); 134 | return CHECK_NOTNULL(getSubmap(submap_id)); 135 | } 136 | } 137 | 138 | bool SubmapAtlas::shouldCreateNewSubmap() { 139 | if (submaps_.empty()) return true; 140 | if (getLatestSubmap()->numKeyframes() >= params_.max_submap_size) return true; 141 | if (getLatestSubmap()->distance() > params_.max_submap_distance) return true; 142 | 143 | return false; 144 | } 145 | 146 | } // namespace kimera_distributed 147 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yulun Tian (yulun@mit.edu) 5 | */ 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kimera_distributed { 13 | 14 | gtsam::Pose3 RosPoseToGtsam(const geometry_msgs::Pose& transform) { 15 | gtsam::Pose3 pose; 16 | pose = gtsam::Pose3( 17 | gtsam::Rot3(transform.orientation.w, 18 | transform.orientation.x, 19 | transform.orientation.y, 20 | transform.orientation.z), 21 | gtsam::Point3(transform.position.x, transform.position.y, transform.position.z)); 22 | return pose; 23 | } 24 | 25 | geometry_msgs::Pose GtsamPoseToRos(const gtsam::Pose3& transform) { 26 | geometry_msgs::Pose pose; 27 | 28 | const gtsam::Point3& position = transform.translation(); 29 | const gtsam::Quaternion& orientation = transform.rotation().toQuaternion(); 30 | 31 | pose.position.x = position.x(); 32 | pose.position.y = position.y(); 33 | pose.position.z = position.z(); 34 | 35 | pose.orientation.x = orientation.x(); 36 | pose.orientation.y = orientation.y(); 37 | pose.orientation.z = orientation.z(); 38 | pose.orientation.w = orientation.w(); 39 | 40 | return pose; 41 | } 42 | 43 | void GtsamPoseToRosTf(const gtsam::Pose3& pose, geometry_msgs::Transform* tf) { 44 | CHECK_NOTNULL(tf); 45 | tf->translation.x = pose.x(); 46 | tf->translation.y = pose.y(); 47 | tf->translation.z = pose.z(); 48 | const gtsam::Quaternion& quat = pose.rotation().toQuaternion(); 49 | tf->rotation.w = quat.w(); 50 | tf->rotation.x = quat.x(); 51 | tf->rotation.y = quat.y(); 52 | tf->rotation.z = quat.z(); 53 | } 54 | 55 | // Convert gtsam posegaph to PoseGraph msg 56 | pose_graph_tools_msgs::PoseGraph GtsamGraphToRos(const gtsam::NonlinearFactorGraph& factors, 57 | const gtsam::Values& values, 58 | const gtsam::Vector& gnc_weights) { 59 | std::vector edges; 60 | size_t single_robot_lcs = 0; 61 | size_t inter_robot_lcs = 0; 62 | size_t single_robot_inliers = 0; 63 | size_t inter_robot_inliers = 0; 64 | // first store the factors as edges 65 | for (size_t i = 0; i < factors.size(); i++) { 66 | // check if between factor 67 | if (boost::dynamic_pointer_cast>(factors[i])) { 68 | // convert to between factor 69 | const gtsam::BetweenFactor& factor = 70 | *boost::dynamic_pointer_cast>(factors[i]); 71 | // convert between factor to PoseGraphEdge type 72 | pose_graph_tools_msgs::PoseGraphEdge edge; 73 | gtsam::Symbol front(factor.front()); 74 | gtsam::Symbol back(factor.back()); 75 | edge.key_from = front.index(); 76 | edge.key_to = back.index(); 77 | edge.robot_from = robot_prefix_to_id.at(front.chr()); 78 | edge.robot_to = robot_prefix_to_id.at(back.chr()); 79 | 80 | if (edge.key_to == edge.key_from + 1 && 81 | edge.robot_from == edge.robot_to) { // check if odom 82 | edge.type = pose_graph_tools_msgs::PoseGraphEdge::ODOM; 83 | 84 | } else { 85 | edge.type = pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE; 86 | if (edge.robot_from == edge.robot_to) { 87 | single_robot_lcs++; 88 | } else { 89 | inter_robot_lcs++; 90 | } 91 | if (gnc_weights.size() == factors.size() && gnc_weights(i) < 0.5) { 92 | edge.type = pose_graph_tools_msgs::PoseGraphEdge::REJECTED_LOOPCLOSE; 93 | } else { 94 | if (edge.robot_from == edge.robot_to) { 95 | single_robot_inliers++; 96 | } else { 97 | inter_robot_inliers++; 98 | } 99 | } 100 | } 101 | 102 | edge.pose = GtsamPoseToRos(factor.measured()); 103 | 104 | // transfer covariance 105 | gtsam::Matrix66 covariance = 106 | boost::dynamic_pointer_cast(factor.noiseModel()) 107 | ->covariance(); 108 | for (size_t i = 0; i < edge.covariance.size(); i++) { 109 | size_t row = static_cast(i / 6); 110 | size_t col = i % 6; 111 | edge.covariance[i] = covariance(row, col); 112 | } 113 | edges.push_back(edge); 114 | } 115 | } 116 | 117 | std::vector nodes; 118 | // Then store the values as nodes 119 | gtsam::KeyVector key_list = values.keys(); 120 | for (size_t i = 0; i < key_list.size(); i++) { 121 | gtsam::Symbol node_symb(key_list[i]); 122 | try { 123 | size_t robot_id = robot_prefix_to_id.at(node_symb.chr()); 124 | 125 | pose_graph_tools_msgs::PoseGraphNode node; 126 | node.key = node_symb.index(); 127 | node.robot_id = robot_id; 128 | const gtsam::Pose3& value = values.at(key_list[i]); 129 | const gtsam::Point3& translation = value.translation(); 130 | const gtsam::Quaternion& quaternion = value.rotation().toQuaternion(); 131 | 132 | node.pose = GtsamPoseToRos(value); 133 | nodes.push_back(node); 134 | } catch (...) { 135 | // ignore 136 | } 137 | } 138 | 139 | pose_graph_tools_msgs::PoseGraph posegraph; 140 | posegraph.nodes = nodes; 141 | posegraph.edges = edges; 142 | // ROS_INFO( 143 | // "Detected %d single robot loop closures with %d inliers and %d " 144 | // "inter-robot loop closures with %d inliers. ", 145 | // single_robot_lcs, 146 | // single_robot_inliers, 147 | // inter_robot_lcs, 148 | // inter_robot_inliers); 149 | return posegraph; 150 | } 151 | 152 | nav_msgs::Path GtsamPoseTrajectoryToPath(const std::vector& gtsam_poses) { 153 | nav_msgs::Path msg; 154 | msg.header.frame_id = "/world"; 155 | msg.header.stamp = ros::Time::now(); 156 | for (size_t i = 0; i < gtsam_poses.size(); i++) { 157 | geometry_msgs::PoseStamped pose_stamped; 158 | pose_stamped.pose = GtsamPoseToRos(gtsam_poses[i]); 159 | msg.poses.push_back(pose_stamped); 160 | } 161 | return msg; 162 | } 163 | 164 | bool hasBetweenFactor(const gtsam::NonlinearFactorGraph& nfg, 165 | const gtsam::Symbol& symbol_src, 166 | const gtsam::Symbol& symbol_dst) { 167 | for (size_t i = 0; i < nfg.size(); i++) { 168 | // check if between factor 169 | if (boost::dynamic_pointer_cast>(nfg[i])) { 170 | // convert to between factor 171 | const gtsam::BetweenFactor& factor = 172 | *boost::dynamic_pointer_cast>(nfg[i]); 173 | gtsam::Symbol front(factor.front()); 174 | gtsam::Symbol back(factor.back()); 175 | if (front == symbol_src && back == symbol_dst) { 176 | return true; 177 | } 178 | } 179 | } 180 | return false; 181 | } 182 | 183 | } // namespace kimera_distributed -------------------------------------------------------------------------------- /srv/addLoopClosure.srv: -------------------------------------------------------------------------------- 1 | pose_graph_tools_msgs/PoseGraphEdge loop_closure 2 | --- 3 | -------------------------------------------------------------------------------- /srv/requestSharedLoopClosures.srv: -------------------------------------------------------------------------------- 1 | uint32 robot_id 2 | --- 3 | pose_graph_tools_msgs/PoseGraphEdge[] loop_closures -------------------------------------------------------------------------------- /test/test_submap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "kimera_distributed/Keyframe.h" 21 | #include "kimera_distributed/Submap.h" 22 | 23 | namespace kimera_distributed { 24 | 25 | TEST(SubmapTest, AddKeyframe) { 26 | auto keyframe_0 = std::make_shared(0, 1e+09); 27 | 28 | Submap submap(0, 1e+09); 29 | submap.addKeyframe(keyframe_0); 30 | 31 | EXPECT_EQ(1, submap.numKeyframes()); 32 | EXPECT_EQ(0, submap.getKeyframe(0)->id()); 33 | EXPECT_EQ(1e+09, submap.getKeyframe(0)->stamp()); 34 | } 35 | 36 | TEST(SubmapTest, SubmapDistancce) { 37 | auto keyframe_0 = std::make_shared(0, 1e+09); 38 | 39 | Submap submap(0, 1e+09); 40 | submap.addKeyframe(keyframe_0); 41 | 42 | auto keyframe_1 = std::make_shared(1, 2e+09); 43 | keyframe_1->setPoseInSubmapFrame(gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1, 0, 0))); 44 | submap.addKeyframe(keyframe_1); 45 | 46 | EXPECT_EQ(1.0, submap.distance()); 47 | } 48 | 49 | TEST(SubmapTest, SubmapPose) { 50 | Submap submap(0, 1e+09); 51 | gtsam::Pose3 pose_in_odom_frame(gtsam::Rot3(0, 1, 0, 0), gtsam::Point3(0, 1, 0)); 52 | gtsam::Pose3 pose_in_world_frame(gtsam::Rot3(0, 0, 1, 0), gtsam::Point3(0, 0, 1)); 53 | submap.setPoseInOdomFrame(pose_in_odom_frame); 54 | submap.setPoseInWorldFrame(pose_in_world_frame); 55 | 56 | EXPECT_TRUE(gtsam::assert_equal(pose_in_odom_frame, submap.getPoseInOdomFrame())); 57 | EXPECT_TRUE(gtsam::assert_equal(pose_in_world_frame, submap.getPoseInWorldFrame())); 58 | } 59 | 60 | } // namespace kimera_distributed 61 | 62 | int main(int argc, char** argv) { 63 | ::testing::InitGoogleTest(&argc, argv); 64 | return RUN_ALL_TESTS(); 65 | } 66 | -------------------------------------------------------------------------------- /test/test_submap_atlas.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "kimera_distributed/SubmapAtlas.h" 21 | 22 | namespace kimera_distributed { 23 | 24 | SubmapAtlas createDefaultAtlas() { 25 | SubmapAtlas::Parameters submap_params; 26 | submap_params.max_submap_size = 3; 27 | submap_params.max_submap_distance = 1.0; 28 | SubmapAtlas atlas(submap_params); 29 | return atlas; 30 | } 31 | 32 | TEST(SubmapAtlasTest, SubmapAtlasParams) { 33 | SubmapAtlas submap_atlas = createDefaultAtlas(); 34 | EXPECT_EQ(3, submap_atlas.params().max_submap_size); 35 | EXPECT_EQ(1.0, submap_atlas.params().max_submap_distance); 36 | } 37 | 38 | TEST(SubmapAtlasTest, CreateKeyFrame) { 39 | SubmapAtlas submap_atlas = createDefaultAtlas(); 40 | gtsam::Pose3 T_odom_keyframe_0 = 41 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.1, 0, 0)); 42 | auto keyframe_0 = submap_atlas.createKeyframe(0, T_odom_keyframe_0, 1e+09); 43 | 44 | EXPECT_TRUE(submap_atlas.hasKeyframe(0)); 45 | EXPECT_TRUE(submap_atlas.hasSubmap(0)); 46 | EXPECT_TRUE(gtsam::assert_equal(gtsam::Pose3(), 47 | submap_atlas.getKeyframe(0)->getPoseInSubmapFrame())); 48 | EXPECT_TRUE(gtsam::assert_equal(T_odom_keyframe_0, 49 | submap_atlas.getKeyframe(0)->getPoseInOdomFrame())); 50 | 51 | gtsam::Pose3 T_odom_keyframe_1 = 52 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.3, 0, 0)); 53 | auto keyframe_1 = submap_atlas.createKeyframe(1, T_odom_keyframe_1, 2e+09); 54 | 55 | EXPECT_TRUE(submap_atlas.hasKeyframe(1)); 56 | EXPECT_FALSE(submap_atlas.hasSubmap(1)); 57 | EXPECT_TRUE(gtsam::assert_equal(gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.2, 0, 0)), 58 | submap_atlas.getKeyframe(1)->getPoseInSubmapFrame())); 59 | EXPECT_TRUE( 60 | gtsam::assert_equal(gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.2, 0, 0)), 61 | submap_atlas.getLatestKeyframe()->getPoseInSubmapFrame())); 62 | EXPECT_TRUE(gtsam::assert_equal(T_odom_keyframe_1, 63 | submap_atlas.getKeyframe(1)->getPoseInOdomFrame())); 64 | } 65 | 66 | TEST(SubmapAtlasTest, CreateSubMapDistance) { 67 | SubmapAtlas submap_atlas = createDefaultAtlas(); 68 | gtsam::Pose3 T_odom_keyframe_0 = 69 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.1, 0, 0)); 70 | auto keyframe_0 = submap_atlas.createKeyframe(0, T_odom_keyframe_0, 1e+09); 71 | 72 | gtsam::Pose3 T_odom_keyframe_1 = 73 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1.2, 0, 0)); 74 | auto keyframe_1 = submap_atlas.createKeyframe(1, T_odom_keyframe_1, 2e+09); 75 | 76 | EXPECT_TRUE(submap_atlas.hasSubmap(0)); 77 | EXPECT_FALSE(submap_atlas.hasSubmap(1)); 78 | 79 | gtsam::Pose3 T_odom_keyframe_2 = 80 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1.3, 0, 0)); 81 | auto keyframe_2 = submap_atlas.createKeyframe(2, T_odom_keyframe_2, 3e+09); 82 | 83 | EXPECT_TRUE(submap_atlas.hasSubmap(1)); 84 | EXPECT_EQ(2, submap_atlas.numSubmaps()); 85 | EXPECT_TRUE(gtsam::assert_equal(gtsam::Pose3(), keyframe_2->getPoseInSubmapFrame())); 86 | EXPECT_TRUE(gtsam::assert_equal(T_odom_keyframe_2, keyframe_2->getPoseInOdomFrame())); 87 | } 88 | 89 | TEST(SubmapAtlasTest, CreateSubMapNumFrames) { 90 | SubmapAtlas submap_atlas = createDefaultAtlas(); 91 | gtsam::Pose3 T_odom_keyframe_0 = 92 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.1, 0, 0)); 93 | auto keyframe_0 = submap_atlas.createKeyframe(0, T_odom_keyframe_0, 1e+09); 94 | 95 | gtsam::Pose3 T_odom_keyframe_1 = 96 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.2, 0, 0)); 97 | auto keyframe_1 = submap_atlas.createKeyframe(1, T_odom_keyframe_1, 2e+09); 98 | 99 | gtsam::Pose3 T_odom_keyframe_2 = 100 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.3, 0, 0)); 101 | auto keyframe_2 = submap_atlas.createKeyframe(2, T_odom_keyframe_2, 3e+09); 102 | 103 | EXPECT_TRUE(submap_atlas.hasSubmap(0)); 104 | EXPECT_FALSE(submap_atlas.hasSubmap(1)); 105 | 106 | gtsam::Pose3 T_odom_keyframe_3 = 107 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0.4, 0, 0)); 108 | auto keyframe_3 = submap_atlas.createKeyframe(3, T_odom_keyframe_3, 4e+09); 109 | 110 | EXPECT_TRUE(submap_atlas.hasSubmap(1)); 111 | EXPECT_EQ(2, submap_atlas.numSubmaps()); 112 | } 113 | 114 | } // namespace kimera_distributed 115 | 116 | int main(int argc, char** argv) { 117 | ::testing::InitGoogleTest(&argc, argv); 118 | return RUN_ALL_TESTS(); 119 | } 120 | -------------------------------------------------------------------------------- /test/test_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright Notes 3 | * 4 | * Authors: Yun Chang (yunchang@mit.edu) 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "kimera_distributed/utils.h" 21 | 22 | namespace kimera_distributed { 23 | 24 | TEST(UtilsTest, RosPoseToGtsam) { 25 | geometry_msgs::Pose ros_pose; 26 | ros_pose.position.x = 1.0; 27 | ros_pose.position.y = 2.0; 28 | ros_pose.position.z = 3.0; 29 | 30 | ros_pose.orientation.x = 0.0; 31 | ros_pose.orientation.y = 1.0; 32 | ros_pose.orientation.z = 0.0; 33 | ros_pose.orientation.w = 0.0; 34 | 35 | gtsam::Pose3 ros_pose_converted = RosPoseToGtsam(ros_pose); 36 | EXPECT_TRUE(gtsam::assert_equal( 37 | ros_pose_converted, 38 | gtsam::Pose3(gtsam::Rot3(0, 0, 1, 0), gtsam::Point3(1, 2, 3)))); 39 | EXPECT_EQ(ros_pose, GtsamPoseToRos(ros_pose_converted)); 40 | } 41 | 42 | TEST(UtilsTest, GtsamGraphToRos) { 43 | gtsam::NonlinearFactorGraph gtsam_graph; 44 | gtsam::Values gtsam_values; 45 | 46 | for (size_t i = 0; i < 3; i++) { 47 | gtsam_values.insert( 48 | gtsam::Symbol('a', i), 49 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(static_cast(i), 0, 0))); 50 | } 51 | static const gtsam::SharedNoiseModel& noise = 52 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 53 | gtsam_graph.add(gtsam::BetweenFactor( 54 | gtsam::Symbol('a', 0), 55 | gtsam::Symbol('a', 1), 56 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1, 0, 0)), 57 | noise)); 58 | 59 | gtsam_graph.add(gtsam::BetweenFactor( 60 | gtsam::Symbol('a', 1), 61 | gtsam::Symbol('a', 2), 62 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1, 0, 0)), 63 | noise)); 64 | 65 | gtsam_graph.add(gtsam::BetweenFactor( 66 | gtsam::Symbol('a', 2), 67 | gtsam::Symbol('a', 0), 68 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1, 0, 0)), 69 | noise)); 70 | 71 | pose_graph_tools::PoseGraph ros_graph = GtsamGraphToRos(gtsam_graph, gtsam_values); 72 | 73 | EXPECT_EQ(ros_graph.edges.size(), 3); 74 | EXPECT_EQ(ros_graph.nodes.size(), 3); 75 | 76 | for (size_t i = 0; i < 3; i++) { 77 | EXPECT_EQ(static_cast(i), ros_graph.nodes[i].pose.position.x); 78 | EXPECT_EQ(1.0, ros_graph.edges[i].pose.position.x); 79 | } 80 | } 81 | 82 | TEST(UtilsTest, GtsamPoseTrajectoryToPath) { 83 | ros::Time::init(); 84 | std::vector pose_path; 85 | 86 | for (size_t i = 0; i < 3; i++) { 87 | pose_path.push_back( 88 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(static_cast(i), 0, 0))); 89 | } 90 | 91 | nav_msgs::Path ros_path = GtsamPoseTrajectoryToPath(pose_path); 92 | 93 | EXPECT_EQ(3, ros_path.poses.size()); 94 | for (size_t i = 0; i < 3; i++) { 95 | EXPECT_EQ(static_cast(i), ros_path.poses[i].pose.position.x); 96 | } 97 | } 98 | 99 | TEST(UtilsTest, hasBetweenFactor) { 100 | gtsam::NonlinearFactorGraph gtsam_graph; 101 | gtsam::Values gtsam_values; 102 | 103 | for (size_t i = 0; i < 2; i++) { 104 | gtsam_values.insert( 105 | gtsam::Symbol('a', i), 106 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(static_cast(i), 0, 0))); 107 | } 108 | static const gtsam::SharedNoiseModel& noise = 109 | gtsam::noiseModel::Isotropic::Variance(6, 1e-2); 110 | gtsam_graph.add(gtsam::BetweenFactor( 111 | gtsam::Symbol('a', 0), 112 | gtsam::Symbol('a', 1), 113 | gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(1, 0, 0)), 114 | noise)); 115 | 116 | EXPECT_TRUE( 117 | hasBetweenFactor(gtsam_graph, gtsam::Symbol('a', 0), gtsam::Symbol('a', 1))); 118 | EXPECT_FALSE( 119 | hasBetweenFactor(gtsam_graph, gtsam::Symbol('a', 0), gtsam::Symbol('a', 2))); 120 | } 121 | 122 | } // namespace kimera_distributed 123 | 124 | int main(int argc, char** argv) { 125 | ::testing::InitGoogleTest(&argc, argv); 126 | return RUN_ALL_TESTS(); 127 | } 128 | --------------------------------------------------------------------------------