SLAM十四讲之第九讲：卡尔曼滤波与BA推导

第九讲：后端1

1 概述

1.1 贝叶斯滤波的推导

1.3.1 结论

$bel(x_t)$是后验概率，$\overline{bel}(x_t)$是预测概率（作为先验），z是观测，u是里程计输入，通过当前的事件分布来估计参数x（位姿、路标点）

$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)\overline{bel}(x_t)=\eta p(z_t|x_t)\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1.t-1},u_{1.t-1})d{x}_{t-1}$$

功能：已知状态量t − 1时刻的概率分布，在给定t时刻的观测数据$(z_t,u_t)$的情况下估计出状态量在𝑢时刻的概率分布

贝叶斯滤波流程

1.3.2 具体推导过程

第一步：贝叶斯公式

$$p(x_t|z_t)=\frac{p(z_t|x)p(x_t)}{p(z_t)}=\eta p(z_t|x_t)p(x_t)$$

第二步： 引入条件$z_{1:t-1},u_{1:t}$，条件贝叶斯

$$\begin{array}{rl}p(x_t|z_{1:t},u_{1:t})& =\frac{p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})}{p(z_t|z_{1:t-1},u_{1:t})}\\ & =\eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})\\ & =\eta p(z_t|x_t)p(x_t|z_{1:t-1},u_{1:t})\end{array}$$

其中（当前的观测z与当前的位姿x构成关系，与引入条件(之前的观测和u)无关，独立；而当前的位姿x与之前的观测等等都有关系）
$$\begin{array}{rl}& p(z_t|x_t,z_{1:t-1},u_{1:t})=p(z_t|x_t)\end{array}$$

第三步：全概率公式$\mathrm{p}(x_t)=\int p(x_t|x_{t-1})p(x_{t-1})dy$,加上条件即下式

$$\begin{array}{rl}p(x_t|z_{1:t-1},u_{1:t})& =\int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}\\ & =\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1},u_{1:t-1})d{x}_{t-1}\end{array}$$

注意$p(x_t|x_{t-1})$这两个时刻的位姿关系和前面的观测$z_{1:t-1}$无关，与$u_{1:t-1}$也无关，与$u_t$有关,因为这里描述的是两个时刻间的位姿关系；$p(x_{t-1})$与之前的观测输入1:t-1都有关，因为当前时刻的位姿是不断累积造成的。

第四步：令$bel(x_t)$为后验概率分布，$\overline{bel}(x_t)$为预测概率分布（先验）

$$\begin{array}{rl}bel(x_t)& =p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)\overline{bel}(x_t)\\ \overline{bel}(x_t)& =p(x_t|z_{1:t-1},u_{1:t})=\overline{bel}(x_t)=\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1},u_{1:t-1})d{x}_{t-1}\end{array}$$

第五步：最终公式

$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)\overline{bel}(x_t)=\eta p(z_t|x_t)\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1.t-1},u_{1.t-1})d{x}_{t-1}$$

总结：首先利用t-1时刻的位姿概率分布，预测t时刻的位姿概率分布，作为先验，然后结合激光/视觉观测模型，得到当前时刻即t时刻的位姿等参数概率分布。

1.2 状态估计的概率解释

​ SLAM过程可以由运动方程和观测方程来描述。一般来讲，观测>>运动方程。每个方程中变量都会受到噪声影响，所以这里位姿x和路标y不能看作一个数，而是服从某种概率分布。

$$\{\begin{array}{l}{x}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}k=1,\dots ,N,j=1,\dots ,M.$$

​ 在十四讲中，令$x_k$表示当前时刻的相机位姿和路标点变量，把k时刻的观测记为$z_k$，就可以得到下面新的方程：

x k = ⁡ d e f { x k , y 1 , … , y m } . x_k\overset{\mathrm{def}}{\operatorname*{=}}\{x_k,y_1,\ldots,y_m\}. xk​=def{xk​,y1​,…,ym​}.

$$\{\begin{array}{ll}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k& \\ {\mathbf{z}}_k=h\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k& \end{array}k=1,\dots ,N.$$

​ 考虑第k时刻的情况，希望利用前面0-k时刻的数据来估计当前的状态分布（渐进式）
$$P({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}).$$

​ 利用1.1推导的贝叶斯滤波公式，就可以得到下面公式：
$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k}\right)\propto P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right).$$

​ 其中（这里是书上的公式，实际上可以按1.1推导简化）：
$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=\int P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)P\left({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}.$$

​ 如果考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心k时刻和k-1时刻的情况。

​ 对这一步的后续处理，存两种选择：

​ 一种方法是假设马尔可夫性，简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以**扩展卡尔曼滤波(EKF)**为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。

​ 另一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。

1.3 线性系统和KF

1.3.1 线性系统

​ 根据马尔可夫性，当前时刻的状态只和上一个时刻有关，对书中的先验公式（预测）右边部分进行简化：

​ 还是参考1.1推导，首先k-1时刻状态到k时刻状态与之前的输入u和观测皆无关(条件概率，k-1时刻状态为条件)

$$P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_k|{\mathbf{x}}_{k-1},{\mathbf{u}}_k\right).$$

​ 其次，k-1时刻的状态受到前面所有时刻的位姿、输入等影响，去掉$u_k$（先验概率）
$$P\left({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=P\left({\mathbf{x}}_{k-1}|{\mathbf{x}}_0,{\mathbf{u}}_{1:k-1},{\mathbf{z}}_{1:k-1}\right).$$

​ 于是，这一系列方程说明，我们实际在做的是“如何把k-1时刻的状态分布推导至k时刻”这样一件事。

​ 也就是说，在程序运行期间，我们只要维护一个状态量，对它不断地进行迭代和更新即可。如果假设状态量服从高斯分布，那么我们只需考虑维护状态量的均值和协方差。

​ 下面推导线性高斯系统，实际上就是线性系统的一种（线性系统理论中很详细讲解），线性高斯系统是指，运动方程和观测方程可以由线性方程来描述：
$$\{\begin{array}{ll}{x}_k={\mathbf{A}}_k{\mathbf{x}}_{k-1}+{\mathbf{u}}_k+{\mathbf{w}}_k& \\ & \\ {\mathbf{z}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k& \end{array}k=1,\dots ,N.$$

​ 噪声变量$w_k$、$v_k$都服从高斯分布：
$${\mathbf{w}}_k\sim N(0,\mathbf{R}).{\mathbf{v}}_k\sim N(0,\mathbf{Q}).$$

​ 书中简略了部分变量的下标，后续以上帽子${\hat{x}}_k$表示后验分布，下帽子${\check{x}}_k$表示先验分布

1.3.2 卡尔曼滤波KF

卡尔曼滤波器第1步：通过运动方程确定$x_k$的先验分布，即预测

$$P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{1:k-1}\right)=N\left({\mathbf{A}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\mathbf{A}}_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\top }+\mathbf{R}\right).$$

​ 这里满足线性高斯变换，N(均值，协方差)，记：
$${\check{\mathbf{x}}}_k=A_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{u}}_k,{\check{\mathbf{P}}}_k=A_k{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_k^{\mathrm{T}}+\mathbf{R}.$$

卡尔曼滤波器第2步：根据观测方程，计算某个状态下应该产生怎样的观测数据

$$P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)=N\left({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q}\right).$$

卡尔曼滤波器第3步：计算后验概率分布${\mathbf{x}}_k\sim N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)$ = 先验*观测（似然）

$$N({\hat{\mathbf{x}}}_k,{\hat{\mathbf{P}}}_k)=\eta N({\mathbf{C}}_k{\mathbf{x}}_k,\mathbf{Q})\cdot N({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k).$$

下面是关于后验概率分布公式的具体推导

$${\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)={\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}^{-1}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)+{\left({\mathbf{x}}_k-{\hat{\mathbf{x}}}_k\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right).$$

​ 为了求左侧的${\hat{x}}_k$和${\hat{P}}_k$,我们把两边展开，并比较$x_k$的二次和一次系数。对于二次系数，有
$${\hat{P}}_k^{-1}=C_k^{\mathrm{T}}{Q}^{-1}{C}_k+{\check{P}}_k^{-1}.$$

​ 该式给出了协方差的计算过程。为了便于后面列写式子，定义一个中间变量K
$$K={\hat{P}}_k{C}_k^{\mathrm{T}}{Q}^{-1}.$$

$$I={\hat{P}}_k{C}_k^{\mathrm{T}}{Q}^{-1}{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}=K{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}.$$

$$\widehat{P_k}=(I-K{C}_k)\check{P_k}.$$

​ 比较一次项的系数
$$-2{\hat{\mathbf{x}}}_k^{\mathrm{T}}{\hat{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k=-2{\mathbf{z}}_k^{\mathrm{T}}{Q}^{-1}{C}_k{\mathbf{x}}_k-2{\check{\mathbf{x}}}_k^{\mathrm{T}}{\check{\mathbf{P}}}_k^{-1}{\mathbf{x}}_k.$$

$${\hat{\mathbf{P}}}_k^{-1}{\hat{\mathbf{x}}}_k=C_k^{\mathrm{T}}{Q}^{-1}{\mathbf{z}}_k+{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k.$$

​ 两侧乘以${\hat{P}}_k$并代入

$$\begin{array}{rl}{\hat{x}}_k& ={\hat{\mathbf{P}}}_k{\mathbf{C}}_k^{\mathrm{T}}{\mathbf{Q}}^{-1}{\mathbf{z}}_k+{\hat{\mathbf{P}}}_k{\check{\mathbf{P}}}_k^{-1}{\check{\mathbf{x}}}_k\\ & =\mathbf{K}{\mathbf{z}}_k+\left(\mathbf{I}-\mathbf{K}{\mathbf{C}}_k\right){\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{C}}_k{\check{\mathbf{x}}}_k\right).\end{array}$$

1.3.3 小结

预测：利用k-1时刻后验得到k时刻先验

更新：k时刻后验 = 观测*k时刻先验（后验 = 先验 + 卡尔曼增益K×不确定量）

1.4 非线性系统和EKF

​ SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型，需要使用相机内参模型及李代数表示的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯分布近似成高斯分布。

​ 我们希望把卡尔曼滤波器的结果拓展到非线性系统中，称为扩展卡尔曼滤波器。通常的做法是，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。令k-1时刻的均值与协方差矩阵为${\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}$。在k时刻，我们把运动方程和观测方程在${\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}$处进行线性化（相当于一阶泰勒展开）：

运动方程一阶泰勒近似

$$x_k\approx f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right)+{\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1})+{\mathbf{w}}_k.$$

$$\mathbf{F}={\frac{\partial f}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1}}.$$

观测方程一阶泰勒近似

$${\mathbf{z}}_k\approx h\left({\check{\mathbf{x}}}_k\right)+{\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k.$$

$$H={\frac{\partial h}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k}.$$

根据运动和观测方程的一阶泰勒近似，推导EKF公式中k时刻先验分布和似然

$$\begin{gathered} P\left({\mathbf{x}}_k|{\mathbf{x}}_0,{\mathbf{u}}_{1:k},{\mathbf{z}}_{0:k-1}\right)=N(f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),\mathbf{F}{\hat{P}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k) \\ P\left({\mathbf{z}}_k|{\mathbf{x}}_k\right)=N(h\left({\check{\mathbf{x}}}_k\right)+H\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{Q}}_k). \end{gathered}$$

​ 其中，k时刻的先验（均值和协方差）为：

$${\check{\mathbf{x}}}_k=f\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{u}}_k\right),{\check{\mathbf{P}}}_k=F{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}^{\mathrm{T}}+{\mathbf{R}}_k.$$

根据运动和观测方程的一阶泰勒近似，推导EKF公式中k时刻后验分布

​ 定义卡尔曼增益$K_k$:
$$K_k={\check{P}}_k{H}^{\mathrm{T}}(H{\check{P}}_k{H}^{\mathrm{T}}+Q_k{)}^{-1}.$$

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{z}}_k-h\left({\check{\mathbf{x}}}_k\right)\right),{\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k\mathbf{H}\right){\check{\mathbf{P}}}_k.$$

2 BA和图优化

​ 所谓的Bundle Adjustment(BA),是指从视觉图像中提炼出最优的3D模型和相机参数（内参数和外参数）。

2.1 投影模型和BA代价函数

​ 投影模型也就是所谓的观测方程$e=\mathbf{z}-h(\mathbf{T},\mathbf{p})$，将世界坐标系下的点投影到像素坐标系下。

​ 观测方程的计算结果必然会与真实观测产生误差，这是数学模型不可避免的。
$$e=\mathbf{z}-h(\mathbf{T},\mathbf{p})$$
​ 一般来讲，优化指的是对整体误差的调整，而不能只是对单一路标点进行优化。如下式，设$z_{ij}$是位姿$T_i$处观察路标$p_j$产生的数据，那么累计的代价函数为：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{e}}_{ij}{\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n\Vert {\mathbf{z}}_{ij}-h({\mathbf{T}}_i,{\mathbf{p}}_j){\Vert }^2$$

2.2 BA求解

​ 定义优化自变量参数如下：

$$\mathbf{x}={\left[{\mathbf{T}}_1,\dots ,{\mathbf{T}}_m,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_n\right]}^{\mathrm{T}}$$

​ 非线性优化中，我们求解的增量是对于整体自变量的增量$\Delta x$。下面式子中，$F_{ij}$表示整个代价函数在当前状态下对相机位姿的偏导，$E_{ij}$表示整个代价函数在当前状态下对路标点的偏导。
$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{p}}_j\Vert }^2$$

​ 把位姿和路标点参数分开表示，即可得到新的代价函数表达式
$$\begin{gathered} {\mathbf{x}}_{\mathrm{c}}={\left[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2,\dots ,{\mathbf{\xi }}_m\right]}^{\mathrm{T}}\in {\mathbb{R}}^{6m} \\ {\mathbf{x}}_p={\left[{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_n\right]}^{\mathrm{T}}\in {\mathbb{R}}^{3n} \end{gathered}$$

$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2=\frac{1}{2}{\Vert \mathbf{e}+\mathbf{F}\Delta {\mathbf{x}}_c+\mathbf{E}\Delta {\mathbf{x}}_p\Vert }^2$$

​ 高斯牛顿法的解式：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$$

​ 因为变量是位姿和空间点两种，我们可以对雅可比矩阵进行分块处理
$$\mathbf{J}=\left[\begin{array}{ll}\mathbf{F}& \mathbf{E}\end{array}\right]$$

$$\mathbf{H}={\mathbf{J}}^{\mathrm{T}}\mathbf{J}=\left[\begin{array}{ll}{\mathbf{F}}^{\mathrm{T}}\mathbf{F}& {\mathbf{F}}^{\mathrm{T}}\mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}\mathbf{F}& {\mathbf{E}}^{\mathrm{T}}\mathbf{E}\end{array}\right]$$

2.3 稀疏性和边缘化

2.3.1 稀疏性

​ 在SLAM中，相机位姿和特征点数量必然很大，也就造成矩阵H维度很大。直接去方程会耗费计算资源，但是H的稀疏结构简化了这一过程。

​ 以下面的例子引出稀疏矩阵H。假设一个场景内有2个相机位姿(C1,C2)和6个路标点(P1,P2,P3,P4,P,P6)。分别对应相机位姿$T_i,i=1,2$和路标点$p_j,j=1,...,6$。如果相机i能够观测到j点，就在它们对应的节点连上一条边。

​ 据图推出当前的代价函数
$$\frac{1}{2}\left({\Vert {\mathbf{e}}_{11}\Vert }^2+{\Vert {\mathbf{e}}_{12}\Vert }^2+{\Vert {\mathbf{e}}_{13}\Vert }^2+{\Vert {\mathbf{e}}_{14}\Vert }^2+{\Vert {\mathbf{e}}_{23}\Vert }^2+{\Vert e_{24}\Vert }^2+{\Vert {\mathbf{e}}_{25}\Vert }^2+{\Vert {\mathbf{e}}_{26}\Vert }^2\right)$$
​ 以$e_{11}$为例，其表示在$C_1$看到了$P_1$,与其它时刻的相机位姿和路标点无关！所以雅可比只在位姿1和路标点1处不为0，其余全为0.
$${\mathbf{J}}_{11}=\frac{\partial {\mathbf{e}}_{11}}{\partial \mathbf{x}}=\left(\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{\xi }}_1},0_{2\times 6},\frac{\partial {\mathbf{e}}_{11}}{\partial {\mathbf{p}}_1},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3}\right)$$
​ 同理可以得到其它的雅可比${\mathbf{J}}_{ij}(\mathbf{x})=\left(0_{2\times 6},\dots 0_{2\times 6},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{T}}_i},0_{2\times 6},\dots 0_{2\times 3},\dots 0_{2\times 3},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j},0_{2\times 3},\dots 0_{2\times 3}\right)$，利用图像矩阵来形象表示：

​ 矩阵$H_{8\ast 8}$中，有值的地方表示存在了误差边$e_{ij}$。矩阵中非对角部分的非0矩阵块可以理解为其对应的两个变量之间存在约束关系！

​ 扩展到任意维度，对其分块处理。其中$H_{11}、$$H_{22}$都是对角阵，只是在对角线处有非0块。对于$H_{12}$和$H_{21}$,它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定。
$$\mathbf{H}=\sum _{i,j}{\mathbf{J}}_{ij}^{\mathrm{T}}{\mathbf{J}}_{ij}=\left[\begin{array}{ll}{\mathbf{H}}_{11}& {\mathbf{H}}_{12}\\ {\mathbf{H}}_{21}& {\mathbf{H}}_{22}\end{array}\right]$$

2.3.2 边缘化

​ 对于具有这种稀疏结构的H,线性方程$H△x=g$的求解,书中介绍一种最常用的手段：Schur消元，也称为Marginalization(边缘化)。

​ 为了更好的描述这个矩阵，将其按规律拆分为四个矩阵块，对应线性方程$H\Delta x=g$ :
$$\left[\begin{array}{ll}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}& \mathbf{C}\end{array}\right]\left[\begin{array}{l}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{l}\mathbf{v}\\ \mathbf{w}\end{array}\right]$$

​ 注意到B和C是对角块矩阵，每个对角块的维度一致。因为对角矩阵更容易求逆，所以对方程进行高斯消元（通过一系列的行变换，将增广矩阵化为上三角形式，这里消除左上角的E）：
$$\begin{array}{c}\left[\begin{array}{cc}\mathbf{I}& -\mathbf{E}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^{\mathrm{T}}& C\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& -\mathbf{E}{\mathbf{C}}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{l}\mathbf{v}\\ \mathbf{w}\end{array}\right].\\ \left[\begin{array}{cc}\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}}& 0\\ {\mathbf{E}}^{\mathrm{T}}& C\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_{\mathrm{c}}\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{c}\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}\\ \mathbf{w}\end{array}\right].\end{array}$$

① 取出第一行，求解位姿增量$\Delta x_c$

$$\left[\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}}\right]\Delta {\mathbf{x}}_{\mathrm{c}}=\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}$$

② 利用求出的$\Delta x_c$，计算路标增量$\Delta x_p$
$$\Delta {\mathbf{x}}_p={\mathbf{C}}^{-1}(\mathbf{w}-{\mathbf{E}}^{\mathrm{T}}\Delta {\mathbf{x}}_{\mathrm{c}})$$
③ 这个方程的系数矩阵$\left[\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}}\right]$，记为S。S矩阵的非对角线上的非0矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点。反之，如果相应矩阵块为0，表示两个相机没有共同观测。
$$\left[\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^{\mathrm{T}}\right]\Delta {\mathbf{x}}_{\mathrm{c}}=\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}$$
④ 从概率角度来看，我们称这一步为边缘化，是因为我们实际上把求$(\Delta x_p,\Delta x_p)$的问题，转化成了先固定$\Delta x_p$,求出$\Delta x_c$,再求$\Delta x_p$的过程。这一步相当于做了条件概率展开：
$$P({\mathbf{x}}_{\mathbf{c}},{\mathbf{x}}_p)=P({\mathbf{x}}_{\mathbf{c}}|{\mathbf{x}}_p)P({\mathbf{x}}_p),$$

2.4 鲁棒核函数

​ 为了避免出现很大误差边带来的干扰，引入了鲁棒核函数。

• Huber核

$$H(e)=\{\begin{array}{ll}\frac{1}{2}{e}^2& \text{ 当 }|e|⩽\delta ,\\ \delta \left(|e|-\frac{1}{2}\delta \right)& \text{ 其他 }\end{array}$$

• Cauchy核函数

$$K(x)=\frac{1}{1+x^2}$$

• Tukey’s Biweight核函数

$$K(x)=(1-x^2{)}^2\cdot I(|x|\le 1)$$

3 实践

3.1 BAL数据集

3.1.1 Available Datasets

1. Ladybug
2. Trafalgar Square
3. Dubrovnik
4. Venice
5. Final

3.1.2 Camera Model

1. BAL的相机内参模型由焦距f和畸变参数k1，k2给出。f类似于我们提到的fx和fy。由于照片像素基本上是正方形，所以在很多实际场合中fx非常接近fy，用同一个值也未尝不可。此外，这个模型中没有cx,cy,因为存储的数据已经去掉了这两个值。
2. 因为BAL数据在投影时假设投影平面在相机光心之后，所以按照我们之前用的模型计算，需要在投影之后乘以系数-1。不过，大部分数据集仍使用光心前面的投影平面。

We use a pinhole camera model(针孔相机模型); the parameters we estimate for each camera area rotation R, a translation t, a focal length f(焦距) and two radial distortion(径向畸变) parameters k1 and k2. The formula for projecting a 3D point X into a camera R,t,f,k1,k2 is(观测方程/投影方程):
P  =  R * X + t       (conversion from world to camera coordinates)
p  = -P / P.z         (perspective division,X和Y分别处于X，转换归一化坐标系)
p' =  f * r(p) * p    (conversion to pixel coordinates)
where P.z is the third (z) coordinate of P. In the last equation, r(p) is a function that computes a scaling factor to undo the radial distortion:
r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4. 注意p是极坐标，也就是p^2=x^2+y^2

This gives a projection in pixels, where the origin of the image is the center of the image(图像的原点是图像的中心，所以没有cx,cy), the positive x-axis points right, and the positive y-axis points up (in addition, in the camera coordinate system, the positive z-axis points backwards, so the camera is looking down the negative z-axis, as in OpenGL).

3.1.3 Data Format

​ 数据格式介绍网站，书中实验数据来源于下图第一个数据，可知为16张图片，共22160个路标点，83718观测结果。观测>>路标点是因为出现了许多共视点。后续给出了其余数据网址，以及数据的结构分析。

Each problem is provided as a bzip2 compressed text file in the following format(每个压缩文本文件都是下面的格式).

# 图像帧数 路标点数 观测点数
<num_cameras> <num_points> <num_observations>
# 图像帧索引 路标点索引  观测u 观测v 
<camera_index_1> <point_index_1> <x_1> <y_1>	# 第1行
...
<camera_index_num_observations> <point_index_num_observations> <x_num_observations> <y_num_observations>	# 第num行

<camera_1>
...			# 相机位姿信息，根据后面注释，Each camera 有9参数，即9行表示1个相机 
<camera_num_cameras>

<point_1>	# 表示点的具体信息，是一个点的坐标 3行表示1个点
...
<point_num_points>

Where, there camera and point indices start from 0. Each camera is a set of 9 parameters - R,t,f,k1 and k2. The rotation R is specified as a Rodrigues' vector角轴.



16 22106 83718 这里实验数据计算 83718 + 16*9 + 22106*3 = 150180行数据

3.2 关于环境配置补充

​ 是关于Sophus库的，在Ubuntu18.04上按仓库的第三方软件源下载即可。在Ubuntu20上，下载默认的Sophus，他需要fmt这个库，为了防止版本不兼容，利用apt search fmt找到当前Ubuntu下的库，

sudo apt install libfmt-dev

​ 既然有这个需要，那么就需要在CMakeLists.txt上面链接fmt库文件（Ubuntu18是不需要的）。

cmake_minimum_required(VERSION 2.8)

project(bundle_adjustment)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3 -std=c++11")	# 支持c++11标准

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)	# 这里把寻找cmake moudle添加到cmake寻找路径

Find_Package(G2O REQUIRED)		
Find_Package(Eigen3 REQUIRED)
Find_Package(Ceres REQUIRED)
Find_Package(Sophus REQUIRED)
Find_Package(CSparse REQUIRED)

SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)

include_directories(${PROJECT_SOURCE_DIR} 		# 包含项目源文件路径，eg random.h
					${EIGEN3_INCLUDE_DIR} 
					${CSPARSE_INCLUDE_DIR})

add_library(bal_common common.cpp)				# 添加库文件
add_executable(bundle_adjustment_g2o bundle_adjustment_g2o.cpp)
add_executable(bundle_adjustment_ceres bundle_adjustment_ceres.cpp)

target_link_libraries(bundle_adjustment_ceres ${CERES_LIBRARIES} bal_common fmt::fmt)	# 添加fmt库
target_link_libraries(bundle_adjustment_g2o ${G2O_LIBS} bal_common fmt::fmt)

​ 除此以外，还需要安装下面软件

sudo apt-get install meshlab  

# 有可能在编译的时候找不到eigen库，先不要用作者写的findeigen3，一般都能找到。或者直接把那个文件删了即可
find_package(Eigen3 3.1.0 REQUIRED NO_MODULE)

3.3 基本函数

1 common.h

#pragma once

/// 从文件读入BAL dataset
class BALProblem {
public:
    /// load bal data from text file  
    // explicit: 这是一个关键字，用于标记构造函数，表示它不能用于隐式转换
    explicit BALProblem(const std::string &filename, bool use_quaternions = false);

    ~BALProblem() {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }

    /// save results to text file
    void WriteToFile(const std::string &filename) const;

    /// save results to ply pointcloud
    void WriteToPLYFile(const std::string &filename) const;

    void Normalize();

    void Perturb(const double rotation_sigma,
                 const double translation_sigma,
                 const double point_sigma);

    // 相机参数数目：是否使用四元数  是10 否9--角轴
    int camera_block_size() const { return use_quaternions_ ? 10 : 9; }

    // 特征点参数数目：3
    int point_block_size() const { return 3; }

    // 图像帧数 16
    int num_cameras() const { return num_cameras_; }    

    // 路标点数 22106 
    int num_points() const { return num_points_; }      

    // 观测点数 83718
    int num_observations() const { return num_observations_; }  

    // num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 相机和路标点的参数总（行）数
    int num_parameters() const { return num_parameters_; }  


    // 每个observation对应的point index 地址
    const int *point_index() const { return point_index_; } 

    // 每个observation对应的camera index 地址
    const int *camera_index() const { return camera_index_; }   

    // u v
    const double *observations() const { return observations_; }   

    //  后面总的参数地址 9 * num_cameras_ + 3 * num_points_
    const double *parameters() const { return parameters_; }  

    //  后面总的参数地址 9 * num_cameras_ + 3 * num_points_
    const double *cameras() const { return parameters_; }  

    // 后面点的首地址
    const double *points() const { return parameters_ + camera_block_size() * num_cameras_; }   



    /// camera参数的起始地址
    double *mutable_cameras() { return parameters_; }
    
    // 路标点首地址
    double *mutable_points() { return parameters_ + camera_block_size() * num_cameras_; }

    // camera参数的起始地址+9*camera_index_[i]
    double *mutable_camera_for_observation(int i) {
        return mutable_cameras() + camera_index_[i] * camera_block_size();
    }

    // 路标点首地址+3*point_index_[i]
    double *mutable_point_for_observation(int i) {
        return mutable_points() + point_index_[i] * point_block_size();
    }

    const double *camera_for_observation(int i) const {
        return cameras() + camera_index_[i] * camera_block_size();
    }

    const double *point_for_observation(int i) const {
        return points() + point_index_[i] * point_block_size();
    }

private:
    void CameraToAngelAxisAndCenter(const double *camera,
                                    double *angle_axis,
                                    double *center) const;

    void AngleAxisAndCenterToCamera(const double *angle_axis,
                                    const double *center,
                                    double *camera) const;

    int num_cameras_;       // 图像帧数 16
    int num_points_;        // 路标点数 22106 
    int num_observations_;  // 观测点数 83718
    int num_parameters_;    // num_parameters_ = 9 * num_cameras_ + 3 * num_points_; 相机和路标点的参数总（行）数
    bool use_quaternions_;  // 是否使用四元数

    int *point_index_;      // 每个observation对应的point index 地址
    int *camera_index_;     // 每个observation对应的camera index 地址  
    double *observations_;  // u v
    double *parameters_;    // 后面总的参数  9 * num_cameras_ + 3 * num_points_
};

2 common.cpp

​ 读取信息，写入信息，写入点云可视化文件，相机信息<=>旋转信息&相机中心空间坐标 ，对空间点、旋转、平移加入噪声。

#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "common.h"
#include "rotation.h"
#include "random.h"

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;


/**
 * @brief 模板函数，可以用于从文件中读取数据
 * @param[in] fptr   一个指向文件的指针，用于从文件中读取数据
 * @param[in] format 一个格式化字符串，它告诉函数如何读取文件中的数据
 * @param[in] value  存储从文件中读取的值
 */
template<typename T>
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
    int num_scanned = fscanf(fptr, format, value);  // FILE 指针在每次成功读取后会自动向前移动，以指向下一个可读取的位置,依次读取
    if (num_scanned != 1)
        std::cerr << "Invalid UW data file. ";
}


/**
 * @brief 对该点的每个坐标分量（x、y、z）进行随机扰动，扰动的大小由 sigma 参数控制
 * @param[in] sigma   决定扰动程度
 * @param[in] point   坐标（x、y、z）
 */
void PerturbPoint3(const double sigma, double *point) {
    for (int i = 0; i < 3; ++i)
        point[i] += RandNormal() * sigma;
}


/**
 * @brief 对数据进行部分排序，使中位数的位置处于正确的位置,返回中位数
 * @param[in] data  动态数组容器
 */
double Median(std::vector<double> *data) {
    int n = data->size();
    std::vector<double>::iterator mid_point = data->begin() + n / 2;
    std::nth_element(data->begin(), mid_point, data->end());    // nth_element 算法对数据进行部分排序，使中位数的位置处于正确的位置
    return *mid_point;
}


/**
 * @brief 类构造函数,读取文件
 * @param[in] filename   文件名
 * @param[in] use_quaternions  
 */
BALProblem::BALProblem(const std::string &filename, bool use_quaternions) {
    FILE *fptr = fopen(filename.c_str(), "r");

    if (fptr == NULL) {
        std::cerr << "Error: unable to open file " << filename;
        return;
    };

    // This wil die horribly on invalid files. Them's the breaks.
    FscanfOrDie(fptr, "%d", &num_cameras_); //  依次读取图像帧数 路标点数 观测点数
    FscanfOrDie(fptr, "%d", &num_points_);
    FscanfOrDie(fptr, "%d", &num_observations_);

    std::cout << "Header: " << num_cameras_
              << " " << num_points_
              << " " << num_observations_;

    point_index_ = new int[num_observations_];  // 一共有这么多观测关系，所以创建相同数量的位姿和路标点数组
    camera_index_ = new int[num_observations_];
    observations_ = new double[2 * num_observations_]; // u v

    num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
    parameters_ = new double[num_parameters_];  // 总参数数组

    for (int i = 0; i < num_observations_; ++i) {
        FscanfOrDie(fptr, "%d", camera_index_ + i);
        FscanfOrDie(fptr, "%d", point_index_ + i);
        for (int j = 0; j < 2; ++j) {
            FscanfOrDie(fptr, "%lf", observations_ + 2 * i + j);
        }
    }

    for (int i = 0; i < num_parameters_; ++i) {
        FscanfOrDie(fptr, "%lf", parameters_ + i);
    }

    fclose(fptr);

    use_quaternions_ = use_quaternions;
    if (use_quaternions) {
        // Switch the angle-axis rotations to quaternions. 
        // 如果提供的数据是四元数，因为四元数的旋转部分为4个数，所以9变成10.前面的观测关系是没有变的，所以无需更改
        num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
        double *quaternion_parameters = new double[num_parameters_];
        double *original_cursor = parameters_;
        double *quaternion_cursor = quaternion_parameters;
        for (int i = 0; i < num_cameras_; ++i) {
            // 角轴转换四元数
            AngleAxisToQuaternion(original_cursor, quaternion_cursor);
            // 更新地址索引
            quaternion_cursor += 4;
            original_cursor += 3;
            // 剩余的6个参数一致
            for (int j = 4; j < 10; ++j) {
                *quaternion_cursor++ = *original_cursor++;
            }
        }
        // Copy the rest of the points.
        for (int i = 0; i < 3 * num_points_; ++i) {
            *quaternion_cursor++ = *original_cursor++;
        }
        // Swap in the quaternion parameters.
        delete[]parameters_;
        parameters_ = quaternion_parameters;
    }
}

// const 这个成员函数是一个常量成员函数，它承诺在函数内部不会修改类的任何成员变量
/**
 * @brief 将数据写入文件
 * @param[in] filename   文件名
 */
void BALProblem::WriteToFile(const std::string &filename) const {
    FILE *fptr = fopen(filename.c_str(), "w");

    if (fptr == NULL) {
        std::cerr << "Error: unable to open file " << filename;
        return;
    }

    fprintf(fptr, "%d %d %d %d\n", num_cameras_, num_cameras_, num_points_, num_observations_);

    for (int i = 0; i < num_observations_; ++i) {
        fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
        for (int j = 0; j < 2; ++j) {
            fprintf(fptr, " %g", observations_[2 * i + j]);
        }
        fprintf(fptr, "\n");
    }

    for (int i = 0; i < num_cameras(); ++i) {
        double angleaxis[9];
        if (use_quaternions_) {
            //OutPut in angle-axis format.
            QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
            memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
        } else {
            memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
        }
        for (int j = 0; j < 9; ++j) {
            fprintf(fptr, "%.16g\n", angleaxis[j]);
        }
    }

    const double *points = parameters_ + camera_block_size() * num_cameras_;
    for (int i = 0; i < num_points(); ++i) {
        const double *point = points + i * point_block_size();
        for (int j = 0; j < point_block_size(); ++j) {
            fprintf(fptr, "%.16g\n", point[j]);
        }
    }

    fclose(fptr);
}

// Write the problem to a PLY file for inspection in Meshlab or CloudCompare
/**
 * @brief 将问题的数据写入一个 PLY（Polygon File Format）文件
 * @param[in] filename   文件名
 */
void BALProblem::WriteToPLYFile(const std::string &filename) const {
    std::ofstream of(filename.c_str());

    of << "ply"
       << '\n' << "format ascii 1.0"
       << '\n' << "element vertex " << num_cameras_ + num_points_
       << '\n' << "property float x"
       << '\n' << "property float y"
       << '\n' << "property float z"
       << '\n' << "property uchar red"
       << '\n' << "property uchar green"
       << '\n' << "property uchar blue"
       << '\n' << "end_header" << std::endl;

    // Export extrinsic data (i.e. camera centers) as green points. 绿色表示相机中心
    double angle_axis[3];
    double center[3];
    for (int i = 0; i < num_cameras(); ++i) {
        const double *camera = cameras() + camera_block_size() * i;

        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        of << center[0] << ' ' << center[1] << ' ' << center[2]
           << " 0 255 0" << '\n';
    }

    // Export the structure (i.e. 3D Points) as white points. 白色表示路标点
    const double *points = parameters_ + camera_block_size() * num_cameras_;    // begin()+16*9
    for (int i = 0; i < num_points(); ++i) {
        const double *point = points + i * point_block_size();
        for (int j = 0; j < point_block_size(); ++j) {
            of << point[j] << ' ';
        }
        of << " 255 255 255\n";
    }
    of.close();
}


/**
 * @brief compute  Ow = -Rwc*tcw;  // twc
 * @param[in] camera   相机参数地址
 * @param[in] angle_axis   角轴数组指针(数组名)
 * @param[in] center   相机中心
 */
void BALProblem::CameraToAngelAxisAndCenter(const double *camera,
                                            double *angle_axis,
                                            double *center) const {
    VectorRef angle_axis_ref(angle_axis, 3);
    if (use_quaternions_) {
        QuaternionToAngleAxis(camera, angle_axis);
    } else {
        angle_axis_ref = ConstVectorRef(camera, 3); // 起始地址  前三个
    }

    // c = -R't      Ow = -Rwc*tcw;  // twc
    Eigen::VectorXd inverse_rotation = -angle_axis_ref;
    AngleAxisRotatePoint(inverse_rotation.data(),
                         camera + camera_block_size() - 6,  // tcw
                         center);
    VectorRef(center, 3) *= -1.0;
}


/**
 * @brief   compute  tcw
 * @param[in] angle_axis   角轴
 * @param[in] center   相机中心
 * @param[in] camera   角轴数组指针(数组名)
 */
void BALProblem::AngleAxisAndCenterToCamera(const double *angle_axis,
                                            const double *center,
                                            double *camera) const {
    ConstVectorRef angle_axis_ref(angle_axis, 3);
    if (use_quaternions_) {
        AngleAxisToQuaternion(angle_axis, camera);
    } else {
        VectorRef(camera, 3) = angle_axis_ref;
    }

    // t = -R * c
    AngleAxisRotatePoint(angle_axis, center, camera + camera_block_size() - 6);
    VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
}


/**
 * @brief  Normalize
 */
void BALProblem::Normalize() {
    // Compute the marginal median of the geometry
    std::vector<double> tmp(num_points_);
    Eigen::Vector3d median;
    double *points = mutable_points();
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < num_points_; ++j) {
            tmp[j] = points[3 * j + i];
        }
        median(i) = Median(&tmp);   // 记录路标点的中值
    }

    for (int i = 0; i < num_points_; ++i) {
        VectorRef point(points + 3 * i, 3);
        tmp[i] = (point - median).lpNorm<1>();  // 减去均值后求1-范数
    }

    const double median_absolute_deviation = Median(&tmp);

    // Scale so that the median absolute deviation of the resulting
    // reconstruction is 100

    const double scale = 100.0 / median_absolute_deviation;

    // X = scale * (X - median)
    for (int i = 0; i < num_points_; ++i) {
        VectorRef point(points + 3 * i, 3);
        point = scale * (point - median);
    }

    double *cameras = mutable_cameras();
    double angle_axis[3];
    double center[3];
    for (int i = 0; i < num_cameras_; ++i) {
        double *camera = cameras + camera_block_size() * i;
        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        // center = scale * (center - median)
        VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
        AngleAxisAndCenterToCamera(angle_axis, center, camera);
    }
}


/**
 * @brief   数据进行扰动，扰动的方式包括对点的坐标、相机的旋转和平移进行随机变动
 * @param[in] rotation_sigma   旋转扰动参数
 * @param[in] translation_sigma   平移扰动参数
 * @param[in] point_sigma   点的坐标扰动参数
 */
void BALProblem::Perturb(const double rotation_sigma,
                         const double translation_sigma,
                         const double point_sigma) {
    assert(point_sigma >= 0.0);
    assert(rotation_sigma >= 0.0);
    assert(translation_sigma >= 0.0);

    double *points = mutable_points();
    if (point_sigma > 0) {
        for (int i = 0; i < num_points_; ++i) {
            PerturbPoint3(point_sigma, points + 3 * i);
        }
    }

    for (int i = 0; i < num_cameras_; ++i) {
        double *camera = mutable_cameras() + camera_block_size() * i;

        double angle_axis[3];
        double center[3];
        // Perturb in the rotation of the camera in the angle-axis
        // representation
        CameraToAngelAxisAndCenter(camera, angle_axis, center);
        if (rotation_sigma > 0.0) {
            PerturbPoint3(rotation_sigma, angle_axis);
        }
        AngleAxisAndCenterToCamera(angle_axis, center, camera);

        if (translation_sigma > 0.0)
            PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
    }
}

3 rotation.h旋转变换

#ifndef ROTATION_H
#define ROTATION_H

#include <algorithm>
#include <cmath>
#include <limits>

//
// 1. 旋转变换--数学函数   a*b   axb

template<typename T>  
inline T DotProduct(const T x[3], const T y[3]) {
    return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);   // 向量点击dot production 
}

template<typename T>  // 向量叉积  cross production 
inline void CrossProduct(const T x[3], const T y[3], T result[3]) {
    result[0] = x[1] * y[2] - x[2] * y[1];
    result[1] = x[2] * y[0] - x[0] * y[2];
    result[2] = x[0] * y[1] - x[1] * y[0];
}


//


// 2. 手写旋转向量到四元数    旋转向量 θn   旋转角θ  旋转轴n-单位长度向量 
// 参数：旋转向量angle_axis = [a0,a1,a2]    四元数：quaternion
// 所以 旋转向量/θ = 旋转轴n  [a0,a1,a2]/theta=[n0,n1,n2] 单位长度向量n
// 这里θ=sqrt(a0*a0+a1*a1+a2*a2) 旋转向量模长即旋转角度θ

template<typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion) {
    const T &a0 = angle_axis[0];
    const T &a1 = angle_axis[1];
    const T &a2 = angle_axis[2];
    const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;  // θ^2
	
    // theta ！= 0  因为要把θ当分母
    // std::numeric_limits<double>::epsilon()运行编译程序的计算机所能识别的最小非零浮点数
    if (theta_squared > T(std::numeric_limits<double>::epsilon())) {
        const T theta = sqrt(theta_squared);
        const T half_theta = theta * T(0.5);
        const T k = sin(half_theta) / theta;
        
        quaternion[0] = cos(half_theta);	// 书上公式(3.44)
        quaternion[1] = a0 * k;   // a0/θ = n   n* sin(half_theta) = q
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    }
    // θ = 0
    else { // in case if theta_squared is zero
        const T k(0.5);   // 这里是c++11标准，赋值  也就是k = 0.5
        quaternion[0] = T(1.0);
        quaternion[1] = a0 * k;
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    }
}


// 3. 四元数到旋转向量
template<typename T>
inline void QuaternionToAngleAxis(const T *quaternion, T *angle_axis) {
    const T &q1 = quaternion[1];
    const T &q2 = quaternion[2];
    const T &q3 = quaternion[3];
    const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
	/*四元数[q0,q1,q2,q3]变角轴[a0,a1,a2]:
    [a0,a1,a2]=[q1,q2,q3]*theta/sin(theta/2) 
    [a0,a1,a2]/theta=[n0,n1,n2]->对上式取模->得到sin(theta/2),为sqrt(q1*q1+q2*q2+q3*q3)*/
        
    // For quaternions representing non-zero rotation, the conversion
    // is numercially stable
    if (sin_squared_theta > T(std::numeric_limits<double>::epsilon())) {
        const T sin_theta = sqrt(sin_squared_theta);
        const T &cos_theta = quaternion[0];

        // If cos_theta is negative, theta is greater than pi/2, which
        // means that angle for the angle_axis vector which is 2 * theta
        // would be greater than pi...
		
        // 真实θ
        const T two_theta = T(2.0) * ((cos_theta < 0.0)
                                      ? atan2(-sin_theta, -cos_theta)
                                      : atan2(sin_theta, cos_theta));
        const T k = two_theta / sin_theta;//定义theta/sin(theta/2)为k 
  //atan2(y,x)函数:计算(x,y)与原点连线与x轴正方向夹角，返回值为弧度，在几何意义上，atan2(y,x)=atan2(y/x),y>0时，夹角为逆时针，y<0时，夹角为顺时针得到；值域为（-pi/2,pi/2)
        
        
        
        
        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    } else {
        // For zero rotation, sqrt() will produce NaN in derivative since
        // the argument is zero. By approximating with a Taylor series,
        // and truncating at one term, the value and first derivatives will be
        // computed correctly when Jets are used..
        const T k(2.0);
        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    }

}


// 4. 旋转向量 旋转 特征点p=[x,y,z]
template<typename T>
// 参数：旋转向量、旋转轴、旋转结果
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
            // Away from zero, use the rodriguez formula
        //    Ow = -Rwc*tcw;  相当于是把角轴直接用罗德里格斯公式转R后直接乘以tcw  
        //   result = pt costheta +
        //            (w x pt) * sintheta +
        //            w (w . pt) (1 - costheta)
        //
        // We want to be careful to only evaluate the square root if the
        // norm of the angle_axis vector is greater than zero. Otherwise
        // we get a division by zero.
    
    
    // angle_axis的点击 即模长平方，也就是θ^2
    const T theta2 = DotProduct(angle_axis, angle_axis);
    // θ不为0
    if (theta2 > T(std::numeric_limits<double>::epsilon())) {
        // Away from zero, use the rodriguez formula
        // 罗德里格斯公式 R*p = cos(θ)*p + (1-cos(θ))*n*n^T*p + sin(θ)*n x p
        //   result = pt costheta +
        //            (w x pt) * sintheta +
        //            w (w . pt) (1 - costheta)
        //
        // We want to be careful to only evaluate the square root if the
        // norm of the angle_axis vector is greater than zero. Otherwise
        // we get a division by zero.
        //
        const T theta = sqrt(theta2);		// θ
        const T costheta = cos(theta);		// cos(θ)
        const T sintheta = sin(theta);		// sin(θ)
        const T theta_inverse = 1.0 / theta;	// 1/θ
		
        //  w旋转轴   = 旋转向量angle_axis/ θ
        const T w[3] = {angle_axis[0] * theta_inverse,  
                        angle_axis[1] * theta_inverse,
                        angle_axis[2] * theta_inverse};

        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        /*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
                                  w[2] * pt[0] - w[0] * pt[2],
                                  w[0] * pt[1] - w[1] * pt[0] };*/
        
        // 他这里其实是把罗德里格斯公式分开计算了
        T w_cross_pt[3];
        CrossProduct(w, pt, w_cross_pt);  // 叉积  w叉积pt  w是旋转轴

        const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
        //    (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);

        result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
        result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
        result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
    } else {
        // Near zero, the first order Taylor approximation of the rotation
        // matrix R corresponding to a vector w and angle w is
        //
        //   R = I + hat(w) * sin(theta)
        //
        // But sintheta ~ theta and theta * w = angle_axis, which gives us
        //
        //  R = I + hat(w)
        //
        // and actually performing multiplication with the point pt, gives us
        // R * pt = pt + w x pt.
        //
        // Switching to the Taylor expansion near zero provides meaningful
        // derivatives when evaluated using Jets.
        //
        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        /*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
                                  angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
                                  angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/
        T w_cross_pt[3];
        CrossProduct(angle_axis, pt, w_cross_pt);

        result[0] = pt[0] + w_cross_pt[0];
        result[1] = pt[1] + w_cross_pt[1];
        result[2] = pt[2] + w_cross_pt[2];
    }
}

#endif // rotation.h

4 random.h 返回随机数

#ifndef RAND_H
#define RAND_H

#include <math.h>
#include <stdlib.h>

inline double RandDouble()
{
    double r = static_cast<double>(rand());    // int rand (void) 无需参数  返回一个0到RAND_MAX间的随机整数int，static_cast<double>强制类型转换维double型
    return r / RAND_MAX;  // #define	RAND_MAX	2147483647
}


/*
Marsaglia polar method算法：  得到满足N（0,1）正态分布的随机点
  x和y是圆： x*x+y*y <=1内的随机点
  RandNormal()处理后  x*w是服从标准正态分布的样本
*/

inline double RandNormal()
{
    double x1, x2, w;
    do{
        x1 = 2.0 * RandDouble() - 1.0;  // 调用RandDouble，返回随机数
        x2 = 2.0 * RandDouble() - 1.0;
        w = x1 * x1 + x2 * x2;
    }while( w >= 1.0 || w == 0.0);   // 是0 或者 大于1那么就接着循环

    w = sqrt((-2.0 * log(w))/w);  //   0 < w < 1
    return x1 * w;
}

#endif // random.h

5 SnavelyReprojectionError.h

#ifndef SnavelyReprojection_H
#define SnavelyReprojection_H

#include <iostream>
#include "ceres/ceres.h"
#include "rotation.h"

class SnavelyReprojectionError {
public:
    SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x),
                                                                           observed_y(observation_y) {}

    template<typename T>
    bool operator()(const T *const camera,
                    const T *const point,
                    T *residuals) const {
        // camera[0,1,2] are the angle-axis rotation
        T predictions[2];
        CamProjectionWithDistortion(camera, point, predictions);
        // 计算残差，像素坐标系uv上的残差
        residuals[0] = predictions[0] - T(observed_x);
        residuals[1] = predictions[1] - T(observed_y);

        return true;
    }

    // camera : 9 dims array
    // [0-2] : angle-axis rotation 旋转
    // [3-5] : translateion
    // [6-8] : camera parameter, [6] focal length, [7-8] second and forth order radial distortion2,4次径向畸变
    // point : 3D location.
    // predictions : 2D predictions with center of the image plane.像素坐标
    template<typename T>
    static inline bool CamProjectionWithDistortion(const T *camera, const T *point, T *predictions) {
        // Rodrigues' formula
        T p[3];
        AngleAxisRotatePoint(camera, point, p);
        // camera[3,4,5] are the translation
        p[0] += camera[3];      // x
        p[1] += camera[4];      // y
        p[2] += camera[5];      // z

        // Compute the center fo distortion
        T xp = -p[0] / p[2];    // -x/z
        T yp = -p[1] / p[2];    // -y/z

        // Apply second and fourth order radial distortion
        const T &l1 = camera[7];    // k1
        const T &l2 = camera[8];    // k2

        T r2 = xp * xp + yp * yp;   
        T distortion = T(1.0) + r2 * (l1 + l2 * r2);    

        const T &focal = camera[6];
        predictions[0] = focal * distortion * xp;   // 忽略cx和cy
        predictions[1] = focal * distortion * yp;   // u = f*x*distortion,v = f*y*distortion

        return true;
    }
    /*使用 Ceres Solver 提供的 AutoDiffCostFunction 类创建一个自动微分的成本函数对象，它接受以下参数：
        SnavelyReprojectionError：误差模型类的类型。
        2：表示误差项的维度，这里是 2，因为我们计算了 x 和 y 方向上的误差。
        9 和 3：表示优化变量的维度，分别代表了相机参数和三维点坐标的维度。*/
    static ceres::CostFunction *Create(const double observed_x, const double observed_y) {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
            new SnavelyReprojectionError(observed_x, observed_y)));
    }

private:
    double observed_x;
    double observed_y;
};

#endif // SnavelyReprojection.h

3.4 bundle_adjustment_ceres.cpp

#include <iostream>
#include <ceres/ceres.h>
#include "common.h"
#include "SnavelyReprojectionError.h"

using namespace std;

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "usage: bundle_adjustment_ceres bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();
    bal_problem.Perturb(0.1, 0.5, 0.5);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

void SolveBA(BALProblem &bal_problem) {
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();

    // Observations is 2 * num_observations long array observations
    // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x
    // and y position of the observation.
    const double *observations = bal_problem.observations();
    ceres::Problem problem;

    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        ceres::CostFunction *cost_function;

        // Each Residual block takes a point and a camera as input
        // and outputs a 2 dimensional Residual
        cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);

        // If enabled use Huber's loss function.
        ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);

        // Each observation corresponds to a pair of a camera and a point
        // which are identified by camera_index()[i] and point_index()[i]
        // respectively.
        double *camera = cameras + camera_block_size * bal_problem.camera_index()[i];
        double *point = points + point_block_size * bal_problem.point_index()[i];

        problem.AddResidualBlock(cost_function, loss_function, camera, point);
    }

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observations. " << std::endl;

    std::cout << "Solving ceres BA ... " << endl;
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport() << "\n";
}

3.5 bundle_adjustment_g2o.cpp

#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/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>
#include <iostream>

#include "common.h"
#include "sophus/se3.hpp"

using namespace Sophus;
using namespace Eigen;
using namespace std;

// 定义了一个结构体，用来表示位姿和相机内参类
struct PoseAndIntrinsics {
    PoseAndIntrinsics() {}

    /// set from given data address
    explicit PoseAndIntrinsics(double *data_addr) {
        rotation = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
        translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
        focal = data_addr[6];
        k1 = data_addr[7];
        k2 = data_addr[8];
    }

    /// 将估计值放入内存
    void set_to(double *data_addr) {
        auto r = rotation.log();
        for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
        for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];
        data_addr[6] = focal;
        data_addr[7] = k1;
        data_addr[8] = k2;
    }

    SO3d rotation;      // 李群
    Vector3d translation = Vector3d::Zero();    
    double focal = 0;
    double k1 = 0, k2 = 0;
};

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
// g2o::BaseVertex<9, PoseAndIntrinsics> 顶点维度   顶点类型
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoseAndIntrinsics() {}

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

    virtual void oplusImpl(const double *update) override {
        _estimate.rotation = SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);
        _estimate.focal += update[6];
        _estimate.k1 += update[7];
        _estimate.k2 += update[8];
    }

    /// 根据估计值投影一个点
    Vector2d project(const Vector3d &point) {
        Vector3d pc = _estimate.rotation * point + _estimate.translation;   // 转相机坐标系
        pc = -pc / pc[2];   // 转到归一化坐标系
        double r2 = pc.squaredNorm();
        double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
        return Vector2d(_estimate.focal * distortion * pc[0],
                        _estimate.focal * distortion * pc[1]);  // 估计的观测值
    }

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};



// 路标点顶点   维度3，类型vector3d
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoint() {}

    virtual void setToOriginImpl() override {
        _estimate = Vector3d(0, 0, 0);
    }
    // 更新量
    virtual void oplusImpl(const double *update) override {
        _estimate += Vector3d(update[0], update[1], update[2]);
    }

    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}
};


// 误差边  <维度2  类型Vector2d   顶点类型>
class EdgeProjection :
    public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual void computeError() override {
        auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
        auto v1 = (VertexPoint *) _vertices[1];
        auto proj = v0->project(v1->estimate());
        _error = proj - _measurement;   // 预测 - 观测
    }

    // use numeric derivatives
    virtual bool read(istream &in) {}

    virtual bool write(ostream &out) const {}

};

void SolveBA(BALProblem &bal_problem);

int main(int argc, char **argv) {

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;
        return 1;
    }

    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();
    bal_problem.Perturb(0.1, 0.5, 0.5);
    bal_problem.WriteToPLYFile("initial.ply");
    SolveBA(bal_problem);
    bal_problem.WriteToPLYFile("final.ply");

    return 0;
}

void SolveBA(BALProblem &bal_problem) {
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // use LM
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    /// build g2o problem
    const double *observations = bal_problem.observations();
    // vertex
    vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
    vector<VertexPoint *> vertex_points;
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
        double *camera = cameras + camera_block_size * i;
        v->setId(i);
        v->setEstimate(PoseAndIntrinsics(camera));  // 赋值相机参数
        optimizer.addVertex(v);
        vertex_pose_intrinsics.push_back(v);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        VertexPoint *v = new VertexPoint();
        double *point = points + point_block_size * i;
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(Vector3d(point[0], point[1], point[2]));
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);
        optimizer.addVertex(v);
        vertex_points.push_back(v);
    }

    // edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        EdgeProjection *edge = new EdgeProjection;
        edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
        edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
        edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
        edge->setInformation(Matrix2d::Identity());
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);

    // set to bal problem
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        double *camera = cameras + camera_block_size * i;
        auto vertex = vertex_pose_intrinsics[i];
        auto estimate = vertex->estimate();
        estimate.set_to(camera);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        double *point = points + point_block_size * i;
        auto vertex = vertex_points[i];
        for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
    }
}