karto算法论文阅读及代码讲解-扫描匹配（correlative scan matching,csm）

原创

已于 2022-03-29 23:07:36 修改 · 5.7k 阅读

46 ·

CC 4.0 BY-SA版权

文章标签：

#人工智能

于 2022-03-21 13:44:47 首次发布

本文详细解读了csm（Correlative Scan Matching）中的位姿得分原理，以及在Karto框架下的具体实现，包括滑动窗口生成局部地图、多分辨率概率查询表的应用，以及角度和位置协方差的计算过程。

扫描匹配算法csm，correlative scan matching

1.csm理论基础-位姿得分
2.csm在karto算法中实现前端匹配

这篇博客首先对csm中位姿得分进行介绍，接着基于karto算法对csm进行整体介绍，同时介绍其中csm的各个要点。

1.csm理论基础-位姿得分

在这里插入图片描述

图2 概率扫描匹配的图形模型

帧间匹配可以理解成一个：已知x_𝑖-1 和m，也测量到了u 和 z_𝑖 ，如何尽可能准确的求出x𝑖 ？

对上述公式两侧取对数，让乘法变加法，我们有：
在这里插入图片描述

公式（3）是我们的观测模型，我们通过下面的思路对其进行求解：

根据前面的观测 m（或若干帧）建立出来的概率栅格地图！
按照位姿xi把当前观测 z_i投影到 m 中，把所有被击中的栅格的概率值相加，就是！在栅格地图的基础上，公式(3) 的求解。所得的即是位姿的得分，代表着在这个位姿下，当前观测与已知环境相一致的程度（i.e., 相关性）。得分越高，表明这个位姿越靠谱。

通过上面的介绍，我们可以总结出CSM的主要思想：
CSM帧匹配算法基于概率栅格地图运行，每个栅格都维护一个对数形式的占据概率；对于新进来的激光scan，将scan中所有点通过一个预估位姿投影到栅格地图上，这样每个激光点都会落到一个栅格中，激光点所在栅格的对数概率值之和既为当前位姿的得分，代表着这个位姿的可信度。但是预估位姿是不准确的，我们在预估位姿附近建立一个搜索空间，通过分支定界策略加速搜索，求出得分最高的备选位姿，作为最优结果输出。

2.csm在karto算法中实现前端匹配

这个章节对csm在karto中的实现进行介绍。首先给出karto中帧间匹配算法的主要步骤：
1）获取先验位姿，通过TF获取里程计的值，作为当前scan的预测位姿，将这个预测位姿当做扫描匹配的先验。
2）使用滑动窗口法来生成局部地图；Karto使用了一个队列保存最近24米范围内的所有雷达数据，通过将滑动窗口内的所有雷达数据写成分辨率为0.01m的栅格地图，这样就生成了局部地图。
3）通过暴力求解的方式，遍历一定范围内所有的平移与旋转的位姿，选出得分最大的一个位姿。（扫描匹配）加速方式：

生成角度查找表，实现暴力求解内部的两个循环(对于xˆ和yˆ)只是转换查询点。
多分辨率概率查询表，先粗匹配计算位置协方差，输出坐标均值，再次进行精匹配，计算角度协方差。

4）位姿的得分：位姿的得分是通过雷达点对应于栅格地图中的格子的占用值来确定的，所有雷达点对应格子的占用值的总和除以点的个数乘以100。
5）对距离先验位姿远的位姿进行惩罚
6）保存位姿与当前scan

然后对其实现代码进行讲解。

2.1 前端匹配整体框架

MatchScan函数主要完成当前激光pScan与rBaseScans进行扫描匹配，先在预估位姿投影到栅格地图上，计算当前位姿的得分，代表着这个位姿的可信度。由于预估位姿是不准确的，然后在预估位姿附近建立一个搜索空间，通过分支定界策略加速搜索，求出得分最高的备选位姿，作为最优结果输出。

/**//pScan与rBaseScans进行扫描匹配，打到最优位置rMean和对应协方差。
   * Match given scan against set of scans
   * @param pScan scan being scan-matched
   * @param rBaseScans set of scans whose points will mark cells in grid as being occupied
   * @param rMean output parameter of mean (best pose) of match
   * @param rCovariance output parameter of covariance of match
   * @param doPenalize whether to penalize matches further from the search center
   * @param doRefineMatch whether to do finer-grained matching if coarse match is good (default is true)
   * @return strength of response
   */
kt_double ScanMatcher::MatchScan(LocalizedRangeScan *pScan,							//当前帧的scan数据
                                 const LocalizedRangeScanVector &rBaseScans,		//已经接收的scan的数组 map
                                 Pose2 &rMean,
                                 Matrix3 &rCovariance,
                                 kt_bool doPenalize,
                                 kt_bool doRefineMatch)
{
   
   
    ///////////////////////////////////////
    // set scan pose to be center of grid

    // 1. get scan position	,获取当前激光雷达的位置
    Pose2 scanPose = pScan->GetSensorPose();

    // scan has no readings; cannot do scan matching
    // best guess of pose is based off of adjusted odometer reading
    if (pScan->GetNumberOfRangeReadings() == 0)
    {
   
   
        rMean = scanPose;

        // maximum covariance
        rCovariance(0, 0) = MAX_VARIANCE;                                                      // XX
        rCovariance(1, 1) = MAX_VARIANCE;                                                      // YY
        rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH

        return 0.0;
    }

    // 2. get size of grid				网格：24m*24m，0.01，2431*2431个格子
    Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();

    // 3. compute offset (in meters - lower left corner)
    Vector2<kt_double> offset;
    offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
    offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
           
    // 4. set offset
    m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);

    ///////////////////////////////////////

    // set up correlation grid，使用已经接收到的激光数据rBaseScans，生成局部地图
    AddScans(rBaseScans, scanPose.GetPosition());

    // compute how far to search in each direction
	// m_pSearchSpaceProbs 是一个 31 * 31 大小的栅格地图，0. 01,XY=0.15
    Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
    Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
                                          0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());

    // a coarse search only checks half the cells in each dimension
    Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),				//0.02
                                              2 * m_pCorrelationGrid->GetResolution());				//0.02

    // actual scan-matching scan to map的过程
    kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,		//0.02
                                           m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),		//0.349 
                                           m_pMapper->m_pCoarseAngleResolution->GetValue(),			//0.0349 
                                           doPenalize, rMean, rCovariance, false);

    if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
    {
   
   
        if (math::DoubleEqual(bestResponse, 0.0))
        {
   
   
#ifdef KARTO_DEBUG
            std::cout << "Mapper Info: Expanding response search space!" << std::endl;
#endif
            // try and increase search angle offset with 20 degrees and do another match
            kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
            for (kt_int32u i = 0; i < 3; i++)
            {
   
   
                newSearchAngleOffset += math::DegreesToRadians(20);

                bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
                                             newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
                                             doPenalize, rMean, rCovariance, false);

                if (math::DoubleEqual(bestResponse, 0.0) == false)
                {
   
   
                    break;
                }
            }

#ifdef KARTO_DEBUG
            if (math::DoubleEqual(bestResponse, 0.0))
            {
   
   
                std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
            }
#endif
        }
    }

    if (doRefineMatch)
    {
   
   
        Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
        Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
        bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
                                     0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
                                     m_pMapper->m_pFineSearchAngleOffset->GetValue(),
                                     doPenalize, rMean, rCovariance, true);
    }

#ifdef KARTO_DEBUG
    std::cout << "  BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ",  VARIANCE = "
              << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
#endif
    assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));

    return bestResponse;
}

2.2 使用滑动窗口法来生成局部地图

Karto使用了一个队列保存最近24米范围内的所有雷达数据，通过将滑动窗口内的所有雷达数据写成分辨率为0.01的栅格地图，这样就生成了局部地图。

1） AddScans()

首先将 m_pCorrelationGrid 中栅格全置为 0 ；
然后遍历 running scan , 将每一帧雷达数据进行 AddScan()；
确定是否是有效的点，首先判断距离大于0.01，再判断是否是同方向的，应该是按照右手定则判断的，直接按公式求cos得正负；
然后将这些点对应的栅格地图中的点不是占用状态的设置成占用状态，同时，更新当前坐标点周围的占用值, SmearPoint(gridPoint)

/**//遍历runningScan中的第一帧激光雷达，将其中的激光点投影到栅格地图中
   * Marks cells where scans' points hit as being occupied
   * @param rScans scans whose points will mark cells in grid as being occupied
   * @param viewPoint do not add points that belong to scans "opposite" the view point
   */
void ScanMatcher::AddScans(const LocalizedRangeScanVector &rScans, Vector2<kt_double> viewPoint)
{
   
   
    m_pCorrelationGrid->Clear();

    // add all scans to grid
    const_forEach(LocalizedRangeScanVector, &rScans)
    {
   
   
        AddScan(*iter, viewPoint);
    }
}

/**
   * Marks cells where scans' points hit as being occupied.  Can smear points as they are added.
   * @param pScan scan whose points will mark cells in grid as being occupied
   * @param viewPoint do not add points that belong to scans "opposite" the view point
   * @param doSmear whether the points wi

最低0.47元/天解锁文章