SLAM_局部束调整(local Bundle Adjustment, BA)步骤与代码示例

喜欢ヅ旅行 2022-10-22 07:38 8阅读 0赞

# 1. LocalBA步骤 #

>  *  1 将当前关键帧及其共视关键帧加入lLocalKeyFrames
>  *  2 遍历 lLocalKeyFrames 中(一级)关键帧，将它们观测的MapPoints加入到lLocalMapPoints
>     
>      *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]
>  *  3 得到能被局部MapPoints（被）观测到，但不属于局部关键帧的(二级)关键帧，这些关键帧在局部BA优化时不优化
>  *  4 构造g2o优化器，按照步骤来就行了
>  *  5 添加待优化的位姿顶点：Pose of Local KeyFrame
>  *  6 添加不优化的位姿顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true) // 不优化为啥也要添加？回答：为了增加约束信息
>  *  7 添加待优化的3D地图点顶点
>  *  8 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
>  *  9 开始优化。分成两个阶段：第一阶段优化，迭代5次；检查是否外部请求停止， 如果有外部请求停止,那么就不在进行第二阶段的优化，即下面一步
>     
>      *  10 检测outlier，并设置下次不优化
>      *  11 排除误差较大的outlier后再次优化 -- 第二阶段优化
>  *  12 在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPoint
>  *  13 优化后，更新关键帧位姿以及地图点的位置、平均观测方向等属性
> 
> 参考：[https://github.com/electech6/ORB\_SLAM2\_detailed\_comments][https_github.com_electech6_ORB_SLAM2_detailed_comments]

# 2. ORB\_SLAM2中的示例 #

/*
     * @brief Local Bundle Adjustment
     *
     * 1. Vertex:
     *     - g2o::VertexSE3Expmap()，LocalKeyFrames，即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
     *     - g2o::VertexSE3Expmap()，FixedCameras，即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrames）的位姿，在优化中这些关键帧的位姿不变
     *     - g2o::VertexSBAPointXYZ()，LocalMapPoints，即LocalKeyFrames能观测到的所有MapPoints的位置
     * 2. Edge:
     *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
     *         + Vertex：关键帧的Tcw，MapPoint的Pw
     *         + measurement：MapPoint在关键帧中的二维位置(u,v)
     *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
     *     - g2o::EdgeStereoSE3ProjectXYZ()，BaseBinaryEdge
     *         + Vertex：关键帧的Tcw，MapPoint的Pw
     *         + measurement：MapPoint在关键帧中的二维位置(ul,v,ur)
     *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
     *         
     * @param pKF        KeyFrame
     * @param pbStopFlag 是否停止优化的标志
     * @param pMap       在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate
     * @note 由局部建图线程调用,对局部地图进行优化的函数
     */
    void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
    {
        // 该优化函数用于LocalMapping线程的局部BA优化
    
        // Local KeyFrames: First Breadth Search from Current Keyframe
        list<KeyFrame*> lLocalKeyFrames;
    
        // Step 1 将当前关键帧及其共视关键帧加入lLocalKeyFrames
        lLocalKeyFrames.push_back(pKF);
        pKF->mnBALocalForKF = pKF->mnId;
    
        // 找到关键帧连接的共视关键帧（一级相连），加入lLocalKeyFrames中
        const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
        for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
        {
            KeyFrame* pKFi = vNeighKFs[i];
    
            // 把参与局部BA的每一个关键帧的 mnBALocalForKF设置为当前关键帧的mnId，防止重复添加
            pKFi->mnBALocalForKF = pKF->mnId;
    
            // 保证该关键帧有效才能加入
            if(!pKFi->isBad())      
                lLocalKeyFrames.push_back(pKFi);
        }
    
        // Local MapPoints seen in Local KeyFrames
        // Step 2 遍历 lLocalKeyFrames 中(一级)关键帧，将它们观测的MapPoints加入到lLocalMapPoints
        list<MapPoint*> lLocalMapPoints;
        // 遍历 lLocalKeyFrames 中的每一个关键帧
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            // 取出该关键帧对应的地图点
            vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
            // 遍历这个关键帧观测到的每一个地图点，加入到lLocalMapPoints
            for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
            {
                MapPoint* pMP = *vit;
                if(pMP)
                {
                    if(!pMP->isBad())   //保证地图点有效
                        // 把参与局部BA的每一个地图点的 mnBALocalForKF设置为当前关键帧的mnId
                        // mnBALocalForKF 是为了防止重复添加
                        if(pMP->mnBALocalForKF!=pKF->mnId)
                        {
                            lLocalMapPoints.push_back(pMP);
                            pMP->mnBALocalForKF=pKF->mnId;
                        }
                }   // 判断这个地图点是否靠谱
            } // 遍历这个关键帧观测到的每一个地图点
        } // 遍历 lLocalKeyFrames 中的每一个关键帧
    
        // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
        // Step 3 得到能被局部MapPoints观测到，但不属于局部关键帧的(二级)关键帧，这些关键帧在局部BA优化时不优化
        list<KeyFrame*> lFixedCameras;
        // 遍历局部地图中的每个地图点
        for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
        {
            // 观测到该MapPoint的KF和该MapPoint在KF中的索引
            map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
            // 遍历所有观测到该地图点的关键帧
            for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
            {
                KeyFrame* pKFi = mit->first;
    
                // pKFi->mnBALocalForKF!=pKF->mnId 表示不属于局部关键帧，
                // pKFi->mnBAFixedForKF!=pKF->mnId 表示还未标记为fixed（固定的）关键帧
                if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
                {                
                    // 将局部地图点能观测到的、但是不属于局部BA范围的关键帧的mnBAFixedForKF标记为pKF（触发局部BA的当前关键帧）的mnId
                    pKFi->mnBAFixedForKF=pKF->mnId;
                    if(!pKFi->isBad())
                        lFixedCameras.push_back(pKFi);
                }
            }
        }
    
        // Setup optimizer
        // Step 4 构造g2o优化器，按照步骤来就行了
        g2o::SparseOptimizer optimizer;
        g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    
        linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    
        g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
        // LM大法好
        g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        optimizer.setAlgorithm(solver);
    
        // 外界设置的停止优化标志
        // 可能在 Tracking::NeedNewKeyFrame() 里置位
        if(pbStopFlag)
            optimizer.setForceStopFlag(pbStopFlag);
    
        // 记录参与局部BA的最大关键帧mnId
        unsigned long maxKFid = 0;
    
        // Set Local KeyFrame vertices
        // Step 5 添加待优化的位姿顶点：Pose of Local KeyFrame
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            KeyFrame* pKFi = *lit;
            g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
            // 设置初始优化位姿
            vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
            vSE3->setId(pKFi->mnId);
            // 如果是初始关键帧，要锁住位姿不优化
            vSE3->setFixed(pKFi->mnId==0);
            optimizer.addVertex(vSE3);
            if(pKFi->mnId>maxKFid)
                maxKFid=pKFi->mnId;
        }
    
        // Set Fixed KeyFrame vertices
        // Step  6 添加不优化的位姿顶点：Pose of Fixed KeyFrame，注意这里调用了vSE3->setFixed(true)
        // 不优化为啥也要添加？回答：为了增加约束信息
        for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
        {
            KeyFrame* pKFi = *lit;
            g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
            vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
            vSE3->setId(pKFi->mnId);
             // 所有的这些顶点的位姿都不优化，只是为了增加约束项
            vSE3->setFixed(true);  
            optimizer.addVertex(vSE3);
            if(pKFi->mnId>maxKFid)
                maxKFid=pKFi->mnId;
        }
    
        // Set MapPoint vertices
        // Step  7 添加待优化的3D地图点顶点
        // 边的数目 = pose数目 * 地图点数目
        const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
    
        vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
        vpEdgesMono.reserve(nExpectedSize);
    
        vector<KeyFrame*> vpEdgeKFMono;
        vpEdgeKFMono.reserve(nExpectedSize);
    
        vector<MapPoint*> vpMapPointEdgeMono;
        vpMapPointEdgeMono.reserve(nExpectedSize);
    
        vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
        vpEdgesStereo.reserve(nExpectedSize);
    
        vector<KeyFrame*> vpEdgeKFStereo;
        vpEdgeKFStereo.reserve(nExpectedSize);
    
        vector<MapPoint*> vpMapPointEdgeStereo;
        vpMapPointEdgeStereo.reserve(nExpectedSize);
    
        // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
        const float thHuberMono = sqrt(5.991);
    
        // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
        const float thHuberStereo = sqrt(7.815);
    
        // 遍历所有的局部地图中的地图点
        for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
        {
            // 添加顶点：MapPoint
            MapPoint* pMP = *lit;
            g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
            vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
            // 前面记录maxKFid的作用在这里体现
            int id = pMP->mnId+maxKFid+1;
            vPoint->setId(id);
            // 因为使用了LinearSolverType，所以需要将所有的三维点边缘化掉
            vPoint->setMarginalized(true);  
            optimizer.addVertex(vPoint);
    
            // 观测到该地图点的KF和该地图点在KF中的索引
            const map<KeyFrame*,size_t> observations = pMP->GetObservations();
    
            // Set edges
            // Step 8 在添加完了一个地图点之后, 对每一对关联的MapPoint和KeyFrame构建边
            // 遍历所有观测到当前地图点的关键帧
            for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
            {
                KeyFrame* pKFi = mit->first;
    
                if(!pKFi->isBad())
                {                
                    const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
                    // 根据单目/双目两种不同的输入构造不同的误差边
                    // Monocular observation
                    // 单目模式下
                    if(pKFi->mvuRight[mit->second]<0)
                    {
                        Eigen::Matrix<double,2,1> obs;
                        obs << kpUn.pt.x, kpUn.pt.y;
    
                        g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
    
                        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                        e->setMeasurement(obs);
                        // 权重为特征点所在图像金字塔的层数的倒数
                        const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                        e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
    
                        // 使用鲁棒核函数抑制外点
                        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                        e->setRobustKernel(rk);
                        rk->setDelta(thHuberMono);
    
                        e->fx = pKFi->fx;
                        e->fy = pKFi->fy;
                        e->cx = pKFi->cx;
                        e->cy = pKFi->cy;
    
                        optimizer.addEdge(e);
                        vpEdgesMono.push_back(e);
                        vpEdgeKFMono.push_back(pKFi);
                        vpMapPointEdgeMono.push_back(pMP);
                    }
                    else // Stereo observation
                    {
                        Eigen::Matrix<double,3,1> obs;
                        const float kp_ur = pKFi->mvuRight[mit->second];
                        obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
    
                        g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
    
                        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                        e->setMeasurement(obs);
                        const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                        Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                        e->setInformation(Info);
    
                        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                        e->setRobustKernel(rk);
                        rk->setDelta(thHuberStereo);
    
                        e->fx = pKFi->fx;
                        e->fy = pKFi->fy;
                        e->cx = pKFi->cx;
                        e->cy = pKFi->cy;
                        e->bf = pKFi->mbf;
    
                        optimizer.addEdge(e);
                        vpEdgesStereo.push_back(e);
                        vpEdgeKFStereo.push_back(pKFi);
                        vpMapPointEdgeStereo.push_back(pMP);
                    } 
                } 
            } // 遍历所有观测到当前地图点的关键帧
        } // 遍历所有的局部地图中的地图点
    
        // 开始BA前再次确认是否有外部请求停止优化，因为这个变量是引用传递，会随外部变化
        // 可能在 Tracking::NeedNewKeyFrame(), mpLocalMapper->InsertKeyFrame 里置位
        if(pbStopFlag)
            if(*pbStopFlag)
                return;
    
        // Step 9 开始优化。分成两个阶段
        // 第一阶段优化
        optimizer.initializeOptimization();
        // 迭代5次
        optimizer.optimize(5);  
    
        bool bDoMore= true;
    
        // 检查是否外部请求停止
        if(pbStopFlag)
            if(*pbStopFlag)
                bDoMore = false;
    
        // 如果有外部请求停止,那么就不在进行第二阶段的优化
        if(bDoMore)
        {
            // Check inlier observations
            // Step 10 检测outlier，并设置下次不优化
            // 遍历所有的单目误差边
            for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
            {
                g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
                MapPoint* pMP = vpMapPointEdgeMono[i];
    
                if(pMP->isBad())
                    continue;
    
                // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
                // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
                // 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，不优化了。
                if(e->chi2()>5.991 || !e->isDepthPositive())
                {
                    // 不优化
                    e->setLevel(1);
                }
                // 第二阶段优化的时候就属于精求解了,所以就不使用核函数
                e->setRobustKernel(0);
            }
    
            // 对于所有的双目的误差边也都进行类似的操作
            for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
            {
                g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
                MapPoint* pMP = vpMapPointEdgeStereo[i];
    
                if(pMP->isBad())
                    continue;
                // 自由度为3的卡方分布，显著性水平为0.05，对应的临界阈值7.815
                if(e->chi2()>7.815 || !e->isDepthPositive())
                {
                    e->setLevel(1);
                }
    
                e->setRobustKernel(0);
            }
    
            // Optimize again without the outliers
            // Step 11：排除误差较大的outlier后再次优化 -- 第二阶段优化
            optimizer.initializeOptimization(0);
            optimizer.optimize(10);
    
        }
    
        vector<pair<KeyFrame*,MapPoint*> > vToErase;
        vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
    
        // Check inlier observations
        // Step 12：在优化后重新计算误差，剔除连接误差比较大的关键帧和MapPoint
        // 对于单目误差边
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
        {
            g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
            MapPoint* pMP = vpMapPointEdgeMono[i];
    
            if(pMP->isBad())
                continue;
    
            // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
            // 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值5.991
            // 如果 当前边误差超出阈值，或者边链接的地图点深度值为负，说明这个边有问题，要删掉了
            if(e->chi2()>5.991 || !e->isDepthPositive())
            {
                // outlier
                KeyFrame* pKFi = vpEdgeKFMono[i];
                vToErase.push_back(make_pair(pKFi,pMP));
            }
        }
    
        // 双目误差边
        for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
        {
            g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
            MapPoint* pMP = vpMapPointEdgeStereo[i];
    
            if(pMP->isBad())
                continue;
    
            if(e->chi2()>7.815 || !e->isDepthPositive())
            {
                KeyFrame* pKFi = vpEdgeKFStereo[i];
                vToErase.push_back(make_pair(pKFi,pMP));
            }
        }
    
        // Get Map Mutex
        unique_lock<mutex> lock(pMap->mMutexMapUpdate);
    
        // 删除点
        // 连接偏差比较大，在关键帧中剔除对该地图点的观测
        // 连接偏差比较大，在地图点中剔除对该关键帧的观测
        if(!vToErase.empty())
        {
            for(size_t i=0;i<vToErase.size();i++)
            {
                KeyFrame* pKFi = vToErase[i].first;
                MapPoint* pMPi = vToErase[i].second;
                pKFi->EraseMapPointMatch(pMPi);
                pMPi->EraseObservation(pKFi);
            }
        }
    
        // Recover optimized data
        // Step 13：优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性
    
        //Keyframes
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            KeyFrame* pKF = *lit;
            g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
            g2o::SE3Quat SE3quat = vSE3->estimate();
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }
    
        //Points
        for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
        {
            MapPoint* pMP = *lit;
            g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
            pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
            pMP->UpdateNormalAndDepth();
        }
    }

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221022/7e495b441f4243aaae98080f2da7be29.png
[https_github.com_electech6_ORB_SLAM2_detailed_comments]: https://github.com/electech6/ORB_SLAM2_detailed_comments