视觉SLAM第10讲：后端2（滑动窗口与位子图优化）

WWZZ20252025-09-15 11:09

目录

[10.1 滑动窗口滤波和优化](#10.1 滑动窗口滤波和优化)

[10.1.1 实际环境下的BA结构](#10.1.1 实际环境下的BA结构)

[10.1.2 滑动窗口法](#10.1.2 滑动窗口法)

[10.2 位姿图](#10.2 位姿图)

[10.2.1 位姿图的意义](#10.2.1 位姿图的意义)

[10.2.2 位姿图的优化](#10.2.2 位姿图的优化)

[10.3 实践：位姿图优化](#10.3 实践：位姿图优化)

[10.3.1 g2o原生位姿图](#10.3.1 g2o原生位姿图)

1.pose_graph_g2o_SE3.cpp

2.CMakeLists.txt

3.运行结果

[10.3.2 李代数上的位姿图优化](#10.3.2 李代数上的位姿图优化)

1.pose_graph_g2o_lie_algebra.cpp

2.CMakeLists.txt

3.注意事项

4.运行结果

目标：

1.理解滑动窗口优化；

2.理解位姿图优化；

3.理解带IMU紧耦合的优化；

4.通过实验掌握g2o的位姿图。
第9讲重点介绍以BA为主的图优化。BA能精确地优化每个相机位姿与特征点位置。不过在更大的场景中，大量特征点的存在会严重降低计算效率，导致计算量越来越大，以至于无法实时化。本讲介绍一种简化的BA：位姿图。

10.1 滑动窗口滤波和优化

10.1.1 实际环境下的BA结构

带有相机位姿和空间点的图优化称为BA，它能够有效地求解大规模的定位与建图问题。这在SfM问题中十分有用，但是在SLAM过程中，我们往往需要控制BA的规模，保持计算的实时性。倘若计算能力无限，那不妨每时每刻都计算整个BA------但是那不符合现实需要。现实条件是，我们必须限制后端的计算时间，比如BA规模不能超过1万个路标点，迭代不超过20次、用时不超过0.5秒，等等。像SfM那样用一周时间重建一个城市地图的算法，在SLAM里不见得有效。

控制计算规模的做法有很多，比如从连续的视频中抽出一部分作为关键帧，仅构造关键帧与路标点之间的BA，于是非关键帧只用于定位，对建图则没有贡献。

即便如此，随着时间的流逝，关键帧数量会越来越多，地图规模也将不断增长。像BA这样的批量优化方法，计算效率会不断下降。为了避免这种情况，我们需要用一定手段控制后端BA的规模。这些手段可以是理论上的，也可以是工程上的。

最简单的控制BA规模的思路，是仅保留离当前时刻最近的N个关键帧，去掉时间上更早的关键帧。于是BA将被固定在一个时间窗口内，离开这个窗口的则被丢弃。这种方法称为滑动窗口法。

取这N个关键帧的具体方法可以有一些改变，例如，不见得必须取时间上最近的，而可以按照某种原则，取时间上靠近，空间上又可以展开的关键帧，从而保证相机即使在停止不动时，BA的结构也不至于缩成一团。如果我们在帧与帧的结构上再考虑得深一些，也可以像ORB-SLAM2那样，定义一种称为"共视图"（Covisibility graph）的结构。所谓共视图，就是指那些与现在的相机存在共同观测的关键帧构成的图。

于是，在BA优化时，我们按照某此原则在共视图内取一些关键帧和路标进行优化，例如，仅优化与当前帧有20个以上共视路标的关键帧，其余部分固定不变。当共视图关系能够正确构造的时候，基于共视图的优化也会在更长时间内保持最优。

滑动窗口也好，共视图也好，大体而言，都是我们对实时计算的某种工程上的折中。不过在理论上，它们也引入了一个新问题：刚才我们谈到要"丢弃"滑动窗口之外，或者"固定"共视图之外的变量，这个"丢弃"和"固定"具体怎样操作呢？"固定"似乎很容易理解，我们只需将共视图之外的关键帧估计值保持不变即可。但是"丢弃"，是指完全弃置不用，即窗口外的变量完全不对窗口内的变量产生任何影响，还是说窗口外的数据应该对窗口内的有一些影响，但实际上被我们忽略了？如果有影响，这种影响应该是什么样子？它够不够明显，能不能忽略？

10.1.2 滑动窗口法

假设滑动窗口内有N个关键帧，它们的位姿表达为：

关键帧意义：位置在哪里，不确定度如何，其对应着它们在高斯分布假设下的均值协方差。

如果这几个关键帧还对应着一个局部地图，则可以顺带着问整个局部系统的均值和方差应该是多少。设这个滑动窗口中还有M个路标点，它们与N个关键帧组成了局部地图。

可以用第9讲介绍的BA方法处理这个滑动窗口，包括建立图优化模型，构建整体的Hessian矩阵，然后边缘化所有路标点来加速求解。在边缘化时，我们考虑关键帧的位姿

其中为第k个关键帧的位姿均值，为所有关键帧的协方差矩阵，均值部分就是指BA迭代之后的结果，而就是对整个BA的H矩阵进行边缘化之后的结果，即第9讲提到的矩阵S。

在滑动窗口中，当窗口结构发生改变，这些状态变量应该如何变化？这件事情可以分成两部分讨论：

1．我们需要在窗口中新增一个关键帧，以及它观测到的路标点。

2．我们需要把窗口中一个旧的关键帧删除，也可能删除它观测到的路标点。

这时，滑动窗口法和传统的BA的区别就显现出来了。显然，如果按照传统的BA来处理，那么这仅仅对应于两个不同结构的BA，在求解上没有任何差别。但在滑动窗口的情况下，我们就要讨论具体的细节问题了。

新增一个关键帧和路标点

考虑在上个时刻，滑动窗口已经建立了N个关键帧，它们服从某个高斯分布，其均值和方差如前所述。此时，新来了一个关键帧，那么整个问题中的变量变为N+1个关键帧和更多路标点的集合，只需按照正常的BA流程处理即可。对所有点进行边缘化时，即得到这N+1个关键帧的高斯分布参数。
删除一个旧的关键帧

要删除旧关键帧，但是并不是孤立的，它会和其他帧观测到同样的路标。将边缘化之后将导致整个问题不再稀疏，如下图。

在这个例子中，假设看到了路标点至。于是，在处理之前，BA问题的Hessian矩阵应该像左图一样，在行的到列存在着非零块，表示看到了它们。

这时考虑边缘化，那么Schur消元过程相当于通过矩阵行和列操作消去非对角线处几个非零矩阵块，显然这将导致右下角的路标点矩阵块不再是非对角矩阵。这个过程称为边缘化中的填入（Fill-in）。

回顾第9讲中介绍的边缘化，当我们边缘化路标点时，Fill-in将出现在左上角的位姿块中。不过，因为BA不要求位姿块为对角块，所以稀疏BA求解仍然可行。但是当边缘化关键帧时，将破坏右下角路标点之间的对角块结构，这时BA就无法按照先前的稀疏方式迭代求解。这显然是个十分糟糕的问题。实际上，在早期的EKF滤波器后端中，人们确实保持着一个稠密的Hessian矩阵，这也使得 EKF后端没法处理较大规模的滑动窗口。

不过，如果我们对边缘化的过程进行一些改造，也可以保持滑动窗口BA的稀疏性。例如，在边缘化某个旧的关键帧时，同时边缘化它观测到的路标点。这样，路标点的信息就会转换成剩下那些关键帧之间的共视信息，从而保持右下角部分的对角块结构。在某些SLAM框架中，边缘化策略会更复杂。例如在OKVIS 中，我们会判断要边缘化的那个关键帧，它看到的路标点是否在最新的关键帧中仍能看到。如果不能，就直接边缘化这个路标点，如果能，就丢弃被边缘化关键帧对这个路标点的观测，从而保持BA的稀疏性。

SWF中边缘化的直观解释

边缘化在概率上的意义就是指条件概率。所以，当我们边缘化某个关键帧，即"保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率"。所以，当某个关键帧被边缘化，它观测到的路标点就会产生一个"这些路标应该在哪里"的先验信息，从而影响其余部分的估计值。如果再边缘化这些路标点，那么它们的观测者将得到一个"观测它们的关键帧应该在哪里"的先验信息。从数学上看，当我们边缘化某个关键帧，整个窗口中的状态变量的描述方式，将从联合分布变成一个条件概率分布。以上面的例子来看，就是说：

然后舍去被边缘化部分的信息。在变量被边缘化之后，在工程中就不应再使用它。所以滑动窗口法比较适合VO系统，而不适合大规模建图的系统。

10.2 位姿图

10.2.1 位姿图的意义

根据前面的讨论，我们发现特征点在优化问题中占据了绝大部分。实际上，经过若干次观测之后，收敛的特征点位置变化很小，发散的外点则已被剔除。对收敛点再进行优化，似乎是有些费力不讨好。因此，更倾向于在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束，而不再实际地优化它们的位置估计。

沿着这个思路继续思考，是否能够完全不管路标而只管轨迹呢？可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系。通过这种方式，我们省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图（Pose Graph），如图。

在BA 中特征点数量远大于位姿节点。一个关键帧往往关联了数百个关键点，而实时BA的最大计算规模，即使利用稀疏性，在当前的主流CPU上一般也就是几万个点左右。这就限制了SLAM应用场景。所以，当机器人在更大范围的时间和空间中运动时，必须考虑一些解决方式：要么像滑动窗口法那样，丢弃一些历史数据；要么像位姿图的做法那样，舍弃对路标点的优化，只保留Pose之间的边。此外，如果我们有额外测量Pose的传感器，那么位姿图也是一种常见的融合Pose测量的方法。

10.2.2 位姿图的优化

位姿图优化中的节点节点表示相机位姿，以来表达，边则是两个位姿节点之间相对运动的估计，该估计可以来自于特征点法或直接法，也可以来自GPS或IMU积分。假设估计了和之间的一个运动。

该运动可以表达为

或者李群的写法

按照图优化的思路，实际中该等式不会精确地成立，因此设立最小二乘误差，然后和以往一样，讨论误差关于优化变量的导数。这里把上式的移至等式右侧，构建误差

注意优化变量有两个：和，因此我们求误差关于这两个变量的导数。按照李代数的求导方式，给和各一个左扰动和。于是误差变为

该式中，两个扰动项被夹在了中间。为了利用BCH近似，我们希望把扰动项移至式子左侧或右侧。回忆第4讲习题中的伴随性质，

即式

稍加改变，有

该式表明，通过引入一个伴随项，我们能够"交换"扰动项左右侧的T。将扰动挪到最右，导出右乘形式的雅可比矩阵（挪到左边时形成左乘)

因此，按照李代数上的求导法则，我们求出了误差关于两个位姿的雅可比矩阵。关于的

关于的

不过，前面也说过，由于se(3)上的左右雅可比形式过于复杂，通常取它们的近似。如果误差接近零，我们就可以设它们近似为I或

理论上，即使在优化之后，由于每条边给定的观测数据并不一致，误差也不见得近似于零，所以简单地把这里的设置为I会有一定的损失。

了解雅可比求导后，剩下的部分就和普通的图优化一样了。简而言之，所有的位姿顶点和位姿---------位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。记为所有边的集合，那么总体目标函数为

10.3 实践：位姿图优化

10.3.1 g2o原生位姿图
1.pose_graph_g2o_SE3.cpp
cpp 复制代码 
#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>

using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin) {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // 设定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;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");
    optimizer.clear();

    return 0;
}
2.CMakeLists.txt
cpp 复制代码 
cmake_minimum_required(VERSION 3.16)
project(slam_10)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -g")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o
list(APPEND CMAKE_MODULE_PATH /home/caohao/g2o/cmake_modules)
find_package(G2O REQUIRED)
set(G2O_LIBS g2o_core g2o_stuff g2o_types_slam3d g2o_csparse_extension cxsparse)
include_directories(${G2O_INCLUDE_DIRS})

# Eigen
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

# --- Executables ---
add_executable(pose_graph_g2o_SE3 pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3 ${G2O_LIBS} ${CHOLMOD_LIBRARIES})

#add_executable(pose_graph_g2o_lie pose_graph_g2o_lie_algebra.cpp)
#target_link_libraries(pose_graph_g2o_lie ${G2O_LIBS} ${CHOLMOD_LIBRARIES} ${Sophus_LIBRARIES})
3.运行结果

该处make时出现核心已转储问题，为库版本问题，未作处理。

10.3.2 李代数上的位姿图优化
1.pose_graph_g2o_lie_algebra.cpp
cpp 复制代码 
// pose_graph_g2o_lie_algebra.cpp
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include <limits>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/core/sparse_optimizer.h>

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;
using Sophus::SE3d;

typedef Matrix<double, 6, 6> Matrix6d;
typedef Matrix<double, 6, 1> Vector6d;

// JRInv: use identity for stability
Matrix6d JRInv(const SE3d & /*e*/) {
    Matrix6d J = Matrix6d::Identity();
    return J;
}

// Vertex using Lie algebra (SE3)
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexSE3LieAlgebra() {}
    virtual ~VertexSE3LieAlgebra() {}

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; ++i) {
            if (!(is >> data[i])) return false;
        }
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setEstimate(SE3d(q, Vector3d(data[0], data[1], data[2])));
        return true;
    }

    virtual bool write(ostream &os) const override {
        os << id() << " ";
        Quaterniond q = _estimate.unit_quaternion();
        Vector3d t = _estimate.translation();
        os << t.transpose() << " ";
        os << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
        return true;
    }

    virtual void setToOriginImpl() override {
        _estimate = SE3d();
    }

    // left-multiply update: se3::exp(update) * estimate
    virtual void oplusImpl(const double *update) override {
        Vector6d upd;
        for (int i = 0; i < 6; ++i) upd[i] = update[i];
        _estimate = SE3d::exp(upd) * _estimate;
    }
};

// Edge between two SE3 Lie vertices
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeSE3LieAlgebra() {}
    virtual ~EdgeSE3LieAlgebra() {}

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; ++i) {
            if (!(is >> data[i])) return false;
        }
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
       setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));

        // read upper triangle of information
        for (int i = 0; i < information().rows() && is.good(); ++i) {
            for (int j = i; j < information().cols() && is.good(); ++j) {
                is >> information()(i, j);
                if (i != j) information()(j, i) = information()(i, j);
            }
        }

        // basic sanity: make sure diagonal positive finite
        for (int i = 0; i < 6; ++i) {
            if (!std::isfinite(information()(i, i)) || information()(i, i) <= 0.0)
                information()(i, i) = 1.0;
        }
        return is.good();
    }

    virtual bool write(ostream &os) const override {
        auto *v1 = static_cast<VertexSE3LieAlgebra *>(_vertices[0]);
        auto *v2 = static_cast<VertexSE3LieAlgebra *>(_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3d m = _measurement;
        Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";
        for (int i = 0; i < information().rows(); ++i)
            for (int j = i; j < information().cols(); ++j)
                os << information()(i, j) << " ";
        os << endl;
        return true;
    }

    // error in tangent space (6D)
    virtual void computeError() override {
        auto *v1 = static_cast<VertexSE3LieAlgebra *>(_vertices[0]);
        auto *v2 = static_cast<VertexSE3LieAlgebra *>(_vertices[1]);
        SE3d T1 = v1->estimate();
        SE3d T2 = v2->estimate();
        // _error is a 6-vector = log( measurement^{-1} * T1^{-1} * T2 )
    }

    virtual void linearizeOplus() override {
        auto *v1 = static_cast<VertexSE3LieAlgebra *>(_vertices[0]);
        auto *v2 = static_cast<VertexSE3LieAlgebra *>(_vertices[1]);
        SE3d T1 = v1->estimate();
        SE3d T2 = v2->estimate();

        // use JRInv identity approximation for stability
        Matrix6d J = JRInv(SE3d::exp(_error));

        // v2.inverse().Adj() gives 6x6 adjoint
        Eigen::Matrix<double, 6, 6> Adj = T2.inverse().Adj();

        _jacobianOplusXi = -J * Adj;
        _jacobianOplusXj =  J * Adj;
    }
};

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_lie sphere.g2o" << endl;
        return 1;
    }

    ifstream fin(argv[1]);
    if (!fin) {
        cerr << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    // Setup g2o solver
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto linearSolver = g2o::make_unique<LinearSolverType>();
    auto blockSolver  = g2o::make_unique<BlockSolverType>(std::move(linearSolver));
    auto solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));

    g2o::SparseOptimizer optimizer;
   optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    int vertexCnt = 0, edgeCnt = 0;
    vector<VertexSE3LieAlgebra *> vertices;
    vector<EdgeSE3LieAlgebra *> edges;

    // read file
    while (fin.good()) {
        string tag;
        fin >> tag;
        if (!fin.good()) break;

        if (tag == "VERTEX_SE3:QUAT") {
            int id;
            fin >> id;
            // create and read vertex
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            v->setId(id);
            if (!v->read(fin)) {
                delete v;
                continue;
            }
            optimizer.addVertex(v);
            vertices.push_back(v);
            vertexCnt++;
            if (id == 0) v->setFixed(true);
        } else if (tag == "EDGE_SE3:QUAT") {
            int id1, id2;
            fin >> id1 >> id2;

            // allocate edge, but bind vertices safely using optimizer.vertex(id)
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();

            // check vertex existence using optimizer.vertex(id)
            g2o::OptimizableGraph::Vertex *vv1 = optimizer.vertex(id1);
            g2o::OptimizableGraph::Vertex *vv2 = optimizer.vertex(id2);

            if (!vv1 || !vv2) {
               // missing vertex -> skip this edge, but consume the rest of line
                cerr << "[WARN] edge references missing vertex: " << id1 << ", " << id2 << endl;
                double dump;
                // measurement (7 numbers)
                for (int k = 0; k < 7; ++k) if (!(fin >> dump)) break;
                // upper triangular info matrix (6x upper)
                for (int r = 0; r < 6; ++r)
                    for (int c = r; c < 6; ++c) if (!(fin >> dump)) break;
                delete e;
                continue;
            }

            // cast to our vertex type
            auto *v1 = static_cast<VertexSE3LieAlgebra *>(vv1);
            auto *v2 = static_cast<VertexSE3LieAlgebra *>(vv2);

            e->setVertex(0, v1);
            e->setVertex(1, v2);

            if (!e->read(fin)) {
                delete e;
                continue;
            }
            e->setId(edgeCnt);
            optimizer.addEdge(e);
            edges.push_back(e);
            edgeCnt++;
        } else {
            // unknown token: skip rest of line or break
            string rest;
            getline(fin, rest);
        }
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    // quick sanity checks
    if (vertexCnt == 0) {
        cerr << "No vertices loaded. Exiting." << endl;
        return 1;
  }

    // initialize and optimize
    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    try {
        optimizer.optimize(30);
    } catch (const std::exception &e) {
        cerr << "Exception during optimization: " << e.what() << endl;
        // continue to try to save partial result
    }

    // save as standard g2o format that g2o_viewer can read
    ofstream fout("result_lie.g2o");
    if (!fout) {
        cerr << "Cannot open result_lie.g2o for writing." << endl;
        return 1;
    }
    for (auto *v : vertices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for (auto *e : edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();

    cout << "done, result saved to result_lie.g2o" << endl;
    return 0;
}
2.CMakeLists.txt
cpp 复制代码 
cmake_minimum_required(VERSION 3.16)
project(slam_10)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -march=native -g")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o
list(APPEND CMAKE_MODULE_PATH /home/caohao/g2o/cmake_modules)
find_package(G2O REQUIRED)
set(G2O_LIBS g2o_core g2o_stuff g2o_types_slam3d g2o_csparse_extension cxsparse)
include_directories(${G2O_INCLUDE_DIRS})

# Eigen
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})

# --- Executables ---
add_executable(pose_graph_g2o_SE3 pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3 ${G2O_LIBS} ${CHOLMOD_LIBRARIES})

add_executable(pose_graph_g2o_lie pose_graph_g2o_lie_algebra.cpp)
target_link_libraries(pose_graph_g2o_lie ${G2O_LIBS} ${CHOLMOD_LIBRARIES} ${Sophus_LIBRARIES})
3.注意事项

（1）sphere.g2o文件较大，如有需要后台给笔者留言；

（2）程序运行时注意指令使用，笔者sphere.g2o在路径~/桌面/slam-14lectures/slam_10下。

4.运行结果
相关推荐
alibli2 小时前
一文学会CMakeLists.txt: CMake现代C++跨平台工程化实战
开发语言·c++·系统架构
在逃牛马2 小时前
【Uni-App+SSM 宠物项目实战】Day12：宠物信息添加
后端
YuTaoShao2 小时前
【LeetCode 每日一题】36. 有效的数独
linux·算法·leetcode
攻城狮7号2 小时前
HunyuanVideo-Foley模型开源，让AI视频告别“默片时代”
人工智能·hunyuanvideo·foley·混元开源模型·ai音频
MMjeaty2 小时前
map/multimap容器
数据结构·c++
IT古董2 小时前
【漫话机器学习系列】003.Agglomerative聚类
人工智能·算法·机器学习
Juchecar2 小时前
一文讲清 torch、torch.nn、torch.nn.functional 及 nn.Module
人工智能
muchan922 小时前
为什么“它”在业务逻辑上是最简单的？
前端·后端·面试
热门推荐
01GitHub 镜像站点02KGG转MP3工具|非KGM文件|解密音频03A股预测还能更准？开源大模型Kronos带你跑通预测+回测全流程04UV安装并设置国内源0546个Nano-banana 精选提示词，持续更新中06UV 工具安装与国内镜像源配置指南07conda中设置镜像地址（附所有可换的地址）08智能库存管理的需求预测模型：从业务痛点到落地代码的完整实践09突破百度网盘的下载限速，两种方法教会你【超详细】10Spec-Kit 使用指南