图优化基础：图论入门、顶点与边、最小二乘与图优化的关系

各位同学好，今天我们进入图优化的世界。说实话，我第一次接触图优化时，也被一堆术语搞得头大。什么顶点、边、雅可比矩阵……但后来我发现，这东西说白了就是「用图来组织优化问题」。你想想看，SLAM 里那么多位姿和观测，乱糟糟的，图优化就是给它们排个队、建个关系网。

图论入门：SLAM 里的「关系网」

图论听起来高大上，其实核心就两个东西：顶点和边。顶点代表「什么东西」，边代表「它们之间有什么关系」。

在视觉 SLAM 里，顶点通常是机器人的位姿（位置+朝向），或者地图里的路标点。边呢，就是观测约束——比如你在位置 A 看到了路标点 P，那 A 和 P 之间就有一条边。我习惯把边想象成一根橡皮筋，它拉着顶点，让它们别跑太偏。

举个例子。你拿着相机拍了三张照片，每张照片里都看到了同一个墙角。那这三张照片的位姿和那个墙角点之间，就形成了一个小图。嗯，这里要注意：图不一定是连成一片的，有时候会有孤立顶点，但 SLAM 里我们通常处理的是连通图。

顶点与边：SLAM 里的「演员」和「剧本」

咱们具体看看顶点和边在代码里长什么样。我用的是 g2o 库，这是图优化的一个经典工具。

// 顶点：机器人的位姿（6自由度）
class VertexPose : public g2o::BaseVertex<6, SE3> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() override {
        _estimate = SE3();
    }
    virtual void oplusImpl(const double* update) override {
        // 更新位姿，用李代数扰动
        Vector6d upd;
        upd << update[0], update[1], update[2],
               update[3], update[4], update[5];
        _estimate = SE3::exp(upd) * _estimate;
    }
    // 读盘、存盘函数省略...
};

你看，顶点需要告诉优化器两件事：一是初始值怎么设（setToOriginImpl），二是怎么更新自己（oplusImpl）。我在项目中遇到过一个问题：如果更新方式写错了，优化会发散。比如位姿更新应该用左乘扰动，有人写成右乘，结果图直接炸了。

边呢？边就是误差函数。比如重投影误差：

// 边：重投影误差
class EdgeProjection : public g2o::BaseBinaryEdge<2, Vector2d, VertexPose, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void computeError() override {
        // 获取顶点
        const VertexPose* v1 = static_cast<const VertexPose*>(_vertices[0]);
        const VertexPoint* v2 = static_cast<const VertexPoint*>(_vertices[1]);
        // 计算投影
        Vector2d proj = camera->project(v1->estimate() * v2->estimate());
        // 误差 = 观测 - 投影
        _error = _measurement - proj;
    }
    virtual void linearizeOplus() override {
        // 计算雅可比矩阵（关键！）
        // 这里省略具体推导...
    }
};

最小二乘与图优化的关系：从「解方程」到「调参数」

好，现在图建好了，顶点和边都定义好了。接下来怎么优化？这就涉及到最小二乘了。

你想想看，每个边都代表一个误差。比如重投影误差，我们希望它越小越好。把所有边的误差平方加起来，就得到一个总代价函数：

F(x) = Σ e_i^T Ω_i e_i

其中 e_i 是第 i 条边的误差向量，Ω_i 是信息矩阵（说白了就是权重，告诉优化器这条边有多可信）。

图优化要做的，就是找到一组顶点值 x，让 F(x) 最小。这本质上就是一个非线性最小二乘问题。为什么是非线性？因为投影模型、位姿变换都是非线性的。

怎么解？最常用的方法是高斯-牛顿法或 LM 算法。核心思路是：在当前估计值附近做线性化，然后解一个线性方程组，得到更新量，再迭代。

我记得有一次做后端优化，地图里有 5000 个位姿和 10 万个路标点。如果用传统方法，光雅可比矩阵就有 60 万列，存都存不下。但用图优化，利用稀疏性，内存占用直接降到十分之一。

图优化的核心流程：一张图说清楚

下面我用一张 SVG 图来总结图优化的核心逻辑。这张图我画了很多遍，每次讲课都会用。

这张图展示了图优化的迭代过程。你从输入开始，构建图，然后反复做「线性化 → 求解 → 更新 → 判断收敛」。直到误差不再下降，就输出结果。

图优化的稀疏性：为什么它能处理大规模问题

图优化能处理大规模 SLAM 问题，关键就在于稀疏性。你看，每个边只连接少数几个顶点（通常是 2 个）。所以整个雅可比矩阵是稀疏的，Hessian 矩阵也是稀疏的。

具体来说，Hessian 矩阵 H = J^TJ 的结构是这样的：

顶点类型	H 矩阵结构	特点
只有位姿顶点	块对角 + 少量非对角	相邻位姿才有连接
位姿 + 路标点	箭头形（箭形矩阵）	路标点只连接一个位姿
多传感器融合	更复杂的稀疏模式	需要自定义边缘化策略

利用这个稀疏性，我们可以用 Schur 补（也叫边缘化）来加速求解。说白了就是先把路标点消掉，只求解位姿，然后再反求路标点。这样求解规模从 (N+M) 降到了 N，而 N 通常远小于 M。

总结一下

图优化不是什么神秘的东西。它就是：

• 顶点：你要优化的变量（位姿、路标点）
• 边：约束条件（观测误差）
• 最小二乘：把所有边的误差平方加起来，求最小值
• 稀疏性：让大规模求解成为可能

你想想看，SLAM 后端本质上就是在做这件事。只不过图优化给了我们一个清晰的框架，把问题组织得井井有条。我个人觉得，理解图优化最好的方式就是动手写一个简单的例子——比如两个位姿、一个路标点，手算一遍优化过程。嗯，这个作业我会在后面布置。

好了，图优化的基础就讲到这里。记住：图是骨架，最小二乘是灵魂，稀疏性是效率。这三者缺一不可。

---