ORB-SLAM optimizer

很有针对性的相关参考链接:https://blog.csdn.net/hzwwpgmwy/article/details/79884070

学会去看模板参数

顶点一般就是位姿、路标点位置

边的话就是1、一般找投影2、找两帧间的相对变换sim3作为边

class  EdgeSE3ProjectXYZ: public  BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>//投影边的模板参数,第一个连的顶点是三维点VertexSBAPointXYZ(三维点),第二点是VertexSE3Expmap(位姿)

sim3的理解https://blog.csdn.net/ABC1225741797/article/details/108044343

 局部BA

https://blog.csdn.net/u010128736/article/details/53395936

局部BA：红圈内红框为优化量，pos1为约束量

优化共视图图内的关键帧的位姿、地图点，同时把能观测到这些地图点的关键帧作为约束量

约束项放入代价函数中，但值保持不变的操作在代价函数求取雅可比矩阵时有所体现，不会求代价函数关于pos1的雅可比矩阵

每个点在所有选出来的帧里面的投影误差

代码实现：

// Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
    // 步骤4：得到能被局部MapPoints观测到，但不属于局部关键帧的关键帧，这些关键帧在局部BA优化时不优化
    list<KeyFrame*> lFixedCameras;
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
        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表示局部关键帧，
            // 其它的关键帧虽然能观测到，但不属于局部关键帧
            if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
            {                
                pKFi->mnBAFixedForKF=pKF->mnId;// 防止重复添加
                if(!pKFi->isBad())
                    lFixedCameras.push_back(pKFi);
            }
        }
           

// Set Fixed KeyFrame vertices
    // 步骤7：添加顶点：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;
    }
           

代码中：optimizer:InfoMatrix是高斯型误差指数上p=error'*Q*error中的Q

essential-graph的优化 

量测量是两帧之间的相似变换sim3,也就是加入优化器中的e->setMeasurement，这个类模板内部会根据添加的顶点，定义的误差类型g2o::EdgeSim3* ，内部计算如上图中的公示（9）这个最小二乘的形式，进行优化

const g2o::Sim3 Sjw = vScw[nIDj];
            // 得到两个pose间的Sim3变换
const g2o::Sim3 Sji = Sjw * Swi;

g2o::EdgeSim3* e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            // 根据两个Pose顶点的位姿算出相对位姿作为边，那还存在误差？优化有用？因为闭环MapPoints调整新形成的边不优化？（wubo???）
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);