Other Examples

除了本章中的示例之外， example 目录还包含许多其他示例：

1. bundle_adjuster.cc

展示了如何使用 Ceres 的各种功能来解决 Boundle Adjustment 问题。

2. circle_fit.cc

展示了如何进行圆的拟合。

3. ellipse_approximation.cc

fits points randomly distributed on an ellipse with an approximate line segment contour. This is done by jointly optimizing the control points of the line segment contour along with the preimage positions for the data points. The purpose of this example is to show an example use case for Solver::Options::dynamic_sparsity, and how it can benefit problems which are numerically dense but dynamically sparse.

4. denoising.cc

实现了通过 Fields of Experts 模型来进行图像去噪。

5. nist.cc

实现并尝试解决 NIST 非线性回归问题。

6. more_garbow_hillstrom.cc

论文中测试问题的子集。

Testing Unconstrained Optimization Software Jorge J. More, Burton S. Garbow and Kenneth E. Hillstrom ACM Transactions on Mathematical Software, 7(1), pp. 17-41, 1981

which were augmented with bounds and used for testing bounds constrained optimization algorithms by

A Trust Region Approach to Linearly Constrained Optimization David M. Gay Numerical Analysis (Griffiths, D.F., ed.), pp. 72-105 Lecture Notes in Mathematics 1066, Springer Verlag, 1984.

7. libmv_bundle_adjuster.cc

是 Blender/libmv 使用的 Boundle Adjustment 算法。

8. libmv_homography.cc

该文件演示了如何通过两组点求解 homography matrix，通过自定义 callback 检查 image-space error 来判断是否退出优化。

9. robot_pose_mle.cc

演示如何使用 CostFunction 的 DynamicAutoDiffCostFunction 变体。在编译时不知道参数块数量或大小的情况下可使用 DynamicAutoDiffCostFunction 。本示例模拟了一个机器人在一维走廊上行走，带有里程计和 range 测量，但是都带有噪声。通过融合里程计和传感器读数，本示例演示了如何在每个时间步计算机器人姿态的最大似然估计值 (MLE)。

10. slam/pose_graph_2d/pose_graph_2d.cc

同步定位和建图（SLAM）问题包括绘制未知环境的地图，根据该地图进行定位，且两者同步进行。这个问题的主要困难在于没有任何额外的外部辅助信息（如全球定位系统）。SLAM 一直被认为是机器人技术的基本挑战之一。位姿图优化就是 SLAM 问题之一。下面将解释如何在具有相对姿态约束的二维空间中提出基于位姿图的 SLAM 问题。

考虑一个在二维平面上移动的机器人。机器人可以使用一组传感器，如轮式里程计或激光测距仪。根据这些原始测量数据，我们希望估算出机器人的运动轨迹，并绘制出环境地图。为了降低问题的计算复杂度，位姿图方法将原始测量数据抽象化。具体来说，它创建了一个由节点和边组成的图，节点代表机器人的位姿，边代表两个节点之间的相对变换（旋转和平移），边是由原始传感器测量数据导出的虚拟测量数据，例如，通过整合原始轮式里程测量数据和从机器人获取的激光测距扫描数据，我们可以计算出一个相对的旋转和平移变换。下图显示了所生成图形的可视化效果。

Visual representation of a graph SLAM problem

该图将机器人的位姿描述为三角形，测量值由实线表示，其中闭环测量为虚线。闭环测量发生在两个非连续的机器人状态之间，可以有效的减少累积误差，接下来将讲述位姿图优化的数学建模过程。

假设在 $t$ 时刻，机器人的状态可以表示为 $x_t=[p^{T},\psi^{T}]$，其中 $p$ 是一个二维向量代表机器人的位置，$\psi$ 代表机器人的方向，用弧度制表示。机器人在 $a,b$ 两个时刻之间的相对测量表示为：$z_{ab}=[\hat{p}^{T}_{ab},\hat{\psi}_{ab}^{T}]$。则 Ceres 的 CostFunction 计算的残差为预测值和测量值之间的差：

$$r_{a b}=\left[\begin{array}{c} R_{a}^{T}\left(p_{b}-p_{a}\right)-\hat{p}_{a b} \\ \text { Normalize }\left(\psi_{b}-\psi_{a}-\hat{\psi}_{a b}\right) \end{array}\right]$$

Normalize() 函数将角度标准化在 $[-\pi, \pi)$ ，$R$ 是 $SO(2)$ 上的旋转矩阵。

$$R_{a}=\left[\begin{array}{cc} \cos \psi_{a} & -\sin \psi_{a} \\ \sin \psi_{a} & \cos \psi_{a} \end{array}\right]$$

我们需要根据测量的不确定性对残差进行加权。因此，我们将残差预先乘以协方差矩阵的 inverse square root，例如 $\sum^{-\frac{1}{2}}_{ab}r_{ab}$，其中 $\sum_{ab}$ 是协方差矩阵。最后我们通过 manifold 上的方法将旋转标准化在 $[-\pi,\pi)$ 。具体来书，Ceres 在 example 中定义了 AngleManifold::Plus() 函数实现 $Normalize(\psi+\Delta)$ ，AngleManifold::Minus() 函数去实现 $Normalize(y)-Normalize(x)$。

Ceres 包含一个 pose_graph_2d 可执行文件，将会读取一个定义位姿图优化问题的文件，通常是 g2o 格式的，当然，读取其他格式（例如 TORO ）的文件也不难。pose_graph_2d 运行时将会打印整个 Ceres 的优化过程和结果，并且会将优化过的位姿输入到磁盘中去，分别是 poses_original.txt 和 poses_optimized.txt，格式如下：

pose_id x y yaw_radians
pose_id x y yaw_radians
pose_id x y yaw_radians

其中 pose_id 是文件定义中相应的整数ID。请注意，文件将按 pose_id 的升序排序。在执行 pose_graph_2d 时的第一个参数是定义优化问题的文件路径，按照如下指令运行：

/path/to/bin/pose_graph_2d /path/to/dataset/dataset.g2o

Ceres 提供了一个 Pyhton 脚本去可视化运行结果

/path/to/repo/examples/slam/pose_graph_2d/plot_results.py\
 --optimized_poses ./poses_optimized.txt --initial_poses\
  ./poses_original.txt

输入为 Edwin Olson 创建的 standard synthetic benchmark，该数据集在网格世界中有 3500 个节点，总共 5598 个边。使用提供的脚本可视化结果会得到如下结果：

Optimization Result

原始姿势为绿色，优化姿势为蓝色。如图所示，优化后的位姿更加规律整齐。需要注意的是，由于缺乏相对约束来提供足够的信息来重建轨迹，图的左侧具有较小的 Yaw 轴漂移。​

11. slam/pose_graph_3d/pose_graph_3d.cc

接下来的例子是展示如何在三维空间中对位姿图优化问题进行建模。同时展示了如何使用基于 EIgen 的几何模块使用 Ceres 的自动求导进行问题的构建和求解。假设机器人在时刻 $t$ 的状态为 $x_t=[p^{T},q^{T}]$，其中 $p^T$ 代表三维空间中的位置，$q^{T}$ 为旋转四元数代表的是三维空间中的姿态，使用 Eigen 的 quaternion 数据结构进行表示。机器人从 $a$ 时刻到 $b$ 时刻的相对测量可以表示为 $z_{ab}=[\hat{p}^{T}_{ab},\hat{1}^{T}_{ab}]$，预测值和观测值之间的误差计算方式如下：

$$r_{a b}=\left[\begin{array}{c} R\left(q_{a}\right)^{T}\left(p_{b}-p_{a}\right)-\hat{p}_{a b} \\ 2.0 \operatorname{vec}\left(\left(q_{a}^{-1} q_{b}\right) \hat{q}_{a b}^{-1}\right) \end{array}\right]$$

函数 $vec()$返回四元数的 $[q_x,q_y,q_z]$，$R(q)$ 是四元数代表的旋转矩阵。为了完成该 CostFunction 的计算，我们还需要对残差加上一个权重系数，因此，我们将残差预先乘以协方差矩阵的 inverse square root，例如 $\sum^{-\frac{1}{2}}_{ab}r_{ab}$，其中 $\sum_{ab}$ 是协方差矩阵。

我们使用四元数来表示旋转，当对其进行更新的时候需要使用 manifold 上的加法，Eigen 的四元数在内存中的存储布局和我们通常使用的方式略有不同，Eigen 在表示时四元数顺序为 $[x,y,z,w]$，实部为最后一个，一般我们是将实部放在第一个。注意，当使用 Eigen 的 Quaternion 去构造四元数时，传入参数的顺序应该为 $w,x,y,z$，因为 Ceres 操作参数块使用的是原始指针，这个对参数块的影响很大。

Ceres 的 example 中有一个 pose_graph_3d 的可执行程序，它将会读取定义位姿图优化的问题的文件，该文件通常是 g2o 格式的，当然，读取其他格式（例如 TORO ）的文件也不难。pose_graph_3d 运行时将会打印整个 Ceres 的优化过程和结果，并且会将优化过的位姿输入到磁盘中去，分别是 poses_original.txt 和 poses_optimized.txt，格式如下：

pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
...

其中 pose_id 是文件定义中相应的 ID。请注意，文件将按 pose_id 的升序排序。在执行 pose_graph_3d 时的第一个参数是定义优化问题的文件路径，按照如下指令运行：

/path/to/bin/pose_graph_3d /path/to/dataset/dataset.g2o

提供了一个脚本来可视化生成的输出文件。还有一个选项可以使用 --axes_equal 启用 equal axes

/path/to/repo/examples/slam/pose_graph_3d/plot_results.py\
--optimized_poses ./poses_optimized.txt \
--initial_poses ./poses_original.txt

最终，解决了一个 standard synthetic benchmark 数据集，其中机器人在球体表面上行驶，该球体有 2500 个节点，总共 4949 条边。使用提供的脚本可视化结果会产生：