【视觉SLAM笔记】第9章 后端1

一、理论

1. 状态估计的概率解释

我们来深入探讨一下视觉SLAM中状态估计的概率解释。这可以说是理解现代SLAM算法（尤其是后端优化）的基石

1. 问题的核心：不确定性

SLAM（同步定位与建图）的本质是在一个未知环境中，用一个移动的传感器（如相机）来同时估计自身的运动轨迹（定位）和环境的结构（建图）。

这个过程充满了不确定性（Uncertainty）：

• 运动模型不准

  ：我们通过轮式里程计、IMU或帧间匹配来估计相机的运动，但这些估计都有误差，会随着时间累积。

• 观测模型不准

  ：相机在拍摄环境中的特征点（路标点）时，由于光照、相机分辨率、图像噪声等因素，我们提取到的像素坐标也不是100%精确的。

概率论正是用来数学化地描述和处理这种不CETP的完美工具。因此，SLAM问题被自然地建模成一个概率问题。

2. 线性系统和KF

我们接着上一个话题，从更基础的线性系统和卡尔曼滤波 (Kalman Filter, KF) 开始。这不仅是理解EKF-SLAM的必要前奏，其本身也是控制论和信号处理领域最璀璨的成果之一。

理解了KF，你就能明白EKF是如何通过“作弊”（线性化）来处理非线性问题的。

3. 非线性系统与EKF

我们现在进入了从理论到实践的关键一步：非线性系统与扩展卡尔曼滤波 (EKF)。这正是为了解决视觉SLAM等真实世界问题而对经典卡尔曼滤波进行的“扩展”。

总结一下变化：

• 在计算状态均值时，我们使用原始的非线性函数 f 和 h，因为这是对真实物理过程的最佳描述。

• 在计算协方差（不确定性）的传播和更新时，我们使用雅可比矩阵 F 和 H，因为协方差的演化需要一个线性的框架。

4. BA与图优化

我们现在已经到达了现代视觉SLAM技术的核心——捆集调整 (Bundle Adjustment, BA) 与图优化 (Graph Optimization)。这可以说是视觉SLAM后端优化中的“圣杯”，是获得高精度轨迹和地图的关键。

核心关系：一般与特殊

首先，我们来理清这两个概念的关系：

• 图优化 (Graph Optimization)

  ：是一种更广泛、更通用的优化理论框架。它将一个复杂的优化问题建模成一个图（Graph），其中节点代表待优化的变量，边代表变量之间的约束（或误差）。它的目标是调整节点的值，使得所有边所代表的误差之和最小。

• 捆集调整 (Bundle Adjustment, BA)

  ：是图优化理论在视觉SLAM领域中最核心、最成功的具体应用。它专门用来优化相机位姿和三维地图点，其误差项是特定的“重投影误差”。

所以，你可以这样理解：BA 是一种特殊的图优化。

5. 投影模型与BA代价函数

这是理解BA如何工作的关键一步。我们将把这个过程分解成两个部分：

1. 几何部分

   ：一个三维点是如何在物理上变成一个二维像素的？这就是投影模型 (Projection Model)。

2. 数学部分

   ：如何用一个数学公式来衡量我们的估计（位姿和地图点）与真实观测之间的差距？这就是BA代价函数 (Cost Function)。

上图中，蓝色点是实际观测，红色叉是三维点根据当前估计重投影的位置，它们之间的像素距离就是重投影误差。

上面的解释为了简化，使用的是最理想的针孔相机模型 (Pinhole Camera Model)，它假设光线沿直线穿过一个无限小的孔。

然而，现实世界中的相机镜头是由多片凸透镜和凹透镜组合而成的，这会导致光线在通过镜头时发生弯曲，使得最终成像的位置与理想模型预测的位置有偏差。这种偏差就是相机畸变 (Camera Distortion)。

不考虑畸变，SLAM系统的精度会受到严重影响，尤其是在使用广角或鱼眼镜头时。因此，在实际的BA中，畸变模型是必须考虑的一部分。

6. BA的求解

我们来深入剖析BA的求解过程。这是一个将理论（代价函数）转化为实际代码和高效计算的关键环节。

7. 鲁棒核函数

这是一个非常深入且至关重要的话题。在真实的SLAM应用中，仅仅使用标准的最小二乘法（即BA代价函数）是远远不够的，因为我们无法保证所有的数据都是完美的。鲁棒核函数 (Robust Kernel Function)，也常被称为鲁棒代价函数 (Robust Cost Function)，正是为了解决这个问题而生的。

特性 (Feature)	Huber 核函数 (Huber)	Cauchy 核函数 (Cauchy)	Geman-McClure 核函数 (Geman-McClure)	Tukey 核函数 (Tukey)
鲁棒性 (Robustness)	中等 (Medium)	强 (Strong)	强 (Strong)	最强 (Strongest)
处理大误差方式 (How Large Errors are Handled)	线性惩罚 (Linear Penalty)	权重衰减 (Weight Decay)	权重衰减 (Weight Decay)	完全忽略 (Completely Ignored)
凸性 (Convexity)	条件凸 (Conditionally Convex)	非凸 (Non-convex)	非凸 (Non-convex)	非凸 (Non-convex)
可导性 (Differentiability)	连续可导 (Continuously Diff.)	连续可导 (Continuously Diff.)	连续可导 (Continuously Diff.)	某些点不可导 (Not Diff. at some points)
参数 (Parameter)	δ	c	c	c
适用场景 (Applicable Scenarios)	对异常值有一定容忍，但又不想完全抛弃其信息时	异常值较多且希望强鲁棒性时	类似 Cauchy 核函数，异常值较多时	异常值非常离散，希望完全忽略其影响

在实际应用中，选择哪种核函数取决于具体的数据特性和对鲁棒性的需求。通常，Huber 核函数是一个很好的起点，因为它在鲁棒性和优化难度之间取得了较好的平衡。如果异常值非常严重且需要强烈的抑制，那么 Cauchy、Geman-McClure 或 Tukey 核函数可能更合适，但需要注意非凸性可能带来的局部最优问题。在机器人和计算机视觉的位姿估计 (pose estimation) 等问题中，由于存在大量错误匹配 (mismatches) 导致的异常值，这些鲁棒核函数扮演着至关重要的角色。

二、代码笔记

1. bundle_adjustment_ceres.cpp

基于 Ceres 的稀疏 Schur 结构 Bundle Adjustment 囊括了：

• BA 的变量与观测模型；

• 重投影误差与鲁棒损失；

• 稀疏 Schur 求解策略；

• 在 Ceres 中的配置与求解流程。

该程序是一个使用Ceres Solver库来解决大规模光束法平差 (Bundle Adjustment, BA)问题的完整示例。BA是视觉SLAM和三维重建中至关重要的后端优化步骤。

程序的主要工作流程如下：

1. 数据加载与预处理：

• 多个相机的初始位姿和内参（如焦距、畸变系数）。

• 大量三维空间点的初始坐标。

• 大量的观测数据，即哪个相机看到了哪个3D点，以及该点在该相机图像上的2D像素坐标。

• 程序从一个特定格式（BAL格式）的文件中加载一个BA问题。这个文件包含了：

• 为了提高数值稳定性，程序对加载的数据进行了归一化。

• 为了模拟真实的优化场景，程序对完美的初始数据添加了随机扰动，创造了一个需要被优化的非理想状态。

• 构建优化问题 (SolveBA函数)：

• • 代价函数 (Cost Function)

    ：一个SnavelyReprojectionError实例，它定义了如何根据给定的相机和3D点计算重投影误差。

  • 损失函数 (Loss Function)

    ：一个HuberLoss实例。这是一个鲁棒核函数，用于减小由错误匹配或外点（outliers）引起的巨大误差对整体优化的不良影响。

  • 待优化参数块 (Parameter Blocks)

    ：指向该观测所关联的相机参数和3D点参数的内存地址。Ceres将通过调整这些内存中的数值来最小化代价函数。

  • 程序的核心是构建一个Ceres的Problem对象。

  • 它遍历每一条观测数据（一个camera-point-2D_observation的组合）。

  • 对于每条观测，它创建一个残差块 (Residual Block) 并添加到Problem中。每个残差块包含：

• 求解与结果输出：

• • 程序配置了Ceres的求解器。最关键的配置是选择了SPARSE_SCHUR作为线性求解器。这种求解器利用了BA问题中雅可比矩阵的稀疏结构（一个相机只看到一部分点，一个点只被一部分相机看到），通过舒尔补 (Schur Complement)技巧，极大地提高了求解大规模BA问题的效率。

  • 调用ceres::Solve()启动优化过程。Ceres会迭代地调整所有相机参数和所有3D点坐标，以联合最小化所有观测的重投影误差之和。

  • 优化完成后，程序打印详细的报告，并将优化前后的相机位姿和3D点云保存为PLY文件，以便通过MeshLab等工具进行可视化比较。

  

  2.bundle_adjustment_g2o.cpp

  扩展相机内参的 g2o Bundle Adjustment总结：

  • 相机位姿与内参的联合参数化；

  • 畸变与透视投影模型；

  • 鲁棒残差定义与边的构造；

  • 基于 g2o 的 Levenberg–Marquardt 优化流程。

  

  该程序使用g2o (General Graph Optimization)库来解决与前一个Ceres示例完全相同的大规模光束法平差 (Bundle Adjustment, BA)问题。它展示了如何使用g2o的框架将BA问题建模为一个图，并进行高效求解。

  程序的主要工作流程如下：

  1. 数据加载与预处理：与Ceres示例完全相同，程序首先从BAL文件中加载相机、地图点和观测数据，并对其进行归一化和扰动。

  2. 定义图的元素：这是使用g2o的核心步骤。

  • EdgeProjection

    ：这是一个二元边，连接一个VertexPoseAndIntrinsics和一个VertexPoint。它代表了一个重投影误差的约束。computeError函数中定义了如何根据连接的两个顶点的当前估计值来计算预测的2D投影，并与真实的2D观测值比较得到误差。本例中，该边没有手动实现雅可比矩阵，而是依赖g2o的数值求导功能。

  • VertexPoseAndIntrinsics

    ：代表一个相机的位姿和内参。这是一个9维的优化变量，其内部数据用一个自定义的PoseAndIntrinsics结构体来存储，该结构体使用Sophus::SO3d来处理旋转，保证了其在李群流形上的正确性。该顶点类还定义了如何进行增量更新（oplusImpl）以及如何将3D点投影到图像上（project）。

  • VertexPoint

    ：代表一个三维地图点，是一个简单的3维向量。

  • 顶点 (Vertex)

    ：程序定义了两类顶点：

  • 边 (Edge)

    ：程序定义了一类边：

• 构建图 (SolveBA函数)：

• • 程序创建了一个g2o的SparseOptimizer。

  • 遍历所有相机和地图点数据，创建相应的顶点实例，并设置其初始估计值，然后添加到优化器中。

  • 特别地，它将所有地图点顶点设置为待边缘化 (setMarginalized(true))。这是一个关键的性能优化步骤，它告诉g2o在求解线性系统时可以使用舒尔补技巧，先消去所有地图点，求解相机位姿，再反向代入求解地图点，这与Ceres的SPARSE_SCHUR求解器原理相同。

  • 遍历所有观测数据，创建EdgeProjection实例，将其连接到对应的相机和地图点顶点，并设置观测值、信息矩阵和鲁棒核函数，然后添加到优化器中。

• 求解与结果输出：

• • 程序配置求解器使用LM（Levenberg-Marquardt）算法和CSparse稀疏线性求解器。

  • 调用optimizer.optimize()启动优化。

  • 优化完成后，从g2o的顶点中提取出优化后的相机参数和地图点坐标，并写回到BALProblem对象中，以便保存到PLY文件进行可视化。

  

  3.common.cpp

   BALProblem 类及其读写、归一化与扰动流程

  • 数据读取与参数布局；

  • 角轴↔四元数转换；

  • 点云（PLY）导出；

  • 基于中值和 MAD 的几何归一化；

  • 对点与相机（旋转、平移）添加高斯扰动。

  

  该代码定义并实现了一个名为 BALProblem 的类，其核心功能是管理和处理用于大规模光束法平差 (Bundle Adjustment, BA) 的数据集。这个类是许多BA求解器示例（如Ceres和g2o）的数据处理前端，负责将原始数据文件加载到内存中，并提供一系列工具函数来操作这些数据。

  主要功能点如下：

  1. 数据加载与解析：

  • 构造函数 BALProblem::BALProblem 负责读取一个特定格式（BAL格式）的文本文件。

  • 它解析文件内容，将相机参数（旋转、平移、内参）、三维点坐标以及成千上万的观测数据（哪个相机看到了哪个点，以及2D像素坐标）加载到内存中的动态数组里。

  • 它还支持在加载时将相机的旋转表示从轴角 (Angle-Axis) 转换为四元数 (Quaternion)，因为四元数在某些优化场景下表现更好（无奇异点）。

• 数据预处理与准备：

• • 归一化 (Normalize)

    ：实现了一个非常重要的预处理步骤。通过将整个场景的坐标系进行平移和缩放，使得3D点云的中心位于原点，并且点的离散程度（通过中位绝对偏差衡量）被缩放到一个固定的尺度（如100）。这可以极大地改善BA优化过程的数值稳定性和收敛速度。

  • 添加扰动 (Perturb)

    ：为了模拟一个真实的、带有误差的初始估计，该函数可以对加载的（通常是真实的）相机位姿和3D点坐标添加指定标准差的高斯噪声。这为后续的BA优化提供了一个有待改进的起点。

• 数据格式转换与访问：

• • 类内部封装了相机参数在不同表示法之间的转换逻辑，例如从优化器友好的 (R, t) 表示法转换为更直观的相机中心 c 表示法（CameraToAngelAxisAndCenter）及其逆运算。

  • 提供了访问内部数据（如相机、点、观测）的接口函数（mutable_cameras(), mutable_points(), etc.）。

• 结果输出与可视化：

• • WriteToFile

     函数可以将内存中的（可能是优化后的）BA问题数据写回到一个BAL格式的文件中。

  • WriteToPLYFile

     函数可以将相机位姿（以相机中心表示）和3D点云导出为一个.ply 文件。PLY是一种标准的三维模型文件格式，可以用MeshLab、CloudCompare等软件打开，从而直观地可视化优化前后的相机位姿和场景结构。

  

  4.common.h BALProblem 类的核心数据布局与接口

  如何以一维数组管理 BA 问题的索引、观测与参数，并提供归一化、扰动及读写接口。

  

  这个头文件定义了一个名为 BALProblem 的C++类，其核心功能是作为一个数据封装和管理工具，专门用于处理和操作大规模光束法平差（Bundle Adjustment, BA）问题的数据。

  该类的设计目标是：

  1. 数据抽象与封装：将从BAL数据文件中读取的复杂数据（包括相机数量、点数量、观测数量、大量的相机参数、3D点坐标和2D观测值）封装在一个单一的对象中。这使得上层应用（如BA求解器）可以方便地通过一个BALProblem对象来访问整个问题，而无需关心底层的文件解析和内存管理细节。

  2. 内存管理：该类在构造时动态分配所需的大块内存来存储所有数据，并在析构时负责释放这些内存，避免了内存泄漏。

  3. 数据访问接口：它提供了一套全面、清晰的**接口函数（API）**来访问数据。这些接口分为两类：

  • 只读 (const) 访问

    ：用于安全地获取数据，如相机数量 num_cameras()，或指向某个观测对应参数的const指针 camera_for_observation()。

  • 可修改 (mutable) 访问

    ：提供可修改的指针，如mutable_cameras()，允许优化器（如Ceres或g2o）直接在这些内存上修改参数值。

• 实用工具函数：该类还包含了一系列高级的、与BA问题紧密相关的工具函数：

• • WriteToFile

     和 WriteToPLYFile：用于结果的保存和可视化。

  • Normalize

    ：提供数据归一化功能，这是改善大规模优化问题数值稳定性的关键步骤。

  • Perturb

    ：用于给数据添加噪声，以模拟真实的、非理想的初始状态，从而可以测试优化算法的性能。

  • CameraToAngelAxisAndCenter

     等私有函数：封装了相机位姿不同表示法之间的转换，简化了类的内部实现。

• 灵活性：通过use_quaternions参数，该类支持在内部使用轴角或四元数来表示旋转，为不同的优化策略提供了灵活性。

• 总之，BALProblem类是一个精心设计的、功能完备的BA问题数据管理器，它极大地简化了开发和测试大规模BA求解器的过程。

  这个头文件本身不执行复杂的数学计算，但它定义了存储和访问各种数学实体的数据结构。

  

  5.random.h

  

  这个头文件 (random.h 或 rand.h) 提供了一组简单而实用的随机数生成工具函数。它的主要功能是：

  1. 生成均匀分布随机数 (RandDouble)：

  • 该函数利用C标准库的 rand() 函数生成一个伪随机整数。

  • 然后通过归一化操作，将其转换为一个在 [0.0, 1.0] 区间内均匀分布的 double 类型浮点数。

  • 这个函数是生成其他分布随机数的基础。

• 生成标准正态分布随机数 (RandNormal)：

• • 该函数实现了一种名为Marsaglia polar method 的算法。

  • 该算法能够从两个独立的均匀分布随机数高效地生成两个独立的标准正态分布（也称高斯分布，其均值为0，标准差/方差为1）的随机数。

  • 函数最终返回其中一个生成的正态分布随机数。

  • 这种随机数在模拟现实世界中的测量噪声、初始化算法等多种场景下都非常有用。

  这两个函数都被声明为 inline，这是一种性能优化建议，对于这样短小的函数，编译器很可能会采纳这个建议，从而避免函数调用的开销。

  注意：该实现依赖于 rand()，它是一个比较基础的伪随机数生成器。为了获得更高质量或可复现的随机序列，现代C++（C++11及以后）推荐使用 <random> 库中的工具，如 std::mt19937 和 std::uniform_real_distribution / std::normal_distribution。但对于许多简单应用，此处的实现是足够且方便的。

  

  6.rotation.h

  • 基本的点积与叉积计算；

  • 角轴与四元数之间的稳定互转公式及其零点近似；

  • 使用 Rodrigues 公式（及其小角近似）对三维点执行旋转变换。

  

  这个头文件 (rotation.h) 提供了一组底层、高效且对自动求导友好的三维空间旋转计算函数。这些函数都使用了C风格的数组指针作为输入输出，并被设计为模板函数，使其可以与double, float，以及像Ceres Solver中的ceres::Jet这样的特殊类型一起工作。

  其核心功能包括：

  1. 基本向量运算：

  • DotProduct

    ：计算两个三维向量的点积。

  • CrossProduct

    ：计算两个三维向量的叉积。

• 旋转表示法之间的转换：

• • AngleAxisToQuaternion

    ：将轴角 (Angle-Axis) 表示的旋转转换为四元数 (Quaternion)。轴角表示为一个三维向量，其方向为旋转轴，模长为旋转角度。

  • QuaternionToAngleAxis

    ：执行相反的转换，将四元数转换为轴角。

• 应用旋转：

• • AngleAxisRotatePoint

    ：使用轴角表示的旋转来旋转一个三维点。这个函数是核心，它内部实现了完整的罗德里格斯旋转公式 (Rodrigues' Rotation Formula)。

• 数值稳定性与自动求导支持：

• • 所有函数都特别处理了旋转角度接近零的退化情况。在这种情况下，标准公式可能会因为除以一个接近零的数而变得数值不稳定。代码通过使用泰勒级数展开来近似计算，这不仅解决了数值问题，还确保了当使用自动求导库（如Ceres）时，能够计算出正确且有意义的导数。

  • 使用 std::numeric_limits<double>::epsilon() 作为判断是否接近零的阈值，这是一个健壮的做法。

  这个头文件是许多基于优化的三维计算机视觉应用（如BA）的基础组件，因为它提供了将旋转表示（如轴角）和其对点的作用（旋转）进行计算的能力，并且这些计算对于优化求解器是“透明的”（即可以被自动求导）。

  

  7. SnavelyReprojectionError.h

  在 Ceres 中实现带径向畸变的相机重投影误差模型及其自动求导。

  

  这个头文件定义了一个名为 SnavelyReprojectionError 的C++类，其唯一且核心的功能是为Ceres Solver框架定义一个代价函数（Cost Function），用于计算在Bundle Adjustment (BA)问题中的重投影误差。

  该类的主要特点和功能如下：

  1. 封装BA的代价函数：它将BA中一个核心的数学概念——重投影误差——封装成一个符合Ceres框架要求的C++类。这个误差是指一个三维空间点根据当前的相机位姿和内参估计投影到图像上时，其预测的2D位置与真实的2D观测位置之间的差异。

  2. 实现Snavely相机模型：在静态函数 CamProjectionWithDistortion 中，它完整地实现了Snavely BAL数据集所使用的相机模型。这个模型包括：

  • 用轴角（Angle-Axis）表示的旋转。

  • 三维平移。

  • 一个简化的内参模型，只包含焦距 f和两个径向畸变系数 k1,k2
    。

• 支持Ceres自动求导：

• • 通过将核心的 operator() 定义为模板函数，该类能够与Ceres的自动求导机制无缝协作。这意味着开发者无需手动计算复杂且容易出错的雅可比矩阵。Ceres会在编译时自动生成计算导数的代码。

• 提供工厂方法 (Create)：

• • 它提供了一个静态的Create方法，这是一个设计模式中的工厂方法。这个方法简化了创建ceres::AutoDiffCostFunction实例的过程，将复杂的模板参数和new操作封装起来，使得上层调用代码更简洁、更易读。

  总之，SnavelyReprojectionError.h是Ceres BA示例中的一个关键组件，它精确地定义了优化问题的目标函数中的误差项，并利用Ceres的特性，使得构建一个复杂的大规模优化问题变得相对简单。