├── .gitignore ├── CMakeLists.txt ├── README.md ├── build_within_docker.sh ├── docker └── docker_run.sh ├── examples ├── README.md ├── data │ ├── KITTI_SCD_download_link.txt │ ├── kcp │ │ ├── 1531883530.449377000.pcd │ │ └── 1531883530.949817000.pcd │ └── kitti_00_partial │ │ ├── 000000.bin │ │ ├── 000001.bin │ │ ├── 000002.bin │ │ ├── 000020.bin │ │ └── 000040.bin ├── test_batch_scd_saver.py ├── test_compare_scd.py ├── test_make_desciptor.py ├── test_single_session_online_place_recognition.py └── test_utils.py ├── python ├── CMakeLists.txt ├── setup.py.in └── wrapper.cpp └── scancontext ├── CMakeLists.txt └── include └── scancontext ├── KDTreeVectorOfVectorsAdaptor.h ├── Scancontext.cpp └── Scancontext.h /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | SCD/ 3 | __pycache__/ 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | project(Scancontext VERSION 1.0.0) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | if(NOT CMAKE_BUILD_TYPE) 6 | # Options: Debug, Release, MinSizeRel, RelWithDebInfo 7 | message(STATUS "No build type selected, default to Release") 8 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose build type." FORCE) 9 | endif() 10 | 11 | include(FetchContent) 12 | 13 | ############################################################################### 14 | # Options 15 | ############################################################################### 16 | 17 | option(SC_BUILD_PYTHON_BINDING "Build Python bindings for Scancontext" ON) 18 | 19 | ############################################################################### 20 | # Dependencies 21 | ############################################################################### 22 | 23 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 24 | 25 | FetchContent_Declare(nanoflann 26 | GIT_REPOSITORY https://github.com/jlblancoc/nanoflann 27 | GIT_TAG v1.4.2 28 | ) 29 | FetchContent_MakeAvailable(nanoflann) 30 | 31 | if(SC_BUILD_PYTHON_BINDING) 32 | FetchContent_Declare(pybind11 33 | GIT_REPOSITORY https://github.com/pybind/pybind11 34 | GIT_TAG v2.9.2 35 | ) 36 | FetchContent_MakeAvailable(pybind11) 37 | endif() 38 | 39 | ############################################################################### 40 | # Targets 41 | ############################################################################### 42 | 43 | add_subdirectory(scancontext) 44 | 45 | if(SC_BUILD_PYTHON_BINDING) 46 | add_subdirectory(python) 47 | endif() 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # scancontext-pybind 2 | 3 | ## News 4 | - Currently (Apr. 2022), not a set of full features are made yet. For checking the features currently supported, see `wrapper.cpp`. 5 | 6 | ## Purpose 7 | - Goal of this repo is 8 | - For fast python support for lidar-based place recognition 9 | - ps. the native python version (see https://github.com/gisbi-kim/PyICP-SLAM) was already supported, but it was slow. 10 | - Original C++ code: see https://github.com/irapkaist/scancontext 11 | 12 | ## Build 13 | - Please ensure eigen3 is installed (e.g., via `sudo apt install libeigen3-dev`) 14 | - Other dependencies (nanoflann and pybind11) are downloaded automatically (for details, see `CMakeLists.txt`) 15 | - Use the following commands to build: 16 | ``` 17 | $ mkdir build 18 | $ cd build 19 | $ cmake .. 20 | $ make 21 | ``` 22 | - Install the Python package via `pip` with 23 | ``` 24 | $ cd build 25 | $ make pip-install 26 | # or install manually with 27 | $ cd build/python 28 | $ pip install . 29 | ``` 30 | 31 | ## Use examples 32 | - For the hands-on exploration of the supported features, for example (you need `numpy` and `open3d`), 33 | ``` 34 | $ python3 examples/test_make_desciptor.py 35 | $ python3 examples/test_compare_scd.py 36 | $ # to be added ... 37 | ``` 38 | 39 | ## Docker support 40 | - see `docker/docker_run.sh`. Modify the REPOSITORY_PATH with your own path. 41 | - For the docker-based test tutorial, see this [video (TBA)](TBA) 42 | 43 | ## Application: PyICP-SLAM2 44 | - TBA ... 45 | 46 | 47 | ## Cite 48 | ``` 49 | @ARTICLE { gskim-2021-tro, 50 | AUTHOR = { Giseop Kim and Sunwook Choi and Ayoung Kim }, 51 | TITLE = { Scan Context++: Structural Place Recognition Robust to Rotation and Lateral Variations in Urban Environments }, 52 | JOURNAL = { IEEE Transactions on Robotics }, 53 | YEAR = { 2021 } 54 | } 55 | 56 | @INPROCEEDINGS { gkim-2018-iros, 57 | author = {Kim, Giseop and Kim, Ayoung}, 58 | title = { Scan Context: Egocentric Spatial Descriptor for Place Recognition within {3D} Point Cloud Map }, 59 | booktitle = { Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems }, 60 | year = { 2018 }, 61 | month = { Oct. }, 62 | address = { Madrid } 63 | } 64 | ``` 65 | ## Ack 66 | - Thanks to the author of [KCP](https://github.com/StephLin/KCP). The pybind wrapper src and the directory structures were borrowed from [KCP](https://github.com/StephLin/KCP). 67 | -------------------------------------------------------------------------------- /build_within_docker.sh: -------------------------------------------------------------------------------- 1 | cd 2 | cd scancontext-pybind 3 | mkdir build 4 | cd build 5 | rm -rf * 6 | cmake .. 7 | make -------------------------------------------------------------------------------- /docker/docker_run.sh: -------------------------------------------------------------------------------- 1 | # change the path /home/gskim/Documents/git/scancontext-pybind to your path 2 | REPOSITORY_PATH='/home/gskim/Documents/git/scancontext-pybind' 3 | 4 | # optional 5 | DATA_PATH='/media/gskim/GS1TB/KITTI/dataset/sequences' 6 | 7 | # X11-unix things for use open3d viewer within docker ps (see http://www.open3d.org/docs/release/docker.html) 8 | docker run -ti --net=host\ 9 | --device=/dev/dri:/dev/dri \ 10 | -v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY \ 11 | -v ${REPOSITORY_PATH}:/root/scancontext-pybind \ 12 | -v ${DATA_PATH}:/data/kitti\ 13 | giseopkim/pyicpslam2 14 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # A list of small examples to be added 2 | - [ ] `test_draw_tp_fp_trajectory.py` 3 | - TBA ... 4 | -------------------------------------------------------------------------------- /examples/data/KITTI_SCD_download_link.txt: -------------------------------------------------------------------------------- 1 | https://www.dropbox.com/s/aknimn9s8l01171/SCD.zip?dl=0 2 | -------------------------------------------------------------------------------- /examples/data/kcp/1531883530.449377000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kcp/1531883530.449377000.pcd -------------------------------------------------------------------------------- /examples/data/kcp/1531883530.949817000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kcp/1531883530.949817000.pcd -------------------------------------------------------------------------------- /examples/data/kitti_00_partial/000000.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kitti_00_partial/000000.bin -------------------------------------------------------------------------------- /examples/data/kitti_00_partial/000001.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kitti_00_partial/000001.bin -------------------------------------------------------------------------------- /examples/data/kitti_00_partial/000002.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kitti_00_partial/000002.bin -------------------------------------------------------------------------------- /examples/data/kitti_00_partial/000020.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kitti_00_partial/000020.bin -------------------------------------------------------------------------------- /examples/data/kitti_00_partial/000040.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gisbi-kim/scancontext-pybind/fb02e0c203a33760fd7701b75d1847db1230786d/examples/data/kitti_00_partial/000040.bin -------------------------------------------------------------------------------- /examples/test_batch_scd_saver.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | from functools import wraps 5 | 6 | import numpy as np 7 | import open3d as o3d 8 | import pyscancontext as sc 9 | from test_utils import * 10 | 11 | scm = sc.SCManager() 12 | 13 | @timeit 14 | def read_bin(bin_path): 15 | scan = np.fromfile(bin_path, dtype=np.float32) 16 | scan = scan.reshape((-1, 4)) 17 | ptcloud_xyz = scan[:, :-1] 18 | return ptcloud_xyz 19 | 20 | @timeit 21 | def downsampling(xyz, voxel_size=0.35): 22 | pcd = o3d.geometry.PointCloud() 23 | pcd.points = o3d.utility.Vector3dVector(xyz) 24 | pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size) 25 | return np.asarray(pcd_down.points) 26 | 27 | @timeit 28 | def make_scancontext(xyz): 29 | return scm.make_scancontext(xyz) 30 | 31 | # @timeit 32 | def bin2scd(filepath, voxel_size=0.35): 33 | xyz = read_bin(filepath) 34 | 35 | use_downsampled_cloud = True 36 | if use_downsampled_cloud: 37 | xyz = downsampling(xyz, voxel_size) 38 | 39 | return make_scancontext(xyz) 40 | 41 | dataset_dir = '/data/kitti/' 42 | seq_idx = '08' 43 | 44 | seq_dir = os.path.join(dataset_dir, seq_idx) 45 | scans_dir = os.path.join(seq_dir, 'velodyne') 46 | 47 | scans_names = os.listdir(scans_dir) 48 | scans_paths = [os.path.join(scans_dir, scan_name) for scan_name in scans_names] 49 | scans_paths.sort() 50 | print(f' KITTI dataset sequence\'s point clouds are from: {scans_dir}') 51 | 52 | scd_list = [] 53 | for ii, scan_path in enumerate(scans_paths): 54 | print(scan_path, 'processed.') 55 | scd = bin2scd(scan_path) 56 | scd_list.append(scd) 57 | 58 | scds = np.array(scd_list) 59 | np.save(seq_idx + '_SCDs.npy', scds) -------------------------------------------------------------------------------- /examples/test_compare_scd.py: -------------------------------------------------------------------------------- 1 | import os 2 | import time 3 | import numpy as np 4 | import open3d as o3d 5 | import pyscancontext as sc 6 | 7 | 8 | curr_dir = os.path.dirname(__file__) 9 | data_dir = os.path.join(curr_dir, "data/kitti_00_partial") 10 | 11 | query_cloud_filepath = os.path.join(data_dir, '000000.bin') 12 | positive_cloud_filepath = os.path.join(data_dir, '000001.bin') 13 | negative_cloud_filepath = os.path.join(data_dir, '000040.bin') 14 | 15 | scm = sc.SCManager() 16 | 17 | def read_bin(bin_path): 18 | scan = np.fromfile(bin_path, dtype=np.float32) 19 | scan = scan.reshape((-1, 4)) 20 | ptcloud_xyz = scan[:, :-1] 21 | return ptcloud_xyz 22 | 23 | def bin2scd(filepath, voxel_size=0.5): 24 | xyz = read_bin(filepath) 25 | print( f" The poitn cloud {filepath} is loaded." ) 26 | pcd = o3d.geometry.PointCloud() 27 | pcd.points = o3d.utility.Vector3dVector(xyz) 28 | pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size) 29 | xyz_down = np.asarray(pcd_down.points) 30 | scd = scm.make_scancontext(xyz_down) 31 | return scd 32 | 33 | print('') 34 | query_scd = bin2scd(query_cloud_filepath) 35 | positive_scd = bin2scd(positive_cloud_filepath) 36 | negative_scd = bin2scd(negative_cloud_filepath) 37 | 38 | def calc_scd_dist_and_time(scd1, scd2): 39 | ts = time.time() 40 | distance, argmin_rot_idx = scm.scd_distance(scd1, scd2) 41 | te = time.time() 42 | t_elaps = te - ts 43 | return distance, t_elaps 44 | 45 | print('') 46 | distance_btn_positive_pair, t1 = calc_scd_dist_and_time(query_scd, positive_scd) 47 | distance_btn_negative_pair, t2 = calc_scd_dist_and_time(query_scd, negative_scd) 48 | 49 | print( f" The time cost of SCD distance calculation is: {((t1 + t2)/2)*1000:.2f} ms" ) 50 | print( f" The SCD distance between the positive pair is: {distance_btn_positive_pair:.3f}" ) 51 | print( f" The SCD distance between the negative pair is: {distance_btn_negative_pair:.3f}" ) 52 | ''' 53 | the result is, for example, 54 | The time cost of SCD distance calculation is: 2.81 ms 55 | The SCD distance between the positive pair is: 0.190 56 | The SCD distance between the negative pair is: 0.776 57 | ''' -------------------------------------------------------------------------------- /examples/test_make_desciptor.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | import numpy as np 5 | import open3d as o3d 6 | import pyscancontext as sc 7 | 8 | curr_dir = os.path.dirname(__file__) 9 | data_dir = os.path.join(curr_dir, "data/kcp") 10 | 11 | scm = sc.SCManager() 12 | scm.print_parameters() 13 | 14 | # original cloud 15 | print('') 16 | source_point_cloud_filename = os.path.join(data_dir, '1531883530.449377000.pcd') 17 | source_o3d = o3d.io.read_point_cloud(source_point_cloud_filename) 18 | print(source_o3d) 19 | 20 | cloud = np.asarray(source_o3d.points) 21 | 22 | ts = time.time() 23 | scd = scm.make_scancontext(cloud) 24 | te = time.time() 25 | print( f" The time cost of Scan Context Descriptor generation for the original cloud: {(te - ts)*1000:.2f} ms" ) 26 | # e.g., The time cost of Scan Context Descriptor generation for the original cloud: 11.21 ms 27 | 28 | 29 | # downsampled cloud (scan context generally works well for downsampled clouds, please see T-RO paepr for the details) 30 | print('') 31 | voxel_size = 0.35 32 | source_down_o3d = source_o3d.voxel_down_sample(voxel_size=voxel_size) 33 | print(source_down_o3d) 34 | 35 | down_cloud = np.asarray(source_down_o3d.points) 36 | 37 | ts = time.time() 38 | scd = scm.make_scancontext(down_cloud) 39 | te = time.time() 40 | print( f" The time cost of Scan Context Descriptor generation for the downsample cloud: {(te - ts)*1000:.2f} ms" ) 41 | # e.g., The time cost of Scan Context Descriptor generation for the downsample cloud: 2.84 ms 42 | -------------------------------------------------------------------------------- /examples/test_single_session_online_place_recognition.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | from functools import wraps 5 | 6 | import numpy as np 7 | import open3d as o3d 8 | import pyscancontext as sc 9 | 10 | from test_utils import * 11 | 12 | # place recognizer 13 | scm = sc.SCManager() 14 | 15 | # loop parameters 16 | LOOP_THRES = 0.2 17 | 18 | # load pre-made data 19 | @timeit 20 | def load_data(seq_idx): 21 | curr_dir = os.path.dirname(__file__) 22 | data_dir = os.path.join(curr_dir, "data/SCD/KITTI") 23 | scds = np.load(os.path.join(data_dir, seq_idx, 'SCDs.npy')) 24 | return scds 25 | 26 | seq_idx = '00' 27 | scds = load_data(seq_idx) 28 | 29 | @timeit_sec 30 | def run_sequence(): 31 | # sensor data stream 32 | num_scds = scds.shape[0] 33 | for ii in range(num_scds): 34 | # point cloud to scd 35 | # this was done in batch for the fast demo. see test_batch_scd_saver.py 36 | 37 | # query data 38 | scd = scds[ii, :] 39 | 40 | # retrieval (querying with online construction of kdtree) 41 | scm.add_scancontext(scd) 42 | nn_idx, nn_dist, yaw_diff = scm.detect_loop() 43 | if nn_idx == -1: # skip the very first scans (see NUM_EXCLUDE_RECENT in Scancontext.h) 44 | continue 45 | 46 | if nn_dist < LOOP_THRES: 47 | print(f'query: scan {ii}') 48 | print(f' detected nn node - idx: {nn_idx}, distance: {nn_dist:.3f}, yaw_diff: {np.rad2deg(yaw_diff):.1f} deg') 49 | # note: if use 60 sectors for a SCD, yaw_diff's minimum resolution is 6 deg. 50 | 51 | run_sequence() 52 | 53 | 54 | # todo 55 | # ''' 56 | # visualization 57 | # ''' 58 | # # read gt pose to draw the matches 59 | # poses = np.loadtxt(os.path.join(seq_dir, 'poses.txt')) 60 | # poses_xyz = poses[:, (3,7,11)] # actually cam pose 61 | # poses_o3d = o3d.geometry.PointCloud() 62 | # poses_o3d.points = o3d.utility.Vector3dVector(poses_xyz) 63 | # o3d.visualization.draw_geometries([poses_o3d]) 64 | 65 | 66 | -------------------------------------------------------------------------------- /examples/test_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | from functools import wraps 5 | 6 | # 7 | curr_dir = os.path.dirname(__file__) 8 | 9 | # 10 | time_verbose = True 11 | 12 | def timeit(func): 13 | @wraps(func) 14 | def timeit_wrapper(*args, **kwargs): 15 | start_time = time.perf_counter() 16 | result = func(*args, **kwargs) 17 | end_time = time.perf_counter() 18 | total_time = end_time - start_time 19 | if time_verbose: 20 | print(f' function {func.__name__}: {total_time*1000:.3f} ms') 21 | return result 22 | return timeit_wrapper 23 | 24 | def timeit_sec(func): 25 | @wraps(func) 26 | def timeit_wrapper(*args, **kwargs): 27 | start_time = time.perf_counter() 28 | result = func(*args, **kwargs) 29 | end_time = time.perf_counter() 30 | total_time = end_time - start_time 31 | if time_verbose: 32 | print(f' function {func.__name__}: {total_time:.1f} s') 33 | return result 34 | return timeit_wrapper 35 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(PYPKG_DIR "${CMAKE_CURRENT_BINARY_DIR}/pyscancontext") 2 | 3 | pybind11_add_module(pyscancontext wrapper.cpp) 4 | target_link_libraries(pyscancontext PUBLIC Scancontext::scancontext) 5 | set_target_properties(pyscancontext PROPERTIES OUTPUT_NAME "pyscancontext") 6 | set_target_properties(pyscancontext PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${PYPKG_DIR}") 7 | 8 | # https://github.com/pybind/pybind11/issues/1818 9 | if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 10 | target_compile_options(pyscancontext PUBLIC -fsized-deallocation) 11 | endif() 12 | 13 | # copy setup.py file binary dir for install with: pip install . 14 | configure_file(setup.py.in ${CMAKE_CURRENT_BINARY_DIR}/setup.py) 15 | 16 | # Create the Python package -- Note that "." is used to conform to PEP 328 17 | file(WRITE "${PYPKG_DIR}/__init__.py" 18 | "from .pyscancontext import *\n" 19 | "from .pyscancontext import __version__\n" 20 | "from .pyscancontext import __doc__") 21 | 22 | set(DIST "none") 23 | if(UNIX AND NOT APPLE) 24 | execute_process(COMMAND bash -c "lsb_release -cs" OUTPUT_VARIABLE UBUNTU_DIST) 25 | string(STRIP "${UBUNTU_DIST}" UBUNTU_DIST) 26 | set(DIST "${UBUNTU_DIST}") 27 | elseif(APPLE) 28 | # TODO: not very specific... 29 | set(DIST "macos") 30 | elseif(WIN32) 31 | # TODO: not very specific... 32 | set(DIST "win10") 33 | endif() 34 | 35 | set(PKGSTR pyscancontext-py3-${DIST}-${PROJECT_VERSION}) 36 | add_custom_target(pypkg 37 | DEPENDS pyscancontext 38 | COMMAND ${CMAKE_COMMAND} -E make_directory ${PKGSTR} 39 | COMMAND ${CMAKE_COMMAND} -E copy setup.py ${PKGSTR}/ 40 | COMMAND ${CMAKE_COMMAND} -E copy_directory ${PYPKG_DIR} ${PKGSTR}/pyscancontext 41 | COMMAND ${CMAKE_COMMAND} -E tar zcvf ${PKGSTR}.tar.gz ${PKGSTR} 42 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) 43 | 44 | add_custom_target(pip-install 45 | DEPENDS pypkg 46 | COMMAND ${PYTHON_EXECUTABLE} -m pip install . 47 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${PKGSTR} 48 | ) -------------------------------------------------------------------------------- /python/setup.py.in: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='pyscancontext', 5 | version='${PROJECT_VERSION}', 6 | author='Giseop Kim', 7 | author_email='giseop.kim@naverlabs.com', 8 | description='Scan Context for LiDAR place recognition', 9 | package_dir={'': '${CMAKE_CURRENT_BINARY_DIR}'}, 10 | packages=['pyscancontext'], 11 | package_data={'': ['*.so']} 12 | ) -------------------------------------------------------------------------------- /python/wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "scancontext/Scancontext.h" 6 | 7 | #define STRINGIFY(x) #x 8 | #define MACRO_STRINGIFY(x) STRINGIFY(x) 9 | 10 | namespace py = pybind11; 11 | 12 | PYBIND11_MODULE(pyscancontext, m) { 13 | m.doc() = "Scan Context for LiDAR place recognition"; 14 | m.attr("__version__") = PROJECT_VERSION; 15 | 16 | py::class_(m, "SCManager") 17 | .def(py::init<>()) 18 | .def("print_parameters", &SCManager::printParameters, py::return_value_policy::copy) 19 | .def("scd_distance", &SCManager::distanceBtnScanContext, py::return_value_policy::copy) 20 | .def("make_scancontext", &SCManager::makeScancontext, py::return_value_policy::copy) 21 | .def("construct_tree", &SCManager::constructTree, py::return_value_policy::copy) 22 | .def("add_scancontext", &SCManager::saveScancontextAndKeys, py::return_value_policy::copy) 23 | .def("add_node", &SCManager::makeAndSaveScancontextAndKeys, py::return_value_policy::copy) 24 | .def("detect_loop", &SCManager::detectLoopClosureID, py::return_value_policy::copy); 25 | } 26 | -------------------------------------------------------------------------------- /scancontext/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(scancontext SHARED 2 | include/scancontext/Scancontext.cpp 3 | ) 4 | 5 | target_include_directories(scancontext PUBLIC 6 | $ 7 | $ 8 | $ 9 | ) 10 | 11 | set_target_properties(scancontext PROPERTIES VERSION ${PROJECT_VERSION}) 12 | target_compile_definitions(scancontext PUBLIC PROJECT_VERSION="${PROJECT_VERSION}") 13 | 14 | target_link_libraries(scancontext 15 | Eigen3::Eigen 16 | nanoflann::nanoflann 17 | ) 18 | 19 | add_library(Scancontext::scancontext ALIAS scancontext) 20 | 21 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ 22 | DESTINATION include 23 | ) 24 | -------------------------------------------------------------------------------- /scancontext/include/scancontext/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 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 | -------------------------------------------------------------------------------- /scancontext/include/scancontext/Scancontext.cpp: -------------------------------------------------------------------------------- 1 | // #include "scancontext/Scancontext.h" 2 | #include "Scancontext.h" 3 | 4 | double deg2rad(double degrees) 5 | { 6 | return degrees * M_PI / 180.0; 7 | } 8 | 9 | double xy2theta( const double & _x, const double & _y ) 10 | { 11 | if ( _x >= 0 & _y >= 0) 12 | return (180/M_PI) * atan(_y / _x); 13 | 14 | else if ( _x < 0 & _y >= 0) 15 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); 16 | 17 | else if ( _x < 0 & _y < 0) 18 | return 180 + ( (180/M_PI) * atan(_y / _x) ); 19 | 20 | else // ( _x >= 0 & _y < 0) 21 | return 360 - ( (180/M_PI) * atan((-_y) / _x) ); 22 | } 23 | 24 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ) 25 | { 26 | // shift columns to right direction 27 | assert(_num_shift >= 0); 28 | 29 | if( _num_shift == 0 ) 30 | { 31 | MatrixXd shifted_mat( _mat ); 32 | return shifted_mat; // Early return 33 | } 34 | 35 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); 36 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) 37 | { 38 | int new_location = (col_idx + _num_shift) % _mat.cols(); 39 | shifted_mat.col(new_location) = _mat.col(col_idx); 40 | } 41 | 42 | return shifted_mat; 43 | 44 | } // circshift 45 | 46 | 47 | std::vector eig2stdvec( MatrixXd _eigmat ) 48 | { 49 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); 50 | return vec; 51 | } // eig2stdvec 52 | 53 | 54 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) 55 | { 56 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector 57 | double sum_sector_similarity = 0; 58 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) 59 | { 60 | VectorXd col_sc1 = _sc1.col(col_idx); 61 | VectorXd col_sc2 = _sc2.col(col_idx); 62 | 63 | if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) 64 | continue; // don't count this sector pair. 65 | 66 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); 67 | 68 | sum_sector_similarity = sum_sector_similarity + sector_similarity; 69 | num_eff_cols = num_eff_cols + 1; 70 | } 71 | 72 | double sc_sim = sum_sector_similarity / num_eff_cols; 73 | return 1.0 - sc_sim; 74 | 75 | } // distDirectSC 76 | 77 | 78 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) 79 | { 80 | int argmin_vkey_shift = 0; 81 | double min_veky_diff_norm = 10000000; 82 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) 83 | { 84 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); 85 | 86 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted; 87 | 88 | double cur_diff_norm = vkey_diff.norm(); 89 | if( cur_diff_norm < min_veky_diff_norm ) 90 | { 91 | argmin_vkey_shift = shift_idx; 92 | min_veky_diff_norm = cur_diff_norm; 93 | } 94 | } 95 | 96 | return argmin_vkey_shift; 97 | 98 | } // fastAlignUsingVkey 99 | 100 | 101 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) 102 | { 103 | // 1. fast align using variant key (not in original IROS18) 104 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); 105 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); 106 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); 107 | 108 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 109 | std::vector shift_idx_search_space { argmin_vkey_shift }; 110 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) 111 | { 112 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); 113 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); 114 | } 115 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); 116 | 117 | // 2. fast columnwise diff 118 | int argmin_shift = 0; 119 | double min_sc_dist = 10000000; 120 | for ( int num_shift: shift_idx_search_space ) 121 | { 122 | MatrixXd sc2_shifted = circshift(_sc2, num_shift); 123 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); 124 | if( cur_sc_dist < min_sc_dist ) 125 | { 126 | argmin_shift = num_shift; 127 | min_sc_dist = cur_sc_dist; 128 | } 129 | } 130 | 131 | return make_pair(min_sc_dist, argmin_shift); 132 | 133 | } // distanceBtnScanContext 134 | 135 | 136 | // MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) 137 | MatrixXd SCManager::makeScancontext( Eigen::MatrixX3d & _scan_down ) 138 | { 139 | const int NO_POINT = -1000; 140 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); 141 | 142 | double azim_angle, azim_range; // wihtin 2d plane 143 | int ring_idx, sctor_idx; 144 | for (int idx = 0; idx < _scan_down.rows(); ++idx) 145 | { 146 | const auto &point = _scan_down.row(idx); 147 | const auto &ptx = point(0); 148 | const auto &pty = point(1); 149 | const auto &ptz = point(2); 150 | 151 | // xyz to ring, sector 152 | azim_range = sqrt(ptx * ptx + pty * pty); 153 | azim_angle = xy2theta(ptx, pty); 154 | 155 | // if range is out of roi, pass 156 | if( azim_range > PC_MAX_RADIUS ) 157 | continue; 158 | 159 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); 160 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); 161 | 162 | // taking maximum z 163 | if ( desc(ring_idx-1, sctor_idx-1) < ptz ) // -1 means cpp starts from 0 164 | desc(ring_idx-1, sctor_idx-1) = ptz; // update for taking maximum value at that bin 165 | } 166 | 167 | // reset no points to zero (for cosine dist later) 168 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) 169 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) 170 | if( desc(row_idx, col_idx) == NO_POINT ) 171 | desc(row_idx, col_idx) = 0; 172 | 173 | return desc; 174 | } // SCManager::makeScancontext 175 | 176 | 177 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) 178 | { 179 | /* 180 | * summary: rowwise mean vector 181 | */ 182 | Eigen::MatrixXd invariant_key(_desc.rows(), 1); 183 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) 184 | { 185 | Eigen::MatrixXd curr_row = _desc.row(row_idx); 186 | invariant_key(row_idx, 0) = curr_row.mean(); 187 | } 188 | 189 | return invariant_key; 190 | } // SCManager::makeRingkeyFromScancontext 191 | 192 | 193 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) 194 | { 195 | /* 196 | * summary: columnwise mean vector 197 | */ 198 | Eigen::MatrixXd variant_key(1, _desc.cols()); 199 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) 200 | { 201 | Eigen::MatrixXd curr_col = _desc.col(col_idx); 202 | variant_key(0, col_idx) = curr_col.mean(); 203 | } 204 | 205 | return variant_key; 206 | } // SCManager::makeSectorkeyFromScancontext 207 | 208 | 209 | void SCManager::saveScancontextAndKeys(Eigen::MatrixXd& sc) 210 | { 211 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); 212 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); 213 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); 214 | 215 | polarcontexts_.push_back( sc ); 216 | polarcontext_invkeys_.push_back( ringkey ); 217 | polarcontext_vkeys_.push_back( sectorkey ); 218 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); 219 | } 220 | 221 | void SCManager::makeAndSaveScancontextAndKeys(Eigen::MatrixX3d& _scan_down) 222 | { 223 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 224 | saveScancontextAndKeys(sc); 225 | } 226 | 227 | void SCManager::constructTree(void) 228 | { 229 | polarcontext_invkeys_to_search_.clear(); 230 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; 231 | 232 | polarcontext_tree_.reset(); 233 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); 234 | } 235 | 236 | std::tuple SCManager::detectLoopClosureID ( void ) 237 | { 238 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") 239 | 240 | auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) 241 | auto curr_desc = polarcontexts_.back(); // current observation (query) 242 | 243 | // step 1: candidates from ringkey tree_ 244 | if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) 245 | { 246 | std::tuple result {loop_id, 0.0, 0.0}; 247 | return result; // Early return 248 | } 249 | 250 | // tree_ reconstruction (not mandatory to make everytime) 251 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost 252 | { 253 | constructTree(); 254 | } 255 | tree_making_period_conter = tree_making_period_conter + 1; 256 | 257 | double min_dist = 10000000; // init with somthing large 258 | int nn_align = 0; 259 | int nn_idx = 0; 260 | 261 | // knn search 262 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 263 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); 264 | 265 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); 266 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); 267 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 268 | 269 | // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) 270 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) 271 | { 272 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; 273 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 274 | 275 | double candidate_dist = sc_dist_result.first; 276 | int candidate_align = sc_dist_result.second; 277 | 278 | if( candidate_dist < min_dist ) 279 | { 280 | min_dist = candidate_dist; 281 | nn_align = candidate_align; 282 | 283 | nn_idx = candidate_indexes[candidate_iter_idx]; 284 | 285 | loop_id = nn_idx; 286 | } 287 | } 288 | 289 | double yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); 290 | std::tuple result {loop_id, min_dist, yaw_diff_rad}; 291 | 292 | return result; 293 | } 294 | -------------------------------------------------------------------------------- /scancontext/include/scancontext/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 | #include 13 | 14 | #include 15 | 16 | #include "KDTreeVectorOfVectorsAdaptor.h" 17 | 18 | using namespace Eigen; 19 | using namespace nanoflann; 20 | 21 | using std::cout; 22 | using std::endl; 23 | using std::make_pair; 24 | 25 | using std::atan2; 26 | using std::cos; 27 | using std::sin; 28 | 29 | using KeyMat = std::vector >; 30 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 31 | 32 | // namespace SC2 33 | // { 34 | 35 | void coreImportTest ( void ); 36 | 37 | // sc param-independent helper functions 38 | float xy2theta( const float & _x, const float & _y ); 39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 40 | std::vector eig2stdvec( MatrixXd _eigmat ); 41 | 42 | class SCManager 43 | { 44 | public: 45 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 46 | 47 | Eigen::MatrixXd makeScancontext( Eigen::MatrixX3d & _scan_down ); 48 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 49 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 50 | 51 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 52 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 53 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 54 | 55 | void constructTree(void); 56 | 57 | void saveScancontextAndKeys( Eigen::MatrixXd & sc ); 58 | void makeAndSaveScancontextAndKeys( Eigen::MatrixX3d & _scan_down ); 59 | std::tuple detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 60 | 61 | void printParameters(void) { 62 | cout << "NUM_RING: " << PC_NUM_RING << endl; 63 | cout << "NUM_SECTOR: " << PC_NUM_SECTOR << endl; 64 | cout << "MAX_RADIUS: " << PC_MAX_RADIUS << endl; 65 | } 66 | 67 | Eigen::MatrixXd getSCD(int _idx) { 68 | // TODO: range safe gaurd 69 | return polarcontexts_[_idx]; 70 | } 71 | 72 | /* to be added ... (some get/setters) 73 | getSCD 74 | getKey 75 | setThres 76 | ... 77 | */ 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 | double SC_DIST_THRES = 0.2; // 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_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / if you want to find a very recent revisits use small value of it (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 | --------------------------------------------------------------------------------