├── .gitignore ├── CMakeLists.txt ├── README.md ├── assets ├── 0_two_loops.png └── 8_two_loops.png ├── config ├── config.yaml └── std_livox.yaml ├── include ├── evaluator.h ├── merge_messages.h └── methods │ ├── STD.h │ ├── STD │ └── STDesc.h │ ├── base.h │ ├── dbow.h │ ├── dbow │ └── orb_vocab.yml.gz │ ├── scan_context.h │ └── scan_context │ ├── KDTreeVectorOfVectorsAdaptor.h │ ├── Scancontext.h │ ├── nanoflann.hpp │ └── tictoc.h ├── launch ├── base.launch ├── evaluate_cpp.launch ├── evaluate_python.launch ├── merge.launch └── methods │ ├── context.launch │ ├── dbow.launch │ ├── logg3d.launch │ ├── mix_vpr.launch │ ├── std.launch │ └── superglue.launch ├── package.xml ├── paper.pdf ├── requirements.txt ├── results ├── .gitignore └── get_results.py ├── scripts ├── __init__.py ├── evaluate.py ├── evaluator.py ├── methods │ ├── LoGG3D.py │ ├── MixVPR_.py │ ├── SuperGlue.py │ ├── __init__.py │ ├── base.py │ └── superglue │ │ ├── __init__.py │ │ ├── matching.py │ │ ├── superglue.py │ │ ├── superpoint.py │ │ ├── utils.py │ │ └── weights │ │ └── .gitignore └── plot_pr_curve.py ├── setup.py └── src ├── dbow_create_vocab.cpp ├── evaluate.cpp ├── evaluator.cpp ├── merge_messages.cpp ├── methods ├── STD.cpp ├── STD │ └── STDesc.cpp ├── dbow.cpp ├── scan_context.cpp └── scan_context │ └── Scancontext.cpp └── msg_converter.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.png 3 | *.ply 4 | *.pcd 5 | *.egg-info 6 | *.bag 7 | .vscode 8 | build/ 9 | LoGG3D_Net/ 10 | MixVPR/ 11 | !assets/*.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(place_recog_eval) 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(PYTHON_VERSION 3) 5 | 6 | ## Compile as C++11, supported in ROS Kinetic and newer 7 | # add_compile_options(-std=c++11) 8 | message("CATKIN_PACKAGE_BIN_DESTINATION: ${CATKIN_PACKAGE_BIN_DESTINATION}") 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | pcl_conversions 15 | pcl_ros 16 | sensor_msgs 17 | geometry_msgs 18 | nav_msgs 19 | message_filters 20 | cv_bridge 21 | image_transport 22 | tf_conversions 23 | livox_ros_driver 24 | ) 25 | 26 | ## System dependencies are found with CMake's conventions 27 | find_package(Boost REQUIRED COMPONENTS system) 28 | find_package(PCL 1.8 REQUIRED) 29 | find_package(OpenCV REQUIRED) 30 | find_package(Eigen3 REQUIRED) 31 | find_package(Ceres REQUIRED) 32 | 33 | catkin_package( 34 | INCLUDE_DIRS include 35 | LIBRARIES ${PROJECT_NAME} 36 | CATKIN_DEPENDS roscpp rospy std_msgs pcl_conversions pcl_ros sensor_msgs geometry_msgs nav_msgs message_filters cv_bridge image_transport 37 | DEPENDS Boost PCL OpenCV 38 | ) 39 | 40 | include_directories( 41 | include 42 | include/methods/ 43 | include/methods/scan_context/ 44 | include/methods/STD/ 45 | ${catkin_INCLUDE_DIRS} 46 | ${Boost_INCLUDE_DIRS} 47 | ${PCL_INCLUDE_DIRS} 48 | ${OpenCV_INCLUDE_DIRS} 49 | ${EIGEN3_INCLUDE_DIR} 50 | ) 51 | 52 | link_directories(${PCL_LIBRARY_DIRS}) 53 | add_definitions(${PCL_DEFINITIONS}) 54 | 55 | # Add source files to a variable 56 | set(EVALUATE_SRC 57 | src/evaluate.cpp 58 | src/evaluator.cpp 59 | src/methods/dbow.cpp 60 | src/methods/scan_context.cpp 61 | src/methods/scan_context/Scancontext.cpp 62 | src/methods/STD.cpp 63 | src/methods/STD/STDesc.cpp 64 | ) 65 | 66 | 67 | add_executable(evaluate ${EVALUATE_SRC}) 68 | target_link_libraries(evaluate 69 | ${catkin_LIBRARIES} 70 | ${Boost_LIBRARIES} 71 | ${PCL_LIBRARIES} 72 | ${OpenCV_LIBRARIES} 73 | ${EIGEN3_LIBRARIES} 74 | ${CERES_LIBRARIES} 75 | DBoW2 76 | ) 77 | 78 | add_executable(merge_messages src/merge_messages.cpp) 79 | target_link_libraries(merge_messages 80 | ${catkin_LIBRARIES} 81 | ${Boost_LIBRARIES} 82 | ${PCL_LIBRARIES} 83 | ${OpenCV_LIBRARIES} 84 | ${EIGEN3_LIBRARIES} 85 | ) 86 | 87 | add_executable(msg_converter src/msg_converter.cpp) 88 | target_link_libraries(msg_converter 89 | ${catkin_LIBRARIES} 90 | ${PCL_LIBRARIES} 91 | ) 92 | 93 | add_executable(dbow_create_vocab src/dbow_create_vocab.cpp src/methods/dbow.cpp) 94 | target_link_libraries(dbow_create_vocab 95 | ${catkin_LIBRARIES} 96 | ${OpenCV_LIBRARIES} 97 | DBoW2 98 | ) 99 | 100 | # add_executable(place_recognition_livox src/methods/place_recognition_livox.cpp src/methods/STD/STDesc.cpp ) 101 | # target_link_libraries(place_recognition_livox 102 | # ${catkin_LIBRARIES} 103 | # ${PCL_LIBRARIES} 104 | # ${OpenCV_LIBS} 105 | # ${CERES_LIBRARIES}) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Table of Contents 2 | 1. [Introduction](#introduction) 3 | 2. [Getting Started](#getting-started) 4 | - [Dependencies](#dependencies) 5 | - [Installation](#installation) 6 | - [LoGG3D-Net Setup](#logg3d-net-setup) 7 | - [SuperPoint + SuperGlue Setup](#superpoint--superglue-setup) 8 | - [MixVPR Setup](#mixvpr-setup) 9 | 3. [Rosbag Files](#rosbag-files) 10 | - [Office Data](#office-data) 11 | - [Garage Data](#garage-data) 12 | 4. [Usage](#usage) 13 | - [Option 1: Launch Rosbag file from launch file](#option-1-launch-rosbag-file-from-launch-file) 14 | - [Option 2: Launch Rosbag file from terminal](#option-2-launch-rosbag-file-from-terminal) 15 | - [Launch Evaluation](#launch-evaluation) 16 | - [Launch Custom Evaluation](#launch-custom-evaluation) 17 | - [Evaluate combination of methods](#evaluate-combination-of-methods) 18 | 5. [Create own vocabulary for DBoW2](#create-own-vocabulary-for-dbow2) 19 | 6. [Results](#results) 20 | - [Distance-based evaluation](#distance-based-evaluation) 21 | - [Distance and angle-based evaluation](#distance-and-angle-based-evaluation) 22 | - [Combined models evaluation (distance-based)](#combined-models-evaluation-distance-based) 23 | - [Combined models evaluation (distance and angle-based)](#combined-models-evaluation-distance-and-angle-based) 24 | - [Execution time performance](#execution-time-performance) 25 | 7. [Citation](#citation) 26 | 27 | 28 | # Introduction 29 | This repository was made to perform comparison and evaluation between different approaches for place recognition. The goal is to provide a comprehensive benchmarking environment for researchers and practitioners to analyze the performance of various place recognition methods. The repository contains the following methods: 30 | 31 | 1. [SuperPoint + SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) 32 | 2. [LoGG3D-Net](https://github.com/csiro-robotics/LoGG3D-Net) 33 | 3. [Scan Context](https://github.com/irapkaist/scancontext) 34 | 4. [DBoW2](https://github.com/dorian3d/DBoW2) 35 | 5. [MixVPR](https://github.com/amaralibey/MixVPR) 36 | 6. [STD](https://github.com/hku-mars/STD) 37 | 38 | # Getting Started 39 | ## Dependencies 40 | 41 | * ROS (tested with ROS Noetic) 42 | * Boost 43 | * PCL 1.8 44 | * OpenCV 45 | * Eigen3 46 | * DBoW2 47 | * Ceres 48 | * [Livox ROS driver](https://github.com/Livox-SDK/livox_ros_driver) 49 | 50 | 51 | ## Installation 52 | 53 | 1. Clone the repository into your catkin workspace: 54 | 55 | ``` 56 | cd ~/catkin_ws/src 57 | git clone https://github.com/yourusername/place_recog_eval.git 58 | ``` 59 | 2. Install the required dependencies: 60 | 61 | ``` 62 | sudo apt-get install ros-$(rosversion -d)-pcl-conversions ros-$(rosversion -d)-pcl-ros ros-$(rosversion -d)-message-filters ros-$(rosversion -d)-image-transport ros-$(rosversion -d)-cv-bridge 63 | ``` 64 | 65 | 3. Build the package: 66 | 67 | ``` 68 | cd ~/catkin_ws 69 | catkin_make 70 | ``` 71 | 72 | 4. Source your workspace: 73 | 74 | ``` 75 | source ~/catkin_ws/devel/setup.bash 76 | ``` 77 | 78 | 5. Setup Python environment: 79 | 80 | ``` 81 | pip install -e . 82 | ``` 83 | 84 | ### LoGG3D-Net Setup 85 | 86 | At first clone LoGG3D-Net repository to **`scripts/methods/` directory!**: 87 | ``` 88 | cd ~/catkin_ws/src/Place-recognition-evaluation/scripts/methods/ 89 | git clone https://github.com/csiro-robotics/LoGG3D-Net.git LoGG3D_Net 90 | ``` 91 | 92 | You can create separate environment to install necessary dependencies for LoGG3D-Net. 93 | 94 | * Install requirements: 95 | 96 | ``` 97 | pip install -r requirements.txt 98 | ``` 99 | 100 | * Install PyTorch with suitable cudatoolkit version. For example: 101 | ``` 102 | pip install torch==1.13.1+cu116 torchvision==0.14.1+cu116 torchaudio==0.13.1 --extra-index-url https://download.pytorch.org/whl/cu116 103 | ``` 104 | 105 | * Install `torchsparse-1.4.0` 106 | 107 | ``` 108 | sudo apt-get install libsparsehash-dev 109 | pip install --upgrade git+https://github.com/mit-han-lab/torchsparse.git@v1.4.0 110 | ``` 111 | 112 | * Download LoGG3D-Net pre-trained models 113 | 114 | ``` 115 | cd scripts/methods/LoGG3D_Net/ 116 | wget -O checkpoints.zip https://cloudstor.aarnet.edu.au/plus/s/G9z6VzR72TRm09S/download 117 | unzip checkpoints.zip 118 | ``` 119 | 120 | 121 | ### SuperPoint + SuperGlue Setup 122 | 123 | 1. Install the required dependencies: 124 | ``` 125 | pip3 install numpy opencv-python torch matplotlib 126 | ``` 127 | 128 | 2. Download the pretrained models from the SuperGlue repository: 129 | ``` 130 | cd scripts/methods/superglue/weights/ 131 | wget https://github.com/magicleap/SuperGluePretrainedNetwork/raw/master/models/weights/superglue_indoor.pth 132 | wget https://github.com/magicleap/SuperGluePretrainedNetwork/raw/master/models/weights/superpoint_v1.pth 133 | ``` 134 | 135 | ### MixVPR Setup 136 | 137 | At first clone MixVPR repository to **`scripts/methods/` directory!**: 138 | ``` 139 | cd ~/catkin_ws/src/Place-recognition-evaluation/scripts/methods/ 140 | git clone https://github.com/amaralibey/MixVPR.git 141 | ``` 142 | 143 | You can create separate environment to install necessary dependencies for MixVPR. 144 | 145 | * Install requirements: 146 | 147 | ``` 148 | pip install -r requirements.txt 149 | ``` 150 | 151 | Download MixVPR pre-trained models [here](https://drive.google.com/file/d/1vuz3PvnR7vxnDDLQrdHJaOA04SQrtk5L/view?usp=share_link) and put it to `scripts/methods/MixVPR/` directory. 152 | 153 | 154 | # Rosbag Files 155 | 156 | These rosbag files contain data collected at Innopolis University for the purpose of evaluating various place recognition methods. The data is stored in two distinct rosbag files, each representing a different environment: an office room and a laboratory. Both environments feature unique geometries and visual appearances, making them suitable for use as training and testing data. 157 | 158 | Each rosbag file contains three topics: 159 | 160 | * `/Odometry` (type: `nav_msgs/Odometry`): Odometry information estimated by the SLAM algorithm 161 | * `/camera/color/image_raw_sync` (type: `sensor_msgs/Image`): RGB images from the Intel Depth Camera D435i 162 | * `/livox/lidar_pc` (type: `sensor_msgs/PointCloud2`): Point cloud data from the Livox LIDAR MID-70 163 | 164 | ## Office Data 165 | 166 | **File**: [office_double_loop.bag](https://innopolis-my.sharepoint.com/:u:/g/personal/r_khusainov_innopolis_ru/EbxIWeJWL25AslMtR9_B6l0BIbix3rvijRL5LP9DNZz4tA?e=ny7b9c) 167 | 168 | The office data contains two loops in and around an office room. See the figure below for a visual representation of the trajectory. 169 | 170 | ![image](./assets/0_two_loops.png) 171 | 172 | ## Garage Data 173 | 174 | **File**: [eight_double_loop.bag](https://innopolis-my.sharepoint.com/:u:/g/personal/r_khusainov_innopolis_ru/ESR00oc6JsFMrPt2d5en0EkBGW5R1DjdN3vbClpj7kf_3g?e=XecIQv) 175 | 176 | The laboratory data captures a figure-eight-shaped trajectory with two loops within a laboratory environment. See the figure below for a visual representation of the trajectory. 177 | 178 | ![image](./assets/8_two_loops.png) 179 | 180 | 181 | # Usage 182 | 183 | It's recommended to read rosbag internally to avoid any delays. To do so, set `use_rosbag` to `true` in the `base.launch` file. If you do evaluation on rosbag file, this is the best option to make results reproducible. 184 | 185 | ## Option 1: Launch Rosbag file from launch file 186 | 187 | 1. In a `base.launch` file set `use_rosbag` to `true`. 188 | 189 | 2. In `config.yaml` file set the desired rosbag file pathes. 190 | 191 | `merge_rosbag_input_path` - source rosbag file with point clouds, odometry and images. 192 | `merge_rosbag_output_path` - path to rosbag file with merged point clouds after preprocessing. 193 | 194 | 3. Launch merge.launch file to merge point clouds from source rosbag file: 195 | 196 | ``` 197 | roslaunch place_recog_eval merge.launch 198 | ``` 199 | 200 | ## Option 2: Launch Rosbag file from terminal 201 | 202 | 1. In a `base.launch` file set `use_rosbag` to `false`. 203 | 204 | 2. Run rosbag file in separate terminal: 205 | 206 | ``` 207 | rosbag play 208 | ``` 209 | 210 | ## Launch Evaluation 211 | 212 | Launch the evaluation node for the desired method: 213 | 214 | * For DBoW: 215 | 216 | ``` 217 | roslaunch place_recog_eval dbow.launch 218 | ``` 219 | 220 | * For Scan Context: 221 | 222 | ``` 223 | roslaunch place_recog_eval context.launch 224 | ``` 225 | 226 | * For LoGG3D-Net: 227 | 228 | ``` 229 | roslaunch place_recog_eval logg3d.launch 230 | ``` 231 | 232 | * For SuperPoint + SuperGlue: 233 | 234 | ``` 235 | roslaunch place_recog_eval superglue.launch 236 | ``` 237 | 238 | * For MixVPR: 239 | 240 | ``` 241 | roslaunch place_recog_eval mix_vpr.launch 242 | ``` 243 | 244 | * For STD: 245 | 246 | ``` 247 | roslaunch place_recog_eval std.launch 248 | ``` 249 | 250 | The evaluation results will be printed on the terminal and trajectory path will be saved as an image. 251 | 252 | You have the option to modify the threshold or other parameters in the respective launch files. By default, the best parameters for each method have been fine-tuned on `office_double_loop.bag`. To further customize the settings, you can edit the `config.yaml` file. 253 | 254 | 255 | ## Launch Custom Evaluation 256 | 257 | You can modify `evaluate.cpp` or `evaluate.py` files to run custom evaluation. It can be useful if you want to find optimal parameters for your method. All you need is to modify `getMethods()` function and add your method to the list of methods. For example: 258 | 259 | To find an optimal threshold for Scan Context just add: 260 | ```cpp 261 | for (double threshold = 0.01; threshold < 0.1; threshold += 0.01) 262 | { 263 | BaseMethod *method = new ScanContext(threshold, 0.2); 264 | methods.push_back(method); 265 | } 266 | ``` 267 | 268 | Then you can run evaluation by launching`evaluate_cpp.launch` or `evaluate_py.launch` files: 269 | 270 | ``` 271 | roslaunch place_recog_eval evaluate_cpp.launch 272 | ``` 273 | 274 | or (for Python): 275 | 276 | ``` 277 | roslaunch place_recog_eval evaluate_py.launch 278 | ``` 279 | 280 | In the `results` folder precision-recall curve will be saved as an image. 281 | 282 | ## Evaluate combination of methods 283 | 284 | 1. Set `save_candidates` to `true` in `config.yaml` file. This will save predicted and real candidates for each method in the `results` folder. 285 | 286 | 2. Launch evaluation for each method separately. Depends on `angle_consideration` parameter, results will be stored in `no_angle` or `with_angle` folder in `results` folder. 287 | 288 | 3. Run `get_results.py` script in `results` folder to get results for combination of methods for each folder (`no_angle` and `with_angle`)): 289 | 290 | ``` 291 | python get_results.py 292 | ``` 293 | It's used element-wise multiplication of predicted candidates for each method to get final candidates matrix. Then precision, recall, f1 score and accuracy are calculated for final candidates matrix. 294 | 295 | **Note**: after evaluation for each method separately, ground truth file will be created in `results` folder (this file looks like this: `real_*.txt`). You have to be make sure, that all `real_*.txt` files are the same. If they are different, corresponding error will be printed in the terminal. 296 | 297 | 298 | # Create own vocabulary for DBoW2 299 | 300 | 1. Run roscore: 301 | 302 | ``` 303 | roscore 304 | ``` 305 | 306 | 2. Run the vocabulary creator node: 307 | 308 | ``` 309 | rosrun place_recog_eval dbow_create_vocab 310 | ``` 311 | 312 | 3. Run rosbag file in separate terminal: 313 | 314 | ``` 315 | rosbag play 316 | ``` 317 | 318 | The resulting vocabulary will be saved in the `include/methods/dbow/` directory. 319 | 320 | # Results 321 | 322 | ## Distance-based evaluation 323 | 324 | | Method | Precision | Recall | F1 Score | 325 | |-----------------------|-----------|--------|----------| 326 | | DBoW2 | 0.946 | 0.137 | 0.240 | 327 | | SuperPoint + SuperGlue| 0.970 | 0.320 | 0.481 | 328 | | Scan Context | 0.951 | 0.178 | 0.300 | 329 | | LoGG3D-Net | 0.792 | 0.122 | 0.211 | 330 | | MixVPR | 0.786 | 0.008 | 0.016 | 331 | | STD | 0.750 | 0.002 | 0.004 | 332 | 333 | *Table 1: Models evaluation on test data (laboratory environment, eight.bag). True positive here is when two points are close to each other (within 3 meters).* 334 | 335 | ## Distance and angle-based evaluation 336 | 337 | | Method | Precision | Recall | F1 Score | 338 | |-----------------------|-----------|--------|----------| 339 | | DBoW2 | 0.941 | 0.324 | 0.482 | 340 | | SuperPoint + SuperGlue| 0.970 | 0.758 | 0.851 | 341 | | Scan Context | 0.711 | 0.316 | 0.437 | 342 | | LoGG3D-Net | 0.782 | 0.285 | 0.418 | 343 | | MixVPR | 0.786 | 0.019 | 0.036 | 344 | | STD | 0.750 | 0.005 | 0.010 | 345 | 346 | *Table 2: Models evaluation on test data (laboratory environment). True positive here is when two points are close to each other (within 3 meters) and oriented in the same direction (within 45 degrees).* 347 | 348 | ## Combined models evaluation (distance-based) 349 | 350 | | Method | Precision | Recall | F1 Score | 351 | |-------------------------|-----------|--------|----------| 352 | | DBoW2 + LoGG3D | 0.977 | 0.060 | 0.113 | 353 | | DBoW2 + Scan Context | 0.990 | 0.068 | 0.127 | 354 | | DBoW2 + STD | 1.000 | 0.001 | 0.001 | 355 | | (S.P. + S.G.) + Scan C. | 1.000 | 0.124 | 0.220 | 356 | | (S.P. + S.G.) + LoGG3D | 0.970 | 0.116 | 0.207 | 357 | | (S.P. + S.G.) + STD | 1.000 | 0.002 | 0.004 | 358 | | MixVPR + Scan C. | 1.000 | 0.006 | 0.013 | 359 | | MixVPR + LoGG3D | 0.714 | 0.004 | 0.007 | 360 | | MixVPR + STD | 0.000 | 0.000 | 0.000 | 361 | 362 | *Table 3: Combined models evaluation on test data (laboratory environment). True positive here is when two points are close to each other (within 3 meters). Note: S.P. = SuperPoint, S.G. = SuperGlue.* 363 | 364 | ## Combined models evaluation (distance and angle-based) 365 | 366 | | Method | Precision | Recall | F1 Score | 367 | |-------------------------|-----------|--------|----------| 368 | | DBoW2 + LoGG3D | 0.977 | 0.142 | 0.248 | 369 | | DBoW2 + Scan Context | 0.990 | 0.160 | 0.276 | 370 | | DBoW2 + STD | 1.000 | 0.002 | 0.003 | 371 | | (S.P. + S.G.) + Scan C. | 1.000 | 0.294 | 0.454 | 372 | | (S.P. + S.G.) + LoGG3D | 0.970 | 0.275 | 0.429 | 373 | | (S.P. + S.G.) + STD | 1.000 | 0.005 | 0.010 | 374 | | MixVPR + Scan C. | 1.000 | 0.015 | 0.030 | 375 | | MixVPR + LoGG3D | 0.714 | 0.008 | 0.017 | 376 | | MixVPR + STD | 0.000 | 0.000 | 0.000 | 377 | 378 | *Table 4: Combined models evaluation on test data (laboratory environment). True positive here is when two points are close to each other (within 3 meters) and oriented in the same direction (within 45 degrees). Note: S.P. = SuperPoint, S.G. = SuperGlue.* 379 | 380 | ## Execution time performance 381 | 382 | | Method | Processing unit(s) | Total Duration (s) | 383 | |-----------------------|--------------------|--------------------| 384 | | DBoW2 | CPU | 2.82 | 385 | | SuperPoint + SuperGlue| CPU + GPU | 359.36 | 386 | | Scan Context | CPU | 9.46 | 387 | | LoGG3D-Net | CPU + GPU | 9.76 | 388 | | MixVPR | CPU + GPU | 1.79 | 389 | | STD | CPU | 30.30 | 390 | 391 | *Table 5: Execution time performance of each method on 100 frames (i.e., 100 seconds of data).* 392 | 393 | # Citation 394 | 395 | If you use this code for your research, please cite our paper: 396 | 397 | ```bibtex 398 | @inproceedings{efremov2023comparative, 399 | title={A Comparative Analysis of Visual and Point Cloud-Based Place Recognition Methods in Indoor Environment}, 400 | author={Efremov, Ivan and Khafizov, Ramil and Khusainov, Ramil}, 401 | booktitle={2023 21st International Conference on Advanced Robotics (ICAR)}, 402 | pages={128--133}, 403 | year={2023}, 404 | organization={IEEE} 405 | } 406 | ``` 407 | -------------------------------------------------------------------------------- /assets/0_two_loops.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/assets/0_two_loops.png -------------------------------------------------------------------------------- /assets/8_two_loops.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/assets/8_two_loops.png -------------------------------------------------------------------------------- /config/config.yaml: -------------------------------------------------------------------------------- 1 | # Input rosbag file for merging 2 | merge_rosbag_input_path: /home/username/eight_double_loop.bag 3 | 4 | # Output rosbag file for merged point clouds 5 | merge_rosbag_output_path: /home/username/eight_merged.bag 6 | 7 | # Raw topics for merging 8 | lidar_raw_topic: "/livox/lidar_pc" # Raw point cloud topic 9 | odom_raw_topic: "/Odometry" # Raw odometry topic 10 | img_raw_topic: "/camera/color/image_raw_sync" # Raw image topic 11 | 12 | # Topic names for merged data 13 | merged_lidar_topic: "/cloud_merged" # Merged point cloud topic 14 | merged_odom_topic: "/odom_merged" # Merged odometry topic 15 | merged_camera_topic: "/img_merged" # Merged image topic 16 | 17 | # Point cloud accumulation method: 18 | # 0: Queue-like method. Adds one new cloud, removes one old cloud, and publishes. 19 | # 1: Batch-like method. Accumulates N clouds, publishes, and then removes N clouds. 20 | keyframe_type: 1 21 | 22 | # Voxel grid filter leaf size for merging point clouds 23 | merge_leaf_size: 0.05 24 | 25 | # Number of clouds to accumulate before publishing 26 | cloud_size: 10 27 | 28 | # Number of triplets (point cloud, odometry and image) for office_double_loop.bag 29 | # record_size: 950 30 | 31 | # Number of triplets for eight_double_loop.bag 32 | record_size: 1000 33 | 34 | # Minimum index difference between loop closure candidates 35 | min_idx_diff: 1 36 | 37 | # Flag to consider the angle between loop closure candidates 38 | angle_consideration: false 39 | 40 | # Maximum angle difference (in degrees) between loop closure candidates 41 | max_angle_deg: 45 42 | 43 | # Maximum distance between loop closure candidates 44 | max_dist: 3 45 | 46 | # Flag to save candidates (predicted and real) to a file in the results folder 47 | save_candidates: true -------------------------------------------------------------------------------- /config/std_livox.yaml: -------------------------------------------------------------------------------- 1 | # pre process 2 | ds_size: 0.25 3 | maximum_corner_num: 500 4 | 5 | # key points 6 | plane_detection_thre: 0.01 7 | plane_merge_normal_thre: 0.1 8 | voxel_size: 1 9 | voxel_init_num: 10 10 | proj_image_resolution: 0.25 11 | proj_dis_min: 0 12 | proj_dis_max: 5 13 | corner_thre: 7 14 | 15 | # std descriptor 16 | descriptor_near_num: 15 17 | descriptor_min_len: 2 18 | descriptor_max_len: 30 19 | non_max_suppression_radius: 2 20 | std_side_resolution: 0.2 21 | 22 | # candidate search 23 | skip_near_num: 0 24 | candidate_num: 1 25 | vertex_diff_threshold: 0.8 26 | rough_dis_threshold: 0.015 27 | normal_threshold: 0.2 28 | dis_threshold: 0.5 29 | icp_threshold: 0.3 30 | -------------------------------------------------------------------------------- /include/evaluator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include "methods/base.h" 12 | 13 | class Evaluator 14 | { 15 | public: 16 | // Constructor with record size parameter 17 | Evaluator(const std::vector &methods, int record_size, 18 | bool angle_consideration, int max_angle_deg, double max_dist, int min_idx_diff, bool save_candidates); 19 | 20 | ~Evaluator() {} 21 | 22 | // Override the callback for synchronized point cloud, image, and pose messages 23 | void synced_callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, 24 | const nav_msgs::Odometry::ConstPtr &odom_msg, 25 | const boost::optional &img_msg); 26 | 27 | // Get ground truth loop closure candidates 28 | void get_real_loop_candidates(); 29 | 30 | // Get predicted loop closure candidates from the place recognition model 31 | void evaluate_models(); 32 | 33 | // Calculate evaluation metrics: precision, recall, and F1 score 34 | void calculate_metrics(std::vector> &predicted_loop_candidates, double &precision, double &recall, double &f1_score, double &accuracy); 35 | 36 | void plot_odometry_path(const std::vector& odom_vec); 37 | 38 | void generate_pr_curve(const std::vector& precisions, const std::vector& recalls, const std::string& method_name); 39 | 40 | private: 41 | // Number of triplets to record 42 | const int RECORD_SIZE; 43 | const bool ANGLE_CONSIDERATION; 44 | const int MAX_ANGLE_DEG; 45 | const double MAX_ANGLE_RAD; // Maximum angle difference between loop candidates 46 | const double MAX_DIST; 47 | 48 | // Minimum index difference between loop candidates 49 | const int MIN_IDX_DIFF; 50 | 51 | // Whether to save a loop closure candidates to a file 52 | const bool SAVE_CANDIDATES = false; 53 | 54 | void save_candidates(std::vector> &candidates, const std::string &filename); 55 | 56 | // Vectors to store recorded data 57 | std::vector odom_vec; // Pose messages 58 | std::vector cloud_vec; // Point cloud messages 59 | std::vector img_vec; // Image messages 60 | 61 | // Vector to store predicted loop closure candidates 62 | std::vector> real_loop_candidates; 63 | 64 | std::vector methods; 65 | }; 66 | -------------------------------------------------------------------------------- /include/merge_messages.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | class MergeRepublisher 20 | { 21 | public: 22 | explicit MergeRepublisher(bool use_rosbag, std::string input_bag_path, std::string output_bag_path, std::string &merged_lidar_topic, 23 | std::string &merged_odom_topic, std::string &merged_cam_topic, int keyframe_type, int cloud_size, double leaf_size); 24 | 25 | void callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, 26 | const nav_msgs::Odometry::ConstPtr &odom_msg, 27 | const boost::optional &img_msg); 28 | 29 | pcl::PointCloud merge_cloud(); 30 | 31 | void processBag(std::string lidar_topic, std::string odom_topic, std::string cam_topic); 32 | 33 | private: 34 | std::vector::Ptr> cloud_vec; 35 | std::vector odom_vec; 36 | std::vector img_vec; 37 | 38 | ros::NodeHandle nh_; 39 | ros::Publisher merged_cloud_publisher, merged_odom_publisher, merged_img_publisher; 40 | 41 | pcl::VoxelGrid downsampler; 42 | 43 | // parameters: 44 | const int CLOUD_SIZE; 45 | const double LEAF_SIZE; 46 | 47 | // 0 - queue like keyframes, batch like keyframes 48 | const int KEYFRAME_TYPE; 49 | 50 | const bool USE_ROSBAG; 51 | rosbag::Bag input_bag; 52 | rosbag::Bag output_bag; 53 | 54 | std::string merged_lidar_topic; 55 | std::string merged_odom_topic; 56 | std::string merged_cam_topic; 57 | }; 58 | -------------------------------------------------------------------------------- /include/methods/STD.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class STD : public BaseMethod 8 | { 9 | public: 10 | explicit STD(ConfigSetting &config_setting); 11 | 12 | virtual void predict_loop_candidates(std::vector> &predicted, 13 | std::vector &cloud_vec, 14 | std::vector &odom_vec, 15 | std::vector &img_vec, 16 | int min_idx_diff) override; 17 | virtual const std::string getName() const override; 18 | 19 | private: 20 | ConfigSetting config_setting; 21 | }; 22 | -------------------------------------------------------------------------------- /include/methods/STD/STDesc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "omp.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #define HASH_P 116101 22 | #define MAX_N 10000000000 23 | #define MAX_FRAME_N 20000 24 | 25 | typedef struct ConfigSetting { 26 | /* for point cloud pre-preocess*/ 27 | int stop_skip_enable_ = 0; 28 | double ds_size_ = 0.5; 29 | int maximum_corner_num_ = 30; 30 | 31 | /* for key points*/ 32 | double plane_merge_normal_thre_; 33 | double plane_merge_dis_thre_; 34 | double plane_detection_thre_ = 0.01; 35 | double voxel_size_ = 1.0; 36 | int voxel_init_num_ = 10; 37 | double proj_image_resolution_ = 0.5; 38 | double proj_dis_min_ = 0.2; 39 | double proj_dis_max_ = 5; 40 | double corner_thre_ = 10; 41 | 42 | /* for STD */ 43 | int descriptor_near_num_ = 10; 44 | double descriptor_min_len_ = 1; 45 | double descriptor_max_len_ = 10; 46 | double non_max_suppression_radius_ = 3.0; 47 | double std_side_resolution_ = 0.2; 48 | 49 | /* for place recognition*/ 50 | int skip_near_num_ = 50; 51 | int candidate_num_ = 50; 52 | int sub_frame_num_ = 10; 53 | double rough_dis_threshold_ = 0.03; 54 | double vertex_diff_threshold_ = 0.7; 55 | double icp_threshold_ = 0.5; 56 | double normal_threshold_ = 0.1; 57 | double dis_threshold_ = 0.3; 58 | 59 | } ConfigSetting; 60 | 61 | // Structure for Stabel Triangle Descriptor 62 | typedef struct STDesc { 63 | // the side lengths of STDesc, arranged from short to long 64 | Eigen::Vector3d side_length_; 65 | 66 | // projection angle between vertices 67 | Eigen::Vector3d angle_; 68 | 69 | Eigen::Vector3d center_; 70 | unsigned int frame_id_; 71 | 72 | // three vertexs 73 | Eigen::Vector3d vertex_A_; 74 | Eigen::Vector3d vertex_B_; 75 | Eigen::Vector3d vertex_C_; 76 | 77 | // some other inform attached to each vertex,e.g., intensity 78 | Eigen::Vector3d vertex_attached_; 79 | } STDesc; 80 | 81 | // plane structure for corner point extraction 82 | typedef struct Plane { 83 | pcl::PointXYZINormal p_center_; 84 | Eigen::Vector3d center_; 85 | Eigen::Vector3d normal_; 86 | Eigen::Matrix3d covariance_; 87 | float radius_ = 0; 88 | float min_eigen_value_ = 1; 89 | float intercept_ = 0; 90 | int id_ = 0; 91 | int sub_plane_num_ = 0; 92 | int points_size_ = 0; 93 | bool is_plane_ = false; 94 | } Plane; 95 | 96 | typedef struct STDMatchList { 97 | std::vector> match_list_; 98 | std::pair match_id_; 99 | double mean_dis_; 100 | } STDMatchList; 101 | 102 | class VOXEL_LOC { 103 | public: 104 | int64_t x, y, z; 105 | 106 | VOXEL_LOC(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0) 107 | : x(vx), y(vy), z(vz) {} 108 | 109 | bool operator==(const VOXEL_LOC &other) const { 110 | return (x == other.x && y == other.y && z == other.z); 111 | } 112 | }; 113 | 114 | // for down sample function 115 | struct M_POINT { 116 | float xyz[3]; 117 | float intensity; 118 | int count = 0; 119 | }; 120 | 121 | // Hash value 122 | 123 | template <> struct std::hash { 124 | int64_t operator()(const VOXEL_LOC &s) const { 125 | using std::hash; 126 | using std::size_t; 127 | return ((((s.z) * HASH_P) % MAX_N + (s.y)) * HASH_P) % MAX_N + (s.x); 128 | } 129 | }; 130 | 131 | class STDesc_LOC { 132 | public: 133 | int64_t x, y, z, a, b, c; 134 | 135 | STDesc_LOC(int64_t vx = 0, int64_t vy = 0, int64_t vz = 0, int64_t va = 0, 136 | int64_t vb = 0, int64_t vc = 0) 137 | : x(vx), y(vy), z(vz), a(va), b(vb), c(vc) {} 138 | 139 | bool operator==(const STDesc_LOC &other) const { 140 | // use three attributes 141 | return (x == other.x && y == other.y && z == other.z); 142 | // use six attributes 143 | // return (x == other.x && y == other.y && z == other.z && a == other.a && 144 | // b == other.b && c == other.c); 145 | } 146 | }; 147 | 148 | template <> struct std::hash { 149 | int64_t operator()(const STDesc_LOC &s) const { 150 | using std::hash; 151 | using std::size_t; 152 | return ((((s.z) * HASH_P) % MAX_N + (s.y)) * HASH_P) % MAX_N + (s.x); 153 | // return ((((((((((s.z) * HASH_P) % MAX_N + (s.y)) * HASH_P) % MAX_N + 154 | // (s.x)) * 155 | // HASH_P) % 156 | // MAX_N + 157 | // s.a) * 158 | // HASH_P) % 159 | // MAX_N + 160 | // s.b) * 161 | // HASH_P) % 162 | // MAX_N + 163 | // s.c; 164 | } 165 | }; 166 | 167 | // OctoTree structure for plane detection 168 | class OctoTree { 169 | public: 170 | ConfigSetting config_setting_; 171 | std::vector voxel_points_; 172 | Plane *plane_ptr_; 173 | int layer_; 174 | int octo_state_; // 0 is end of tree, 1 is not 175 | int merge_num_ = 0; 176 | bool is_project_ = false; 177 | std::vector proj_normal_vec_; 178 | 179 | // check 6 direction: x,y,z,-x,-y,-z 180 | bool is_check_connect_[6]; 181 | bool connect_[6]; 182 | OctoTree *connect_tree_[6]; 183 | 184 | bool is_publish_ = false; 185 | OctoTree *leaves_[8]; 186 | double voxel_center_[3]; // x, y, z 187 | float quater_length_; 188 | bool init_octo_; 189 | OctoTree(const ConfigSetting &config_setting) 190 | : config_setting_(config_setting) { 191 | voxel_points_.clear(); 192 | octo_state_ = 0; 193 | layer_ = 0; 194 | init_octo_ = false; 195 | for (int i = 0; i < 8; i++) { 196 | leaves_[i] = nullptr; 197 | } 198 | for (int i = 0; i < 6; i++) { 199 | is_check_connect_[i] = false; 200 | connect_[i] = false; 201 | connect_tree_[i] = nullptr; 202 | } 203 | plane_ptr_ = new Plane; 204 | } 205 | void init_plane(); 206 | void init_octo_tree(); 207 | }; 208 | 209 | void down_sampling_voxel(pcl::PointCloud &pl_feat, 210 | double voxel_size); 211 | 212 | void load_pose_with_time( 213 | const std::string &pose_file, 214 | std::vector> &poses_vec, 215 | std::vector ×_vec); 216 | 217 | void read_parameters(ros::NodeHandle &nh, ConfigSetting &config_setting); 218 | 219 | double time_inc(std::chrono::_V2::system_clock::time_point &t_end, 220 | std::chrono::_V2::system_clock::time_point &t_begin); 221 | 222 | pcl::PointXYZI vec2point(const Eigen::Vector3d &vec); 223 | Eigen::Vector3d point2vec(const pcl::PointXYZI &pi); 224 | 225 | void publish_std_pairs( 226 | const std::vector> &match_std_pairs, 227 | const ros::Publisher &std_publisher); 228 | 229 | bool attach_greater_sort(std::pair a, std::pair b); 230 | 231 | struct PlaneSolver { 232 | PlaneSolver(Eigen::Vector3d curr_point_, Eigen::Vector3d curr_normal_, 233 | Eigen::Vector3d target_point_, Eigen::Vector3d target_normal_) 234 | : curr_point(curr_point_), curr_normal(curr_normal_), 235 | target_point(target_point_), target_normal(target_normal_){}; 236 | template 237 | bool operator()(const T *q, const T *t, T *residual) const { 238 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 239 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 240 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), 241 | T(curr_point.z())}; 242 | Eigen::Matrix point_w; 243 | point_w = q_w_curr * cp + t_w_curr; 244 | Eigen::Matrix point_target( 245 | T(target_point.x()), T(target_point.y()), T(target_point.z())); 246 | Eigen::Matrix norm(T(target_normal.x()), T(target_normal.y()), 247 | T(target_normal.z())); 248 | residual[0] = norm.dot(point_w - point_target); 249 | return true; 250 | } 251 | 252 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, 253 | const Eigen::Vector3d curr_normal_, 254 | Eigen::Vector3d target_point_, 255 | Eigen::Vector3d target_normal_) { 256 | return ( 257 | new ceres::AutoDiffCostFunction(new PlaneSolver( 258 | curr_point_, curr_normal_, target_point_, target_normal_))); 259 | } 260 | 261 | Eigen::Vector3d curr_point; 262 | Eigen::Vector3d curr_normal; 263 | Eigen::Vector3d target_point; 264 | Eigen::Vector3d target_normal; 265 | }; 266 | 267 | class STDescManager { 268 | public: 269 | STDescManager() = default; 270 | 271 | ConfigSetting config_setting_; 272 | 273 | unsigned int current_frame_id_; 274 | 275 | STDescManager(ConfigSetting &config_setting) 276 | : config_setting_(config_setting) { 277 | current_frame_id_ = 0; 278 | }; 279 | 280 | // hash table, save all descriptors 281 | std::unordered_map> data_base_; 282 | 283 | // save all key clouds, optional 284 | std::vector::Ptr> key_cloud_vec_; 285 | 286 | // save all corner points, optional 287 | std::vector::Ptr> corner_cloud_vec_; 288 | 289 | // save all planes of key frame, required 290 | std::vector::Ptr> plane_cloud_vec_; 291 | 292 | /*Three main processing functions*/ 293 | 294 | // generate STDescs from a point cloud 295 | void GenerateSTDescs(pcl::PointCloud::Ptr &input_cloud, 296 | std::vector &stds_vec); 297 | 298 | // search result . -1 for no loop 299 | void SearchLoop(const std::vector &stds_vec, 300 | std::pair &loop_result, 301 | std::pair &loop_transform, 302 | std::vector> &loop_std_pair); 303 | 304 | // add descriptors to database 305 | void AddSTDescs(const std::vector &stds_vec); 306 | 307 | // Geometrical optimization by plane-to-plane ico 308 | void PlaneGeomrtricIcp( 309 | const pcl::PointCloud::Ptr &source_cloud, 310 | const pcl::PointCloud::Ptr &target_cloud, 311 | std::pair &transform); 312 | 313 | // Select a specified number of candidate frames according to the number of 314 | // STDesc rough matches 315 | void candidate_selector(const std::vector &stds_vec, 316 | std::vector &candidate_matcher_vec); 317 | 318 | // Get the best candidate frame by geometry check 319 | void 320 | candidate_verify(const STDMatchList &candidate_matcher, double &verify_score, 321 | std::pair &relative_pose, 322 | std::vector> &sucess_match_vec); 323 | private: 324 | /*Following are sub-processing functions*/ 325 | 326 | // voxelization and plane detection 327 | void init_voxel_map(const pcl::PointCloud::Ptr &input_cloud, 328 | std::unordered_map &voxel_map); 329 | 330 | // build connection for planes 331 | void build_connection(std::unordered_map &feat_map); 332 | 333 | // acquire planes from voxel_map 334 | void getPlane(const std::unordered_map &voxel_map, 335 | pcl::PointCloud::Ptr &plane_cloud); 336 | 337 | // extract corner points from pre-build voxel map and clouds 338 | void 339 | corner_extractor(std::unordered_map &voxel_map, 340 | const pcl::PointCloud::Ptr &input_cloud, 341 | pcl::PointCloud::Ptr &corner_points); 342 | 343 | void 344 | extract_corner(const Eigen::Vector3d &proj_center, 345 | const Eigen::Vector3d proj_normal, 346 | const std::vector proj_points, 347 | pcl::PointCloud::Ptr &corner_points); 348 | 349 | // non maximum suppression, to control the number of corners 350 | void non_maxi_suppression( 351 | pcl::PointCloud::Ptr &corner_points); 352 | 353 | // build STDescs from corner points. 354 | void 355 | build_stdesc(const pcl::PointCloud::Ptr &corner_points, 356 | std::vector &stds_vec); 357 | 358 | 359 | // Get the transform between a matched std pair 360 | void triangle_solver(std::pair &std_pair, Eigen::Vector3d &t, 361 | Eigen::Matrix3d &rot); 362 | 363 | // Geometrical verification by plane-to-plane icp threshold 364 | double plane_geometric_verify( 365 | const pcl::PointCloud::Ptr &source_cloud, 366 | const pcl::PointCloud::Ptr &target_cloud, 367 | const std::pair &transform); 368 | }; -------------------------------------------------------------------------------- /include/methods/base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class BaseMethod 9 | { 10 | public: 11 | virtual void predict_loop_candidates(std::vector> &predicted, 12 | std::vector &cloud_vec, 13 | std::vector &odom_vec, 14 | std::vector &img_vec, 15 | int min_idx_diff) = 0; 16 | 17 | virtual const std::string getName() const = 0; 18 | }; -------------------------------------------------------------------------------- /include/methods/dbow.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class DBoW : public BaseMethod 9 | { 10 | public: 11 | explicit DBoW(double threshold); 12 | 13 | virtual void predict_loop_candidates(std::vector> &predicted, 14 | std::vector &cloud_vec, 15 | std::vector &odom_vec, 16 | std::vector &img_vec, 17 | int min_idx_diff) override; 18 | 19 | virtual const std::string getName() const override; 20 | 21 | static void createVocabulary(const std::vector &img_vec, std::string &voc_name); 22 | 23 | private: 24 | cv::Ptr orb; 25 | cv_bridge::CvImagePtr cv_ptr; 26 | OrbVocabulary orb_vocab; 27 | const double THRESHOLD; 28 | }; -------------------------------------------------------------------------------- /include/methods/dbow/orb_vocab.yml.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/include/methods/dbow/orb_vocab.yml.gz -------------------------------------------------------------------------------- /include/methods/scan_context.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class ScanContext : public BaseMethod 8 | { 9 | public: 10 | explicit ScanContext(double threshold, double leaf_size); 11 | 12 | virtual void predict_loop_candidates(std::vector> &predicted, 13 | std::vector &cloud_vec, 14 | std::vector &odom_vec, 15 | std::vector &img_vec, 16 | int min_idx_diff) override; 17 | 18 | virtual const std::string getName() const override; 19 | 20 | private: 21 | pcl::VoxelGrid voxel_grid; 22 | 23 | SCManager sc_manager; 24 | 25 | const double LEAF_SIZE; 26 | const double THRESHOLD; 27 | }; 28 | -------------------------------------------------------------------------------- /include/methods/scan_context/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 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 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include "nanoflann.hpp" 32 | 33 | #include 34 | 35 | // ===== This example shows how to use nanoflann with these types of containers: ======= 36 | //typedef std::vector > my_vector_of_vectors_t; 37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include 38 | // ===================================================================================== 39 | 40 | 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. 42 | * The i'th vector represents a point in the state space. 43 | * 44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 45 | * \tparam num_t The type of the point coordinates (typically, double or float). 46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 48 | */ 49 | template 50 | struct KDTreeVectorOfVectorsAdaptor 51 | { 52 | typedef KDTreeVectorOfVectorsAdaptor self_t; 53 | typedef typename Distance::template traits::distance_t metric_t; 54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 55 | 56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 57 | 58 | /// Constructor: takes a const ref to the vector of vectors object with the data points 59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) 60 | { 61 | assert(mat.size() != 0 && mat[0].size() != 0); 62 | const size_t dims = mat[0].size(); 63 | if (DIM>0 && static_cast(dims) != DIM) 64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); 66 | index->buildIndex(); 67 | } 68 | 69 | ~KDTreeVectorOfVectorsAdaptor() { 70 | delete index; 71 | } 72 | 73 | const VectorOfVectorsType &m_data; 74 | 75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 76 | * Note that this is a short-cut method for index->findNeighbors(). 77 | * The user can also call index->... methods as desired. 78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 79 | */ 80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 81 | { 82 | nanoflann::KNNResultSet resultSet(num_closest); 83 | resultSet.init(out_indices, out_distances_sq); 84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 85 | } 86 | 87 | /** @name Interface expected by KDTreeSingleIndexAdaptor 88 | * @{ */ 89 | 90 | const self_t & derived() const { 91 | return *this; 92 | } 93 | self_t & derived() { 94 | return *this; 95 | } 96 | 97 | // Must return the number of data points 98 | inline size_t kdtree_get_point_count() const { 99 | return m_data.size(); 100 | } 101 | 102 | // Returns the dim'th component of the idx'th point in the class: 103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { 104 | return m_data[idx][dim]; 105 | } 106 | 107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 110 | template 111 | bool kdtree_get_bbox(BBOX & /*bb*/) const { 112 | return false; 113 | } 114 | 115 | /** @} */ 116 | 117 | }; // end of KDTreeVectorOfVectorsAdaptor 118 | -------------------------------------------------------------------------------- /include/methods/scan_context/Scancontext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "nanoflann.hpp" 26 | #include "KDTreeVectorOfVectorsAdaptor.h" 27 | 28 | #include "tictoc.h" 29 | 30 | using namespace Eigen; 31 | using namespace nanoflann; 32 | 33 | using std::cout; 34 | using std::endl; 35 | using std::make_pair; 36 | 37 | using std::atan2; 38 | using std::cos; 39 | using std::sin; 40 | 41 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) 42 | using KeyMat = std::vector >; 43 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 44 | 45 | 46 | // namespace SC2 47 | // { 48 | 49 | void coreImportTest ( void ); 50 | 51 | 52 | // sc param-independent helper functions 53 | float xy2theta( const float & _x, const float & _y ); 54 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 55 | std::vector eig2stdvec( MatrixXd _eigmat ); 56 | 57 | 58 | class SCManager 59 | { 60 | public: 61 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 62 | 63 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); 64 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 65 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 66 | 67 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 68 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 69 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 70 | 71 | // User-side API 72 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); 73 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 74 | 75 | 76 | // find distance between gives indexes 77 | double getDistance(int idx1, int idx2); 78 | 79 | public: 80 | // hyper parameters () 81 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. 82 | 83 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) 84 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) 85 | const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) 86 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); 87 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); 88 | 89 | // tree 90 | const int NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok. 91 | const int NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper) 92 | 93 | // loop thres 94 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. 95 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) 96 | const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 97 | 98 | // config 99 | const int TREE_MAKING_PERIOD_ = 10; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N. 100 | int tree_making_period_conter = 0; 101 | 102 | // data 103 | std::vector polarcontexts_timestamp_; // optional. 104 | std::vector polarcontexts_; 105 | std::vector polarcontext_invkeys_; 106 | std::vector polarcontext_vkeys_; 107 | 108 | KeyMat polarcontext_invkeys_mat_; 109 | KeyMat polarcontext_invkeys_to_search_; 110 | std::unique_ptr polarcontext_tree_; 111 | 112 | }; // SCManager 113 | 114 | // } // namespace SC2 115 | -------------------------------------------------------------------------------- /include/methods/scan_context/tictoc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class TicToc 13 | { 14 | public: 15 | TicToc() 16 | { 17 | tic(); 18 | } 19 | 20 | TicToc( bool _disp ) 21 | { 22 | disp_ = _disp; 23 | tic(); 24 | } 25 | 26 | void tic() 27 | { 28 | start = std::chrono::system_clock::now(); 29 | } 30 | 31 | void toc( std::string _about_task ) 32 | { 33 | end = std::chrono::system_clock::now(); 34 | std::chrono::duration elapsed_seconds = end - start; 35 | double elapsed_ms = elapsed_seconds.count() * 1000; 36 | 37 | if( disp_ ) 38 | { 39 | std::cout.precision(3); // 10 for sec, 3 for ms 40 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 41 | } 42 | } 43 | 44 | private: 45 | std::chrono::time_point start, end; 46 | bool disp_ = false; 47 | }; 48 | -------------------------------------------------------------------------------- /launch/base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/evaluate_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/evaluate_python.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /launch/merge.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/methods/context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/methods/dbow.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/methods/logg3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/methods/mix_vpr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/methods/std.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/methods/superglue.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | place_recog_eval 4 | 0.0.1 5 | The place_recog_eval package 6 | 7 | 4ku 8 | MIT 9 | 10 | catkin 11 | roscpp 12 | rospy 13 | pcl_conversions 14 | pcl_ros 15 | geometry_msgs 16 | nav_msgs 17 | cv_bridge 18 | image_transport 19 | std_msgs 20 | sensor_msgs 21 | message_filters 22 | 23 | 24 | -------------------------------------------------------------------------------- /paper.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/paper.pdf -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | open3d 2 | torchpack 3 | pyntcloud 4 | mpi4py -------------------------------------------------------------------------------- /results/.gitignore: -------------------------------------------------------------------------------- 1 | *.txt 2 | no_angle/ 3 | with_angle/ 4 | -------------------------------------------------------------------------------- /results/get_results.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | 4 | def read_matrix(file_path): 5 | with open(file_path, 'r') as f: 6 | lines = f.readlines() 7 | matrix = [] 8 | for line in lines: 9 | row = list(map(int, line.strip().split(' '))) 10 | matrix.append(row) 11 | return matrix 12 | 13 | 14 | def compare_matrices(files): 15 | first_matrix = read_matrix(files[0]) 16 | 17 | for file in files[1:]: 18 | current_matrix = read_matrix(file) 19 | if first_matrix != current_matrix: 20 | return False 21 | return True 22 | 23 | 24 | def and_matrices(matrices): 25 | result = matrices[0] 26 | for matrix in matrices[1:]: 27 | for i, row in enumerate(matrix): 28 | for j, element in enumerate(row): 29 | result[i][j] &= element 30 | return result 31 | 32 | 33 | def calculate_metrics(real, predicted): 34 | TP, FP, TN, FN = 0, 0, 0, 0 35 | 36 | for i, row in enumerate(real): 37 | for j, element in enumerate(row): 38 | if real[i][j] == 1 and predicted[i][j] == 1: 39 | TP += 1 40 | elif real[i][j] == 0 and predicted[i][j] == 1: 41 | FP += 1 42 | elif real[i][j] == 0 and predicted[i][j] == 0: 43 | TN += 1 44 | elif real[i][j] == 1 and predicted[i][j] == 0: 45 | FN += 1 46 | 47 | precision = TP / (TP + FP) if TP + FP > 0 else 0 48 | recall = TP / (TP + FN) if TP + FN > 0 else 0 49 | f1_score = 2 * precision * recall / \ 50 | (precision + recall) if precision + recall > 0 else 0 51 | accuracy = (TP + TN) / (TP + FP + TN + FN) if TP + FP + TN + FN > 0 else 0 52 | 53 | return precision, recall, f1_score, accuracy 54 | 55 | 56 | def calculate_metrics_for_directory(directory): 57 | file_prefix = 'real_' 58 | 59 | # Get a list of files that start with the given prefix 60 | files = [os.path.join(directory, f) for f in os.listdir( 61 | directory) if f.startswith(file_prefix) and f.endswith('.txt')] 62 | 63 | # Check if all matrices are the same 64 | if not compare_matrices(files): 65 | print("Not all real loop candidates matrices are the same in directory {}. Redo evaluations.".format(directory)) 66 | return None 67 | 68 | real_loop_candidates = read_matrix(files[0]) 69 | 70 | # Get a list of files that do not start with the given prefix 71 | files = [os.path.join(directory, f) for f in os.listdir( 72 | directory) if not f.startswith(file_prefix) and f.endswith('.txt')] 73 | 74 | # Read the matrices from the files 75 | matrices = [read_matrix(file) for file in files] 76 | 77 | # Perform elementwise AND operation on the matrices 78 | total_predicted_loop_candidates = and_matrices(matrices) 79 | 80 | # Calculate metrics 81 | precision, recall, f1_score, accuracy = calculate_metrics( 82 | real_loop_candidates, total_predicted_loop_candidates) 83 | 84 | total_method_name = " + ".join( 85 | [".".join(os.path.basename(file).split('.')[:-1]) for file in files]) 86 | 87 | print(f"Calculated results for {directory}") 88 | print(f"Combined method: {total_method_name}") 89 | print("Precision: {:.3f}".format(precision)) 90 | print("Recall: {:.3f}".format(recall)) 91 | print("F1 Score: {:.3f}".format(f1_score)) 92 | print("Accuracy: {:.3f}".format(accuracy)) 93 | 94 | 95 | if __name__ == "__main__": 96 | path = '.' # Set the path to the folder containing the subdirectories 97 | 98 | # Get a list of directories in the current path 99 | subdirectories = [os.path.join(path, d) for d in os.listdir( 100 | path) if os.path.isdir(os.path.join(path, d))] 101 | 102 | # subdirectories = ['./no_angle', './with_angle'] 103 | 104 | for directory in subdirectories: 105 | calculate_metrics_for_directory(directory) 106 | -------------------------------------------------------------------------------- /scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/scripts/__init__.py -------------------------------------------------------------------------------- /scripts/evaluate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import rosbag 5 | from sensor_msgs.msg import PointCloud2, Image 6 | from nav_msgs.msg import Odometry 7 | from message_filters import TimeSynchronizer, Subscriber 8 | 9 | from scripts.evaluator import Evaluator 10 | from typing import List 11 | from scripts.methods.base import BaseMethod 12 | 13 | def getMethods() -> List[BaseMethod]: 14 | """ 15 | Get the methods to be evaluated. 16 | 17 | :return: List[BaseMethod], the list of methods to be evaluated 18 | """ 19 | # Create methods 20 | methods = [] 21 | method_name = rospy.get_param('method', None) 22 | 23 | if method_name is not None: 24 | threshold = float(rospy.get_param('threshold')) 25 | 26 | if method_name == "logg3d": 27 | from scripts.methods.LoGG3D import LoGG3D 28 | methods.append(LoGG3D(threshold)) 29 | elif method_name == "superglue": 30 | from scripts.methods.SuperGlue import SuperGlue 31 | methods.append(SuperGlue(threshold)) 32 | elif method_name == "mix_vpr": 33 | from scripts.methods.MixVPR_ import MixVPR 34 | methods.append(MixVPR(threshold)) 35 | else: 36 | rospy.logerr( 37 | "Invalid method parameter. Use 'logg3d' or 'superglue'.") 38 | exit(-1) 39 | else: 40 | # from scripts.methods.LoGG3D import LoGG3D 41 | # methods.append(LoGG3D(0.08)) 42 | 43 | # from scripts.methods.SuperGlue import SuperGlue 44 | # methods.append(SuperGlue(240)) 45 | 46 | # from scripts.methods.MixVPR_ import MixVPR 47 | # methods.append(MixVPR(0.13)) 48 | 49 | # Example for finding best threshold for LoGG3D 50 | from scripts.methods.LoGG3D import LoGG3D 51 | for threshold in range(5, 17): 52 | threshold = threshold / 100.0 # Convert to float equivalent 53 | methods.append(LoGG3D(threshold)) 54 | 55 | # Example for finding best threshold for SuperGlue 56 | # from scripts.methods.SuperGlue import SuperGlue 57 | # for threshold in range(150, 301, 30): 58 | # methods.append(SuperGlue(threshold)) 59 | 60 | # Example for finding best threshold for MixVPR 61 | # from scripts.methods.MixVPR_ import MixVPR 62 | # for threshold in range(9, 21): 63 | # threshold = threshold / 100.0 # Convert to float equivalent 64 | # methods.append(MixVPR(threshold)) 65 | 66 | return methods 67 | 68 | 69 | def process_bag(evaluator, input_bag_path, lidar_topic, odom_topic, camera_topic): 70 | cloud_msg = None 71 | odom_msg = None 72 | img_msg = None 73 | 74 | with rosbag.Bag(input_bag_path, 'r') as bag: 75 | for topic, msg, t in bag.read_messages(topics=[lidar_topic, odom_topic, camera_topic]): 76 | if topic == lidar_topic: 77 | cloud_msg = msg 78 | elif topic == odom_topic: 79 | odom_msg = msg 80 | elif topic == camera_topic: 81 | img_msg = msg 82 | 83 | if cloud_msg is not None and odom_msg is not None and img_msg is not None: 84 | evaluator.synced_callback(cloud_msg, odom_msg, img_msg) 85 | cloud_msg = None 86 | odom_msg = None 87 | img_msg = None 88 | 89 | 90 | rospy.init_node('evaluation_node') 91 | 92 | # Retrieve parameters from the ROS parameter server 93 | keyframe_type = int(rospy.get_param('keyframe_type')) 94 | cloud_size = int(rospy.get_param('cloud_size')) 95 | 96 | # Common params 97 | record_size = int(rospy.get_param('record_size')) 98 | min_idx_diff = int(rospy.get_param('min_idx_diff')) 99 | angle_consideration = bool(rospy.get_param('angle_consideration')) 100 | max_angle_deg = int(rospy.get_param('max_angle_deg')) 101 | max_dist = float(rospy.get_param('max_dist')) 102 | save_candidates = bool(rospy.get_param('save_candidates')) 103 | 104 | if keyframe_type == 1: 105 | record_size = int(record_size / cloud_size) 106 | 107 | # Get methods 108 | methods = getMethods() 109 | 110 | # Create evaluator 111 | evaluator = Evaluator(methods, record_size, angle_consideration, 112 | max_angle_deg, max_dist, min_idx_diff, save_candidates) 113 | 114 | 115 | # Get merged topics 116 | lidar_topic = rospy.get_param('merged_lidar_topic') 117 | odom_topic = rospy.get_param('merged_odom_topic') 118 | camera_topic = rospy.get_param('merged_camera_topic') 119 | 120 | use_rosbag = bool(rospy.get_param('use_rosbag')) 121 | input_bag_path = rospy.get_param('merge_rosbag_output_path') 122 | 123 | if use_rosbag: 124 | process_bag(evaluator, input_bag_path, 125 | lidar_topic, odom_topic, camera_topic) 126 | else: 127 | # Create subscribers 128 | cloud_sub = Subscriber(lidar_topic, PointCloud2, queue_size=5) 129 | odom_sub = Subscriber(odom_topic, Odometry, queue_size=20) 130 | img_sub = Subscriber(camera_topic, Image, queue_size=5) 131 | 132 | # Synchronize messages from different topics 133 | sync = TimeSynchronizer([cloud_sub, odom_sub, img_sub], queue_size=10) 134 | 135 | # Register the synchronized callback 136 | sync.registerCallback(evaluator.synced_callback) 137 | 138 | # Process callbacks and wait for new messages 139 | rospy.spin() 140 | -------------------------------------------------------------------------------- /scripts/evaluator.py: -------------------------------------------------------------------------------- 1 | import abc 2 | import rospy 3 | from cv_bridge import CvBridge 4 | from sensor_msgs.msg import PointCloud2, Image 5 | from nav_msgs.msg import Odometry 6 | import numpy as np 7 | import math 8 | import matplotlib.pyplot as plt 9 | from scripts.methods.base import BaseMethod 10 | from typing import Optional, Tuple, List 11 | import time 12 | import os 13 | from pathlib import Path 14 | import subprocess 15 | import re 16 | 17 | 18 | def angular_difference(quat_1, quat_2): 19 | """ 20 | Calculate the angular difference between two quaternions. 21 | 22 | sql 23 | 24 | :param quat_1: Quaternion, the first quaternion 25 | :param quat_2: Quaternion, the second quaternion 26 | :return: float, the angle between the two quaternions in radians 27 | """ 28 | cos_angle = quat_1.x * quat_2.x + quat_1.y * \ 29 | quat_2.y + quat_1.z * quat_2.z + quat_1.w * quat_2.w 30 | angle = 2 * math.acos(abs(cos_angle)) 31 | return angle 32 | 33 | 34 | def euclidean_difference(point_1, point_2): 35 | """ 36 | Calculate the Euclidean distance between two 3D points. 37 | 38 | sql 39 | 40 | :param point_1: Point, the first 3D point 41 | :param point_2: Point, the second 3D point 42 | :return: float, the Euclidean distance between the two points 43 | """ 44 | dx = point_1.x - point_2.x 45 | dy = point_1.y - point_2.y 46 | dz = point_1.z - point_2.z 47 | dist = math.sqrt(dx*dx + dy*dy + dz*dz) 48 | return dist 49 | 50 | 51 | class Evaluator(abc.ABC): 52 | """ 53 | Evaluator class for evaluating loop closure detection methods. 54 | 55 | This class collects data, calculates real loop closures and predicted loop closures 56 | based on the provided methods, and calculates evaluation metrics (precision, recall, and F1 score). 57 | """ 58 | 59 | def __init__(self, methods: List[BaseMethod], record_size: int, angle_consideration: bool, max_angle_deg: int, max_dist: float, min_idx_diff: int, save_candidates: bool): 60 | """ 61 | Initialize the Evaluator object. 62 | 63 | :param methods: List[BaseMethod], list of loop closure detection methods to evaluate 64 | :param record_size: int, number of records to collect before evaluating 65 | :param angle_consideration: bool, flag to consider angular difference between poses for loop closure 66 | :param max_angle_deg: int, maximum angular difference in degrees for considering loop closure 67 | :param max_dist: float, maximum Euclidean distance for considering loop closure 68 | :param min_idx_diff: int, minimum index difference between two poses to consider them as potential loop closures 69 | """ 70 | self.methods = methods 71 | self.RECORD_SIZE = record_size 72 | self.ANGLE_CONSIDERATION = angle_consideration 73 | self.MAX_ANGLE_DEG = max_angle_deg 74 | self.MIN_IDX_DIFF = min_idx_diff 75 | 76 | self.MAX_DIST = max_dist 77 | self.MAX_ANGLE_RAD = (self.MAX_ANGLE_DEG/180)*3.14 78 | 79 | self.SAVE_CANDIDATES = save_candidates 80 | 81 | self.odom_vec = [] 82 | self.cloud_vec = [] 83 | self.img_vec = [] 84 | 85 | self.real_loop_candidates = np.full( 86 | (record_size, record_size), False) 87 | 88 | self.counter = 0 89 | # ros->numpy converter 90 | self.bridge = CvBridge() 91 | 92 | def synced_callback(self, cloud_msg: PointCloud2, odom_msg: Odometry, img_msg: Optional[Image]) -> None: 93 | """ 94 | ROS callback function to receive synchronized sensor data and evaluate loop closure detection methods. 95 | 96 | :param cloud_msg: PointCloud2, point cloud message 97 | :param odom_msg: Odometry, odometry message 98 | :param img_msg: Optional[Image], image message (optional) 99 | :return: None 100 | """ 101 | log_frequency = 50 102 | if self.counter % log_frequency == 0: 103 | rospy.loginfo("Amount of data recorded: %d", self.counter) 104 | 105 | # Store data to lists 106 | self.cloud_vec.append(cloud_msg) 107 | self.odom_vec.append(odom_msg.pose.pose) 108 | if img_msg is not None: 109 | image = self.bridge.imgmsg_to_cv2( 110 | img_msg, desired_encoding='mono8') 111 | self.img_vec.append(image) 112 | 113 | self.counter += 1 114 | 115 | if self.counter == self.RECORD_SIZE: 116 | 117 | self.get_real_loop_candidates() 118 | self.evaluate_models() 119 | 120 | # Plot odometry path 121 | self.plot_path() 122 | 123 | rospy.signal_shutdown("Evaluation completed.") 124 | 125 | def save_candidates(self, candidates, filename): 126 | if self.SAVE_CANDIDATES: 127 | path = Path(os.path.dirname(__file__)).parent / "results" 128 | if self.ANGLE_CONSIDERATION: 129 | path = os.path.join(path, "with_angle") 130 | else: 131 | path = os.path.join(path, "no_angle") 132 | 133 | # Check if the directory exists and create it if it doesn't 134 | if not os.path.exists(path): 135 | os.makedirs(path) 136 | 137 | path = os.path.join(path, filename) 138 | 139 | with open(path, 'w') as f: 140 | for i in range(1, self.RECORD_SIZE): 141 | for j in range(i - self.MIN_IDX_DIFF + 1): 142 | f.write(f"{int(candidates[i][j])} ") 143 | f.write("\n") 144 | 145 | def get_real_loop_candidates(self) -> None: 146 | """ 147 | Calculate real loop candidates based on Euclidean distance and angular difference between odometry poses. 148 | Save real loop candidates to a file if SAVE_CANDIDATES is True. 149 | 150 | :return: None 151 | """ 152 | for i in range(1, self.RECORD_SIZE): 153 | for j in range(i - self.MIN_IDX_DIFF + 1): 154 | odom_diff = euclidean_difference( 155 | self.odom_vec[i].position, self.odom_vec[j].position) 156 | condition = odom_diff < self.MAX_DIST 157 | 158 | if self.ANGLE_CONSIDERATION: 159 | ang_diff = angular_difference( 160 | self.odom_vec[i].orientation, self.odom_vec[j].orientation) 161 | condition = condition and ang_diff < self.MAX_ANGLE_RAD 162 | 163 | if condition: 164 | self.real_loop_candidates[i][j] = True 165 | 166 | method_name = str(self.methods[0]).split("_")[0] 167 | self.save_candidates(self.real_loop_candidates, 168 | f"real_{method_name}.txt") 169 | 170 | def evaluate_models(self) -> None: 171 | """ 172 | Calculate predicted loop candidates using the provided loop closure detection methods. 173 | 174 | :return: None 175 | """ 176 | 177 | best_f1_score = 0 178 | best_method = None 179 | 180 | # Initialize precision and recall lists 181 | precisions = [] 182 | recalls = [] 183 | 184 | for method in self.methods: 185 | start = time.time() 186 | predicted_loop_candidates = method.predict_loop_candidates( 187 | self.cloud_vec, self.odom_vec, self.img_vec, self.MIN_IDX_DIFF) 188 | duration = time.time() - start 189 | rospy.loginfo(f"Results for method: {method}") 190 | rospy.loginfo( 191 | f"It took {duration :.5f} seconds to predict loop candidates ({len(self.cloud_vec)} frames in total).") 192 | rospy.loginfo( 193 | f"It took {duration / len(self.cloud_vec) :.5f} seconds per frame.") 194 | 195 | precision, recall, f1_score, accuracy = self.calculate_metrics( 196 | predicted_loop_candidates) 197 | 198 | # Save precision and recall 199 | precisions.append(precision) 200 | recalls.append(recall) 201 | 202 | rospy.loginfo("----------------------------------------------") 203 | rospy.loginfo(f"Precision: {precision:.3f}") 204 | rospy.loginfo(f"Recall : {recall:.3f}") 205 | rospy.loginfo(f"F1 Score : {f1_score:.3f}") 206 | rospy.loginfo(f"Accuracy : {accuracy:.3f}") 207 | rospy.loginfo("----------------------------------------------") 208 | rospy.loginfo("----------------------------------------------") 209 | rospy.loginfo(" ") 210 | 211 | if f1_score > best_f1_score: 212 | best_f1_score = f1_score 213 | best_method = method 214 | 215 | self.save_candidates(predicted_loop_candidates, f"{method}.txt") 216 | 217 | rospy.loginfo( 218 | f"Best method with f1_score {best_f1_score:.3f}: {best_method}") 219 | 220 | # Generate Precision-Recall curve if more than one method is provided 221 | if len(self.methods) > 1: 222 | # Remove parentheses content from the method name 223 | method_name = re.sub(r'\(.*\)', '', str(best_method)) 224 | self.generate_pr_curve(precisions, recalls, method_name.strip()) 225 | 226 | def generate_pr_curve(self, precisions, recalls, method_name): 227 | # Convert the lists to strings 228 | precisions_str = ','.join(map(str, precisions)) 229 | recalls_str = ','.join(map(str, recalls)) 230 | 231 | # Get the path to the Python script 232 | script_path = Path(os.path.dirname(__file__)).parent / \ 233 | "scripts" / "plot_pr_curve.py" 234 | 235 | # Get the results path 236 | results_path = Path(os.path.dirname(__file__)).parent / "results" 237 | if self.ANGLE_CONSIDERATION: 238 | results_path = os.path.join(results_path, "with_angle") 239 | else: 240 | results_path = os.path.join(results_path, "no_angle") 241 | 242 | # Build the command to run the Python script 243 | command = f"python3 {script_path} {method_name} {results_path} {precisions_str} {recalls_str}" 244 | 245 | # Execute the command 246 | subprocess.run(command, shell=True) 247 | 248 | def calculate_metrics(self, predicted_loop_candidates: List[List[bool]]) -> Tuple[float, float, float, float]: 249 | """ 250 | Calculate evaluation metrics (precision, recall, F1 score, and accuracy) based on the real and predicted loop candidates. 251 | 252 | :return: Tuple[float, float, float, float], evaluation metrics (precision, recall, F1 score, and accuracy) 253 | """ 254 | TP = 0 255 | FP = 0 256 | FN = 0 257 | TN = 0 258 | 259 | for i in range(1, self.RECORD_SIZE): 260 | for j in range(i - self.MIN_IDX_DIFF + 1): 261 | if self.real_loop_candidates[i][j] and predicted_loop_candidates[i][j]: 262 | TP += 1 263 | elif not self.real_loop_candidates[i][j] and predicted_loop_candidates[i][j]: 264 | FP += 1 265 | elif self.real_loop_candidates[i][j] and not predicted_loop_candidates[i][j]: 266 | FN += 1 267 | else: 268 | TN += 1 269 | 270 | if TP + FP == 0: 271 | precision = 0 272 | else: 273 | precision = TP / (TP + FP) 274 | 275 | if TP + FN == 0: 276 | recall = 0 277 | else: 278 | recall = TP / (TP + FN) 279 | 280 | if precision + recall == 0: 281 | f1_score = 0 282 | else: 283 | f1_score = 2 * (precision * recall) / (precision + recall) 284 | 285 | if TP + FP + TN + FN == 0: 286 | accuracy = 0 287 | else: 288 | accuracy = (TP + TN) / (TP + FP + TN + FN) 289 | 290 | return precision, recall, f1_score, accuracy 291 | 292 | def plot_path(self): 293 | 294 | # Extract x and y coordinates from odom_vec 295 | x_coords = [pose.position.x for pose in self.odom_vec] 296 | y_coords = [pose.position.y for pose in self.odom_vec] 297 | 298 | # Plot the data 299 | plt.plot(x_coords, y_coords) 300 | plt.xlabel('X') 301 | plt.ylabel('Y') 302 | plt.title('Odometry Plot') 303 | plt.savefig("../trajectory.png") 304 | -------------------------------------------------------------------------------- /scripts/methods/LoGG3D.py: -------------------------------------------------------------------------------- 1 | from scripts.methods.base import BaseMethod 2 | from sensor_msgs import point_cloud2 3 | 4 | import numpy as np 5 | from tqdm import tqdm 6 | import os 7 | import torch 8 | 9 | from .LoGG3D_Net.models.pipelines.pipeline_utils import make_sparse_tensor 10 | from .LoGG3D_Net.models.pipeline_factory import get_pipeline 11 | from scipy.spatial.distance import cdist 12 | import numpy as np 13 | 14 | def descriptor_difference(desc_1, desc_2): 15 | # Calculate the dot product between desc_1 and desc_2 16 | dot_product = np.dot(desc_1, desc_2) 17 | 18 | # Calculate the product of the magnitudes of desc_1 and desc_2 19 | magnitude_product = np.linalg.norm(desc_1) * np.linalg.norm(desc_2) 20 | 21 | # Calculate the cosine similarity using the dot product and magnitude product 22 | cosine_similarity = 1 - (dot_product / magnitude_product) 23 | 24 | # Return the cosine similarity 25 | return cosine_similarity 26 | 27 | 28 | class LoGG3D(BaseMethod): 29 | 30 | def __init__(self, threshold): 31 | self.THRESHOLD = threshold 32 | 33 | model = get_pipeline('LOGG3D') 34 | save_path = os.path.join(os.path.dirname(__file__), 'LoGG3D_Net/checkpoints') 35 | save_path = str(save_path) +\ 36 | '/mulran_10cm/2021-09-14_08-59-00_3n24h_MulRan_v10_q29_4s_263039.pth' 37 | 38 | self.device = torch.device( 39 | "cuda" if torch.cuda.is_available() else "cpu") 40 | checkpoint = torch.load( 41 | save_path, map_location=torch.device(self.device)) 42 | model.load_state_dict(checkpoint['model_state_dict']) 43 | # model = model.cuda() 44 | model.eval() 45 | self.model = model 46 | self.seen_descriptors = [] 47 | 48 | def __str__(self) -> str: 49 | return self.__class__.__name__ + f' (threshold {self.THRESHOLD})' 50 | 51 | def predict_loop_candidates(self, cloud_vec, odom_vec, img_vec, min_idx_diff=1) -> np.ndarray: 52 | desc_list = [] 53 | for cloud in tqdm(cloud_vec): 54 | 55 | # convert cloud into numpy 56 | cloud_current = [] 57 | points = point_cloud2.read_points( 58 | cloud, field_names=['x', 'y', 'z', 'intensity'], skip_nans=True) 59 | 60 | for point in points: 61 | cloud_current.append(point[:4]) 62 | 63 | cloud = np.array(cloud_current).astype('float32') 64 | out = self.predict_simple(cloud) 65 | desc_list.append(out) 66 | 67 | size = len(desc_list) 68 | predicted_loop_candidates = np.full((size, size), False) 69 | for i in range(1, size): 70 | for j in range(i - min_idx_diff + 1): 71 | desc_dist = descriptor_difference(desc_list[i], desc_list[j]) 72 | if desc_dist < self.THRESHOLD: 73 | predicted_loop_candidates[i][j] = True 74 | 75 | return np.array(predicted_loop_candidates) 76 | 77 | def predict_simple(self, point_cloud): 78 | input = make_sparse_tensor(point_cloud, 0.1).to(self.device) 79 | output_desc, output_feats = self.model(input) 80 | global_descriptor = output_desc.detach().to('cpu').numpy() 81 | return global_descriptor 82 | 83 | def predict(self, point_cloud): 84 | input = make_sparse_tensor(point_cloud, 0.1).to(self.device) 85 | output_desc, output_feats = self.model(input) # .squeeze() 86 | output_feats = output_feats[0] 87 | global_descriptor = output_desc.cpu().detach().to('cpu').numpy() 88 | 89 | global_descriptor = np.reshape(global_descriptor, (1, -1)) 90 | 91 | if len(global_descriptor) < 1: 92 | return None 93 | 94 | self.seen_descriptors.append(global_descriptor) 95 | 96 | if len(self.seen_descriptors) < 15: 97 | print("Not enough descriptors") 98 | print("len(seen_descriptors)", len(self.seen_descriptors)) 99 | return None 100 | 101 | db_seen_descriptors = np.copy(self.seen_descriptors) 102 | db_seen_descriptors = db_seen_descriptors[:-14] 103 | db_seen_descriptors = db_seen_descriptors.reshape( 104 | -1, np.shape(global_descriptor)[1]) 105 | 106 | feat_dists = cdist(global_descriptor, db_seen_descriptors, 107 | metric='cosine').reshape(-1) 108 | min_dist, nearest_idx = np.min(feat_dists), np.argmin(feat_dists) 109 | return min_dist, nearest_idx 110 | -------------------------------------------------------------------------------- /scripts/methods/MixVPR_.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | current_file_path = os.path.abspath(__file__) 5 | current_directory = os.path.dirname(current_file_path) 6 | mixVPR_path = os.path.join(current_directory, 'MixVPR') 7 | sys.path.insert(0, mixVPR_path) 8 | 9 | import utils 10 | import numpy as np 11 | import faiss.contrib.torch_utils 12 | import faiss 13 | from .MixVPR.models import helper 14 | 15 | import rospy 16 | import os 17 | import torch 18 | import cv2 19 | 20 | import sys 21 | import os 22 | 23 | import pytorch_lightning as pl 24 | import torch 25 | from torch.optim import lr_scheduler 26 | 27 | from scripts.methods.base import BaseMethod 28 | from tqdm import tqdm 29 | 30 | 31 | 32 | class VPRModel(pl.LightningModule): 33 | """This is the main model for Visual Place Recognition 34 | we use Pytorch Lightning for modularity purposes. 35 | 36 | Args: 37 | pl (_type_): _description_ 38 | """ 39 | 40 | def __init__(self, 41 | # ---- Backbone 42 | backbone_arch='resnet50', 43 | pretrained=True, 44 | layers_to_freeze=1, 45 | layers_to_crop=[], 46 | 47 | # ---- Aggregator 48 | agg_arch='ConvAP', # CosPlace, NetVLAD, GeM 49 | agg_config={}, 50 | 51 | # ---- Train hyperparameters 52 | lr=0.03, 53 | optimizer='sgd', 54 | weight_decay=1e-3, 55 | momentum=0.9, 56 | warmpup_steps=500, 57 | milestones=[5, 10, 15], 58 | lr_mult=0.3, 59 | 60 | # ----- Loss 61 | loss_name='MultiSimilarityLoss', 62 | miner_name='MultiSimilarityMiner', 63 | miner_margin=0.1, 64 | faiss_gpu=True 65 | ): 66 | super().__init__() 67 | self.encoder_arch = backbone_arch 68 | self.pretrained = pretrained 69 | self.layers_to_freeze = layers_to_freeze 70 | self.layers_to_crop = layers_to_crop 71 | 72 | self.agg_arch = agg_arch 73 | self.agg_config = agg_config 74 | 75 | self.lr = lr 76 | self.optimizer = optimizer 77 | self.weight_decay = weight_decay 78 | self.momentum = momentum 79 | self.warmpup_steps = warmpup_steps 80 | self.milestones = milestones 81 | self.lr_mult = lr_mult 82 | 83 | self.loss_name = loss_name 84 | self.miner_name = miner_name 85 | self.miner_margin = miner_margin 86 | 87 | self.save_hyperparameters() # write hyperparams into a file 88 | 89 | self.loss_fn = utils.get_loss(loss_name) 90 | self.miner = utils.get_miner(miner_name, miner_margin) 91 | # we will keep track of the % of trivial pairs/triplets at the loss level 92 | self.batch_acc = [] 93 | 94 | self.faiss_gpu = faiss_gpu 95 | 96 | # ---------------------------------- 97 | # get the backbone and the aggregator 98 | self.backbone = helper.get_backbone( 99 | backbone_arch, pretrained, layers_to_freeze, layers_to_crop) 100 | self.aggregator = helper.get_aggregator(agg_arch, agg_config) 101 | 102 | # the forward pass of the lightning model 103 | def forward(self, x): 104 | x = self.backbone(x) 105 | x = self.aggregator(x) 106 | return x 107 | 108 | # configure the optimizer 109 | def configure_optimizers(self): 110 | if self.optimizer.lower() == 'sgd': 111 | optimizer = torch.optim.SGD(self.parameters(), 112 | lr=self.lr, 113 | weight_decay=self.weight_decay, 114 | momentum=self.momentum) 115 | elif self.optimizer.lower() == 'adamw': 116 | optimizer = torch.optim.AdamW(self.parameters(), 117 | lr=self.lr, 118 | weight_decay=self.weight_decay) 119 | elif self.optimizer.lower() == 'adam': 120 | optimizer = torch.optim.AdamW(self.parameters(), 121 | lr=self.lr, 122 | weight_decay=self.weight_decay) 123 | else: 124 | raise ValueError( 125 | f'Optimizer {self.optimizer} has not been added to "configure_optimizers()"') 126 | scheduler = lr_scheduler.MultiStepLR( 127 | optimizer, milestones=self.milestones, gamma=self.lr_mult) 128 | return [optimizer], [scheduler] 129 | 130 | # configure the optizer step, takes into account the warmup stage 131 | def optimizer_step(self, epoch, batch_idx, 132 | optimizer, optimizer_idx, optimizer_closure, 133 | on_tpu, using_native_amp, using_lbfgs): 134 | # warm up lr 135 | if self.trainer.global_step < self.warmpup_steps: 136 | lr_scale = min( 137 | 1., float(self.trainer.global_step + 1) / self.warmpup_steps) 138 | for pg in optimizer.param_groups: 139 | pg['lr'] = lr_scale * self.lr 140 | optimizer.step(closure=optimizer_closure) 141 | 142 | # The loss function call (this method will be called at each training iteration) 143 | def loss_function(self, descriptors, labels): 144 | # we mine the pairs/triplets if there is an online mining strategy 145 | if self.miner is not None: 146 | miner_outputs = self.miner(descriptors, labels) 147 | loss = self.loss_fn(descriptors, labels, miner_outputs) 148 | 149 | # calculate the % of trivial pairs/triplets 150 | # which do not contribute in the loss value 151 | nb_samples = descriptors.shape[0] 152 | nb_mined = len(set(miner_outputs[0].detach().cpu().numpy())) 153 | batch_acc = 1.0 - (nb_mined/nb_samples) 154 | 155 | else: # no online mining 156 | loss = self.loss_fn(descriptors, labels) 157 | batch_acc = 0.0 158 | if type(loss) == tuple: 159 | # somes losses do the online mining inside (they don't need a miner objet), 160 | # so they return the loss and the batch accuracy 161 | # for example, if you are developping a new loss function, you might be better 162 | # doing the online mining strategy inside the forward function of the loss class, 163 | # and return a tuple containing the loss value and the batch_accuracy (the % of valid pairs or triplets) 164 | loss, batch_acc = loss 165 | 166 | # keep accuracy of every batch and later reset it at epoch start 167 | self.batch_acc.append(batch_acc) 168 | # log it 169 | self.log('b_acc', sum(self.batch_acc) / 170 | len(self.batch_acc), prog_bar=True, logger=True) 171 | return loss 172 | 173 | # This is the training step that's executed at each iteration 174 | def training_step(self, batch, batch_idx): 175 | places, labels = batch 176 | 177 | # Note that GSVCities yields places (each containing N images) 178 | # which means the dataloader will return a batch containing BS places 179 | BS, N, ch, h, w = places.shape 180 | 181 | # reshape places and labels 182 | images = places.view(BS*N, ch, h, w) 183 | labels = labels.view(-1) 184 | 185 | # Feed forward the batch to the model 186 | # Here we are calling the method forward that we defined above 187 | descriptors = self(images) 188 | # Call the loss_function we defined above 189 | loss = self.loss_function(descriptors, labels) 190 | 191 | self.log('loss', loss.item(), logger=True) 192 | return {'loss': loss} 193 | 194 | # This is called at the end of eatch training epoch 195 | def training_epoch_end(self, training_step_outputs): 196 | # we empty the batch_acc list for next epoch 197 | self.batch_acc = [] 198 | 199 | # For validation, we will also iterate step by step over the validation set 200 | # this is the way Pytorch Lghtning is made. All about modularity, folks. 201 | def validation_step(self, batch, batch_idx, dataloader_idx=None): 202 | places, _ = batch 203 | # calculate descriptors 204 | descriptors = self(places) 205 | return descriptors.detach().cpu() 206 | 207 | def validation_epoch_end(self, val_step_outputs): 208 | """this return descriptors in their order 209 | depending on how the validation dataset is implemented 210 | for this project (MSLS val, Pittburg val), it is always references then queries 211 | [R1, R2, ..., Rn, Q1, Q2, ...] 212 | """ 213 | dm = self.trainer.datamodule 214 | # The following line is a hack: if we have only one validation set, then 215 | # we need to put the outputs in a list (Pytorch Lightning does not do it presently) 216 | if len(dm.val_datasets) == 1: # we need to put the outputs in a list 217 | val_step_outputs = [val_step_outputs] 218 | 219 | for i, (val_set_name, val_dataset) in enumerate(zip(dm.val_set_names, dm.val_datasets)): 220 | feats = torch.concat(val_step_outputs[i], dim=0) 221 | 222 | if 'pitts' in val_set_name: 223 | # split to ref and queries 224 | num_references = val_dataset.dbStruct.numDb 225 | num_queries = len(val_dataset)-num_references 226 | positives = val_dataset.getPositives() 227 | elif 'msls' in val_set_name: 228 | # split to ref and queries 229 | num_references = val_dataset.num_references 230 | num_queries = len(val_dataset)-num_references 231 | positives = val_dataset.pIdx 232 | else: 233 | print( 234 | f'Please implement validation_epoch_end for {val_set_name}') 235 | raise NotImplemented 236 | 237 | r_list = feats[: num_references] 238 | q_list = feats[num_references:] 239 | pitts_dict = utils.get_validation_recalls(r_list=r_list, 240 | q_list=q_list, 241 | k_values=[ 242 | 1, 5, 10, 15, 20, 50, 100], 243 | gt=positives, 244 | print_results=True, 245 | dataset_name=val_set_name, 246 | faiss_gpu=self.faiss_gpu 247 | ) 248 | del r_list, q_list, feats, num_references, positives 249 | 250 | self.log(f'{val_set_name}/R1', 251 | pitts_dict[1], prog_bar=False, logger=True) 252 | self.log(f'{val_set_name}/R5', 253 | pitts_dict[5], prog_bar=False, logger=True) 254 | self.log(f'{val_set_name}/R10', 255 | pitts_dict[10], prog_bar=False, logger=True) 256 | print('\n\n') 257 | 258 | 259 | class MixVPR(BaseMethod): 260 | 261 | def __init__(self, threshold): 262 | super().__init__() 263 | self.threshold = threshold 264 | 265 | use_cuda = torch.cuda.is_available() 266 | # Note that images must be resized to 320x320 267 | model = VPRModel(backbone_arch='resnet50', 268 | layers_to_crop=[4], 269 | agg_arch='MixVPR', 270 | agg_config={'in_channels': 1024, 271 | 'in_h': 20, 272 | 'in_w': 20, 273 | 'out_channels': 1024, 274 | 'mix_depth': 4, 275 | 'mlp_ratio': 1, 276 | 'out_rows': 4}, 277 | faiss_gpu=use_cuda 278 | ) 279 | 280 | current_dir = os.path.dirname(os.path.realpath(__file__)) 281 | model_path = os.path.join( 282 | current_dir, 'MixVPR', 'resnet50_MixVPR_4096_channels(1024)_rows(4).ckpt') 283 | state_dict = torch.load(model_path) 284 | model.load_state_dict(state_dict) 285 | model.eval() 286 | 287 | self.device = torch.device( 288 | "cuda" if use_cuda else "cpu") 289 | model.to(self.device) 290 | self.model = model 291 | 292 | rospy.loginfo(f'Loading model from {model_path}') 293 | if use_cuda: 294 | res = faiss.StandardGpuResources() 295 | flat_config = faiss.GpuIndexFlatConfig() 296 | flat_config.useFloat16 = True 297 | flat_config.device = 0 298 | self.faiss_index = faiss.GpuIndexFlatL2(res, 4096, flat_config) 299 | else: 300 | self.faiss_index = faiss.IndexFlatL2(4096) 301 | rospy.loginfo(f'Model is loaded') 302 | 303 | 304 | def __str__(self) -> str: 305 | return self.__class__.__name__ + f' (threshold {self.threshold})' 306 | 307 | def predict_loop_candidates(self,cloud_vec, odom_vec, img_vec, min_idx_diff=1) -> np.ndarray: 308 | size = len(img_vec) 309 | predicted_loop_candidates = np.full((size, size), False) 310 | 311 | descriptors = [] 312 | for i in tqdm(range(size)): 313 | image = img_vec[i] 314 | 315 | # Convert grayscale image to RGB 316 | if len(image.shape) == 2: 317 | image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) 318 | elif len(image.shape) == 3 and image.shape[2] == 1: 319 | image = cv2.cvtColor(image, cv2.COLOR_GRAY2RGB) 320 | elif len(image.shape) == 3 and image.shape[2] > 3: 321 | image = cv2.cvtColor(image, cv2.COLOR_RGBA2RGB) 322 | 323 | # Resize the image. Final shape is (320, 320) 324 | resized = cv2.resize(image, (320, 320)) 325 | 326 | with torch.no_grad(): 327 | img_tensor = torch.from_numpy(resized).permute( 328 | 2, 0, 1).unsqueeze(0).float().to(self.device) 329 | descriptor = self.model(img_tensor).cpu() 330 | 331 | descriptors.append(descriptor) 332 | 333 | if i < min_idx_diff: 334 | continue 335 | 336 | self.faiss_index.reset() 337 | self.faiss_index.add( 338 | np.vstack(descriptors[:-min_idx_diff])) 339 | 340 | distances, indices = self.faiss_index.search(descriptor, len(descriptors[:-min_idx_diff])) 341 | 342 | for idx, desc_dist in zip(indices[0], distances[0]): 343 | if desc_dist < self.threshold: 344 | predicted_loop_candidates[i][idx] = True 345 | 346 | return np.array(predicted_loop_candidates) 347 | -------------------------------------------------------------------------------- /scripts/methods/SuperGlue.py: -------------------------------------------------------------------------------- 1 | from scripts.methods.base import BaseMethod 2 | from scripts.methods.superglue.matching import Matching 3 | from .superglue.utils import frame2tensor 4 | 5 | import cv2 6 | import torch 7 | from tqdm import tqdm 8 | import numpy as np 9 | 10 | 11 | class SuperGlue(BaseMethod): 12 | 13 | def __init__(self, threshold): 14 | self.THRESHOLD = threshold 15 | 16 | self.device = 'cuda' if torch.cuda.is_available() else 'cpu' 17 | 18 | config = { 19 | 'superpoint': 20 | { 21 | 'nms_radius': 4, 22 | 'keypoint_threshold': 0.005, 23 | 'max_keypoints': 1024 24 | }, 25 | 'superglue': 26 | { 27 | 'weights': 'indoor', 28 | 'sinkhorn_iterations': 20, 29 | 'match_threshold': 0.2 30 | } 31 | } 32 | 33 | self.matching = Matching(config).eval().to(self.device) 34 | 35 | self.N_ROWS = 480 36 | self.N_COLS = 640 37 | 38 | def __str__(self) -> str: 39 | return self.__class__.__name__ + f' (threshold {self.THRESHOLD})' 40 | 41 | def predict_loop_candidates(self, cloud_vec, odom_vec, img_vec, min_idx_diff=1) -> np.ndarray: 42 | size = len(img_vec) 43 | predicted_loop_candidates = np.full((size, size), False) 44 | 45 | for i in tqdm(range(size)): 46 | 47 | resized1 = cv2.resize(img_vec[i], (self.N_COLS, self.N_ROWS)) 48 | tensor1 = frame2tensor(resized1, self.device) 49 | 50 | for j in range(i - min_idx_diff + 1): 51 | tensor2 = frame2tensor(cv2.resize( 52 | img_vec[j], (self.N_COLS, self.N_ROWS)), self.device) 53 | 54 | pred = self.matching({'image0': tensor1, 'image1': tensor2}) 55 | pred = {k: v[0].cpu().detach().numpy() 56 | for k, v in pred.items()} 57 | 58 | kpts0, kpts1 = pred['keypoints0'], pred['keypoints1'] 59 | matches, conf = pred['matches0'], pred['matching_scores0'] 60 | valid = matches > -1 61 | mkpts0 = kpts0[valid] 62 | 63 | del tensor2 64 | del pred 65 | 66 | desc_dist = mkpts0.shape[0] 67 | if desc_dist > self.THRESHOLD: 68 | predicted_loop_candidates[i][j] = True 69 | 70 | del tensor1 71 | 72 | return np.array(predicted_loop_candidates) 73 | -------------------------------------------------------------------------------- /scripts/methods/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/scripts/methods/__init__.py -------------------------------------------------------------------------------- /scripts/methods/base.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from typing import List 3 | from sensor_msgs.msg import PointCloud2, Image 4 | from geometry_msgs.msg import Pose 5 | 6 | 7 | class BaseMethod(ABC): 8 | @abstractmethod 9 | def predict_loop_candidates(self, cloud_vec: List[PointCloud2], odom_vec: List[Pose], 10 | img_vec: List[Image], min_idx_diff: int) -> List[List[bool]]: 11 | pass 12 | -------------------------------------------------------------------------------- /scripts/methods/superglue/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/4ku/Place-recognition-evaluation/d13443c03eaf3ef9b6de5c07b6ebbbf8caa4e84a/scripts/methods/superglue/__init__.py -------------------------------------------------------------------------------- /scripts/methods/superglue/matching.py: -------------------------------------------------------------------------------- 1 | # %BANNER_BEGIN% 2 | # --------------------------------------------------------------------- 3 | # %COPYRIGHT_BEGIN% 4 | # 5 | # Magic Leap, Inc. ("COMPANY") CONFIDENTIAL 6 | # 7 | # Unpublished Copyright (c) 2020 8 | # Magic Leap, Inc., All Rights Reserved. 9 | # 10 | # NOTICE: All information contained herein is, and remains the property 11 | # of COMPANY. The intellectual and technical concepts contained herein 12 | # are proprietary to COMPANY and may be covered by U.S. and Foreign 13 | # Patents, patents in process, and are protected by trade secret or 14 | # copyright law. Dissemination of this information or reproduction of 15 | # this material is strictly forbidden unless prior written permission is 16 | # obtained from COMPANY. Access to the source code contained herein is 17 | # hereby forbidden to anyone except current COMPANY employees, managers 18 | # or contractors who have executed Confidentiality and Non-disclosure 19 | # agreements explicitly covering such access. 20 | # 21 | # The copyright notice above does not evidence any actual or intended 22 | # publication or disclosure of this source code, which includes 23 | # information that is confidential and/or proprietary, and is a trade 24 | # secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, 25 | # PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS 26 | # SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS 27 | # STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND 28 | # INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE 29 | # CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS 30 | # TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, 31 | # USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART. 32 | # 33 | # %COPYRIGHT_END% 34 | # ---------------------------------------------------------------------- 35 | # %AUTHORS_BEGIN% 36 | # 37 | # Originating Authors: Paul-Edouard Sarlin 38 | # 39 | # %AUTHORS_END% 40 | # --------------------------------------------------------------------*/ 41 | # %BANNER_END% 42 | 43 | import torch 44 | 45 | from .superpoint import SuperPoint 46 | from .superglue import SuperGlue 47 | 48 | 49 | class Matching(torch.nn.Module): 50 | """ Image Matching Frontend (SuperPoint + SuperGlue) """ 51 | def __init__(self, config={}): 52 | super().__init__() 53 | self.superpoint = SuperPoint(config.get('superpoint', {})) 54 | self.superglue = SuperGlue(config.get('superglue', {})) 55 | 56 | def forward(self, data): 57 | """ Run SuperPoint (optionally) and SuperGlue 58 | SuperPoint is skipped if ['keypoints0', 'keypoints1'] exist in input 59 | Args: 60 | data: dictionary with minimal keys: ['image0', 'image1'] 61 | """ 62 | pred = {} 63 | 64 | # Extract SuperPoint (keypoints, scores, descriptors) if not provided 65 | if 'keypoints0' not in data: 66 | pred0 = self.superpoint({'image': data['image0']}) 67 | pred = {**pred, **{k+'0': v for k, v in pred0.items()}} 68 | if 'keypoints1' not in data: 69 | pred1 = self.superpoint({'image': data['image1']}) 70 | pred = {**pred, **{k+'1': v for k, v in pred1.items()}} 71 | 72 | # Batch all features 73 | # We should either have i) one image per batch, or 74 | # ii) the same number of local features for all images in the batch. 75 | data = {**data, **pred} 76 | 77 | for k in data: 78 | if isinstance(data[k], (list, tuple)): 79 | data[k] = torch.stack(data[k]) 80 | 81 | # Perform the matching 82 | pred = {**pred, **self.superglue(data)} 83 | 84 | return pred 85 | -------------------------------------------------------------------------------- /scripts/methods/superglue/superglue.py: -------------------------------------------------------------------------------- 1 | # %BANNER_BEGIN% 2 | # --------------------------------------------------------------------- 3 | # %COPYRIGHT_BEGIN% 4 | # 5 | # Magic Leap, Inc. ("COMPANY") CONFIDENTIAL 6 | # 7 | # Unpublished Copyright (c) 2020 8 | # Magic Leap, Inc., All Rights Reserved. 9 | # 10 | # NOTICE: All information contained herein is, and remains the property 11 | # of COMPANY. The intellectual and technical concepts contained herein 12 | # are proprietary to COMPANY and may be covered by U.S. and Foreign 13 | # Patents, patents in process, and are protected by trade secret or 14 | # copyright law. Dissemination of this information or reproduction of 15 | # this material is strictly forbidden unless prior written permission is 16 | # obtained from COMPANY. Access to the source code contained herein is 17 | # hereby forbidden to anyone except current COMPANY employees, managers 18 | # or contractors who have executed Confidentiality and Non-disclosure 19 | # agreements explicitly covering such access. 20 | # 21 | # The copyright notice above does not evidence any actual or intended 22 | # publication or disclosure of this source code, which includes 23 | # information that is confidential and/or proprietary, and is a trade 24 | # secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, 25 | # PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS 26 | # SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS 27 | # STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND 28 | # INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE 29 | # CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS 30 | # TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, 31 | # USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART. 32 | # 33 | # %COPYRIGHT_END% 34 | # ---------------------------------------------------------------------- 35 | # %AUTHORS_BEGIN% 36 | # 37 | # Originating Authors: Paul-Edouard Sarlin 38 | # 39 | # %AUTHORS_END% 40 | # --------------------------------------------------------------------*/ 41 | # %BANNER_END% 42 | 43 | from copy import deepcopy 44 | from pathlib import Path 45 | from typing import List, Tuple 46 | 47 | import torch 48 | from torch import nn 49 | 50 | 51 | def MLP(channels: List[int], do_bn: bool = True) -> nn.Module: 52 | """ Multi-layer perceptron """ 53 | n = len(channels) 54 | layers = [] 55 | for i in range(1, n): 56 | layers.append( 57 | nn.Conv1d(channels[i - 1], channels[i], kernel_size=1, bias=True)) 58 | if i < (n-1): 59 | if do_bn: 60 | layers.append(nn.BatchNorm1d(channels[i])) 61 | layers.append(nn.ReLU()) 62 | return nn.Sequential(*layers) 63 | 64 | 65 | def normalize_keypoints(kpts, image_shape): 66 | """ Normalize keypoints locations based on image image_shape""" 67 | _, _, height, width = image_shape 68 | one = kpts.new_tensor(1) 69 | size = torch.stack([one*width, one*height])[None] 70 | center = size / 2 71 | scaling = size.max(1, keepdim=True).values * 0.7 72 | return (kpts - center[:, None, :]) / scaling[:, None, :] 73 | 74 | 75 | class KeypointEncoder(nn.Module): 76 | """ Joint encoding of visual appearance and location using MLPs""" 77 | def __init__(self, feature_dim: int, layers: List[int]) -> None: 78 | super().__init__() 79 | self.encoder = MLP([3] + layers + [feature_dim]) 80 | nn.init.constant_(self.encoder[-1].bias, 0.0) 81 | 82 | def forward(self, kpts, scores): 83 | inputs = [kpts.transpose(1, 2), scores.unsqueeze(1)] 84 | return self.encoder(torch.cat(inputs, dim=1)) 85 | 86 | 87 | def attention(query: torch.Tensor, key: torch.Tensor, value: torch.Tensor) -> Tuple[torch.Tensor,torch.Tensor]: 88 | dim = query.shape[1] 89 | scores = torch.einsum('bdhn,bdhm->bhnm', query, key) / dim**.5 90 | prob = torch.nn.functional.softmax(scores, dim=-1) 91 | return torch.einsum('bhnm,bdhm->bdhn', prob, value), prob 92 | 93 | 94 | class MultiHeadedAttention(nn.Module): 95 | """ Multi-head attention to increase model expressivitiy """ 96 | def __init__(self, num_heads: int, d_model: int): 97 | super().__init__() 98 | assert d_model % num_heads == 0 99 | self.dim = d_model // num_heads 100 | self.num_heads = num_heads 101 | self.merge = nn.Conv1d(d_model, d_model, kernel_size=1) 102 | self.proj = nn.ModuleList([deepcopy(self.merge) for _ in range(3)]) 103 | 104 | def forward(self, query: torch.Tensor, key: torch.Tensor, value: torch.Tensor) -> torch.Tensor: 105 | batch_dim = query.size(0) 106 | query, key, value = [l(x).view(batch_dim, self.dim, self.num_heads, -1) 107 | for l, x in zip(self.proj, (query, key, value))] 108 | x, _ = attention(query, key, value) 109 | return self.merge(x.contiguous().view(batch_dim, self.dim*self.num_heads, -1)) 110 | 111 | 112 | class AttentionalPropagation(nn.Module): 113 | def __init__(self, feature_dim: int, num_heads: int): 114 | super().__init__() 115 | self.attn = MultiHeadedAttention(num_heads, feature_dim) 116 | self.mlp = MLP([feature_dim*2, feature_dim*2, feature_dim]) 117 | nn.init.constant_(self.mlp[-1].bias, 0.0) 118 | 119 | def forward(self, x: torch.Tensor, source: torch.Tensor) -> torch.Tensor: 120 | message = self.attn(x, source, source) 121 | return self.mlp(torch.cat([x, message], dim=1)) 122 | 123 | 124 | class AttentionalGNN(nn.Module): 125 | def __init__(self, feature_dim: int, layer_names: List[str]) -> None: 126 | super().__init__() 127 | self.layers = nn.ModuleList([ 128 | AttentionalPropagation(feature_dim, 4) 129 | for _ in range(len(layer_names))]) 130 | self.names = layer_names 131 | 132 | def forward(self, desc0: torch.Tensor, desc1: torch.Tensor) -> Tuple[torch.Tensor,torch.Tensor]: 133 | for layer, name in zip(self.layers, self.names): 134 | if name == 'cross': 135 | src0, src1 = desc1, desc0 136 | else: # if name == 'self': 137 | src0, src1 = desc0, desc1 138 | delta0, delta1 = layer(desc0, src0), layer(desc1, src1) 139 | desc0, desc1 = (desc0 + delta0), (desc1 + delta1) 140 | return desc0, desc1 141 | 142 | 143 | def log_sinkhorn_iterations(Z: torch.Tensor, log_mu: torch.Tensor, log_nu: torch.Tensor, iters: int) -> torch.Tensor: 144 | """ Perform Sinkhorn Normalization in Log-space for stability""" 145 | u, v = torch.zeros_like(log_mu), torch.zeros_like(log_nu) 146 | for _ in range(iters): 147 | u = log_mu - torch.logsumexp(Z + v.unsqueeze(1), dim=2) 148 | v = log_nu - torch.logsumexp(Z + u.unsqueeze(2), dim=1) 149 | return Z + u.unsqueeze(2) + v.unsqueeze(1) 150 | 151 | 152 | def log_optimal_transport(scores: torch.Tensor, alpha: torch.Tensor, iters: int) -> torch.Tensor: 153 | """ Perform Differentiable Optimal Transport in Log-space for stability""" 154 | b, m, n = scores.shape 155 | one = scores.new_tensor(1) 156 | ms, ns = (m*one).to(scores), (n*one).to(scores) 157 | 158 | bins0 = alpha.expand(b, m, 1) 159 | bins1 = alpha.expand(b, 1, n) 160 | alpha = alpha.expand(b, 1, 1) 161 | 162 | couplings = torch.cat([torch.cat([scores, bins0], -1), 163 | torch.cat([bins1, alpha], -1)], 1) 164 | 165 | norm = - (ms + ns).log() 166 | log_mu = torch.cat([norm.expand(m), ns.log()[None] + norm]) 167 | log_nu = torch.cat([norm.expand(n), ms.log()[None] + norm]) 168 | log_mu, log_nu = log_mu[None].expand(b, -1), log_nu[None].expand(b, -1) 169 | 170 | Z = log_sinkhorn_iterations(couplings, log_mu, log_nu, iters) 171 | Z = Z - norm # multiply probabilities by M+N 172 | return Z 173 | 174 | 175 | def arange_like(x, dim: int): 176 | return x.new_ones(x.shape[dim]).cumsum(0) - 1 # traceable in 1.1 177 | 178 | 179 | class SuperGlue(nn.Module): 180 | """SuperGlue feature matching middle-end 181 | 182 | Given two sets of keypoints and locations, we determine the 183 | correspondences by: 184 | 1. Keypoint Encoding (normalization + visual feature and location fusion) 185 | 2. Graph Neural Network with multiple self and cross-attention layers 186 | 3. Final projection layer 187 | 4. Optimal Transport Layer (a differentiable Hungarian matching algorithm) 188 | 5. Thresholding matrix based on mutual exclusivity and a match_threshold 189 | 190 | The correspondence ids use -1 to indicate non-matching points. 191 | 192 | Paul-Edouard Sarlin, Daniel DeTone, Tomasz Malisiewicz, and Andrew 193 | Rabinovich. SuperGlue: Learning Feature Matching with Graph Neural 194 | Networks. In CVPR, 2020. https://arxiv.org/abs/1911.11763 195 | 196 | """ 197 | default_config = { 198 | 'descriptor_dim': 256, 199 | 'weights': 'indoor', 200 | 'keypoint_encoder': [32, 64, 128, 256], 201 | 'GNN_layers': ['self', 'cross'] * 9, 202 | 'sinkhorn_iterations': 100, 203 | 'match_threshold': 0.2, 204 | } 205 | 206 | def __init__(self, config): 207 | super().__init__() 208 | self.config = {**self.default_config, **config} 209 | 210 | self.kenc = KeypointEncoder( 211 | self.config['descriptor_dim'], self.config['keypoint_encoder']) 212 | 213 | self.gnn = AttentionalGNN( 214 | feature_dim=self.config['descriptor_dim'], layer_names=self.config['GNN_layers']) 215 | 216 | self.final_proj = nn.Conv1d( 217 | self.config['descriptor_dim'], self.config['descriptor_dim'], 218 | kernel_size=1, bias=True) 219 | 220 | bin_score = torch.nn.Parameter(torch.tensor(1.)) 221 | self.register_parameter('bin_score', bin_score) 222 | 223 | assert self.config['weights'] in ['indoor', 'outdoor'] 224 | path = Path(__file__).parent 225 | path = path / 'weights/superglue_{}.pth'.format(self.config['weights']) 226 | self.load_state_dict(torch.load(str(path))) 227 | print('Loaded SuperGlue model (\"{}\" weights)'.format( 228 | self.config['weights'])) 229 | 230 | def forward(self, data): 231 | """Run SuperGlue on a pair of keypoints and descriptors""" 232 | desc0, desc1 = data['descriptors0'], data['descriptors1'] 233 | kpts0, kpts1 = data['keypoints0'], data['keypoints1'] 234 | 235 | if kpts0.shape[1] == 0 or kpts1.shape[1] == 0: # no keypoints 236 | shape0, shape1 = kpts0.shape[:-1], kpts1.shape[:-1] 237 | return { 238 | 'matches0': kpts0.new_full(shape0, -1, dtype=torch.int), 239 | 'matches1': kpts1.new_full(shape1, -1, dtype=torch.int), 240 | 'matching_scores0': kpts0.new_zeros(shape0), 241 | 'matching_scores1': kpts1.new_zeros(shape1), 242 | } 243 | 244 | # Keypoint normalization. 245 | kpts0 = normalize_keypoints(kpts0, data['image0'].shape) 246 | kpts1 = normalize_keypoints(kpts1, data['image1'].shape) 247 | 248 | # Keypoint MLP encoder. 249 | desc0 = desc0 + self.kenc(kpts0, data['scores0']) 250 | desc1 = desc1 + self.kenc(kpts1, data['scores1']) 251 | 252 | # Multi-layer Transformer network. 253 | desc0, desc1 = self.gnn(desc0, desc1) 254 | 255 | # Final MLP projection. 256 | mdesc0, mdesc1 = self.final_proj(desc0), self.final_proj(desc1) 257 | 258 | # Compute matching descriptor distance. 259 | scores = torch.einsum('bdn,bdm->bnm', mdesc0, mdesc1) 260 | scores = scores / self.config['descriptor_dim']**.5 261 | 262 | # Run the optimal transport. 263 | scores = log_optimal_transport( 264 | scores, self.bin_score, 265 | iters=self.config['sinkhorn_iterations']) 266 | 267 | # Get the matches with score above "match_threshold". 268 | max0, max1 = scores[:, :-1, :-1].max(2), scores[:, :-1, :-1].max(1) 269 | indices0, indices1 = max0.indices, max1.indices 270 | mutual0 = arange_like(indices0, 1)[None] == indices1.gather(1, indices0) 271 | mutual1 = arange_like(indices1, 1)[None] == indices0.gather(1, indices1) 272 | zero = scores.new_tensor(0) 273 | mscores0 = torch.where(mutual0, max0.values.exp(), zero) 274 | mscores1 = torch.where(mutual1, mscores0.gather(1, indices1), zero) 275 | valid0 = mutual0 & (mscores0 > self.config['match_threshold']) 276 | valid1 = mutual1 & valid0.gather(1, indices1) 277 | indices0 = torch.where(valid0, indices0, indices0.new_tensor(-1)) 278 | indices1 = torch.where(valid1, indices1, indices1.new_tensor(-1)) 279 | 280 | return { 281 | 'matches0': indices0, # use -1 for invalid match 282 | 'matches1': indices1, # use -1 for invalid match 283 | 'matching_scores0': mscores0, 284 | 'matching_scores1': mscores1, 285 | } 286 | -------------------------------------------------------------------------------- /scripts/methods/superglue/superpoint.py: -------------------------------------------------------------------------------- 1 | # %BANNER_BEGIN% 2 | # --------------------------------------------------------------------- 3 | # %COPYRIGHT_BEGIN% 4 | # 5 | # Magic Leap, Inc. ("COMPANY") CONFIDENTIAL 6 | # 7 | # Unpublished Copyright (c) 2020 8 | # Magic Leap, Inc., All Rights Reserved. 9 | # 10 | # NOTICE: All information contained herein is, and remains the property 11 | # of COMPANY. The intellectual and technical concepts contained herein 12 | # are proprietary to COMPANY and may be covered by U.S. and Foreign 13 | # Patents, patents in process, and are protected by trade secret or 14 | # copyright law. Dissemination of this information or reproduction of 15 | # this material is strictly forbidden unless prior written permission is 16 | # obtained from COMPANY. Access to the source code contained herein is 17 | # hereby forbidden to anyone except current COMPANY employees, managers 18 | # or contractors who have executed Confidentiality and Non-disclosure 19 | # agreements explicitly covering such access. 20 | # 21 | # The copyright notice above does not evidence any actual or intended 22 | # publication or disclosure of this source code, which includes 23 | # information that is confidential and/or proprietary, and is a trade 24 | # secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, 25 | # PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS 26 | # SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS 27 | # STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND 28 | # INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE 29 | # CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS 30 | # TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, 31 | # USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART. 32 | # 33 | # %COPYRIGHT_END% 34 | # ---------------------------------------------------------------------- 35 | # %AUTHORS_BEGIN% 36 | # 37 | # Originating Authors: Paul-Edouard Sarlin 38 | # 39 | # %AUTHORS_END% 40 | # --------------------------------------------------------------------*/ 41 | # %BANNER_END% 42 | 43 | from pathlib import Path 44 | import torch 45 | from torch import nn 46 | 47 | def simple_nms(scores, nms_radius: int): 48 | """ Fast Non-maximum suppression to remove nearby points """ 49 | assert(nms_radius >= 0) 50 | 51 | def max_pool(x): 52 | return torch.nn.functional.max_pool2d( 53 | x, kernel_size=nms_radius*2+1, stride=1, padding=nms_radius) 54 | 55 | zeros = torch.zeros_like(scores) 56 | max_mask = scores == max_pool(scores) 57 | for _ in range(2): 58 | supp_mask = max_pool(max_mask.float()) > 0 59 | supp_scores = torch.where(supp_mask, zeros, scores) 60 | new_max_mask = supp_scores == max_pool(supp_scores) 61 | max_mask = max_mask | (new_max_mask & (~supp_mask)) 62 | return torch.where(max_mask, scores, zeros) 63 | 64 | 65 | def remove_borders(keypoints, scores, border: int, height: int, width: int): 66 | """ Removes keypoints too close to the border """ 67 | mask_h = (keypoints[:, 0] >= border) & (keypoints[:, 0] < (height - border)) 68 | mask_w = (keypoints[:, 1] >= border) & (keypoints[:, 1] < (width - border)) 69 | mask = mask_h & mask_w 70 | return keypoints[mask], scores[mask] 71 | 72 | 73 | def top_k_keypoints(keypoints, scores, k: int): 74 | if k >= len(keypoints): 75 | return keypoints, scores 76 | scores, indices = torch.topk(scores, k, dim=0) 77 | return keypoints[indices], scores 78 | 79 | 80 | def sample_descriptors(keypoints, descriptors, s: int = 8): 81 | """ Interpolate descriptors at keypoint locations """ 82 | b, c, h, w = descriptors.shape 83 | keypoints = keypoints - s / 2 + 0.5 84 | keypoints /= torch.tensor([(w*s - s/2 - 0.5), (h*s - s/2 - 0.5)], 85 | ).to(keypoints)[None] 86 | keypoints = keypoints*2 - 1 # normalize to (-1, 1) 87 | args = {'align_corners': True} if torch.__version__ >= '1.3' else {} 88 | descriptors = torch.nn.functional.grid_sample( 89 | descriptors, keypoints.view(b, 1, -1, 2), mode='bilinear', **args) 90 | descriptors = torch.nn.functional.normalize( 91 | descriptors.reshape(b, c, -1), p=2, dim=1) 92 | return descriptors 93 | 94 | 95 | class SuperPoint(nn.Module): 96 | """SuperPoint Convolutional Detector and Descriptor 97 | 98 | SuperPoint: Self-Supervised Interest Point Detection and 99 | Description. Daniel DeTone, Tomasz Malisiewicz, and Andrew 100 | Rabinovich. In CVPRW, 2019. https://arxiv.org/abs/1712.07629 101 | 102 | """ 103 | default_config = { 104 | 'descriptor_dim': 256, 105 | 'nms_radius': 4, 106 | 'keypoint_threshold': 0.005, 107 | 'max_keypoints': -1, 108 | 'remove_borders': 4, 109 | } 110 | 111 | def __init__(self, config): 112 | super().__init__() 113 | self.config = {**self.default_config, **config} 114 | 115 | self.relu = nn.ReLU(inplace=True) 116 | self.pool = nn.MaxPool2d(kernel_size=2, stride=2) 117 | c1, c2, c3, c4, c5 = 64, 64, 128, 128, 256 118 | 119 | self.conv1a = nn.Conv2d(1, c1, kernel_size=3, stride=1, padding=1) 120 | self.conv1b = nn.Conv2d(c1, c1, kernel_size=3, stride=1, padding=1) 121 | self.conv2a = nn.Conv2d(c1, c2, kernel_size=3, stride=1, padding=1) 122 | self.conv2b = nn.Conv2d(c2, c2, kernel_size=3, stride=1, padding=1) 123 | self.conv3a = nn.Conv2d(c2, c3, kernel_size=3, stride=1, padding=1) 124 | self.conv3b = nn.Conv2d(c3, c3, kernel_size=3, stride=1, padding=1) 125 | self.conv4a = nn.Conv2d(c3, c4, kernel_size=3, stride=1, padding=1) 126 | self.conv4b = nn.Conv2d(c4, c4, kernel_size=3, stride=1, padding=1) 127 | 128 | self.convPa = nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 129 | self.convPb = nn.Conv2d(c5, 65, kernel_size=1, stride=1, padding=0) 130 | 131 | self.convDa = nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 132 | self.convDb = nn.Conv2d( 133 | c5, self.config['descriptor_dim'], 134 | kernel_size=1, stride=1, padding=0) 135 | 136 | path = Path(__file__).parent / 'weights/superpoint_v1.pth' 137 | self.load_state_dict(torch.load(str(path))) 138 | 139 | mk = self.config['max_keypoints'] 140 | if mk == 0 or mk < -1: 141 | raise ValueError('\"max_keypoints\" must be positive or \"-1\"') 142 | 143 | print('Loaded SuperPoint model') 144 | 145 | def forward(self, data): 146 | """ Compute keypoints, scores, descriptors for image """ 147 | # Shared Encoder 148 | x = self.relu(self.conv1a(data['image'])) 149 | x = self.relu(self.conv1b(x)) 150 | x = self.pool(x) 151 | x = self.relu(self.conv2a(x)) 152 | x = self.relu(self.conv2b(x)) 153 | x = self.pool(x) 154 | x = self.relu(self.conv3a(x)) 155 | x = self.relu(self.conv3b(x)) 156 | x = self.pool(x) 157 | x = self.relu(self.conv4a(x)) 158 | x = self.relu(self.conv4b(x)) 159 | 160 | # Compute the dense keypoint scores 161 | cPa = self.relu(self.convPa(x)) 162 | scores = self.convPb(cPa) 163 | scores = torch.nn.functional.softmax(scores, 1)[:, :-1] 164 | b, _, h, w = scores.shape 165 | scores = scores.permute(0, 2, 3, 1).reshape(b, h, w, 8, 8) 166 | scores = scores.permute(0, 1, 3, 2, 4).reshape(b, h*8, w*8) 167 | scores = simple_nms(scores, self.config['nms_radius']) 168 | 169 | # Extract keypoints 170 | keypoints = [ 171 | torch.nonzero(s > self.config['keypoint_threshold']) 172 | for s in scores] 173 | scores = [s[tuple(k.t())] for s, k in zip(scores, keypoints)] 174 | 175 | # Discard keypoints near the image borders 176 | keypoints, scores = list(zip(*[ 177 | remove_borders(k, s, self.config['remove_borders'], h*8, w*8) 178 | for k, s in zip(keypoints, scores)])) 179 | 180 | # Keep the k keypoints with highest score 181 | if self.config['max_keypoints'] >= 0: 182 | keypoints, scores = list(zip(*[ 183 | top_k_keypoints(k, s, self.config['max_keypoints']) 184 | for k, s in zip(keypoints, scores)])) 185 | 186 | # Convert (h, w) to (x, y) 187 | keypoints = [torch.flip(k, [1]).float() for k in keypoints] 188 | 189 | # Compute the dense descriptors 190 | cDa = self.relu(self.convDa(x)) 191 | descriptors = self.convDb(cDa) 192 | descriptors = torch.nn.functional.normalize(descriptors, p=2, dim=1) 193 | 194 | # Extract descriptors 195 | descriptors = [sample_descriptors(k[None], d[None], 8)[0] 196 | for k, d in zip(keypoints, descriptors)] 197 | 198 | return { 199 | 'keypoints': keypoints, 200 | 'scores': scores, 201 | 'descriptors': descriptors, 202 | } 203 | -------------------------------------------------------------------------------- /scripts/methods/superglue/utils.py: -------------------------------------------------------------------------------- 1 | # %BANNER_BEGIN% 2 | # --------------------------------------------------------------------- 3 | # %COPYRIGHT_BEGIN% 4 | # 5 | # Magic Leap, Inc. ("COMPANY") CONFIDENTIAL 6 | # 7 | # Unpublished Copyright (c) 2020 8 | # Magic Leap, Inc., All Rights Reserved. 9 | # 10 | # NOTICE: All information contained herein is, and remains the property 11 | # of COMPANY. The intellectual and technical concepts contained herein 12 | # are proprietary to COMPANY and may be covered by U.S. and Foreign 13 | # Patents, patents in process, and are protected by trade secret or 14 | # copyright law. Dissemination of this information or reproduction of 15 | # this material is strictly forbidden unless prior written permission is 16 | # obtained from COMPANY. Access to the source code contained herein is 17 | # hereby forbidden to anyone except current COMPANY employees, managers 18 | # or contractors who have executed Confidentiality and Non-disclosure 19 | # agreements explicitly covering such access. 20 | # 21 | # The copyright notice above does not evidence any actual or intended 22 | # publication or disclosure of this source code, which includes 23 | # information that is confidential and/or proprietary, and is a trade 24 | # secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, 25 | # PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS 26 | # SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS 27 | # STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND 28 | # INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE 29 | # CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS 30 | # TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, 31 | # USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART. 32 | # 33 | # %COPYRIGHT_END% 34 | # ---------------------------------------------------------------------- 35 | # %AUTHORS_BEGIN% 36 | # 37 | # Originating Authors: Paul-Edouard Sarlin 38 | # Daniel DeTone 39 | # Tomasz Malisiewicz 40 | # 41 | # %AUTHORS_END% 42 | # --------------------------------------------------------------------*/ 43 | # %BANNER_END% 44 | 45 | from pathlib import Path 46 | import time 47 | from collections import OrderedDict 48 | from threading import Thread 49 | import numpy as np 50 | import cv2 51 | import torch 52 | import matplotlib.pyplot as plt 53 | import matplotlib 54 | matplotlib.use('Agg') 55 | 56 | 57 | class AverageTimer: 58 | """ Class to help manage printing simple timing of code execution. """ 59 | 60 | def __init__(self, smoothing=0.3, newline=False): 61 | self.smoothing = smoothing 62 | self.newline = newline 63 | self.times = OrderedDict() 64 | self.will_print = OrderedDict() 65 | self.reset() 66 | 67 | def reset(self): 68 | now = time.time() 69 | self.start = now 70 | self.last_time = now 71 | for name in self.will_print: 72 | self.will_print[name] = False 73 | 74 | def update(self, name='default'): 75 | now = time.time() 76 | dt = now - self.last_time 77 | if name in self.times: 78 | dt = self.smoothing * dt + (1 - self.smoothing) * self.times[name] 79 | self.times[name] = dt 80 | self.will_print[name] = True 81 | self.last_time = now 82 | 83 | def print(self, text='Timer'): 84 | total = 0. 85 | print('[{}]'.format(text), end=' ') 86 | for key in self.times: 87 | val = self.times[key] 88 | if self.will_print[key]: 89 | print('%s=%.3f' % (key, val), end=' ') 90 | total += val 91 | print('total=%.3f sec {%.1f FPS}' % (total, 1./total), end=' ') 92 | if self.newline: 93 | print(flush=True) 94 | else: 95 | print(end='\r', flush=True) 96 | self.reset() 97 | 98 | 99 | class VideoStreamer: 100 | """ Class to help process image streams. Four types of possible inputs:" 101 | 1.) USB Webcam. 102 | 2.) An IP camera 103 | 3.) A directory of images (files in directory matching 'image_glob'). 104 | 4.) A video file, such as an .mp4 or .avi file. 105 | """ 106 | def __init__(self, basedir, resize, skip, image_glob, max_length=1000000): 107 | self._ip_grabbed = False 108 | self._ip_running = False 109 | self._ip_camera = False 110 | self._ip_image = None 111 | self._ip_index = 0 112 | self.cap = [] 113 | self.camera = True 114 | self.video_file = False 115 | self.listing = [] 116 | self.resize = resize 117 | self.interp = cv2.INTER_AREA 118 | self.i = 0 119 | self.skip = skip 120 | self.max_length = max_length 121 | if isinstance(basedir, int) or basedir.isdigit(): 122 | print('==> Processing USB webcam input: {}'.format(basedir)) 123 | self.cap = cv2.VideoCapture(int(basedir)) 124 | self.listing = range(0, self.max_length) 125 | elif basedir.startswith(('http', 'rtsp')): 126 | print('==> Processing IP camera input: {}'.format(basedir)) 127 | self.cap = cv2.VideoCapture(basedir) 128 | self.start_ip_camera_thread() 129 | self._ip_camera = True 130 | self.listing = range(0, self.max_length) 131 | elif Path(basedir).is_dir(): 132 | print('==> Processing image directory input: {}'.format(basedir)) 133 | self.listing = list(Path(basedir).glob(image_glob[0])) 134 | for j in range(1, len(image_glob)): 135 | image_path = list(Path(basedir).glob(image_glob[j])) 136 | self.listing = self.listing + image_path 137 | self.listing.sort() 138 | self.listing = self.listing[::self.skip] 139 | self.max_length = np.min([self.max_length, len(self.listing)]) 140 | if self.max_length == 0: 141 | raise IOError('No images found (maybe bad \'image_glob\' ?)') 142 | self.listing = self.listing[:self.max_length] 143 | self.camera = False 144 | elif Path(basedir).exists(): 145 | print('==> Processing video input: {}'.format(basedir)) 146 | self.cap = cv2.VideoCapture(basedir) 147 | self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) 148 | num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT)) 149 | self.listing = range(0, num_frames) 150 | self.listing = self.listing[::self.skip] 151 | self.video_file = True 152 | self.max_length = np.min([self.max_length, len(self.listing)]) 153 | self.listing = self.listing[:self.max_length] 154 | else: 155 | raise ValueError('VideoStreamer input \"{}\" not recognized.'.format(basedir)) 156 | if self.camera and not self.cap.isOpened(): 157 | raise IOError('Could not read camera') 158 | 159 | def load_image(self, impath): 160 | """ Read image as grayscale and resize to img_size. 161 | Inputs 162 | impath: Path to input image. 163 | Returns 164 | grayim: uint8 numpy array sized H x W. 165 | """ 166 | grayim = cv2.imread(impath, 0) 167 | if grayim is None: 168 | raise Exception('Error reading image %s' % impath) 169 | w, h = grayim.shape[1], grayim.shape[0] 170 | w_new, h_new = process_resize(w, h, self.resize) 171 | grayim = cv2.resize( 172 | grayim, (w_new, h_new), interpolation=self.interp) 173 | return grayim 174 | 175 | def next_frame(self): 176 | """ Return the next frame, and increment internal counter. 177 | Returns 178 | image: Next H x W image. 179 | status: True or False depending whether image was loaded. 180 | """ 181 | 182 | if self.i == self.max_length: 183 | return (None, False) 184 | if self.camera: 185 | 186 | if self._ip_camera: 187 | #Wait for first image, making sure we haven't exited 188 | while self._ip_grabbed is False and self._ip_exited is False: 189 | time.sleep(.001) 190 | 191 | ret, image = self._ip_grabbed, self._ip_image.copy() 192 | if ret is False: 193 | self._ip_running = False 194 | else: 195 | ret, image = self.cap.read() 196 | if ret is False: 197 | print('VideoStreamer: Cannot get image from camera') 198 | return (None, False) 199 | w, h = image.shape[1], image.shape[0] 200 | if self.video_file: 201 | self.cap.set(cv2.CAP_PROP_POS_FRAMES, self.listing[self.i]) 202 | 203 | w_new, h_new = process_resize(w, h, self.resize) 204 | image = cv2.resize(image, (w_new, h_new), 205 | interpolation=self.interp) 206 | image = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) 207 | else: 208 | image_file = str(self.listing[self.i]) 209 | image = self.load_image(image_file) 210 | self.i = self.i + 1 211 | return (image, True) 212 | 213 | def start_ip_camera_thread(self): 214 | self._ip_thread = Thread(target=self.update_ip_camera, args=()) 215 | self._ip_running = True 216 | self._ip_thread.start() 217 | self._ip_exited = False 218 | return self 219 | 220 | def update_ip_camera(self): 221 | while self._ip_running: 222 | ret, img = self.cap.read() 223 | if ret is False: 224 | self._ip_running = False 225 | self._ip_exited = True 226 | self._ip_grabbed = False 227 | return 228 | 229 | self._ip_image = img 230 | self._ip_grabbed = ret 231 | self._ip_index += 1 232 | #print('IPCAMERA THREAD got frame {}'.format(self._ip_index)) 233 | 234 | 235 | def cleanup(self): 236 | self._ip_running = False 237 | 238 | # --- PREPROCESSING --- 239 | 240 | def process_resize(w, h, resize): 241 | assert(len(resize) > 0 and len(resize) <= 2) 242 | if len(resize) == 1 and resize[0] > -1: 243 | scale = resize[0] / max(h, w) 244 | w_new, h_new = int(round(w*scale)), int(round(h*scale)) 245 | elif len(resize) == 1 and resize[0] == -1: 246 | w_new, h_new = w, h 247 | else: # len(resize) == 2: 248 | w_new, h_new = resize[0], resize[1] 249 | 250 | # Issue warning if resolution is too small or too large. 251 | if max(w_new, h_new) < 160: 252 | print('Warning: input resolution is very small, results may vary') 253 | elif max(w_new, h_new) > 2000: 254 | print('Warning: input resolution is very large, results may vary') 255 | 256 | return w_new, h_new 257 | 258 | 259 | def frame2tensor(frame, device): 260 | return torch.from_numpy(frame/255.).float()[None, None].to(device) 261 | 262 | 263 | def read_image(path, device, resize, rotation, resize_float): 264 | image = cv2.imread(str(path), cv2.IMREAD_GRAYSCALE) 265 | if image is None: 266 | return None, None, None 267 | w, h = image.shape[1], image.shape[0] 268 | w_new, h_new = process_resize(w, h, resize) 269 | scales = (float(w) / float(w_new), float(h) / float(h_new)) 270 | 271 | if resize_float: 272 | image = cv2.resize(image.astype('float32'), (w_new, h_new)) 273 | else: 274 | image = cv2.resize(image, (w_new, h_new)).astype('float32') 275 | 276 | if rotation != 0: 277 | image = np.rot90(image, k=rotation) 278 | if rotation % 2: 279 | scales = scales[::-1] 280 | 281 | inp = frame2tensor(image, device) 282 | return image, inp, scales 283 | 284 | 285 | # --- GEOMETRY --- 286 | 287 | 288 | def estimate_pose(kpts0, kpts1, K0, K1, thresh, conf=0.99999): 289 | if len(kpts0) < 5: 290 | return None 291 | 292 | f_mean = np.mean([K0[0, 0], K1[1, 1], K0[0, 0], K1[1, 1]]) 293 | norm_thresh = thresh / f_mean 294 | 295 | kpts0 = (kpts0 - K0[[0, 1], [2, 2]][None]) / K0[[0, 1], [0, 1]][None] 296 | kpts1 = (kpts1 - K1[[0, 1], [2, 2]][None]) / K1[[0, 1], [0, 1]][None] 297 | 298 | E, mask = cv2.findEssentialMat( 299 | kpts0, kpts1, np.eye(3), threshold=norm_thresh, prob=conf, 300 | method=cv2.RANSAC) 301 | 302 | assert E is not None 303 | 304 | best_num_inliers = 0 305 | ret = None 306 | for _E in np.split(E, len(E) / 3): 307 | n, R, t, _ = cv2.recoverPose( 308 | _E, kpts0, kpts1, np.eye(3), 1e9, mask=mask) 309 | if n > best_num_inliers: 310 | best_num_inliers = n 311 | ret = (R, t[:, 0], mask.ravel() > 0) 312 | return ret 313 | 314 | 315 | def rotate_intrinsics(K, image_shape, rot): 316 | """image_shape is the shape of the image after rotation""" 317 | assert rot <= 3 318 | h, w = image_shape[:2][::-1 if (rot % 2) else 1] 319 | fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2] 320 | rot = rot % 4 321 | if rot == 1: 322 | return np.array([[fy, 0., cy], 323 | [0., fx, w-1-cx], 324 | [0., 0., 1.]], dtype=K.dtype) 325 | elif rot == 2: 326 | return np.array([[fx, 0., w-1-cx], 327 | [0., fy, h-1-cy], 328 | [0., 0., 1.]], dtype=K.dtype) 329 | else: # if rot == 3: 330 | return np.array([[fy, 0., h-1-cy], 331 | [0., fx, cx], 332 | [0., 0., 1.]], dtype=K.dtype) 333 | 334 | 335 | def rotate_pose_inplane(i_T_w, rot): 336 | rotation_matrices = [ 337 | np.array([[np.cos(r), -np.sin(r), 0., 0.], 338 | [np.sin(r), np.cos(r), 0., 0.], 339 | [0., 0., 1., 0.], 340 | [0., 0., 0., 1.]], dtype=np.float32) 341 | for r in [np.deg2rad(d) for d in (0, 270, 180, 90)] 342 | ] 343 | return np.dot(rotation_matrices[rot], i_T_w) 344 | 345 | 346 | def scale_intrinsics(K, scales): 347 | scales = np.diag([1./scales[0], 1./scales[1], 1.]) 348 | return np.dot(scales, K) 349 | 350 | 351 | def to_homogeneous(points): 352 | return np.concatenate([points, np.ones_like(points[:, :1])], axis=-1) 353 | 354 | 355 | def compute_epipolar_error(kpts0, kpts1, T_0to1, K0, K1): 356 | kpts0 = (kpts0 - K0[[0, 1], [2, 2]][None]) / K0[[0, 1], [0, 1]][None] 357 | kpts1 = (kpts1 - K1[[0, 1], [2, 2]][None]) / K1[[0, 1], [0, 1]][None] 358 | kpts0 = to_homogeneous(kpts0) 359 | kpts1 = to_homogeneous(kpts1) 360 | 361 | t0, t1, t2 = T_0to1[:3, 3] 362 | t_skew = np.array([ 363 | [0, -t2, t1], 364 | [t2, 0, -t0], 365 | [-t1, t0, 0] 366 | ]) 367 | E = t_skew @ T_0to1[:3, :3] 368 | 369 | Ep0 = kpts0 @ E.T # N x 3 370 | p1Ep0 = np.sum(kpts1 * Ep0, -1) # N 371 | Etp1 = kpts1 @ E # N x 3 372 | d = p1Ep0**2 * (1.0 / (Ep0[:, 0]**2 + Ep0[:, 1]**2) 373 | + 1.0 / (Etp1[:, 0]**2 + Etp1[:, 1]**2)) 374 | return d 375 | 376 | 377 | def angle_error_mat(R1, R2): 378 | cos = (np.trace(np.dot(R1.T, R2)) - 1) / 2 379 | cos = np.clip(cos, -1., 1.) # numercial errors can make it out of bounds 380 | return np.rad2deg(np.abs(np.arccos(cos))) 381 | 382 | 383 | def angle_error_vec(v1, v2): 384 | n = np.linalg.norm(v1) * np.linalg.norm(v2) 385 | return np.rad2deg(np.arccos(np.clip(np.dot(v1, v2) / n, -1.0, 1.0))) 386 | 387 | 388 | def compute_pose_error(T_0to1, R, t): 389 | R_gt = T_0to1[:3, :3] 390 | t_gt = T_0to1[:3, 3] 391 | error_t = angle_error_vec(t, t_gt) 392 | error_t = np.minimum(error_t, 180 - error_t) # ambiguity of E estimation 393 | error_R = angle_error_mat(R, R_gt) 394 | return error_t, error_R 395 | 396 | 397 | def pose_auc(errors, thresholds): 398 | sort_idx = np.argsort(errors) 399 | errors = np.array(errors.copy())[sort_idx] 400 | recall = (np.arange(len(errors)) + 1) / len(errors) 401 | errors = np.r_[0., errors] 402 | recall = np.r_[0., recall] 403 | aucs = [] 404 | for t in thresholds: 405 | last_index = np.searchsorted(errors, t) 406 | r = np.r_[recall[:last_index], recall[last_index-1]] 407 | e = np.r_[errors[:last_index], t] 408 | aucs.append(np.trapz(r, x=e)/t) 409 | return aucs 410 | 411 | 412 | # --- VISUALIZATION --- 413 | 414 | 415 | def plot_image_pair(imgs, dpi=100, size=6, pad=.5): 416 | n = len(imgs) 417 | assert n == 2, 'number of images must be two' 418 | figsize = (size*n, size*3/4) if size is not None else None 419 | _, ax = plt.subplots(1, n, figsize=figsize, dpi=dpi) 420 | for i in range(n): 421 | ax[i].imshow(imgs[i], cmap=plt.get_cmap('gray'), vmin=0, vmax=255) 422 | ax[i].get_yaxis().set_ticks([]) 423 | ax[i].get_xaxis().set_ticks([]) 424 | for spine in ax[i].spines.values(): # remove frame 425 | spine.set_visible(False) 426 | plt.tight_layout(pad=pad) 427 | 428 | 429 | def plot_keypoints(kpts0, kpts1, color='w', ps=2): 430 | ax = plt.gcf().axes 431 | ax[0].scatter(kpts0[:, 0], kpts0[:, 1], c=color, s=ps) 432 | ax[1].scatter(kpts1[:, 0], kpts1[:, 1], c=color, s=ps) 433 | 434 | 435 | def plot_matches(kpts0, kpts1, color, lw=1.5, ps=4): 436 | fig = plt.gcf() 437 | ax = fig.axes 438 | fig.canvas.draw() 439 | 440 | transFigure = fig.transFigure.inverted() 441 | fkpts0 = transFigure.transform(ax[0].transData.transform(kpts0)) 442 | fkpts1 = transFigure.transform(ax[1].transData.transform(kpts1)) 443 | 444 | fig.lines = [matplotlib.lines.Line2D( 445 | (fkpts0[i, 0], fkpts1[i, 0]), (fkpts0[i, 1], fkpts1[i, 1]), zorder=1, 446 | transform=fig.transFigure, c=color[i], linewidth=lw) 447 | for i in range(len(kpts0))] 448 | ax[0].scatter(kpts0[:, 0], kpts0[:, 1], c=color, s=ps) 449 | ax[1].scatter(kpts1[:, 0], kpts1[:, 1], c=color, s=ps) 450 | 451 | 452 | def make_matching_plot(image0, image1, kpts0, kpts1, mkpts0, mkpts1, 453 | color, text, path, show_keypoints=False, 454 | fast_viz=False, opencv_display=False, 455 | opencv_title='matches', small_text=[]): 456 | 457 | if fast_viz: 458 | make_matching_plot_fast(image0, image1, kpts0, kpts1, mkpts0, mkpts1, 459 | color, text, path, show_keypoints, 10, 460 | opencv_display, opencv_title, small_text) 461 | return 462 | 463 | plot_image_pair([image0, image1]) 464 | if show_keypoints: 465 | plot_keypoints(kpts0, kpts1, color='k', ps=4) 466 | plot_keypoints(kpts0, kpts1, color='w', ps=2) 467 | plot_matches(mkpts0, mkpts1, color) 468 | 469 | fig = plt.gcf() 470 | txt_color = 'k' if image0[:100, :150].mean() > 200 else 'w' 471 | fig.text( 472 | 0.01, 0.99, '\n'.join(text), transform=fig.axes[0].transAxes, 473 | fontsize=15, va='top', ha='left', color=txt_color) 474 | 475 | txt_color = 'k' if image0[-100:, :150].mean() > 200 else 'w' 476 | fig.text( 477 | 0.01, 0.01, '\n'.join(small_text), transform=fig.axes[0].transAxes, 478 | fontsize=5, va='bottom', ha='left', color=txt_color) 479 | 480 | plt.savefig(str(path), bbox_inches='tight', pad_inches=0) 481 | plt.close() 482 | 483 | 484 | def make_matching_plot_fast(image0, image1, kpts0, kpts1, mkpts0, 485 | mkpts1, color, text, path=None, 486 | show_keypoints=False, margin=10, 487 | opencv_display=False, opencv_title='', 488 | small_text=[]): 489 | H0, W0 = image0.shape 490 | H1, W1 = image1.shape 491 | H, W = max(H0, H1), W0 + W1 + margin 492 | 493 | out = 255*np.ones((H, W), np.uint8) 494 | out[:H0, :W0] = image0 495 | out[:H1, W0+margin:] = image1 496 | out = np.stack([out]*3, -1) 497 | 498 | if show_keypoints: 499 | kpts0, kpts1 = np.round(kpts0).astype(int), np.round(kpts1).astype(int) 500 | white = (255, 255, 255) 501 | black = (0, 0, 0) 502 | for x, y in kpts0: 503 | cv2.circle(out, (x, y), 2, black, -1, lineType=cv2.LINE_AA) 504 | cv2.circle(out, (x, y), 1, white, -1, lineType=cv2.LINE_AA) 505 | for x, y in kpts1: 506 | cv2.circle(out, (x + margin + W0, y), 2, black, -1, 507 | lineType=cv2.LINE_AA) 508 | cv2.circle(out, (x + margin + W0, y), 1, white, -1, 509 | lineType=cv2.LINE_AA) 510 | 511 | mkpts0, mkpts1 = np.round(mkpts0).astype(int), np.round(mkpts1).astype(int) 512 | color = (np.array(color[:, :3])*255).astype(int)[:, ::-1] 513 | for (x0, y0), (x1, y1), c in zip(mkpts0, mkpts1, color): 514 | c = c.tolist() 515 | cv2.line(out, (x0, y0), (x1 + margin + W0, y1), 516 | color=c, thickness=1, lineType=cv2.LINE_AA) 517 | # display line end-points as circles 518 | cv2.circle(out, (x0, y0), 2, c, -1, lineType=cv2.LINE_AA) 519 | cv2.circle(out, (x1 + margin + W0, y1), 2, c, -1, 520 | lineType=cv2.LINE_AA) 521 | 522 | # Scale factor for consistent visualization across scales. 523 | sc = min(H / 640., 2.0) 524 | 525 | # Big text. 526 | Ht = int(30 * sc) # text height 527 | txt_color_fg = (255, 255, 255) 528 | txt_color_bg = (0, 0, 0) 529 | for i, t in enumerate(text): 530 | cv2.putText(out, t, (int(8*sc), Ht*(i+1)), cv2.FONT_HERSHEY_DUPLEX, 531 | 1.0*sc, txt_color_bg, 2, cv2.LINE_AA) 532 | cv2.putText(out, t, (int(8*sc), Ht*(i+1)), cv2.FONT_HERSHEY_DUPLEX, 533 | 1.0*sc, txt_color_fg, 1, cv2.LINE_AA) 534 | 535 | # Small text. 536 | Ht = int(18 * sc) # text height 537 | for i, t in enumerate(reversed(small_text)): 538 | cv2.putText(out, t, (int(8*sc), int(H-Ht*(i+.6))), cv2.FONT_HERSHEY_DUPLEX, 539 | 0.5*sc, txt_color_bg, 2, cv2.LINE_AA) 540 | cv2.putText(out, t, (int(8*sc), int(H-Ht*(i+.6))), cv2.FONT_HERSHEY_DUPLEX, 541 | 0.5*sc, txt_color_fg, 1, cv2.LINE_AA) 542 | 543 | if path is not None: 544 | cv2.imwrite(str(path), out) 545 | 546 | if opencv_display: 547 | cv2.imshow(opencv_title, out) 548 | cv2.waitKey(1) 549 | 550 | return out 551 | 552 | 553 | def error_colormap(x): 554 | return np.clip( 555 | np.stack([2-x*2, x*2, np.zeros_like(x), np.ones_like(x)], -1), 0, 1) 556 | -------------------------------------------------------------------------------- /scripts/methods/superglue/weights/.gitignore: -------------------------------------------------------------------------------- 1 | * 2 | !.gitignore -------------------------------------------------------------------------------- /scripts/plot_pr_curve.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from sklearn.metrics import auc 4 | import sys 5 | import os 6 | 7 | method_name = sys.argv[1] # The method name 8 | result_path = sys.argv[2] # The path to the results file 9 | # Read precisions and recalls from the input arguments 10 | precisions = np.fromstring(sys.argv[3], sep=',') 11 | recalls = np.fromstring(sys.argv[4], sep=',') 12 | 13 | # Calculate AUPRC 14 | auprc = auc(recalls, precisions) 15 | print(f'AUPRC for {method_name}: {auprc}') 16 | 17 | # Create a new figure 18 | plt.figure() 19 | 20 | # Plot the precision-recall curve 21 | plt.plot(recalls, precisions, marker='.') 22 | 23 | # Set the labels and title 24 | plt.xlabel('Recall') 25 | plt.ylabel('Precision') 26 | plt.title('Precision-Recall Curve for ' + method_name) 27 | 28 | # Display the average precision on the plot 29 | plt.figtext(0.5, 0.5, 'AUPRC: {:.3f}'.format(auprc), ha='center', va='center') 30 | 31 | 32 | # Save the figure 33 | full_path = os.path.join(result_path, method_name + '_curve.png') 34 | plt.savefig(full_path) 35 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from setuptools import setup, find_packages 4 | 5 | PACKAGE_NAME = 'place_recog_eval' 6 | VERSION = '0.0.1' 7 | AUTHOR = 'Ivan Efremov' 8 | AUTHOR_EMAIL = 'ef.i.a@ya.ru' 9 | DESCRIPTION = 'Place recognition evaluation package' 10 | PACKAGES = find_packages() 11 | INSTALL_REQUIRES = [ 12 | 'rospy', 13 | 'numpy', 14 | 'cv_bridge', 15 | 'tqdm', 16 | 'torch' 17 | ] 18 | 19 | setup( 20 | name=PACKAGE_NAME, 21 | version=VERSION, 22 | author=AUTHOR, 23 | author_email=AUTHOR_EMAIL, 24 | description=DESCRIPTION, 25 | packages=PACKAGES, 26 | install_requires=INSTALL_REQUIRES 27 | ) 28 | -------------------------------------------------------------------------------- /src/dbow_create_vocab.cpp: -------------------------------------------------------------------------------- 1 | #include "methods/dbow.h" 2 | #include 3 | 4 | void imageCallback(const sensor_msgs::ImageConstPtr &msg) 5 | { 6 | static std::vector images; 7 | static int counter = 0; 8 | 9 | try 10 | { 11 | counter++; 12 | if (counter % 5 != 0) 13 | return; 14 | images.push_back(*msg); 15 | 16 | if (images.size() % 10 == 0) 17 | ROS_INFO("Received images: %lu", images.size()); 18 | 19 | if (images.size() >= 500) 20 | { 21 | std::string voc_name = "orb_vocab.yml.gz"; 22 | DBoW::createVocabulary(images, voc_name); 23 | 24 | // Shutdown the node after the vocabulary is created and saved 25 | ros::shutdown(); 26 | } 27 | } 28 | catch (cv_bridge::Exception &e) 29 | { 30 | ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); 31 | } 32 | } 33 | 34 | int main(int argc, char **argv) 35 | { 36 | ros::init(argc, argv, "image_listener"); 37 | ros::NodeHandle nh; 38 | 39 | image_transport::ImageTransport it(nh); 40 | image_transport::Subscriber sub = it.subscribe("/camera/color/image_raw_sync", 1, imageCallback); 41 | 42 | ros::spin(); 43 | 44 | return 0; 45 | } -------------------------------------------------------------------------------- /src/evaluate.cpp: -------------------------------------------------------------------------------- 1 | #include "methods/scan_context.h" 2 | #include "methods/dbow.h" 3 | #include "methods/STD.h" 4 | #include "evaluator.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | void getMethods(std::vector &methods, ros::NodeHandle &nh) 15 | { 16 | std::string method_name; 17 | 18 | if (nh.getParam("method", method_name)) 19 | { 20 | double threshold, leaf_size; 21 | nh.getParam("threshold", threshold); 22 | if (method_name == "dbow") 23 | methods.push_back(new DBoW(threshold)); 24 | else if (method_name == "context") 25 | { 26 | nh.getParam("leaf_size", leaf_size); 27 | methods.push_back(new ScanContext(threshold, leaf_size)); 28 | } 29 | else if (method_name == "std"){ 30 | ConfigSetting config_setting; 31 | read_parameters(nh, config_setting); 32 | methods.push_back(new STD(config_setting)); 33 | } 34 | else 35 | { 36 | ROS_ERROR("Invalid method parameter. Use 'dbow' or 'context'."); 37 | exit(-1); 38 | } 39 | } 40 | else 41 | { 42 | // BaseMethod *method1 = new DBoW(2.2); 43 | // methods.push_back(method1); 44 | 45 | // BaseMethod *method2 = new ScanContext(0.06, 0.2); 46 | // methods.push_back(method2); 47 | 48 | // Find best threshold for Scan Context 49 | for (double threshold = 0.025; threshold < 0.1; threshold += 0.01) 50 | { 51 | BaseMethod *method = new ScanContext(threshold, 0.2); 52 | methods.push_back(method); 53 | } 54 | 55 | // Find best threshold for DBoW 56 | // for (double threshold = 1.8; threshold <= 2.5; threshold += 0.1) 57 | // { 58 | // BaseMethod *method = new DBoW(threshold); 59 | // methods.push_back(method); 60 | // } 61 | 62 | // Find best threshold for STD 63 | // for (double threshold = 0.0; threshold <= 0.7; threshold += 0.1) 64 | // { 65 | // ConfigSetting config_setting; 66 | // read_parameters(nh, config_setting); 67 | // config_setting.icp_threshold_ = threshold; 68 | // methods.push_back(new STD(config_setting)); 69 | // } 70 | 71 | } 72 | } 73 | 74 | void processBag(Evaluator &evaluator, std::string input_bag_path, std::string lidar_topic, std::string odom_topic, std::string camera_topic) 75 | { 76 | rosbag::Bag bag; 77 | try 78 | { 79 | bag.open(input_bag_path, rosbag::bagmode::Read); 80 | } 81 | catch (rosbag::BagException &e) 82 | { 83 | ROS_ERROR("Error opening bag file: %s", e.what()); 84 | } 85 | 86 | // Specify topics to read from the rosbag 87 | std::vector topics = {lidar_topic, odom_topic, camera_topic}; 88 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 89 | 90 | sensor_msgs::PointCloud2::ConstPtr cloud_msg; 91 | nav_msgs::Odometry::ConstPtr odom_msg; 92 | sensor_msgs::Image::ConstPtr img_msg; 93 | 94 | for (const rosbag::MessageInstance &m : view) 95 | { 96 | if (m.getTopic() == lidar_topic) 97 | { 98 | cloud_msg = m.instantiate(); 99 | } 100 | else if (m.getTopic() == odom_topic) 101 | { 102 | odom_msg = m.instantiate(); 103 | } 104 | else if (m.getTopic() == camera_topic) 105 | { 106 | img_msg = m.instantiate(); 107 | } 108 | 109 | if (cloud_msg && odom_msg && img_msg) 110 | { 111 | evaluator.synced_callback(cloud_msg, odom_msg, img_msg); 112 | cloud_msg.reset(); 113 | odom_msg.reset(); 114 | img_msg.reset(); 115 | } 116 | } 117 | 118 | bag.close(); 119 | } 120 | 121 | int main(int argc, char **argv) 122 | { 123 | // Initialize ROS node 124 | ros::init(argc, argv, "evaluation_node"); 125 | ros::NodeHandle nh; 126 | 127 | // Retrieve parameters from the ROS parameter server 128 | std::string method_name; 129 | int record_size, keyframe_type, cloud_size, min_idx_diff, max_angle_deg; 130 | double max_dist, threshold; 131 | bool angle_consideration, save_candidates; 132 | 133 | // Merge params 134 | nh.getParam("keyframe_type", keyframe_type); 135 | nh.getParam("cloud_size", cloud_size); 136 | 137 | // Common params 138 | nh.getParam("record_size", record_size); 139 | nh.getParam("min_idx_diff", min_idx_diff); 140 | nh.getParam("angle_consideration", angle_consideration); 141 | nh.getParam("max_angle_deg", max_angle_deg); 142 | nh.getParam("max_dist", max_dist); 143 | nh.getParam("save_candidates", save_candidates); 144 | 145 | if (keyframe_type == 1) 146 | record_size /= cloud_size; 147 | 148 | // Get the method parameters 149 | std::vector methods; 150 | getMethods(methods, nh); 151 | 152 | // Create the evaluation object based on the selected methods 153 | Evaluator evaluator(methods, record_size, angle_consideration, max_angle_deg, max_dist, min_idx_diff, save_candidates); 154 | 155 | // Get merged topic names 156 | std::string lidar_topic, odom_topic, camera_topic; 157 | nh.getParam("merged_lidar_topic", lidar_topic); 158 | nh.getParam("merged_odom_topic", odom_topic); 159 | nh.getParam("merged_camera_topic", camera_topic); 160 | 161 | // Get rosbag params 162 | bool use_rosbag; 163 | std::string input_bag_path; 164 | nh.getParam("use_rosbag", use_rosbag); 165 | nh.getParam("merge_rosbag_output_path", input_bag_path); 166 | 167 | if (use_rosbag) 168 | { 169 | processBag(evaluator, input_bag_path, lidar_topic, odom_topic, camera_topic); 170 | } 171 | else 172 | { 173 | message_filters::Subscriber cloud_sub(nh, lidar_topic, 20); 174 | message_filters::Subscriber odom_sub(nh, odom_topic, 100); 175 | message_filters::Subscriber img_sub(nh, camera_topic, 20); 176 | 177 | // Synchronize messages from different topics 178 | message_filters::TimeSynchronizer sync(cloud_sub, odom_sub, img_sub, 100); 179 | 180 | // Register the synchronized callback 181 | sync.registerCallback(boost::bind(&Evaluator::synced_callback, &evaluator, _1, _2, _3)); 182 | 183 | // Process callbacks and wait for new messages 184 | ros::spin(); 185 | } 186 | 187 | return 0; 188 | } 189 | -------------------------------------------------------------------------------- /src/evaluator.cpp: -------------------------------------------------------------------------------- 1 | #include "evaluator.h" 2 | 3 | #include 4 | #include "ros/package.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | /** 20 | * \brief Calculate the angular difference between two quaternions 21 | * 22 | * \param quat_1 The first quaternion 23 | * \param quat_2 The second quaternion 24 | * 25 | * \return The angular difference between quat_1 and quat_2 26 | */ 27 | static double angular_difference(const geometry_msgs::Quaternion &quat_1, const geometry_msgs::Quaternion &quat_2) 28 | { 29 | const double cos_angle = quat_1.x * quat_2.x + quat_1.y * quat_2.y + quat_1.z * quat_2.z + quat_1.w * quat_2.w; 30 | const double angle = 2 * acos(abs(cos_angle)); // use absolute value to avoid NaNs due to rounding errors 31 | return angle; 32 | } 33 | 34 | /** 35 | * \brief Calculate the Euclidean distance between two points 36 | * 37 | * \param point_1 The first point 38 | * \param point_2 The second point 39 | * 40 | * \return The Euclidean distance between point_1 and point_2 41 | */ 42 | static double euclidean_difference(const geometry_msgs::Point &point_1, const geometry_msgs::Point &point_2) 43 | { 44 | const double dx = point_1.x - point_2.x; 45 | const double dy = point_1.y - point_2.y; 46 | const double dz = point_1.z - point_2.z; 47 | const double dist = sqrt(dx * dx + dy * dy + dz * dz); 48 | return dist; 49 | } 50 | 51 | Evaluator::Evaluator(const std::vector &methods, int record_size, bool angle_consideration, int max_angle_deg, double max_dist, int min_idx_diff, bool save_candidates) 52 | : methods(methods), RECORD_SIZE(record_size), ANGLE_CONSIDERATION(angle_consideration), 53 | MAX_ANGLE_DEG(max_angle_deg), MAX_DIST(max_dist), MIN_IDX_DIFF(min_idx_diff), MAX_ANGLE_RAD(((double)max_angle_deg / 180) * 3.14), SAVE_CANDIDATES(save_candidates) 54 | { 55 | real_loop_candidates.resize(RECORD_SIZE, std::vector(RECORD_SIZE, false)); 56 | } 57 | 58 | void Evaluator::synced_callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, 59 | const nav_msgs::Odometry::ConstPtr &odom_msg, 60 | const boost::optional &img_msg) 61 | { 62 | static int counter = 0; 63 | static int log_frequency = 50; 64 | 65 | if (counter % log_frequency == 0) 66 | { 67 | ROS_INFO("Amount of data recorded: %d", counter); 68 | } 69 | 70 | // Store data to lists 71 | cloud_vec.push_back(*cloud_msg); 72 | odom_vec.push_back(odom_msg->pose.pose); 73 | if (img_msg) 74 | { 75 | img_vec.push_back(**img_msg); 76 | } 77 | 78 | counter++; 79 | 80 | if (counter == RECORD_SIZE) 81 | { 82 | ROS_INFO("Starting evaluation..."); 83 | 84 | get_real_loop_candidates(); 85 | evaluate_models(); 86 | 87 | plot_odometry_path(odom_vec); 88 | ros::shutdown(); 89 | } 90 | } 91 | 92 | static std::string getFirstWord(const std::string &input) 93 | { 94 | // Find the first space character 95 | size_t firstSpace = input.find_first_of(' '); 96 | 97 | // If there is no space character, the entire string is considered one word 98 | if (firstSpace == std::string::npos) 99 | { 100 | return input; 101 | } 102 | 103 | // Get the substring from the start of the string to the first space character 104 | return input.substr(0, firstSpace); 105 | } 106 | 107 | void Evaluator::save_candidates(std::vector> &candidates, const std::string &filename) 108 | { 109 | if (SAVE_CANDIDATES) 110 | { 111 | std::ofstream file; 112 | std::string path = ros::package::getPath("place_recog_eval") + "/results/"; 113 | 114 | if (ANGLE_CONSIDERATION) 115 | { 116 | path += "with_angle/"; 117 | } 118 | else 119 | { 120 | path += "no_angle/"; 121 | } 122 | 123 | // Check if the directory exists and create it if it doesn't 124 | if (!boost::filesystem::exists(path)) 125 | { 126 | boost::filesystem::create_directories(path); 127 | } 128 | path += filename; 129 | 130 | file.open(path); 131 | 132 | for (int i = 1; i < RECORD_SIZE; i++) 133 | { 134 | for (int j = 0; j <= i - MIN_IDX_DIFF; j++) 135 | { 136 | file << candidates[i][j] << " "; 137 | } 138 | file << "\n"; 139 | } 140 | } 141 | } 142 | 143 | void Evaluator::get_real_loop_candidates() 144 | { 145 | // Calculate real loop candidates 146 | for (int i = 1; i < RECORD_SIZE; i++) 147 | { 148 | for (int j = 0; j <= i - MIN_IDX_DIFF; j++) 149 | { 150 | double odom_diff = euclidean_difference(odom_vec[i].position, odom_vec[j].position); 151 | bool condition = odom_diff < MAX_DIST; 152 | 153 | if (ANGLE_CONSIDERATION) 154 | { 155 | double ang_diff = angular_difference(odom_vec[i].orientation, odom_vec[j].orientation); 156 | condition = condition && (ang_diff < MAX_ANGLE_RAD); 157 | } 158 | 159 | if (condition) 160 | real_loop_candidates[i][j] = true; 161 | } 162 | } 163 | 164 | // Save real loop candidates to file 165 | std::string method_name = getFirstWord(methods[0]->getName()); 166 | save_candidates(real_loop_candidates, "real_" + method_name + ".txt"); 167 | } 168 | 169 | std::string remove_parentheses_text(const std::string &str) 170 | { 171 | std::size_t pos = str.find(" ("); 172 | if (pos != std::string::npos) 173 | { 174 | return str.substr(0, pos); 175 | } 176 | else 177 | { 178 | return str; 179 | } 180 | } 181 | 182 | void Evaluator::evaluate_models() 183 | { 184 | double best_f1_score = 0; 185 | BaseMethod *best_method = nullptr; 186 | 187 | // Declare vectors to store the precision and recall values 188 | std::vector precisions; 189 | std::vector recalls; 190 | 191 | for (BaseMethod *method : methods) 192 | { 193 | // Predict loop candidates 194 | std::vector> predicted_loop_candidates(RECORD_SIZE, std::vector(RECORD_SIZE, false)); 195 | 196 | auto start = std::chrono::high_resolution_clock::now(); 197 | method->predict_loop_candidates(predicted_loop_candidates, cloud_vec, odom_vec, img_vec, MIN_IDX_DIFF); 198 | auto duration = std::chrono::high_resolution_clock::now() - start; 199 | 200 | ROS_INFO("Results for method: %s", method->getName().c_str()); 201 | ROS_INFO("It took %f seconds to predict loop candidates (%lu frames total).", 202 | std::chrono::duration(duration).count(), cloud_vec.size()); 203 | 204 | ROS_INFO("It took %f seconds to predict one loop candidate.", 205 | std::chrono::duration(duration).count() / cloud_vec.size()); 206 | 207 | // Calculate metrics 208 | double precision, recall, f1_score, accuracy; 209 | calculate_metrics(predicted_loop_candidates, precision, recall, f1_score, accuracy); 210 | 211 | // Save precision and recall 212 | precisions.push_back(precision); 213 | recalls.push_back(recall); 214 | 215 | // Display results 216 | ROS_INFO("----------------------------------------------"); 217 | ROS_INFO("Precision: %.3f", precision); 218 | ROS_INFO("Recall : %.3f", recall); 219 | ROS_INFO("F1 Score : %.3f", f1_score); 220 | ROS_INFO("Accuracy : %.3f", accuracy); 221 | ROS_INFO("----------------------------------------------"); 222 | ROS_INFO("----------------------------------------------"); 223 | ROS_INFO(" "); 224 | 225 | if (f1_score > best_f1_score) 226 | { 227 | best_f1_score = f1_score; 228 | best_method = method; 229 | } 230 | 231 | // Save predicted loop candidates to file 232 | save_candidates(predicted_loop_candidates, method->getName() + ".txt"); 233 | } 234 | 235 | ROS_INFO("Best method: %s with f1 score: %.3f", best_method->getName().c_str(), best_f1_score); 236 | 237 | if (methods.size() > 1) 238 | { 239 | // Generate Precision-Recall Curve 240 | generate_pr_curve(precisions, recalls, remove_parentheses_text(best_method->getName())); 241 | } 242 | } 243 | 244 | void Evaluator::generate_pr_curve(const std::vector &precisions, const std::vector &recalls, const std::string &method_name) 245 | { 246 | // Convert the vectors to strings 247 | std::ostringstream precisions_stream, recalls_stream; 248 | std::copy(precisions.begin(), precisions.end(), std::ostream_iterator(precisions_stream, ",")); 249 | std::copy(recalls.begin(), recalls.end(), std::ostream_iterator(recalls_stream, ",")); 250 | 251 | // Get the path to the Python script 252 | std::string script_path = ros::package::getPath("place_recog_eval") + "/scripts/plot_pr_curve.py"; 253 | std::string results_path = ros::package::getPath("place_recog_eval") + "/results/"; 254 | if (ANGLE_CONSIDERATION) 255 | results_path += "with_angle"; 256 | else 257 | results_path += "no_angle"; 258 | 259 | // Build the command to run the Python script 260 | std::string command = "python " + script_path + " " + method_name + " " + results_path + 261 | " " + precisions_stream.str() + " " + recalls_stream.str(); 262 | 263 | // Execute the command 264 | system(command.c_str()); 265 | } 266 | 267 | void Evaluator::calculate_metrics(std::vector> &predicted_loop_candidates, double &precision, double &recall, double &f1_score, double &accuracy) 268 | { 269 | int tp = 0, fp = 0, fn = 0, tn = 0; 270 | 271 | for (int i = 1; i < RECORD_SIZE; i++) 272 | { 273 | for (int j = 0; j <= i - MIN_IDX_DIFF; j++) 274 | { 275 | bool predicted = predicted_loop_candidates[i][j]; 276 | bool real = real_loop_candidates[i][j]; 277 | 278 | if (predicted && real) 279 | tp++; 280 | else if (predicted && !real) 281 | fp++; 282 | else if (!predicted && real) 283 | fn++; 284 | else if (!predicted && !real) 285 | tn++; 286 | } 287 | } 288 | precision = (tp + fp) > 0 ? static_cast(tp) / (tp + fp) : 0; 289 | recall = (tp + fn) > 0 ? static_cast(tp) / (tp + fn) : 0; 290 | f1_score = (precision + recall) > 0 ? 2 * precision * recall / (precision + recall) : 0; 291 | accuracy = (tp + tn + fp + fn) > 0 ? static_cast(tp + tn) / (tp + tn + fp + fn) : 0; 292 | } 293 | 294 | void Evaluator::plot_odometry_path(const std::vector &odom_vec) 295 | { 296 | // Create a blank image 297 | int img_size = 512; 298 | cv::Mat img(img_size, img_size, CV_8UC3, cv::Scalar(255, 255, 255)); 299 | 300 | // Find min and max coordinates of the path 301 | double min_x = std::numeric_limits::max(); 302 | double min_y = std::numeric_limits::max(); 303 | double max_x = std::numeric_limits::min(); 304 | double max_y = std::numeric_limits::min(); 305 | 306 | for (const auto &pose : odom_vec) 307 | { 308 | min_x = std::min(min_x, pose.position.x); 309 | min_y = std::min(min_y, pose.position.y); 310 | max_x = std::max(max_x, pose.position.x); 311 | max_y = std::max(max_y, pose.position.y); 312 | } 313 | 314 | // Calculate scale and offsets 315 | double scale = std::min(img_size / (max_x - min_x), img_size / (max_y - min_y)); 316 | double x_offset = (img_size - (max_x - min_x) * scale) / 2.0 - min_x * scale; 317 | double y_offset = (img_size - (max_y - min_y) * scale) / 2.0 - min_y * scale; 318 | 319 | // Plot each point 320 | cv::Point2i prev_point; 321 | bool prev_point_valid = false; 322 | for (const auto &pose : odom_vec) 323 | { 324 | cv::Point2i point( 325 | static_cast(pose.position.x * scale + x_offset), 326 | static_cast(img_size - (pose.position.y * scale + y_offset))); 327 | 328 | cv::circle(img, point, 2, cv::Scalar(0, 0, 255), -1); 329 | 330 | // Draw a line between the previous point and the current point, if the previous point is valid 331 | if (prev_point_valid) 332 | { 333 | cv::line(img, prev_point, point, cv::Scalar(255, 0, 0), 1); 334 | } 335 | 336 | // Update the previous point and set it as valid 337 | prev_point = point; 338 | prev_point_valid = true; 339 | } 340 | 341 | // Save the image to a file 342 | std::string path = ros::package::getPath("place_recog_eval"); 343 | std::string filename = "odometry_path.png"; 344 | cv::imwrite(path + "/" + filename, img); 345 | } -------------------------------------------------------------------------------- /src/merge_messages.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "merge_messages.h" 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | static Eigen::Matrix quat2rot(const geometry_msgs::Quaternion &quat) 17 | { 18 | tf::Quaternion q( 19 | quat.x, 20 | quat.y, 21 | quat.z, 22 | quat.w); 23 | 24 | tf::Matrix3x3 m(q); 25 | 26 | Eigen::Matrix rot; 27 | 28 | rot << m[0][0], m[0][1], m[0][2], 29 | m[1][0], m[1][1], m[1][2], 30 | m[2][0], m[2][1], m[2][2]; 31 | 32 | return rot; 33 | } 34 | 35 | static Eigen::Matrix quatpos2mat(const geometry_msgs::Quaternion &quat, const geometry_msgs::Point &pos) 36 | { 37 | Eigen::Matrix rot = quat2rot(quat); 38 | 39 | Eigen::Matrix mat = Eigen::Matrix4d::Identity(); 40 | 41 | for (size_t i = 0; i < rot.rows(); i++) 42 | { 43 | for (size_t j = 0; j < rot.cols(); j++) 44 | { 45 | mat(i, j) = rot(i, j); 46 | } 47 | } 48 | mat(0, 3) = pos.x; 49 | mat(1, 3) = pos.y; 50 | mat(2, 3) = pos.z; 51 | 52 | return mat; 53 | } 54 | 55 | MergeRepublisher::MergeRepublisher(bool use_rosbag, std::string input_bag_path, std::string output_bag_path, 56 | std::string &merged_lidar_topic, std::string &merged_odom_topic, std::string &merged_cam_topic, 57 | int keyframe_type, int cloud_size, double leaf_size) : USE_ROSBAG(use_rosbag), merged_lidar_topic(merged_lidar_topic), 58 | merged_odom_topic(merged_odom_topic), merged_cam_topic(merged_cam_topic), 59 | KEYFRAME_TYPE(keyframe_type), CLOUD_SIZE(cloud_size), LEAF_SIZE(leaf_size) 60 | { 61 | if (USE_ROSBAG) 62 | { 63 | try 64 | { 65 | input_bag.open(input_bag_path, rosbag::bagmode::Read); 66 | output_bag.open(output_bag_path, rosbag::bagmode::Write); 67 | } 68 | catch (rosbag::BagException &e) 69 | { 70 | ROS_ERROR("Error opening bag file: %s", e.what()); 71 | } 72 | } 73 | else 74 | { 75 | merged_cloud_publisher = nh_.advertise(merged_lidar_topic, 20); 76 | merged_odom_publisher = nh_.advertise(merged_odom_topic, 20); 77 | merged_img_publisher = nh_.advertise(merged_cam_topic, 20); 78 | } 79 | 80 | downsampler.setLeafSize(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE); 81 | } 82 | 83 | static int msg_count = 0; 84 | void MergeRepublisher::callback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, 85 | const nav_msgs::Odometry::ConstPtr &odom_msg, 86 | const boost::optional &img_msg) 87 | { 88 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 89 | pcl::fromROSMsg(*cloud_msg, *cloud); 90 | 91 | cloud_vec.push_back(cloud); 92 | odom_vec.push_back(odom_msg->pose.pose); 93 | 94 | if (img_msg) 95 | img_vec.push_back(**img_msg); 96 | 97 | if (cloud_vec.size() >= CLOUD_SIZE) 98 | { 99 | // Publish data 100 | sensor_msgs::PointCloud2 cloud_msg; 101 | nav_msgs::Odometry odom_msg; 102 | sensor_msgs::Image img_msg; 103 | 104 | pcl::PointCloud cloud_total = merge_cloud(); 105 | 106 | pcl::toROSMsg(cloud_total, cloud_msg); 107 | cloud_msg.header.frame_id = "trunk"; 108 | 109 | odom_msg.pose.pose = odom_vec[0]; 110 | odom_msg.header.stamp = cloud_msg.header.stamp; 111 | odom_msg.header.frame_id = cloud_msg.header.frame_id; 112 | 113 | if (!img_vec.empty()) 114 | { 115 | img_msg = img_vec[0]; 116 | img_msg.header.stamp = cloud_msg.header.stamp; 117 | img_msg.header.frame_id = cloud_msg.header.frame_id; 118 | } 119 | 120 | if (USE_ROSBAG) 121 | { 122 | output_bag.write(merged_lidar_topic, cloud_msg.header.stamp, cloud_msg); 123 | output_bag.write(merged_odom_topic, cloud_msg.header.stamp, odom_msg); 124 | output_bag.write(merged_cam_topic, cloud_msg.header.stamp, img_msg); 125 | } 126 | else 127 | { 128 | merged_cloud_publisher.publish(cloud_msg); 129 | merged_odom_publisher.publish(odom_msg); 130 | if (!img_vec.empty()) 131 | merged_img_publisher.publish(img_msg); 132 | } 133 | 134 | // Number of messages published or written to bag 135 | if (++msg_count % 50 == 0) 136 | ROS_INFO("Published/written %d messages", msg_count); 137 | 138 | // Update buffers 139 | size_t erase_count = (KEYFRAME_TYPE == 0) ? 1 : CLOUD_SIZE; 140 | cloud_vec.erase(cloud_vec.begin(), cloud_vec.begin() + erase_count); 141 | odom_vec.erase(odom_vec.begin(), odom_vec.begin() + erase_count); 142 | if (!img_vec.empty()) 143 | img_vec.erase(img_vec.begin(), img_vec.begin() + erase_count); 144 | } 145 | } 146 | 147 | // Merges point clouds in the cloud_vec and applies a voxel grid downsampling filter. 148 | // Transforms all clouds into local frame 149 | pcl::PointCloud MergeRepublisher::merge_cloud() 150 | { 151 | pcl::PointCloud cloud_total; 152 | 153 | // Get the SE3 matrix for the first cloud 154 | Eigen::Matrix tf_mat1 = quatpos2mat(odom_vec[0].orientation, odom_vec[0].position); 155 | 156 | // Add the first cloud to the resulting cloud 157 | cloud_total += *cloud_vec[0]; 158 | 159 | // Iterate through the remaining clouds 160 | for (int i = 1; i < cloud_vec.size(); i++) 161 | { 162 | // Get the SE3 matrix for the current cloud 163 | Eigen::Matrix tf_mat2 = quatpos2mat(odom_vec[i].orientation, odom_vec[i].position); 164 | 165 | // Compute the relative transformation between the first cloud and the current cloud 166 | Eigen::Matrix tf_mat12 = tf_mat1.inverse() * tf_mat2; 167 | 168 | // Create a transformation matrix from the relative transformation 169 | Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); 170 | transform.block(0, 0, 3, 3) = tf_mat12.block(0, 0, 3, 3); 171 | transform.block(0, 3, 3, 1) = tf_mat12.block(0, 3, 3, 1); 172 | 173 | // Transform the current cloud into the first cloud's frame 174 | pcl::PointCloud::Ptr cloud_transformed(new pcl::PointCloud()); 175 | pcl::transformPointCloud(*cloud_vec[i], *cloud_transformed, transform); 176 | 177 | // Add the transformed cloud to the resulting cloud 178 | cloud_total += *cloud_transformed; 179 | } 180 | 181 | // Apply the voxel grid downsampling filter to the merged cloud 182 | downsampler.setInputCloud(cloud_total.makeShared()); 183 | downsampler.filter(cloud_total); 184 | 185 | return cloud_total; 186 | } 187 | 188 | void MergeRepublisher::processBag(std::string lidar_topic, std::string odom_topic, std::string cam_topic) 189 | { 190 | // Specify topics to read from the rosbag 191 | std::vector topics = {lidar_topic, odom_topic, cam_topic}; 192 | rosbag::View view(input_bag, rosbag::TopicQuery(topics)); 193 | 194 | sensor_msgs::PointCloud2::ConstPtr cloud_msg; 195 | nav_msgs::Odometry::ConstPtr odom_msg; 196 | sensor_msgs::Image::ConstPtr img_msg; 197 | 198 | std::multimap odom_map; 199 | std::multimap img_map; 200 | 201 | ros::Duration max_time_diff(0.1); // Adjust this value as needed 202 | int count = 0; 203 | 204 | for (const rosbag::MessageInstance &m : view) 205 | { 206 | if (m.getTopic() == lidar_topic) 207 | { 208 | cloud_msg = m.instantiate(); 209 | 210 | // Find an odom_msg and img_msg that are close in time to the cloud_msg 211 | nav_msgs::Odometry::ConstPtr odom_msg; 212 | sensor_msgs::Image::ConstPtr img_msg; 213 | 214 | auto odom_iter = odom_map.lower_bound(cloud_msg->header.stamp - max_time_diff); 215 | if (odom_iter != odom_map.end() && (odom_iter->first - cloud_msg->header.stamp) <= max_time_diff) 216 | { 217 | odom_msg = odom_iter->second; 218 | odom_map.erase(odom_iter); 219 | } 220 | 221 | auto img_iter = img_map.lower_bound(cloud_msg->header.stamp - max_time_diff); 222 | if (img_iter != img_map.end() && (img_iter->first - cloud_msg->header.stamp) <= max_time_diff) 223 | { 224 | img_msg = img_iter->second; 225 | img_map.erase(img_iter); 226 | } 227 | 228 | if (odom_msg && img_msg) 229 | { 230 | count++; 231 | callback(cloud_msg, odom_msg, img_msg); 232 | } 233 | } 234 | else if (m.getTopic() == odom_topic) 235 | { 236 | odom_msg = m.instantiate(); 237 | odom_map.insert({odom_msg->header.stamp, odom_msg}); 238 | } 239 | else if (m.getTopic() == cam_topic) 240 | { 241 | img_msg = m.instantiate(); 242 | img_map.insert({img_msg->header.stamp, img_msg}); 243 | } 244 | } 245 | 246 | ROS_INFO("Processed %d messages", count); 247 | ROS_INFO("Number of messages written to output bag: %d", msg_count); 248 | 249 | input_bag.close(); 250 | output_bag.close(); 251 | } 252 | 253 | int main(int argc, char **argv) 254 | { 255 | ros::init(argc, argv, "merge_republisher"); 256 | 257 | ros::NodeHandle nh; 258 | 259 | std::string lidar_topic, odom_topic, cam_topic; 260 | nh.getParam("lidar_raw_topic", lidar_topic); 261 | nh.getParam("odom_raw_topic", odom_topic); 262 | nh.getParam("img_raw_topic", cam_topic); 263 | 264 | std::string merged_lidar_topic, merged_odom_topic, merged_cam_topic; 265 | nh.getParam("merged_lidar_topic", merged_lidar_topic); 266 | nh.getParam("merged_odom_topic", merged_odom_topic); 267 | nh.getParam("merged_camera_topic", merged_cam_topic); 268 | 269 | int keyframe_type; 270 | nh.getParam("keyframe_type", keyframe_type); 271 | 272 | int cloud_size; 273 | nh.getParam("cloud_size", cloud_size); 274 | 275 | double leaf_size; 276 | nh.getParam("merge_leaf_size", leaf_size); 277 | 278 | bool use_rosbag; 279 | nh.getParam("use_rosbag", use_rosbag); 280 | 281 | std::string rosbag_input_path; 282 | nh.getParam("merge_rosbag_input_path", rosbag_input_path); 283 | 284 | std::string rosbag_output_path; 285 | nh.getParam("merge_rosbag_output_path", rosbag_output_path); 286 | 287 | MergeRepublisher merge_republisher(use_rosbag, rosbag_input_path, rosbag_output_path, 288 | merged_lidar_topic, merged_odom_topic, merged_cam_topic, 289 | keyframe_type, cloud_size, leaf_size); 290 | 291 | if (use_rosbag) 292 | { 293 | merge_republisher.processBag(lidar_topic, odom_topic, cam_topic); 294 | ROS_INFO("Rosbag saved to %s", rosbag_output_path.c_str()); 295 | } 296 | else 297 | { 298 | message_filters::Subscriber cloud_sub(nh, lidar_topic, 1000); 299 | message_filters::Subscriber odom_sub(nh, odom_topic, 2000); 300 | message_filters::Subscriber img_sub(nh, cam_topic, 1000); 301 | 302 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 303 | message_filters::Synchronizer sync(MySyncPolicy(100), cloud_sub, odom_sub, img_sub); 304 | sync.registerCallback(boost::bind(&MergeRepublisher::callback, &merge_republisher, _1, _2, _3)); 305 | 306 | ros::spin(); 307 | } 308 | 309 | return 0; 310 | } -------------------------------------------------------------------------------- /src/methods/STD.cpp: -------------------------------------------------------------------------------- 1 | #include "methods/STD.h" 2 | #include 3 | 4 | STD::STD(ConfigSetting &config_setting) : config_setting(config_setting) 5 | { 6 | } 7 | 8 | const std::string STD::getName() const 9 | { 10 | const std::string name = "STD (threshold " + std::to_string(config_setting.icp_threshold_) + ")"; 11 | return name; 12 | } 13 | 14 | void STD::predict_loop_candidates(std::vector> &predicted, 15 | std::vector &cloud_vec, 16 | std::vector &odom_vec, 17 | std::vector &img_vec, 18 | int min_idx_diff) 19 | { 20 | 21 | STDescManager *std_manager = new STDescManager(config_setting); 22 | for (int i = 0; i < cloud_vec.size(); ++i) 23 | { 24 | std_manager->config_setting_.candidate_num_ = i - min_idx_diff + 1; 25 | 26 | pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); 27 | pcl::fromROSMsg(cloud_vec[i], *temp_cloud); 28 | 29 | std::vector stds_vec; 30 | std_manager->GenerateSTDescs(temp_cloud, stds_vec); 31 | 32 | std::vector candidate_matcher_vec; 33 | std_manager->candidate_selector(stds_vec, candidate_matcher_vec); 34 | 35 | for (size_t j = 0; j < candidate_matcher_vec.size(); j++) 36 | { 37 | double verify_score = -1; 38 | std::pair relative_pose; 39 | std::vector> sucess_match_vec; 40 | std_manager->candidate_verify(candidate_matcher_vec[j], verify_score, relative_pose, 41 | sucess_match_vec); 42 | if (verify_score > std_manager->config_setting_.icp_threshold_) 43 | { 44 | int candidate_id = candidate_matcher_vec[j].match_id_.second; 45 | predicted[i][candidate_id] = true; 46 | } 47 | } 48 | 49 | std_manager->AddSTDescs(stds_vec); 50 | } 51 | delete std_manager; 52 | } -------------------------------------------------------------------------------- /src/methods/dbow.cpp: -------------------------------------------------------------------------------- 1 | #include "methods/dbow.h" 2 | #include 3 | #include "ros/package.h" 4 | 5 | DBoW::DBoW(double threshold) : THRESHOLD(threshold) 6 | { 7 | orb = cv::ORB::create(); 8 | 9 | std::string path = ros::package::getPath("place_recog_eval"); 10 | std::string voc_path = path + "/include/methods/dbow/orb_vocab.yml.gz"; 11 | 12 | orb_vocab.load(voc_path); 13 | } 14 | 15 | const std::string DBoW::getName() const 16 | { 17 | const std::string name = "DBoW (threshold " + std::to_string(THRESHOLD) + ")"; 18 | return name; 19 | } 20 | 21 | void DBoW::predict_loop_candidates(std::vector> &predicted, 22 | std::vector &cloud_vec, 23 | std::vector &odom_vec, 24 | std::vector &img_vec, 25 | int min_idx_diff) 26 | { 27 | std::vector desc_vec; 28 | 29 | // Create a DBoW2 scorer for L1 distance 30 | DBoW2::L1Scoring scorer; 31 | 32 | // Extract features from all prerecorded images 33 | for (int i = 0; i < img_vec.size(); ++i) 34 | { 35 | // Convert the image message to a CvImagePtr 36 | cv_ptr = cv_bridge::toCvCopy(img_vec[i], sensor_msgs::image_encodings::MONO8); 37 | 38 | // Detect keypoints and compute ORB descriptors 39 | std::vector keypoints; 40 | cv::Mat descriptors; 41 | cv::Mat mask; 42 | orb->detectAndCompute(cv_ptr->image, mask, keypoints, descriptors); 43 | 44 | // Convert descriptors to a vector of Mats 45 | std::vector features_curr; 46 | for (size_t i = 0; i < descriptors.rows; i++) 47 | { 48 | features_curr.push_back(descriptors.row(i)); 49 | } 50 | 51 | // Transform the features using the ORB vocabulary 52 | DBoW2::BowVector vec_curr; 53 | orb_vocab.transform(features_curr, vec_curr); 54 | 55 | // Add the transformed feature vector to the descriptor vector 56 | desc_vec.push_back(vec_curr); 57 | 58 | for (int j = 0; j <= i - min_idx_diff; ++j) 59 | { 60 | double dist = 1 / (scorer.score(desc_vec[i], desc_vec[j])); 61 | if (dist < THRESHOLD) 62 | predicted[i][j] = true; 63 | } 64 | } 65 | } 66 | 67 | void DBoW::createVocabulary(const std::vector &img_vec, std::string &voc_name) 68 | { 69 | cv::Ptr orb = cv::ORB::create(); 70 | 71 | std::string path = ros::package::getPath("place_recog_eval"); 72 | std::string voc_path = path + "/include/methods/dbow/" + voc_name; 73 | 74 | // Extract features from all images in img_vec and store them in a vector> 75 | std::vector> features; 76 | features.reserve(img_vec.size()); 77 | 78 | for (const auto &img_msg : img_vec) 79 | { 80 | // Convert the image message to a CvImagePtr 81 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); 82 | 83 | // Detect keypoints and compute ORB descriptors 84 | std::vector keypoints; 85 | cv::Mat descriptors; 86 | cv::Mat mask; 87 | orb->detectAndCompute(cv_ptr->image, mask, keypoints, descriptors); 88 | 89 | // Convert descriptors to a vector of Mats 90 | std::vector features_curr; 91 | for (size_t i = 0; i < descriptors.rows; i++) 92 | { 93 | features_curr.push_back(descriptors.row(i)); 94 | } 95 | 96 | // Add the features to the main features vector 97 | features.push_back(features_curr); 98 | } 99 | 100 | // Create the vocabulary 101 | const int k = 9; 102 | const int L = 3; 103 | const DBoW2::WeightingType weight = DBoW2::WeightingType::TF_IDF; 104 | const DBoW2::ScoringType scoring = DBoW2::ScoringType::L1_NORM; 105 | OrbVocabulary voc = OrbVocabulary(k, L, weight, scoring); 106 | voc.create(features); 107 | 108 | // Save the vocabulary to a file 109 | voc.save(voc_path); 110 | } 111 | 112 | -------------------------------------------------------------------------------- /src/methods/scan_context.cpp: -------------------------------------------------------------------------------- 1 | #include "methods/scan_context.h" 2 | 3 | ScanContext::ScanContext(double threshold, double leaf_size) 4 | : THRESHOLD(threshold), LEAF_SIZE(leaf_size) 5 | { 6 | // Set the leaf size for the voxel grid filter 7 | voxel_grid.setLeafSize(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE); 8 | } 9 | 10 | const std::string ScanContext::getName() const 11 | { 12 | const std::string name = "ScanContext (threshold " + std::to_string(THRESHOLD) + ", leaf size " + std::to_string(LEAF_SIZE) + ")"; 13 | return name; 14 | } 15 | 16 | void ScanContext::predict_loop_candidates(std::vector> &predicted, 17 | std::vector &cloud_vec, 18 | std::vector &odom_vec, 19 | std::vector &img_vec, 20 | int min_idx_diff) 21 | { 22 | // Create a new point cloud for storing the downsampled cloud 23 | pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); 24 | 25 | for (int i = 0; i < cloud_vec.size(); ++i) 26 | { 27 | // Convert, downsample, and send all recorded clouds to the SCManager 28 | // Convert the ROS point cloud message to a PCL point cloud 29 | pcl::fromROSMsg(cloud_vec[i], *downsampled_cloud); 30 | 31 | // Downsample the point cloud using the voxel grid filter 32 | voxel_grid.setInputCloud(downsampled_cloud); 33 | voxel_grid.filter(*downsampled_cloud); 34 | 35 | // Send the downsampled point cloud to the SCManager and save the Scan Context and key 36 | sc_manager.makeAndSaveScancontextAndKeys(*downsampled_cloud); 37 | 38 | // Calculate the descriptor distances between the current and previous scans 39 | for (int j = 0; j <= i - min_idx_diff; ++j) 40 | { 41 | double dist = sc_manager.getDistance(i, j); 42 | if (dist < THRESHOLD) 43 | predicted[i][j] = true; 44 | } 45 | } 46 | } -------------------------------------------------------------------------------- /src/methods/scan_context/Scancontext.cpp: -------------------------------------------------------------------------------- 1 | #include "Scancontext.h" 2 | 3 | // namespace SC2 4 | // { 5 | 6 | void coreImportTest (void) 7 | { 8 | cout << "scancontext lib is successfully imported." << endl; 9 | } // coreImportTest 10 | 11 | 12 | float rad2deg(float radians) 13 | { 14 | return radians * 180.0 / M_PI; 15 | } 16 | 17 | float deg2rad(float degrees) 18 | { 19 | return degrees * M_PI / 180.0; 20 | } 21 | 22 | 23 | float xy2theta( const float & _x, const float & _y ) 24 | { 25 | if ( _x >= 0 & _y >= 0) 26 | return (180/M_PI) * atan(_y / _x); 27 | 28 | if ( _x < 0 & _y >= 0) 29 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); 30 | 31 | if ( _x < 0 & _y < 0) 32 | return 180 + ( (180/M_PI) * atan(_y / _x) ); 33 | 34 | if ( _x >= 0 & _y < 0) 35 | return 360 - ( (180/M_PI) * atan((-_y) / _x) ); 36 | } // xy2theta 37 | 38 | 39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ) 40 | { 41 | // shift columns to right direction 42 | assert(_num_shift >= 0); 43 | 44 | if( _num_shift == 0 ) 45 | { 46 | MatrixXd shifted_mat( _mat ); 47 | return shifted_mat; // Early return 48 | } 49 | 50 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); 51 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) 52 | { 53 | int new_location = (col_idx + _num_shift) % _mat.cols(); 54 | shifted_mat.col(new_location) = _mat.col(col_idx); 55 | } 56 | 57 | return shifted_mat; 58 | 59 | } // circshift 60 | 61 | 62 | std::vector eig2stdvec( MatrixXd _eigmat ) 63 | { 64 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); 65 | return vec; 66 | } // eig2stdvec 67 | 68 | 69 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) 70 | { 71 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector 72 | double sum_sector_similarity = 0; 73 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) 74 | { 75 | VectorXd col_sc1 = _sc1.col(col_idx); 76 | VectorXd col_sc2 = _sc2.col(col_idx); 77 | 78 | if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) 79 | continue; // don't count this sector pair. 80 | 81 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); 82 | 83 | sum_sector_similarity = sum_sector_similarity + sector_similarity; 84 | num_eff_cols = num_eff_cols + 1; 85 | } 86 | 87 | double sc_sim = sum_sector_similarity / num_eff_cols; 88 | return 1.0 - sc_sim; 89 | 90 | } // distDirectSC 91 | 92 | 93 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) 94 | { 95 | int argmin_vkey_shift = 0; 96 | double min_veky_diff_norm = 10000000; 97 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) 98 | { 99 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); 100 | 101 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted; 102 | 103 | double cur_diff_norm = vkey_diff.norm(); 104 | if( cur_diff_norm < min_veky_diff_norm ) 105 | { 106 | argmin_vkey_shift = shift_idx; 107 | min_veky_diff_norm = cur_diff_norm; 108 | } 109 | } 110 | 111 | return argmin_vkey_shift; 112 | 113 | } // fastAlignUsingVkey 114 | 115 | 116 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) 117 | { 118 | // 1. fast align using variant key (not in original IROS18) 119 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); 120 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); 121 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); 122 | 123 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 124 | std::vector shift_idx_search_space { argmin_vkey_shift }; 125 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) 126 | { 127 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); 128 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); 129 | } 130 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); 131 | 132 | // 2. fast columnwise diff 133 | int argmin_shift = 0; 134 | double min_sc_dist = 10000000; 135 | for ( int num_shift: shift_idx_search_space ) 136 | { 137 | MatrixXd sc2_shifted = circshift(_sc2, num_shift); 138 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); 139 | if( cur_sc_dist < min_sc_dist ) 140 | { 141 | argmin_shift = num_shift; 142 | min_sc_dist = cur_sc_dist; 143 | } 144 | } 145 | 146 | return make_pair(min_sc_dist, argmin_shift); 147 | 148 | } // distanceBtnScanContext 149 | 150 | 151 | MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) 152 | { 153 | TicToc t_making_desc; 154 | 155 | int num_pts_scan_down = _scan_down.points.size(); 156 | 157 | // main 158 | const int NO_POINT = -1000; 159 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); 160 | 161 | SCPointType pt; 162 | float azim_angle, azim_range; // wihtin 2d plane 163 | int ring_idx, sctor_idx; 164 | for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) 165 | { 166 | pt.x = _scan_down.points[pt_idx].x; 167 | pt.y = _scan_down.points[pt_idx].y; 168 | pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). 169 | 170 | // xyz to ring, sector 171 | azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); 172 | azim_angle = xy2theta(pt.x, pt.y); 173 | 174 | // if range is out of roi, pass 175 | if( azim_range > PC_MAX_RADIUS ) 176 | continue; 177 | 178 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); 179 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); 180 | 181 | // taking maximum z 182 | if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 183 | desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin 184 | } 185 | 186 | // reset no points to zero (for cosine dist later) 187 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) 188 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) 189 | if( desc(row_idx, col_idx) == NO_POINT ) 190 | desc(row_idx, col_idx) = 0; 191 | 192 | t_making_desc.toc("PolarContext making"); 193 | 194 | return desc; 195 | } // SCManager::makeScancontext 196 | 197 | 198 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) 199 | { 200 | /* 201 | * summary: rowwise mean vector 202 | */ 203 | Eigen::MatrixXd invariant_key(_desc.rows(), 1); 204 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) 205 | { 206 | Eigen::MatrixXd curr_row = _desc.row(row_idx); 207 | invariant_key(row_idx, 0) = curr_row.mean(); 208 | } 209 | 210 | return invariant_key; 211 | } // SCManager::makeRingkeyFromScancontext 212 | 213 | 214 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) 215 | { 216 | /* 217 | * summary: columnwise mean vector 218 | */ 219 | Eigen::MatrixXd variant_key(1, _desc.cols()); 220 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) 221 | { 222 | Eigen::MatrixXd curr_col = _desc.col(col_idx); 223 | variant_key(0, col_idx) = curr_col.mean(); 224 | } 225 | 226 | return variant_key; 227 | } // SCManager::makeSectorkeyFromScancontext 228 | 229 | 230 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) 231 | { 232 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 233 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); 234 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); 235 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); 236 | 237 | polarcontexts_.push_back( sc ); 238 | polarcontext_invkeys_.push_back( ringkey ); 239 | polarcontext_vkeys_.push_back( sectorkey ); 240 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); 241 | 242 | // cout < dist_shift = distanceBtnScanContext(context1, context2); 256 | 257 | return dist_shift.first; 258 | } 259 | 260 | std::pair SCManager::detectLoopClosureID ( void ) 261 | { 262 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") 263 | 264 | auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) 265 | auto curr_desc = polarcontexts_.back(); // current observation (query) 266 | 267 | /* 268 | * step 1: candidates from ringkey tree_ 269 | */ 270 | if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) 271 | { 272 | std::pair result {loop_id, 0.0}; 273 | return result; // Early return 274 | } 275 | 276 | // tree_ reconstruction (not mandatory to make everytime) 277 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost 278 | { 279 | TicToc t_tree_construction; 280 | 281 | polarcontext_invkeys_to_search_.clear(); 282 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; 283 | 284 | polarcontext_tree_.reset(); 285 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); 286 | // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) 287 | t_tree_construction.toc("Tree construction"); 288 | } 289 | tree_making_period_conter = tree_making_period_conter + 1; 290 | 291 | double min_dist = 10000000; // init with somthing large 292 | int nn_align = 0; 293 | int nn_idx = 0; 294 | 295 | // knn search 296 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 297 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); 298 | 299 | TicToc t_tree_search; 300 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); 301 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); 302 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 303 | t_tree_search.toc("Tree search"); 304 | 305 | /* 306 | * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) 307 | */ 308 | TicToc t_calc_dist; 309 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) 310 | { 311 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; 312 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 313 | 314 | double candidate_dist = sc_dist_result.first; 315 | int candidate_align = sc_dist_result.second; 316 | 317 | if( candidate_dist < min_dist ) 318 | { 319 | min_dist = candidate_dist; 320 | nn_align = candidate_align; 321 | 322 | nn_idx = candidate_indexes[candidate_iter_idx]; 323 | } 324 | } 325 | t_calc_dist.toc("Distance calc"); 326 | 327 | /* 328 | * loop threshold check 329 | */ 330 | if( min_dist < SC_DIST_THRES ) 331 | { 332 | loop_id = nn_idx; 333 | 334 | // std::cout.precision(3); 335 | cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 336 | cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; 337 | } 338 | else 339 | { 340 | std::cout.precision(3); 341 | cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 342 | cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; 343 | } 344 | 345 | // To do: return also nn_align (i.e., yaw diff) 346 | float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); 347 | std::pair result {loop_id, yaw_diff_rad}; 348 | 349 | return result; 350 | 351 | } // SCManager::detectLoopClosureID 352 | 353 | // } // namespace SC2 -------------------------------------------------------------------------------- /src/msg_converter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | double calc_dist3d(pcl::PointXYZI &pt) 14 | { 15 | return sqrt((pt.x * pt.x) + (pt.y * pt.y) + (pt.z * pt.z)); 16 | } 17 | 18 | void process_cloud(const livox_ros_driver::CustomMsg::ConstPtr &msg, 19 | pcl::PointCloud::Ptr &pcl_cloud_filtered, 20 | bool print_info = false, double dist_threshold = 1.0) 21 | { 22 | pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); 23 | 24 | int high_noise = 0; 25 | 26 | std::map point_info; 27 | point_info["ret_0"] = 0; 28 | point_info["ret_1"] = 0; 29 | point_info["ret_2"] = 0; 30 | point_info["ret_3"] = 0; 31 | point_info["noise"] = 0; 32 | 33 | for (size_t i = 0; i < msg->point_num; i++) 34 | { 35 | // 2 types of points are considered good: 36 | // 1. normal point(spatial position), normal point(intensity), return 37 | if ((msg->points[i].tag & 0x30) == 0x30) 38 | { 39 | point_info["ret_3"]++; 40 | } 41 | else if ((msg->points[i].tag & 0x30) == 0x20) 42 | { 43 | point_info["ret_2"]++; 44 | } 45 | else if ((msg->points[i].tag & 0x30) == 0x10) 46 | { 47 | point_info["ret_1"]++; 48 | 49 | pcl::PointXYZI cur_point; 50 | cur_point.x = msg->points[i].x; 51 | cur_point.y = msg->points[i].y; 52 | cur_point.z = msg->points[i].z; 53 | cur_point.intensity = msg->points[i].reflectivity; 54 | pcl_cloud->push_back(cur_point); 55 | } 56 | else if ((msg->points[i].tag & 0x30) == 0x00) 57 | { 58 | // ret_num0++; 59 | point_info["ret_0"]++; 60 | 61 | pcl::PointXYZI cur_point; 62 | cur_point.x = msg->points[i].x; 63 | cur_point.y = msg->points[i].y; 64 | cur_point.z = msg->points[i].z; 65 | cur_point.intensity = msg->points[i].reflectivity; 66 | pcl_cloud->push_back(cur_point); 67 | } 68 | 69 | if (msg->points[i].tag % 16 != 0) 70 | { 71 | // high_noise++; 72 | point_info["noise"]++; 73 | } 74 | } 75 | 76 | // pcl::PointCloud::Ptr pcl_cloud_filtered(new pcl::PointCloud); 77 | // 2 filters: 78 | // 1. remove points that are too close to lidar 79 | // 2. remove one of the points if 2 points are too close to each other 80 | for (size_t i = 0; i < pcl_cloud->size(); i++) 81 | { 82 | if (calc_dist3d(pcl_cloud->points[i]) < dist_threshold) 83 | { 84 | continue; 85 | } 86 | 87 | pcl_cloud_filtered->push_back(pcl_cloud->points[i]); 88 | } 89 | 90 | int normal_points = point_info["ret_0"] + point_info["ret_1"] + point_info["ret_2"] + point_info["ret_3"]; 91 | 92 | if (print_info) 93 | { 94 | std::cout << "ret_num0: " << point_info["ret_0"] << "| ret_num1: " << point_info["ret_1"] << "| ret_num2: " 95 | << point_info["ret_2"] << "| ret_num3: " << point_info["ret_3"] << std::endl; 96 | std::cout << "normal: " << normal_points << "| noise: " << high_noise << std::endl; 97 | } 98 | } 99 | 100 | class msgConverter 101 | { 102 | 103 | public: 104 | ros::NodeHandle nh; 105 | ros::Publisher lidar_pub; 106 | ros::Subscriber lidar_sub; 107 | 108 | ros::Time last_time; 109 | 110 | msgConverter() 111 | { 112 | lidar_pub = nh.advertise("/livox/lidar_pc", 10000); 113 | lidar_sub = nh.subscribe("/livox/lidar", 10000, &msgConverter::convert_cloud, this); 114 | } 115 | 116 | void convert_cloud(const livox_ros_driver::CustomMsg::ConstPtr &input_cloud) 117 | { 118 | // custom->pointcloud2 119 | 120 | pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud); 121 | process_cloud(input_cloud, pcl_cloud); 122 | 123 | sensor_msgs::PointCloud2 temp_out_msg; 124 | 125 | pcl::toROSMsg(*pcl_cloud, temp_out_msg); 126 | last_time = ros::Time::now(); 127 | 128 | temp_out_msg.header.stamp = input_cloud->header.stamp; 129 | 130 | temp_out_msg.header.frame_id = input_cloud->header.frame_id; 131 | temp_out_msg.header.frame_id = "trunk"; 132 | 133 | lidar_pub.publish(temp_out_msg); 134 | } 135 | }; 136 | 137 | int main(int argc, char **argv) 138 | { 139 | ros::init(argc, argv, "cloud_converter"); 140 | 141 | msgConverter converter; 142 | 143 | ros::spin(); 144 | 145 | return 0; 146 | } --------------------------------------------------------------------------------