SLAM~CH10

最新推荐文章于 2023-06-16 19:29:04 发布

闲某欧.

最新推荐文章于 2023-06-16 19:29:04 发布

阅读量178

点赞数

分类专栏： SLAM 个人学习文章标签： slam

本文链接：https://blog.csdn.net/m0_54225834/article/details/119709924

版权

个人学习同时被 2 个专栏收录

17 篇文章 0 订阅

订阅专栏

SLAM

12 篇文章 0 订阅

订阅专栏

本章主要讲了滑动窗口法和位姿图

滑动窗口法

def：仅仅保留离当前时刻最近的几个N个关键帧，把BA问题控制在一个时间窗口内，离开这个窗口则被抛弃。分为两步：1.新建关键帧及其观测到的路标。2.删除一个旧的关键帧及其观测到的路标点。

1. 新增一个关键帧很普通只不过是加了一个x，和数个p的变量而已

2.在删除一个关键帧时，如不同时边缘化其观测到的路标会导致以下问题：

由于在Schur消元时是边缘化路标点，因此左上角的对角结构被破坏。（fill-in）

而我们这里是边缘化的是位姿，会导致右下角的C对角结构被破坏,从而加大了后面的计算难度(要求C的逆)。于是我们同时边缘化观测到的关键点，使右下角恢复对角结构。

最后问题被归结为：

构建图优化模型---构建整体的Hessian矩阵---边缘化所有的路标点进行加速求解

位姿图

def：由两个得到运动估计的初始值后，不再优化路标点的位置，只关心相机位姿之间的关系。

我们假设估计了 $T_i$ 到 $\small T_j$ 之间的一个运动 $T_{ij}$

得到：

其中：

（、）

实践

//由于g2o_viewer怎么都打不开，上网查教程也没有解决，所以这部分的代码只是稍微看看

1. 设定图模型、求解器、调试输出

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> BlockSolverType;//顶点6维，边6维
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
            );
    g2o::SparseOptimizer optimizer;//图模型
    optimizer.setAlgorithm(solver);//设置求解器
    optimizer.setVerbose(true);//打开调试输出
    int vertexCnt=0,edgeCnt=0;//分别记录顶点和边的个数

2. 设定顶点

    if(name=="VERTEX_SE3:QUAT")
        {
            //顶点
            g2o::VertexSE3 *v = new g2o::VertexSE3();
            int index=0;
            fin>>index;//编号
            v->setId(index);//设置顶点编号
            v->read(fin);//读取边 就是setEstimate()
            optimizer.addVertex(v);//加入顶点
            vertexCnt++;//顶点个数++
            //设置是否固定，第一帧固定
            if(index==0)
            v->setFixed(true);
        }

3. 设定边

        else if(name=="EDGE_SE3:QUAT")
        {
            //边
            g2o::EdgeSE3 *e= new g2o::EdgeSE3();
            int idx1,idx2;
            fin>> idx1>> idx2;
            e->setId(edgeCnt++);
            //设置idx所对应的顶点
            e->setVertex(0,optimizer.vertices()[idx1]);
            e->setVertex(1,optimizer.vertices()[idx2]);
            e->read(fin);//读取观测数据
            optimizer.addEdge(e);//加入边
        }

4. 迭代优化

    optimizer.initializeOptimization();//优化初始化
    optimizer.optimize(30);//迭代次数
    cout<<" saving optimization results "<<endl;
    optimizer.save("result.g2o");//保存优化后的文件至result.g2o中