后端2-学习笔记

滑动窗口法

为了控制BA优化的规模，需要选择那些应该被优化的帧的位姿和地图点，orbslam2中采用的是一种共视图的结构，例如，仅优化与当前帧有20个以上共视路标点的关键帧。

不难想到滑动窗口分为两步：

1. 新增一个关键帧以及它观测到的路标点

2. 删除旧的关键帧，如果旧的关键帧观测到的路标点没有被其他帧观测到，连同路标点一并删除

后端优化中要求解方程Hx=b，研究发现H是一个稀疏矩阵，于是对该矩阵分块，使用Schur消元也就是边缘化的方法，先求出了关键帧的位姿，再求解路标点的坐标。

边缘化某个关键帧就是固定当前帧的位姿估计值，求其他变量以这个帧的位姿估计值为条件的条件概率

边缘化某个关键帧会导致右下角路标点矩阵不再是对角矩阵，这个过程也叫填入，使得BA无法按照原先的稀疏方式进行求解，这也导致了EKF无法处理较大规模的滑动窗口。

位姿图优化

舍弃对路标点的优化，只保留对位姿的优化。也就是固定地图点，求位姿。

优化前：

优化后：

 

使用g2o进行优化的步骤

1. 确认每个误差项优化变量维度和误差值的维度。如在曲线优化中，每一个点有3个待优化变量abc，误差值只有一项，所以是3,1;而在位姿优化中，每一个点有6个待优化变量，误差项有6项。

2. 创建一个线性求解器

3. 创建一个块求解器，并用线性求解器进行初始化

4. 创建总求解器，选择优化方法，再用上述块求解器进行初始化

5. 创建稀疏优化器SparseOptimizer

6. 定义图的顶点和边，添加到SparseOptimizer中

7.设置优化参数，开始优化

    // 设定g2o 点代表待优化变量，边代表观测(误差项）
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;//确定待优化的变量个数以及误差项个数
    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; // 顶点和边的数量
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // 添加SE3 顶点
            g2o::VertexSE3 *v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // 添加SE3-SE3 边
            g2o::EdgeSE3 *e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if (!fin.good()) break;
    }

    optimizer.initializeOptimization();
    optimizer.optimize(30);
    optimizer.save("result.g2o");
    return 0;
}

 

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//选择了LM
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器 用前面定义好的求解器作为求解方法
    optimizer.setVerbose( true );       // 打开调试输出
    
    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }
    
    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    
    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;