├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── colcon.meta ├── config └── cslam │ ├── README.md │ └── example.yaml ├── cslam ├── __init__.py ├── algebraic_connectivity_maximization.py ├── broker.py ├── global_descriptor_loop_closure_detection.py ├── lidar_handler_node.py ├── lidar_pr │ ├── icp_utils.py │ ├── scancontext.py │ ├── scancontext_matching.py │ └── scancontext_utils.py ├── loop_closure_detection_node.py ├── loop_closure_sparse_matching.py ├── mac │ ├── .gitignore │ ├── LICENSE │ ├── README.md │ ├── __init__.py │ ├── mac.py │ ├── requirements.txt │ └── utils.py ├── neighbor_monitor.py ├── neighbors_manager.py ├── nns_matching.py ├── utils │ ├── misc.py │ └── point_cloud2.py └── vpr │ ├── cosplace.py │ ├── cosplace_utils │ ├── README.md │ ├── layers.py │ └── network.py │ └── netvlad.py ├── include └── cslam │ ├── back_end │ ├── decentralized_pgo.h │ ├── gtsam_utils.h │ └── utils │ │ ├── logger.h │ │ └── simulated_rendezvous.h │ └── front_end │ ├── map_manager.h │ ├── rgbd_handler.h │ ├── sensor_handler_interface.h │ ├── stereo_handler.h │ ├── utils │ └── depth_traits.h │ └── visualization_utils.h ├── models ├── .gitignore └── download.sh ├── package.xml ├── spec-file.txt ├── src ├── back_end │ ├── decentralized_pgo.cpp │ ├── gtsam_utils.cpp │ ├── pose_graph_manager_node.cpp │ └── utils │ │ ├── logger.cpp │ │ └── simulated_rendezvous.cpp └── front_end │ ├── map_manager.cpp │ ├── map_manager_node.cpp │ ├── rgbd_handler.cpp │ ├── stereo_handler.cpp │ └── visualization_utils.cpp └── tests ├── README.md ├── test_algebraic_connectivity.py ├── test_broker.py └── test_sparse_matching.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | .markdownlint.yaml 4 | .flake8 5 | .isort.cfg 6 | .trunk/ 7 | *__pycache__/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cslam) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic -g) 16 | endif() 17 | 18 | IF("$ENV{ROS_DISTRO}" STRLESS "iron") 19 | add_definitions(-DPRE_ROS_IRON) 20 | ENDIF() 21 | 22 | # find dependencies 23 | find_package(ament_cmake REQUIRED) 24 | find_package(builtin_interfaces REQUIRED) 25 | find_package(rosidl_default_generators REQUIRED) 26 | find_package(rclcpp REQUIRED) 27 | find_package(rclcpp_components REQUIRED) 28 | find_package(cv_bridge REQUIRED) 29 | find_package(sensor_msgs REQUIRED) 30 | find_package(std_msgs REQUIRED) 31 | find_package(std_srvs REQUIRED) 32 | find_package(nav_msgs REQUIRED) 33 | find_package(nav2_msgs REQUIRED) 34 | find_package(stereo_msgs REQUIRED) 35 | find_package(geometry_msgs REQUIRED) 36 | find_package(visualization_msgs REQUIRED) 37 | find_package(diagnostic_msgs REQUIRED) 38 | find_package(image_transport REQUIRED) 39 | find_package(tf2 REQUIRED) 40 | find_package(tf2_eigen REQUIRED) 41 | find_package(tf2_ros REQUIRED) 42 | find_package(tf2_geometry_msgs REQUIRED) 43 | find_package(pcl_ros REQUIRED) 44 | find_package(pcl_msgs REQUIRED) 45 | find_package(perception_pcl REQUIRED) 46 | #find_package(eigen_conversions REQUIRED) 47 | find_package(laser_geometry REQUIRED) 48 | find_package(pcl_conversions REQUIRED) 49 | #find_package(pcl_ros REQUIRED) 50 | #find_package(dynamic_reconfigure REQUIRED) 51 | find_package(message_filters REQUIRED) 52 | find_package(class_loader REQUIRED) 53 | #find_package(rosgraph_msgs REQUIRED) 54 | find_package(image_geometry REQUIRED) 55 | #find_package(pluginlib REQUIRED) 56 | # find_package(rtabmap_ros REQUIRED) 57 | find_package(rtabmap_msgs REQUIRED) 58 | find_package(rtabmap_conversions REQUIRED) 59 | 60 | # Optional components 61 | #find_package(costmap_2d) 62 | #find_package(octomap_msgs) 63 | #find_package(apriltag_msgs) 64 | #find_package(find_object_2d) 65 | #find_package(fiducial_msgs) 66 | 67 | find_package(RTABMap 0.20.18 REQUIRED) 68 | 69 | # Find GTSAM 70 | find_package(GTSAM CONFIG REQUIRED) 71 | 72 | # kinetic issue, rtabmap now requires at least c++11 73 | include(CheckCXXCompilerFlag) 74 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 75 | set(CMAKE_CXX_STANDARD 14) 76 | 77 | find_package(PCL 1.7 78 | REQUIRED QUIET COMPONENTS 79 | common 80 | io 81 | kdtree 82 | search 83 | surface 84 | filters 85 | registration 86 | sample_consensus 87 | segmentation 88 | ) #This crashes idl generation if all components are found?! see https://github.com/ros2/rosidl/issues/402#issuecomment-565586908 89 | 90 | ########### 91 | ## Build ## 92 | ########### 93 | 94 | ## Specify additional locations of header files 95 | ## Your package locations should be listed before other locations 96 | # include_directories(include) 97 | include_directories( 98 | ${CMAKE_CURRENT_SOURCE_DIR}/include 99 | ${RTABMap_INCLUDE_DIRS} 100 | # ${rtabmap_ros_INCLUDE_DIRS} 101 | ${rtabmap_msgs_INCLUDE_DIRS} 102 | ${rtabmap_conversions_INCLUDE_DIRS} 103 | ${PCL_INCLUDE_DIRS} 104 | ${GTSAM_INCLUDE_DIR} 105 | ) 106 | 107 | # libraries 108 | SET(Libraries 109 | pcl_conversions 110 | cv_bridge 111 | rclcpp 112 | rclcpp_components 113 | sensor_msgs 114 | std_msgs 115 | nav_msgs 116 | nav2_msgs 117 | geometry_msgs 118 | image_transport 119 | tf2 120 | tf2_eigen 121 | tf2_ros 122 | laser_geometry 123 | message_filters 124 | class_loader 125 | visualization_msgs 126 | image_geometry 127 | stereo_msgs 128 | diagnostic_msgs 129 | tf2_geometry_msgs 130 | pcl_ros 131 | pcl_msgs 132 | perception_pcl 133 | RTABMap 134 | # rtabmap_ros 135 | rtabmap_msgs 136 | rtabmap_conversions 137 | ) 138 | 139 | find_package(cslam_common_interfaces REQUIRED) 140 | find_package(cslam_common_interfaces REQUIRED) 141 | 142 | add_executable(map_manager 143 | src/front_end/map_manager.cpp 144 | src/front_end/map_manager_node.cpp 145 | src/front_end/stereo_handler.cpp 146 | src/front_end/rgbd_handler.cpp 147 | #src/front_end/sensor_msg_utils.cpp 148 | src/front_end/visualization_utils.cpp) 149 | add_dependencies(map_manager 150 | map_manager 151 | ${${PROJECT_NAME}_EXPORTED_TARGETS}) 152 | ament_target_dependencies(map_manager 153 | ${Libraries} 154 | cslam_common_interfaces cslam_common_interfaces) 155 | set_target_properties(map_manager 156 | PROPERTIES OUTPUT_NAME "map_manager") 157 | 158 | add_executable(pose_graph_manager 159 | src/back_end/decentralized_pgo.cpp 160 | src/back_end/pose_graph_manager_node.cpp 161 | src/back_end/utils/logger.cpp 162 | src/back_end/gtsam_utils.cpp 163 | src/back_end/utils/simulated_rendezvous.cpp) 164 | add_dependencies(pose_graph_manager 165 | pose_graph_manager 166 | ${${PROJECT_NAME}_EXPORTED_TARGETS}) 167 | ament_target_dependencies(pose_graph_manager 168 | ${Libraries} 169 | cslam_common_interfaces cslam_common_interfaces) 170 | target_link_libraries(pose_graph_manager gtsam) 171 | set_target_properties(pose_graph_manager 172 | PROPERTIES OUTPUT_NAME "pose_graph_manager") 173 | 174 | # Only required when using messages built from the same package 175 | # https://index.ros.org/doc/ros2/Tutorials/Rosidl-Tutorial/ 176 | get_default_rmw_implementation(rmw_implementation) 177 | find_package("${rmw_implementation}" REQUIRED) 178 | get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp") 179 | 180 | ## Mark cpp header files for installation 181 | install(DIRECTORY include/${PROJECT_NAME}/ 182 | DESTINATION include/${PROJECT_NAME} 183 | FILES_MATCHING PATTERN "*.h" 184 | PATTERN ".svn" EXCLUDE 185 | ) 186 | 187 | install(DIRECTORY 188 | models 189 | DESTINATION share/${PROJECT_NAME}/ 190 | ) 191 | 192 | # Install nodes 193 | install( 194 | TARGETS map_manager pose_graph_manager 195 | DESTINATION lib/${PROJECT_NAME} 196 | ) 197 | 198 | # Install Python modules 199 | ament_python_install_package(${PROJECT_NAME}) 200 | 201 | install(PROGRAMS 202 | cslam/loop_closure_detection_node.py 203 | cslam/lidar_handler_node.py 204 | DESTINATION lib/${PROJECT_NAME}) 205 | 206 | # Add tests 207 | find_package(ament_cmake_pytest REQUIRED) 208 | set(_cslam_python_tests 209 | tests/test_algebraic_connectivity.py 210 | tests/test_sparse_matching.py 211 | tests/test_broker.py 212 | ) 213 | foreach(_test_path ${_cslam_python_tests}) 214 | get_filename_component(_test_name ${_test_path} NAME_WE) 215 | ament_add_pytest_test(${_test_name} ${_test_path} 216 | APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} 217 | TIMEOUT 120 218 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} 219 | ) 220 | endforeach() 221 | 222 | ament_package() -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Pierre-Yves Lajoie 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | For details, look up main [Swarm-SLAM repo](https://github.com/MISTLab/Swarm-SLAM) and [documentation](https://lajoiepy.github.io/cslam_documentation/html/index.html). 2 | -------------------------------------------------------------------------------- /colcon.meta: -------------------------------------------------------------------------------- 1 | { 2 | "build": 3 | { 4 | "symlink-install": true, 5 | "cmake-args": [ 6 | "-DCMAKE_BUILD_TYPE=RelWithDebInfo", 7 | ] 8 | } 9 | } 10 | -------------------------------------------------------------------------------- /config/cslam/README.md: -------------------------------------------------------------------------------- 1 | ## C-SLAM Parameters 2 | 3 | `left_image_topic, right_image_topic, left_camera_info_topic, right_camera_info_topic, etc.`: ROS 2 topic names on which the sensor publish its data and calibration. 4 | 5 | `odom_topic`: Topic name for input odometry estimates 6 | 7 | `sensor_base_frame_id`: ROS 2 TF base frame of the sensor 8 | 9 | ### Front-End: 10 | 11 | `inter_robot_loop_closure_budget`: Maximum number of loop closures that can be geometrically verified every detection periods. 12 | 13 | `inter_robot_detection_period_sec`: Period (in seconds) for inter-robot loop closure detection. 14 | 15 | `max_queue_size`: Maximum number of keyframe to store in buffer for processing. 16 | 17 | `similarity_threshold`: Minimum similarity value required to consider a potential match. 18 | 19 | `global_descriptors_topic`: Topic name for global descriptors 20 | 21 | `global_descriptor_technique`: Global descriptors computation technique (e.g., netvlad) 22 | 23 | `image_crop_size`: Cropping size for embedding computation 24 | 25 | `nn_checkpoint`: Neural network model path 26 | 27 | `pca_checkpoint`: PCA model path 28 | 29 | `pnp_min_inliers`: Minimum number of match inliers for transformation computation between keyframes. 30 | 31 | `detection_publication_period_sec`: Period of publication of global descriptors. 32 | 33 | `detection_publication_max_elems_per_msg`: Split global descriptors messages in maximum N elements. 34 | 35 | `enable_intra_robot_loop_closures`: Enable intra-robot loop closures computation. 36 | 37 | `intra_loop_min_inbetween_keyframes`: Minimum number of keyframes between matches for intra-robot loop closures. Aims to avoid matching subsequent images. 38 | 39 | `keyframe_generation_ratio_threshold`: Generate a new keyframe if the number of inlier matches between the current frame and the previous keyframe is below the specified ratio. If set to 1.0, all frames are keyframes. 40 | 41 | `use_vertex_cover_selection`: Use vertex cover computation to minimize communication of keyframe data. 42 | 43 | `map_manager_process_period_ms`: Period for loop closure processing. 44 | 45 | ### Neighbor Management: 46 | 47 | `enable_neighbor_monitoring`: Enable monitoring of robots in communication range. If false, all robots are always considered neighbors. Set to false only when connectivity is always maintained between all the robots. 48 | 49 | `max_heartbeat_delay_sec`: Communication delay beyond which a neighboring robot is not considered in range if it did not communicate. 50 | 51 | `heartbeat_period_sec`: Period of heartbeat publication. 52 | 53 | ### Back-End: 54 | 55 | `pose_graph_optimization_start_period_ms`: Period of PGO start trigger. 56 | 57 | `pose_graph_optimization_loop_period_ms`: Period of inner loop of PGO process. 58 | 59 | `max_waiting_time_sec`: Maximum waiting time during PGO before going back to an idle state. Prevents being lock in an intermediate state in case of disconnections or failures. 60 | 61 | `enable_logs`: Enable logging of optimization result files. 62 | 63 | `log_folder`: Path where to store the logged optimization files. 64 | 65 | TODO:update -------------------------------------------------------------------------------- /config/cslam/example.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | frontend: 4 | left_image_topic: "stereo_camera/left/image_rect_color" 5 | right_image_topic: "stereo_camera/right/image_rect_color" 6 | left_camera_info_topic: "stereo_camera/left/camera_info" 7 | right_camera_info_topic: "stereo_camera/right/camera_info" 8 | odom_topic: "odom" 9 | sensor_base_frame_id: "camera_link" 10 | sensor_type: "stereo" 11 | inter_robot_loop_closure_budget: 5 12 | inter_robot_detection_period_sec: 5 13 | max_queue_size: 10 14 | similarity_threshold: 0.1 15 | global_descriptors_topic: "/global_descriptors" 16 | inter_robot_matches_topic: "/inter_robot_matches" 17 | image_crop_size: 376 18 | pnp_min_inliers: 6 19 | detection_publication_period_sec: 1.0 20 | detection_publication_max_elems_per_msg: 10 21 | enable_intra_robot_loop_closures: true 22 | intra_loop_min_inbetween_keyframes: 20 23 | keyframe_generation_ratio_threshold: 1.0 24 | use_vertex_cover_selection: true 25 | map_manager_process_period_ms: 100 26 | global_descriptor_technique: "netvlad" 27 | nn_checkpoint: "models/netvlad.pth.tar" 28 | netvlad: 29 | pca_checkpoint: "models/netvlad_pca.pkl" 30 | neighbor_management: 31 | enable_neighbor_monitoring: true 32 | max_heartbeat_delay_sec: 5.0 33 | heartbeat_period_sec: 1.0 34 | backend: 35 | pose_graph_optimization_start_period_ms: 500 36 | pose_graph_optimization_loop_period_ms: 100 37 | max_waiting_time_sec: 60 38 | enable_broadcast_tf_frames: true 39 | visualization: 40 | enable: false 41 | publishing_period_ms: 1000 42 | evaluation: 43 | enable_logs: false 44 | log_folder: "" 45 | enable_gps_recording: false 46 | gps_topic: "gps/fix" 47 | -------------------------------------------------------------------------------- /cslam/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lajoiepy/cslam/fccf88765167431d293a1245c19411fd874f937d/cslam/__init__.py -------------------------------------------------------------------------------- /cslam/broker.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from cslam.algebraic_connectivity_maximization import EdgeInterRobot 3 | import numpy as np 4 | from networkx.algorithms import bipartite 5 | import networkx as nx 6 | 7 | 8 | class Broker(object): 9 | """The broker decides which vertices in the matching 10 | graph are to be shared between the robots. 11 | """ 12 | 13 | def __init__(self, edges, robots_involved): 14 | """Initialize the broker 15 | 16 | Args: 17 | edges (list(EdgeInterRobot)): _description_ 18 | robots_involved (list(int)): Robot ids of the 19 | robots involved in the exchange 20 | """ 21 | self.edges = edges 22 | robots_involved_with_edges = set() 23 | for e in self.edges: 24 | if e.robot0_id in robots_involved: 25 | robots_involved_with_edges.add(e.robot0_id) 26 | if e.robot1_id in robots_involved: 27 | robots_involved_with_edges.add(e.robot1_id) 28 | robots_involved_with_edges = list(robots_involved_with_edges) 29 | self.is_multi_robot_graph = len(robots_involved_with_edges) >= 2 30 | 31 | if self.is_multi_robot_graph: 32 | if len(robots_involved_with_edges) == 2: 33 | self.is_bipartite = True 34 | elif len(robots_involved_with_edges) > 2: 35 | self.is_bipartite = False 36 | else: 37 | raise Exception( 38 | "Broker: The communication brokerage needs to be between at least 2 robots" 39 | ) 40 | 41 | # Build graph 42 | self.matching_graph = nx.Graph() 43 | for e in self.edges: 44 | edge_vertices = [(e.robot0_id, e.robot0_keyframe_id), 45 | (e.robot1_id, e.robot1_keyframe_id)] 46 | # Add vertices (required for bipartite) 47 | for vertex in edge_vertices: 48 | if vertex not in self.matching_graph: 49 | if self.is_bipartite: 50 | if vertex[0] == robots_involved_with_edges[0]: 51 | self.matching_graph.add_node(vertex, 52 | bipartite=0) 53 | elif vertex[0] == robots_involved_with_edges[1]: 54 | self.matching_graph.add_node(vertex, 55 | bipartite=1) 56 | else: 57 | if vertex[0] in robots_involved_with_edges: 58 | self.matching_graph.add_node(vertex) 59 | # Add edges 60 | if edge_vertices[0][ 61 | 0] in robots_involved_with_edges and edge_vertices[1][ 62 | 0] in robots_involved_with_edges: 63 | self.matching_graph.add_edge(edge_vertices[0], 64 | edge_vertices[1]) 65 | 66 | def brokerage(self, use_vertex_cover): 67 | """Return the broker selection of vertices to send. 68 | Either using vertex cover or simple dialog strategy. 69 | 70 | Args: 71 | use_vertex_cover (bool): use vertex cover stratehy 72 | 73 | Returns: 74 | List(set((int,int)): Vertices to be transmitted 75 | """ 76 | if self.is_multi_robot_graph: 77 | if use_vertex_cover: 78 | return self.vertex_cover() 79 | else: 80 | return self.simple_dialog() 81 | else: 82 | return [] 83 | 84 | def vertex_cover(self): 85 | """Computes the minimum vertex cover over the edges 86 | If the graph is bipartite: we compute the maximum matching and 87 | recover the minimum vertex cover with Konig's theorem 88 | 89 | Otherwise, we use an approximate greedy algorithm. 90 | 91 | Returns: 92 | List(set((int,int)): Vertices to be transmitted 93 | """ 94 | vertex_covers = [] 95 | # Divide into components 96 | matching_subgraphs = [ 97 | self.matching_graph.subgraph(c).copy() 98 | for c in nx.connected_components(self.matching_graph) 99 | ] 100 | # Compute min cover on connected components 101 | for graph in matching_subgraphs: 102 | if self.is_bipartite: 103 | matching = nx.bipartite.maximum_matching(graph) 104 | vertex_covers.append( 105 | nx.bipartite.to_vertex_cover(graph, matching)) 106 | else: 107 | vertex_covers.append( 108 | nx.algorithms.approximation.vertex_cover. 109 | min_weighted_vertex_cover(graph)) 110 | return vertex_covers 111 | 112 | def simple_dialog(self): 113 | """Simple dialog exchange 114 | For each edge, transmit one of the two vertices randomly 115 | unless one of the 2 vertices is already transmitted. 116 | 117 | Returns: 118 | List(set((int,int)): Vertices to be transmitted 119 | """ 120 | vertices_dialog = set() 121 | for e in self.edges: 122 | edge_vertices = [(e.robot0_id, e.robot0_keyframe_id), 123 | (e.robot1_id, e.robot1_keyframe_id)] 124 | if edge_vertices[0] not in vertices_dialog and edge_vertices[ 125 | 1] not in vertices_dialog: 126 | rand_idx = np.random.randint(2) 127 | vertices_dialog.add(edge_vertices[rand_idx]) 128 | 129 | return [vertices_dialog] 130 | -------------------------------------------------------------------------------- /cslam/lidar_handler_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import numpy as np 3 | from message_filters import ApproximateTimeSynchronizer, Subscriber 4 | from sensor_msgs.msg import PointCloud2, PointField, NavSatFix 5 | from nav_msgs.msg import Odometry 6 | from cslam_common_interfaces.msg import KeyframeOdom, KeyframePointCloud 7 | from cslam_common_interfaces.msg import LocalDescriptorsRequest, LocalPointCloudDescriptors, InterRobotLoopClosure, IntraRobotLoopClosure, LocalKeyframeMatch 8 | from cslam_common_interfaces.msg import VizPointCloud 9 | import cslam.lidar_pr.icp_utils as icp_utils 10 | import rclpy 11 | from rclpy.node import Node 12 | from rclpy.clock import Clock 13 | from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy 14 | from diagnostic_msgs.msg import KeyValue 15 | 16 | class LidarHandler: 17 | """Front-End data handler for lidar data 18 | """ 19 | def __init__(self, node, params): 20 | self.node = node 21 | self.params = params 22 | 23 | qos_profile = QoSProfile( 24 | reliability=ReliabilityPolicy.BEST_EFFORT, 25 | history=HistoryPolicy.KEEP_LAST, 26 | depth=100 27 | ) 28 | 29 | tss = ApproximateTimeSynchronizer( [ Subscriber(self.node, PointCloud2, self.params["frontend.pointcloud_topic"], qos_profile=qos_profile), 30 | Subscriber(self.node, Odometry, self.params["frontend.odom_topic"])], 100, self.params["frontend.pointcloud_odom_approx_time_sync_s"] ) 31 | tss.registerCallback(self.lidar_callback) 32 | 33 | self.keyframe_odom_publisher = self.node.create_publisher(KeyframeOdom, "cslam/keyframe_odom", 100) 34 | 35 | self.keyframe_pointcloud_publisher = self.node.create_publisher(KeyframePointCloud, "cslam/keyframe_data", 100) 36 | 37 | self.send_local_descriptors_subscriber = self.node.create_subscription(LocalDescriptorsRequest, 38 | "cslam/local_descriptors_request", self.send_local_descriptors_request, 100) 39 | 40 | self.local_keyframe_match_subscriber = self.node.create_subscription(LocalKeyframeMatch, 41 | "cslam/local_keyframe_match", self.receive_local_keyframe_match, 100) 42 | 43 | self.pointcloud_descriptors_publisher = self.node.create_publisher(LocalPointCloudDescriptors, "/cslam/local_descriptors", 100) 44 | 45 | self.pointcloud_descriptors_subscriber = self.node.create_subscription(LocalPointCloudDescriptors, "/cslam/local_descriptors", self.receive_local_descriptors, 100) 46 | 47 | self.intra_robot_loop_closure_publisher = self.node.create_publisher(IntraRobotLoopClosure, "cslam/intra_robot_loop_closure", 100) 48 | 49 | self.inter_robot_loop_closure_publisher = self.node.create_publisher(InterRobotLoopClosure, "/cslam/inter_robot_loop_closure", 100) 50 | 51 | self.viz_pointcloud_publisher = self.node.create_publisher(VizPointCloud, "/cslam/viz/keyframe_pointcloud", 100) 52 | 53 | period_ms = self.params["frontend.map_manager_process_period_ms"] 54 | self.processing_timer = self.node.create_timer(float(period_ms)/1000, self.process_new_sensor_data, clock=Clock()) 55 | 56 | self.received_data = [] 57 | self.local_descriptors_map = {} 58 | self.nb_local_keyframes = 0 59 | self.previous_keyframe = None 60 | self.previous_odom = None 61 | 62 | if self.params["evaluation.enable_logs"]: 63 | self.log_publisher = self.node.create_publisher( 64 | KeyValue, 'log_info', 100) 65 | self.log_local_descriptors_cumulative_communication = 0 66 | 67 | if self.params["evaluation.enable_gps_recording"]: 68 | self.gps_subscriber = self.node.create_subscription(NavSatFix, self.params["evaluation.gps_topic"], self.gps_callback, 100) 69 | self.gps_data = [] 70 | self.latest_gps = NavSatFix() 71 | 72 | def lidar_callback(self, pc_msg, odom_msg): 73 | """Sensor data callback 74 | 75 | Args: 76 | pc_msg (sensor_msgs/Pointcloud2): point cloud 77 | odom_msg (nav_msgs/Odometry): odometry estimate 78 | """ 79 | if (odom_msg.pose.covariance[0] > 1000): 80 | self.node.get_logger().warn("Odom tracking failed, skipping frame") 81 | return 82 | self.received_data.append((pc_msg, odom_msg)) 83 | if self.params["evaluation.enable_gps_recording"]: 84 | self.gps_data.append(self.latest_gps) 85 | 86 | def gps_callback(self, gps_msg): 87 | """GPS data callback 88 | """ 89 | self.latest_gps = gps_msg 90 | 91 | def send_local_descriptors_request(self, request): 92 | """Callback for local descriptors request 93 | """ 94 | out_msg = LocalPointCloudDescriptors() 95 | out_msg.data = icp_utils.open3d_to_ros(self.local_descriptors_map[request.keyframe_id]) 96 | out_msg.keyframe_id = request.keyframe_id 97 | out_msg.robot_id = self.params["robot_id"] 98 | out_msg.matches_robot_id = request.matches_robot_id 99 | out_msg.matches_keyframe_id = request.matches_keyframe_id 100 | 101 | self.pointcloud_descriptors_publisher.publish(out_msg) 102 | if self.params["evaluation.enable_logs"]: 103 | self.log_local_descriptors_cumulative_communication += len(out_msg.data.data) 104 | self.log_publisher.publish(KeyValue(key="local_descriptors_cumulative_communication", value=str(self.log_local_descriptors_cumulative_communication))) 105 | 106 | def receive_local_descriptors(self, msg): 107 | """Callback for local descriptors from other robots 108 | """ 109 | frame_ids = [] 110 | for i in range(len(msg.matches_robot_id)): 111 | if msg.matches_robot_id[i] == self.params["robot_id"]: 112 | frame_ids.append(msg.matches_keyframe_id[i]) 113 | for frame_id in frame_ids: 114 | pc = self.local_descriptors_map[frame_id] 115 | transform, success = icp_utils.compute_transform(pc, icp_utils.ros_to_open3d(msg.data), self.params["frontend.voxel_size"], self.params["frontend.registration_min_inliers"]) 116 | out_msg = InterRobotLoopClosure() 117 | out_msg.robot0_id = self.params["robot_id"] 118 | out_msg.robot0_keyframe_id = frame_id 119 | out_msg.robot1_id = msg.robot_id 120 | out_msg.robot1_keyframe_id = msg.keyframe_id 121 | if success: 122 | out_msg.success = True 123 | out_msg.transform = transform 124 | else: 125 | out_msg.success = False 126 | self.inter_robot_loop_closure_publisher.publish(out_msg) 127 | 128 | def receive_local_keyframe_match(self, msg): 129 | """Callback for local keyframe match for intra-robot loop closures 130 | """ 131 | pc0 = self.local_descriptors_map[msg.keyframe0_id] 132 | pc1 = self.local_descriptors_map[msg.keyframe1_id] 133 | transform, success = icp_utils.compute_transform(pc0, pc1, self.params["frontend.voxel_size"], self.params["frontend.registration_min_inliers"]) 134 | out_msg = IntraRobotLoopClosure() 135 | out_msg.keyframe0_id = msg.keyframe0_id 136 | out_msg.keyframe1_id = msg.keyframe1_id 137 | if success: 138 | out_msg.success = True 139 | out_msg.transform = transform 140 | else: 141 | out_msg.success = False 142 | self.intra_robot_loop_closure_publisher.publish(out_msg) 143 | 144 | def odom_distance_squared(self, odom0, odom1): 145 | """Compute the squared distance between two odometry messages 146 | """ 147 | return (odom0.pose.pose.position.x - odom1.pose.pose.position.x)**2 + (odom0.pose.pose.position.y - odom1.pose.pose.position.y)**2 + (odom0.pose.pose.position.z - odom1.pose.pose.position.z)**2 148 | 149 | def generate_new_keyframe(self, msg): 150 | """Check if we should generate a new keyframe 151 | 152 | Args: 153 | msg (nav_msgs/Odometry): odometry msg 154 | 155 | Returns: 156 | bool: flag indicating if a new keyframe should be created. 157 | """ 158 | if self.previous_odom is None: 159 | self.previous_odom = msg[1] 160 | return True 161 | dist = self.odom_distance_squared(self.previous_odom, msg[1]) 162 | if dist > self.params["frontend.keyframe_generation_ratio_distance"]**2: 163 | self.previous_odom = msg[1] 164 | return True 165 | else: 166 | return False 167 | 168 | def process_new_sensor_data(self): 169 | """ Process new sensor data 170 | """ 171 | if len(self.received_data) > 0: 172 | data = self.received_data[0] 173 | self.received_data.pop(0) 174 | gps_data = None 175 | if self.params["evaluation.enable_gps_recording"]: 176 | gps = self.gps_data[0] 177 | self.gps_data.pop(0) 178 | if self.generate_new_keyframe(data): 179 | try: 180 | self.local_descriptors_map[self.nb_local_keyframes] = icp_utils.downsample_ros_pointcloud(data[0], self.params["frontend.voxel_size"]) 181 | except Exception as e: 182 | self.local_descriptors_map[self.nb_local_keyframes] = [] 183 | self.node.get_logger().info("Failure to downsample point cloud to voxel size {}. {}".format(self.params["frontend.voxel_size"], e)) 184 | return 185 | # Publish pointcloud 186 | msg_pointcloud = KeyframePointCloud() 187 | msg_pointcloud.id = self.nb_local_keyframes 188 | msg_pointcloud.pointcloud = icp_utils.open3d_to_ros(self.local_descriptors_map[self.nb_local_keyframes]) 189 | self.keyframe_pointcloud_publisher.publish(msg_pointcloud) 190 | # Publish odom 191 | msg_odom = KeyframeOdom() 192 | msg_odom.id = self.nb_local_keyframes 193 | msg_odom.odom = data[1] 194 | if self.params["evaluation.enable_gps_recording"]: 195 | msg_odom.gps = gps 196 | self.keyframe_odom_publisher.publish(msg_odom) 197 | if self.params["visualization.enable"]: 198 | viz_msg = VizPointCloud() 199 | viz_msg.robot_id = self.params["robot_id"] 200 | viz_msg.keyframe_id = self.nb_local_keyframes 201 | viz_msg.pointcloud = msg_pointcloud.pointcloud 202 | self.viz_pointcloud_publisher.publish(viz_msg) 203 | self.nb_local_keyframes = self.nb_local_keyframes + 1 204 | 205 | if __name__ == '__main__': 206 | 207 | rclpy.init(args=None) 208 | node = Node("map_manager") 209 | node.declare_parameters( 210 | namespace='', 211 | parameters=[('frontend.pointcloud_topic', 'pointcloud'), 212 | ('frontend.odom_topic', 'odom'), 213 | ('frontend.map_manager_process_period_ms', 100), 214 | ('frontend.voxel_size', 0.5), 215 | ('frontend.registration_min_inliers', 60), 216 | ('frontend.keyframe_generation_ratio_distance', 0.5), 217 | ('frontend.pointcloud_odom_approx_time_sync_s', 0.1), 218 | ('robot_id', 0), 219 | ('evaluation.enable_logs', False), 220 | ('evaluation.enable_gps_recording', False), 221 | ('evaluation.gps_topic', ""), 222 | ('evaluation.gps_topic', ""), 223 | ('visualization.enable', False), 224 | ]) 225 | params = {} 226 | params['frontend.pointcloud_topic'] = node.get_parameter( 227 | 'frontend.pointcloud_topic').value 228 | params['frontend.odom_topic'] = node.get_parameter( 229 | 'frontend.odom_topic').value 230 | params['frontend.map_manager_process_period_ms'] = node.get_parameter( 231 | 'frontend.map_manager_process_period_ms').value 232 | params['frontend.voxel_size'] = node.get_parameter( 233 | 'frontend.voxel_size').value 234 | params['frontend.registration_min_inliers'] = node.get_parameter( 235 | 'frontend.registration_min_inliers').value 236 | params['frontend.keyframe_generation_ratio_distance'] = node.get_parameter( 237 | 'frontend.keyframe_generation_ratio_distance').value 238 | params['frontend.pointcloud_odom_approx_time_sync_s'] = node.get_parameter( 239 | 'frontend.pointcloud_odom_approx_time_sync_s').value 240 | params['robot_id'] = node.get_parameter( 241 | 'robot_id').value 242 | params["evaluation.enable_logs"] = node.get_parameter( 243 | 'evaluation.enable_logs').value 244 | params["evaluation.enable_gps_recording"] = node.get_parameter( 245 | 'evaluation.enable_gps_recording').value 246 | params["evaluation.gps_topic"] = node.get_parameter( 247 | 'evaluation.gps_topic').value 248 | params["visualization.enable"] = node.get_parameter( 249 | 'visualization.enable').value 250 | lidar_handler = LidarHandler(node, params) 251 | node.get_logger().info('Initialization done.') 252 | rclpy.spin(node) 253 | rclpy.shutdown() -------------------------------------------------------------------------------- /cslam/lidar_pr/icp_utils.py: -------------------------------------------------------------------------------- 1 | import cslam.utils.point_cloud2 as pc2_utils 2 | from geometry_msgs.msg import Transform 3 | from scipy.spatial.transform import Rotation as R 4 | from std_msgs.msg import Header 5 | from sensor_msgs.msg import PointCloud2, PointField 6 | 7 | import numpy as np 8 | import teaserpp_python 9 | import open3d 10 | from scipy.spatial import cKDTree 11 | import rclpy 12 | 13 | FIELDS_XYZ = [ 14 | PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), 15 | PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), 16 | PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), 17 | ] 18 | 19 | # Partially adapted from https://github.com/MIT-SPARK/TEASER-plusplus/tree/master/examples/teaser_python_fpfh_icp 20 | 21 | 22 | def pcd2xyz(pcd): 23 | return np.asarray(pcd.points).T 24 | 25 | 26 | def extract_fpfh(pcd, voxel_size): 27 | radius_normal = voxel_size * 2 28 | pcd.estimate_normals( 29 | open3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, 30 | max_nn=30)) 31 | 32 | radius_feature = voxel_size * 5 33 | fpfh = open3d.pipelines.registration.compute_fpfh_feature( 34 | pcd, 35 | open3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, 36 | max_nn=100)) 37 | return np.array(fpfh.data).T 38 | 39 | 40 | def find_knn_cpu(feat0, feat1, knn=1, return_distance=False): 41 | feat1tree = cKDTree(feat1) 42 | dists, nn_inds = feat1tree.query(feat0, k=knn, workers=-1) 43 | if return_distance: 44 | return nn_inds, dists 45 | else: 46 | return nn_inds 47 | 48 | 49 | def find_correspondences(feats0, feats1, mutual_filter=True): 50 | nns01 = find_knn_cpu(feats0, feats1, knn=1, return_distance=False) 51 | corres01_idx0 = np.arange(len(nns01)) 52 | corres01_idx1 = nns01 53 | 54 | if not mutual_filter: 55 | return corres01_idx0, corres01_idx1 56 | 57 | nns10 = find_knn_cpu(feats1, feats0, knn=1, return_distance=False) 58 | corres10_idx1 = np.arange(len(nns10)) 59 | corres10_idx0 = nns10 60 | 61 | mutual_filter = (corres10_idx0[corres01_idx1] == corres01_idx0) 62 | corres_idx0 = corres01_idx0[mutual_filter] 63 | corres_idx1 = corres01_idx1[mutual_filter] 64 | 65 | return corres_idx0, corres_idx1 66 | 67 | 68 | def get_teaser_solver(noise_bound): 69 | solver_params = teaserpp_python.RobustRegistrationSolver.Params() 70 | solver_params.cbar2 = 1.0 71 | solver_params.noise_bound = noise_bound 72 | solver_params.estimate_scaling = False 73 | solver_params.inlier_selection_mode = \ 74 | teaserpp_python.RobustRegistrationSolver.INLIER_SELECTION_MODE.PMC_EXACT 75 | solver_params.rotation_tim_graph = \ 76 | teaserpp_python.RobustRegistrationSolver.INLIER_GRAPH_FORMULATION.CHAIN 77 | solver_params.rotation_estimation_algorithm = \ 78 | teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS 79 | solver_params.rotation_gnc_factor = 1.4 80 | solver_params.rotation_max_iterations = 10000 81 | solver_params.rotation_cost_threshold = 1e-16 82 | solver = teaserpp_python.RobustRegistrationSolver(solver_params) 83 | return solver 84 | 85 | 86 | def Rt2T(R, t): 87 | T = np.identity(4) 88 | T[:3, :3] = R 89 | T[:3, 3] = t 90 | return T 91 | 92 | 93 | def downsample(points, voxel_size): 94 | 95 | mask = np.isfinite(points).all(axis=1) 96 | filtered_points = points[mask] 97 | 98 | open3d_cloud = open3d.geometry.PointCloud() 99 | open3d_cloud.points = open3d.utility.Vector3dVector(filtered_points) 100 | return open3d_cloud.voxel_down_sample(voxel_size=voxel_size) 101 | 102 | 103 | def solve_teaser(src, dst, voxel_size, min_inliers): 104 | src_feats = extract_fpfh(src, voxel_size) 105 | dst_feats = extract_fpfh(dst, voxel_size) 106 | 107 | corrs_src, corrs_dst = find_correspondences(src_feats, 108 | dst_feats, 109 | mutual_filter=True) 110 | 111 | src_xyz = pcd2xyz(src) # np array of size 3 by N 112 | dst_xyz = pcd2xyz(dst) # np array of size 3 by M 113 | src_corr = src_xyz[:, corrs_src] # np array of size 3 by num_corrs 114 | dst_corr = dst_xyz[:, corrs_dst] # np array of size 3 by num_corrs 115 | 116 | solver = get_teaser_solver(voxel_size) 117 | solver.solve(src_corr, dst_corr) 118 | 119 | solution = solver.getSolution() 120 | 121 | valid = len(solver.getInlierMaxClique()) > min_inliers 122 | 123 | if valid: 124 | # ICP refinement 125 | T_teaser = Rt2T(solution.rotation, solution.translation) 126 | icp_sol = open3d.pipelines.registration.registration_icp( 127 | src, dst, voxel_size, T_teaser, 128 | open3d.pipelines.registration.TransformationEstimationPointToPoint( 129 | ), 130 | open3d.pipelines.registration.ICPConvergenceCriteria( 131 | max_iteration=100)) 132 | T_icp = icp_sol.transformation 133 | solution.translation = T_icp[:3, 3] 134 | solution.rotation = T_icp[:3, :3] 135 | else: 136 | rclpy.logging.get_logger('cslam').info( 137 | 'Failed to compute loop closure. Number of inliers ( {} / {} )'. 138 | format(len(solver.getInlierMaxClique()), min_inliers)) 139 | return valid, solution.translation, solution.rotation 140 | 141 | 142 | def to_transform_msg(translation, rotation): 143 | T = Transform() 144 | T.translation.x = translation[0] 145 | T.translation.y = translation[1] 146 | T.translation.z = translation[2] 147 | rotation_matrix = R.from_matrix(np.array(rotation)) 148 | q = rotation_matrix.as_quat() 149 | T.rotation.x = q[0] 150 | T.rotation.y = q[1] 151 | T.rotation.z = q[2] 152 | T.rotation.w = q[3] 153 | return T 154 | 155 | 156 | def open3d_to_ros(open3d_cloud): 157 | header = Header() 158 | fields = FIELDS_XYZ 159 | points = np.asarray(open3d_cloud.points) 160 | return pc2_utils.create_cloud(header, fields, points) 161 | 162 | 163 | def ros_to_open3d(msg): 164 | points = ros_pointcloud_to_points(msg) 165 | open3d_cloud = open3d.geometry.PointCloud() 166 | open3d_cloud.points = open3d.utility.Vector3dVector(points) 167 | return open3d_cloud 168 | 169 | 170 | def ros_pointcloud_to_points(pc_msg): 171 | return pc2_utils.read_points_numpy_filtered(pc_msg)[:, :3] 172 | 173 | 174 | def downsample_ros_pointcloud(pc_msg, voxel_size): 175 | points = ros_pointcloud_to_points(pc_msg) 176 | return downsample(points, voxel_size) 177 | 178 | def compute_transform(src, dst, voxel_size, min_inliers): 179 | """Computes a 3D transform between 2 point clouds using TEASER++. 180 | 181 | Be careful: TEASER++ computes the transform from dst to src. 182 | 183 | Args: 184 | src (Open3D point cloud): pointcloud from 185 | dst (Open3D point cloud): pointcloud to 186 | voxel_size (int): Voxel size 187 | min_inliers (int): Minimum number of inlier points correspondance to consider the registration a success 188 | 189 | Returns: 190 | (Transform, bool): transform and success flag 191 | """ 192 | valid, translation, rotation = solve_teaser(src, dst, voxel_size, 193 | min_inliers) 194 | success = valid 195 | transform = to_transform_msg(translation, rotation) 196 | return transform, success 197 | -------------------------------------------------------------------------------- /cslam/lidar_pr/scancontext.py: -------------------------------------------------------------------------------- 1 | import cslam.lidar_pr.scancontext_utils as sc_utils 2 | 3 | class ScanContext: 4 | """ 5 | Scan Context descriptor for point clouds 6 | From: https://github.com/irapkaist/scancontext 7 | """ 8 | def __init__(self, params, node): 9 | self.node = node 10 | self.params = params 11 | self.shape = [20,60] # Same as in ScanContext paper 12 | self.max_length = 80 # Same as in ScanContext paper 13 | 14 | def compute_embedding(self, keyframe): 15 | desc = sc_utils.ptcloud2sc(keyframe, self.shape, self.max_length) 16 | return desc.reshape(-1) 17 | -------------------------------------------------------------------------------- /cslam/lidar_pr/scancontext_matching.py: -------------------------------------------------------------------------------- 1 | import cslam.lidar_pr.scancontext_utils as sc_utils 2 | import numpy as np 3 | from scipy import spatial 4 | 5 | class ScanContextMatching(object): 6 | """Nearest Neighbor matching of description vectors 7 | """ 8 | 9 | def __init__(self, shape=[20,60], num_candidates=10, threshold=0.15): 10 | """ Initialization 11 | Default configs are the same as in the original paper 12 | 13 | """ 14 | self.shape = shape 15 | self.num_candidates = num_candidates 16 | self.threshold = threshold 17 | 18 | self.scancontexts = np.zeros((1000, self.shape[0], self.shape[1])) 19 | self.ringkeys = np.zeros((1000, self.shape[0])) 20 | self.items = dict() 21 | self.nb_items = 0 22 | 23 | def add_item(self, descriptor, item): 24 | """Add item to the matching list 25 | 26 | Args: 27 | descriptor (np.array): descriptor 28 | item: identification info (e.g., int) 29 | """ 30 | sc = descriptor.reshape(self.shape) 31 | 32 | if self.nb_items >= len(self.ringkeys): 33 | self.scancontexts.resize((2 * len(self.scancontexts), self.shape[0], self.shape[1]), 34 | refcheck=False) 35 | self.ringkeys.resize((2 * len(self.ringkeys), self.shape[0]), 36 | refcheck=False) 37 | 38 | rk = sc_utils.sc2rk(sc) 39 | 40 | self.scancontexts[self.nb_items] = sc 41 | self.ringkeys[self.nb_items] = rk 42 | self.items[self.nb_items] = item 43 | 44 | self.nb_items = self.nb_items + 1 45 | 46 | def search(self, query, k): 47 | """Search for nearest neighbors 48 | 49 | Args: 50 | query (np.array): descriptor to match 51 | k (int): number of best matches to return 52 | 53 | Returns: 54 | list(int, np.array): best matches 55 | """ 56 | if self.nb_items < 1: 57 | return [None], [None] 58 | 59 | # step 1 60 | ringkey_history = np.array(self.ringkeys[:self.nb_items]) 61 | ringkey_tree = spatial.KDTree(ringkey_history) 62 | 63 | query_sc = query.reshape(self.shape) 64 | ringkey_query = sc_utils.sc2rk(query_sc) 65 | _, nncandidates_idx = ringkey_tree.query(ringkey_query, k=self.num_candidates) 66 | 67 | # step 2 68 | nn_dist = 1.0 # initialize with the largest value of distance 69 | nn_idx = None 70 | nn_yawdiff = None 71 | for ith in range(self.num_candidates): 72 | candidate_idx = nncandidates_idx[ith] 73 | candidate_sc = self.scancontexts[candidate_idx] 74 | dist, yaw_diff = sc_utils.distance_sc(candidate_sc, query_sc) 75 | if(dist < nn_dist): 76 | nn_dist = dist 77 | nn_yawdiff = yaw_diff 78 | nn_idx = candidate_idx 79 | 80 | if nn_idx is None: 81 | nn_idx = 0 82 | nn_yawdiff_deg = 0 83 | similarity = 0.0 84 | else: 85 | nn_yawdiff_deg = nn_yawdiff * (360/self.shape[1]) 86 | similarity = 1 - nn_dist # For now we return only 1 match, but we could return the n best matches 87 | return [self.items[nn_idx]], [similarity] 88 | 89 | def search_best(self, query): 90 | """Search for the nearest neighbor 91 | Implementation for compatibily only 92 | 93 | Args: 94 | query (np.array): descriptor to match 95 | 96 | Returns: 97 | int, np.array: best match 98 | """ 99 | if self.nb_items < 1: 100 | return None, None 101 | 102 | idxs, sims = self.search(query, 1) 103 | 104 | return idxs[0], sims[0] 105 | -------------------------------------------------------------------------------- /cslam/lidar_pr/scancontext_utils.py: -------------------------------------------------------------------------------- 1 | # Adapted from https://github.com/gisbi-kim/PyICP-SLAM/blob/master/utils/ScanContextManager.py 2 | 3 | import numpy as np 4 | np.set_printoptions(precision=4) 5 | 6 | import time 7 | from scipy import spatial 8 | 9 | 10 | def xy2theta(x, y): 11 | if (x >= 0 and y >= 0): 12 | theta = 180/np.pi * np.arctan(y/x) 13 | elif (x < 0 and y >= 0): 14 | theta = 180 - ((180/np.pi) * np.arctan(y/(-x))) 15 | elif (x < 0 and y < 0): 16 | theta = 180 + ((180/np.pi) * np.arctan(y/x)) 17 | elif ( x >= 0 and y < 0): 18 | theta = 360 - ((180/np.pi) * np.arctan((-y)/x)) 19 | return theta 20 | 21 | 22 | def pt2rs(point, gap_ring, gap_sector, num_ring, num_sector): 23 | x = point[0] 24 | y = point[1] 25 | # z = point[2] 26 | 27 | if(x == 0.0): 28 | x = 0.001 29 | if(y == 0.0): 30 | y = 0.001 31 | 32 | theta = xy2theta(x, y) 33 | faraway = np.sqrt(x*x + y*y) 34 | 35 | idx_ring = np.divmod(faraway, gap_ring)[0] 36 | idx_sector = np.divmod(theta, gap_sector)[0] 37 | 38 | if(idx_ring >= num_ring): 39 | idx_ring = num_ring-1 # python starts with 0 and ends with N-1 40 | 41 | return int(idx_ring), int(idx_sector) 42 | 43 | 44 | def ptcloud2sc(ptcloud, sc_shape, max_length): 45 | num_ring = sc_shape[0] 46 | num_sector = sc_shape[1] 47 | 48 | gap_ring = max_length/num_ring 49 | gap_sector = 360/num_sector 50 | 51 | enough_large = 500 52 | sc_storage = np.zeros([enough_large, num_ring, num_sector]) 53 | sc_counter = np.zeros([num_ring, num_sector]) 54 | 55 | num_points = ptcloud.shape[0] 56 | for pt_idx in range(num_points): 57 | point = ptcloud[pt_idx, :] 58 | if np.isnan(point[0]) or np.isnan(point[1]) or np.isnan(point[2]): 59 | # Reject spurious points 60 | continue 61 | point_height = point[2] + 2.0 # for setting ground is roughly zero 62 | 63 | idx_ring, idx_sector = pt2rs(point, gap_ring, gap_sector, num_ring, num_sector) 64 | 65 | if sc_counter[idx_ring, idx_sector] >= enough_large: 66 | continue 67 | sc_storage[int(sc_counter[idx_ring, idx_sector]), idx_ring, idx_sector] = point_height 68 | sc_counter[idx_ring, idx_sector] = sc_counter[idx_ring, idx_sector] + 1 69 | 70 | sc = np.amax(sc_storage, axis=0) 71 | 72 | return sc 73 | 74 | 75 | def sc2rk(sc): 76 | return np.mean(sc, axis=1) 77 | 78 | def distance_sc(sc1, sc2): 79 | num_sectors = sc1.shape[1] 80 | 81 | # repeat to move 1 columns 82 | _one_step = 1 # const 83 | sim_for_each_cols = np.zeros(num_sectors) 84 | for i in range(num_sectors): 85 | # Shift 86 | sc1 = np.roll(sc1, _one_step, axis=1) # columne shift 87 | 88 | #compare 89 | sum_of_cossim = 0 90 | num_col_engaged = 0 91 | for j in range(num_sectors): 92 | col_j_1 = sc1[:, j] 93 | col_j_2 = sc2[:, j] 94 | if (~np.any(col_j_1) or ~np.any(col_j_2)): 95 | # to avoid being divided by zero when calculating cosine similarity 96 | # - but this part is quite slow in python, you can omit it. 97 | continue 98 | 99 | cossim = np.dot(col_j_1, col_j_2) / (np.linalg.norm(col_j_1) * np.linalg.norm(col_j_2)) 100 | sum_of_cossim = sum_of_cossim + cossim 101 | 102 | num_col_engaged = num_col_engaged + 1 103 | 104 | # save 105 | if num_col_engaged == 0: 106 | sim_for_each_cols[i] = 0.0 107 | else: 108 | sim_for_each_cols[i] = sum_of_cossim / num_col_engaged 109 | 110 | yaw_diff = np.argmax(sim_for_each_cols) + 1 # because python starts with 0 111 | sim = np.max(sim_for_each_cols) 112 | dist = 1 - sim 113 | 114 | return dist, yaw_diff -------------------------------------------------------------------------------- /cslam/loop_closure_detection_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Loop Closure Detection service 3 | # Abstraction to support multiple implementations of loop closure detection for benchmarking 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | from rclpy.clock import Clock 8 | from sensor_msgs.msg import Image 9 | 10 | from cslam.global_descriptor_loop_closure_detection import GlobalDescriptorLoopClosureDetection 11 | 12 | 13 | class LoopClosureDetection(Node): 14 | """ Global image descriptor matching for loop closure detection """ 15 | 16 | def __init__(self): 17 | """Initialization and parameter parsing""" 18 | super().__init__('loop_closure_detection') 19 | 20 | self.declare_parameters( 21 | namespace='', 22 | parameters=[('frontend.similarity_threshold', 0.9), 23 | ('frontend.global_descriptor_technique', 'cosplace'), 24 | ('frontend.netvlad.pca_checkpoint', ''), ('frontend.nn_checkpoint', 'models/resnet18_64.pth'), 25 | ('robot_id', 0), ('max_nb_robots', 10), 26 | ('frontend.inter_robot_loop_closure_budget', 5), 27 | ('frontend.inter_robot_detection_period_sec', 5), 28 | ('frontend.nb_best_matches', 10), ('frontend.image_crop_size', 376), 29 | ('frontend.intra_loop_min_inbetween_keyframes', 10), 30 | ('neighbor_management.max_heartbeat_delay_sec', 5.0), 31 | ('neighbor_management.init_delay_sec', 5.0), 32 | ('neighbor_management.heartbeat_period_sec', 0.5), 33 | ('frontend.detection_publication_period_sec', 1.0), 34 | ('frontend.detection_publication_max_elems_per_msg', 35 | 10), ('neighbor_management.enable_neighbor_monitoring', False), 36 | ('frontend.enable_intra_robot_loop_closures', False), 37 | ('frontend.global_descriptors_topic', "global_descriptors"), 38 | ('frontend.inter_robot_matches_topic', "inter_robot_matches"), 39 | ('frontend.enable_sparsification', True), 40 | ('frontend.use_vertex_cover_selection', True), 41 | ('frontend.cosplace.descriptor_dim', 64), 42 | ('frontend.cosplace.backbone', "resnet18"), 43 | ('frontend.sensor_type', "stereo"), 44 | ('evaluation.enable_logs', False), 45 | ('evaluation.enable_sparsification_comparison', False), 46 | ]) 47 | self.params = {} 48 | self.params['frontend.similarity_threshold'] = self.get_parameter( 49 | 'frontend.similarity_threshold').value 50 | self.params['frontend.intra_loop_min_inbetween_keyframes'] = self.get_parameter( 51 | 'frontend.intra_loop_min_inbetween_keyframes').value 52 | self.params['frontend.nb_best_matches'] = self.get_parameter( 53 | 'frontend.nb_best_matches').value 54 | self.params['frontend.global_descriptor_technique'] = self.get_parameter( 55 | 'frontend.global_descriptor_technique').value 56 | self.params['frontend.nn_checkpoint'] = self.get_parameter( 57 | 'frontend.nn_checkpoint').value 58 | self.params['robot_id'] = self.get_parameter('robot_id').value 59 | self.params['max_nb_robots'] = self.get_parameter('max_nb_robots').value 60 | self.params['frontend.inter_robot_loop_closure_budget'] = self.get_parameter( 61 | 'frontend.inter_robot_loop_closure_budget').value 62 | self.params['frontend.enable_intra_robot_loop_closures'] = self.get_parameter( 63 | 'frontend.enable_intra_robot_loop_closures').value 64 | self.params['frontend.inter_robot_detection_period_sec'] = self.get_parameter( 65 | 'frontend.inter_robot_detection_period_sec').value 66 | self.params["frontend.image_crop_size"] = self.get_parameter( 67 | 'frontend.image_crop_size').value 68 | self.params["frontend.enable_sparsification"] = self.get_parameter( 69 | 'frontend.enable_sparsification').value 70 | self.params["neighbor_management.enable_neighbor_monitoring"] = self.get_parameter( 71 | 'neighbor_management.enable_neighbor_monitoring').value 72 | self.params["neighbor_management.max_heartbeat_delay_sec"] = self.get_parameter( 73 | 'neighbor_management.max_heartbeat_delay_sec').value 74 | self.params["neighbor_management.init_delay_sec"] = self.get_parameter( 75 | 'neighbor_management.init_delay_sec').value 76 | self.params["neighbor_management.heartbeat_period_sec"] = self.get_parameter( 77 | 'neighbor_management.heartbeat_period_sec').value 78 | self.params[ 79 | "frontend.detection_publication_period_sec"] = self.get_parameter( 80 | 'frontend.detection_publication_period_sec').value 81 | self.params[ 82 | "frontend.detection_publication_max_elems_per_msg"] = self.get_parameter( 83 | 'frontend.detection_publication_max_elems_per_msg').value 84 | self.params["frontend.use_vertex_cover_selection"] = self.get_parameter( 85 | 'frontend.use_vertex_cover_selection').value 86 | self.params["frontend.cosplace.descriptor_dim"] = self.get_parameter( 87 | 'frontend.cosplace.descriptor_dim').value 88 | self.params["frontend.cosplace.backbone"] = self.get_parameter( 89 | 'frontend.cosplace.backbone').value 90 | self.params["frontend.sensor_type"] = self.get_parameter( 91 | 'frontend.sensor_type').value.lower() 92 | self.params["evaluation.enable_logs"] = self.get_parameter( 93 | 'evaluation.enable_logs').value 94 | self.params["evaluation.enable_sparsification_comparison"] = self.get_parameter( 95 | 'evaluation.enable_sparsification_comparison').value 96 | 97 | self.glcd = GlobalDescriptorLoopClosureDetection( 98 | self.params, self) 99 | self.inter_robot_detection_timer = self.create_timer( 100 | self.params['frontend.inter_robot_detection_period_sec'], 101 | self.glcd.detect_inter, clock=Clock()) 102 | 103 | 104 | if __name__ == '__main__': 105 | 106 | rclpy.init(args=None) 107 | lcd = LoopClosureDetection() 108 | lcd.get_logger().info('Initialization done.') 109 | rclpy.spin(lcd) 110 | rclpy.shutdown() 111 | -------------------------------------------------------------------------------- /cslam/loop_closure_sparse_matching.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cslam.nns_matching import NearestNeighborsMatching 3 | from cslam.lidar_pr.scancontext_matching import ScanContextMatching 4 | from cslam.algebraic_connectivity_maximization import AlgebraicConnectivityMaximization, EdgeInterRobot 5 | 6 | class LoopClosureSparseMatching(object): 7 | """Sparse matching for loop closure detection 8 | Matches global descriptors to generate loop closure candidates 9 | Then candidates are selected such that we respect the communication budget 10 | """ 11 | 12 | def __init__(self, params): 13 | """ Initialization of loop closure matching 14 | 15 | Args: 16 | params (dict): ROS 2 parameters 17 | """ 18 | # Extract params 19 | self.params = params 20 | # Initialize matching structs 21 | if self.params["frontend.sensor_type"] == "lidar": 22 | self.local_nnsm = ScanContextMatching() 23 | else: 24 | self.local_nnsm = NearestNeighborsMatching() 25 | self.other_robots_nnsm = {} 26 | for i in range(self.params['max_nb_robots']): 27 | if i != self.params['robot_id']: 28 | if self.params["frontend.sensor_type"] == "lidar": 29 | self.other_robots_nnsm[i] = ScanContextMatching() 30 | else: 31 | self.other_robots_nnsm[i] = NearestNeighborsMatching() 32 | # Initialize candidate selection algorithm 33 | self.candidate_selector = AlgebraicConnectivityMaximization( 34 | self.params['robot_id'], self.params['max_nb_robots'], extra_params=self.params) 35 | 36 | def add_local_global_descriptor(self, embedding, keyframe_id): 37 | """ Add a local keyframe for matching 38 | 39 | Args: 40 | embedding (np.array): global descriptor 41 | id (int): keyframe id 42 | """ 43 | matches = [] 44 | self.local_nnsm.add_item(embedding, keyframe_id) 45 | for i in range(self.params['max_nb_robots']): 46 | if i != self.params['robot_id']: 47 | kf, similarity = self.other_robots_nnsm[i].search_best(embedding) 48 | if kf is not None: 49 | if similarity >= self.params['frontend.similarity_threshold']: 50 | match = EdgeInterRobot(self.params['robot_id'], keyframe_id, i, kf, 51 | similarity) 52 | self.candidate_selector.add_match(match) 53 | matches.append(match) 54 | return matches 55 | 56 | def add_other_robot_global_descriptor(self, msg): 57 | """ Add keyframe global descriptor info from other robot 58 | 59 | Args: 60 | msg (cslam_common_interfaces.msg.GlobalDescriptor): global descriptor info 61 | """ 62 | self.other_robots_nnsm[msg.robot_id].add_item( 63 | np.asarray(msg.descriptor), msg.keyframe_id) 64 | 65 | match = None 66 | kf, similarity = self.local_nnsm.search_best(np.asarray(msg.descriptor)) 67 | if kf is not None: 68 | if similarity >= self.params['frontend.similarity_threshold']: 69 | match = EdgeInterRobot(self.params['robot_id'], kf, msg.robot_id, 70 | msg.keyframe_id, similarity) 71 | self.candidate_selector.add_match(match) 72 | return match 73 | 74 | def match_local_loop_closures(self, descriptor, kf_id): 75 | kfs, similarities = self.local_nnsm.search(descriptor, 76 | k=self.params['frontend.nb_best_matches']) 77 | 78 | if len(kfs) > 0 and kfs[0] == kf_id: 79 | kfs, similarities = kfs[1:], similarities[1:] 80 | if len(kfs) == 0 or kfs[0] == None: 81 | return None, None 82 | 83 | for kf, similarity in zip(kfs, similarities): 84 | if abs(kf - 85 | kf_id) < self.params['frontend.intra_loop_min_inbetween_keyframes']: 86 | continue 87 | 88 | if similarity < self.params['frontend.similarity_threshold']: 89 | continue 90 | 91 | return kf, kfs 92 | return None, None 93 | 94 | def select_candidates(self, 95 | number_of_candidates, 96 | is_neighbor_in_range, 97 | greedy_initialization=True): 98 | """Select inter-robot loop closure candidates according to budget 99 | 100 | Args: 101 | number_of_candidates (int): inter-robot loop closure budget, 102 | is_neighbor_in_range: dict(int, bool): indicates which other robots are in communication range 103 | greedy_initialization: bool: use greedy initialization for selection 104 | 105 | Returns: 106 | list(EdgeInterRobot): selected edges 107 | """ 108 | return self.candidate_selector.select_candidates( 109 | number_of_candidates, is_neighbor_in_range, 110 | greedy_initialization) 111 | -------------------------------------------------------------------------------- /cslam/mac/.gitignore: -------------------------------------------------------------------------------- 1 | # OS generated files # 2 | ###################### 3 | .DS_Store 4 | .DS_Store? 5 | ._* 6 | .Spotlight-V100 7 | .Trashes 8 | ehthumbs.db 9 | [Tt]humbs.db 10 | 11 | # VS Code generated files # 12 | ########################## 13 | .vscode/ 14 | 15 | # Jupyter Notebooks # 16 | ##################### 17 | .ipynb_checkpoints 18 | 19 | # Binaries # 20 | ############ 21 | *.pyc 22 | *.egg-info 23 | 24 | # Latex # 25 | ######### 26 | *.aux 27 | *.fdb_latexmk 28 | *.fls 29 | *.log 30 | *.out 31 | *.synctex.gz 32 | *.bbl 33 | *.blg 34 | *.toc 35 | *.dvi 36 | *.lof 37 | *.lot 38 | main.pdf 39 | 40 | # spacemacs # 41 | ############# 42 | auto/ 43 | .#* 44 | 45 | # Python # 46 | ########## 47 | __pycache__/ 48 | 49 | 50 | # PyTorch # 51 | ########### 52 | *.pt 53 | 54 | # Executables/C++ # 55 | ################### 56 | build/ 57 | 58 | # Tensorflow # 59 | ############## 60 | *.tfrecord 61 | *.pb 62 | 63 | # Temporary Files # 64 | ################### 65 | *.*~ 66 | *.swp 67 | 68 | # Large Files # 69 | ############### 70 | *.png 71 | *.mp4 72 | *.jpg 73 | 74 | # Blender # 75 | ########### 76 | *.blend 77 | 78 | # MATLAB # 79 | ########## 80 | *.m~ 81 | 82 | # ROS # 83 | ####### 84 | *.bag 85 | 86 | # Jekyll # 87 | ########## 88 | .jekyll-metadata 89 | .sass-cache/ 90 | Gemfile.lock 91 | _site/ 92 | -------------------------------------------------------------------------------- /cslam/mac/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 MIT's Marine Robotics Group 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cslam/mac/README.md: -------------------------------------------------------------------------------- 1 | # Source 2 | https://github.com/MarineRoboticsGroup/mac (accessed June 2022) 3 | 4 | # mac 5 | Maximizing algebraic connectivity for graph sparsification 6 | 7 | MAC is an algorithm for solving the *maximum algebraic connectivity augmentation* problem. Specfifically, given a graph containing a (potentially empty) set of "fixed" edges and a set of "candidate" edges, as well as a cardinality constraint K, MAC tries to find the set of K cadidate edges whose addition to the fixed edges produces a graph with the largest possible [algebraic connectivity](https://en.wikipedia.org/wiki/Algebraic_connectivity). MAC does this by solving a convex relaxation of the maximum algebraic connectivity augmentation problem (which is itself NP-hard). The relaxation allows for "soft" inclusion of edges in the candidate set. When a solution to the relaxation is obtained, we _round_ it to the feasible set for the original problem. 8 | 9 | ## Getting started 10 | 11 | Install MAC locally with 12 | ```bash 13 | pip install -e . 14 | ``` 15 | 16 | Now you are ready to use MAC. 17 | 18 | ## Running the examples 19 | 20 | ### Basic examples 21 | 22 | From the `examples` directory, run: 23 | ```bash 24 | python3 random_graph_sparsification.py 25 | ``` 26 | which demonstrates our sparsification approach on an [Erdos-Renyi graph](https://en.wikipedia.org/wiki/Erd%C5%91s%E2%80%93R%C3%A9nyi_model). 27 | 28 | In the same directory, running: 29 | ```bash 30 | python3 petersen_graph_sparsification.py 31 | ``` 32 | will show the results of our approach on the [Petersen graph](https://en.wikipedia.org/wiki/Petersen_graph). 33 | 34 | In each case, the set of fixed edges is a chain, and the remaining edges are considered candidates. 35 | 36 | ### Pose graph sparsification 37 | 38 | For the pose graph examples, you will need to install [SE-Sync](https://github.com/david-m-rosen/SE-Sync) with [Python bindings](https://github.com/david-m-rosen/SE-Sync#python). 39 | 40 | Once that is installed, you need to modify the SE-Sync path in `g2o_experiment.py`: 41 | 42 | ```python 43 | # SE-Sync setup 44 | sesync_lib_path = "/path/to/SESync/C++/build/lib" 45 | sys.path.insert(0, sesync_lib_path) 46 | ``` 47 | 48 | Finally, run: 49 | ```bash 50 | python3 g2o_experiment.py [path to .g2o file] 51 | ``` 52 | to run MAC for pose graph sparsification and compute SLAM solutions. Several plots will be saved in the `examples` directory for inspection. 53 | 54 | ## Reference 55 | 56 | If you found this code useful, please cite our paper [here](https://arxiv.org/abs/2203.13897): 57 | ```bibtex 58 | @article{doherty2022spectral, 59 | title={Spectral {M}easurement {S}parsification for {P}ose-{G}raph {SLAM}}, 60 | author={Doherty, Kevin J and Rosen, David M and Leonard, John J}, 61 | journal={arXiv preprint arXiv:2203.13897}, 62 | year={2022} 63 | } 64 | ``` 65 | 66 | ## Notes 67 | 68 | - Currently `mac.py` assumes that there is at most one candidate edge between 69 | any pair of nodes. If your data includes multiple edges between the same pair 70 | of nodes, you can combine the edges into a single edge with weight equal to 71 | the sum of the individual edge weights before using MAC. 72 | -------------------------------------------------------------------------------- /cslam/mac/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lajoiepy/cslam/fccf88765167431d293a1245c19411fd874f937d/cslam/mac/__init__.py -------------------------------------------------------------------------------- /cslam/mac/mac.py: -------------------------------------------------------------------------------- 1 | # Modified by Pierre-Yves Lajoie (2022) 2 | 3 | from collections import namedtuple 4 | from timeit import default_timer as timer 5 | 6 | import networkx as nx 7 | import networkx.generators as gen 8 | import networkx.linalg as la 9 | import numpy as np 10 | from scipy.sparse import csc_matrix, csr_matrix 11 | from scipy.sparse.linalg import eigsh, lobpcg 12 | 13 | from cslam.mac.utils import * 14 | 15 | MACResult = namedtuple( 16 | 'MACResult', ['w', 'F_unrounded', 'objective_values', 'duality_gaps']) 17 | 18 | 19 | class MAC: 20 | 21 | def __init__(self, fixed_measurements, candidate_measurements, num_poses): 22 | self.L_odom = weight_graph_lap_from_edge_list(fixed_measurements, 23 | num_poses) 24 | self.num_poses = num_poses 25 | self.weights = [] 26 | self.edge_list = [] 27 | 28 | for meas in candidate_measurements: 29 | self.weights.append(meas.weight) 30 | self.edge_list.append((meas.i, meas.j)) 31 | 32 | self.weights = np.array(self.weights) 33 | self.edge_list = np.array(self.edge_list) 34 | 35 | def find_fiedler_pair(self, L, method='tracemin_lu', tol=1e-8): 36 | """ 37 | Compute the second smallest eigenvalue of L and corresponding 38 | eigenvector using `method` and tolerance `tol`. 39 | 40 | w: An element of [0,1]^m; this is the edge selection to use 41 | method: Any method supported by NetworkX for computing algebraic 42 | connectivity. See: 43 | https://networkx.org/documentation/stable/reference/generated/networkx.linalg.algebraicconnectivity.algebraic_connectivity.html 44 | 45 | tol: Numerical tolerance for eigenvalue computation 46 | 47 | returns a tuple (lambda_2(L), v_2(L)) containing the Fiedler 48 | value and corresponding vector. 49 | 50 | """ 51 | assert (method != 'lobpcg') # LOBPCG not supported at the moment 52 | find_fiedler_func = la.algebraicconnectivity._get_fiedler_func(method) 53 | x = None 54 | output = find_fiedler_func(L, 55 | x=x, 56 | normalized=False, 57 | tol=tol, 58 | seed=np.random.RandomState(7)) 59 | return output 60 | 61 | def combined_laplacian(self, w, tol=1e-10): 62 | """ 63 | Construct the combined Laplacian (fixed edges plus candidate edges weighted by w). 64 | 65 | w: An element of [0,1]^m; this is the edge selection to use 66 | tol: Tolerance for edges that are numerically zero. This improves speed 67 | in situations where edges are not *exactly* zero, but close enough that 68 | they have almost no influence on the graph. 69 | 70 | returns the matrix L(w) 71 | """ 72 | idx = np.where(w > tol) 73 | prod = w[idx] * self.weights[idx] 74 | C1 = weight_graph_lap_from_edges(self.edge_list[idx], prod, 75 | self.num_poses) 76 | C = self.L_odom + C1 77 | return C 78 | 79 | def evaluate_fiedler_pair(self, w, method='tracemin_lu', tol=1e-8): 80 | """ 81 | Compute the second smallest eigenvalue of L(w) and corresponding 82 | eigenvector using `method` and tolerance `tol`. This is just a helper 83 | that constructs L(w) and calls `self.find_fiedler_pair` on the 84 | resulting matrix, passing along the arguments. 85 | 86 | w: An element of [0,1]^m; this is the edge selection to use 87 | method: Any method supported by NetworkX for computing algebraic 88 | connectivity. See: 89 | https://networkx.org/documentation/stable/reference/generated/networkx.linalg.algebraicconnectivity.algebraic_connectivity.html 90 | 91 | tol: Numerical tolerance for eigenvalue computation 92 | 93 | returns a tuple (lambda_2(L(w)), v_2(L(w))) containing the Fiedler 94 | value and corresponding vector. 95 | 96 | """ 97 | return self.find_fiedler_pair(self.combined_laplacian(w), method, tol) 98 | 99 | def evaluate_objective(self, w): 100 | """ 101 | Compute lambda_2(L(w)) where L(w) is the Laplacian with edge i weighted 102 | by w_i and lambda_2 is the second smallest eigenvalue (this is the 103 | algebraic connectivity). 104 | 105 | w: Weights for each edge 106 | 107 | returns F(w) = lambda_2(L(w)). 108 | """ 109 | L = self.combined_laplacian(w) 110 | return self.find_fiedler_pair(L)[0] 111 | 112 | def grad_from_fiedler(self, fiedler_vec): 113 | """ 114 | Compute a (super)gradient of the algebraic connectivity with respect to w 115 | from the Fiedler vector. 116 | 117 | fiedler_vec: Eigenvector of the Laplacian corresponding to the second eigenvalue. 118 | 119 | returns grad F(w) from equation (8) of our paper: https://arxiv.org/pdf/2203.13897.pdf. 120 | """ 121 | grad = np.zeros(len(self.weights)) 122 | 123 | for k in range(len(self.weights)): 124 | edge = self.edge_list[k] # get edge (i,j) 125 | v_i = fiedler_vec[edge[0]] 126 | v_j = fiedler_vec[edge[1]] 127 | weight_k = self.weights[k] 128 | kdelta = weight_k * (v_i - v_j) 129 | grad[k] = kdelta * (v_i - v_j) 130 | return grad 131 | 132 | def round_solution(self, w, k): 133 | """ 134 | Round a solution w to the relaxed problem, i.e. w \\in [0,1]^m, |w| = k to a 135 | solution to the original problem with w_i \\in {0,1}. Ties between edges 136 | are broken arbitrarily. 137 | 138 | w: A solution in the feasible set for the relaxed problem 139 | k: The number of edges to select 140 | 141 | returns w': A solution in the feasible set for the original problem 142 | """ 143 | idx = np.argpartition(w, -k)[-k:] 144 | rounded = np.zeros(len(w)) 145 | if k > 0: 146 | rounded[idx] = 1.0 147 | return rounded 148 | 149 | def simple_random_round(self, w, k): 150 | """ 151 | Round a solution w to the relaxed problem, i.e. w \\in [0,1]^m, |w| = k to 152 | one with hard edge constraints and satisfying the constraint that the 153 | expected number of selected edges is equal to k. 154 | 155 | w: A solution in the feasible set for the relaxed problem 156 | k: The number of edges to select _in expectation_ 157 | 158 | returns w': A solution containing hard edge selections with an expected 159 | number of selected edges equal to k. 160 | """ 161 | x = np.zeros(len(w)) 162 | for i in range(len(w)): 163 | r = np.random.rand() 164 | if w[i] > r: 165 | x[i] = 1.0 166 | return x 167 | 168 | def round_solution_tiebreaker(self, w, k, decimal_tol=10): 169 | """ 170 | Round a solution w to the relaxed problem, i.e. w \\in [0,1]^m, |w| = k to a 171 | solution to the original problem with w_i \\in {0,1}. Ties between edges 172 | are broken based on the original weight (all else being equal, we 173 | prefer edges with larger weight). 174 | 175 | w: A solution in the feasible set for the relaxed problem 176 | k: The number of edges to select 177 | decimal_tol: tolerance for determining floating point equality of weights w 178 | 179 | returns w': A solution in the feasible set for the original problem 180 | """ 181 | truncated_w = w.round(decimals=decimal_tol) 182 | zipped_vals = np.array([(truncated_w[i], self.weights[i]) 183 | for i in range(len(w))], 184 | dtype=[('w', 'float'), ('weight', 'float')]) 185 | idx = np.argpartition(zipped_vals, -k, order=['w', 'weight'])[-k:] 186 | rounded = np.zeros(len(w)) 187 | if k > 0: 188 | rounded[idx] = 1.0 189 | return rounded 190 | 191 | def fw_subset(self, w_init, k, max_iters=5, duality_gap_tol=1e-8): 192 | """ 193 | Use the Frank-Wolfe method to solve the subset selection problem,. 194 | 195 | w_init: Array-like, must satisfy 0 <= w_i <= 1, |w| <= k 196 | k: size of max allowed subset 197 | max_iters: Maximum number of Frank-Wolfe iterations before returning 198 | duality_gap_tol: Minimum duality gap (for early stopping) 199 | 200 | returns a tuple (solution, unrounded, upper_bound) where 201 | solution: the (rounded) solution w \\in {0,1}^m |w| = k 202 | unrounded: the solution obtained prior to rounding 203 | upper_bound: the value of the dual at the last iteration 204 | """ 205 | u_i = float("inf") 206 | w_i = w_init 207 | zeros = np.zeros(len(w_init)) 208 | obj_prev = None 209 | for it in range(max_iters): 210 | # Compute gradient 211 | f_i, vec_i = self.evaluate_fiedler_pair(w_i) 212 | grad_i = self.grad_from_fiedler(vec_i) 213 | 214 | # Solve the direction-finding subproblem by maximizing the linear 215 | # approximation of f at w_i 216 | s_i = self.round_solution(grad_i, k) 217 | 218 | # Compute dual upper bound from linear approximation 219 | # f_i = self.evaluate_objective(w_i) 220 | u_i = min(u_i, f_i + grad_i @ (s_i - w_i)) 221 | 222 | # If the duality gap is sufficiently small, we are done 223 | if u_i - f_i < duality_gap_tol: 224 | print("Duality gap tolerance reached, found optimal solution") 225 | return self.round_solution_tiebreaker(w_i, k), w_i, u_i 226 | # return self.simple_random_round(w_i, k), w_i 227 | 228 | # Step size determination - naive method. No line search 229 | alpha = 2.0 / (it + 2.0) 230 | w_i = w_i + alpha * (s_i - w_i) 231 | 232 | print("Reached maximum iterations") 233 | return self.round_solution_tiebreaker(w_i, k), w_i, u_i 234 | -------------------------------------------------------------------------------- /cslam/mac/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.21.1 2 | networkx==2.7.1 3 | scipy>=1.8.0 4 | numba>=0.55.1 -------------------------------------------------------------------------------- /cslam/mac/utils.py: -------------------------------------------------------------------------------- 1 | # Modified by Pierre-Yves Lajoie (2022) 2 | 3 | import numpy as np 4 | from collections import namedtuple 5 | from scipy.sparse import csr_matrix, coo_matrix 6 | import networkx as nx 7 | from typing import List 8 | 9 | import numba 10 | from numba import jit 11 | 12 | # Define Edge container 13 | Edge = namedtuple('Edge', ['i', 'j', 'weight']) 14 | 15 | 16 | def nx_to_mac(G: nx.Graph) -> List[Edge]: 17 | """Returns the list of edges in the graph G 18 | 19 | Args: 20 | G (nx.Graph): the graph 21 | 22 | Returns: 23 | List[Edge]: the list of edges 24 | """ 25 | edges = [] 26 | for nxedge in G.edges(): 27 | edge = Edge(nxedge[0], nxedge[1], 1.0) 28 | edges.append(edge) 29 | return edges 30 | 31 | 32 | def mac_to_nx(edges: List[Edge]) -> nx.Graph: 33 | """returns the graph corresponding to the list of edges 34 | 35 | Args: 36 | edges (List[Edge]): the list of edges 37 | 38 | Returns: 39 | nx.Graph: the networkx graph 40 | """ 41 | G = nx.Graph() 42 | for edge in edges: 43 | G.add_edge(edge.i, edge.j, weight=edge.weight) 44 | return G 45 | 46 | 47 | def weight_graph_lap_from_edge_list(edges: List[Edge], 48 | num_vars: int) -> csr_matrix: 49 | """Returns the (sparse) weighted graph Laplacian matrix from the list of edges 50 | 51 | Args: 52 | edges (List[Edge]): the list of edges 53 | num_vars (int): the number of variables 54 | 55 | Returns: 56 | csr_matrix: the weighted graph Laplacian matrix 57 | """ 58 | # Preallocate triplets 59 | rows = [] 60 | cols = [] 61 | data = [] 62 | for edge in edges: 63 | # Diagonal elem (u,u) 64 | rows.append(edge.i) 65 | cols.append(edge.i) 66 | data.append(edge.weight) 67 | 68 | # Diagonal elem (v,v) 69 | rows.append(edge.j) 70 | cols.append(edge.j) 71 | data.append(edge.weight) 72 | 73 | # Off diagonal (u,v) 74 | rows.append(edge.i) 75 | cols.append(edge.j) 76 | data.append(-edge.weight) 77 | 78 | # Off diagonal (v,u) 79 | rows.append(edge.j) 80 | cols.append(edge.i) 81 | data.append(-edge.weight) 82 | 83 | return csr_matrix( 84 | coo_matrix((data, (rows, cols)), shape=[num_vars, num_vars])) 85 | 86 | 87 | def weight_graph_lap_from_edges(edges: List[Edge], weights: List[int], 88 | num_poses: int) -> csr_matrix: 89 | """Returns the sparse rotational weighted graph Laplacian matrix from a list 90 | of edges and edge weights 91 | 92 | Args: 93 | edges (List[Edge]): the list of edges 94 | weights (List[float]): the list of edge weights 95 | num_poses (int): the number of poses 96 | 97 | Returns: 98 | csr_matrix: the rotational weighted graph Laplacian matrix 99 | """ 100 | # Preallocate triplets 101 | rows = [] 102 | cols = [] 103 | data = [] 104 | for i in range(len(edges)): 105 | # Diagonal elem (u,u) 106 | rows.append(edges[i, 0]) 107 | cols.append(edges[i, 0]) 108 | data.append(weights[i]) 109 | 110 | # Diagonal elem (v,v) 111 | rows.append(edges[i, 1]) 112 | cols.append(edges[i, 1]) 113 | data.append(weights[i]) 114 | 115 | # Off diagonal (u,v) 116 | rows.append(edges[i, 0]) 117 | cols.append(edges[i, 1]) 118 | data.append(-weights[i]) 119 | 120 | # Off diagonal (v,u) 121 | rows.append(edges[i, 1]) 122 | cols.append(edges[i, 0]) 123 | data.append(-weights[i]) 124 | 125 | return csr_matrix( 126 | coo_matrix((data, (rows, cols)), shape=[num_poses, num_poses])) 127 | 128 | 129 | def split_measurements(measurements): 130 | """ 131 | Splits list of "measurements" and returns two lists: 132 | "odometry" - measurements where |i - j| = 1, and 133 | "loop closures" - measurements where |i - j| != 1 134 | """ 135 | odom_measurements = [] 136 | lc_measurements = [] 137 | for measurement in measurements: 138 | id1 = measurement.i 139 | id2 = measurement.j 140 | if abs(id2 - id1) > 1: 141 | lc_measurements.append(measurement) 142 | else: 143 | odom_measurements.append(measurement) 144 | 145 | return odom_measurements, lc_measurements 146 | 147 | 148 | def select_measurements(measurements, w): 149 | """ 150 | Select the subset of edges from `measurements` with weight equal to one in 151 | `w`. 152 | """ 153 | assert (len(measurements) == len(w)) 154 | meas_out = [] 155 | for i, meas in enumerate(measurements): 156 | if w[i] == 1.0: 157 | meas_out.append(meas) 158 | return meas_out 159 | -------------------------------------------------------------------------------- /cslam/neighbor_monitor.py: -------------------------------------------------------------------------------- 1 | from std_msgs.msg import UInt32 2 | from time import time 3 | 4 | class NeighborMonitor(): 5 | """Monitors if a neighboring robot is in range 6 | """ 7 | 8 | def __init__(self, node, rid, is_enabled, init_delay_sec, max_delay_sec): 9 | """Initialization 10 | Args: 11 | id (int): Robot ID 12 | """ 13 | self.node = node 14 | self.robot_id = rid 15 | self.is_enabled = is_enabled 16 | self.origin_robot_id = self.robot_id 17 | 18 | self.init_delay_sec = init_delay_sec 19 | self.max_delay_sec = max_delay_sec 20 | self.first_heartbeat_received = False 21 | self.init_time = time() 22 | self.latest_time_stamp = self.init_time 23 | self.last_keyframe_received = -1 24 | self.last_keyframe_sent = -1 25 | self.last_match_sent = -1 26 | 27 | self.heartbeat_subscriber = self.node.create_subscription( 28 | UInt32, '/r' + str(rid) + '/' + 'cslam/heartbeat', 29 | self.heartbeat_callback, 10) 30 | 31 | def heartbeat_callback(self, msg): 32 | """Callback to indicate that it is alive 33 | 34 | Args: 35 | msg (UInt32): 36 | """ 37 | self.origin_robot_id = msg.data 38 | self.latest_time_stamp = time() 39 | if not self.first_heartbeat_received: 40 | self.first_heartbeat_received = True 41 | self.init_time = time() 42 | 43 | def is_alive(self): 44 | """Check if it recently received a heartbeat signal 45 | 46 | Returns: 47 | bool: liveliness indicator 48 | """ 49 | if self.is_enabled: 50 | now = time() 51 | return self.first_heartbeat_received and now - self.init_time > self.init_delay_sec and now - self.latest_time_stamp < self.max_delay_sec 52 | else: 53 | True 54 | -------------------------------------------------------------------------------- /cslam/neighbors_manager.py: -------------------------------------------------------------------------------- 1 | from cslam.neighbor_monitor import NeighborMonitor 2 | from cslam_common_interfaces.msg import RobotIdsAndOrigin 3 | from std_msgs.msg import String 4 | 5 | import rclpy 6 | 7 | 8 | class NeighborManager(): 9 | """Manage neighbors to keep track of which other robots are in communication range 10 | """ 11 | def __init__(self, node, params): 12 | self.node = node 13 | self.params = params 14 | self.robot_id = self.params['robot_id'] 15 | self.max_nb_robots = self.params['max_nb_robots'] 16 | self.neighbors_monitors = {} 17 | for rid in range(self.max_nb_robots): 18 | if rid != self.robot_id: 19 | self.neighbors_monitors[rid] = NeighborMonitor( 20 | self.node, rid, self. 21 | params['neighbor_management.enable_neighbor_monitoring'], 22 | self.params['neighbor_management.init_delay_sec'], 23 | self.params['neighbor_management.max_heartbeat_delay_sec']) 24 | 25 | self.subscriber = self.node.create_subscription( 26 | String, 'cslam/get_current_neighbors', 27 | self.get_current_neighbors_callback, 100) 28 | self.neighbors_publisher = self.node.create_publisher( 29 | RobotIdsAndOrigin, 'cslam/current_neighbors', 100) 30 | 31 | def check_neighbors_in_range(self): 32 | """Check which neighbors are in range 33 | 34 | """ 35 | is_robot_in_range = {} 36 | robots_in_range_list = [] 37 | for i in range(self.max_nb_robots): 38 | if i == self.robot_id: 39 | is_robot_in_range[i] = True 40 | robots_in_range_list.append(i) 41 | elif self.neighbors_monitors[i].is_alive(): 42 | is_robot_in_range[i] = True 43 | robots_in_range_list.append(i) 44 | else: 45 | is_robot_in_range[i] = False 46 | return is_robot_in_range, robots_in_range_list 47 | 48 | def local_robot_is_broker(self): 49 | """This method check if the local robot (that runs this node), is the 50 | default broker based on its current neighbors. 51 | 52 | Returns: 53 | bool: is the local robot the default broker 54 | among the robots in range 55 | """ 56 | is_broker = True 57 | for i in range(self.max_nb_robots): 58 | if i != self.robot_id and self.neighbors_monitors[i].is_alive(): 59 | # Note: This is an arbitrary condition that selects the 60 | # lowest ID alive as broker. Could be change to any other cond. 61 | # (e.g., selecting the robot with the most computing power) 62 | if self.robot_id > i: 63 | is_broker = False 64 | return is_broker 65 | 66 | def select_from_which_kf_to_send(self, latest_local_id): 67 | """This function finds the range of descriptors to send 68 | so that we do not loose info 69 | """ 70 | 71 | from_kf_id = latest_local_id 72 | for i in range(self.max_nb_robots): 73 | if i != self.robot_id: 74 | if self.neighbors_monitors[i].is_alive(): 75 | from_kf_id = min( 76 | self.neighbors_monitors[i].last_keyframe_sent, 77 | from_kf_id) 78 | 79 | for i in range(self.max_nb_robots): 80 | if i != self.robot_id: 81 | if self.neighbors_monitors[i].is_alive(): 82 | self.neighbors_monitors[ 83 | i].last_keyframe_sent = latest_local_id 84 | 85 | return from_kf_id + 1 86 | 87 | def select_from_which_match_to_send(self, latest_local_match_idx): 88 | """This function finds the range of matches to send 89 | so that we do not loose info 90 | """ 91 | 92 | from_match_id = latest_local_match_idx 93 | for i in range(self.max_nb_robots): 94 | if i != self.robot_id: 95 | if self.neighbors_monitors[i].is_alive(): 96 | from_match_id = min( 97 | self.neighbors_monitors[i].last_match_sent, 98 | from_match_id) 99 | 100 | for i in range(self.max_nb_robots): 101 | if i != self.robot_id: 102 | if self.neighbors_monitors[i].is_alive(): 103 | self.neighbors_monitors[ 104 | i].last_match_sent = latest_local_match_idx 105 | 106 | return from_match_id + 1 107 | 108 | def useless_descriptors(self, last_kf_id): 109 | """Determines which descriptors are not useless and can be deleted 110 | 111 | Args: 112 | last_kf_id (int): last keyframe id in the list of descriptors 113 | Returns: 114 | int: the id of the first descriptor that is not useless 115 | """ 116 | from_kf_id = last_kf_id 117 | for i in range(self.max_nb_robots): 118 | if i != self.robot_id: 119 | from_kf_id = min(self.neighbors_monitors[i].last_keyframe_sent, 120 | from_kf_id) 121 | return from_kf_id 122 | 123 | def useless_matches(self, last_match_id): 124 | """Determines which matches are not useless and can be deleted 125 | 126 | Args: 127 | last_match_id (int): last match id in the list of matches 128 | Returns: 129 | int: the id of the first match that is not useless 130 | """ 131 | from_match_id = last_match_id 132 | for i in range(self.max_nb_robots): 133 | if i != self.robot_id: 134 | from_match_id = min(self.neighbors_monitors[i].last_match_sent, 135 | from_match_id) 136 | return from_match_id 137 | 138 | def update_received_kf_id(self, other_robot_id, kf_id): 139 | """Keep monitors up to date with received keyframes 140 | 141 | Args: 142 | other_robot_id (int): other robot id 143 | kf_id (int): keyframe id 144 | """ 145 | self.neighbors_monitors[other_robot_id].last_keyframe_received = kf_id 146 | 147 | def get_unknown_range(self, descriptors): 148 | """_summary_ 149 | 150 | Args: 151 | descriptors (list): list of descriptors with ids info 152 | 153 | Returns: 154 | range: indexes in list to process 155 | """ 156 | other_robot_id = descriptors[0].robot_id 157 | kf_ids = [d.keyframe_id for d in descriptors] 158 | last_id = max(kf_ids) 159 | 160 | list_index_range = [ 161 | i for i in range(len(descriptors)) if descriptors[i].keyframe_id > 162 | self.neighbors_monitors[other_robot_id].last_keyframe_received 163 | ] 164 | 165 | self.update_received_kf_id( 166 | other_robot_id, 167 | max(self.neighbors_monitors[other_robot_id].last_keyframe_received, 168 | last_id)) 169 | return list_index_range 170 | 171 | def get_current_neighbors_callback(self, msg): 172 | """Publish the current neighbors in range 173 | 174 | Args: 175 | msg (String): Empty 176 | """ 177 | is_robot_in_range, robots_in_range_list = self.check_neighbors_in_range( 178 | ) 179 | robots_in_range_list.remove(self.robot_id) 180 | msg = RobotIdsAndOrigin() 181 | msg.robots.ids = robots_in_range_list 182 | for i in robots_in_range_list: 183 | msg.origins.ids.append(self.neighbors_monitors[i].origin_robot_id) 184 | 185 | self.neighbors_publisher.publish(msg) 186 | -------------------------------------------------------------------------------- /cslam/nns_matching.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | from scipy.spatial import distance 4 | 5 | 6 | class NearestNeighborsMatching(object): 7 | """Nearest Neighbor matching of description vectors 8 | """ 9 | 10 | def __init__(self, dim=None): 11 | """Initialization 12 | 13 | Args: 14 | dim (int, optional): Global descriptor size. Defaults to None. 15 | """ 16 | self.n = 0 17 | self.dim = dim 18 | self.items = dict() 19 | self.data = [] 20 | if dim is not None: 21 | self.data = np.zeros((1000, dim), dtype='float32') 22 | 23 | def add_item(self, vector, item): 24 | """Add item to the matching list 25 | 26 | Args: 27 | vector (np.array): descriptor 28 | item: identification info (e.g., int) 29 | """ 30 | assert vector.ndim == 1 31 | if self.n >= len(self.data): 32 | if self.dim is None: 33 | self.dim = len(vector) 34 | self.data = np.zeros((1000, self.dim), dtype='float32') 35 | else: 36 | self.data.resize((2 * len(self.data), self.dim), 37 | refcheck=False) 38 | self.items[self.n] = item 39 | self.data[self.n] = vector 40 | self.n += 1 41 | 42 | def search(self, query, k): # searching from 100000 items consume 30ms 43 | """Search for nearest neighbors 44 | 45 | Args: 46 | query (np.array): descriptor to match 47 | k (int): number of best matches to return 48 | 49 | Returns: 50 | list(int, np.array): best matches 51 | """ 52 | if len(self.data) == 0: 53 | return [], [] 54 | 55 | similarities = np.zeros(self.n) 56 | 57 | for i in range(self.n): 58 | similarities[i] = 1 - distance.cosine(query, self.data[i,:].squeeze()) 59 | 60 | ns = np.argsort(similarities)[::-1][:k] 61 | return [self.items[n] for n in ns], similarities[ns] 62 | 63 | def search_best(self, query): 64 | """Search for the nearest neighbor 65 | 66 | Args: 67 | query (np.array): descriptor to match 68 | 69 | Returns: 70 | int, np.array: best match 71 | """ 72 | if len(self.data) == 0: 73 | return None, None 74 | 75 | items, similarities = self.search(query, 1) 76 | return items[0], similarities[0] -------------------------------------------------------------------------------- /cslam/utils/misc.py: -------------------------------------------------------------------------------- 1 | """" 2 | This file contains various utilitary functions 3 | for basic number and list operations 4 | """ 5 | 6 | def clamp(num, min_value, max_value): 7 | return max(min(num, max_value), min_value) 8 | 9 | def list_clamp(l, idx): 10 | i = clamp(idx, 0, len(l)-1) 11 | return l[i] 12 | 13 | def list_range(l, start): 14 | s = clamp(start, 0, len(l)-1) 15 | return [l[i] for i in range(s, len(l)-1)] 16 | 17 | def list_chunks(l, start, chunk_size): 18 | s = clamp(start, 0, len(l)-1) 19 | return [l[i:i+chunk_size] for i in range(s, len(l), chunk_size)] 20 | 21 | def dict_to_list_chunks(d, start, chunk_size): 22 | chunks = [] 23 | tmp = [] 24 | for k in d.keys(): 25 | if k >= start: 26 | tmp.append(d[k]) 27 | if len(tmp) == chunk_size: 28 | chunks.append(tmp) 29 | tmp = [] 30 | if len(tmp) > 0: 31 | chunks.append(tmp) 32 | 33 | return chunks -------------------------------------------------------------------------------- /cslam/vpr/cosplace.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import os 4 | from os.path import join, exists, isfile, realpath, dirname 5 | import torch 6 | import torch.nn as nn 7 | import torch.nn.functional as F 8 | import torch.optim as optim 9 | from torch.autograd import Variable 10 | from torch.utils.data import DataLoader, SubsetRandomSampler 11 | from torch.utils.data.dataset import Subset 12 | import torchvision.transforms as transforms 13 | from PIL import Image 14 | from datetime import datetime 15 | import torchvision.datasets as datasets 16 | import torchvision.models as models 17 | import numpy as np 18 | import sys 19 | import pickle 20 | import sklearn 21 | from sklearn.neighbors import NearestNeighbors 22 | from cslam.vpr.cosplace_utils.network import GeoLocalizationNet 23 | from ament_index_python.packages import get_package_share_directory 24 | 25 | IMAGENET_DEFAULT_MEAN = (0.485, 0.456, 0.406) 26 | IMAGENET_DEFAULT_STD = (0.229, 0.224, 0.225) 27 | 28 | 29 | class CosPlace(object): 30 | """CosPlace matcher 31 | """ 32 | 33 | def __init__(self, params, node): 34 | """Initialization 35 | 36 | Args: 37 | params (dict): parameters 38 | """ 39 | self.params = params 40 | self.node = node 41 | 42 | self.enable = self.params['frontend.nn_checkpoint'].lower( 43 | ) != 'disable' 44 | if self.enable: 45 | pkg_folder = get_package_share_directory("cslam") 46 | self.params['frontend.nn_checkpoint'] = join( 47 | pkg_folder, self.params['frontend.nn_checkpoint']) 48 | 49 | if torch.cuda.is_available(): 50 | self.device = torch.device("cuda") 51 | else: 52 | self.device = torch.device("cpu") 53 | 54 | self.descriptor_dim = self.params[ 55 | 'frontend.cosplace.descriptor_dim'] 56 | self.model = GeoLocalizationNet( 57 | self.params['frontend.cosplace.backbone'], self.descriptor_dim, 58 | node) 59 | 60 | resume_ckpt = self.params['frontend.nn_checkpoint'] 61 | if isfile(resume_ckpt): 62 | self.node.get_logger().info("loading checkpoint '{}'".format(resume_ckpt)) 63 | checkpoint = torch.load( 64 | resume_ckpt, map_location=lambda storage, loc: storage) 65 | 66 | self.model.load_state_dict(checkpoint) 67 | self.model = self.model.to(self.device) 68 | else: 69 | self.node.get_logger().error("Error: Checkpoint path is incorrect {}".format(resume_ckpt)) 70 | exit() 71 | 72 | self.model.eval() 73 | self.transform = transforms.Compose([ 74 | transforms.CenterCrop(self.params["frontend.image_crop_size"]), 75 | transforms.Resize(224, interpolation=3), 76 | transforms.ToTensor(), 77 | transforms.Normalize(IMAGENET_DEFAULT_MEAN, 78 | IMAGENET_DEFAULT_STD), 79 | ]) 80 | 81 | def compute_embedding(self, keyframe): 82 | """Load image to device and extract the global image descriptor 83 | 84 | Args: 85 | keyframe (image): image to match 86 | 87 | Returns: 88 | np.array: global image descriptor 89 | """ 90 | if self.enable: 91 | with torch.no_grad(): 92 | image = Image.fromarray(keyframe) 93 | input = self.transform(image) 94 | input = torch.unsqueeze(input, 0) 95 | input = input.to(self.device) 96 | 97 | image_encoding = self.model.forward(input) 98 | 99 | output = image_encoding[0].detach().cpu().numpy() 100 | del input, image_encoding, image 101 | return output 102 | else: 103 | # Random descriptor if disabled 104 | # Use this option only for testing 105 | return np.random.rand(self.descriptor_dim) 106 | -------------------------------------------------------------------------------- /cslam/vpr/cosplace_utils/README.md: -------------------------------------------------------------------------------- 1 | Third party code from: https://github.com/gmberton/cosplace 2 | Commit: 298a849 -------------------------------------------------------------------------------- /cslam/vpr/cosplace_utils/layers.py: -------------------------------------------------------------------------------- 1 | 2 | import torch 3 | import torch.nn as nn 4 | import torch.nn.functional as F 5 | from torch.nn.parameter import Parameter 6 | 7 | 8 | def gem(x, p=3, eps=1e-6): 9 | return F.avg_pool2d(x.clamp(min=eps).pow(p), (x.size(-2), x.size(-1))).pow(1./p) 10 | 11 | 12 | class GeM(nn.Module): 13 | def __init__(self, p=3, eps=1e-6): 14 | super().__init__() 15 | self.p = Parameter(torch.ones(1)*p) 16 | self.eps = eps 17 | def forward(self, x): 18 | return gem(x, p=self.p, eps=self.eps) 19 | def __repr__(self): 20 | return self.__class__.__name__ + '(' + 'p=' + '{:.4f}'.format(self.p.data.tolist()[0]) + ', ' + 'eps=' + str(self.eps) + ')' 21 | 22 | 23 | class Flatten(torch.nn.Module): 24 | def __init__(self): 25 | super().__init__() 26 | def forward(self, x): 27 | assert x.shape[2] == x.shape[3] == 1, f"{x.shape[2]} != {x.shape[3]} != 1" 28 | return x[:,:,0,0] 29 | 30 | 31 | class L2Norm(nn.Module): 32 | def __init__(self, dim=1): 33 | super().__init__() 34 | self.dim = dim 35 | def forward(self, x): 36 | return F.normalize(x, p=2, dim=self.dim) 37 | 38 | -------------------------------------------------------------------------------- /cslam/vpr/cosplace_utils/network.py: -------------------------------------------------------------------------------- 1 | 2 | import torch 3 | import logging 4 | import torchvision 5 | from torch import nn 6 | 7 | from cslam.vpr.cosplace_utils.layers import Flatten, L2Norm, GeM 8 | 9 | 10 | CHANNELS_NUM_IN_LAST_CONV = { 11 | "resnet18": 512, 12 | "resnet50": 2048, 13 | "resnet101": 2048, 14 | "resnet152": 2048, 15 | "vgg16": 512, 16 | } 17 | 18 | 19 | class GeoLocalizationNet(nn.Module): 20 | def __init__(self, backbone, fc_output_dim, node): 21 | super().__init__() 22 | self.backbone, features_dim = get_backbone(backbone) 23 | self.aggregation = nn.Sequential( 24 | L2Norm(), 25 | GeM(), 26 | Flatten(), 27 | nn.Linear(features_dim, fc_output_dim), 28 | L2Norm() 29 | ) 30 | self.node = node 31 | 32 | def forward(self, x): 33 | x = self.backbone(x) 34 | x = self.aggregation(x) 35 | return x 36 | 37 | 38 | def get_backbone(backbone_name): 39 | if backbone_name.startswith("resnet"): 40 | if backbone_name == "resnet18": 41 | backbone = torchvision.models.resnet18(pretrained=True) 42 | elif backbone_name == "resnet50": 43 | backbone = torchvision.models.resnet50(pretrained=True) 44 | elif backbone_name == "resnet101": 45 | backbone = torchvision.models.resnet101(pretrained=True) 46 | elif backbone_name == "resnet152": 47 | backbone = torchvision.models.resnet152(pretrained=True) 48 | 49 | for name, child in backbone.named_children(): 50 | if name == "layer3": # Freeze layers before conv_3 51 | break 52 | for params in child.parameters(): 53 | params.requires_grad = False 54 | logging.debug(f"Train only layer3 and layer4 of the {backbone_name}, freeze the previous ones") 55 | layers = list(backbone.children())[:-2] # Remove avg pooling and FC layer 56 | 57 | elif backbone_name == "vgg16": 58 | backbone = torchvision.models.vgg16(pretrained=True) 59 | layers = list(backbone.features.children())[:-2] # Remove avg pooling and FC layer 60 | for l in layers[:-5]: 61 | for p in l.parameters(): p.requires_grad = False 62 | logging.debug("Train last layers of the VGG-16, freeze the previous ones") 63 | 64 | backbone = torch.nn.Sequential(*layers) 65 | 66 | features_dim = CHANNELS_NUM_IN_LAST_CONV[backbone_name] 67 | 68 | return backbone, features_dim 69 | 70 | -------------------------------------------------------------------------------- /cslam/vpr/netvlad.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import os 4 | from os.path import join, exists, isfile, realpath, dirname 5 | import torch 6 | import torch.nn as nn 7 | import torch.nn.functional as F 8 | import torch.optim as optim 9 | from torch.autograd import Variable 10 | from torch.utils.data import DataLoader, SubsetRandomSampler 11 | from torch.utils.data.dataset import Subset 12 | import torchvision.transforms as transforms 13 | from PIL import Image 14 | from datetime import datetime 15 | import torchvision.datasets as datasets 16 | import torchvision.models as models 17 | import numpy as np 18 | import sys 19 | import pickle 20 | import sklearn 21 | from sklearn.neighbors import NearestNeighbors 22 | from ament_index_python.packages import get_package_share_directory 23 | 24 | IMAGENET_DEFAULT_MEAN = (0.485, 0.456, 0.406) 25 | IMAGENET_DEFAULT_STD = (0.229, 0.224, 0.225) 26 | 27 | 28 | class NetVLADLayer(nn.Module): 29 | """ NetVLAD layer implementation 30 | based on https://github.com/lyakaap/NetVLAD-pytorch/blob/master/netvlad.py 31 | """ 32 | 33 | def __init__(self, 34 | num_clusters=64, 35 | dim=128, 36 | normalize_input=True, 37 | vladv2=False): 38 | """ 39 | Args: 40 | num_clusters : int 41 | The number of clusters 42 | dim : int 43 | Dimension of descriptors 44 | alpha : float 45 | Parameter of initialization. Larger value is harder assignment. 46 | normalize_input : bool 47 | If true, descriptor-wise L2 normalization is applied to input. 48 | vladv2 : bool 49 | If true, use vladv2 otherwise use vladv1 50 | """ 51 | super(NetVLADLayer, self).__init__() 52 | self.num_clusters = num_clusters 53 | self.dim = dim 54 | self.alpha = 0 55 | self.vladv2 = vladv2 56 | self.normalize_input = normalize_input 57 | self.conv = nn.Conv2d(dim, 58 | num_clusters, 59 | kernel_size=(1, 1), 60 | bias=vladv2) 61 | self.centroids = nn.Parameter(torch.rand(num_clusters, dim)) 62 | 63 | def init_params(self, clsts, traindescs): 64 | if self.vladv2 == False: 65 | clstsAssign = clsts / np.linalg.norm(clsts, axis=1, keepdims=True) 66 | dots = np.dot(clstsAssign, traindescs.T) 67 | dots.sort(0) 68 | dots = dots[::-1, :] # sort, descending 69 | 70 | self.alpha = (-np.log(0.01) / 71 | np.mean(dots[0, :] - dots[1, :])).item() 72 | self.centroids = nn.Parameter(torch.from_numpy(clsts)) 73 | self.conv.weight = nn.Parameter( 74 | torch.from_numpy(self.alpha * 75 | clstsAssign).unsqueeze(2).unsqueeze(3)) 76 | self.conv.bias = None 77 | else: 78 | knn = NearestNeighbors(n_jobs=-1) 79 | knn.fit(traindescs) 80 | del traindescs 81 | dsSq = np.square(knn.kneighbors(clsts, 2)[1]) 82 | del knn 83 | self.alpha = (-np.log(0.01) / 84 | np.mean(dsSq[:, 1] - dsSq[:, 0])).item() 85 | self.centroids = nn.Parameter(torch.from_numpy(clsts)) 86 | del clsts, dsSq 87 | 88 | self.conv.weight = nn.Parameter( 89 | (2.0 * self.alpha * 90 | self.centroids).unsqueeze(-1).unsqueeze(-1)) 91 | self.conv.bias = nn.Parameter(-self.alpha * 92 | self.centroids.norm(dim=1)) 93 | 94 | def forward(self, x): 95 | """Forward pass through the NetVLAD network 96 | 97 | Args: 98 | x (image): image to match 99 | 100 | Returns: 101 | torch array: Global image descriptor 102 | """ 103 | N, C = x.shape[:2] 104 | 105 | if self.normalize_input: 106 | x = F.normalize(x, p=2, dim=1) # across descriptor dim 107 | 108 | # soft-assignment 109 | soft_assign = self.conv(x).view(N, self.num_clusters, -1) 110 | soft_assign = F.softmax(soft_assign, dim=1) 111 | 112 | x_flatten = x.view(N, C, -1) 113 | 114 | # calculate residuals to each clusters 115 | vlad = torch.zeros([N, self.num_clusters, C], 116 | dtype=x.dtype, 117 | layout=x.layout, 118 | device=x.device) 119 | for C in range(self.num_clusters 120 | ): # slower than non-looped, but lower memory usage 121 | residual = x_flatten.unsqueeze(0).permute(1, 0, 2, 3) - \ 122 | self.centroids[C:C+1, :].expand(x_flatten.size(-1), -1, -1).permute(1, 2, 0).unsqueeze(0) 123 | residual *= soft_assign[:, C:C + 1, :].unsqueeze(2) 124 | vlad[:, C:C + 1, :] = residual.sum(dim=-1) 125 | 126 | vlad = F.normalize(vlad, p=2, dim=2) # intra-normalization 127 | vlad = vlad.view(x.size(0), -1) # flatten 128 | vlad = F.normalize(vlad, p=2, dim=1) # L2 normalize 129 | 130 | return vlad 131 | 132 | 133 | class NetVLAD(object): 134 | """NetVLAD matcher 135 | """ 136 | 137 | def __init__(self, params, node): 138 | """Initialization 139 | 140 | Args: 141 | params (dict): parameters 142 | """ 143 | self.params = params 144 | self.node = node 145 | 146 | self.enable = self.params['frontend.nn_checkpoint'].lower( 147 | ) != 'disable' 148 | if self.enable: 149 | pkg_folder = get_package_share_directory("cslam") 150 | self.params['frontend.nn_checkpoint'] = join( 151 | pkg_folder, self.params['frontend.nn_checkpoint']) 152 | self.params['frontend.netvlad.pca_checkpoint'] = join( 153 | pkg_folder, 154 | self.node.get_parameter( 155 | 'frontend.netvlad.pca_checkpoint').value) 156 | 157 | if torch.cuda.is_available(): 158 | self.device = torch.device("cuda") 159 | else: 160 | self.device = torch.device("cpu") 161 | 162 | encoder_dim = 512 163 | encoder = models.vgg16(pretrained=True) 164 | # capture only feature part and remove last relu and maxpool 165 | layers = list(encoder.features.children())[:-2] 166 | # if using pretrained then only train conv5_1, conv5_2, and conv5_3 167 | for l in layers[:-5]: 168 | for p in l.parameters(): 169 | p.requires_grad = False 170 | 171 | encoder = nn.Sequential(*layers) 172 | self.model = nn.Module() 173 | self.model.add_module('encoder', encoder) 174 | netvlad_layer = NetVLADLayer(num_clusters=64, 175 | dim=encoder_dim, 176 | vladv2=False) 177 | self.model.add_module('pool', netvlad_layer) 178 | 179 | self.isParallel = False 180 | print('=> Number of CUDA devices = ' + 181 | str(torch.cuda.device_count())) 182 | if torch.cuda.device_count() > 1: 183 | self.model.encoder = nn.DataParallel(self.model.encoder) 184 | self.model.pool = nn.DataParallel(self.model.pool) 185 | self.isParallel = True 186 | 187 | resume_ckpt = self.params['frontend.nn_checkpoint'] 188 | if isfile(resume_ckpt): 189 | print("=> loading checkpoint '{}'".format(resume_ckpt)) 190 | checkpoint = torch.load( 191 | resume_ckpt, map_location=lambda storage, loc: storage) 192 | start_epoch = checkpoint['epoch'] 193 | best_metric = checkpoint['best_score'] 194 | self.model.load_state_dict(checkpoint['state_dict']) 195 | self.model = self.model.to(self.device) 196 | print("=> loaded checkpoint '{}' (epoch {})".format( 197 | resume_ckpt, checkpoint['epoch'])) 198 | else: 199 | print("Error: Checkpoint path is incorrect") 200 | 201 | self.model.eval() 202 | self.transform = transforms.Compose([ 203 | transforms.CenterCrop(self.params["frontend.image_crop_size"]), 204 | transforms.Resize(224, interpolation=3), 205 | transforms.ToTensor(), 206 | transforms.Normalize(IMAGENET_DEFAULT_MEAN, 207 | IMAGENET_DEFAULT_STD), 208 | ]) 209 | self.pca = pickle.load( 210 | open(self.params['frontend.netvlad.pca_checkpoint'], 'rb')) 211 | 212 | def compute_embedding(self, keyframe): 213 | """Load image to device and extract the global image descriptor 214 | 215 | Args: 216 | keyframe (image): image to match 217 | 218 | Returns: 219 | np.array: global image descriptor 220 | """ 221 | if self.enable: 222 | with torch.no_grad(): 223 | image = Image.fromarray(keyframe) 224 | input = self.transform(image) 225 | input = torch.unsqueeze(input, 0) 226 | input = input.to(self.device) 227 | image_encoding = self.model.encoder(input) 228 | vlad_encoding = self.model.pool(image_encoding) 229 | 230 | # Compute NetVLAD 231 | embedding = vlad_encoding.detach().cpu().numpy() 232 | 233 | # Run PCA transform 234 | reduced_embedding = self.pca.transform(embedding) 235 | normalized_embedding = sklearn.preprocessing.normalize( 236 | reduced_embedding) 237 | output = normalized_embedding[0] 238 | 239 | del input, image_encoding, vlad_encoding, reduced_embedding, normalized_embedding, image 240 | 241 | return output 242 | else: 243 | # Random descriptor if disabled 244 | # Use this option only for testing 245 | return np.random.rand(128) 246 | -------------------------------------------------------------------------------- /include/cslam/back_end/gtsam_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef GTSAMMSGCONVERSION_H_ 2 | #define GTSAMMSGCONVERSION_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #define GRAPH_LABEL 'g' 21 | #define ROBOT_LABEL(id) ('A' + id) 22 | #define ROBOT_ID(ch) ((unsigned int)(ch - 'A')) 23 | 24 | namespace cslam { 25 | 26 | /** 27 | * @brief Converts odometry message to gtsam::Pose3 28 | * 29 | * @param odom_msg Odometry message 30 | * @return pose Pose data 31 | */ 32 | gtsam::Pose3 odometry_msg_to_pose3(const nav_msgs::msg::Odometry &odom_msg); 33 | 34 | /** 35 | * @brief Converts a transform msg into a gtsam::Pose3 36 | * 37 | * @param msg Transform message 38 | * @return pose Pose data 39 | */ 40 | gtsam::Pose3 transform_msg_to_pose3(const geometry_msgs::msg::Transform &msg); 41 | 42 | /** 43 | * @brief Converts from GTSAM to ROS 2 message 44 | * 45 | * @param pose 46 | * @return geometry_msgs::msg::Pose 47 | */ 48 | geometry_msgs::msg::Pose gtsam_pose_to_msg(const gtsam::Pose3 &pose); 49 | 50 | /** 51 | * @brief Converts from GTSAM to ROS 2 message 52 | * 53 | * @param pose 54 | * @return geometry_msgs::msg::Transform 55 | */ 56 | geometry_msgs::msg::Transform 57 | gtsam_pose_to_transform_msg(const gtsam::Pose3 &pose); 58 | 59 | /** 60 | * @brief Converts from GTSAM to ROS 2 message 61 | * 62 | * @param values 63 | * @return std::vector 64 | */ 65 | std::vector 66 | gtsam_values_to_msg(const gtsam::Values::shared_ptr values); 67 | 68 | /** 69 | * @brief Converts from GTSAM to ROS 2 message 70 | * 71 | * @param factors 72 | * @return std::vector 73 | */ 74 | std::vector 75 | gtsam_factors_to_msg(const gtsam::NonlinearFactorGraph::shared_ptr factors); 76 | 77 | /** 78 | * @brief Converts from GTSAM to ROS 2 message 79 | * 80 | * @param values 81 | * @return std::vector 82 | */ 83 | std::vector 84 | gtsam_values_to_msg(const gtsam::Values &values); 85 | 86 | /** 87 | * @brief Converts from GTSAM to ROS 2 message 88 | * 89 | * @param factors 90 | * @return std::vector 91 | */ 92 | std::vector 93 | gtsam_factors_to_msg(const gtsam::NonlinearFactorGraph &factors); 94 | 95 | /** 96 | * @brief Converts from ROS 2 message to GTSAM 97 | * 98 | * @param pose 99 | * @return gtsam::Pose3 100 | */ 101 | gtsam::Pose3 pose_msg_to_gtsam(const geometry_msgs::msg::Pose &pose); 102 | 103 | /** 104 | * @brief Converts from ROS 2 message to GTSAM 105 | * 106 | * @param values 107 | * @return gtsam::Values::shared_ptr 108 | */ 109 | gtsam::Values::shared_ptr values_msg_to_gtsam( 110 | const std::vector &values); 111 | 112 | /** 113 | * @brief Converts from ROS 2 message to GTSAM 114 | * 115 | * @param edges 116 | * @return gtsam::NonlinearFactorGraph::shared_ptr 117 | */ 118 | gtsam::NonlinearFactorGraph::shared_ptr edges_msg_to_gtsam( 119 | const std::vector &edges); 120 | 121 | } // namespace cslam 122 | 123 | #endif -------------------------------------------------------------------------------- /include/cslam/back_end/utils/logger.h: -------------------------------------------------------------------------------- 1 | #ifndef _CSLAM_LOGGER_H_ 2 | #define _CSLAM_LOGGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 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 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | namespace cslam 31 | { 32 | 33 | class Logger 34 | { 35 | /** 36 | * @brief Logger class to log various metrics 37 | * 38 | */ 39 | public: 40 | Logger(std::shared_ptr &node, const unsigned int &robot_id, const unsigned int &max_nb_robots, const std::string &log_folder); 41 | 42 | void add_pose_graph_log_info(const cslam_common_interfaces::msg::PoseGraph &msg); 43 | 44 | void log_initial_global_pose_graph(const gtsam::NonlinearFactorGraph::shared_ptr &graph, 45 | const gtsam::Values::shared_ptr &initial); 46 | 47 | void log_optimized_global_pose_graph(const gtsam::NonlinearFactorGraph::shared_ptr &graph, 48 | const gtsam::Values &result, 49 | const unsigned int &origin_robot_id); 50 | 51 | void start_timer(); 52 | 53 | void stop_timer(); 54 | 55 | void write_logs(); 56 | 57 | /** 58 | * @brief Receive log messages 59 | * 60 | * @param msg 61 | */ 62 | void log_callback(const diagnostic_msgs::msg::KeyValue::ConstSharedPtr msg); 63 | 64 | /** 65 | * @brief Receive log messages 66 | * 67 | * @param msg 68 | */ 69 | void log_matches_callback(const cslam_common_interfaces::msg::InterRobotMatches::ConstSharedPtr msg); 70 | 71 | void fill_msg(cslam_common_interfaces::msg::PoseGraph & msg); 72 | 73 | void log_pose_timestamp(const gtsam::LabeledSymbol & symbol, const int& sec, const int& nanosec); 74 | 75 | private: 76 | 77 | double compute_error(const gtsam::NonlinearFactorGraph::shared_ptr &graph, 78 | const gtsam::Values::shared_ptr &result); 79 | 80 | std::vector, double>> compute_inter_robot_loop_closure_errors(const gtsam::NonlinearFactorGraph::shared_ptr &graph, 81 | const gtsam::Values::shared_ptr &result); 82 | 83 | std::shared_ptr node_; 84 | 85 | std::string log_folder_; 86 | unsigned int robot_id_, origin_robot_id_, max_nb_robots_; 87 | std::chrono::steady_clock::time_point start_time_; 88 | uint64_t elapsed_time_, total_pgo_time_; 89 | 90 | std::map> gps_values_; 91 | std::pair initial_global_pose_graph_; 92 | std::pair optimized_global_pose_graph_; 93 | std::vector pose_graphs_log_info_; 94 | 95 | rclcpp::Subscription::SharedPtr 96 | logger_subscriber_; 97 | 98 | rclcpp::Subscription::SharedPtr 99 | inter_robot_matches_subscriber_; 100 | 101 | cslam_common_interfaces::msg::InterRobotMatches spectral_matches_; 102 | 103 | unsigned int log_nb_matches_, log_nb_failed_matches_, log_nb_vertices_transmitted_, log_nb_matches_selected_, log_detection_cumulative_communication_, log_local_descriptors_cumulative_communication_; 104 | float log_sparsification_cumulative_computation_time_; 105 | 106 | std::map> pose_time_map_; 107 | }; 108 | 109 | } // namespace cslam 110 | 111 | #endif // _CSLAM_LOGGER_H_ -------------------------------------------------------------------------------- /include/cslam/back_end/utils/simulated_rendezvous.h: -------------------------------------------------------------------------------- 1 | #ifndef _CSLAM_SIMULATED_RENDEZVOUS_H_ 2 | #define _CSLAM_SIMULATED_RENDEZVOUS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace cslam 16 | { 17 | 18 | class SimulatedRendezVous 19 | { 20 | /** 21 | * @brief Rendez-vous simulation. 22 | * Reads a config file indicating when the robot should be considered alive 23 | * 24 | */ 25 | public: 26 | SimulatedRendezVous(std::shared_ptr &node, const std::string& schedule_file, const unsigned int &robot_id); 27 | 28 | /** 29 | * @brief Check if the robot is alive (aka in the rendez-vous schedule) 30 | * 31 | * @return is alive flag 32 | */ 33 | bool is_alive(); 34 | 35 | private: 36 | std::shared_ptr node_; 37 | unsigned int robot_id_; 38 | std::vector> rendezvous_ranges_; 39 | bool enabled_; 40 | }; 41 | 42 | } // namespace cslam 43 | 44 | #endif // _CSLAM_SIMULATED_RENDEZVOUS_H_ -------------------------------------------------------------------------------- /include/cslam/front_end/map_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAPMANAGER_H_ 2 | #define _MAPMANAGER_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "cslam/front_end/stereo_handler.h" 9 | #include "cslam/front_end/rgbd_handler.h" 10 | 11 | namespace cslam { 12 | 13 | /** 14 | * @brief Map management interface class 15 | * 16 | */ 17 | class IMapManager { 18 | public: 19 | virtual ~IMapManager() {}; 20 | }; 21 | 22 | /** 23 | * @brief Loop Closure Detection Management 24 | * - Receives keyframes from RTAB-map 25 | * - Generate keypoints from frames 26 | * - Sends/Receives keypoints from other robot frames 27 | * - Computes geometric verification 28 | * 29 | * @tparam DataHandlerType Depends on the type of input data (stereo, rgbd, 30 | * etc.) 31 | */ 32 | template class MapManager: public IMapManager { 33 | public: 34 | /** 35 | * @brief Initialization of parameters and ROS 2 objects 36 | * 37 | * @param node ROS 2 node handle 38 | */ 39 | MapManager(std::shared_ptr &node); 40 | ~MapManager(){}; 41 | 42 | /** 43 | * @brief Looks for loop closures in the current keyframe queue 44 | * 45 | */ 46 | void process_new_sensor_data(); 47 | 48 | private: 49 | std::shared_ptr node_; 50 | 51 | unsigned int max_nb_robots_, robot_id_; 52 | 53 | int map_manager_process_period_ms_; 54 | 55 | DataHandlerType local_data_handler_; 56 | 57 | rclcpp::TimerBase::SharedPtr process_timer_; 58 | }; 59 | 60 | // List possible data types for C++ linker 61 | template class MapManager; 62 | template class MapManager; 63 | } // namespace cslam 64 | #endif -------------------------------------------------------------------------------- /include/cslam/front_end/rgbd_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef _RGBDHANDLER_H_ 2 | #define _RGBDHANDLER_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 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 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #ifdef PRE_ROS_IRON 30 | #include 31 | #else 32 | #include 33 | #endif 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | 53 | #include 54 | #include "cslam/front_end/sensor_handler_interface.h" 55 | #include "cslam/front_end/visualization_utils.h" 56 | 57 | namespace cslam 58 | { 59 | 60 | class RGBDHandler : public ISensorHandler 61 | { 62 | public: 63 | /** 64 | * @brief Initialization of parameters and ROS 2 objects 65 | * 66 | * @param node ROS 2 node handle 67 | */ 68 | RGBDHandler(std::shared_ptr &node); 69 | ~RGBDHandler(){}; 70 | 71 | /** 72 | * @brief Processes Latest received image 73 | * 74 | */ 75 | virtual void process_new_sensor_data(); 76 | 77 | /** 78 | * @brief Service callback to publish local descriptors 79 | * 80 | * @param request Image ID to send and matching info 81 | */ 82 | void local_descriptors_request( 83 | cslam_common_interfaces::msg::LocalDescriptorsRequest:: 84 | ConstSharedPtr request); 85 | 86 | /** 87 | * @brief Receives a local match and tries to compute a local loop closure 88 | * 89 | * @param msg 90 | */ 91 | void receive_local_keyframe_match( 92 | cslam_common_interfaces::msg::LocalKeyframeMatch::ConstSharedPtr 93 | msg); 94 | 95 | /** 96 | * @brief Message callback to receive descriptors and compute 97 | * 98 | * @param msg local descriptors 99 | */ 100 | void receive_local_image_descriptors( 101 | const std::shared_ptr< 102 | cslam_common_interfaces::msg::LocalImageDescriptors> 103 | msg); 104 | 105 | /** 106 | * @brief Computes local 3D descriptors from frame data and store them 107 | * 108 | * @param frame_data Full frame data 109 | */ 110 | void 111 | compute_local_descriptors(std::shared_ptr &frame_data); 112 | 113 | /** 114 | * @brief converts descriptors to sensore data 115 | * 116 | * @param msg local descriptors 117 | * @return rtabmap::SensorData& 118 | */ 119 | virtual void local_descriptors_msg_to_sensor_data( 120 | const std::shared_ptr< 121 | cslam_common_interfaces::msg::LocalImageDescriptors> 122 | msg, 123 | rtabmap::SensorData &sensor_data); 124 | 125 | /** 126 | * @brief converts sensor data to descriptor msg 127 | * 128 | * @param sensor_data local descriptors 129 | * @param msg_data rtabmap_msgs::msg::RGBDImage& 130 | */ 131 | void sensor_data_to_rgbd_msg( 132 | const std::shared_ptr sensor_data, 133 | rtabmap_msgs::msg::RGBDImage &msg_data); 134 | 135 | /** 136 | * @brief Generate a new keyframe according to the policy 137 | * 138 | * @param keyframe Sensor data 139 | * @return true A new keyframe is added to the map 140 | * @return false The frame is rejected 141 | */ 142 | bool generate_new_keyframe(std::shared_ptr &keyframe); 143 | 144 | /** 145 | * @brief Function to send the image to the python node 146 | * 147 | * @param keypoints_data keyframe keypoints data 148 | */ 149 | void send_keyframe(const std::pair, std::shared_ptr> &keypoints_data); 150 | 151 | /** 152 | * @brief Function to send the image to the python node 153 | * 154 | * @param keypoints_data keyframe keypoints data 155 | * @param gps_data GPS data 156 | */ 157 | void send_keyframe(const std::pair, std::shared_ptr> &keypoints_data, const sensor_msgs::msg::NavSatFix& gps_data); 158 | 159 | /** 160 | * @brief Send keypoints for visualizations 161 | * 162 | * @param keypoints_data keyframe keypoints data 163 | */ 164 | virtual void send_visualization(const std::pair, std::shared_ptr> &keypoints_data); 165 | 166 | /** 167 | * @brief Send keypoints for visualizations 168 | * 169 | * @param keypoints_data keyframe keypoints data 170 | */ 171 | void send_visualization_keypoints(const std::pair, std::shared_ptr> &keypoints_data); 172 | 173 | /** 174 | * @brief Send colored pointcloud for visualizations 175 | * 176 | * @param sensor_data RGBD image 177 | */ 178 | void send_visualization_pointcloud(const std::shared_ptr &sensor_data); 179 | 180 | /** 181 | * @brief Callback receiving sync data from camera 182 | * 183 | * @param image_rgb 184 | * @param image_depth 185 | * @param camera_info_rgb 186 | * @param camera_info_depth 187 | * @param odom 188 | */ 189 | void rgbd_callback( 190 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_rgb, 191 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_depth, 192 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_rgb, 193 | const nav_msgs::msg::Odometry::ConstSharedPtr odom); 194 | 195 | /** 196 | * @brief Clear images and large data fields in sensor data 197 | * 198 | * @param sensor_data frame data 199 | */ 200 | void clear_sensor_data(std::shared_ptr &sensor_data); 201 | 202 | /** 203 | * @brief GPS data callback 204 | * 205 | * @param msg 206 | */ 207 | void gps_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg); 208 | 209 | /** 210 | * @brief Subsample pointcloud to reduce size for visualization 211 | * 212 | * @param input_cloud 213 | * @return pcl::PointCloud& 214 | */ 215 | sensor_msgs::msg::PointCloud2 visualization_pointcloud_voxel_subsampling( 216 | const sensor_msgs::msg::PointCloud2 &input_cloud); 217 | 218 | protected: 219 | std::deque, 220 | nav_msgs::msg::Odometry::ConstSharedPtr>> 221 | received_data_queue_; 222 | 223 | std::shared_ptr previous_keyframe_; 224 | 225 | std::map> local_descriptors_map_; 226 | 227 | std::shared_ptr node_; 228 | 229 | unsigned int min_inliers_, max_nb_robots_, robot_id_, max_queue_size_, 230 | nb_local_keyframes_; 231 | 232 | message_filters::Subscriber sub_odometry_; 233 | 234 | rclcpp::Subscription< 235 | cslam_common_interfaces::msg::LocalDescriptorsRequest>::SharedPtr 236 | send_local_descriptors_subscriber_; 237 | 238 | rclcpp::Publisher< 239 | cslam_common_interfaces::msg::LocalImageDescriptors>::SharedPtr 240 | local_descriptors_publisher_, 241 | visualization_local_descriptors_publisher_; 242 | 243 | rclcpp::Publisher::SharedPtr 244 | keyframe_data_publisher_; 245 | 246 | rclcpp::Publisher::SharedPtr 247 | keyframe_odom_publisher_; 248 | 249 | rclcpp::Publisher::SharedPtr 250 | keyframe_pointcloud_publisher_; 251 | 252 | rclcpp::Subscription< 253 | cslam_common_interfaces::msg::LocalKeyframeMatch>::SharedPtr 254 | local_keyframe_match_subscriber_; 255 | 256 | rclcpp::Subscription< 257 | cslam_common_interfaces::msg::LocalImageDescriptors>::SharedPtr 258 | local_descriptors_subscriber_; 259 | 260 | rtabmap::RegistrationVis registration_; 261 | 262 | rclcpp::Publisher< 263 | cslam_common_interfaces::msg::InterRobotLoopClosure>::SharedPtr 264 | inter_robot_loop_closure_publisher_; 265 | 266 | rclcpp::Publisher< 267 | cslam_common_interfaces::msg::IntraRobotLoopClosure>::SharedPtr 268 | intra_robot_loop_closure_publisher_; 269 | 270 | rclcpp::Publisher< 271 | diagnostic_msgs::msg::KeyValue>::SharedPtr 272 | log_publisher_; 273 | unsigned int log_total_local_descriptors_cumulative_communication_; 274 | bool enable_logs_; 275 | 276 | std::shared_ptr 277 | tf_buffer_; 278 | std::shared_ptr tf_listener_; 279 | 280 | std::string base_frame_id_; 281 | float keyframe_generation_ratio_threshold_; 282 | bool generate_new_keyframes_based_on_inliers_ratio_; 283 | 284 | unsigned int visualization_period_ms_; 285 | bool enable_visualization_; 286 | float visualization_voxel_size_, visualization_max_range_; 287 | 288 | bool enable_gps_recording_; 289 | std::string gps_topic_; 290 | rclcpp::Subscription::SharedPtr gps_subscriber_; 291 | sensor_msgs::msg::NavSatFix latest_gps_fix_; 292 | std::deque 293 | received_gps_queue_; 294 | 295 | private: 296 | image_transport::SubscriberFilter sub_image_color_; 297 | message_filters::Subscriber sub_camera_info_color_; 298 | image_transport::SubscriberFilter sub_image_depth_; 299 | message_filters::Subscriber sub_camera_info_depth_; 300 | typedef message_filters::sync_policies::ApproximateTime< 301 | sensor_msgs::msg::Image, sensor_msgs::msg::Image, 302 | sensor_msgs::msg::CameraInfo, 303 | nav_msgs::msg::Odometry> 304 | RGBDSyncPolicy; 305 | message_filters::Synchronizer *rgbd_sync_policy_; 306 | 307 | sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_; 308 | }; 309 | } // namespace cslam 310 | #endif 311 | -------------------------------------------------------------------------------- /include/cslam/front_end/sensor_handler_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef _ISENSORHANDLER_H_ 2 | #define _ISENSORHANDLER_H_ 3 | 4 | namespace cslam 5 | { 6 | /** 7 | * @brief Interface class for handling sensor data 8 | * 9 | */ 10 | class ISensorHandler 11 | { 12 | public: 13 | /** 14 | * @brief Virtual destructor 15 | */ 16 | virtual ~ISensorHandler() {}; 17 | 18 | /** 19 | * @brief Process new data callback 20 | * 21 | */ 22 | virtual void process_new_sensor_data() = 0; 23 | }; 24 | } // namespace cslam 25 | #endif -------------------------------------------------------------------------------- /include/cslam/front_end/stereo_handler.h: -------------------------------------------------------------------------------- 1 | #ifndef _STEREOHANDLER_H_ 2 | #define _STEREOHANDLER_H_ 3 | 4 | #include "cslam/front_end/rgbd_handler.h" 5 | 6 | namespace cslam 7 | { 8 | 9 | class StereoHandler : public RGBDHandler 10 | { 11 | public: 12 | /** 13 | * @brief Initialization of parameters and ROS 2 objects 14 | * 15 | * @param node ROS 2 node handle 16 | */ 17 | StereoHandler(std::shared_ptr &node); 18 | ~StereoHandler(){}; 19 | 20 | /** 21 | * @brief Callback receiving sync data from camera 22 | * 23 | * @param image_rect_left 24 | * @param image_rect_right 25 | * @param camera_info_left 26 | * @param camera_info_right 27 | * @param odom 28 | */ 29 | void stereo_callback( 30 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_left, 31 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_right, 32 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_left, 33 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_right, 34 | const nav_msgs::msg::Odometry::ConstSharedPtr odom); 35 | 36 | /** 37 | * @brief converts descriptors to sensore data 38 | * 39 | * @param msg local descriptors 40 | * @return rtabmap::SensorData& 41 | */ 42 | virtual void local_descriptors_msg_to_sensor_data( 43 | const std::shared_ptr msg, 44 | rtabmap::SensorData &sensor_data); 45 | 46 | /** 47 | * @brief Send keypoints for visualizations 48 | * 49 | * @param keypoints_data keyframe keypoints data 50 | */ 51 | virtual void send_visualization(const std::pair, std::shared_ptr> &keypoints_data); 52 | 53 | private: 54 | image_transport::SubscriberFilter sub_image_rect_left_; 55 | image_transport::SubscriberFilter sub_image_rect_right_; 56 | message_filters::Subscriber sub_camera_info_left_; 57 | message_filters::Subscriber sub_camera_info_right_; 58 | typedef message_filters::sync_policies::ApproximateTime< 59 | sensor_msgs::msg::Image, sensor_msgs::msg::Image, 60 | sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo, 61 | nav_msgs::msg::Odometry> 62 | StereoSyncPolicy; 63 | message_filters::Synchronizer *stereo_sync_policy_; 64 | }; 65 | } // namespace cslam 66 | #endif -------------------------------------------------------------------------------- /include/cslam/front_end/utils/depth_traits.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | #ifndef DEPTH_IMAGE_PROC_DEPTH_TRAITS 35 | #define DEPTH_IMAGE_PROC_DEPTH_TRAITS 36 | 37 | // From https://github.com/ros-perception/image_pipeline/tree/foxy/depth_image_proc 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace depth_image_proc { 46 | 47 | // Encapsulate differences between processing float and uint16_t depths 48 | template struct DepthTraits {}; 49 | 50 | template<> 51 | struct DepthTraits 52 | { 53 | static inline bool valid(uint16_t depth) { return depth != 0; } 54 | static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm 55 | static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; } 56 | }; 57 | 58 | template<> 59 | struct DepthTraits 60 | { 61 | static inline bool valid(float depth) { return std::isfinite(depth); } 62 | static inline float toMeters(float depth) { return depth; } 63 | static inline float fromMeters(float depth) { return depth; } 64 | }; 65 | 66 | } // namespace depth_image_proc 67 | 68 | #endif -------------------------------------------------------------------------------- /include/cslam/front_end/visualization_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _VISUALIZATION_UTILS_H_ 2 | #define _VISUALIZATION_UTILS_H_ 3 | 4 | #include 5 | 6 | #ifdef PRE_ROS_IRON 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #ifdef PRE_ROS_IRON 18 | #include 19 | #else 20 | #include 21 | #endif 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include "cslam/front_end/utils/depth_traits.h" 31 | 32 | namespace cslam 33 | { 34 | /** 35 | * @brief Create a colored pointcloud message 36 | * 37 | * @param sensor_data keyframe data 38 | * @param header timestamp + frame_id 39 | * @return sensor_msgs::msg::PointCloud2 message 40 | */ 41 | sensor_msgs::msg::PointCloud2 create_colored_pointcloud(const std::shared_ptr &sensor_data, const std_msgs::msg::Header &header); 42 | 43 | /** 44 | * @brief Convert depth image to pointcloud 45 | * 46 | * @tparam T 47 | * @param sensor_data 48 | * @param cloud_msg 49 | * @param model 50 | * @param range_max 51 | */ 52 | template 53 | void depth_image_to_pointcloud( 54 | const std::shared_ptr &sensor_data, 55 | sensor_msgs::msg::PointCloud2::SharedPtr &cloud_msg, 56 | const image_geometry::PinholeCameraModel &model, 57 | double range_max = 0.0); 58 | 59 | /** 60 | * @brief Add color to pointcloud 61 | * 62 | * @param sensor_data 63 | * @param cloud_msg 64 | */ 65 | void add_rgb_to_pointcloud( 66 | const std::shared_ptr &sensor_data, 67 | sensor_msgs::msg::PointCloud2::SharedPtr &cloud_msg); 68 | 69 | } // namespace cslam 70 | #endif 71 | -------------------------------------------------------------------------------- /models/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore 3 | !download.sh -------------------------------------------------------------------------------- /models/download.sh: -------------------------------------------------------------------------------- 1 | # TODO: CosPlace Instructions 2 | # Download from https://github.com/gmberton/CosPlace 3 | # The default parameters are for ResNet-18 with dimension=64 4 | 5 | # TODO: recommend symlink 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cslam 4 | 0.0.1 5 | Loop Closure Detection for C-SLAM 6 | Pierre-Yves Lajoie 7 | Pierre-Yves Lajoie 8 | BSD 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | builtin_interfaces 14 | rosidl_default_generators 15 | builtin_interfaces 16 | rosidl_default_runtime 17 | rosidl_interface_packages 18 | 19 | cv_bridge 20 | rclcpp 21 | rclcpp_components 22 | rclpy 23 | sensor_msgs 24 | std_msgs 25 | std_srvs 26 | nav_msgs 27 | stereo_msgs 28 | geometry_msgs 29 | visualization_msgs 30 | rosgraph_msgs 31 | image_transport 32 | tf2 33 | tf2_eigen 34 | tf2_ros 35 | laser_geometry 36 | pcl_conversions 37 | message_filters 38 | class_loader 39 | rtabmap 40 | rtabmap_msgs 41 | rtabmap_conversions 42 | octomap 43 | octomap_msgs 44 | image_geometry 45 | pluginlib 46 | rviz_common 47 | rviz_rendering 48 | rviz_default_plugins 49 | nav2_msgs 50 | libpcl-all-dev 51 | tf2_geometry_msgs 52 | pcl_ros 53 | pcl_msgs 54 | perception_pcl 55 | diagnostic_msgs 56 | cslam_common_interfaces 57 | cslam_common_interfaces 58 | 59 | cv_bridge 60 | rclcpp 61 | rclcpp_components 62 | rclpy 63 | sensor_msgs 64 | std_msgs 65 | std_srvs 66 | nav_msgs 67 | nav2_common 68 | stereo_msgs 69 | geometry_msgs 70 | visualization_msgs 71 | rosgraph_msgs 72 | image_transport 73 | compressed_depth_image_transport 74 | compressed_image_transport 75 | tf2 76 | tf2_eigen 77 | tf2_ros 78 | laser_geometry 79 | pcl_conversions 80 | message_filters 81 | class_loader 82 | rtabmap 83 | rtabmap_msgs 84 | rtabmap_conversions 85 | octomap 86 | octomap_msgs 87 | image_geometry 88 | pluginlib 89 | rviz_common 90 | rviz_rendering 91 | rviz_default_plugins 92 | nav2_msgs 93 | tf2_geometry_msgs 94 | pcl_ros 95 | pcl_msgs 96 | perception_pcl 97 | diagnostic_msgs 98 | cslam_common_interfaces 99 | cslam_common_interfaces 100 | 101 | 102 | ament_cmake_gtest 103 | ament_cmake_pytest 104 | ament_lint_auto 105 | ament_lint_common 106 | python3-pytest 107 | 108 | 109 | ament_cmake 110 | 111 | 112 | -------------------------------------------------------------------------------- /spec-file.txt: -------------------------------------------------------------------------------- 1 | # This file may be used to create an environment using: 2 | # $ conda create --name --file 3 | # platform: linux-64 4 | @EXPLICIT 5 | https://repo.anaconda.com/pkgs/main/linux-64/_libgcc_mutex-0.1-main.conda 6 | https://repo.anaconda.com/pkgs/main/linux-64/blas-1.0-mkl.conda 7 | https://repo.anaconda.com/pkgs/main/linux-64/ca-certificates-2022.07.19-h06a4308_0.conda 8 | https://repo.anaconda.com/pkgs/main/linux-64/intel-openmp-2021.4.0-h06a4308_3561.conda 9 | https://repo.anaconda.com/pkgs/main/linux-64/ld_impl_linux-64-2.35.1-h7274673_9.conda 10 | https://repo.anaconda.com/pkgs/main/linux-64/libgfortran4-7.5.0-ha8ba4b0_17.conda 11 | https://repo.anaconda.com/pkgs/main/linux-64/libstdcxx-ng-9.3.0-hd4cf53a_17.conda 12 | https://conda.anaconda.org/pytorch/noarch/pytorch-mutex-1.0-cuda.tar.bz2 13 | https://repo.anaconda.com/pkgs/main/linux-64/libgfortran-ng-7.5.0-ha8ba4b0_17.conda 14 | https://repo.anaconda.com/pkgs/main/linux-64/libgomp-9.3.0-h5101ec6_17.conda 15 | https://repo.anaconda.com/pkgs/main/linux-64/mkl-2021.4.0-h06a4308_640.conda 16 | https://repo.anaconda.com/pkgs/main/linux-64/_openmp_mutex-4.5-1_gnu.tar.bz2 17 | https://conda.anaconda.org/conda-forge/linux-64/libblas-3.9.0-12_linux64_mkl.tar.bz2 18 | https://repo.anaconda.com/pkgs/main/linux-64/libgcc-ng-9.3.0-h5101ec6_17.conda 19 | https://conda.anaconda.org/conda-forge/linux-64/liblapack-3.9.0-12_linux64_mkl.tar.bz2 20 | https://repo.anaconda.com/pkgs/main/linux-64/brotli-1.0.9-he6710b0_2.conda 21 | https://repo.anaconda.com/pkgs/main/linux-64/bzip2-1.0.8-h7b6447c_0.conda 22 | https://repo.anaconda.com/pkgs/main/linux-64/cudatoolkit-11.3.1-h2bc3f7f_2.conda 23 | https://repo.anaconda.com/pkgs/main/linux-64/expat-2.4.4-h295c915_0.conda 24 | https://repo.anaconda.com/pkgs/main/linux-64/giflib-5.2.1-h7b6447c_0.conda 25 | https://repo.anaconda.com/pkgs/main/linux-64/gmp-6.2.1-h295c915_3.conda 26 | https://repo.anaconda.com/pkgs/main/linux-64/icu-58.2-he6710b0_3.conda 27 | https://repo.anaconda.com/pkgs/main/linux-64/jpeg-9e-h7f8727e_0.conda 28 | https://repo.anaconda.com/pkgs/main/linux-64/lame-3.100-h7b6447c_0.conda 29 | https://repo.anaconda.com/pkgs/main/linux-64/libffi-3.3-he6710b0_2.conda 30 | https://repo.anaconda.com/pkgs/main/linux-64/libiconv-1.16-h7f8727e_2.conda 31 | https://repo.anaconda.com/pkgs/main/linux-64/libtasn1-4.16.0-h27cfd23_0.conda 32 | https://repo.anaconda.com/pkgs/main/linux-64/libunistring-0.9.10-h27cfd23_0.conda 33 | https://repo.anaconda.com/pkgs/main/linux-64/libuuid-1.0.3-h7f8727e_2.conda 34 | https://repo.anaconda.com/pkgs/main/linux-64/libwebp-base-1.2.2-h7f8727e_0.conda 35 | https://repo.anaconda.com/pkgs/main/linux-64/libxcb-1.15-h7f8727e_0.conda 36 | https://repo.anaconda.com/pkgs/main/linux-64/lz4-c-1.9.3-h295c915_1.conda 37 | https://repo.anaconda.com/pkgs/main/linux-64/ncurses-6.3-h7f8727e_2.conda 38 | https://repo.anaconda.com/pkgs/main/linux-64/openh264-2.1.1-h4ff587b_0.conda 39 | https://repo.anaconda.com/pkgs/main/linux-64/openssl-1.1.1q-h7f8727e_0.conda 40 | https://repo.anaconda.com/pkgs/main/linux-64/pcre-8.45-h295c915_0.conda 41 | https://repo.anaconda.com/pkgs/main/linux-64/tbb-2020.3-hfd86e86_0.conda 42 | https://repo.anaconda.com/pkgs/main/linux-64/xz-5.2.5-h7b6447c_0.conda 43 | https://repo.anaconda.com/pkgs/main/linux-64/yaml-0.2.5-h7b6447c_0.conda 44 | https://repo.anaconda.com/pkgs/main/linux-64/zlib-1.2.12-h7f8727e_1.conda 45 | https://repo.anaconda.com/pkgs/main/linux-64/glib-2.69.1-h4ff587b_1.conda 46 | https://repo.anaconda.com/pkgs/main/linux-64/hdf5-1.10.6-hb1b8bf9_0.conda 47 | https://conda.anaconda.org/conda-forge/linux-64/libfaiss-1.7.1-cuda112h5bea7ad_1_cuda.tar.bz2 48 | https://conda.anaconda.org/conda-forge/linux-64/libfaiss-avx2-1.7.1-cuda112h1234567_1_cuda.tar.bz2 49 | https://repo.anaconda.com/pkgs/main/linux-64/libidn2-2.3.2-h7f8727e_0.conda 50 | https://repo.anaconda.com/pkgs/main/linux-64/libllvm10-10.0.1-hbcb73fb_5.conda 51 | https://repo.anaconda.com/pkgs/main/linux-64/libpng-1.6.37-hbc83047_0.conda 52 | https://repo.anaconda.com/pkgs/main/linux-64/libprotobuf-3.20.1-h4ff587b_0.conda 53 | https://repo.anaconda.com/pkgs/main/linux-64/libxml2-2.9.14-h74e7548_0.conda 54 | https://repo.anaconda.com/pkgs/main/linux-64/nettle-3.7.3-hbbd107a_1.conda 55 | https://repo.anaconda.com/pkgs/main/linux-64/readline-8.1.2-h7f8727e_1.conda 56 | https://repo.anaconda.com/pkgs/main/linux-64/tk-8.6.11-h1ccaba5_0.conda 57 | https://repo.anaconda.com/pkgs/main/linux-64/zstd-1.5.2-ha4553b6_0.conda 58 | https://repo.anaconda.com/pkgs/main/linux-64/dbus-1.13.18-hb2f20db_0.conda 59 | https://repo.anaconda.com/pkgs/main/linux-64/freetype-2.11.0-h70c0345_0.conda 60 | https://repo.anaconda.com/pkgs/main/linux-64/gnutls-3.6.15-he1e5248_0.conda 61 | https://repo.anaconda.com/pkgs/main/linux-64/gstreamer-1.14.0-h28cd5cc_2.conda 62 | https://repo.anaconda.com/pkgs/main/linux-64/libtiff-4.2.0-h2818925_1.conda 63 | https://repo.anaconda.com/pkgs/main/linux-64/sqlite-3.38.2-hc218d9a_0.conda 64 | https://conda.anaconda.org/pytorch/linux-64/ffmpeg-4.3-hf484d3e_0.tar.bz2 65 | https://repo.anaconda.com/pkgs/main/linux-64/fontconfig-2.13.1-h6c09931_0.conda 66 | https://repo.anaconda.com/pkgs/main/linux-64/gst-plugins-base-1.14.0-h8213a91_2.conda 67 | https://repo.anaconda.com/pkgs/main/linux-64/lcms2-2.12-h3be6417_0.conda 68 | https://repo.anaconda.com/pkgs/main/linux-64/libwebp-1.2.2-h55f646e_0.conda 69 | https://repo.anaconda.com/pkgs/main/linux-64/python-3.8.13-h12debd9_0.conda 70 | https://repo.anaconda.com/pkgs/main/noarch/atomicwrites-1.4.0-py_0.conda 71 | https://repo.anaconda.com/pkgs/main/noarch/attrs-21.4.0-pyhd3eb1b0_0.conda 72 | https://repo.anaconda.com/pkgs/main/linux-64/certifi-2022.6.15-py38h06a4308_0.conda 73 | https://repo.anaconda.com/pkgs/main/noarch/charset-normalizer-2.0.4-pyhd3eb1b0_0.conda 74 | https://repo.anaconda.com/pkgs/main/noarch/colorama-0.4.4-pyhd3eb1b0_0.conda 75 | https://repo.anaconda.com/pkgs/main/noarch/cycler-0.11.0-pyhd3eb1b0_0.conda 76 | https://repo.anaconda.com/pkgs/main/linux-64/docutils-0.18.1-py38h06a4308_3.conda 77 | https://conda.anaconda.org/conda-forge/noarch/empy-3.3.4-pyh9f0ad1d_1.tar.bz2 78 | https://repo.anaconda.com/pkgs/main/noarch/idna-3.3-pyhd3eb1b0_0.conda 79 | https://repo.anaconda.com/pkgs/main/noarch/iniconfig-1.1.1-pyhd3eb1b0_0.tar.bz2 80 | https://repo.anaconda.com/pkgs/main/noarch/joblib-1.1.0-pyhd3eb1b0_0.conda 81 | https://repo.anaconda.com/pkgs/main/linux-64/kiwisolver-1.4.2-py38h295c915_0.conda 82 | https://repo.anaconda.com/pkgs/main/linux-64/llvmlite-0.36.0-py38h612dafd_4.conda 83 | https://repo.anaconda.com/pkgs/main/noarch/munkres-1.1.4-py_0.conda 84 | https://repo.anaconda.com/pkgs/main/linux-64/networkx-2.8.4-py38h06a4308_0.conda 85 | https://repo.anaconda.com/pkgs/main/linux-64/pillow-9.0.1-py38h22f2fdc_0.conda 86 | https://repo.anaconda.com/pkgs/main/linux-64/pluggy-1.0.0-py38h06a4308_1.conda 87 | https://repo.anaconda.com/pkgs/main/linux-64/protobuf-3.20.1-py38h295c915_0.conda 88 | https://repo.anaconda.com/pkgs/main/noarch/py-1.11.0-pyhd3eb1b0_0.conda 89 | https://repo.anaconda.com/pkgs/main/linux-64/pycosat-0.6.3-py38h7b6447c_1.conda 90 | https://repo.anaconda.com/pkgs/main/noarch/pycparser-2.21-pyhd3eb1b0_0.conda 91 | https://conda.anaconda.org/conda-forge/noarch/pyjsparser-2.7.1-pyh8c360ce_0.tar.bz2 92 | https://repo.anaconda.com/pkgs/main/noarch/pyparsing-3.0.4-pyhd3eb1b0_0.conda 93 | https://repo.anaconda.com/pkgs/main/linux-64/pysocks-1.7.1-py38h06a4308_0.conda 94 | https://conda.anaconda.org/conda-forge/linux-64/python_abi-3.8-2_cp38.tar.bz2 95 | https://repo.anaconda.com/pkgs/main/linux-64/pytz-2022.1-py38h06a4308_0.conda 96 | https://repo.anaconda.com/pkgs/main/linux-64/pyyaml-6.0-py38h7f8727e_1.conda 97 | https://repo.anaconda.com/pkgs/main/linux-64/qt-5.9.7-h5867ecd_1.conda 98 | https://repo.anaconda.com/pkgs/main/linux-64/regex-2022.3.15-py38h7f8727e_0.conda 99 | https://repo.anaconda.com/pkgs/main/linux-64/ruamel_yaml-0.15.100-py38h27cfd23_0.conda 100 | https://repo.anaconda.com/pkgs/main/linux-64/sip-4.19.13-py38h295c915_0.conda 101 | https://repo.anaconda.com/pkgs/main/noarch/six-1.16.0-pyhd3eb1b0_1.conda 102 | https://repo.anaconda.com/pkgs/main/noarch/threadpoolctl-2.2.0-pyh0d69192_0.conda 103 | https://repo.anaconda.com/pkgs/main/linux-64/tomli-2.0.1-py38h06a4308_0.conda 104 | https://repo.anaconda.com/pkgs/main/noarch/typing_extensions-4.1.1-pyh06a4308_0.conda 105 | https://repo.anaconda.com/pkgs/main/noarch/wheel-0.37.1-pyhd3eb1b0_0.conda 106 | https://repo.anaconda.com/pkgs/main/noarch/yapf-0.31.0-pyhd3eb1b0_0.conda 107 | https://repo.anaconda.com/pkgs/main/linux-64/zipp-3.8.0-py38h06a4308_0.conda 108 | https://repo.anaconda.com/pkgs/main/linux-64/cffi-1.15.0-py38hd667e15_1.conda 109 | https://repo.anaconda.com/pkgs/main/noarch/fonttools-4.25.0-pyhd3eb1b0_0.conda 110 | https://repo.anaconda.com/pkgs/main/linux-64/importlib-metadata-4.11.3-py38h06a4308_0.conda 111 | https://repo.anaconda.com/pkgs/main/linux-64/mkl-service-2.4.0-py38h7f8727e_0.conda 112 | https://repo.anaconda.com/pkgs/main/noarch/packaging-21.3-pyhd3eb1b0_0.conda 113 | https://repo.anaconda.com/pkgs/main/linux-64/pyqt-5.9.2-py38h05f1152_4.conda 114 | https://repo.anaconda.com/pkgs/main/noarch/python-dateutil-2.8.2-pyhd3eb1b0_0.conda 115 | https://conda.anaconda.org/pytorch/linux-64/pytorch-1.12.0-py3.8_cuda11.3_cudnn8.3.2_0.tar.bz2 116 | https://repo.anaconda.com/pkgs/main/linux-64/setuptools-61.2.0-py38h06a4308_0.conda 117 | https://repo.anaconda.com/pkgs/main/noarch/tqdm-4.63.0-pyhd3eb1b0_0.conda 118 | https://repo.anaconda.com/pkgs/main/linux-64/tzlocal-2.1-py38_0.conda 119 | https://repo.anaconda.com/pkgs/main/linux-64/brotlipy-0.7.0-py38h27cfd23_1003.conda 120 | https://conda.anaconda.org/conda-forge/noarch/catkin_pkg-0.5.2-pyhd8ed1ab_0.tar.bz2 121 | https://repo.anaconda.com/pkgs/main/linux-64/conda-package-handling-1.8.1-py38h7f8727e_0.conda 122 | https://repo.anaconda.com/pkgs/main/linux-64/cryptography-36.0.0-py38h9ce1e76_0.conda 123 | https://repo.anaconda.com/pkgs/main/noarch/importlib_metadata-4.11.3-hd3eb1b0_0.conda 124 | https://conda.anaconda.org/conda-forge/noarch/js2py-0.71-pyhd8ed1ab_0.tar.bz2 125 | https://repo.anaconda.com/pkgs/main/linux-64/numpy-base-1.22.3-py38hf524024_0.conda 126 | https://repo.anaconda.com/pkgs/main/linux-64/pip-22.1.2-py38h06a4308_0.conda 127 | https://repo.anaconda.com/pkgs/main/linux-64/pytest-7.1.2-py38h06a4308_0.conda 128 | https://repo.anaconda.com/pkgs/main/noarch/argcomplete-1.12.3-pyhd3eb1b0_0.conda 129 | https://repo.anaconda.com/pkgs/main/noarch/conda-content-trust-0.1.1-pyhd3eb1b0_0.conda 130 | https://conda.anaconda.org/conda-forge/noarch/lark-1.1.2-pyhd8ed1ab_0.tar.bz2 131 | https://repo.anaconda.com/pkgs/main/noarch/pyopenssl-22.0.0-pyhd3eb1b0_0.conda 132 | https://repo.anaconda.com/pkgs/main/noarch/urllib3-1.26.8-pyhd3eb1b0_0.conda 133 | https://repo.anaconda.com/pkgs/main/noarch/requests-2.27.1-pyhd3eb1b0_0.conda 134 | https://conda.anaconda.org/conda-forge/linux-64/faiss-1.7.1-py38cuda112hf4212ac_1_cuda.tar.bz2 135 | https://repo.anaconda.com/pkgs/main/linux-64/bottleneck-1.3.4-py38hce1f21e_0.conda 136 | https://repo.anaconda.com/pkgs/main/linux-64/h5py-3.6.0-py38ha0f2276_0.conda 137 | https://repo.anaconda.com/pkgs/main/linux-64/matplotlib-3.5.1-py38h06a4308_1.conda 138 | https://repo.anaconda.com/pkgs/main/linux-64/matplotlib-base-3.5.1-py38ha18d171_1.conda 139 | https://repo.anaconda.com/pkgs/main/linux-64/mkl_fft-1.3.1-py38hd3c417c_0.conda 140 | https://repo.anaconda.com/pkgs/main/linux-64/mkl_random-1.2.2-py38h51133e4_0.conda 141 | https://repo.anaconda.com/pkgs/main/linux-64/numpy-1.22.3-py38he7a7128_0.conda 142 | https://repo.anaconda.com/pkgs/main/linux-64/numba-0.53.1-py38ha9443f7_0.conda 143 | https://repo.anaconda.com/pkgs/main/linux-64/numexpr-2.8.1-py38h6abb31d_0.conda 144 | https://repo.anaconda.com/pkgs/main/noarch/tensorboardx-2.2-pyhd3eb1b0_0.conda 145 | https://conda.anaconda.org/pytorch/linux-64/torchvision-0.13.0-py38_cu113.tar.bz2 146 | https://repo.anaconda.com/pkgs/main/linux-64/pandas-1.4.2-py38h295c915_0.conda 147 | -------------------------------------------------------------------------------- /src/back_end/gtsam_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/back_end/gtsam_utils.h" 2 | 3 | namespace cslam { 4 | 5 | geometry_msgs::msg::Pose gtsam_pose_to_msg(const gtsam::Pose3 &pose) { 6 | geometry_msgs::msg::Pose msg; 7 | msg.position.x = pose.x(); 8 | msg.position.y = pose.y(); 9 | msg.position.z = pose.z(); 10 | auto rotation = pose.rotation().quaternion(); 11 | msg.orientation.w = rotation[0]; 12 | msg.orientation.x = rotation[1]; 13 | msg.orientation.y = rotation[2]; 14 | msg.orientation.z = rotation[3]; 15 | return msg; 16 | } 17 | 18 | geometry_msgs::msg::Transform 19 | gtsam_pose_to_transform_msg(const gtsam::Pose3 &pose) { 20 | geometry_msgs::msg::Transform msg; 21 | msg.translation.x = pose.x(); 22 | msg.translation.y = pose.y(); 23 | msg.translation.z = pose.z(); 24 | 25 | auto rotation = pose.rotation().quaternion(); 26 | msg.rotation.w = rotation[0]; 27 | msg.rotation.x = rotation[1]; 28 | msg.rotation.y = rotation[2]; 29 | msg.rotation.z = rotation[3]; 30 | 31 | return msg; 32 | } 33 | 34 | std::vector 35 | gtsam_values_to_msg(const gtsam::Values &values) { 36 | std::vector poses; 37 | for (const auto key_value : values) { 38 | auto p = dynamic_cast *>( 39 | &key_value.value); 40 | if (!p) 41 | continue; 42 | cslam_common_interfaces::msg::PoseGraphValue pose_msg; 43 | pose_msg.pose = gtsam_pose_to_msg(p->value()); 44 | auto key = gtsam::LabeledSymbol(key_value.key); 45 | pose_msg.key.robot_id = ROBOT_ID(key.label()); 46 | pose_msg.key.keyframe_id = key.index(); 47 | 48 | poses.emplace_back(pose_msg); 49 | } 50 | return poses; 51 | } 52 | 53 | std::vector 54 | gtsam_values_to_msg(const gtsam::Values::shared_ptr values) { 55 | return gtsam_values_to_msg(*values); 56 | } 57 | 58 | std::vector 59 | gtsam_factors_to_msg(const gtsam::NonlinearFactorGraph &factors) { 60 | std::vector edges; 61 | for (const auto &factor_ : factors) { 62 | auto factor = 63 | boost::dynamic_pointer_cast>( 64 | factor_); 65 | if (factor) { 66 | cslam_common_interfaces::msg::PoseGraphEdge edge_msg; 67 | 68 | auto key_from = gtsam::LabeledSymbol(factor->key1()); 69 | auto key_to = gtsam::LabeledSymbol(factor->key2()); 70 | edge_msg.key_from.robot_id = ROBOT_ID(key_from.label()); 71 | edge_msg.key_from.keyframe_id = key_from.index(); 72 | edge_msg.key_to.robot_id = ROBOT_ID(key_to.label()); 73 | edge_msg.key_to.keyframe_id = key_to.index(); 74 | 75 | edge_msg.measurement = gtsam_pose_to_msg(factor->measured()); 76 | 77 | gtsam::SharedNoiseModel model = factor->noiseModel(); 78 | auto noise = 79 | boost::dynamic_pointer_cast(model); 80 | auto sigmas = noise->sigmas(); 81 | for (unsigned int i = 0; i < 6; i++) { 82 | edge_msg.noise_std[i] = sigmas[i]; 83 | } 84 | 85 | edges.emplace_back(edge_msg); 86 | } 87 | } 88 | return edges; 89 | } 90 | 91 | std::vector 92 | gtsam_factors_to_msg(const gtsam::NonlinearFactorGraph::shared_ptr factors) { 93 | return gtsam_factors_to_msg(*factors); 94 | } 95 | 96 | gtsam::Pose3 transform_msg_to_pose3(const geometry_msgs::msg::Transform &msg) { 97 | gtsam::Rot3 rotation(msg.rotation.w, msg.rotation.x, msg.rotation.y, 98 | msg.rotation.z); 99 | return gtsam::Pose3( 100 | rotation, {msg.translation.x, msg.translation.y, msg.translation.z}); 101 | } 102 | 103 | gtsam::Pose3 pose_msg_to_gtsam(const geometry_msgs::msg::Pose &pose) { 104 | gtsam::Rot3 rotation(pose.orientation.w, pose.orientation.x, 105 | pose.orientation.y, pose.orientation.z); 106 | return gtsam::Pose3(rotation, 107 | {pose.position.x, pose.position.y, pose.position.z}); 108 | } 109 | 110 | gtsam::Pose3 odometry_msg_to_pose3(const nav_msgs::msg::Odometry &odom_msg) { 111 | return pose_msg_to_gtsam(odom_msg.pose.pose); 112 | } 113 | 114 | gtsam::Values::shared_ptr values_msg_to_gtsam( 115 | const std::vector &msg) { 116 | auto values = boost::make_shared(); 117 | for (auto v : msg) { 118 | gtsam::Pose3 pose = pose_msg_to_gtsam(v.pose); 119 | gtsam::LabeledSymbol symbol(GRAPH_LABEL, ROBOT_LABEL(v.key.robot_id), 120 | v.key.keyframe_id); 121 | values->insert(symbol, pose); 122 | } 123 | return values; 124 | } 125 | 126 | gtsam::NonlinearFactorGraph::shared_ptr edges_msg_to_gtsam( 127 | const std::vector &msg) { 128 | auto graph = boost::make_shared(); 129 | for (auto e : msg) { 130 | gtsam::Pose3 pose = pose_msg_to_gtsam(e.measurement); 131 | gtsam::LabeledSymbol symbol_from( 132 | GRAPH_LABEL, ROBOT_LABEL(e.key_from.robot_id), e.key_from.keyframe_id); 133 | gtsam::LabeledSymbol symbol_to(GRAPH_LABEL, ROBOT_LABEL(e.key_to.robot_id), 134 | e.key_to.keyframe_id); 135 | 136 | Eigen::VectorXd sigmas(6); 137 | sigmas << e.noise_std[0], e.noise_std[1], e.noise_std[2], e.noise_std[3], 138 | e.noise_std[4], e.noise_std[5]; 139 | auto noise_model = gtsam::noiseModel::Diagonal::Sigmas(sigmas); 140 | 141 | gtsam::BetweenFactor factor(symbol_from, symbol_to, pose, 142 | noise_model); 143 | graph->push_back(factor); 144 | } 145 | return graph; 146 | } 147 | 148 | } // namespace cslam -------------------------------------------------------------------------------- /src/back_end/pose_graph_manager_node.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/back_end/decentralized_pgo.h" 2 | 3 | using namespace cslam; 4 | 5 | /** 6 | * @brief Node to manage the pose graph data 7 | * 8 | * @param argc 9 | * @param argv 10 | * @return int 11 | */ 12 | int main(int argc, char **argv) { 13 | 14 | rclcpp::init(argc, argv); 15 | 16 | auto node = std::make_shared("pose_graph_manager"); 17 | 18 | node->declare_parameter("max_nb_robots", 1); 19 | node->declare_parameter("robot_id", 0); 20 | node->declare_parameter("backend.pose_graph_optimization_start_period_ms", 1000); 21 | node->declare_parameter("backend.pose_graph_optimization_loop_period_ms", 100); 22 | node->declare_parameter("backend.max_waiting_time_sec", 100); 23 | node->declare_parameter("backend.enable_broadcast_tf_frames", true); 24 | node->declare_parameter("neighbor_management.heartbeat_period_sec", 1.0); 25 | node->declare_parameter("evaluation.enable_logs", false); 26 | node->declare_parameter("evaluation.log_folder", ""); 27 | node->declare_parameter("evaluation.enable_gps_recording", false); 28 | node->declare_parameter("evaluation.enable_simulated_rendezvous", false); 29 | node->declare_parameter("evaluation.rendezvous_schedule_file", ""); 30 | node->declare_parameter("evaluation.enable_pose_timestamps_recording", false); 31 | node->declare_parameter("visualization.enable", false); 32 | node->declare_parameter("visualization.publishing_period_ms", 0); 33 | 34 | DecentralizedPGO manager(node); 35 | 36 | rclcpp::spin(node); 37 | 38 | rclcpp::shutdown(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /src/back_end/utils/simulated_rendezvous.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/back_end/utils/simulated_rendezvous.h" 2 | 3 | using namespace cslam; 4 | 5 | SimulatedRendezVous::SimulatedRendezVous(std::shared_ptr &node, 6 | const std::string &schedule_file, 7 | const unsigned int &robot_id) : node_(node), robot_id_(robot_id), enabled_(true) 8 | { 9 | try 10 | { 11 | int64_t initial_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 12 | std::ifstream schedule(schedule_file); 13 | if (schedule.is_open()) 14 | { 15 | for (std::string line; std::getline(schedule, line);) 16 | { 17 | auto delim0 = line.find(","); 18 | if (robot_id_ == std::stoul(line.substr(0, delim0))) 19 | { 20 | RCLCPP_INFO(node_->get_logger(), "Simulated rendezvous schedule of robot %s", line.c_str()); 21 | while (delim0 != std::string::npos) 22 | { 23 | auto delim1 = line.find(",", delim0 + 1); 24 | 25 | auto start = std::stoull(line.substr(delim0 + 1, delim1)) + initial_time; 26 | 27 | delim0 = delim1; 28 | delim1 = line.find(",", delim0); 29 | 30 | auto end = std::stoull(line.substr(delim0 + 1, delim1)) + initial_time; 31 | 32 | rendezvous_ranges_.push_back(std::make_pair(start, end)); 33 | 34 | delim0 = line.find(",", delim1 +1); 35 | } 36 | } 37 | } 38 | schedule.close(); 39 | } else { 40 | RCLCPP_ERROR(node_->get_logger(), "Unable to open rendezvous schedule file"); 41 | enabled_ = false; 42 | } 43 | } 44 | catch (const std::exception &e) 45 | { 46 | RCLCPP_ERROR(node_->get_logger(), "Reading simulated rendezvous schedule failed: %s", e.what()); 47 | enabled_ = false; 48 | } 49 | } 50 | 51 | bool SimulatedRendezVous::is_alive() 52 | { 53 | if (enabled_) 54 | { 55 | bool is_alive = false; 56 | uint64_t current_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); 57 | 58 | for (const auto &range : rendezvous_ranges_) 59 | { 60 | //RCLCPP_INFO(node_->get_logger(), "Time until rendezvous (%d,%d) = %d | Time until rendezvous end = %d", ((int) range.first), ((int) range.second), range.first - ((int)current_time), range.second - ((int)current_time)); 61 | if (current_time >= range.first && current_time <= range.second) 62 | { 63 | is_alive = true; 64 | } 65 | } 66 | return is_alive; 67 | } 68 | return true; 69 | } 70 | -------------------------------------------------------------------------------- /src/front_end/map_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/front_end/map_manager.h" 2 | #include "cslam/front_end/stereo_handler.h" 3 | 4 | using namespace cslam; 5 | 6 | template 7 | MapManager::MapManager(std::shared_ptr &node) 8 | : node_(node), local_data_handler_(node) { 9 | 10 | node_->get_parameter("max_nb_robots", max_nb_robots_); 11 | node_->get_parameter("robot_id", robot_id_); 12 | node_->get_parameter("frontend.map_manager_process_period_ms", 13 | map_manager_process_period_ms_); 14 | 15 | std::chrono::milliseconds period(map_manager_process_period_ms_); 16 | 17 | process_timer_ = node_->create_wall_timer( 18 | std::chrono::milliseconds(period), 19 | std::bind(&MapManager::process_new_sensor_data, this)); 20 | 21 | RCLCPP_INFO(node_->get_logger(), "Initialization done."); 22 | } 23 | 24 | template 25 | void MapManager::process_new_sensor_data() { 26 | local_data_handler_.process_new_sensor_data(); 27 | } 28 | -------------------------------------------------------------------------------- /src/front_end/map_manager_node.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/front_end/map_manager.h" 2 | 3 | using namespace cslam; 4 | 5 | /** 6 | * @brief Node to manage the sensor data and registration 7 | * 8 | * @param argc 9 | * @param argv 10 | * @return int 11 | */ 12 | int main(int argc, char **argv) { 13 | 14 | rclcpp::init(argc, argv); 15 | 16 | auto node = std::make_shared("map_manager"); 17 | 18 | node->declare_parameter("frontend.pnp_min_inliers", 20); 19 | node->declare_parameter("frontend.max_queue_size", 10); 20 | node->declare_parameter("max_nb_robots", 1); 21 | node->declare_parameter("robot_id", 0); 22 | node->declare_parameter("frontend.map_manager_process_period_ms", 100); 23 | node->declare_parameter("frontend.sensor_type", "stereo"); 24 | node->declare_parameter("visualization.enable", false); 25 | node->declare_parameter("visualization.publishing_period_ms", 0); 26 | node->declare_parameter("visualization.voxel_size", 0.05); 27 | node->declare_parameter("visualization.max_range", 2.0); 28 | node->declare_parameter("evaluation.enable_gps_recording", false); 29 | node->declare_parameter("evaluation.gps_topic", ""); 30 | 31 | std::string sensor_type; 32 | node->get_parameter("frontend.sensor_type", sensor_type); 33 | 34 | std::shared_ptr handler; 35 | if (sensor_type == "stereo") { 36 | handler = std::make_shared>(node); 37 | } 38 | else if (sensor_type == "rgbd") { 39 | handler = std::make_shared>(node); 40 | } 41 | else { 42 | RCLCPP_ERROR(node->get_logger(), "Sensor type not supported: %s", 43 | sensor_type.c_str()); 44 | return -1; 45 | } 46 | 47 | rclcpp::spin(node); 48 | 49 | rclcpp::shutdown(); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /src/front_end/stereo_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/front_end/stereo_handler.h" 2 | #include 3 | 4 | using namespace rtabmap; 5 | using namespace cslam; 6 | 7 | StereoHandler::StereoHandler(std::shared_ptr &node) 8 | : RGBDHandler(node) { 9 | node->declare_parameter("frontend.left_image_topic", "left/image_rect"); 10 | node->declare_parameter("frontend.right_image_topic", "right/image_rect"); 11 | node->declare_parameter("frontend.left_camera_info_topic", 12 | "left/camera_info"); 13 | node->declare_parameter("frontend.right_camera_info_topic", 14 | "right/camera_info"); 15 | 16 | // Subscriber for stereo images 17 | sub_image_rect_left_.subscribe( 18 | node_.get(), node_->get_parameter("frontend.left_image_topic").as_string(), "raw", 19 | rclcpp::QoS(max_queue_size_) 20 | .reliability((rmw_qos_reliability_policy_t)2) 21 | .get_rmw_qos_profile()); 22 | sub_image_rect_right_.subscribe( 23 | node_.get(), node_->get_parameter("frontend.right_image_topic").as_string(), "raw", 24 | rclcpp::QoS(max_queue_size_) 25 | .reliability((rmw_qos_reliability_policy_t)2) 26 | .get_rmw_qos_profile()); 27 | sub_camera_info_left_.subscribe( 28 | node_.get(), node_->get_parameter("frontend.left_camera_info_topic").as_string(), 29 | rclcpp::QoS(max_queue_size_) 30 | .reliability((rmw_qos_reliability_policy_t)2) 31 | .get_rmw_qos_profile()); 32 | sub_camera_info_right_.subscribe( 33 | node_.get(), node_->get_parameter("frontend.right_camera_info_topic").as_string(), 34 | rclcpp::QoS(max_queue_size_) 35 | .reliability((rmw_qos_reliability_policy_t)2) 36 | .get_rmw_qos_profile()); 37 | 38 | stereo_sync_policy_ = new message_filters::Synchronizer( 39 | StereoSyncPolicy(max_queue_size_), sub_image_rect_left_, sub_image_rect_right_, 40 | sub_camera_info_left_, sub_camera_info_right_, sub_odometry_); 41 | stereo_sync_policy_->registerCallback( 42 | std::bind(&StereoHandler::stereo_callback, this, std::placeholders::_1, 43 | std::placeholders::_2, std::placeholders::_3, 44 | std::placeholders::_4, std::placeholders::_5)); 45 | 46 | } 47 | 48 | void StereoHandler::stereo_callback( 49 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_left, 50 | const sensor_msgs::msg::Image::ConstSharedPtr image_rect_right, 51 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_left, 52 | const sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_right, 53 | const nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) { 54 | // If odom tracking failed, do not process the frame 55 | if (odom_ptr->pose.covariance[0] > 1000) 56 | { 57 | RCLCPP_WARN(node_->get_logger(), "Odom tracking failed, skipping frame"); 58 | return; 59 | } 60 | // Fix timestamps for logging 61 | auto odom = std::make_shared(*odom_ptr); 62 | odom->header.stamp = image_rect_left->header.stamp; 63 | 64 | if (!(image_rect_left->encoding.compare( 65 | sensor_msgs::image_encodings::TYPE_8UC1) == 0 || 66 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::MONO8) == 67 | 0 || 68 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::MONO16) == 69 | 0 || 70 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::BGR8) == 71 | 0 || 72 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::RGB8) == 73 | 0 || 74 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 75 | 0 || 76 | image_rect_left->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 77 | 0) || 78 | !(image_rect_right->encoding.compare( 79 | sensor_msgs::image_encodings::TYPE_8UC1) == 0 || 80 | image_rect_right->encoding.compare(sensor_msgs::image_encodings::MONO8) == 81 | 0 || 82 | image_rect_right->encoding.compare( 83 | sensor_msgs::image_encodings::MONO16) == 0 || 84 | image_rect_right->encoding.compare(sensor_msgs::image_encodings::BGR8) == 85 | 0 || 86 | image_rect_right->encoding.compare(sensor_msgs::image_encodings::RGB8) == 87 | 0 || 88 | image_rect_right->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 89 | 0 || 90 | image_rect_right->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 91 | 0)) { 92 | RCLCPP_ERROR( 93 | node_->get_logger(), 94 | "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 " 95 | "recommended), received types are %s (left) and %s (right)", 96 | image_rect_left->encoding.c_str(), image_rect_right->encoding.c_str()); 97 | return; 98 | } 99 | 100 | rclcpp::Time stamp = 101 | rtabmap_conversions::timestampFromROS(image_rect_left->header.stamp) > 102 | rtabmap_conversions::timestampFromROS(image_rect_right->header.stamp) 103 | ? image_rect_left->header.stamp 104 | : image_rect_right->header.stamp; 105 | 106 | Transform localTransform(0,0,0,0,0,0); 107 | if (base_frame_id_ != "") 108 | { 109 | localTransform = rtabmap_conversions::getTransform( 110 | base_frame_id_, image_rect_left->header.frame_id, stamp, *tf_buffer_, 0.1); 111 | if (localTransform.isNull()) { 112 | RCLCPP_INFO(node_->get_logger(), 113 | "Could not get transform from %s to %s after 0.1 s!", 114 | base_frame_id_.c_str(), image_rect_left->header.frame_id.c_str()); 115 | return; 116 | } 117 | } 118 | 119 | if (image_rect_left->data.size() && image_rect_right->data.size()) { 120 | bool alreadyRectified = true; 121 | rtabmap::Transform stereoTransform; 122 | if (!alreadyRectified) { 123 | stereoTransform = rtabmap_conversions::getTransform( 124 | camera_info_right->header.frame_id, camera_info_left->header.frame_id, 125 | camera_info_left->header.stamp, *tf_buffer_, 0.1); 126 | if (stereoTransform.isNull()) { 127 | RCLCPP_ERROR(node_->get_logger(), 128 | "Parameter %s is false but we cannot get TF between the " 129 | "two cameras! (between frames %s and %s)", 130 | Parameters::kRtabmapImagesAlreadyRectified().c_str(), 131 | camera_info_right->header.frame_id.c_str(), 132 | camera_info_left->header.frame_id.c_str()); 133 | return; 134 | } else if (stereoTransform.isIdentity()) { 135 | RCLCPP_ERROR(node_->get_logger(), 136 | "Parameter %s is false but we cannot get a valid TF " 137 | "between the two cameras! " 138 | "Identity transform returned between left and right " 139 | "cameras. Verify that if TF between " 140 | "the cameras is valid: \"rosrun tf tf_echo %s %s\".", 141 | Parameters::kRtabmapImagesAlreadyRectified().c_str(), 142 | camera_info_right->header.frame_id.c_str(), 143 | camera_info_left->header.frame_id.c_str()); 144 | return; 145 | } 146 | } 147 | 148 | rtabmap::StereoCameraModel stereoModel = 149 | rtabmap_conversions::stereoCameraModelFromROS(*camera_info_left, *camera_info_right, 150 | localTransform, stereoTransform); 151 | 152 | if (stereoModel.baseline() == 0 && alreadyRectified) { 153 | stereoTransform = rtabmap_conversions::getTransform( 154 | camera_info_left->header.frame_id, camera_info_right->header.frame_id, 155 | camera_info_left->header.stamp, *tf_buffer_, 0.1); 156 | 157 | if (!stereoTransform.isNull() && stereoTransform.x() > 0) { 158 | static bool warned = false; 159 | if (!warned) { 160 | RCLCPP_WARN( 161 | node_->get_logger(), 162 | "Right camera info doesn't have Tx set but we are assuming that " 163 | "stereo images are already rectified (see %s parameter). While " 164 | "not " 165 | "recommended, we used TF to get the baseline (%s->%s = %fm) for " 166 | "convenience (e.g., D400 ir stereo issue). It is preferred to " 167 | "feed " 168 | "a valid right camera info if stereo images are already " 169 | "rectified. This message is only printed once...", 170 | rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(), 171 | camera_info_right->header.frame_id.c_str(), 172 | camera_info_left->header.frame_id.c_str(), stereoTransform.x()); 173 | warned = true; 174 | } 175 | stereoModel = rtabmap::StereoCameraModel( 176 | stereoModel.left().fx(), stereoModel.left().fy(), 177 | stereoModel.left().cx(), stereoModel.left().cy(), 178 | stereoTransform.x(), stereoModel.localTransform(), 179 | stereoModel.left().imageSize()); 180 | } 181 | } 182 | 183 | if (alreadyRectified && stereoModel.baseline() <= 0) { 184 | RCLCPP_ERROR( 185 | node_->get_logger(), 186 | "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We " 187 | "assume a horizontal left/right stereo " 188 | "setup where the Tx (or P(0,3)) is negative in the right camera info " 189 | "msg.", 190 | stereoModel.baseline()); 191 | return; 192 | } 193 | 194 | if (stereoModel.baseline() > 10.0) { 195 | static bool shown = false; 196 | if (!shown) { 197 | RCLCPP_WARN( 198 | node_->get_logger(), 199 | "Detected baseline (%f m) is quite large! Is your " 200 | "right camera_info P(0,3) correctly set? Note that " 201 | "baseline=-P(0,3)/P(0,0). This warning is printed only once.", 202 | stereoModel.baseline()); 203 | shown = true; 204 | } 205 | } 206 | 207 | cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy( 208 | image_rect_left, image_rect_left->encoding.compare( 209 | sensor_msgs::image_encodings::TYPE_8UC1) == 0 || 210 | image_rect_left->encoding.compare( 211 | sensor_msgs::image_encodings::MONO8) == 0 212 | ? "" 213 | : image_rect_left->encoding.compare( 214 | sensor_msgs::image_encodings::MONO16) != 0 215 | ? "bgr8" 216 | : "mono8"); 217 | cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy( 218 | image_rect_right, image_rect_right->encoding.compare( 219 | sensor_msgs::image_encodings::TYPE_8UC1) == 0 || 220 | image_rect_right->encoding.compare( 221 | sensor_msgs::image_encodings::MONO8) == 0 222 | ? "" 223 | : "mono8"); 224 | 225 | auto data = std::make_shared( 226 | ptrImageLeft->image, ptrImageRight->image, stereoModel, 227 | 0, rtabmap_conversions::timestampFromROS(stamp)); 228 | 229 | received_data_queue_.push_back(std::make_pair(data, odom)); 230 | if (received_data_queue_.size() > max_queue_size_) { 231 | // Remove the oldest keyframes if we exceed the maximum size 232 | received_data_queue_.pop_front(); 233 | RCLCPP_WARN( 234 | node_->get_logger(), 235 | "Stereo: Maximum queue size (%d) exceeded, the oldest element was removed.", 236 | max_queue_size_); 237 | } 238 | 239 | if (enable_gps_recording_) { 240 | received_gps_queue_.push_back(latest_gps_fix_); 241 | if (received_gps_queue_.size() > max_queue_size_) 242 | { 243 | received_gps_queue_.pop_front(); 244 | } 245 | } 246 | } else { 247 | RCLCPP_WARN(node_->get_logger(), "Odom: input images empty?!"); 248 | } 249 | } 250 | 251 | void StereoHandler::local_descriptors_msg_to_sensor_data( 252 | const std::shared_ptr< 253 | cslam_common_interfaces::msg::LocalImageDescriptors> 254 | msg, 255 | rtabmap::SensorData &sensor_data) { 256 | // Fill descriptors 257 | rtabmap::StereoCameraModel stereo_model = 258 | rtabmap_conversions::stereoCameraModelFromROS(msg->data.rgb_camera_info, 259 | msg->data.depth_camera_info, 260 | rtabmap::Transform::getIdentity()); 261 | sensor_data = rtabmap::SensorData( 262 | cv::Mat(), cv::Mat(), stereo_model, 0, 263 | rtabmap_conversions::timestampFromROS(msg->data.header.stamp)); 264 | 265 | std::vector kpts; 266 | rtabmap_conversions::keypointsFromROS(msg->data.key_points, kpts); 267 | std::vector kpts3D; 268 | rtabmap_conversions::points3fFromROS(msg->data.points, kpts3D); 269 | auto descriptors = rtabmap::uncompressData(msg->data.descriptors); 270 | sensor_data.setFeatures(kpts, kpts3D, descriptors); 271 | } 272 | 273 | void StereoHandler::send_visualization(const std::pair, std::shared_ptr> &keypoints_data) 274 | { 275 | send_visualization_keypoints(keypoints_data); 276 | } 277 | -------------------------------------------------------------------------------- /src/front_end/visualization_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "cslam/front_end/visualization_utils.h" 2 | 3 | namespace cslam 4 | { 5 | 6 | // Conversions implementations are from https://github.com/ros-perception/image_pipeline/tree/foxy/depth_image_proc 7 | 8 | sensor_msgs::msg::PointCloud2 create_colored_pointcloud(const std::shared_ptr & sensor_data, const std_msgs::msg::Header & header) 9 | { 10 | // Update camera model 11 | image_geometry::PinholeCameraModel model; 12 | sensor_msgs::msg::CameraInfo info_msg; 13 | rtabmap_conversions::cameraModelToROS(sensor_data->cameraModels()[0], info_msg); 14 | model.fromCameraInfo(info_msg); 15 | 16 | auto cloud_msg = std::make_shared(); 17 | cloud_msg->header = header; 18 | cloud_msg->height = sensor_data->depthRaw().rows; 19 | cloud_msg->width = sensor_data->depthRaw().cols; 20 | cloud_msg->is_dense = false; 21 | cloud_msg->is_bigendian = false; 22 | 23 | if (sensor_data->depthRaw().cols != sensor_data->imageRaw().cols || sensor_data->depthRaw().rows != sensor_data->imageRaw().rows) { 24 | RCLCPP_WARN( 25 | rclcpp::get_logger("cslam"), 26 | "Depth image size (%dx%d) doesn't match RGB image size (%dx%d)!", 27 | sensor_data->depthRaw().cols, sensor_data->depthRaw().rows, sensor_data->imageRaw().cols, sensor_data->imageRaw().rows); 28 | cloud_msg->height = 0; 29 | cloud_msg->width = 0; 30 | return *cloud_msg; 31 | } 32 | 33 | sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); 34 | pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); 35 | 36 | // Convert Depth Image to Pointcloud 37 | if (sensor_data->depthRaw().type() == CV_16UC1) { 38 | depth_image_to_pointcloud(sensor_data, cloud_msg, model); 39 | } else if (sensor_data->depthRaw().type() == CV_32FC1) { 40 | depth_image_to_pointcloud(sensor_data, cloud_msg, model); 41 | } else { 42 | // Depth image has unsupported encoding 43 | return *cloud_msg; 44 | } 45 | 46 | // Add colors to pointcloud 47 | if (sensor_data->imageRaw().type() == CV_8UC3) { 48 | add_rgb_to_pointcloud(sensor_data, cloud_msg); 49 | } else if (sensor_data->imageRaw().type() == CV_8UC1) { 50 | add_rgb_to_pointcloud(sensor_data, cloud_msg); 51 | } else { 52 | // RGB image has unsupported encoding 53 | return *cloud_msg; 54 | } 55 | return *cloud_msg; 56 | } 57 | 58 | // Handles float or uint16 depths 59 | template 60 | void depth_image_to_pointcloud( 61 | const std::shared_ptr & sensor_data, 62 | sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg, 63 | const image_geometry::PinholeCameraModel & model, 64 | double range_max) 65 | { 66 | // Use correct principal point from calibration 67 | float center_x = model.cx(); 68 | float center_y = model.cy(); 69 | 70 | // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) 71 | double unit_scaling = depth_image_proc::DepthTraits::toMeters(T(1) ); 72 | float constant_x = unit_scaling / model.fx(); 73 | float constant_y = unit_scaling / model.fy(); 74 | float bad_point = std::numeric_limits::quiet_NaN(); 75 | 76 | sensor_msgs::PointCloud2Iterator iter_x(*cloud_msg, "x"); 77 | sensor_msgs::PointCloud2Iterator iter_y(*cloud_msg, "y"); 78 | sensor_msgs::PointCloud2Iterator iter_z(*cloud_msg, "z"); 79 | const T * depth_row = reinterpret_cast(&sensor_data->depthRaw().data[0]); 80 | int row_step = sensor_data->depthRaw().step / sizeof(T); 81 | for (int v = 0; v < static_cast(cloud_msg->height); ++v, depth_row += row_step) { 82 | for (int u = 0; u < static_cast(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) { 83 | T depth = depth_row[u]; 84 | 85 | // Missing points denoted by NaNs 86 | if (!depth_image_proc::DepthTraits::valid(depth)) { 87 | if (range_max != 0.0) { 88 | depth = depth_image_proc::DepthTraits::fromMeters(range_max); 89 | } else { 90 | *iter_x = *iter_y = *iter_z = bad_point; 91 | continue; 92 | } 93 | } 94 | 95 | // Fill in XYZ 96 | *iter_x = (u - center_x) * depth * constant_x; 97 | *iter_y = (v - center_y) * depth * constant_y; 98 | *iter_z = depth_image_proc::DepthTraits::toMeters(depth); 99 | } 100 | } 101 | } 102 | 103 | void add_rgb_to_pointcloud( 104 | const std::shared_ptr & sensor_data, 105 | sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg) 106 | { 107 | // Supported color encodings: BGR8, MONO8 108 | int red_offset, green_offset, blue_offset, color_step; 109 | if (sensor_data->imageRaw().type() == CV_8UC3) { 110 | red_offset = 2; 111 | green_offset = 1; 112 | blue_offset = 0; 113 | color_step = 3; 114 | } else if (sensor_data->imageRaw().type() == CV_8UC1) { 115 | red_offset = 0; 116 | green_offset = 0; 117 | blue_offset = 0; 118 | color_step = 1; 119 | } else { 120 | return; 121 | } 122 | 123 | sensor_msgs::PointCloud2Iterator iter_r(*cloud_msg, "r"); 124 | sensor_msgs::PointCloud2Iterator iter_g(*cloud_msg, "g"); 125 | sensor_msgs::PointCloud2Iterator iter_b(*cloud_msg, "b"); 126 | const uint8_t * rgb = &sensor_data->imageRaw().data[0]; 127 | int rgb_skip = sensor_data->imageRaw().step - sensor_data->imageRaw().cols * color_step; 128 | for (int v = 0; v < static_cast(cloud_msg->height); ++v, rgb += rgb_skip) { 129 | for (int u = 0; u < static_cast(cloud_msg->width); ++u, 130 | rgb += color_step, ++iter_r, ++iter_g, ++iter_b) 131 | { 132 | *iter_r = rgb[red_offset]; 133 | *iter_g = rgb[green_offset]; 134 | *iter_b = rgb[blue_offset]; 135 | } 136 | } 137 | } 138 | 139 | } // namespace cslam -------------------------------------------------------------------------------- /tests/README.md: -------------------------------------------------------------------------------- 1 | ## Unit Tests 2 | This folder contains unit tests for the developed libraries. 3 | 4 | Additional integration tests (i.e., ROS 2 nodes and their interactions) are available in the [cslam_tests](TODO) repository. 5 | 6 | All tests can be run by running the following command at the root of the workspace: 7 | ``` 8 | colcon test 9 | ``` 10 | 11 | TODO: evo dependency -------------------------------------------------------------------------------- /tests/test_broker.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from cslam.broker import Broker 4 | from cslam.algebraic_connectivity_maximization import AlgebraicConnectivityMaximization, EdgeInterRobot 5 | from cslam.loop_closure_sparse_matching import LoopClosureSparseMatching 6 | from test_algebraic_connectivity import build_multi_robot_graph 7 | import random 8 | import numpy as np 9 | import networkx as nx 10 | import scipy 11 | from collections import namedtuple 12 | import math 13 | 14 | 15 | def build_graph_and_extract_selection(nb_poses, nb_candidate_edges, max_nb_robots, 16 | robot_id, nb_candidates_to_choose): 17 | """Build a graph and perform selection based on algebraic connectivity 18 | """ 19 | fixed_edges_list, candidate_edges_list = build_multi_robot_graph( 20 | nb_poses, nb_candidate_edges, max_nb_robots) 21 | 22 | params = {} 23 | params['robot_id'] = robot_id 24 | params['max_nb_robots'] = max_nb_robots 25 | params['frontend.similarity_threshold'] = 0.0 # Doesn't change anything 26 | params['frontend.sensor_type'] = 'stereo' 27 | params["frontend.enable_sparsification"] = True 28 | params["evaluation.enable_sparsification_comparison"] = False 29 | lcsm = LoopClosureSparseMatching(params) 30 | lcsm.candidate_selector.set_graph(fixed_edges_list, candidate_edges_list) 31 | 32 | # Solve the initial graph 33 | is_robot_considered = {} 34 | for i in range(max_nb_robots): 35 | is_robot_considered[i] = True 36 | return lcsm.select_candidates(nb_candidates_to_choose, 37 | is_robot_considered, 38 | greedy_initialization=False) 39 | 40 | 41 | def verif_broker(unittest_framework, nb_poses, nb_candidate_edges, max_nb_robots, 42 | robot_id, nb_candidates_to_choose, use_vertex_cover): 43 | """Test broker based on params 44 | """ 45 | # Build graph 46 | selection = build_graph_and_extract_selection(nb_poses, nb_candidate_edges, 47 | max_nb_robots, robot_id, 48 | nb_candidates_to_choose) 49 | unittest_framework.assertEqual( 50 | len(selection), min(nb_candidate_edges, nb_candidates_to_choose)) 51 | neighbors_in_range_list = range(max_nb_robots) 52 | 53 | # Brokerage 54 | broker = Broker(selection, neighbors_in_range_list) 55 | components = broker.brokerage(use_vertex_cover) 56 | 57 | # Extract vertices 58 | initial_vertices = set() 59 | duplicates = [] 60 | for e in selection: 61 | v0 = (e.robot0_id, e.robot0_keyframe_id) 62 | if v0 in initial_vertices: 63 | duplicates.append(v0) 64 | initial_vertices.add(v0) 65 | v1 = (e.robot1_id, e.robot1_keyframe_id) 66 | if v1 in initial_vertices: 67 | duplicates.append(v1) 68 | initial_vertices.add(v1) 69 | # Sanity check 70 | unittest_framework.assertEqual( 71 | len(initial_vertices) + len(duplicates), 72 | min(nb_candidate_edges, nb_candidates_to_choose) * 2) 73 | 74 | vertices = [] 75 | for c in components: 76 | for v in c: 77 | vertices.append(v) 78 | 79 | # Check that we do not send more vertices than the trivial solution 80 | # Trivial solution: sending one vertex per edge 81 | # Upper Bounds: 82 | unittest_framework.assertLessEqual( 83 | len(vertices), min(nb_candidate_edges, nb_candidates_to_choose)) 84 | if use_vertex_cover and max_nb_robots == 2: 85 | unittest_framework.assertLessEqual( 86 | len(vertices), math.ceil(float(len(initial_vertices)) / 2)) 87 | # Lower Bounds: 88 | unittest_framework.assertGreaterEqual(len(vertices), 1) 89 | 90 | # Check no duplicates 91 | for i in range(len(vertices)): 92 | for j in range(len(vertices)): 93 | if i != j: 94 | unittest_framework.assertNotEqual(vertices[i], vertices[j]) 95 | 96 | # Check that all edges are covered 97 | for e in selection: 98 | v0 = (e.robot0_id, e.robot0_keyframe_id) 99 | v1 = (e.robot1_id, e.robot1_keyframe_id) 100 | v0_in = v0 in vertices 101 | v1_in = v1 in vertices 102 | unittest_framework.assertTrue(v0_in or v1_in) 103 | 104 | 105 | class TestBroker(unittest.TestCase): 106 | """Unit tests for communication broker 107 | """ 108 | 109 | def test_simple_dialog_2robots(self): 110 | """Simple dialog strategy 111 | """ 112 | # Parameters 113 | robot_id = 0 114 | nb_poses = 100 115 | nb_candidate_edges = 50 116 | max_nb_robots = 2 117 | nb_candidates_to_choose = 30 118 | use_vertex_cover = False 119 | 120 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 121 | nb_candidates_to_choose, use_vertex_cover) 122 | 123 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 124 | nb_candidate_edges, use_vertex_cover) 125 | 126 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 127 | robot_id, nb_candidates_to_choose * 10, use_vertex_cover) 128 | 129 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 130 | robot_id, nb_candidate_edges * 10, use_vertex_cover) 131 | 132 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 133 | 2 * nb_candidate_edges, use_vertex_cover) 134 | 135 | def test_vertex_cover_2robots(self): 136 | """Vertex cover strategy 137 | """ 138 | # Parameters 139 | robot_id = 0 140 | nb_poses = 100 141 | nb_candidate_edges = 50 142 | max_nb_robots = 2 143 | nb_candidates_to_choose = 30 144 | use_vertex_cover = True 145 | 146 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 147 | nb_candidates_to_choose, use_vertex_cover) 148 | 149 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 150 | nb_candidate_edges, use_vertex_cover) 151 | 152 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 153 | robot_id, nb_candidates_to_choose * 10, use_vertex_cover) 154 | 155 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 156 | robot_id, nb_candidate_edges * 10, use_vertex_cover) 157 | 158 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 159 | 2 * nb_candidate_edges, use_vertex_cover) 160 | 161 | def test_simple_dialog_5robots(self): 162 | """Simple dialog strategy 163 | """ 164 | # Parameters 165 | robot_id = 1 166 | nb_poses = 100 167 | nb_candidate_edges = 200 168 | max_nb_robots = 5 169 | nb_candidates_to_choose = 100 170 | use_vertex_cover = False 171 | 172 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 173 | nb_candidates_to_choose, use_vertex_cover) 174 | 175 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 176 | nb_candidate_edges, use_vertex_cover) 177 | 178 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 179 | robot_id, nb_candidates_to_choose * 10, use_vertex_cover) 180 | 181 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 182 | robot_id, nb_candidate_edges * 10, use_vertex_cover) 183 | 184 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 185 | 2 * nb_candidate_edges, use_vertex_cover) 186 | 187 | def test_vertex_cover_5robots(self): 188 | """Vertex cover strategy 189 | """ 190 | # Parameters 191 | robot_id = 2 192 | nb_poses = 100 193 | nb_candidate_edges = 200 194 | max_nb_robots = 5 195 | nb_candidates_to_choose = 100 196 | use_vertex_cover = True 197 | 198 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 199 | nb_candidates_to_choose, use_vertex_cover) 200 | 201 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 202 | nb_candidate_edges, use_vertex_cover) 203 | 204 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 205 | robot_id, nb_candidates_to_choose * 10, use_vertex_cover) 206 | 207 | verif_broker(self, nb_poses * 10, nb_candidate_edges * 10, max_nb_robots, 208 | robot_id, nb_candidate_edges * 10, use_vertex_cover) 209 | 210 | verif_broker(self, nb_poses, nb_candidate_edges, max_nb_robots, robot_id, 211 | 2 * nb_candidate_edges, use_vertex_cover) 212 | 213 | def test_manual_vertex_cover(self): 214 | # Build graph 215 | fixed_edges_list = [] 216 | candidate_edges_list = [] 217 | fixed_weight = 1.0 218 | candidate_edges_list.append(EdgeInterRobot(0, 1, 1, 1, fixed_weight)) 219 | candidate_edges_list.append(EdgeInterRobot(0, 1, 1, 2, fixed_weight)) 220 | candidate_edges_list.append(EdgeInterRobot(0, 1, 1, 3, fixed_weight)) 221 | candidate_edges_list.append(EdgeInterRobot(0, 1, 1, 4, fixed_weight)) 222 | candidate_edges_list.append(EdgeInterRobot(0, 2, 1, 5, fixed_weight)) 223 | 224 | robot_id = 0 225 | nb_poses = 100 226 | nb_candidate_edges = 5 227 | max_nb_robots = 2 228 | nb_candidates_to_choose = 5 229 | use_vertex_cover = True 230 | 231 | params = {} 232 | params['robot_id'] = robot_id 233 | params['max_nb_robots'] = max_nb_robots 234 | params['frontend.similarity_threshold'] = 0.0 # Doesn't change anything 235 | params['frontend.sensor_type'] = 'stereo' 236 | params["frontend.enable_sparsification"] = True 237 | params["evaluation.enable_sparsification_comparison"] = False 238 | lcsm = LoopClosureSparseMatching(params) 239 | lcsm.candidate_selector.set_graph(fixed_edges_list, candidate_edges_list) 240 | 241 | # Solve the initial graph 242 | is_robot_considered = {} 243 | for i in range(max_nb_robots): 244 | is_robot_considered[i] = True 245 | selection = lcsm.select_candidates(nb_candidates_to_choose, 246 | is_robot_considered, 247 | greedy_initialization=False) 248 | 249 | self.assertEqual( 250 | len(selection), min(nb_candidate_edges, nb_candidates_to_choose)) 251 | neighbors_in_range_list = range(max_nb_robots) 252 | 253 | # Brokerage 254 | broker = Broker(selection, neighbors_in_range_list) 255 | components = broker.brokerage(use_vertex_cover) 256 | 257 | nb_components = len(components) 258 | nb_vertices_transmitted = 0 259 | 260 | for component in components: 261 | for v in component: 262 | nb_vertices_transmitted += 1 263 | 264 | self.assertEqual(nb_components, 2) 265 | self.assertEqual(nb_vertices_transmitted, 2) 266 | 267 | 268 | 269 | 270 | if __name__ == "__main__": 271 | unittest.main() 272 | -------------------------------------------------------------------------------- /tests/test_sparse_matching.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from cslam.loop_closure_sparse_matching import LoopClosureSparseMatching 4 | from cslam.nns_matching import NearestNeighborsMatching 5 | import random 6 | import numpy as np 7 | from collections import namedtuple 8 | 9 | GlobalDescriptor = namedtuple('GlobalDescriptor', 10 | ['keyframe_id', 'robot_id', 'descriptor']) 11 | 12 | def set_params(): 13 | params = {} 14 | params['robot_id'] = 0 15 | params['max_nb_robots'] = 2 16 | params['frontend.sensor_type'] = 'stereo' 17 | params['frontend.similarity_threshold'] = 0.0 18 | params["frontend.enable_sparsification"] = True 19 | params["evaluation.enable_sparsification_comparison"] = False 20 | return params 21 | 22 | 23 | class TestSparseMatching(unittest.TestCase): 24 | """Unit tests for sparse matching 25 | """ 26 | 27 | def test_add_local_global_descriptor(self): 28 | """Add local keyframe 29 | """ 30 | params = set_params() 31 | lcsm = LoopClosureSparseMatching(params) 32 | descriptor = np.random.rand(10) 33 | descriptor = descriptor / np.linalg.norm(descriptor) 34 | lcsm.add_local_global_descriptor(descriptor, 1) 35 | for i in range(len(descriptor)): 36 | self.assertAlmostEqual(descriptor[i], lcsm.local_nnsm.data[0, i]) 37 | 38 | def test_add_other_robot_global_descriptor(self): 39 | """Add other robot keyframe 40 | """ 41 | params = set_params() 42 | lcsm = LoopClosureSparseMatching(params) 43 | descriptor = np.random.rand(10) 44 | descriptor = descriptor / np.linalg.norm(descriptor) 45 | msg = GlobalDescriptor(0, 1, descriptor.tolist()) 46 | lcsm.add_other_robot_global_descriptor(msg) 47 | for i in range(len(descriptor)): 48 | self.assertAlmostEqual(descriptor[i], 49 | lcsm.other_robots_nnsm[1].data[0, i]) 50 | 51 | def test_similarity(self): 52 | """Test that cosine similarity matching gives the same ordering as euclidean distance 53 | """ 54 | nb_descriptors_in_db = 100 55 | nnsm = NearestNeighborsMatching() 56 | for i in range(nb_descriptors_in_db): 57 | descriptor = np.random.rand(100) 58 | descriptor = descriptor / np.linalg.norm(descriptor) # normalize 59 | nnsm.add_item(descriptor, i) 60 | 61 | nb_descriptors_to_test = 100 62 | k = 100 63 | for i in range(nb_descriptors_to_test): 64 | query = np.random.rand(100) 65 | query = query / np.linalg.norm(query) 66 | ds = np.linalg.norm(query[np.newaxis, :] - nnsm.data[:nnsm.n], axis=1) 67 | ns_dist = np.argsort(ds)[:k] 68 | ns_sim, sims = nnsm.search(query, k) 69 | # Check if sorted by similarity 70 | self.assertTrue(np.all(sims[:-1] >= sims[1:])) 71 | # Check if same order as distance 72 | for j in range(k): 73 | id_d = ns_dist[j] 74 | id_s = ns_sim[j] 75 | if (id_d != id_s): 76 | # If the distance or similarity are the same than the order does not matter. 77 | if (abs(sims[id_d] - sims[id_s]) < 1e-6) or (abs(ds[id_d] - ds[id_s]) < 1e-6): 78 | continue 79 | self.assertEqual(ns_dist[j], ns_sim[j]) 80 | ns_sim_best, sim_best = nnsm.search_best(query) 81 | self.assertEqual(ns_dist[0], ns_sim_best) 82 | 83 | def test_matches(self): 84 | """Matches between descriptors 85 | """ 86 | params = set_params() 87 | lcsm = LoopClosureSparseMatching(params) 88 | 89 | descriptor0 = np.random.rand(10) 90 | descriptor0 = descriptor0 / np.linalg.norm(descriptor0) 91 | lcsm.add_local_global_descriptor(descriptor0, 2) 92 | 93 | descriptor1 = 1 - descriptor0 94 | descriptor1 = descriptor1 / np.linalg.norm(descriptor1) 95 | msg = GlobalDescriptor(3, 1, descriptor1.tolist()) 96 | lcsm.add_other_robot_global_descriptor(msg) 97 | 98 | descriptor2 = descriptor0 99 | descriptor2[0] = 0.0 100 | descriptor2[1] = 0.0 101 | descriptor2 = descriptor2 / np.linalg.norm(descriptor2) 102 | msg2 = GlobalDescriptor(4, 1, descriptor2.tolist()) 103 | lcsm.add_other_robot_global_descriptor(msg2) 104 | 105 | id = lcsm.candidate_selector.candidate_edges[(0, 2, 1, 4)].robot1_id 106 | best_match_descriptor = lcsm.other_robots_nnsm[id].data[0] 107 | for i in range(len(descriptor1)): 108 | self.assertAlmostEqual(descriptor1[i], best_match_descriptor[i]) 109 | 110 | def test_select_candidates0(self): 111 | """Select candidates 112 | """ 113 | params = set_params() 114 | params['max_nb_robots'] = 3 115 | lcsm = LoopClosureSparseMatching(params) 116 | 117 | nb_local_kfs = 100 118 | for i in range(nb_local_kfs): 119 | descriptor = np.random.rand(10) 120 | descriptor = descriptor / np.linalg.norm(descriptor) 121 | lcsm.add_local_global_descriptor(descriptor, i) 122 | nb_other_kfs = 100 123 | for i in range(nb_other_kfs): 124 | descriptor = np.random.rand(10) 125 | descriptor = descriptor / np.linalg.norm(descriptor) 126 | msg = GlobalDescriptor(i, 1, descriptor.tolist()) 127 | lcsm.add_other_robot_global_descriptor(msg) 128 | for i in range(nb_other_kfs): 129 | descriptor = np.random.rand(10) 130 | descriptor = descriptor / np.linalg.norm(descriptor) 131 | msg = GlobalDescriptor(i, 2, descriptor.tolist()) 132 | lcsm.add_other_robot_global_descriptor(msg) 133 | 134 | nb_candidates = 20 135 | is_robot_considered = {} 136 | for i in range(params['max_nb_robots']): 137 | is_robot_considered[i] = True 138 | selection = lcsm.select_candidates(nb_candidates, is_robot_considered) 139 | self.assertEqual(len(selection), nb_candidates) 140 | 141 | def test_select_candidates1(self): 142 | """Select candidates 143 | No robot 1 in range 144 | """ 145 | params = set_params() 146 | params['max_nb_robots'] = 4 147 | lcsm = LoopClosureSparseMatching(params) 148 | 149 | nb_local_kfs = 100 150 | for i in range(nb_local_kfs): 151 | descriptor = np.random.rand(10) 152 | descriptor = descriptor / np.linalg.norm(descriptor) 153 | lcsm.add_local_global_descriptor(descriptor, i) 154 | nb_other_kfs = 100 155 | for i in range(nb_other_kfs): 156 | descriptor = np.random.rand(10) 157 | descriptor = descriptor / np.linalg.norm(descriptor) 158 | msg = GlobalDescriptor(i, 2, descriptor.tolist()) 159 | lcsm.add_other_robot_global_descriptor(msg) 160 | for i in range(nb_other_kfs): 161 | descriptor = np.random.rand(10) 162 | descriptor = descriptor / np.linalg.norm(descriptor) 163 | msg = GlobalDescriptor(i, 3, descriptor.tolist()) 164 | lcsm.add_other_robot_global_descriptor(msg) 165 | 166 | nb_candidates = 20 167 | 168 | is_robot_considered = {} 169 | for i in range(params['max_nb_robots']): 170 | is_robot_considered[i] = True 171 | selection = lcsm.select_candidates(nb_candidates, is_robot_considered) 172 | self.assertEqual(len(selection), nb_candidates) 173 | 174 | def test_select_candidates2(self): 175 | """Select candidates 176 | No robot 0 in range 177 | """ 178 | params = set_params() 179 | params['max_nb_robots'] = 4 180 | params['robot_id'] = 1 181 | lcsm = LoopClosureSparseMatching(params) 182 | 183 | nb_local_kfs = 100 184 | for i in range(nb_local_kfs): 185 | descriptor = np.random.rand(10) 186 | descriptor = descriptor / np.linalg.norm(descriptor) 187 | lcsm.add_local_global_descriptor(descriptor, i) 188 | nb_other_kfs = 100 189 | for i in range(nb_other_kfs): 190 | descriptor = np.random.rand(10) 191 | descriptor = descriptor / np.linalg.norm(descriptor) 192 | msg = GlobalDescriptor(i, 2, descriptor.tolist()) 193 | lcsm.add_other_robot_global_descriptor(msg) 194 | for i in range(nb_other_kfs): 195 | descriptor = np.random.rand(10) 196 | descriptor = descriptor / np.linalg.norm(descriptor) 197 | msg = GlobalDescriptor(i, 3, descriptor.tolist()) 198 | lcsm.add_other_robot_global_descriptor(msg) 199 | 200 | nb_candidates = 20 201 | 202 | is_robot_considered = {} 203 | for i in range(params['max_nb_robots']): 204 | is_robot_considered[i] = True 205 | 206 | selection = lcsm.select_candidates(nb_candidates, is_robot_considered) 207 | self.assertEqual(len(selection), nb_candidates) 208 | 209 | 210 | if __name__ == "__main__": 211 | unittest.main() 212 | --------------------------------------------------------------------------------