Scan Context 学习记录

Scan Context 学习记录

知乎上看到一篇有关scan context的文章,感觉内容不错,便进行了学习与转载,文章链接附在最后

scan context 是一篇论文中提出的,通过激光点云做场景识别或者定位,当然也可以用来做闭环检测。定位,通常是在历史帧中找到与搜索帧pose最接近的一帧,当然这只用到了pose。如果用点云去做匹配,找到最相似的那一帧点云,怎么做呢,直接3d-3d匹配是可以的,但是不够快。那么降维,把3D点云变成二维,然后去匹配。scan context的做法就是从3D点云上空俯视,得到一张俯视图,当然图上每个点有高度信息,用这个俯视图去做匹配。

1. 生成scan context

首先看这两张图,两张图一定要结合起来看,包括图的注释和文字。
在这里插入图片描述
在这里插入图片描述
第一幅图是一帧点云的俯视图。黄色的一圈叫做ring(环),从中心点开始,有一个很小的环,慢慢往外扩大,最大到80米的距离,一层一层的环加起来就是整个点云了,一共20个环。青色的那个径向轴叫做sector(扇形),那么很显然这个径向轴划分成了20段,每段对应一个环。一共有60个sector,也就是把360°的ring划分成了60份。

const int    PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
const int    PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)

那么整个点云就可以用20*60=1200个bin(格子),一个二维矩阵来表示。那么每个格子的值是什么呢?是落在这个格子里面所有点的最大高度(z值)。对照第二幅图像看一下,把俯视的圆环图剪开,变成矩形,横向对应一个环,纵向对应距离从近到远。不同的颜色,表示这个格子中点的最大高度。想象成一个带高度的俯视图,或者地形图。

// scan context是一个二维矩阵
MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);

上面就把一帧点云用二维的形式表达了,称之为scan context。下面就是算法的第一个接口,根据一帧点云,生成scan context。

// 接口一,一帧点云生成scan context,计算ring key, sector key
void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
    // scancontext 带高度的俯视图
    Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 
    // 每个环ring的最大高度平均值
    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
    // 每个轴向sector的最大高度平均值
    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
    // 换成数组,这玩意作为scancontext的key,也就是在历史帧里面通过找相同的key来得到候选匹配,然后计算scan context的距离
    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 );

} 

// 一帧点云生成scan context
MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
    // 激光点数量
    int num_pts_scan_down = _scan_down.points.size();

    const int NO_POINT = -1000;
    // 这里矩阵的记法跟paper的示意图是一致的
    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;
        // 让高度大于0,所有点的高度都加2,不影响匹配结果
        pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).

        // 距离
        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
        // 角度,0~360°
        azim_angle = xy2theta(pt.x, pt.y);

        // 距离超过80米的点不考虑了
        if( azim_range > PC_MAX_RADIUS )
            continue;

        // 计算这个点落在哪个bin里面,下标从1开始数
        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 );

        // 用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;

    return desc;
} 

2. 用scan context 搜索匹配

用scan context来表达一帧点云之后,要做的就是用scan context在历史帧里面找到最相近的scan context。为了加速查找,通常都是把历史帧数据构建kd-tree,直接用scan context消耗计算机内存较大(主要是相似度计算耗时)。进一步的,把scan context再降维,得到一个ring key(向量),用这个去构造kd-tree,以及搜索。ring key是scan context中每一个ring的最大高度的均值,然后组合在一起,组成20*1的矩阵。直观上ring key就是20个环的最大高度的均值所组合在一起的矩阵。可以想到ring key跟scan context是一对多的关系。但是没关系,通过ring key做一个初步筛选,找到很多候选scan context,然后再精细比较scan context得到最后的结果。

// 由scan context计算ring key,Nx1
MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    // 每行计算一个均值
    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;
} 

通过上面这个ring key在kd-tree里面找到了最邻近的几帧,有对应的scan context,接下来就要用当前帧的scan context与这些scan context进行比较,计算距离。这里呢,又要引入一个sector key,与ring key对应,这里每列计算一个均值,组成一个1*60的矩阵。

// 由scan context计算sector key,1xM
MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
{
    // 每列计算一个均值
    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;
} 

为什么要按列计算均值呢?因为在场景中同一个位置,旋转一下,看向不同的方向,得到scan context是不一样的,对应的scan context上的表现就是矩阵左右偏移了。如果直接用scan context去比,显然就认为两帧是不匹配的,但是实际上是在同一个位置。所以就要对scan context左右循环偏移,找一个最佳的匹配位置,然后用偏移后的scan context去作比较。那么实际上用sector key去计算偏移量,然后施加到scan context上,最后用偏移后的scan context作比较。

// _vkey1、_vkey2是两个sector key
// 对_vkey2做循环偏移,计算与_vkey1最佳匹配时的偏移量
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++ )
    {
        // 矩阵的列,循环右移shift个单位
        MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
        // 直接相减,sector key是1xN的矩阵
        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;
} 

第二个接口,闭环检测

// 接口二,执行闭环检测
std::pair<int, float> SCManager::detectLoopClosureID ( void )/**
 * 计算两个scan context之间的距离
*/
std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
{
    // 1. fast align using variant key (not in original IROS18)
    // 计算sector Key,也就是sector最大高度均值组成的数组,1xN
    MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
    MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
    // 这里将_vkey2循环右移,然后跟_vkey1作比较,找到一个最相似(二者做差最小)的时候,记下循环右移的量
    int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );

    // 上面用sector key匹配,找到一个初始的偏移量,但肯定不是准确的,再在这个偏移量左右扩展一下搜索空间
    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 )
    {
        // 把scan context循环右移一下
        MatrixXd sc2_shifted = circshift(_sc2, num_shift);
        // 计算两个scan context之间的距离
        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;
        }
    }
    // 最小scan context距离,偏移量
    return make_pair(min_sc_dist, argmin_shift);
}

小结:

  1. 给定一帧点云,划分成20个环,每个环分成60等份,一共1200个格子
  2. 每个格子存里面点的最大高度值(z值),这样一帧点云就用一个二维图像表示了,想象成一个带高度的俯视图,或者地形图,记为scan context
  3. scan context进一步计算列的均值,得到一个1x60的向量,记为ring key;计算行的均值,得到一个20x1的向量,记为sector key
  4. 用ring key构造kd-tree,并且执行knn搜索
  5. 对于候选匹配scan context,首先要左右循环偏移一下,对齐,实际会用sector key去对齐,得到一个偏移量
  6. 对候选匹配scan context,施加偏移量,然后作比较

文章链接:https://zhuanlan.zhihu.com/p/359523177

  • 13
    点赞
  • 61
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
### 回答1: 这个错误提示意味着无法扫描默认的文档上下文。这通常发生在文档生成工具(如Sphinx)中,因为它无法找到文档的相关上下文信息。 要解决这个问题,你可以尝试以下几个步骤: 1. 确认文档是否存在并且位于正确的位置。 2. 确认文档格式是否正确。例如,在Sphinx中,文档应该使用reStructuredText格式。 3. 确认文档是否已经正确配置,包括正确设置文档上下文信息。 4. 尝试重新生成文档并检查错误是否已经解决。 5. 如果以上步骤都失败了,你可以考虑搜索相关的错误解决方案或者向相应的文档工具社区或论坛提问寻求帮助。 希望这些步骤能帮助你解决这个问题! ### 回答2: 这个错误信息说明出现了扫描文档上下文默认值的错误。在机器学习和自然语言处理中,文档扫描是非常常见的操作,它可以帮助我们了解文档中的关键词、语义和主题等信息。然而,当出现“unable to scan documentation context default”的错误信息时,可能会导致我们无法完成扫描。 这个错误信息通常与文档扫描代码中的某些问题有关,可能是在代码中指定了无效的文档上下文参数或没有正确安装依赖包等。为了解决这个问题,以下是一些可能的解决办法: 1. 检查参数是否正确:检查文档扫描代码中指定的文档上下文参数是否正确,如确保指定了正确的文件路径、文档编码、语言等,如果有任何问题,请进行更正。 2. 安装必要的依赖包:确保所需的文档扫描库已经正确地安装在你的系统中,如nltk、pandas等。如果尚未安装,请使用pip等工具安装它们。 3. 确认是否有足够的资源:确保你的计算机具有足够的内存和计算资源来完成文档扫描任务,尤其是对于大规模的文本数据集。如果你的计算机资源较少,可以尝试使用分布式计算方法来处理任务。 4. 检查运行日志:如有任何问题,请检查代码运行日志以了解更多信息,通常代码中会提供一些错误提示和排错信息。 总之,无法扫描文档上下文默认值这个错误信息可能与文档扫描代码的某些问题有关,如果你按照上述解决方案进行了尝试,但仍然无法解决这个问题,请参考代码文档和在线资源,或与相关的开发人员进行联系以获得更多的帮助。 ### 回答3: “Unable to scan documentation context default”是指在使用某些程序时,出现无法扫描默认文档上下文的错误提示。通常情况下,这可能是因为程序无法找到或访问相应的文档或文档路径遭到损坏或更改所导致的。 在解决这个问题时,首先需要确认程序是否已正确安装并已启动。如果程序已安装但无法正常启动,则需要检查是否存在已知的故障或其他问题。可以在程序官网或相关社区社区寻找其他用户的帮助或建议。如果这些方法无法解决问题,则需要检查程序的配置文件,以确定文档路径是否正确。 此外,也可以尝试重新安装程序或更新程序到最新版本,以确保不会发生版本不兼容或其他问题。如果一切正常并且程序仍无法扫描文档,则可以考虑使用其他工具来替代。例如,在文档处理方面,可以尝试使用Microsoft Office或Google Docs等替代工具,而在代码编辑方面,可以尝试使用Visual Studio Code或Atom等其他软件。 总之,“Unable to scan documentation context default”问题可能由多种原因引起,但可以通过检查程序是否正确安装、配置和更新版本来解决。同样,也建议在使用其他工具时保持灵活,以确保能够在遇到问题时快速切换到替代工具。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值