一文详解SLAM回环及优化

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨 lovely_yoshino

来源丨古月居

0. 前言

随着路径的不断延伸，机器人的建图过程会存在不断地累计误差。而传统的以gmapping为代表的使用粒子滤波进行定位的slam建图方式。以及ORB-SLAM为代表包含的局部优化和全局优化来调整外。但是这些处理方式只能减缓误差累计的程度，无法消除，而现在最为常用消除累计误差的方法就是利用回环检测来优化位姿。

当新的关键帧加入到优化模型时，在关键帧附近进行一次局部优化。

在全局优化中，所有的关键帧（除了第一帧）和三维点都参与优化。

1. 回环检测

回环检测作为近年来slam行业必备的部分，是指机器人识别曾到达某场景，使得地图闭环的能力。

回环检测之所以能成为一个难点，是因为：如果回环检测成功，可以显著地减小累积误差，帮助机器人更精准、快速的进行避障导航工作。而错误的检测结果可能使地图变得很糟糕。因此，回环检测在大面积、大场景地图构建上是非常有必要的 

词袋模型(bag of words,BoW)早期是一种文本表征方法，后引入到计算机视觉领域，逐渐成为一种很有效的图像特征建模方法。它通过提取图像特征，再将特征进行分类构建视觉字典，然后采用视觉字典中的单词集合可以表征任一幅图像。

换句话说，通过BOW可以把一张图片表示成一个向量。这对判断图像间的关联很有帮助，所以目前比较流行的回环解决方案都是采用的BoW及其基础上衍生的算法IAB-MAP、FAB-MAP是在滤波框架下计算回环概率，RTAB-MAP采用关键帧比较相似性，DLoopDetector（在DBoW2基础上开发的回环检测库）采用连续帧的相似性检测判断是否存在回环。回环检测主要由BoW模块、算法模块、验证模块三部分组成。

词袋模型（BoW模块）

每一帧都可以用单词来描述，也就是这一帧中有哪些单词，这里只关心了有没有，而不必关心具体在哪里。只有两帧中单词种类相近才可能构成回环。

（ａ）图像预处理，假设训练集有Ｍ幅图像，将图像标准化为patch，统一格式和规格;
（ｂ）特征提取，假设Ｍ幅图像，对每一幅图像提取特征，共提取出Ｎ个SIFT特征;
（ｃ）特征聚类，采用K-means算法把N个对象分为K个簇 （视觉单词表），使簇内具有较高的相似度，而簇间相似度较低；

（ｄ）统计得到图像的码本，每幅图像以单词表为规范对该幅图像的每一个SIFT特征点计算它与单词表中每个单词的距 离，最近的加1，便得到该幅图像的码本；还需要码本矢量归一化，因为每一幅图像的SIFT特征个数不定，所以需要归一化。

相似度计算（算法模块）

现在有了字典，但还有一点需要注意。我们利用单词表示图像，目的是发现回环，也就是两帧图像具有较高的相似性。那其实不同的单词对这一目的的贡献性是不同的。例如，我这篇文章中有很多“我们”这个词，但这个词并不能告诉我们太多信息。

而有些单词例如“回环”、“K-means”就比较具有代表性，能大概告诉我们这句话讲了什么。因此不同的单词应该具有不同的权重。

（ａ）贝叶斯估计方法。采用BoW描述机器人每一位置的场景图像，估计已获取图像与对应位置的先验概率，对当前时刻计算该新场景图像与已访问位置匹配的后验概率，概率大于阈值则标记为闭环。

（ｂ）相似性方法。有了字典以后，给定任意特征点f_i，只要在字典树中逐层查找，最后都能找到与之对应的单词w_i。通常字典足够大，可以说它们来自同一类物体。但是这种方法对所有单词都是同样对待，常规的做法是采用TF-IDF(term frequency-inverse document frequency)

IDF（Inverse Document Frequency）：描述单词在字典中出现的频率（构建字典时），越低越具有代表性

n为所有描述子数， n_i为该单词出现次数。ln的作用大概是降低量级，毕竟n很大。

TF（Term Frequency）：单词在单帧图像中出现的频率，越高越具有代表性

n为一帧图像中所有单词数， n_i为一帧图像中该单词出现次数。

因此将一帧图像转化为单词表示时，我们要计算其单词的权重：

因此一帧图像A由单词 w_i，次数n_i以及权重η_i组成。

验证模块（回环处理）

回环的判断也并没有这么简单，含有很多的筛选环节，毕竟错误的回环将带来巨大灾难，宁可不要。例如某一位姿附近连续多次（ORB-SLAM中为3次）与历史中某一位姿附近出现回环才判断为回环；回环候选帧仍然要匹配，匹配点足够才为回环。方式主要有两种：

（ａ）时间一致性。正确的回环往往存在时间上的连续性，所以如果之后一段时间内能用同样的方法找到回环，则认为当前回环是正确的，也叫做顺序一致性约束。
（ｂ）结构一致性校验。对回环检测到的两帧进行特征匹配并估计相机运动，因为各个特征点在空间中的位置是唯一不变的，与之前的估计误差比较大小。

然后下面主要就会涉及如何对找到的回环进行优化了。

2. SLAM优化

闭环纠正

在判断出现回环后，两帧计算Sim3变换（因为有尺度漂移），也就是从历史帧直接推算当前位姿。下面我们以ORB-SLAM2为代表减少形成闭环时的Sim3优化。

单目SLAM一般都会发生尺度(scale)漂移，因此Sim3上的优化是必要的。相对于SE3，Sim3的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量。

//BRIEF 形成闭环时进行Sim3优化//Sim3用于回环检测时计算关键帧与回环帧之间的关系 R 、t、 s//相似变换群Sim3int Optimizer::OptimizeSim3(KeyFrame *pKF1, //关键帧1                            KeyFrame *pKF2, //关键帧2                            vector<MapPoint *> &vpMatches1,//两帧匹配关系                             g2o::Sim3 &g2oS12, //两个关键帧间的Sim3变换                            const float th2, //核函数阈值                            const bool bFixScale//是否进行尺度优化                            ){    //创建优化器    g2o::SparseOptimizer optimizer;    g2o::BlockSolverX::LinearSolverType * linearSolver;    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);    //使用LM算法    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);    optimizer.setAlgorithm(solver);    // Calibration    //得到两帧相机内参    const cv::Mat &K1 = pKF1->mK;    const cv::Mat &K2 = pKF2->mK;    // Camera poses    //得到两帧的旋转与平移位姿    const cv::Mat R1w = pKF1->GetRotation();    const cv::Mat t1w = pKF1->GetTranslation();    const cv::Mat R2w = pKF2->GetRotation();    const cv::Mat t2w = pKF2->GetTranslation();    // Set Sim3 vertex    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();        vSim3->_fix_scale=bFixScale;    //两帧之间的位姿变化为待估计量    vSim3->setEstimate(g2oS12);    vSim3->setId(0);    //不进行固定参数    vSim3->setFixed(false);    //提取相机内参参数    vSim3->_principle_point1[0] = K1.at<float>(0,2);    vSim3->_principle_point1[1] = K1.at<float>(1,2);    vSim3->_focal_length1[0] = K1.at<float>(0,0);    vSim3->_focal_length1[1] = K1.at<float>(1,1);    vSim3->_principle_point2[0] = K2.at<float>(0,2);    vSim3->_principle_point2[1] = K2.at<float>(1,2);    vSim3->_focal_length2[0] = K2.at<float>(0,0);    vSim3->_focal_length2[1] = K2.at<float>(1,1);    //加入图优化顶点    optimizer.addVertex(vSim3);    // Set MapPoint vertices    //建立地图点 顶点    const int N = vpMatches1.size();    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();    //定义两帧的公共地图点    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;    //定义边的尺寸    vector<size_t> vnIndexEdge;    //分配空间    vnIndexEdge.reserve(2*N);    vpEdges12.reserve(2*N);    vpEdges21.reserve(2*N);    const float deltaHuber = sqrt(th2);    int nCorrespondences = 0;    //遍历所有匹配点    for(int i=0; i<N; i++)    {        if(!vpMatches1[i])            continue;        MapPoint* pMP1 = vpMapPoints1[i];        MapPoint* pMP2 = vpMatches1[i];        const int id1 = 2*i+1;        const int id2 = 2*(i+1);        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);        //如果匹配的两帧看到的地图点都存在        if(pMP1 && pMP2)        {            //判断是不是好点            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)            {                //建立优化器 优化位姿1                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();                //提取世界坐标转化相机坐标                cv::Mat P3D1w = pMP1->GetWorldPos();                cv::Mat P3D1c = R1w*P3D1w + t1w;                //待优化为相机位姿                vPoint1->setEstimate(Converter::toVector3d(P3D1c));                vPoint1->setId(id1);                //固定原点坐标                vPoint1->setFixed(true);                optimizer.addVertex(vPoint1);                //优化位姿2                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();                cv::Mat P3D2w = pMP2->GetWorldPos();                cv::Mat P3D2c = R2w*P3D2w + t2w;                vPoint2->setEstimate(Converter::toVector3d(P3D2c));                vPoint2->setId(id2);                vPoint2->setFixed(true);                optimizer.addVertex(vPoint2);            }            else                continue;        }        else            continue;        //成功一个点加一个        nCorrespondences++;        // Set edge x1 = S12*X2        //建立边，就是计算重投影误差        //提取像素坐标        Eigen::Matrix<double,2,1> obs1;        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];        obs1 << kpUn1.pt.x, kpUn1.pt.y;        //建立边，也就是误差        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();        //连接的两个顶点        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));        //观测值        e12->setMeasurement(obs1);        //信息矩阵        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);        //鲁棒核        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;        e12->setRobustKernel(rk1);        rk1->setDelta(deltaHuber);        //添加边        optimizer.addEdge(e12);        // Set edge x2 = S21*X1        //反向投影在进行优化        Eigen::Matrix<double,2,1> obs2;        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];        obs2 << kpUn2.pt.x, kpUn2.pt.y;        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();        e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));        e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));        e21->setMeasurement(obs2);        float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];        e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);        g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;        e21->setRobustKernel(rk2);        rk2->setDelta(deltaHuber);        optimizer.addEdge(e21);        vpEdges12.push_back(e12);        vpEdges21.push_back(e21);        vnIndexEdge.push_back(i);    }    // Optimize!    //开始优化    optimizer.initializeOptimization();    optimizer.optimize(5);    // Check inliers    //清除外点和误差过大的点    int nBad=0;    for(size_t i=0; i<vpEdges12.size();i++)    {        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];        if(!e12 || !e21)            continue;        if(e12->chi2()>th2 || e21->chi2()>th2)        {            size_t idx = vnIndexEdge[i];            vpMatches1[idx]=static_cast<MapPoint*>(NULL);            optimizer.removeEdge(e12);            optimizer.removeEdge(e21);            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);            nBad++;        }    }    //确定迭代次数    int nMoreIterations;    if(nBad>0)        nMoreIterations=10;    else        nMoreIterations=5;    if(nCorrespondences-nBad<10)        return 0;    // Optimize again only with inliers    //再次优化，只优化内点    optimizer.initializeOptimization();    optimizer.optimize(nMoreIterations);    int nIn = 0;    for(size_t i=0; i<vpEdges12.size();i++)    {        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];        if(!e12 || !e21)            continue;        if(e12->chi2()>th2 || e21->chi2()>th2)        {            size_t idx = vnIndexEdge[i];            vpMatches1[idx]=static_cast<MapPoint*>(NULL);        }        else            nIn++;    }    // Recover optimized Sim3    //恢复出位姿变化关系    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));    g2oS12= vSim3_recov->estimate();    return nIn;}

当我们用第m帧推算了回环帧n的位姿时，使得n的位姿漂移误差较小，但其实同时可以用第n帧来计算n-1帧的位姿，使n-1帧的位姿漂移误差也减小。因此，这里还要有一个位姿传播。

另外我们可以优化所有的位姿，也就是进行一个位姿图优化（由位姿变换构建位姿约束）。

最后，我们还可以进行一起全局所有变量的BA优化。

//  初始化g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver     = new g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >();g2o::BlockSolver_6_3* blockSolver = new g2o::BlockSolver_6_3(linearSolver);g2o::OptimizationAlgorithmLevenberg* solver= new g2o::OptimizationAlgorithmLevenberg( blockSolver );    optimizer.setAlgorithm( solver );optimizer.setVerbose( false );// 初始顶点    g2o::VertexSE3* v = new g2o::VertexSE3();   v->setId( cutIndex );   v->setEstimate( Eigen::Isometry3d::Identity() );    v->setFixed( true );    optimizer.addVertex( v )// 添加新的节点（顶点）g2o::VertexSE3* vNext = new g2o::VertexSE3();vNext->setId(currFrame.frameID);vNext->setEstimate( Eigen::Isometry3d::Identity());optimizer.addVertex(vNext);// 添加边的数据g2o::EdgeSE3* edgeNew = new g2o::EdgeSE3();edgeNew->setVertex(0, optimizer.vertex(pousFrame.frameID));edgeNew->setVertex(1, optimizer.vertex(currFrame.frameID));edgeNew->setRobustKernel( new g2o::RobustKernelHuber() );//edgeNew->setInformation( Eigen::Matrix2d::Identity());  //the size is worthy to research// 信息矩阵// 两个节点的转换矩阵Eigen::Isometry3d T = cvMat2Eigen( m_result.rotaMatrix, m_result.tranMatrix );    edgeNew->setMeasurement(T);optimizer.addEdge(edgeNew);// 开始优化optimizer.initializeOptimization();optimizer.optimize(10);

3. 参考链接

https://baijiahao.baidu.com/s?id=1627227355343777871&wfr=spider&for=pc
https://zhuanlan.zhihu.com/p/45573552
https://blog.csdn.net/stihy/article/details/62219842

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术