开贴记录一下学习与应用scan context的经验。（由于苦于寻找segmatch的odom，然后用loam做odom重定位效果并不是很理想，遂转战sacn context。）

一切不以应用为目的经验贴都是耍流氓

其应用比较简单，只需在函数中加入

      SCManager scManager;//初始化
      auto 结果 = scManager.detectLoopClosureID();//返回两个值
      scManager.makeAndSaveScancontextAndKeys(*点云);//其中点云为pcl::PointCloud<pcl::PointXYZI> & cloud_name
对于其两个返回值有
      SCclosestHistoryFrameID = 结果.first;//第一个值是帧ID
      yawDiffRad = 结果.second; //第二个值是航向偏差 由序号得出将360度分为60份
2.源码解析
float rad2deg(float radians)
float deg2rad(float degrees)
float xy2theta( const float & _x, const float & _y )// xy2theta
MatrixXd circshift( MatrixXd &_mat, int _num_shift )//将mat阵行右移得到新矩阵num shift
    // shift columns to right direction 将列右移
    assert(_num_shift >= 0);//如果表达式为FALSE (0), 程序将报告错误，并终止执行。如果表达式不为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();//行右移输入量num shift
        shifted_mat.col(new_location) = _mat.col(col_idx);//shifted新矩阵赋值
    return shifted_mat;
} // circshift
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. 0，0的时候不计算
        double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());//点积/叉积 = cos
        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;//减去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).雷达自身高2.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//更新Z最小值
    // 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;//若没有点则置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();//均值 行 //16线
    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(); //求均值 列 //360°
    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)//  线号size <51 从0到50 
        std::pair<int, float> result {loop_id, 0.0}; //构造result
        return result; // Early return
    // tree_ reconstruction (not mandatory to make everytime)
    if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost // %50
        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 ) ;//用于拷贝、赋值操作
        polarcontext_tree_.reset(); //智能指针 kdtree适应
        polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );//赋值
        // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
        t_tree_construction.toc("Tree construction");
    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<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE );
    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
    TicToc t_tree_search;
    nanoflann::KNNResultSet<float> 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) ); 
    t_tree_search.toc("Tree search");
     *  step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
    TicToc t_calc_dist;   
    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<double, int> 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];// 更新
    t_calc_dist.toc("Distance calc");
     * loop threshold check
    if( min_dist < SC_DIST_THRES )
        loop_id = nn_idx; 
        // std::cout.precision(3); 
        cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
        cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
        std::cout.precision(3); 
        cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
        cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
    // To do: return also nn_align (i.e., yaw diff)
    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
    std::pair<int, float> result {loop_id, yaw_diff_rad};
    return result;
} // SCManager::detectLoopClosureID
// } // namespace SC2
3.应用开发
pcd文件需要分割然后进行scancontext特征的抽取
对PCD文件数据进行解析，希望找到跳变沿
4037 1442 712 4227 4437 6097 8724 14062在这些地方发生了20以上的跳变或者增减顺序的跳变（-20增加到50 突然变成-20或者变为20），并无任何顺序
后面准备在建图时将scancontext信息保存下来，用来做后续的全局定位
后续准备用其做全局定位/重定位