2021SC@SDUSC

1.前言

这部分代码量巨大，结合数据查阅了大量代码，分为以下部分进行分析

1. 单帧优化
2. 局部地图优化
3. 全局优化
4. 规模和重力优化
5. sim3优化
6. 地图回环优化
7. 地图集成优化

以下是逐步注释和分析

2.代码分析

LoopClosing::CorrectLoop() 使用回环矫正，纯视觉，全局本质图优化 优化目标：

• 地图中所有MP
• 关键帧

void Optimizer::OptimizeEssentialGraph(Map *pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,                                         const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,                                         const LoopClosing::KeyFrameAndPose &CorrectedSim3,                                         const map<KeyFrame *, set<KeyFrame *>> &LoopConnections, const bool &bFixScale) { 
             // Setup optimizer     // Step 1.结构优化器     g2o::SparseOptimizer optimizer;     optimizer.setVerbose(false);     // 使用指定的线性方程求解器Eigen的块求解器     // 7表示位姿是sim3 33表示三维点坐标维度     g2o::BlockSolver_7_3::LinearSolverType *linearSolver =         new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();     // 构造线性求解器     g2o::BlockSolver_7_3 *solver_ptr = new g2o::BlockSolver_7_3(linearSolver);     // 使用LM非线性迭代算法     g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);      // 第一次迭代的开始lambda如果没有指定值，合适的值将自动计算     solver->setUserLambdaInit(
       
        1e-16
        )
        ; optimizer
        .
        setAlgorithm
        (solver
        )
        ; 
        // 获取当前地图中的所有关键帧 和地图点 
        const vector
        <KeyFrame 
        *
        > vpKFs 
        = pMap
        ->
        GetAllKeyFrames
        (
        )
        ; 
        const vector
        <MapPoint 
        *
        > vpMPs 
        = pMap
        ->
        GetAllMapPoints
        (
        )
        ; 
        // 最大关键帧id，用于添加顶点时使用 
        const 
        unsigned 
        int nMaxKFid 
        = pMap
        ->
        GetMaxKFid
        (
        )
        ; 
        // 记录所有优化前关键帧的位姿，优先使用在闭环时通过Sim3传播调整过的Sim3位姿 vector
        <g2o
        ::Sim3
        , Eigen
        ::aligned_allocator
        <g2o
        ::Sim3
        >> 
        vScw
        (nMaxKFid 
        + 
        1
        )
        ; 
        // 存放每一帧优化前的sim3 
        // 记录所有关键帧经过本次本质图优化过的位姿 vector
        <g2o
        ::Sim3
        , Eigen
        ::aligned_allocator
        <g2o
        ::Sim3
        >> 
        vCorrectedSwc
        (nMaxKFid 
        + 
        1
        )
        ; 
        // 存放每一帧优化后的sim3，修正mp位姿用 
        // 这个变量没有用 vector
        <g2o
        ::VertexSim3Expmap 
        *
        > 
        vpVertices
        (nMaxKFid 
        + 
        1
        )
        ; 
        // 存放节点，没用，还占地方 
        // 调试用，暂时不去管 vector
        <Eigen
        ::Vector3d
        > 
        vZvectors
        (nMaxKFid 
        + 
        1
        )
        ; 
        // For debugging Eigen
        ::Vector3d z_vec
        ; z_vec 
        << 
        0.0
        , 
        0.0
        , 
        1.0
        ; 
        // 两个关键帧之间共视关系的权重（也就是共视点的数目，单目x1，双目/rgbd x2）的最小值 
        const 
        int minFeat 
        = 
        100
        ; 
        // MODIFICATION originally was set to 100 
        // Set KeyFrame vertices 
        // Step 2：将地图中所有关键帧的pose作为顶点添加到优化器 
        // 尽可能使用经过Sim3调整的位姿 
        // 遍历全局地图中的所有的关键帧 
        for 
        (size_t i 
        = 
        0
        , iend 
        = vpKFs
        .
        size
        (
        )
        ; i 
        < iend
        ; i
        ++
        ) 
        { 
          KeyFrame 
        *pKF 
        = vpKFs
        [i
        ]
        ; 
        if 
        (pKF
        ->
        isBad
        (
        )
        ) 
        continue
        ; g2o
        ::VertexSim3Expmap 
        *VSim3 
        = 
        new g2o
        ::
        VertexSim3Expmap
        (
        )
        ; 
        // 关键帧在所有关键帧中的id，用来设置为顶点的id 
        const 
        int nIDi 
        = pKF
        ->mnId
        ; LoopClosing
        ::KeyFrameAndPose
        ::const_iterator it 
        = CorrectedSim3
        .
        find
        (pKF
        )
        ; 
        if 
        (it 
        != CorrectedSim3
        .
        end
        (
        )
        ) 
        { 
          
        // 如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿 vScw
        [nIDi
        ] 
        = it
        ->second
        ; VSim3
        ->
        setEstimate
        (it
        ->second
        )
        ; 
        } 
        else 
        { 
          
        // 如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿 Eigen
        ::Matrix
        <
        double
        , 
        3
        , 
        3
        > Rcw 
        = 
        Converter
        ::
        toMatrix3d
        (pKF
        ->
        GetRotation
        (
        )
        )
        ; Eigen
        ::Matrix
        <
        double
        , 
        3
        , 
        1
        > tcw 
        = 
        Converter
        ::
        toVector3d
        (pKF
        ->
        GetTranslation
        (
        )
        )
        ; g2o
        ::Sim3 
        Siw
        (Rcw
        , tcw
        , 
        1.0
        )
        ; 
        //尺度为1 vScw
        [nIDi
        ] 
        = Siw
        ; VSim3
        ->
        setEstimate
        (Siw
        )
        ; 
        } 
        // 固定第一帧 
        if 
        (pKF
        ->mnId 
        == pMap
        ->
        GetInitKFid
        (
        )
        ) VSim3
        ->
        setFixed
        (
        true
        )
        ; VSim3
        ->
        setId
        (nIDi
        )
        ; VSim3
        ->
        setMarginalized
        (
        false
        )
        ; 
        // 和当前系统的传感器有关，如果是RGBD或者是双目，那么就不需要优化sim3的缩放系数，保持为1即可 VSim3
        ->_fix_scale 
        = bFixScale
        ; optimizer
        .
        addVertex
        (VSim3
        )
        ; vZvectors
        [nIDi
        ] 
        = vScw
        [nIDi
        ]
        .
        rotation
        (
        )
        .
        toRotationMatrix
        (
        ) 
        * z_vec
        ; 
        // For debugging 
        // 优化前的pose顶点，后面代码中没有使用 vpVertices
        [nIDi
        ] 
        = VSim3
        ; 
        } 
        // 保存由于闭环后优化sim3而出现的新的关键帧和关键帧之间的连接关系,因为回环而新建立的连接关系，其中id比较小的关键帧在前,id比较大的关键帧在后 set
        <pair
        <
        long 
        unsigned 
        int
        , 
        long 
        unsigned 
        int
        >> sInsertedEdges
        ; 
        const Eigen
        ::Matrix
        <
        double
        , 
        7
        , 
        7
        > matLambda 
        = Eigen
        ::
        Matrix
        <
        double
        , 
        7
        , 
        7
        >
        ::
        Identity
        (
        )
        ; 
        // Set Loop edges 
        // Step 3：添加第1种边：LoopConnections是闭环时因为MapPoints调整而出现的新关键帧连接关系（包括当前帧与闭环匹配帧之间的连接关系） 
        int count_loop 
        = 
        0
        ; 
        for 
        (map
        <KeyFrame 
        *
        , set
        <KeyFrame 
        *
        >>
        ::const_iterator mit 
        = LoopConnections
        .
        begin
        (
        )
        , mend 
        = LoopConnections
        .
        end
        (
        )
        ; mit 
        != mend
        ; mit
        ++
        ) 
        { 
          
        // 3.1 取出帧与帧们 KeyFrame 
        *pKF 
        = mit
        ->first
        ; 
        const 
        long 
        unsigned 
        int nIDi 
        = pKF
        ->mnId
        ; 
        // 和pKF 有连接关系的关键帧 
        const set
        <KeyFrame 
        *
        > 
        &spConnections 
        = mit
        ->second
        ; 
        const g2o
        ::Sim3 Siw 
        = vScw
        [nIDi
        ]
        ; 
        // 优化前的位姿 
        const g2o
        ::Sim3 Swi 
        = Siw
        .
        inverse
        (
        )
        ; 
        // 对于当前关键帧nIDi而言，遍历每一个新添加的关键帧nIDj链接关系 
        for 
        (set
        <KeyFrame 
        *
        >
        ::const_iterator sit 
        = spConnections
        .
        begin
        (
        )
        , send 
        = spConnections
        .
        end
        (
        )
        ; sit 
        != send
        ; sit
        ++
        ) 
        { 
          
        const 
        long 
        unsigned 
        int nIDj 
        = 
        (
        *sit
        )
        ->mnId
        ; 
        // 这里的约束有点意思，对于每一个连接，只要是存在pCurKF或者pLoopKF 那这个连接不管共视了多少MP都优化 
        // 反之没有的话共视度要大于100 构建本质图 
        if 
        (
        (nIDi 
        != pCurKF
        ->mnId 
        || nIDj 
        != pLoopKF
        ->mnId
        ) 
        && pKF
        ->
        GetWeight
        (
        *sit
        ) 
        < minFeat
        ) 
        continue
        ; 
        // 通过上面考验的帧有两种情况： 
        // 得到两个pose间的Sim3变换，个人认为这里好像有些问题，假设说一个当前帧的共视帧，他在vScw中保存的位姿是更新后的 
        // 如果与之相连的关键帧没有更新，那么是不是两个相对位姿的边有问题，先留个记号，可以调试看看 
        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
        )
        )
        )
        ; 
        // Sji内部是经过了Sim调整的观测 e
        ->
        setMeasurement
        (Sji
        )
        ; 
        // 信息矩阵是单位阵,说明这类新增加的边对总误差的贡献也都是一样大的 e
        ->
        information
        (
        ) 
        = matLambda
        ; optimizer
        .
        addEdge
        (e
        )
        ; count_loop
        ++
        ; 
        // 保证id小的在前,大的在后 sInsertedEdges
        .
        insert
        (
        make_pair
        (
        min
        (nIDi
        , nIDj
        )
        , 
        max
        (nIDi
        , nIDj
        )
        )
        )
        ; 
        } 
        } 
        int count_spa_tree 
        = 
        0
        ; 
        int count_cov 
        = 
        0
        ; 
        int count_imu 
        = 
        0
        ; 
        int count_kf 
        = 
        0
        ; 
        // Set normal edges 
        // 4. 添加跟踪时形成的边、闭环匹配成功形成的边 
        for 
        (size_t i 
        = 
        0
        , iend 
        = vpKFs
        .
        size
        (
        )
        ; i 
        < iend
        ; i
        ++
        ) 
        { 
          count_kf 
        = 
        0
        ; KeyFrame 
        *pKF 
        = vpKFs
        [i
        ]
        ; 
        const 
        int nIDi 
        = pKF
        ->mnId
        ; g2o
        ::Sim3 Swi
        ; 
        // 校正前的sim3 LoopClosing
        ::KeyFrameAndPose
        ::const_iterator iti 
        = NonCorrectedSim3
        .
        find
        (pKF
        )
        ; 
        // 找到的话说明是关键帧的共视帧，没找到表示非共视帧，非共视帧vScw[nIDi]里面装的都是矫正前的 
        // 所以不管怎样说 Swi都是校正前的 
        if 
        (iti 
        != NonCorrectedSim3
        .
        end
        (
        )
        ) Swi 
        = 
        (iti
        ->second
        )
        .
        inverse
        (
        )
        ; 
        else Swi 
        = vScw
        [nIDi
        ]
        .
        inverse
        (
        )
        ; KeyFrame 
        *pParentKF 
        = pKF
        ->
        GetParent
        (
        )
        ; 
        // Spanning tree edge 
        // 4.1 只添加扩展树的边（有父关键帧） 
        if 
        (pParentKF
        ) 
        { 
          
        int nIDj 
        = pParentKF
        ->mnId
        ; g2o
        ::Sim3 Sjw
        ; LoopClosing
        ::KeyFrameAndPose
        ::const_iterator itj 
        = NonCorrectedSim3
        .
        find
        (pParentKF
        )
        ; 
        if 
        (itj 
        != NonCorrectedSim3
        .
        end
        (
        )
        ) Sjw 
        = itj
        ->second
        ; 
        else Sjw 
        = vScw
        [nIDj
        ]
        ; 
        // 又是未校正的结果作为观测值 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
        )
        )
        )
        ; e
        ->
        setMeasurement
        (Sji
        )
        ; count_kf
        ++
        ; count_spa_tree
        ++
        ; e
        ->
        information
        (
        ) 
        = matLambda
        ; optimizer
        .
        addEdge
        (e
        )
        ; 
        } 
        // Loop edges 
        // 4.2 添加在CorrectLoop函数中AddLoopEdge函数添加的闭环连接边（当前帧与闭环匹配帧之间的连接关系） 
        // 使用经过Sim3调整前关键帧之间的相对关系作为边 
        const set
        <KeyFrame 
        *
        > sLoopEdges 
        = pKF
        ->
        GetLoopEdges
        (
        )
        ; 
        for 
        (set
        <KeyFrame 
        *
        >
        ::const_iterator sit 
        = sLoopEdges
        .
        begin
        (
        )
        , send 
        = sLoopEdges
        .
        end
        (
        )
        ; sit 
        != send
        ; sit
        ++
        ) 
        { 
          KeyFrame 
        *pLKF 
        = 
        *sit
        ; 
        if 
        (pLKF
        ->mnId 
        < pKF
        ->mnId
        ) 
        { 
          g2o
        ::Sim3 Slw
        ; LoopClosing
        ::KeyFrameAndPose
        ::const_iterator itl 
        = NonCorrectedSim3
        .
        find
        (pLKF
        )
        ; 
        if 
        (itl 
        != NonCorrectedSim3
        .
        end
        (
        )
        ) Slw 
        = itl
        ->second
        ; 
        else Slw 
        = vScw
        [pLKF
        ->mnId
        ]
        ; g2o
        ::Sim3 Sli 
        = Slw 
        * Swi
        ; g2o
        ::EdgeSim3 
        *el 
        = 
        new g2o
        ::
        EdgeSim3
        (
        )
        ; el
        ->
        setVertex
        (
        1
        , 
        dynamic_cast
        <g2o
        ::OptimizableGraph
        ::Vertex 
        *
        >
        (optimizer
        .
        vertex
        (pLKF
        ->mnId
        )
        )
        )
        ; el
        ->
        setVertex
        (
        0
        , 
        dynamic_cast
        <g2o
        ::OptimizableGraph
        ::Vertex 
        *
        >
        (optimizer
        .
        vertex
        (nIDi
        )
        )
        )
        ; el
        ->
        setMeasurement
        (Sli
        )
        ; el
        ->
        information
        (
        ) 
        = matLambda
        ; optimizer
        .
        addEdge
        (el
        )
        ; count_kf
        ++
        ; count_loop
        ++
        ; 
        } 
        } 
        // Covisibility graph edges 
        // 4.3 对有很好共视关系的关键帧也作为边进行优化 
        // 使用经过Sim3调整前关键帧之间的相对关系作为边 
        const vector
        <KeyFrame 
        *
        > vpConnectedKFs 
        = pKF
        ->
        GetCovisiblesByWeight
        (minFeat
        )
        ; 
        for 
        (vector
        <KeyFrame 
        *
        >
        ::const_iterator vit 
        = vpConnectedKFs
        .
        begin
        (
        )
        ; vit 
        != vpConnectedKFs
        .
        end
        (
        )
        ; vit
        ++
        ) 
        { 
          KeyFrame 
        *pKFn 
        = 
        *vit
        ; 
        if 
        (pKFn 
        && pKFn 
        != pParentKF 
        && 
        !pKF
        ->
        hasChild
        (pKFn
        ) 
        && 
        !sLoopEdges
        .
        count
        (pKFn
        )
        ) 
        { 
          
        if 
        (
        !pKFn
        ->
        isBad
        (
        ) 
        && pKFn
        ->mnId 
        < pKF
        ->mnId
        ) 
        { 
          
        if 
        (sInsertedEdges
        .
        count
        (
        make_pair
        (
        min
        (pKF
        ->mnId
        , pKFn
        ->mnId
        )
        , 
        max
        (pKF
        ->mnId
        , pKFn
        ->mnId
        )
        )
        )
        ) 
        continue
        ; g2o
        ::Sim3 Snw
        ; LoopClosing
        ::KeyFrameAndPose
        ::const_iterator itn 
        = NonCorrectedSim3
        .
        find
        (pKFn
        )
        ; 
        if 
        (itn 
        != NonCorrectedSim3
        .
        end
        (
        )
        ) Snw 
        = itn
        ->second
        ; 
        else Snw 
        = vScw
        [pKFn
        ->mnId
        ]
        ; g2o
        ::Sim3 Sni 
        = Snw 
        * Swi
        ; g2o
        ::EdgeSim3 
        *en 
        = 
        new g2o
        ::
        EdgeSim3
        (
        )
        ; en
        ->
        setVertex
        (
        1
        , 
        dynamic_cast
        <g2o
        ::OptimizableGraph
        ::Vertex 
        *
        >
        (optimizer
        .
        vertex
        (pKFn
        ->mnId
        )
        )
        )
        ; en
        ->
        setVertex
        (
        0
        , 
        dynamic_cast
        <g2o
        ::OptimizableGraph
        ::Vertex 
        *
        >
        (optimizer
        .
        vertex
        (nIDi
        )
        )
        )
        ; en
        ->
        setMeasurement
        (Sni
        )
        ;