├── pose_graph_tools_msgs ├── msg │ ├── BowVector.msg │ ├── BowQuery.msg │ ├── PoseGraphNode.msg │ ├── BowQueries.msg │ ├── VLCFrames.msg │ ├── LoopClosures.msg │ ├── PoseGraph.msg │ ├── LoopClosuresAck.msg │ ├── BowRequests.msg │ ├── VLCRequests.msg │ ├── TimeRangeQuery.msg │ ├── PoseGraphEdge.msg │ └── VLCFrameMsg.msg ├── srv │ ├── PoseGraphQuery.srv │ ├── VLCFrameQuery.srv │ └── LcdFrameRegistration.srv ├── package.xml └── CMakeLists.txt ├── pose_graph_tools ├── include │ └── pose_graph_tools │ │ ├── all.h │ │ ├── bow_query.h │ │ ├── bow_vector.h │ │ ├── bow_queries.h │ │ ├── pose_graph_node.h │ │ ├── pose_graph.h │ │ └── pose_graph_edge.h ├── package.xml ├── cmake │ └── pose_graph_toolsConfig.cmake.in ├── CMakeLists.txt └── src │ └── ostream.cpp ├── pose_graph_tools_ros ├── src │ ├── visualizer_node.cpp │ ├── utils.cpp │ ├── conversions.cpp │ └── visualizer.cpp ├── launch │ └── posegraph_view.launch ├── package.xml ├── include │ └── pose_graph_tools_ros │ │ ├── utils.h │ │ ├── visualizer.h │ │ └── conversions.h └── CMakeLists.txt ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE.BSD └── .clang-format /pose_graph_tools_msgs/msg/BowVector.msg: -------------------------------------------------------------------------------- 1 | uint32[] word_ids 2 | float32[] word_values 3 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/srv/PoseGraphQuery.srv: -------------------------------------------------------------------------------- 1 | uint16 robot_id 2 | --- 3 | pose_graph_tools_msgs/PoseGraph pose_graph 4 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/BowQuery.msg: -------------------------------------------------------------------------------- 1 | uint32 robot_id 2 | uint32 pose_id 3 | pose_graph_tools_msgs/BowVector bow_vector 4 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/PoseGraphNode.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int32 robot_id 4 | 5 | uint64 key 6 | 7 | geometry_msgs/Pose pose 8 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/srv/VLCFrameQuery.srv: -------------------------------------------------------------------------------- 1 | uint32 robot_id 2 | uint32[] pose_ids 3 | --- 4 | pose_graph_tools_msgs/VLCFrameMsg[] frames 5 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/BowQueries.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32 destination_robot_id 3 | pose_graph_tools_msgs/BowQuery[] queries 4 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/VLCFrames.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32 destination_robot_id 3 | pose_graph_tools_msgs/VLCFrameMsg[] frames 4 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/LoopClosures.msg: -------------------------------------------------------------------------------- 1 | uint16 publishing_robot_id 2 | uint16 destination_robot_id 3 | pose_graph_tools_msgs/PoseGraphEdge[] edges 4 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/all.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pose_graph_tools/bow_queries.h" 4 | #include "pose_graph_tools/pose_graph.h" 5 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/PoseGraph.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # Nodes and edges 4 | pose_graph_tools_msgs/PoseGraphNode[] nodes 5 | pose_graph_tools_msgs/PoseGraphEdge[] edges 6 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/srv/LcdFrameRegistration.srv: -------------------------------------------------------------------------------- 1 | uint64 query_robot 2 | uint64 match_robot 3 | uint64 query 4 | uint64 match 5 | --- 6 | bool valid 7 | geometry_msgs/Pose match_T_query 8 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/LoopClosuresAck.msg: -------------------------------------------------------------------------------- 1 | uint16 publishing_robot_id 2 | uint16 destination_robot_id 3 | uint32[] robot_src 4 | uint32[] frame_src 5 | uint32[] robot_dst 6 | uint32[] frame_dst 7 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/BowRequests.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32 source_robot_id # Robot that sends this request 3 | uint32 destination_robot_id # Robot that receives this request 4 | uint32[] pose_ids # Pose IDs of requested Bow of the destination robot 5 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/VLCRequests.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32 source_robot_id # Robot that sends this request 3 | uint32 destination_robot_id # Robot that receives this request 4 | uint32[] pose_ids # Pose IDs of requested VLC frames of the destination robot 5 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/TimeRangeQuery.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32 source_robot_id # Robot that sends this request 3 | uint32 destination_robot_id # Robot that receives this request 4 | time start # Start of time range 5 | time end # End of time range 6 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/src/visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "pose_graph_tools_ros/visualizer.h" 5 | 6 | int main(int argc, char *argv[]) { 7 | // Initiallize visualizer 8 | ros::init(argc, argv, "visualizer"); 9 | ros::NodeHandle nh("~"); 10 | Visualizer viz(nh); 11 | } 12 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/PoseGraphEdge.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint64 key_from 4 | uint64 key_to 5 | 6 | int32 robot_from 7 | int32 robot_to 8 | 9 | int32 type 10 | 11 | # Type enums 12 | int32 ODOM = 0 13 | int32 LOOPCLOSE = 1 14 | int32 LANDMARK = 2 15 | int32 REJECTED_LOOPCLOSE = 3 16 | int32 MESH = 4 17 | int32 POSE_MESH = 5 18 | int32 MESH_POSE = 6 19 | 20 | # Transforms in ede 21 | geometry_msgs/Pose pose 22 | float64[36] covariance 23 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/msg/VLCFrameMsg.msg: -------------------------------------------------------------------------------- 1 | uint32 robot_id 2 | uint32 pose_id 3 | uint32 submap_id 4 | sensor_msgs/Image descriptors_mat # Descriptor of all keypoints stored as a matrix. 5 | sensor_msgs/PointCloud2 versors # Bearing vector of each keypoint expressed as a 3D vector. 6 | float32[] depths # Depth of each keypoint. The 3D keypoint v can be recovered from the depth d and bearing u by v = d * u / u[2] 7 | geometry_msgs/Pose T_submap_pose 8 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/launch/posegraph_view.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/bow_query.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "pose_graph_tools/bow_vector.h" 6 | 7 | namespace pose_graph_tools { 8 | 9 | struct BowQuery { 10 | using Ptr = std::shared_ptr; 11 | using ConstPtr = std::shared_ptr; 12 | 13 | uint32_t robot_id; 14 | uint32_t pose_id; 15 | BowVector bow_vector; 16 | 17 | friend std::ostream& operator<<(std::ostream& os, const BowQuery& query); 18 | }; 19 | 20 | } // namespace pose_graph_tools 21 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/bow_vector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pose_graph_tools { 7 | 8 | struct BowVector { 9 | using Ptr = std::shared_ptr; 10 | using ConstPtr = std::shared_ptr; 11 | 12 | std::vector word_ids; 13 | std::vector word_values; 14 | 15 | friend std::ostream& operator<<(std::ostream& os, 16 | const BowVector& bow_vector); 17 | }; 18 | 19 | } // namespace pose_graph_tools 20 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/bow_queries.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "pose_graph_tools/bow_query.h" 7 | 8 | namespace pose_graph_tools { 9 | 10 | struct BowQueries { 11 | using Ptr = std::shared_ptr; 12 | using ConstPtr = std::shared_ptr; 13 | 14 | uint32_t destination_robot_id; 15 | std::vector queries; 16 | 17 | friend std::ostream& operator<<(std::ostream& os, const BowQueries& queries); 18 | }; 19 | 20 | } // namespace pose_graph_tools 21 | -------------------------------------------------------------------------------- /pose_graph_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_graph_tools 4 | 1.0.0 5 | Type definitions for working with pose graphs. 6 | 7 | Yun Chang 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | cmake 13 | eigen 14 | 15 | 16 | cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/pose_graph_node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace pose_graph_tools { 9 | 10 | struct PoseGraphNode { 11 | using Ptr = std::shared_ptr; 12 | using ConstPtr = std::shared_ptr; 13 | 14 | uint64_t stamp_ns; 15 | int32_t robot_id; 16 | uint64_t key; 17 | Eigen::Affine3d pose; 18 | 19 | friend std::ostream& operator<<(std::ostream& os, const PoseGraphNode& node); 20 | }; 21 | 22 | } // namespace pose_graph_tools 23 | -------------------------------------------------------------------------------- /pose_graph_tools/cmake/pose_graph_toolsConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | include(CMakeFindDependencyMacro) 3 | 4 | get_filename_component(pose_graph_tools_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" 5 | PATH) 6 | 7 | find_dependency(Eigen3 REQUIRED) 8 | 9 | if(NOT TARGET pose_graph_tools::pose_graph_tools) 10 | include("${pose_graph_tools_CMAKE_DIR}/pose_graph_toolsTargets.cmake") 11 | endif() 12 | 13 | set(pose_graph_tools_LIBRARIES pose_graph_tools::pose_graph_tools) 14 | set(pose_graph_tools_FOUND_CATKIN_PROJECT TRUE) 15 | 16 | check_required_components(pose_graph_tools) 17 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_graph_tools_msgs 4 | 1.0.0 5 | ROS message and service definitions of pose_graph_tools. 6 | 7 | Yun Chang 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | message_generation 14 | message_runtime 15 | std_msgs 16 | geometry_msgs 17 | sensor_msgs 18 | 19 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/pose_graph.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "pose_graph_tools/pose_graph_edge.h" 7 | #include "pose_graph_tools/pose_graph_node.h" 8 | 9 | namespace pose_graph_tools { 10 | 11 | struct PoseGraph { 12 | using Ptr = std::shared_ptr; 13 | using ConstPtr = std::shared_ptr; 14 | 15 | uint64_t stamp_ns; 16 | std::vector nodes; 17 | std::vector edges; 18 | 19 | inline bool empty() const { return nodes.empty() && edges.empty(); } 20 | friend std::ostream& operator<<(std::ostream& os, const PoseGraph& graph); 21 | }; 22 | 23 | } // namespace pose_graph_tools 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | # VSCode 54 | .vscode/ 55 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pose_graph_tools_ros 4 | 1.0.0 5 | ROS message definitions of pose_graph_tools. 6 | 7 | Yun Chang 8 | Lukas Schmid 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | 15 | pose_graph_tools 16 | 17 | roscpp 18 | tf2_eigen 19 | 20 | pose_graph_tools_msgs 21 | std_msgs 22 | geometry_msgs 23 | visualization_msgs 24 | interactive_markers 25 | 26 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: check-yaml 6 | - id: check-json 7 | - id: check-xml 8 | - id: end-of-file-fixer 9 | - id: trailing-whitespace 10 | - id: check-added-large-files 11 | - id: check-merge-conflict 12 | 13 | - repo: https://github.com/psf/black 14 | rev: '23.3.0' 15 | hooks: 16 | - id: black 17 | 18 | - repo: https://github.com/pylint-dev/pylint 19 | rev: 'v3.0.0a6' 20 | hooks: 21 | - id: pylint 22 | 23 | - repo: https://github.com/pre-commit/mirrors-clang-format 24 | rev: 'v16.0.3' 25 | hooks: 26 | - id: clang-format 27 | 28 | - repo: https://github.com/cpplint/cpplint 29 | rev: '1.6.1' 30 | hooks: 31 | - id: cpplint 32 | args: ['--filter=-whitespace/line_length,-legal/copyright,-build/include_order,-runtime/references,-build/c++11'] 33 | exclude: 3rd_party/ 34 | -------------------------------------------------------------------------------- /pose_graph_tools/include/pose_graph_tools/pose_graph_edge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace pose_graph_tools { 9 | 10 | struct PoseGraphEdge { 11 | using Ptr = std::shared_ptr; 12 | using ConstPtr = std::shared_ptr; 13 | 14 | enum Type : int { 15 | ODOM = 0, 16 | LOOPCLOSE = 1, 17 | LANDMARK = 2, 18 | REJECTED_LOOPCLOSE = 3, 19 | MESH = 4, 20 | POSE_MESH = 5, 21 | MESH_POSE = 6, 22 | PRIOR = 7, 23 | REJECTED_PRIOR = 8, 24 | }; 25 | 26 | uint64_t key_from; 27 | uint64_t key_to; 28 | 29 | int32_t robot_from; 30 | int32_t robot_to; 31 | 32 | Type type; 33 | uint64_t stamp_ns; 34 | 35 | Eigen::Affine3d pose; 36 | Eigen::Matrix covariance; 37 | 38 | friend std::ostream& operator<<(std::ostream& os, const PoseGraphEdge& edge); 39 | }; 40 | 41 | } // namespace pose_graph_tools 42 | -------------------------------------------------------------------------------- /pose_graph_tools_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(pose_graph_tools_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs 5 | geometry_msgs sensor_msgs) 6 | 7 | add_message_files( 8 | DIRECTORY 9 | msg 10 | FILES 11 | BowQueries.msg 12 | BowQuery.msg 13 | BowRequests.msg 14 | BowVector.msg 15 | LoopClosures.msg 16 | LoopClosuresAck.msg 17 | PoseGraph.msg 18 | PoseGraphEdge.msg 19 | PoseGraphNode.msg 20 | TimeRangeQuery.msg 21 | VLCFrameMsg.msg 22 | VLCFrames.msg 23 | VLCRequests.msg) 24 | 25 | add_service_files(DIRECTORY srv FILES LcdFrameRegistration.srv 26 | PoseGraphQuery.srv VLCFrameQuery.srv) 27 | 28 | generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs) 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS 32 | message_runtime 33 | std_msgs 34 | geometry_msgs 35 | sensor_msgs 36 | DEPENDS 37 | INCLUDE_DIRS 38 | LIBRARIES) 39 | 40 | install(DIRECTORY launch/ 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 42 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/include/pose_graph_tools_ros/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace pose_graph_tools { 10 | 11 | bool savePoseGraphEdgesToFile(const pose_graph_tools_msgs::PoseGraph &graph, 12 | const std::string &filename); 13 | 14 | // Filter duplicate edges in the input pose graph 15 | // Two edges are considered duplicate if they share the common key_from, key_to, 16 | // robot_from, robot_to 17 | pose_graph_tools_msgs::PoseGraph filterDuplicateEdges( 18 | const pose_graph_tools_msgs::PoseGraph &graph_in); 19 | 20 | // Buffers 21 | class PoseGraphStampCompare { 22 | public: 23 | bool operator()(pose_graph_tools_msgs::PoseGraphConstPtr x, 24 | pose_graph_tools_msgs::PoseGraphConstPtr y) { 25 | return x->header.stamp > y->header.stamp; 26 | } 27 | }; 28 | 29 | typedef std::priority_queue, 31 | PoseGraphStampCompare> 32 | StampedQueue; 33 | 34 | } // namespace pose_graph_tools 35 | -------------------------------------------------------------------------------- /LICENSE.BSD: -------------------------------------------------------------------------------- 1 | Copyright 2019 Massachusetts Institute of Technology. 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/include/pose_graph_tools_ros/visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class Visualizer { 16 | public: 17 | explicit Visualizer(const ros::NodeHandle& nh); 18 | 19 | void visualize(); 20 | 21 | typedef std::pair Node; // robot id, key 22 | typedef std::pair Edge; 23 | 24 | private: 25 | void PoseGraphCallback(const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg); 26 | 27 | geometry_msgs::Point getPositionFromKey(int robot_id, uint64_t key) const; 28 | 29 | private: 30 | std::string frame_id_; 31 | 32 | // subscribers 33 | ros::Subscriber pose_graph_sub_; 34 | 35 | // publishers 36 | ros::Publisher marker_array_pub_; 37 | 38 | std::vector odometry_edges_; 39 | std::vector loop_edges_; 40 | std::vector rejected_loop_edges_; 41 | std::map> keyed_poses_; 42 | 43 | std::shared_ptr 44 | interactive_mrkr_srvr_; 45 | }; 46 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/include/pose_graph_tools_ros/conversions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace pose_graph_tools { 8 | 9 | // Conversions for the pose_graph_tools C++ types to and from ROS messages. 10 | 11 | pose_graph_tools_msgs::BowVector toMsg(const BowVector& bow_vector); 12 | BowVector fromMsg(const pose_graph_tools_msgs::BowVector& bow_vector); 13 | 14 | pose_graph_tools_msgs::BowQuery toMsg(const BowQuery& bow_query); 15 | BowQuery fromMsg(const pose_graph_tools_msgs::BowQuery& bow_query); 16 | 17 | pose_graph_tools_msgs::BowQueries toMsg(const BowQueries& bow_queries); 18 | BowQueries fromMsg(const pose_graph_tools_msgs::BowQueries& bow_queries); 19 | 20 | pose_graph_tools_msgs::PoseGraphEdge toMsg(const PoseGraphEdge& pose_graph_edge); 21 | PoseGraphEdge fromMsg(const pose_graph_tools_msgs::PoseGraphEdge& pose_graph_edge); 22 | 23 | pose_graph_tools_msgs::PoseGraphNode toMsg(const PoseGraphNode& pose_graph_node); 24 | PoseGraphNode fromMsg(const pose_graph_tools_msgs::PoseGraphNode& pose_graph_node); 25 | 26 | pose_graph_tools_msgs::PoseGraph toMsg(const PoseGraph& pose_graph); 27 | PoseGraph fromMsg(const pose_graph_tools_msgs::PoseGraph& pose_graph); 28 | 29 | } // namespace pose_graph_tools 30 | -------------------------------------------------------------------------------- /pose_graph_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(pose_graph_tools VERSION 1.0.0) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_CXX_EXTENSIONS OFF) 7 | 8 | include(GNUInstallDirs) 9 | include(CMakePackageConfigHelpers) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | 13 | add_library(${PROJECT_NAME} src/ostream.cpp) 14 | target_include_directories( 15 | ${PROJECT_NAME} 16 | PUBLIC $ 17 | $) 18 | target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen) 19 | set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) 20 | add_library(pose_graph_tools::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 21 | 22 | install( 23 | TARGETS ${PROJECT_NAME} 24 | EXPORT pose_graph_tools-targets 25 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 26 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}) 27 | 28 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 29 | install( 30 | EXPORT pose_graph_tools-targets 31 | FILE pose_graph_toolsTargets.cmake 32 | NAMESPACE pose_graph_tools:: 33 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/pose_graph_tools) 34 | 35 | configure_package_config_file( 36 | ${CMAKE_CURRENT_LIST_DIR}/cmake/pose_graph_toolsConfig.cmake.in 37 | ${CMAKE_CURRENT_BINARY_DIR}/pose_graph_toolsConfig.cmake 38 | INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/pose_graph_tools) 39 | write_basic_package_version_file( 40 | pose_graph_toolsConfigVersion.cmake 41 | VERSION ${PACKAGE_VERSION} 42 | COMPATIBILITY AnyNewerVersion) 43 | 44 | install(FILES ${CMAKE_CURRENT_BINARY_DIR}/pose_graph_toolsConfig.cmake 45 | ${CMAKE_CURRENT_BINARY_DIR}/pose_graph_toolsConfigVersion.cmake 46 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/pose_graph_tools) 47 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(pose_graph_tools_ros) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | set(CMAKE_CXX_EXTENSIONS OFF) 7 | 8 | add_compile_options(-Wall -Wextra) 9 | 10 | find_package(pose_graph_tools REQUIRED) 11 | find_package( 12 | catkin REQUIRED 13 | COMPONENTS roscpp 14 | tf2_eigen 15 | pose_graph_tools_msgs 16 | std_msgs 17 | geometry_msgs 18 | visualization_msgs 19 | interactive_markers) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS 23 | roscpp 24 | tf2_eigen 25 | pose_graph_tools_msgs 26 | std_msgs 27 | geometry_msgs 28 | visualization_msgs 29 | interactive_markers 30 | DEPENDS 31 | pose_graph_tools 32 | INCLUDE_DIRS 33 | include 34 | LIBRARIES 35 | ${PROJECT_NAME}) 36 | 37 | add_library(${PROJECT_NAME} src/conversions.cpp src/visualizer.cpp 38 | src/utils.cpp) 39 | target_include_directories(${PROJECT_NAME} PUBLIC include 40 | ${catkin_INCLUDE_DIRS}) 41 | target_link_libraries(${PROJECT_NAME} PUBLIC pose_graph_tools::pose_graph_tools 42 | ${catkin_LIBRARIES}) 43 | 44 | add_executable(visualizer_node src/visualizer_node.cpp) 45 | target_link_libraries(visualizer_node ${PROJECT_NAME}) 46 | 47 | install( 48 | TARGETS ${PROJECT_NAME} visualizer_node 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 52 | 53 | install(DIRECTORY include/${PROJECT_NAME}/ 54 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 55 | 56 | install(DIRECTORY launch/ 57 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 58 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_graph_tools_ros/utils.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace pose_graph_tools { 8 | 9 | bool savePoseGraphEdgesToFile(const pose_graph_tools_msgs::PoseGraph &graph, 10 | const std::string &filename) { 11 | std::ofstream file; 12 | file.open(filename); 13 | if (!file.is_open()) { 14 | ROS_ERROR_STREAM("Error opening log file: " << filename); 15 | return false; 16 | } 17 | 18 | file << "robot_from,key_from,robot_to,key_to,qx,qy,qz,qw,tx,ty,tz\n"; 19 | for (size_t i = 0; i < graph.edges.size(); ++i) { 20 | pose_graph_tools_msgs::PoseGraphEdge edge = graph.edges[i]; 21 | geometry_msgs::Point position = edge.pose.position; 22 | geometry_msgs::Quaternion orientation = edge.pose.orientation; 23 | file << edge.robot_from << ","; 24 | file << edge.key_from << ","; 25 | file << edge.robot_to << ","; 26 | file << edge.key_to << ","; 27 | file << orientation.x << ","; 28 | file << orientation.y << ","; 29 | file << orientation.z << ","; 30 | file << orientation.w << ","; 31 | file << position.x << ","; 32 | file << position.y << ","; 33 | file << position.z << "\n"; 34 | } 35 | 36 | return true; 37 | } 38 | 39 | pose_graph_tools_msgs::PoseGraph filterDuplicateEdges( 40 | const pose_graph_tools_msgs::PoseGraph &graph_in) { 41 | pose_graph_tools_msgs::PoseGraph graph_out; 42 | 43 | graph_out.nodes = graph_in.nodes; 44 | 45 | for (size_t i = 0; i < graph_in.edges.size(); ++i) { 46 | pose_graph_tools_msgs::PoseGraphEdge edge_in = graph_in.edges[i]; 47 | bool skip = false; 48 | for (size_t j = 0; j < graph_out.edges.size(); ++j) { 49 | pose_graph_tools_msgs::PoseGraphEdge edge = graph_out.edges[j]; 50 | if (edge.robot_from == edge_in.robot_from && edge.robot_to == edge_in.robot_to && 51 | edge.key_from == edge_in.key_from && edge.key_to == edge_in.key_to) { 52 | skip = true; 53 | break; 54 | } 55 | } 56 | if (!skip) { 57 | graph_out.edges.push_back(edge_in); 58 | } 59 | } 60 | 61 | unsigned int num_edges_in = graph_in.edges.size(); 62 | unsigned int num_edges_out = graph_out.edges.size(); 63 | printf("Detected and removed %u duplicate edges from pose graph.\n", 64 | num_edges_in - num_edges_out); 65 | 66 | return graph_out; 67 | } 68 | 69 | } // namespace pose_graph_tools 70 | -------------------------------------------------------------------------------- /pose_graph_tools/src/ostream.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_graph_tools/all.h" 2 | 3 | namespace pose_graph_tools { 4 | 5 | // TODO(lschmid): If important consider better pretty printing with linebreaks, 6 | // naming the enums etc. 7 | 8 | std::ostream& operator<<(std::ostream& os, const BowVector& bow_vector) { 9 | os << "BowVector{word_ids=["; 10 | for (const auto& word_id : bow_vector.word_ids) { 11 | os << word_id << ", "; 12 | } 13 | os << "], word_values=["; 14 | for (const auto& word_value : bow_vector.word_values) { 15 | os << word_value << ", "; 16 | } 17 | os << "]}"; 18 | return os; 19 | } 20 | 21 | std::ostream& operator<<(std::ostream& os, const BowQuery& query) { 22 | os << "BowQuery{robot_id=" << query.robot_id << ", pose_id=" << query.pose_id 23 | << ", bow_vector=" << query.bow_vector << "}"; 24 | return os; 25 | } 26 | 27 | std::ostream& operator<<(std::ostream& os, const BowQueries& queries) { 28 | os << "BowQueries{destination_robot_id=" << queries.destination_robot_id 29 | << ", queries=["; 30 | for (const auto& query : queries.queries) { 31 | os << query << ", "; 32 | } 33 | os << "]}"; 34 | return os; 35 | } 36 | 37 | std::ostream& operator<<(std::ostream& os, const PoseGraphNode& node) { 38 | os << "PoseGraphNode{stamp_ns=" << node.stamp_ns 39 | << ", robot_id=" << node.robot_id << ", key=" << node.key 40 | << ", translation=[" << node.pose.translation().transpose() 41 | << "], rotation=[" 42 | << Eigen::Quaterniond(node.pose.rotation()).coeffs().transpose() << "]}"; 43 | return os; 44 | } 45 | 46 | std::ostream& operator<<(std::ostream& os, const PoseGraphEdge& edge) { 47 | os << "PoseGraphEdge{type=" << edge.type << ", key_from=" << edge.key_from 48 | << ", key_to=" << edge.key_to << ", robot_from=" << edge.robot_from 49 | << ", robot_to=" << edge.robot_to << ", stamp_ns=" << edge.stamp_ns 50 | << ", translation=[" << edge.pose.translation().transpose() 51 | << "], rotation=[" 52 | << Eigen::Quaterniond(edge.pose.rotation()).coeffs().transpose() << "]}"; 53 | return os; 54 | } 55 | 56 | std::ostream& operator<<(std::ostream& os, const PoseGraph& graph) { 57 | os << "PoseGraph{stamp_ns=" << graph.stamp_ns << ", nodes=["; 58 | for (const auto& node : graph.nodes) { 59 | os << node << ", "; 60 | } 61 | os << "], edges=["; 62 | for (const auto& edge : graph.edges) { 63 | os << edge << ", "; 64 | } 65 | os << "]}"; 66 | return os; 67 | } 68 | 69 | } // namespace pose_graph_tools 70 | -------------------------------------------------------------------------------- /.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 | CommentPragmas: '^ IWYU pragma:' 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: true 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | # Spacers 52 | - Regex: '^$' 53 | Priority: 15 54 | - Regex: '^$' 55 | Priority: 25 56 | - Regex: '^$' 57 | Priority: 35 58 | - Regex: '^$' 59 | Priority: 45 60 | # C system headers 61 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 62 | Priority: 10 63 | # C++ system headers 64 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 65 | Priority: 20 66 | # Other library h files (with angles) 67 | - Regex: '^<' 68 | Priority: 30 69 | # Your project's h files (with angles) 70 | - Regex: '^ 4 | 5 | namespace pose_graph_tools { 6 | 7 | pose_graph_tools_msgs::BowVector toMsg(const BowVector& bow_vector) { 8 | pose_graph_tools_msgs::BowVector result; 9 | result.word_ids = bow_vector.word_ids; 10 | result.word_values = bow_vector.word_values; 11 | return result; 12 | } 13 | 14 | BowVector fromMsg(const pose_graph_tools_msgs::BowVector& bow_vector) { 15 | BowVector result; 16 | result.word_ids = bow_vector.word_ids; 17 | result.word_values = bow_vector.word_values; 18 | return result; 19 | } 20 | 21 | pose_graph_tools_msgs::BowQuery toMsg(const BowQuery& bow_query) { 22 | pose_graph_tools_msgs::BowQuery result; 23 | result.robot_id = bow_query.robot_id; 24 | result.pose_id = bow_query.pose_id; 25 | result.bow_vector = toMsg(bow_query.bow_vector); 26 | return result; 27 | } 28 | 29 | BowQuery fromMsg(const pose_graph_tools_msgs::BowQuery& bow_query) { 30 | BowQuery result; 31 | result.robot_id = bow_query.robot_id; 32 | result.pose_id = bow_query.pose_id; 33 | result.bow_vector = fromMsg(bow_query.bow_vector); 34 | return result; 35 | } 36 | 37 | pose_graph_tools_msgs::BowQueries toMsg(const BowQueries& bow_queries) { 38 | pose_graph_tools_msgs::BowQueries result; 39 | result.destination_robot_id = bow_queries.destination_robot_id; 40 | result.queries.reserve(bow_queries.queries.size()); 41 | for (const auto& query : bow_queries.queries) { 42 | result.queries.emplace_back(toMsg(query)); 43 | } 44 | return result; 45 | } 46 | 47 | BowQueries fromMsg(const pose_graph_tools_msgs::BowQueries& bow_queries) { 48 | BowQueries result; 49 | result.destination_robot_id = bow_queries.destination_robot_id; 50 | result.queries.reserve(bow_queries.queries.size()); 51 | for (const auto& query : bow_queries.queries) { 52 | result.queries.emplace_back(fromMsg(query)); 53 | } 54 | return result; 55 | } 56 | 57 | pose_graph_tools_msgs::PoseGraphEdge toMsg( 58 | const PoseGraphEdge& pose_graph_edge) { 59 | pose_graph_tools_msgs::PoseGraphEdge result; 60 | result.key_from = pose_graph_edge.key_from; 61 | result.key_to = pose_graph_edge.key_to; 62 | result.robot_from = pose_graph_edge.robot_from; 63 | result.robot_to = pose_graph_edge.robot_to; 64 | result.type = static_cast(pose_graph_edge.type); 65 | result.header.stamp.fromNSec(pose_graph_edge.stamp_ns); 66 | tf2::convert(pose_graph_edge.pose, result.pose); 67 | 68 | // Store covariance in row-major order. 69 | for (size_t r = 0; r < 6; ++r) { 70 | for (size_t c = 0; c < 6; ++c) { 71 | result.covariance[r * 6 + c] = pose_graph_edge.covariance(r, c); 72 | } 73 | } 74 | return result; 75 | } 76 | 77 | PoseGraphEdge fromMsg( 78 | const pose_graph_tools_msgs::PoseGraphEdge& pose_graph_edge) { 79 | PoseGraphEdge result; 80 | result.key_from = pose_graph_edge.key_from; 81 | result.key_to = pose_graph_edge.key_to; 82 | result.robot_from = pose_graph_edge.robot_from; 83 | result.robot_to = pose_graph_edge.robot_to; 84 | result.stamp_ns = pose_graph_edge.header.stamp.toNSec(); 85 | result.type = static_cast(pose_graph_edge.type); 86 | tf2::convert(pose_graph_edge.pose, result.pose); 87 | 88 | // Store covariance in row-major order. 89 | for (size_t r = 0; r < 6; ++r) { 90 | for (size_t c = 0; c < 6; ++c) { 91 | result.covariance(r, c) = pose_graph_edge.covariance[r * 6 + c]; 92 | } 93 | } 94 | return result; 95 | } 96 | 97 | pose_graph_tools_msgs::PoseGraphNode toMsg( 98 | const PoseGraphNode& pose_graph_node) { 99 | pose_graph_tools_msgs::PoseGraphNode result; 100 | result.header.stamp.fromNSec(pose_graph_node.stamp_ns); 101 | result.key = pose_graph_node.key; 102 | result.robot_id = pose_graph_node.robot_id; 103 | tf2::convert(pose_graph_node.pose, result.pose); 104 | 105 | return result; 106 | } 107 | 108 | PoseGraphNode fromMsg( 109 | const pose_graph_tools_msgs::PoseGraphNode& pose_graph_node) { 110 | PoseGraphNode result; 111 | result.stamp_ns = pose_graph_node.header.stamp.toNSec(); 112 | result.key = pose_graph_node.key; 113 | result.robot_id = pose_graph_node.robot_id; 114 | tf2::convert(pose_graph_node.pose, result.pose); 115 | return result; 116 | } 117 | 118 | pose_graph_tools_msgs::PoseGraph toMsg(const PoseGraph& pose_graph) { 119 | pose_graph_tools_msgs::PoseGraph result; 120 | result.header.stamp.fromNSec(pose_graph.stamp_ns); 121 | result.nodes.reserve(pose_graph.nodes.size()); 122 | for (const auto& node : pose_graph.nodes) { 123 | result.nodes.emplace_back(toMsg(node)); 124 | } 125 | result.edges.reserve(pose_graph.edges.size()); 126 | for (const auto& edge : pose_graph.edges) { 127 | result.edges.emplace_back(toMsg(edge)); 128 | } 129 | return result; 130 | } 131 | 132 | PoseGraph fromMsg(const pose_graph_tools_msgs::PoseGraph& pose_graph) { 133 | PoseGraph result; 134 | result.stamp_ns = pose_graph.header.stamp.toNSec(); 135 | result.nodes.reserve(pose_graph.nodes.size()); 136 | for (const auto& node : pose_graph.nodes) { 137 | result.nodes.emplace_back(fromMsg(node)); 138 | } 139 | result.edges.reserve(pose_graph.edges.size()); 140 | for (const auto& edge : pose_graph.edges) { 141 | result.edges.emplace_back(fromMsg(edge)); 142 | } 143 | return result; 144 | } 145 | 146 | } // namespace pose_graph_tools 147 | -------------------------------------------------------------------------------- /pose_graph_tools_ros/src/visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_graph_tools_ros/visualizer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "geometry_msgs/Quaternion.h" 8 | #include "interactive_markers/interactive_marker_server.h" 9 | 10 | Visualizer::Visualizer(const ros::NodeHandle& nh) { 11 | ROS_INFO("Initializing pose graph visualizer"); 12 | 13 | // start subscribers 14 | ros::NodeHandle nl(nh); 15 | pose_graph_sub_ = nl.subscribe( 16 | "graph", 10, &Visualizer::PoseGraphCallback, this); 17 | 18 | marker_array_pub_ = nl.advertise( 19 | "pose_graph_markers", 10, false); 20 | 21 | interactive_mrkr_srvr_ = 22 | std::make_shared( 23 | "interactive_node", "", false); 24 | 25 | ros::spin(); 26 | } 27 | 28 | void Visualizer::PoseGraphCallback( 29 | const pose_graph_tools_msgs::PoseGraph::ConstPtr& msg) { 30 | // iterate through nodes in pose graph 31 | for (const auto& msg_node : msg->nodes) { 32 | // Fill pose nodes (representing the robot position) 33 | keyed_poses_[msg_node.robot_id][msg_node.key] = msg_node.pose; 34 | } 35 | 36 | // update frame id 37 | frame_id_ = msg->header.frame_id; 38 | 39 | odometry_edges_.clear(); 40 | loop_edges_.clear(); 41 | rejected_loop_edges_.clear(); 42 | // iterate through edges in pose graph 43 | for (const auto& msg_edge : msg->edges) { 44 | Node from = std::make_pair(msg_edge.robot_from, msg_edge.key_from); 45 | Node to = std::make_pair(msg_edge.robot_to, msg_edge.key_to); 46 | if (msg_edge.type == pose_graph_tools_msgs::PoseGraphEdge::ODOM) { 47 | // initialize first seen robot id 48 | odometry_edges_.emplace_back(std::make_pair(from, to)); 49 | } else if (msg_edge.type == 50 | pose_graph_tools_msgs::PoseGraphEdge::LOOPCLOSE) { 51 | loop_edges_.emplace_back(std::make_pair(from, to)); 52 | } else if (msg_edge.type == 53 | pose_graph_tools_msgs::PoseGraphEdge::REJECTED_LOOPCLOSE) { 54 | rejected_loop_edges_.emplace_back(std::make_pair(from, to)); 55 | } 56 | } 57 | 58 | visualize(); 59 | } 60 | 61 | geometry_msgs::Point positionFromKey( 62 | std::map> keyed_poses, 63 | int robot_id, 64 | uint64_t key) { 65 | return keyed_poses.at(robot_id).at(key).position; 66 | } 67 | 68 | geometry_msgs::Quaternion orientationFromKey( 69 | std::map> keyed_poses, 70 | int robot_id, 71 | uint64_t key) { 72 | return keyed_poses.at(robot_id).at(key).orientation; 73 | } 74 | 75 | geometry_msgs::Point Visualizer::getPositionFromKey(int robot_id, 76 | uint64_t key) const { 77 | return keyed_poses_.at(robot_id).at(key).position; 78 | } 79 | 80 | // Interactive Marker Menu to click and see key of node 81 | void MakeMenuMarker( 82 | const std::string frame_id, 83 | std::shared_ptr 84 | interactive_mrkr_srvr, 85 | const geometry_msgs::Pose& position, 86 | const std::string& id_number) { 87 | interactive_markers::MenuHandler menu_handler; 88 | 89 | visualization_msgs::InteractiveMarker int_marker; 90 | int_marker.header.frame_id = frame_id; 91 | int_marker.scale = 1.0; 92 | int_marker.pose = position; 93 | int_marker.name = id_number; 94 | 95 | visualization_msgs::Marker marker; 96 | marker.type = visualization_msgs::Marker::SPHERE; 97 | marker.scale.x = 0.3; 98 | marker.scale.y = 0.3; 99 | marker.scale.z = 0.3; 100 | marker.color.r = 0.0; 101 | marker.color.g = 1.0; 102 | marker.color.b = 1.0; 103 | marker.color.a = 0.5; 104 | marker.pose.orientation.w = 1.0; 105 | 106 | visualization_msgs::InteractiveMarkerControl control; 107 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU; 108 | control.name = id_number; 109 | control.markers.push_back(marker); 110 | control.always_visible = true; 111 | int_marker.controls.push_back(control); 112 | 113 | menu_handler.insert(id_number); 114 | interactive_mrkr_srvr->insert(int_marker); 115 | menu_handler.apply(*interactive_mrkr_srvr, int_marker.name); 116 | } 117 | 118 | visualization_msgs::Marker build_odom_marker( 119 | std::string frame_id, 120 | std::vector odom_edges, 121 | std::map> keyed_poses) { 122 | visualization_msgs::Marker m; 123 | m.header.frame_id = frame_id; 124 | m.ns = "odom_edges"; 125 | m.id = 0; 126 | m.action = visualization_msgs::Marker::ADD; 127 | m.type = visualization_msgs::Marker::LINE_LIST; 128 | for (size_t ii = 0; ii < odom_edges.size(); ++ii) { 129 | int robot_id = odom_edges[ii].first.first; 130 | const auto key1 = odom_edges[ii].first.second; 131 | const auto key2 = odom_edges[ii].second.second; 132 | 133 | // TODO(Yun) currently the below color formula 134 | // means that only support up to 5 robots 135 | std_msgs::ColorRGBA color; 136 | color.r = static_cast(robot_id) / 5; 137 | color.g = 1 - static_cast(robot_id) / 5; 138 | color.b = 0.0; 139 | color.a = 0.8; 140 | m.scale.x = 0.02; 141 | m.pose.orientation.w = 1.0; 142 | 143 | m.points.push_back(positionFromKey(keyed_poses, robot_id, key1)); 144 | m.points.push_back(positionFromKey(keyed_poses, robot_id, key2)); 145 | m.colors.push_back(color); 146 | m.colors.push_back(color); 147 | } 148 | return m; 149 | } 150 | 151 | std::vector build_heading_markers( 152 | std::string frame_id, 153 | std::vector odom_edges, 154 | std::map> keyed_poses) { 155 | std::vector arrows; 156 | 157 | int id = 0; 158 | for (size_t ii = 0; ii < odom_edges.size(); ++ii) { 159 | visualization_msgs::Marker m; 160 | m.header.frame_id = frame_id; 161 | m.ns = "heading"; 162 | m.id = id++; 163 | m.action = visualization_msgs::Marker::ADD; 164 | m.type = visualization_msgs::Marker::ARROW; 165 | int robot_id = odom_edges[ii].first.first; 166 | const auto key = odom_edges[ii].first.second; 167 | 168 | // TODO(Yun) currently the below color formula 169 | // means that only support up to 5 robots 170 | std_msgs::ColorRGBA color; 171 | m.color.r = 1 - static_cast(robot_id) / 5; 172 | m.color.g = static_cast(robot_id) / 5; 173 | m.color.b = 0.0; 174 | m.color.a = 0.8; 175 | m.scale.x = 1; 176 | m.scale.y = 0.1; 177 | m.scale.z = 0.1; 178 | m.pose.orientation.w = 1.0; 179 | 180 | auto pos = positionFromKey(keyed_poses, robot_id, key); 181 | auto quat = orientationFromKey(keyed_poses, robot_id, key); 182 | m.pose.position = pos; 183 | m.pose.orientation = quat; 184 | arrows.push_back(m); 185 | } 186 | return arrows; 187 | } 188 | 189 | visualization_msgs::Marker build_lc_marker( 190 | std::string frame_id, 191 | std::vector loop_edges, 192 | std::map> keyed_poses) { 193 | // Publish loop closure edges. 194 | visualization_msgs::Marker m; 195 | m.header.frame_id = frame_id; 196 | m.ns = "LoopClosures"; 197 | m.id = 1; 198 | m.action = visualization_msgs::Marker::ADD; 199 | m.type = visualization_msgs::Marker::LINE_LIST; 200 | m.color.r = 0.0; 201 | m.color.g = 0.2; 202 | m.color.b = 1.0; 203 | m.color.a = 0.8; 204 | m.scale.x = 0.02; 205 | m.pose.orientation.w = 1.0; 206 | 207 | for (size_t ii = 0; ii < loop_edges.size(); ++ii) { 208 | const auto robot1 = loop_edges[ii].first.first; 209 | const auto robot2 = loop_edges[ii].second.first; 210 | const auto key1 = loop_edges[ii].first.second; 211 | const auto key2 = loop_edges[ii].second.second; 212 | 213 | m.points.push_back(positionFromKey(keyed_poses, robot1, key1)); 214 | m.points.push_back(positionFromKey(keyed_poses, robot2, key2)); 215 | } 216 | return m; 217 | } 218 | 219 | visualization_msgs::Marker build_rejected_lc_marker( 220 | std::string frame_id, 221 | std::vector loop_edges, 222 | std::map> keyed_poses) { 223 | // Publish loop closure edges. 224 | // Publish the rejected loop closure edges 225 | visualization_msgs::Marker m; 226 | m.header.frame_id = frame_id; 227 | m.ns = "RejectedLoopClosures"; 228 | m.id = 1; 229 | m.action = visualization_msgs::Marker::ADD; 230 | m.type = visualization_msgs::Marker::LINE_LIST; 231 | m.color.r = 0.5; 232 | m.color.g = 0.5; 233 | m.color.b = 0.5; 234 | m.color.a = 0.7; 235 | m.scale.x = 0.02; 236 | m.pose.orientation.w = 1.0; 237 | 238 | for (size_t ii = 0; ii < loop_edges.size(); ++ii) { 239 | const auto robot1 = loop_edges[ii].first.first; 240 | const auto robot2 = loop_edges[ii].second.first; 241 | const auto key1 = loop_edges[ii].first.second; 242 | const auto key2 = loop_edges[ii].second.second; 243 | 244 | m.points.push_back(positionFromKey(keyed_poses, robot1, key1)); 245 | m.points.push_back(positionFromKey(keyed_poses, robot2, key2)); 246 | } 247 | return m; 248 | } 249 | 250 | std::vector build_node_ids( 251 | std::string frame_id, 252 | std::map> keyed_poses) { 253 | // Publish loop closure edges. 254 | // Publish node IDs in the pose graph. 255 | 256 | std::vector m_ids; 257 | int id_base = 100; 258 | for (const auto& robot : keyed_poses) { 259 | for (const auto& keyedPose : robot.second) { 260 | visualization_msgs::Marker m; 261 | m.header.frame_id = frame_id; 262 | m.ns = "NodeIds"; 263 | m.action = visualization_msgs::Marker::ADD; 264 | m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 265 | m.color.r = 1.0; 266 | m.color.g = 1.0; 267 | m.color.b = 0.2; 268 | m.color.a = 0.8; 269 | m.scale.z = 270 | 0.01; // Only Scale z is used - height of capital A in the text 271 | m.pose.orientation.w = 1.0; 272 | 273 | m.pose = keyedPose.second; 274 | // Display text for the node 275 | std::string robot_id = std::to_string(keyedPose.first); 276 | m.text = robot_id; 277 | m.id = id_base + keyedPose.first; 278 | m_ids.push_back(m); 279 | } 280 | } 281 | return m_ids; 282 | } 283 | 284 | visualization_msgs::Marker build_keyframes( 285 | std::string frame_id, 286 | std::map> keyed_poses) { 287 | // Publish keyframe nodes in the pose graph. 288 | visualization_msgs::Marker m; 289 | m.header.frame_id = frame_id; 290 | m.ns = "keyframes"; 291 | m.id = 0; 292 | m.action = visualization_msgs::Marker::ADD; 293 | m.type = visualization_msgs::Marker::SPHERE_LIST; 294 | m.color.r = 0.0; 295 | m.color.g = 1.0; 296 | m.color.b = 0.3; 297 | m.color.a = 0.8; 298 | m.scale.x = 0.05; 299 | m.scale.y = 0.05; 300 | m.scale.z = 0.05; 301 | m.pose.orientation.w = 1.0; 302 | 303 | for (const auto& robot : keyed_poses) { 304 | for (const auto& keyedPose : robot.second) { 305 | m.points.push_back( 306 | positionFromKey(keyed_poses, robot.first, keyedPose.first)); 307 | } 308 | } 309 | return m; 310 | } 311 | 312 | void Visualizer::visualize() { 313 | visualization_msgs::MarkerArray ma; 314 | if (marker_array_pub_.getNumSubscribers() <= 0) { 315 | return; 316 | } 317 | 318 | // Odometry Edges 319 | visualization_msgs::Marker m_odom = 320 | build_odom_marker(frame_id_, odometry_edges_, keyed_poses_); 321 | ma.markers.push_back(m_odom); 322 | 323 | // Heading (orientation) Edges 324 | std::vector m_headings = 325 | build_heading_markers(frame_id_, odometry_edges_, keyed_poses_); 326 | ma.markers.insert(ma.markers.end(), m_headings.begin(), m_headings.end()); 327 | 328 | // Loop Closures 329 | visualization_msgs::Marker m_lc = 330 | build_lc_marker(frame_id_, loop_edges_, keyed_poses_); 331 | ma.markers.push_back(m_lc); 332 | 333 | // Rejected Loop Closures 334 | visualization_msgs::Marker m_rejected_lc = 335 | build_rejected_lc_marker(frame_id_, rejected_loop_edges_, keyed_poses_); 336 | ma.markers.push_back(m_rejected_lc); 337 | 338 | // Node IDs 339 | std::vector m_node_ids = 340 | build_node_ids(frame_id_, keyed_poses_); 341 | ma.markers.insert(ma.markers.end(), m_node_ids.begin(), m_node_ids.end()); 342 | 343 | // Keyframes 344 | visualization_msgs::Marker m_keyframes = 345 | build_keyframes(frame_id_, keyed_poses_); 346 | ma.markers.push_back(m_keyframes); 347 | 348 | // Publish it all! 349 | marker_array_pub_.publish(ma); 350 | 351 | // Interactive Markers 352 | for (const auto& robot : keyed_poses_) { 353 | for (const auto& keyedPose : robot.second) { 354 | // Display text for the node 355 | std::string robot_id = std::to_string(keyedPose.first); 356 | MakeMenuMarker( 357 | frame_id_, interactive_mrkr_srvr_, keyedPose.second, robot_id); 358 | } 359 | } 360 | 361 | if (interactive_mrkr_srvr_ != nullptr) { 362 | interactive_mrkr_srvr_->applyChanges(); 363 | } 364 | } 365 | --------------------------------------------------------------------------------