使用 g2o 进行BA调整：优化相机位姿和三维点

简介： 在计算机视觉和机器人领域，准确估计相机位姿和三维点位置对于诸如三维重建、增强现实和同时定位与地图构建（SLAM）等应用至关重要。稀疏束调整（Sparse Bundle Adjustment，SBA）是一种流行的技术，通过基于观察到的二维-三维对应关系对它们进行优化。在本篇博客中，我们将探讨如何使用 C++ 中的 g2o 库执行 SBA。

1. 稀疏束调整（SBA）简介：

   • 对 SBA 的简要概述及其在计算机视觉和机器人领域的重要性。
   • 解释优化问题：通过最小化重投影误差来优化相机位姿和三维点位置。

2. g2o 概述：

   • 介绍 g2o 库，这是一个功能强大的 C++ 库，用于优化基于图的非线性误差函数。
   • g2o 的特性和功能，用于执行 SBA 和其他优化任务。

3. 代码解释：

   • 逐步解释提供的 C++ 代码，用于使用 g2o 执行稀疏束调整。
   • 解释从文件读取数据、设置优化问题、向优化器添加顶点和边以及执行优化迭代的函数。

4. 理解数据：

   • 描述示例中使用的数据：三维点、相机位姿和二维-三维对应关系。
   • 准确数据表示的重要性及其在成功束调整中的作用。

在给定的代码中，主要涉及到了g2o库中的一些API，这些API是用来构建和优化稀疏图优化问题的。下面是一些主要API的参数解释

1. g2o::SparseOptimizer：

   • setAlgorithm(algorithm)：设置优化算法，参数 algorithm 是一个指向优化算法的指针，用于指定优化过程中采用的算法。
   • setVerbose(true)：设置是否输出优化过程的详细信息，参数 true 表示输出详细信息。

2. g2o::VertexSE3Expmap：

   • setId(vertex_id)：设置顶点的ID，参数 vertex_id 是顶点的唯一标识符。
   • setFixed(true)：设置顶点是否固定，参数 true 表示将顶点固定，不参与优化。
   • setEstimate(g2o::SE3Quat(poses[i].block<3, 3>(0, 0), poses[i].block<3, 1>(0, 3)))：设置顶点的初始估计值，参数是一个 SE3Quat 类型的对象，表示位姿的旋转和平移部分。

3. g2o::VertexSBAPointXYZ：

   • setId(vertex_id)：设置顶点的ID，参数 vertex_id 是顶点的唯一标识符。
   • setEstimate(points3d[i])：设置顶点的初始估计值，参数是一个 Eigen::Vector3d 类型的对象，表示3D点的坐标。

4. g2o::CameraParameters：

   • setId(0)：设置相机参数的ID，通常在添加相机参数时使用，参数 0 表示相机参数的唯一标识符。
   • addParameter(camera)：将相机参数添加到优化器中，参数 camera 是一个指向相机参数对象的指针。

5. g2o::EdgeProjectXYZ2UV：

   • setVertex(0, v1) 和 setVertex(1, v2)：设置边连接的顶点，参数 v1 和 v2 分别是边连接的两个顶点，用于构建相机投影模型。
   • setMeasurement(points2d[i][j])：设置边的测量值，参数是一个 Eigen::Vector2d 类型的对象，表示2D观测点的坐标。
   • setInformation(Eigen::Matrix2d::Identity())：设置边的信息矩阵，参数是一个 2x2 的单位矩阵，表示观测的权重。
   • setParameterId(0, 0)：设置边所需的参数ID，参数 0 表示相机参数的ID。
   • setRobustKernel(rk)：设置鲁棒核函数，参数 rk 是一个指向鲁棒核函数对象的指针，用于处理异常值

#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>
#include <unordered_set>
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
// #include "g2o/math_groups/se3quat.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include <vector>

// read file
#include <fstream>
#include <string>

// 读取3d点
void readfile(const std::string &filename, std::vector<Eigen::Vector3d> &points)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "could not open file: " << filename << std::endl;
        return;
    }
    // read line
    std::string line;
    while (std::getline(file, line))
    {
        std::istringstream iss(line);
        double x, y, z;
        if (!(iss >> x >> y >> z))
        {
            std::cerr << "error reading file: " << filename << std::endl;
            break;
        }
        points.push_back(Eigen::Vector3d(x, y, z));
    }
}
// 读取位姿
void readfile(const std::string &filename, std::vector<Eigen::Matrix4d> &points)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "could not open file: " << filename << std::endl;
        return;
    }
    // read line
    std::string line;
    double data[12] = {0};
    while (std::getline(file, line))
    {
        std::istringstream iss(line);
        for (int i = 0; i < 12; i++)
        {
            if (!(iss >> data[i]))
            {
                std::cerr << "error reading file: " << filename << std::endl;
                break;
            }
        }
        Eigen::Matrix3d R = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(data);
        Eigen::Vector3d t(data[9], data[10], data[11]);
        //Rwc -> Rcw
        R.transposeInPlace();
        //twc -> tcw
        t = -R * t;
        Eigen::Matrix4d pose = Eigen::Matrix4d::Zero();
        pose.block<3, 3>(0, 0) = R;
        pose.block<3, 1>(0, 3) = t;
        pose(3, 3) = 1;
        points.push_back(pose);
    }
}

// 读取观测
void readfile(const std::string &filename, std::vector<std::vector<Eigen::Vector2d>> &points2d, std::vector<std::vector<int>> &ids)
{
    std::ifstream file(filename);
    if (!file.is_open())
    {
        std::cerr << "could not open file: " << filename << std::endl;
        return;
    }
    // read line
    std::string line;
    // u1 v1 id1 u2 v2 id2 ...
    while (std::getline(file, line))
    {
        std::istringstream iss(line);
        std::vector<Eigen::Vector2d> obs;
        std::vector<int> ob_ids;
        double u, v;
        int i;
        while (iss >> u >> v >> i)
        {
            obs.emplace_back(u, v);
            ob_ids.emplace_back(i);
        }
        points2d.push_back(std::move(obs));
        ids.push_back(std::move(ob_ids));
    }
}

int main(int argc, const char *argv[])
{
    std::vector<std::vector<Eigen::Vector2d>> points2d;
    std::vector<std::vector<int>> ids;
    readfile("/root/g2o-example/data/3d2d.txt", points2d, ids);
    std::vector<Eigen::Vector3d> points3d;
    readfile("/root/g2o-example/data/3d.txt", points3d);
    std::vector<Eigen::Matrix4d> poses;
    readfile("/root/g2o-example/data/pose.txt", poses);

    // Set up the problem
    g2o::SparseOptimizer optimizer;
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>> BlockSolverType;
    typedef g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto linearSolver = new LinearSolverType();
    auto solver = new BlockSolverType(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *algorithm = new g2o::OptimizationAlgorithmLevenberg(solver);
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);

    // add pose vertex
    int vertex_id = 0;
    for (size_t i = 0; i < poses.size(); i++, ++vertex_id)
    {
        g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
        v->setId(vertex_id);
        // 固定第一个顶点
        if (i == 0)
        {
            v->setFixed(true);
        }
        v->setEstimate(g2o::SE3Quat(poses[i].block<3, 3>(0, 0), poses[i].block<3, 1>(0, 3)));
        optimizer.addVertex(v);
    }
    // add point vertex
    for (size_t i = 0; i < points3d.size(); i++, ++vertex_id)
    {
        g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
        v->setId(vertex_id);
        v->setEstimate(points3d[i]);
        optimizer.addVertex(v);
    }
    // add camera parameter
    float focal = 720, cx = 640, cy = 360;
    g2o::CameraParameters *camera = new g2o::CameraParameters(focal, Eigen::Vector2d(cx, cy), 0);
    camera->setId(0);
    if (!optimizer.addParameter(camera))
    {
        assert(false);
    }
    // add edge
    for (size_t i = 0; i < points2d.size(); i++)
    {
        for (size_t j = 0; j < points2d[i].size(); j++)
        {
            g2o::EdgeProjectXYZ2UV *e = new g2o::EdgeProjectXYZ2UV();
            // 3d点
            int id = ids[i][j];
            g2o::VertexSBAPointXYZ *v1 = dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(id + poses.size()));
            e->setVertex(0, v1);
            // 位姿
            g2o::VertexSE3Expmap *v2 = dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(i));
            e->setVertex(1, v2);
            // 观测
            e->setMeasurement(points2d[i][j]);
            e->setInformation(Eigen::Matrix2d::Identity());
            e->setParameterId(0, 0);
            // 核函数
            g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
            e->setRobustKernel(rk);
            optimizer.addEdge(e);
        }
    }
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    optimizer.save("/root/g2o-example/data/result.g2o");

    return 0;
}