【g2o】g2o学习笔记 BA相关

（1）曲线拟合问题抽象成图优化

​ 重点：节点为优化变量，边为误差项

​ 第一步：找出优化变量。个人总结：通过计算能将其转化为与观测值相减而构成最小二乘问题的变量。

1）模型的节点

​ 1 顶点初始化

​ 2 估计值更新 使用 _estimate变量

​ 3 （可选）需要将其转化成可以与观测值相减构成误差的形式

​ 类的继承：class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics>，待优化变量个数 9 存储形式为待优化变量的类型（非内设需要自己编写）

/// 姿态和内参的结构 
struct PoseAndIntrinsics {      //包含 R T f k1 k2
    PoseAndIntrinsics() {}

    /// set from given data address
    explicit PoseAndIntrinsics(double *data_addr) {
        rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
        translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
        focal = data_addr[6];
        k1 = data_addr[7];
        k2 = data_addr[8];
    }

    /// 将估计值放入内存
    void set_to(double *data_addr) {
        Vector3d r = rotation.log();
        for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
        for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
        data_addr[6] = focal;
        data_addr[7] = k1;
        data_addr[8] = k2;
    }

    SO3d rotation;
    Vector3d translation = Vector3d::Zero();
    double focal = 0;
    double k1 = 0, k2 = 0;
};

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {    //优化变量维度  数据类型
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoseAndIntrinsics() {}

    virtual void setToOriginImpl() override {                                      //顶点初始化
        _estimate = PoseAndIntrinsics();
    }

    virtual void oplusImpl(const double *update) override {                      //顶点估计值更新
        _estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);
        _estimate.focal += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    /// 根据估计值投影一个点
    Vector2d project(const Vector3d &point) {
        Vector3d pc = _estimate.rotation * point + _estimate.translation;
        pc = -pc / pc[2];                                               //归一化平面  BAL数据集说明  
        double r2 = pc.squaredNorm();
        //去畸变 1 + k1*r2 + k2*r2*r2 
        double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);      
        return Vector2d(_estimate.focal * distortion * pc[0],           //像素平面
                        _estimate.focal * distortion * pc[1]);
    }

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};

class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoint() {}

    virtual void setToOriginImpl() override {                 //顶点初始化
        _estimate = Vector3d(0, 0, 0);
    }

    virtual void oplusImpl(const double *update) override {             //顶点估计值更新
        _estimate += Vector3d(update[0], update[1], update[2]);
    }

    virtual bool read(istream &in) {return false;}

    virtual bool write(ostream &out) const {return false;}
};

2）模型的边

​ 关键：计算误差。实例化上述定义的点，需要 _vertices[]变量，这里面有一些专门的函数例如 estimate()，专门记忆吧。

​ 继承的类：第一个参数：误差的维度，第二个参数：误差的类型 第三四个参数：相连的点的类型

​ 继承什么类需要具体问题具体分析。

class EdgeProjection :
    public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        VertexPoseAndIntrinsics *pose = static_cast<VertexPoseAndIntrinsics*> (_vertices[0]);
        VertexPoint *point = static_cast<VertexPoint*> (_vertices[1]);
        Vector2d proj = pose->project(point->estimate());
        _error = proj - _measurement;                               //误差项
    }
    // 没有实现linearizeOplus（） 的重写时，   g2o会自动提供一个数值计算的雅克比

    // use numeric derivatives
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

};

（2）构建图优化

​ 1 创建一个线性求解器 LinearSolver

​ 2 创建块求解器 BlockSolver

​ 3 创建总求解器 从GN， LM， Dogleg中选择一个，用线性和块求解器初始化

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;              //块求解器
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;  //线性求解器
    // use LM
    auto solver = new g2o::OptimizationAlgorithmLevenberg(                              //总求解器
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

​ 4 创建稀疏矩阵优化器（SparseOptimizer）

    g2o::SparseOptimizer optimizer;                                                     //建立稀疏矩阵优化器
    optimizer.setAlgorithm(solver);                                                      //设置求解器
    optimizer.setVerbose(true);                                                           //打开调试输出

​ 5 向图中添加顶点

​ 遍历每个顶点，实例化，设置id，设置初始估计值，然后添加顶点到图

    vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
    vector<VertexPoint *> vertex_points;
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {                              //相机位姿顶点
        VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
        double *camera = cameras + camera_block_size * i;
        PoseAndIntrinsics estimate (camera);
        v->setId(i);
        v->setEstimate(estimate);                                                    //设置初始估计值
        optimizer.addVertex(v);                                                      //增加pose顶点
        vertex_pose_intrinsics.push_back(v);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {                              //观测点顶点
        VertexPoint *v = new VertexPoint();
        double *point = points + point_block_size * i;
        Eigen::Vector3d estimate (point[0], point[1], point[2]);
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(estimate);
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);                                                  //手动设置需要边缘化的顶点
        optimizer.addVertex(v);                                                    //增加point顶点
        vertex_points.push_back(v);
    }

​ 6 向图中添加边

​ 遍历观测变量（已知的），实例化，设置边的两个顶点，设置观测数值，添加边到图

    for (int i = 0; i < bal_problem.num_observations(); ++i) {                    //g2o的边是误差
        EdgeProjection *edge = new EdgeProjection;
        edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
        edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
        edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));    //设置观测值
        edge->setInformation(Matrix2d::Identity());
        edge->setRobustKernel(new g2o::RobustKernelHuber());                     //设置鲁棒核函数
        optimizer.addEdge(edge);
    }

​ 7 设置优化器进行优化

​ 初始化，然后根据最大迭代次数进行优化

    optimizer.initializeOptimization();
    optimizer.optimize(40);

后记：简单总结了一下使用g2o的步骤，目的总结出一个可以针对不同问题可重复利用的模板。工具方面暂时告一段落，接下来再总结一下可复用的程序。