Initial commit

This commit is contained in:
Hydra 2021-10-19 08:52:28 +08:00
parent 81a2b1cbd9
commit fe51ac42df
129 changed files with 12540 additions and 27 deletions

View File

@ -1,40 +1,34 @@
# scan context
# 基于scan context和CNN的重定位研究
# Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map
#1-Day Learning, 1-Year Localization: Long-Term LiDAR Localization Using Scan Context Image
**2018 IROS Giseop Kim and Ayoung Kim**
2019 ICRA
## Background
##SCI localization framework
- 回环检测(场景识别)=场景描述+搜索
- 3D点云缺乏色彩信息纹理信息等无法提取出传统的图像所特有的特征ORBSIFT等
- 如果不对点云数据进行预处理的话,就只能进行几何匹配,消耗较高
![](1.png)
## challenge
##SCI
- 降维的形式,尽可能多的保留深度信息
- 描述符的编码
- 相似度打分
通过实验验证了与单通道图像训练相比的一点改 进。提出的sci比sc具有更高的分辨能力是一种更适合cnn输入的格式。这个过程如图所示。我们注意到进一步研究单色图像或彩色地图选择的网络调谐可以提高定位性能。
## Framework
![](2.png)
![](http://www.write-bug.com/myres/static/uploads/2021/10/19/8a17e34d3f6faf54ea8c0e47e6ba9172.writebug)
Matlab使用colormap Jet 可以将灰度图像生成彩色的热度图,灰度值越高,色彩偏向暖色调。相反亦然。
## scan-context
![](3.png)
##CNN选择
cnn网络
![](4.png)
输入:
1.N-way SCI Augmentation
2.热编码向量指示类别
输出:
scorevector
将点云分为环形的一块一块,每一块的数值就是这一块点云海拔最高值。这样就实现了降维。
![](http://www.write-bug.com/myres/static/uploads/2021/10/19/599a52d6cffd7c2004f900720e2cc849.writebug)
![](5.png)
## Similarity Score between Scan Contexts
##Unseen place
由于雷达视角的不同,即当雷达在同一地点纯转动了一定角度之后,列向量向量值不变,但是会出现偏移;行向量的行为是向量中元素的顺序会发生改变,但是行向量不会发生偏移。采用列向比较。
![](http://www.write-bug.com/myres/static/uploads/2021/10/19/8d44cf044dc2a086b4d8f318b96bdf9d.writebug)
## Two-phase Search Algorithm
- 利用ring key 构造KD—Tree后最近邻检索
![](http://www.write-bug.com/myres/static/uploads/2021/10/19/5d858d0b4b53d0163f0833203c678591.writebug)
- 相似度评分
- 找到闭环对应帧后使用ICP
![](6.png)

92
src2/README.md Normal file
View File

@ -0,0 +1,92 @@
<!-- md preview: Show the rendered HTML markdown to the right of the current editor using ctrl-shift-m.-->
# Scan Context
## NEWS (Nov, 2020): integrated with LIO-SAM
- A Scan Context integration for LIO-SAM, named [SC-LIO-SAM (link)](https://github.com/gisbi-kim/SC-LIO-SAM), is also released.
## NEWS (Oct, 2020): Radar Scan Context
- An evaluation code for radar place recognition (a.k.a. Radar Scan Context) is uploaded.
- please see the *fast_evaluator_radar* directory.
## NEWS (April, 2020): C++ implementation
- C++ implementation released!
- See the directory `cpp/module/Scancontext`
- Features
- Light-weight: a single header and cpp file named "Scancontext.h" and "Scancontext.cpp"
- Our module has KDtree and we used <a href="https://github.com/jlblancoc/nanoflann"> nanoflann</a>. nanoflann is an also single-header-program and that file is in our directory.
- Easy to use: A user just remembers and uses only two API functions; `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`.
- Fast: tested the loop detector runs at 10-15Hz (for 20 x 60 size, 10 candidates)
- Example: Real-time LiDAR SLAM
- We integrated the C++ implementation within the recent popular LiDAR odometry code, <a href="https://github.com/RobustFieldAutonomyLab/LeGO-LOAM"> LeGO-LOAM </a>.
- That is, LiDAR SLAM = LiDAR Odometry (LeGO-LOAM) + Loop detection (Scan Context) and closure (GTSAM)
- For details, see `cpp/example/lidar_slam` or refer this <a href="https://github.com/irapkaist/SC-LeGO-LOAM"> repository (SC-LeGO-LOAM)</a>.
---
- Scan Context is a global descriptor for LiDAR point cloud, which is proposed in this paper and details are easily summarized in this <a href="https://www.youtube.com/watch?v=_etNafgQXoY"> video </a>.
```
@INPROCEEDINGS { gkim-2018-iros,
author = {Kim, Giseop and Kim, Ayoung},
title = { Scan Context: Egocentric Spatial Descriptor for Place Recognition within {3D} Point Cloud Map },
booktitle = { Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems },
year = { 2018 },
month = { Oct. },
address = { Madrid }
}
```
- This point cloud descriptor is used for place retrieval problem such as place
recognition and long-term localization.
## What is Scan Context?
- Scan Context is a global descriptor for LiDAR point cloud, which is especially designed for a sparse and noisy point cloud acquired in outdoor environment.
- It encodes egocentric visible information as below:
<p align="center"><img src="example/basic/scmaking.gif" width=400></p>
- A user can vary the resolution of a Scan Context. Below is the example of Scan Contexts' various resolutions for the same point cloud.
<p align="center"><img src="example/basic/various_res.png" width=300></p>
## How to use?: example cases
- The structure of this repository is composed of 3 example use cases.
- Most of the codes are written in Matlab.
- A directory _matlab_ contains main functions including Scan Context generation and the distance function.
- A directory _example_ contains a full example code for a few applications. We provide a total 3 examples.
1. _**basics**_ contains a literally basic codes such as generation and can be a start point to understand Scan Context.
2. _**place recognition**_ is an example directory for our IROS18 paper. The example is conducted using KITTI sequence 00 and PlaceRecognizer.m is the main code. You can easily grasp the full pipeline of Scan Context-based place recognition via watching and following the PlaceRecognizer.m code. Our Scan Context-based place recognition system consists of two steps; description and search. The search step is then composed of two hierarchical stages (1. ring key-based KD tree for fast candidate proposal, 2. candidate to query pairwise comparison-based nearest search). We note that our coarse yaw aligning-based pairwise distance enables reverse-revisit detection well, unlike others. The pipeline is below.
<p align="center"><img src="example/place_recognition/sc_pipeline.png" width=600></p>
3. _**long-term localization**_ is an example directory for our RAL19 paper. For the separation of mapping and localization, there are separated train and test steps. The main training and test codes are written in python and Keras, only excluding data generation and performance evaluation codes (they are written in Matlab), and those python codes are provided using jupyter notebook. We note that some path may not directly work for your environment but the evaluation codes (e.g., makeDataForPRcurveForSCIresult.m) will help you understand how this classification-based SCI-localization system works. The figure below depicts our long-term localization pipeline. <p align="center"><img src="example/longterm_localization/sci_pipeline.png" width=600></p> More details of our long-term localization pipeline is found in the below paper and we also recommend you to watch this <a href="https://www.youtube.com/watch?v=apmmduXTnaE"> video </a>.
```
@ARTICLE{ gkim-2019-ral,
author = {G. {Kim} and B. {Park} and A. {Kim}},
journal = {IEEE Robotics and Automation Letters},
title = {1-Day Learning, 1-Year Localization: Long-Term LiDAR Localization Using Scan Context Image},
year = {2019},
volume = {4},
number = {2},
pages = {1948-1955},
month = {April}
}
```
4. _**SLAM**_ directory contains the practical use case of Scan Context for SLAM pipeline. The details are maintained in the related other repository _[PyICP SLAM](https://github.com/kissb2/PyICP-SLAM)_; the full-python LiDAR SLAM codes using Scan Context as a loop detector.
## Acknowledgment
This work is supported by the Korea Agency for Infrastructure Technology Advancement (KAIA) grant funded by the Ministry of Land, Infrastructure and Transport of Korea (19CTAP-C142170-02), and [High-Definition Map Based Precise Vehicle Localization Using Cameras and LIDARs] project funded by NAVER LABS Corporation.
## Contact
If you have any questions, contact here please
```
paulgkim@kaist.ac.kr
```
## License
<a rel="license" href="http://creativecommons.org/licenses/by-nc-sa/4.0/"><img alt="Creative Commons License" style="border-width:0" src="https://i.creativecommons.org/l/by-nc-sa/4.0/88x31.png" /></a><br />This work is licensed under a <a rel="license" href="http://creativecommons.org/licenses/by-nc-sa/4.0/">Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License</a>.
### Copyright
- All codes on this page are copyrighted by KAIST and Naver Labs and published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. You must attribute the work in the manner specified by the author. You may not use the work for commercial purposes, and you may only distribute the resulting work under the same license if you alter, transform, or create the work.

View File

@ -0,0 +1,2 @@
# Go to
- https://github.com/irapkaist/SC-LeGO-LOAM

View File

@ -0,0 +1,117 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#pragma once
#include <nanoflann.hpp>
#include <vector>
// ===== This example shows how to use nanoflann with these types of containers: =======
//typedef std::vector<std::vector<double> > my_vector_of_vectors_t;
//typedef std::vector<Eigen::VectorXd> my_vector_of_vectors_t; // This requires #include <Eigen/Dense>
// =====================================================================================
/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
* The i'th vector represents a point in the state space.
*
* \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
* \tparam num_t The type of the point coordinates (typically, double or float).
* \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
* \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
*/
template <class VectorOfVectorsType, typename num_t = double, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
struct KDTreeVectorOfVectorsAdaptor
{
typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType,num_t,DIM,Distance> self_t;
typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
/// Constructor: takes a const ref to the vector of vectors object with the data points
KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
{
assert(mat.size() != 0 && mat[0].size() != 0);
const size_t dims = mat[0].size();
if (DIM>0 && static_cast<int>(dims) != DIM)
throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
index = new index_t( static_cast<int>(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
index->buildIndex();
}
~KDTreeVectorOfVectorsAdaptor() {
delete index;
}
const VectorOfVectorsType &m_data;
/** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
* Note that this is a short-cut method for index->findNeighbors().
* The user can also call index->... methods as desired.
* \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
*/
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
{
nanoflann::KNNResultSet<num_t,IndexType> resultSet(num_closest);
resultSet.init(out_indices, out_distances_sq);
index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
}
/** @name Interface expected by KDTreeSingleIndexAdaptor
* @{ */
const self_t & derived() const {
return *this;
}
self_t & derived() {
return *this;
}
// Must return the number of data points
inline size_t kdtree_get_point_count() const {
return m_data.size();
}
// Returns the dim'th component of the idx'th point in the class:
inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
return m_data[idx][dim];
}
// Optional bounding-box computation: return false to default to a standard bbox computation loop.
// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
template <class BBOX>
bool kdtree_get_bbox(BBOX & /*bb*/) const {
return false;
}
/** @} */
}; // end of KDTreeVectorOfVectorsAdaptor

View File

@ -0,0 +1,340 @@
#include "Scancontext.h"
// namespace SC2
// {
void coreImportTest (void)
{
cout << "scancontext lib is successfully imported." << endl;
} // coreImportTest
float rad2deg(float radians)
{
return radians * 180.0 / M_PI;
}
float deg2rad(float degrees)
{
return degrees * M_PI / 180.0;
}
float xy2theta( const float & _x, const float & _y )
{
if ( _x >= 0 & _y >= 0)
return (180/M_PI) * atan(_y / _x);
if ( _x < 0 & _y >= 0)
return 180 - ( (180/M_PI) * atan(_y / (-_x)) );
if ( _x < 0 & _y < 0)
return 180 + ( (180/M_PI) * atan(_y / _x) );
if ( _x >= 0 & _y < 0)
return 360 - ( (180/M_PI) * atan((-_y) / _x) );
} // xy2theta
MatrixXd circshift( MatrixXd &_mat, int _num_shift )
{
// shift columns to right direction
assert(_num_shift >= 0);
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);
}
return shifted_mat;
} // circshift
std::vector<float> eig2stdvec( MatrixXd _eigmat )
{
std::vector<float> vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
return vec;
} // eig2stdvec
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.
double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());
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;
} // 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);
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;
}
}
return argmin_vkey_shift;
} // fastAlignUsingVkey
std::pair<double, int> 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<int> 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 )
{
argmin_shift = num_shift;
min_sc_dist = cur_sc_dist;
}
}
return make_pair(min_sc_dist, argmin_shift);
} // distanceBtnScanContext
MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
TicToc t_making_desc;
int num_pts_scan_down = _scan_down.points.size();
// main
const int NO_POINT = -1000;
MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
SCPointType pt;
float azim_angle, azim_range; // wihtin 2d plane
int ring_idx, sctor_idx;
for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
{
pt.x = _scan_down.points[pt_idx].x;
pt.y = _scan_down.points[pt_idx].y;
pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
// xyz to ring, sector
azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
azim_angle = xy2theta(pt.x, pt.y);
// 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) < pt.z ) // -1 means cpp starts from 0
desc(ring_idx-1, sctor_idx-1) = pt.z; // 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;
t_making_desc.toc("PolarContext making");
return desc;
} // SCManager::makeScancontext
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;
} // SCManager::makeRingkeyFromScancontext
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;
} // SCManager::makeSectorkeyFromScancontext
void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
std::vector<float> 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 );
// cout <<polarcontext_vkeys_.size() << endl;
} // SCManager::makeAndSaveScancontextAndKeys
std::pair<int, float> 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)
{
std::pair<int, float> result {loop_id, 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
{
TicToc t_tree_construction;
polarcontext_invkeys_to_search_.clear();
polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;