diff --git a/CMakeLists.txt b/CMakeLists.txt index e153dfc..658c43f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.11) project(Scancontext VERSION 1.0.0) set(CMAKE_CXX_STANDARD 14) + if(NOT CMAKE_BUILD_TYPE) # Options: Debug, Release, MinSizeRel, RelWithDebInfo message(STATUS "No build type selected, default to Release") @@ -10,36 +11,35 @@ endif() include(FetchContent) -############################################################################### -# Options -############################################################################### +set(PYTHONVERSION "3.10") +# ############################################################################## +# Options +# ############################################################################## option(SC_BUILD_PYTHON_BINDING "Build Python bindings for Scancontext" ON) -############################################################################### +# ############################################################################## # Dependencies -############################################################################### - +# ############################################################################## find_package(Eigen3 3.3 REQUIRED NO_MODULE) FetchContent_Declare(nanoflann GIT_REPOSITORY https://github.com/jlblancoc/nanoflann - GIT_TAG v1.4.2 + GIT_TAG v1.4.2 ) FetchContent_MakeAvailable(nanoflann) -if(SC_BUILD_PYTHON_BINDING) +if(SC_BUILD_PYTHON_BINDING) FetchContent_Declare(pybind11 GIT_REPOSITORY https://github.com/pybind/pybind11 - GIT_TAG v2.9.2 + GIT_TAG v2.9.2 ) FetchContent_MakeAvailable(pybind11) endif() -############################################################################### +# ############################################################################## # Targets -############################################################################### - +# ############################################################################## add_subdirectory(scancontext) if(SC_BUILD_PYTHON_BINDING) diff --git a/README.md b/README.md index 844a1f0..af54556 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,13 @@ $ cmake .. $ make ``` +- CMake doesn’t detect the right Python version +The CMake-based build system will try to automatically detect the installed version of Python and link against that. When this fails, or when there are multiple versions of Python and it finds the wrong one, delete CMakeCache.txt and then add -DPYTHON_EXECUTABLE=$(which python) to your CMake configure line. (Replace $(which python) with a path to python if your prefer.) + +You can alternatively try -DPYBIND11_FINDPYTHON=ON, which will activate the new CMake FindPython support instead of pybind11’s custom search. Requires CMake 3.12+, and 3.15+ or 3.18.2+ are even better. You can set this in your CMakeLists.txt before adding or finding pybind11, as well. + + + - Install the Python package via `pip` with ``` $ cd build diff --git a/examples/ScanContextManagerCPP.py b/examples/ScanContextManagerCPP.py new file mode 100644 index 0000000..e7d5c05 --- /dev/null +++ b/examples/ScanContextManagerCPP.py @@ -0,0 +1,35 @@ +import pyscancontext as sc + +import numpy as np +np.set_printoptions(precision=4) + + +scm = sc.SCManager() + + +# ScanContextManager using python binding + +class ScanContextManagerCPP: + def __init__(self, shape=[20,60], num_candidates=10, threshold=0.15): # defualt configs are same as the original paper + self.shape = shape + self.num_candidates = num_candidates + self.threshold = threshold + + self.max_length = 80 # recommended but other (e.g., 100m) is also ok. + + self.ENOUGH_LARGE = 15000 # capable of up to ENOUGH_LARGE number of nodes + self.ptclouds = [None] * self.ENOUGH_LARGE + + self.curr_node_idx = 0 + + def addNode(self, node_idx, ptcloud): + scm.add_node(ptcloud) + self.curr_node_idx = node_idx + self.ptclouds[node_idx] = ptcloud + + + def getPtcloud(self, node_idx): + return self.ptclouds[node_idx] + + def detectLoop(self): + return scm.detect_loop() \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9bc51fa..6471adc 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,5 +1,4 @@ set(PYPKG_DIR "${CMAKE_CURRENT_BINARY_DIR}/pyscancontext") - pybind11_add_module(pyscancontext wrapper.cpp) target_link_libraries(pyscancontext PUBLIC Scancontext::scancontext) set_target_properties(pyscancontext PROPERTIES OUTPUT_NAME "pyscancontext") @@ -15,34 +14,35 @@ configure_file(setup.py.in ${CMAKE_CURRENT_BINARY_DIR}/setup.py) # Create the Python package -- Note that "." is used to conform to PEP 328 file(WRITE "${PYPKG_DIR}/__init__.py" - "from .pyscancontext import *\n" - "from .pyscancontext import __version__\n" - "from .pyscancontext import __doc__") + "from .pyscancontext import *\n" + "from .pyscancontext import __version__\n" + "from .pyscancontext import __doc__") set(DIST "none") + if(UNIX AND NOT APPLE) - execute_process(COMMAND bash -c "lsb_release -cs" OUTPUT_VARIABLE UBUNTU_DIST) - string(STRIP "${UBUNTU_DIST}" UBUNTU_DIST) - set(DIST "${UBUNTU_DIST}") + execute_process(COMMAND bash -c "lsb_release -cs" OUTPUT_VARIABLE UBUNTU_DIST) + string(STRIP "${UBUNTU_DIST}" UBUNTU_DIST) + set(DIST "${UBUNTU_DIST}") elseif(APPLE) - # TODO: not very specific... - set(DIST "macos") + # TODO: not very specific... + set(DIST "macos") elseif(WIN32) - # TODO: not very specific... - set(DIST "win10") + # TODO: not very specific... + set(DIST "win10") endif() set(PKGSTR pyscancontext-py3-${DIST}-${PROJECT_VERSION}) add_custom_target(pypkg - DEPENDS pyscancontext - COMMAND ${CMAKE_COMMAND} -E make_directory ${PKGSTR} - COMMAND ${CMAKE_COMMAND} -E copy setup.py ${PKGSTR}/ - COMMAND ${CMAKE_COMMAND} -E copy_directory ${PYPKG_DIR} ${PKGSTR}/pyscancontext - COMMAND ${CMAKE_COMMAND} -E tar zcvf ${PKGSTR}.tar.gz ${PKGSTR} - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + DEPENDS pyscancontext + COMMAND ${CMAKE_COMMAND} -E make_directory ${PKGSTR} + COMMAND ${CMAKE_COMMAND} -E copy setup.py ${PKGSTR}/ + COMMAND ${CMAKE_COMMAND} -E copy_directory ${PYPKG_DIR} ${PKGSTR}/pyscancontext + COMMAND ${CMAKE_COMMAND} -E tar zcvf ${PKGSTR}.tar.gz ${PKGSTR} + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) add_custom_target(pip-install - DEPENDS pypkg - COMMAND ${PYTHON_EXECUTABLE} -m pip install . - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${PKGSTR} + DEPENDS pypkg + COMMAND ${PYTHON_EXECUTABLE} -m pip install . + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${PKGSTR} ) \ No newline at end of file diff --git a/scancontext/include/scancontext/Scancontext.cpp b/scancontext/include/scancontext/Scancontext.cpp index 0e874e1..5670409 100644 --- a/scancontext/include/scancontext/Scancontext.cpp +++ b/scancontext/include/scancontext/Scancontext.cpp @@ -3,291 +3,289 @@ double deg2rad(double degrees) { - return degrees * M_PI / 180.0; + return degrees * M_PI / 180.0; } -double xy2theta( const double & _x, const double & _y ) +double xy2theta(const double &_x, const double &_y) { - if ( _x >= 0 & _y >= 0) - return (180/M_PI) * atan(_y / _x); + if (_x >= 0 & _y >= 0) + return (180 / M_PI) * atan(_y / _x); - else if ( _x < 0 & _y >= 0) - return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); + else if (_x < 0 & _y >= 0) + return 180 - ((180 / M_PI) * atan(_y / (-_x))); - else if ( _x < 0 & _y < 0) - return 180 + ( (180/M_PI) * atan(_y / _x) ); + else if (_x < 0 & _y < 0) + return 180 + ((180 / M_PI) * atan(_y / _x)); - else // ( _x >= 0 & _y < 0) - return 360 - ( (180/M_PI) * atan((-_y) / _x) ); + else // ( _x >= 0 & _y < 0) + return 360 - ((180 / M_PI) * atan((-_y) / _x)); } -MatrixXd circshift( MatrixXd &_mat, int _num_shift ) +MatrixXd circshift(MatrixXd &_mat, int _num_shift) { - // shift columns to right direction - assert(_num_shift >= 0); + // shift columns to right direction + assert(_num_shift >= 0); - if( _num_shift == 0 ) - { - MatrixXd shifted_mat( _mat ); - return shifted_mat; // Early return - } + if (_num_shift == 0) + { + MatrixXd shifted_mat(_mat); + return shifted_mat; // Early return + } - MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); - for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) - { - int new_location = (col_idx + _num_shift) % _mat.cols(); - shifted_mat.col(new_location) = _mat.col(col_idx); - } + MatrixXd shifted_mat = MatrixXd::Zero(_mat.rows(), _mat.cols()); + for (int col_idx = 0; col_idx < _mat.cols(); col_idx++) + { + int new_location = (col_idx + _num_shift) % _mat.cols(); + shifted_mat.col(new_location) = _mat.col(col_idx); + } - return shifted_mat; + return shifted_mat; } // circshift - -std::vector eig2stdvec( MatrixXd _eigmat ) +std::vector eig2stdvec(MatrixXd _eigmat) { - std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); - return vec; + std::vector vec(_eigmat.data(), _eigmat.data() + _eigmat.size()); + return vec; } // eig2stdvec - -double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) +double SCManager::distDirectSC(MatrixXd &_sc1, MatrixXd &_sc2) { - int num_eff_cols = 0; // i.e., to exclude all-nonzero sector - double sum_sector_similarity = 0; - for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) - { - VectorXd col_sc1 = _sc1.col(col_idx); - VectorXd col_sc2 = _sc2.col(col_idx); - - if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) - continue; // don't count this sector pair. + int num_eff_cols = 0; // i.e., to exclude all-nonzero sector + double sum_sector_similarity = 0; + for (int col_idx = 0; col_idx < _sc1.cols(); col_idx++) + { + VectorXd col_sc1 = _sc1.col(col_idx); + VectorXd col_sc2 = _sc2.col(col_idx); - double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); + if (col_sc1.norm() == 0 | col_sc2.norm() == 0) + continue; // don't count this sector pair. - sum_sector_similarity = sum_sector_similarity + sector_similarity; - num_eff_cols = num_eff_cols + 1; - } - - double sc_sim = sum_sector_similarity / num_eff_cols; - return 1.0 - sc_sim; + double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); -} // distDirectSC + sum_sector_similarity = sum_sector_similarity + sector_similarity; + num_eff_cols = num_eff_cols + 1; + } + double sc_sim = sum_sector_similarity / num_eff_cols; + return 1.0 - sc_sim; -int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) +} // distDirectSC + +int SCManager::fastAlignUsingVkey(MatrixXd &_vkey1, MatrixXd &_vkey2) { - int argmin_vkey_shift = 0; - double min_veky_diff_norm = 10000000; - for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) - { - MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); + int argmin_vkey_shift = 0; + double min_veky_diff_norm = 10000000; + for (int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++) + { + MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); - MatrixXd vkey_diff = _vkey1 - vkey2_shifted; + MatrixXd vkey_diff = _vkey1 - vkey2_shifted; - double cur_diff_norm = vkey_diff.norm(); - if( cur_diff_norm < min_veky_diff_norm ) - { - argmin_vkey_shift = shift_idx; - min_veky_diff_norm = cur_diff_norm; - } + double cur_diff_norm = vkey_diff.norm(); + if (cur_diff_norm < min_veky_diff_norm) + { + argmin_vkey_shift = shift_idx; + min_veky_diff_norm = cur_diff_norm; } + } - return argmin_vkey_shift; + return argmin_vkey_shift; } // fastAlignUsingVkey - -std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) +std::pair SCManager::distanceBtnScanContext(MatrixXd &_sc1, MatrixXd &_sc2) { - // 1. fast align using variant key (not in original IROS18) - MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); - MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); - int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); - - const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range - std::vector shift_idx_search_space { argmin_vkey_shift }; - for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) - { - shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); - shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); - } - std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); - - // 2. fast columnwise diff - int argmin_shift = 0; - double min_sc_dist = 10000000; - for ( int num_shift: shift_idx_search_space ) + // 1. fast align using variant key (not in original IROS18) + MatrixXd vkey_sc1 = makeSectorkeyFromScancontext(_sc1); + MatrixXd vkey_sc2 = makeSectorkeyFromScancontext(_sc2); + int argmin_vkey_shift = fastAlignUsingVkey(vkey_sc1, vkey_sc2); + + const int SEARCH_RADIUS = round(0.5 * SEARCH_RATIO * _sc1.cols()); // a half of search range + std::vector shift_idx_search_space{argmin_vkey_shift}; + for (int ii = 1; ii < SEARCH_RADIUS + 1; ii++) + { + shift_idx_search_space.push_back((argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols()); + shift_idx_search_space.push_back((argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols()); + } + std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); + + // 2. fast columnwise diff + int argmin_shift = 0; + double min_sc_dist = 10000000; + for (int num_shift : shift_idx_search_space) + { + MatrixXd sc2_shifted = circshift(_sc2, num_shift); + double cur_sc_dist = distDirectSC(_sc1, sc2_shifted); + if (cur_sc_dist < min_sc_dist) { - MatrixXd sc2_shifted = circshift(_sc2, num_shift); - double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); - if( cur_sc_dist < min_sc_dist ) - { - argmin_shift = num_shift; - min_sc_dist = cur_sc_dist; - } + argmin_shift = num_shift; + min_sc_dist = cur_sc_dist; } + } - return make_pair(min_sc_dist, argmin_shift); + return make_pair(min_sc_dist, argmin_shift); } // distanceBtnScanContext - // MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) -MatrixXd SCManager::makeScancontext( Eigen::MatrixX3d & _scan_down ) +MatrixXd SCManager::makeScancontext(Eigen::MatrixX3d &_scan_down) { - const int NO_POINT = -1000; - MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); - - double azim_angle, azim_range; // wihtin 2d plane - int ring_idx, sctor_idx; - for (int idx = 0; idx < _scan_down.rows(); ++idx) - { - const auto &point = _scan_down.row(idx); - const auto &ptx = point(0); - const auto &pty = point(1); - const auto &ptz = point(2); - - // xyz to ring, sector - azim_range = sqrt(ptx * ptx + pty * pty); - azim_angle = xy2theta(ptx, pty); - - // if range is out of roi, pass - if( azim_range > PC_MAX_RADIUS ) - continue; - - ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); - sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); - - // taking maximum z - if ( desc(ring_idx-1, sctor_idx-1) < ptz ) // -1 means cpp starts from 0 - desc(ring_idx-1, sctor_idx-1) = ptz; // update for taking maximum value at that bin - } - - // reset no points to zero (for cosine dist later) - for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) - for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) - if( desc(row_idx, col_idx) == NO_POINT ) - desc(row_idx, col_idx) = 0; - - return desc; + const int NO_POINT = -1000; + MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); + + double azim_angle, azim_range; // wihtin 2d plane + int ring_idx, sctor_idx; + for (int idx = 0; idx < _scan_down.rows(); ++idx) + { + const auto &point = _scan_down.row(idx); + const auto &ptx = point(0); + const auto &pty = point(1); + const auto &ptz = point(2); + + // xyz to ring, sector + azim_range = sqrt(ptx * ptx + pty * pty); + azim_angle = xy2theta(ptx, pty); + + // if range is out of roi, pass + if (azim_range > PC_MAX_RADIUS) + continue; + + ring_idx = std::max(std::min(PC_NUM_RING, int(ceil((azim_range / PC_MAX_RADIUS) * PC_NUM_RING))), 1); + sctor_idx = std::max(std::min(PC_NUM_SECTOR, int(ceil((azim_angle / 360.0) * PC_NUM_SECTOR))), 1); + + // taking maximum z + if (desc(ring_idx - 1, sctor_idx - 1) < ptz) // -1 means cpp starts from 0 + desc(ring_idx - 1, sctor_idx - 1) = ptz; // update for taking maximum value at that bin + } + + // reset no points to zero (for cosine dist later) + for (int row_idx = 0; row_idx < desc.rows(); row_idx++) + for (int col_idx = 0; col_idx < desc.cols(); col_idx++) + if (desc(row_idx, col_idx) == NO_POINT) + desc(row_idx, col_idx) = 0; + + return desc; } // SCManager::makeScancontext - -MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) +MatrixXd SCManager::makeRingkeyFromScancontext(Eigen::MatrixXd &_desc) { - /* - * summary: rowwise mean vector - */ - Eigen::MatrixXd invariant_key(_desc.rows(), 1); - for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) - { - Eigen::MatrixXd curr_row = _desc.row(row_idx); - invariant_key(row_idx, 0) = curr_row.mean(); - } - - return invariant_key; + /* + * summary: rowwise mean vector + */ + Eigen::MatrixXd invariant_key(_desc.rows(), 1); + for (int row_idx = 0; row_idx < _desc.rows(); row_idx++) + { + Eigen::MatrixXd curr_row = _desc.row(row_idx); + invariant_key(row_idx, 0) = curr_row.mean(); + } + + return invariant_key; } // SCManager::makeRingkeyFromScancontext - -MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) +MatrixXd SCManager::makeSectorkeyFromScancontext(Eigen::MatrixXd &_desc) { - /* - * summary: columnwise mean vector - */ - Eigen::MatrixXd variant_key(1, _desc.cols()); - for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) - { - Eigen::MatrixXd curr_col = _desc.col(col_idx); - variant_key(0, col_idx) = curr_col.mean(); - } - - return variant_key; + /* + * summary: columnwise mean vector + */ + Eigen::MatrixXd variant_key(1, _desc.cols()); + for (int col_idx = 0; col_idx < _desc.cols(); col_idx++) + { + Eigen::MatrixXd curr_col = _desc.col(col_idx); + variant_key(0, col_idx) = curr_col.mean(); + } + + return variant_key; } // SCManager::makeSectorkeyFromScancontext - -void SCManager::saveScancontextAndKeys(Eigen::MatrixXd& sc) +void SCManager::saveScancontextAndKeys(Eigen::MatrixXd &sc) { - Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); - Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); - std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); - - polarcontexts_.push_back( sc ); - polarcontext_invkeys_.push_back( ringkey ); - polarcontext_vkeys_.push_back( sectorkey ); - polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); -} + Eigen::MatrixXd ringkey = makeRingkeyFromScancontext(sc); + Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext(sc); + std::vector polarcontext_invkey_vec = eig2stdvec(ringkey); + + polarcontexts_.push_back(sc); + polarcontext_invkeys_.push_back(ringkey); + polarcontext_vkeys_.push_back(sectorkey); + polarcontext_invkeys_mat_.push_back(polarcontext_invkey_vec); +} -void SCManager::makeAndSaveScancontextAndKeys(Eigen::MatrixX3d& _scan_down) +void SCManager::makeAndSaveScancontextAndKeys(Eigen::MatrixX3d &_scan_down) { - Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 - saveScancontextAndKeys(sc); -} + Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 + saveScancontextAndKeys(sc); +} -void SCManager::constructTree(void) +void SCManager::constructTree(void) { - polarcontext_invkeys_to_search_.clear(); - polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; + polarcontext_invkeys_to_search_.clear(); + polarcontext_invkeys_to_search_.assign(polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT); - polarcontext_tree_.reset(); - polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); + polarcontext_tree_.reset(); + polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */); } -std::tuple SCManager::detectLoopClosureID ( void ) +std::tuple SCManager::detectLoopClosureID(void) { - int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") - - auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) - auto curr_desc = polarcontexts_.back(); // current observation (query) - - // step 1: candidates from ringkey tree_ - if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) + int loop_id{-1}; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") + + auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) + auto curr_desc = polarcontexts_.back(); // current observation (query) + + // step 1: candidates from ringkey tree_ + if (polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) + { + std::tuple result{loop_id, 0.0, 0.0}; + return result; // Early return + } + + // tree_ reconstruction (not mandatory to make everytime) + if (tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost + { + constructTree(); + } + tree_making_period_conter = tree_making_period_conter + 1; + + double min_dist = 10000000; // init with somthing large + int nn_align = 0; + int nn_idx = 0; + + // knn search + std::vector candidate_indexes(NUM_CANDIDATES_FROM_TREE); + std::vector out_dists_sqr(NUM_CANDIDATES_FROM_TREE); + + nanoflann::KNNResultSet knnsearch_result(NUM_CANDIDATES_FROM_TREE); + knnsearch_result.init(&candidate_indexes[0], &out_dists_sqr[0]); + polarcontext_tree_->index->findNeighbors(knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10)); + + // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) + for (int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++) + { + MatrixXd polarcontext_candidate = polarcontexts_[candidate_indexes[candidate_iter_idx]]; + std::pair sc_dist_result = distanceBtnScanContext(curr_desc, polarcontext_candidate); + + double candidate_dist = sc_dist_result.first; + int candidate_align = sc_dist_result.second; + + if (candidate_dist < min_dist) { - std::tuple result {loop_id, 0.0, 0.0}; - return result; // Early return - } + min_dist = candidate_dist; + nn_align = candidate_align; - // tree_ reconstruction (not mandatory to make everytime) - if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost - { - constructTree(); + nn_idx = candidate_indexes[candidate_iter_idx]; } - tree_making_period_conter = tree_making_period_conter + 1; - - double min_dist = 10000000; // init with somthing large - int nn_align = 0; - int nn_idx = 0; - - // knn search - std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); - std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); - - nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); - knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); - polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); - - // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) - for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) - { - MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; - std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); - - double candidate_dist = sc_dist_result.first; - int candidate_align = sc_dist_result.second; - - if( candidate_dist < min_dist ) - { - min_dist = candidate_dist; - nn_align = candidate_align; + } - nn_idx = candidate_indexes[candidate_iter_idx]; + /* + * loop threshold check + */ + if (min_dist < SC_DIST_THRES) + { + loop_id = nn_idx; + } - loop_id = nn_idx; - } - } - - double yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); - std::tuple result {loop_id, min_dist, yaw_diff_rad}; + double yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); + std::tuple result{loop_id, min_dist, yaw_diff_rad}; - return result; -} + return result; +}