通过g2o实现逆深度表示的重投影误差优化

原创已于 2024-03-16 12:37:38 修改 · 1.2k 阅读

20 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #笔记

于 2024-03-16 12:37:07 首次发布

g2o 专栏收录该内容

2 篇文章

订阅专栏

本文详细介绍了如何使用G2O库在视觉SLAM中处理重投影误差，通过设置环境、定义辅助函数、创建特定的顶点和边类，以及实现主要函数，展示了如何构建和优化一个包含逆深度和重投影误差边的优化图，以估计相机姿态和三维点的位置。

使用g2o库进行重投影误差约束：逐步指南

第一步：设置环境：
首先包含必要的头文件，例如Eigen库、标准输入输出操作和g2o库组件。还要确保您有所需的数据文件（3d2d.txt、3d.txt、pose.txt）用于此项目。

第二步：辅助函数：
定义辅助函数如readfile，用于从文件读取数据到向量，并normalize将像素坐标转换为相机归一化坐标。

第三步：顶点和边类：
引入VertexInverseDepth和EdgeReprojection类来表示优化图中的顶点和边。

第四步：主函数：
在主函数中，使用适当的求解器和参数设置g2o优化器。将姿势顶点添加到优化器中，并固定第一个姿势。对每个三维点进行迭代，为第一次观测添加lambda顶点，并为后续观测添加重投影边。最后，使用g2o初始化和优化图。

使用的 g2o API：
SparseOptimizer: 这是一个稀疏优化器，用于管理优化问题的图结构。

BlockSolver: 该类表示用于解决优化问题的块求解器。在这里，我们使用了 BlockSolverType，其特征为动态大小的块。

LinearSolverCholmod: 这是一个基于 CHOLMOD 的线性求解器，用于求解线性方程组。它在块求解器中被使用。

OptimizationAlgorithmLevenberg: 这是优化算法之一，实现了 Levenberg-Marquardt 算法，用于在优化过程中调整步长。

BaseVertex: 顶点基类，表示优化问题中的状态变量。在代码中，我们使用了 VertexInverseDepth 类来表示逆深度参数的顶点。

BaseMultiEdge: 边基类，表示优化问题中的残差。在代码中，我们使用了 EdgeReprojection 类来表示重投影误差的边。

优化的残差：
在视觉 SLAM 中，通常优化的残差是指观测到的二维特征点与通过当前估计的相机姿态和三维点位置进行重投影得到的预测值之间的差异。这些残差通常以像素为单位，可以使用各种方式来定义，例如像素坐标之间的欧几里得距离。

在提供的代码中，EdgeReprojection 类计算了重投影误差。该类的 computeError() 方法计算了观测值与预测值之间的差异，并将其作为优化过程的残差。

状态变量：
在视觉 SLAM 中，状态变量通常包括相机的姿态和三维点的位置。在这个问题中，状态变量是优化问题的未知数，我们希望通过优化算法找到它们的最佳估计值。

在提供的代码中，相机的姿态由 VertexSE3Expmap 表示，而三维点的位置由 VertexInverseDepth 表示。这些顶点是优化问题的状态变量，它们的估计值会在优化过程中被调整以最小化残差。

自定义类：
EdgeReprojection 类：
作用：

EdgeReprojection 类用于表示优化问题中的重投影误差边。在视觉SLAM中，重投影误差是指观测到的二维特征点与通过当前估计的相机姿态和三维点位置进行重投影得到的预测值之间的差异。这些误差是优化问题的残差，通过最小化这些残差来优化相机姿态和三维点的位置。
成员函数：

computeError(): 这个函数计算了观测值与预测值之间的差异，并将其作为优化过程的残差。在这个函数中，使用了三维点的逆深度参数来计算重投影点，然后计算观测值与重投影点之间的差异。
VertexInverseDepth 类：
作用：

VertexInverseDepth 类用于表示优化问题中的逆深度顶点。在视觉SLAM中，逆深度参数是一种表示三维点位置的方式，它与相机之间的距离成反比。通过优化逆深度参数，可以估计三维点的位置，并在优化过程中调整这些参数以最小化重投影误差。
成员函数：

oplusImpl(const double *update): 这个函数实现了顶点更新的操作。在优化过程中，通过添加一个增量（通常称为步长）来更新顶点的估计值。在 VertexInverseDepth 类中，增量表示为逆深度参数的变化量，因此通过将增量添加到当前估计值来更新逆深度参数。
这些类在整个优化过程中起着关键的作用，通过定义优化问题的结构和约束，帮助优化算法找到相机姿态和三维点的最优估计值。

#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) * 0.01f);
    }
}
// 读取位姿
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 = Eigen::Vector3d(data[9], data[10], data[11]) * 0.01f;
        // 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));
    }
}

// 定义逆深度顶点
class VertexInverseDepth : public g2o::BaseVertex<1, double>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexInverseDepth() {}
    bool read(std::istream & /*is*/) override
    {
        std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override
    {
        std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
        return false;
    }
    virtual void setToOriginImpl() {}
    virtual void oplusImpl(const double *update)
    {
        _estimate += update[0];
    }
};
typedef g2o::VertexSE3Expmap iVertexSE3Expmap;
typedef g2o::VertexSE3Expmap jVertexSE3Expmap;
// 定义重投影误差边
/*
观测：(ui,vi),(uj,vj)
顶点：VertexInverseDepth ,iVertexSE3Expmap，jVertexSE3Expmap
*/

class EdgeReprojection : public g2o::BaseMultiEdge<2, Eigen::Vector4d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeReprojection() {}
    bool read(std::istream & /*is*/) override
    {
        std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override
    {
        std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
        return false;
    }
    virtual void computeError() override
    {
        const iVertexSE3Expmap *vi = static_cast<const iVertexSE3Expmap *>(_vertices[0]);
        const jVertexSE3Expmap *vj = static_cast<const jVertexSE3Expmap *>(_vertices[1]);
        const VertexInverseDepth *vd = static_cast<const VertexInverseDepth *>(_vertices[2]);
        Eigen::Vector2d uvi = _measurement.head<2>();
        Eigen::Vector2d uvj = _measurement.tail<2>();
        const double &lambda = vd->estimate();
        Eigen::Vector3d Pci = Eigen::Vector3d(uvi(0), uvi(1), 1) / lambda;
        Eigen::Vector3d Pw = vi->estimate().inverse().map(Pci);
        Eigen::Vector3d Pcj = vj->estimate().map(Pw);
        Eigen::Vector2d uvj_ = Pcj.head<2>() / Pcj(2);
        _error = uvj - uvj_;
    }
};

// 像素坐标转相机归一化坐标
void normalize(Eigen::Vector2d &p, double fx, double fy, double cx, double cy)
{
    p(0) = (p(0) - cx) / fx;
    p(1) = (p(1) - cy) / fy;
}

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);
    // normalize
    double fx = 720, fy = 720, cx = 640, cy = 360;
    for (auto &obs : points2d)
    {
        for (auto &p : obs)
        {
            normalize(p, fx, fy, cx, cy);
        }
    }
    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
    for (int i = 0; i < (int)poses.size(); ++i)
    {
        g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
        v->setEstimate(g2o::SE3Quat(poses[i].block<3, 3>(0, 0), poses[i].block<3, 1>(0, 3)));
        // fix the first pose
        if (i == 0)
        {
            v->setFixed(true);
        }
        v->setId(i);
        optimizer.addVertex(v);
    }
    // 定义第一次观测到3d点的相机frame id
    std::vector<int> firstObsFrameId(points3d.size(), -1);

    // for each 3d points
    for (int i = 0; i < (int)points3d.size(); ++i)
    {
        // for each camera pose
        // 第一次观测的pos
        const Eigen::Matrix4d *firstObsPose = nullptr;
        const Eigen::Vector2d *firstObsPoint = nullptr;
        double firstObsLambda = 0;

        int obs_num = 0;
        for (int j = 0; j < (int)poses.size(); ++j)
        {
            // 判断该frame是否观测到points3d[i]
            const std::vector<int> &obs_ids = ids[j];
            auto it = std::find(obs_ids.begin(), obs_ids.end(), i);
            if (it == obs_ids.end())
            {
                continue;
            }
            // 该frame观测到了points3d[i]
            int find_index = it - obs_ids.begin();
            if (++obs_num == 1)
            {
                firstObsPose = &poses[j];
                firstObsPoint = &points2d[j][find_index];
                Eigen::Vector3d Pw = points3d[i];
                Eigen::Vector3d Pc = firstObsPose->block<3, 3>(0, 0) * Pw + firstObsPose->block<3, 1>(0, 3);
                firstObsLambda = 1.0 / Pc(2);
                firstObsFrameId[i] = j;
                // add lambda vertex
                VertexInverseDepth *v = new VertexInverseDepth();
                v->setEstimate(firstObsLambda);
                v->setId(poses.size() + i);
                optimizer.addVertex(v);
            }
            else
            {
                // add edge
                EdgeReprojection *e = new EdgeReprojection();
                e->resize(3);
                e->setVertex(0, optimizer.vertex(firstObsFrameId[i]));
                e->setVertex(1, optimizer.vertex(j));
                e->setVertex(2, optimizer.vertex(poses.size() + i));
                e->setMeasurement(Eigen::Vector4d((*firstObsPoint)(0), (*firstObsPoint)(1), points2d[j][find_index](0), points2d[j][find_index](1)));
                e->setInformation(Eigen::Matrix2d::Identity());
                e->setParameterId(0, 0);
                // add robust kernel
                g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber();
                e->setRobustKernel(rk);
                optimizer.addEdge(e);
            }
        }
    }
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    optimizer.save("/root/g2o-example/data/result.g2o");
}