【视觉SLAM十四讲】后端 1

这一章内容不算多，但难度并不低，主要是因为有着大量的数学式子要推导，我会尽可能将推导拆碎一些方便理解（所以不要被大量的数学式子吓到，只是因为大部分中间式和推导都写出来了，实际上没那么多）。强烈建议对着写写实现代码，不然很难将理论落地

状态估计问题

回顾我们之前提到过的运动和观测方程，在 $t=0$ 到 $t=N$ ，有位姿 $x_0,\cdots ,x_k$ ，路标 $y_1,\cdots ,y_j$

为了简化表达，我们设定 $x_k$ 为 $k$ 时刻的所有未知量
x k = def { x k , y 1 , ⋯   , y j } x_k \overset{\text{def}}{=}\{x_k,y_1,\cdots,y_j\} xk​=def{xk​,y1​,⋯,yj​}
此时运动方程和观测方程可以简化为
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+{\omega }_k\\ z_k=h(x_k)+v_k\end{array}$$
由于每个方程都受到噪声的影响，所以这里的 $x$ 和 $y$ 可以看成是服从某个概率分布的随机变量。我们希望通过使用过去的 0 到 k 中的数据来估计现在的状态分布
$$P(x_k|x_0,u_{1:k},z_{1:k})$$
按照贝叶斯法则 $P(A|B)=\frac{P(B|A)\cdot P(A)}{P(B)}$
$$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k})$$
让我们回忆一下非线性优化中关于贝叶斯法则的内容：

在这个情境下，事件：状态 X 的取值；证据：实际观测数据 Z ；先验 $P(X)$ ：根据 $0\sim k-1$ 的各种数据推测得到的 $k$ 时刻状态；
似然 $P(Z|X)$ ：假设状态是 X ，我们观测到数据 Z 的可能性有多大；后验 $P(X|Z)$ ：在观测到 $k$ 时刻的观测数据 Z 时，状态 X 的概率分布

回到式子中来，可以理解为：根据 $0\sim k$ 时刻所有数据得到的状态概率分布，正比于“对于假设的 $k$ 时刻状态，最符合的观测数据”与“根据 $0\sim k-1$ 的各种数据推测得到的 $k$ 时刻状态”的乘积。即：后验 = 似然 * 先验

对于这里的先验，它是由过去所有状态估计得来的，所以也可以展开为积分形式
$$P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1})P(x_{k-1}|x_0,u_{1:k},z_{1:k-1})d{x}_{k-1}$$
自此，我们已经得到了状态估计的贝叶斯估计，要进一步的使用它，有两种方法：

1. 滤波派：以卡尔曼滤波为代表，基于马尔科夫性，认为 $k$ 时刻状态只与 $k-1$ 时刻状态有关
2. 优化派：以非线性优化为主体，考虑 $k$ 时刻之前的所有状态

线性系统和 KF

当我们假设了马尔科夫性，那么 $k$ 只与 $k-1$ 时刻状态有关，状态估计的贝叶斯估计还可以进一步简化
$$P(x_k|x_0,u_{1:k},z_{1:k-1})=\int P(x_k|x_{k-1},u_k)P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})d{x}_{k-1}$$
卡尔曼滤波建立在线性高斯系统上的，即满足

1. 线性运动方程和观测方程
   $$\{\begin{array}{l}{x}_k=A_k{x}_{k-1}+u_k+{\omega }_k\\ z_k=C_k{x}_k+v_k\end{array}$$

2. 状态和噪声满足高斯分布
   $${\omega }_k\sim N(0,R_k)v_k\sim N(0,Q_k)$$

卡尔曼滤波可以分为预测和更新两个步骤（ 为方便区分和表达，$\hat{x}$ 表示后验， $\check{x}$ 表示先验，后续的 $R$ 和 $Q$ 不再带下标。如果不想看推导的也可以直接跳到后面看公式）

预测

从上一时刻状态出发，根据输入信息（带噪声）推断当前时刻的状态分布，就是预测

根据高斯分布的性质，对于 $y=Ax+b+ϵ$ ，如果 $x\sim N({\mu }_x,{\Sigma }_x)ϵ\sim N(0,{\Sigma }_{ϵ})$ ，那么 $y\sim N(A{\mu }_x+b,A{\Sigma }_x{A}^T+{\Sigma }_{ϵ})$

• $k-1$后验分布：$P(x_{k-1}|x_0,u_{1:k-1},z_{1:k-1})=N({\hat{x}}_{k-1},{\hat{P}}_{k-1})$ 其中 ${\hat{x}}_{k-1}$是状态估计均值，${\hat{P}}_{k-1}$是协方差矩阵
• 状态转移模型：$P(x_k|x_{k-1},u_k)=N(A_k{x}_{k-1}+u_k,R)$ 其中 $A_k$ 是状态转移矩阵，$u_k$ 是控制输入， $R_k$ 是过程噪声协方差

那么 $P(x_k|x_0,u_{1:k},z_{1:k-1})=\int N(A_k{x}_{k-1}+u_k,R)\cdot N({\hat{x}}_{k-1},{\hat{P}}_{k-1}))d{x}_{k-1}$ ，即两个高斯分布的卷积

1. 期望计算：
   其期望 $\mathbb{E}[x_k|\cdots ]=\mathbb{E}[A_k{x}_{k-1}+u_k+{\omega }_k|x_0,u_{1:k},z_{1:k-1}]$

   • $\mathbb{E}[A_k{x}_{k-1}|\cdots ]=A_k\mathbb{E}[x_{k-1}|\cdots ]=A_k{\hat{x}}_{k-1}$
   • $\mathbb{E}[u_k|\cdots ]=u_k$ （确定项）
   • $\mathbb{E}[{\omega }_k|\cdots ]=0$ （零均值噪声）

   则 $\mathbb{E}[x_k|\cdots ]=A_k{\hat{x}}_{k-1}+u_k$

2. 协方差计算：
   定义状态估计误差项 $x_k-\mathbb{E}[x_k]=A_k(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_k$ ，已知 $\text{Cov}[x]=\mathbb{E}[(x-\mathbb{E}[x])(x-\mathbb{E}[x]{)}^T]$
   其协方差 $\text{Cov}[x_k]=\mathbb{E}[(A_k(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_k)(A_k(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_k{)}^T]$
   第二项计算内层转置得到：$(x_{k-1}-{\hat{x}}_{k-1}{)}^T{A}_k^T+{\omega }_k^T$
   展开得到：$\mathbb{E}[A_k(x_{k-1}-{\hat{x}}_{k-1})(x_{k-1}-{\hat{x}}_{k-1}{)}^T{A}_K^T]+\mathbb{E}[A_k(x_{k-1}-{\hat{x}}_{k-1}){\omega }_k^T]+\mathbb{E}[{\omega }_k{A}_k(x_{k-1}-{\hat{x}}_{k-1}^T)]+\mathbb{E}[{\omega }_k{\omega }_k^T]$

   • 第一项：$\cdots =A_k\mathbb{E}[(x_{k-1}-{\hat{x}}_{k-1})(x_{k-1}-{\hat{x}}_{k-1}{)}^T]A_k^T=A_k\widehat{P_{k-1}}{A}_k^T$
   • 第二项和第三项：因为 ${\omega }_k$ 与 $x_{k-1}$ 独立，故均等于 $0$
   • 第四项：$\mathbb{E}[{\omega }_k{\omega }_k^T]=R$

   则 $\text{Cov}[x_k]=A_k{\hat{P}}_{k-1}{A}_k^T+R$ 最后我们得到了 $P(x_k|x_0,u_{1:k},z_{1:k-1})=N(A_k{\hat{x}}_{k-1}+u_k,A_k{\hat{P}}_{k-1}{A}_k^T+R)$ ，即 $k$ 时刻的先验状态估计分布。为简化表达可定义

$${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$

更新

从当前时刻的预测状态出发，根据观测信息（带噪声）修正状态估计，得到更准确的后验分布，就是更新

为了求解 $k$ 时刻的后验状态估计分布，我们还需要求解 $k$ 时刻的似然

这里方法和上面求解先验的一样，都是可以通过期望和协方差得到的，不再赘述：$P(z_k|x_k)=N(C_k{x}_k,Q_k)$ ，那么：
$$N({\hat{x}}_k,{\hat{P}}_k)=N(C_k{x}_k,Q)\cdot N({\check{x}}_k,{\check{P}}_k)$$

已知高斯分布可以表达为（归一化常数和指数）：
$$f(x)=\frac{1}{(2\pi {)}^{\frac{\pi }{2}}|\Sigma {|}^{\frac{1}{2}}}\exp (-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))$$
其中 $x$ 是我们要估计的系统状态，$\mu$ 则是 $x$ 的期望值

因为我们只关心和 $x_k$ 有关的部分，所以这里舍去归一化常数，只保留指数部分展开得到
$$(x_k-{\hat{x}}_k{)}^T{\hat{P}}_k^{-1}(x_k-{\hat{x}}_k)=(z_k-C_k{x}_k{)}^T{Q}^{-1}(z_k-C_k{x}_k)+(x_k-{\check{x}}_k{)}^T{\check{P}}_k^{-1}(x_k-{\check{x}}_k)$$

两边展开（单纯比较长而已，其实不难算）：
$$x_k^T{\hat{P}}_k^{-1}{x}_k-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k+{\hat{x}}_k^T{\hat{P}}_k^{-1}{\hat{x}}_k=x_k^T(C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1})x_k-2(z_k^T{Q}^{-1}{C}_k+{\check{x}}_k^T{\check{P}}_k^{-1})x_k+(z_k^T{Q}^{-1}{z}_k+{\check{x}}_k^T{\check{P}}_k^{-1}{\check{x}}_k)$$
取二次项系数
$${\hat{P}}_k^{-1}=C_k^T{Q}^{-1}{C}_k+{\check{P}}_k^{-1}$$
两边同乘 ${\hat{P}}_k$
$$I={\hat{P}}_k{C}_k^T{Q}^{-1}{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}$$
定义卡尔曼增益为
$$K={\hat{P}}_k{C}_k^T{Q}^{-1}$$
从而得到协方差更新公式（后续的式子转换都是以建立 $k$ 时刻的先验与后验关系为核心）
$$I=K{C}_k+{\hat{P}}_k{\check{P}}_k^{-1}\Rightarrow {\hat{P}}_k=(I-K{C}_k){\check{P}}_k$$
如果将这里的协方差更新公式代回到卡尔曼增益的定义中，我们就等到了卡尔曼增益的标准表达式
$$K={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+Q{)}^{-1}$$
取第一项系数
$$-2{\hat{x}}_k^T{\hat{P}}_k^{-1}{x}_k=-2z_k^T{Q}^{-1}{C}_k{x}_k-2{\check{x}}_k^T{\check{P}}_k^{-1}{x}_k$$
整理并转置得到（注意标量的转置等于它本身）
$${\hat{P}}_k^{-1}{\hat{x}}_k=C_k^T{Q}^{-1}{z}_k-{\check{P}}_k^{-1}{\check{x}}_k$$
两边乘以 ${\hat{P}}_k$ 并代回 $K={\hat{P}}_k{C}_k^T{Q}^{-1}$ 得到
$$\begin{array}{rl}{\hat{x}}_k& ={\hat{P}}_k{C}_k^T{Q}^{-1}{z}_k-{\hat{P}}_k{\check{P}}_k^{-1}{\check{x}}_k\\ & =K{z}_k+(I-K{C}_k){\check{x}}_k\\ & ={\check{x}}_k+K(z_k-C_k{\check{x}}_k)\end{array}$$

总结

至此，预测和更新的主要公式我们都已推导完毕，这里做个总结

1. 预测
   $${\check{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$

2. 更新
   先计算卡尔曼增益：
   $$K={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+Q_k{)}^{-1}$$
   然后计算后验概率的分布：
   $$\begin{gathered} \hat{x}={\check{x}}_k+K(z_k-C_k{\check{x}}_k) \\ {\hat{P}}_k=(I-K{C}_k){\check{P}}_k \end{gathered}$$

非线性系统和 EKF

理解完卡尔曼滤波之后，我们必须要明白 SLAM 中的运动方程和观测方程通常是非线性的。一个高斯分布在经过非线性变换后，其结果往往不再是高斯分布，所以在非线性系统中，我们必须将一个非高斯分布近似为高斯分布

扩展卡尔曼滤波（EKF）和卡尔曼滤波（KF）本质上是一样的，只不过适用于非线性系统。其实现思路并不复杂：在某个点附近，对运动方程和观测方程进行一阶泰勒展开，保留一阶项（线性部分），然后按照线性系统推导

线性化过程

在 ${\hat{x}}_{k-1}$ 处对运动方程 $f(x_{k-1},u_k)$ 进行一阶泰勒展开
$$x_k\approx f({\hat{x}}_{k-1},u_k)+{\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_k$$
记偏导数为
$$F={\frac{\partial f}{\partial x_{k-1}}|}_{{\hat{x}}_{k-1}}$$
同理，在 ${\hat{x}}_k$ 处对运动方程 $h(x_k,u_k)$ 进行一阶泰勒展开
$$z_k\approx h({\check{x}}_k)+{\frac{\partial h}{\partial x_k}|}_{{\hat{x}}_k}+v_k$$
记偏导数为
$$H={\frac{\partial h}{\partial x_k}|}_{{\hat{x}}_k}$$

预测

推导其实和卡尔曼滤波是极其相似的，这里就只展示结果了

状态预测：
$${\check{x}}_k=f({\hat{x}}_{k-1},u_k)$$
协方差预测：
$${\check{P}}_k=F_k{\hat{P}}_{k-1}{F}_k^T+R$$

更新

卡尔曼增益：
$$K={\check{P}}_k{H}_k^T(H_k{\check{P}}_k{H}_k^T+Q{)}^{-1}$$
状态更新
$${\hat{x}}_k={\check{x}}_k+K(z_k-h({\check{x}}_k))$$
协方差更新
$${\hat{P}}_k=(I-K{H}_k){\check{P}}_k$$

代码实现

class ExtendedKalmanFilter{
public:
    ExtendedKalmanFilter() : state_size_(8)
    {
        // 初始化状态向量
        state_ = Eigen::VectorXd::Zero(state_size_);
        // 初始化状态协方差矩阵
        P_ = Eigen::MatrixXd::Identity(state_size_, state_size_);

        // 初始化过程噪声协方差矩阵
        R_ = Eigen::MatrixXd::Zero(state_size_, state_size_);
        R_.diagonal() << 0.1, 0.1, 0.1, 0.05, 0.5, 0.5, 0.5, 0.2;

        // 初始化测量噪声协方差矩阵
        Q_ = Eigen::MatrixXd::Zero(4, 4);
        Q_.diagonal() << 0.1, 0.1, 0.05, 0.05;

        // 初始化雅可比矩阵
        F_ = Eigen::MatrixXd::Identity(state_size_, state_size_);
        H_ = Eigen::MatrixXd::Zero(4, state_size_);
        H_.block(0, 0, 4, 4) = Eigen::MatrixXd::Identity(4, 4);
    }

    // 状态转移函数（运动模型）
    Eigen::VectorXd stateTransitionFunction(const Eigen::VectorXd& state, double dt)
    {
        Eigen::VectorXd new_state = state;

        // 位置更新
        new_state(0) += state(4) * dt;
        new_state(1) += state(5) * dt;
        new_state(2) += state(6) * dt;
        // yaw 更新
        new_state(3) += state(7) * dt;

        return new_state;
    }

    // 计算状态转移雅可比矩阵
    Eigen::MatrixXd computeStateJacobian(const Eigen::VectorXd& state, double dt)
    {
        Eigen::MatrixXd J = Eigen::MatrixXd::Identity(state_size_, state_size_);

        // 位置对速度的偏导
        J(0, 4) = dt;
        J(1, 5) = dt;
        J(2, 6) = dt;
        J(3, 7) = dt;
        return J;
    }

    // 观测函数（测量模型）
    Eigen::VectorXd observationFunction(const Eigen::VectorXd& state)
    {
        Eigen::VectorXd observation = state.head(4);  // x, y, z, yaw
        return observation;
    }

    // 计算观测函数雅可比矩阵
    Eigen::MatrixXd computeObservationJacobian(const Eigen::VectorXd& state)
    {
        Eigen::MatrixXd J = Eigen::MatrixXd::Zero(4, state_size_);
        J.block(0, 0, 4, 4) = Eigen::MatrixXd::Identity(4, 4);
        return J;
    }

    // 预测步骤
    void predict(double dt)
    {
        // 计算状态转移函数的雅可比矩阵
        F_ = computeStateJacobian(state_, dt);

        // 预测状态
        state_ = stateTransitionFunction(state_, dt);

        // 预测协方差
        P_ = F_ * P_ * F_.transpose() + R_;
    }

    // 更新步骤
    void update(const Eigen::VectorXd& z)
    {
        // 计算观测函数的雅可比矩阵
        H_ = computeObservationJacobian(state_);

        // 计算卡尔曼增益
        Eigen::MatrixXd tmp = H_ * P_ * H_.transpose() + Q_;
        Eigen::MatrixXd K = P_ * H_.transpose() * tmp.inverse();

        // 计算观测残差： y = z - h(x)
        Eigen::VectorXd z_pred = observationFunction(state_);
        Eigen::VectorXd y = z - z_pred;

        // 角度归一化
        if (y(3) > M_PI) y(3) -= 2 * M_PI;
        if (y(3) < -M_PI) y(3) += 2 * M_PI;

        // 更新状态
        state_ = state_ + K * y;

        // 更新协方差
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(state_size_, state_size_);
        P_ = (I - K * H_) * P_;

        std::cout << "state: " << state_.transpose() << std::endl;
    }

    // 获取当前状态
    Eigen::VectorXd getState() const { return state_; }

    // 获取当前协方差
    Eigen::MatrixXd getCovariance() const { return P_; }

    // 设置初始状态
    void setState(const Eigen::VectorXd& init_state) { state_ = init_state; }

private:
    // 状态向量：[x, y, z, yaw, vx, vy, vz, v_yaw]
    Eigen::VectorXd state_;              // 状态向量
    Eigen::MatrixXd P_;                  // 状态协方差矩阵
    Eigen::MatrixXd R_;                  // 过程噪声协方差矩阵
    Eigen::MatrixXd Q_;                  // 测量噪声协方差矩阵
    Eigen::MatrixXd F_;                  // 状态转移函数的雅可比矩阵
    Eigen::MatrixXd H_;                  // 观测函数的雅可比矩阵

    int state_size_;                     // 状态向量大小
};

int main()
{
    ExtendedKalmanFilter ekf;

    // 设置初始状态 [x, y, z, yaw, vx, vy, vz, v_yaw]
    Eigen::VectorXd init_state(8);
    init_state << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.1;
    ekf.setState(init_state);

    double dt = 0.1;
    int step = 10;

    for (int i = 0; i < step; ++i)
    {
        std::cout << "step: " << i << std::endl;

        // 预测
        ekf.predict(dt);

        // 生成模拟观测
        Eigen::VectorXd true_state = ekf.getState();
        Eigen::VectorXd z(4);
        z << true_state(0) + 0.05 * (rand() / (double)RAND_MAX - 0.5),
             true_state(1) + 0.05 * (rand() / (double)RAND_MAX - 0.5),
             true_state(2) + 0.02 * (rand() / (double)RAND_MAX - 0.5),
             true_state(3) + 0.02 * (rand() / (double)RAND_MAX - 0.5);

        // 更新步骤
        ekf.update(z);

        // 输出结果
        std::cout << "True State: " << true_state.transpose() << std::endl;
        std::cout << "Estimated State: " << ekf.getState().transpose() << std::endl;
        std::cout << "Covariance: " << ekf.getCovariance() << std::endl;
    }
}

BA 与图优化

“Bundle Adjustment” 直译过来就是光束调整，即考虑从任意特征点发射出来的几束光线，通过调整个相机姿态和各特征点的空间位置，使这些光线最终收束到相机的光心（简单点说就是要让所有图像中的特征点看起来都来自同一个空间位置）、

投影模型和代价函数

回顾一下前面的坐标系转换，从世界坐标系中的 $p$ 点出发，考虑相机的内外参数和畸变，最后投影成像素坐标

1. 世界坐标系 $\to$ 相机坐标系
   $$P^{\prime }=Rp+t=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$

2. 将 $P$ 投影至归一化平面
   $$P_c=[u_c,v_c,1{]}^T=[X^{\prime }/Z^{\prime },Y^{\prime }/Z^{\prime },1{]}^T$$

3. 去畸变（这里仅考虑径向）
   $$\{\begin{array}{l}{u}_c^{\prime }=u_c(1+k_1{r}_c^2+k_2{r}_c^4)\\ v_c^{\prime }=v_c(1+k_1{r}_c^2+k_2{r}_c^4)\end{array}$$

4. 相机坐标系 $\to$ 像素坐标系
   $$\{\begin{array}{l}{u}_s^{\prime }=f_x{u}_c^{\prime }+c_x\\ v_s=f_y{v}_c^{\prime }+c_y\end{array}$$

这个过程也是我们前面讲到过的观测方程：$z=h(x,y)$

如果假定 $x$ 为此时相机的位姿（外参 $R,t$ ），对应的李群为 $T$ ，李代数为 $\xi$ 。路标 $y$ 即三维点 $p$ ，观测数据即像素坐标 $z\stackrel{def}{=}[u_s,v_s{]}^T$ 。那么，此次观测的误差可表示为：
$$e=z-h(T,p)$$
考虑上其他时刻的数据，设 $z_{i,j}$ 为在位姿 $T_i$ 处观测路标 $p_j$ 产生的数据，那么代价函数可写为：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||z_{ij}-h(T_i,p_j)|{|}^2$$

BA 的求解

在前面的非线性优化中，我们已经提过了误差项，不同的是，那一章的误差项是针对单个位姿和路标的，但是在整体 BA 的目标函数上，我们要定义自变量为所有待优化的变量

和前面的最小重投影误差不同，虽然原理是相同的，但是前面视觉里程计中使用的是交替优化，先优化位姿，再利用得到位姿优化 3D 点；BA 则是联合优化，同时优化所有位姿和 3D 点

设对相机姿态的偏导为 $F$ ，对路标点的偏导为 $E$ ，目标函数变为：
$$\frac{1}{2}||f(x+\Delta x)|{|}^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||e_{ij}+F_{ij}\Delta {\xi }_i+E_{ij}\Delta p_j|{|}^2$$
BA 的变量是针对整体的，所以把所有位姿作为一个变量 $x_c$ ，所有空间点作为一个变量 $x_p$
$$\begin{gathered} x_c=[{\xi }_1,{\xi }_2,\cdots ,{\xi }_m{]}^T\in {\mathbb{R}}^{6m} \\ {x}_p=[p_1,p_2,\cdots ,p_n{]}^T\in {\mathbb{R}}^{3n} \end{gathered}$$
那么目标函数可简化为：
$$\frac{1}{2}||f(x+\Delta x)|{|}^2=\frac{1}{2}||e+F\Delta x_c+E\Delta x_p|{|}^2$$

该式将许多小型二次项之和变为矩阵形式，那么这里的雅可比矩阵 $E$ 和 $F$ 必须是整体目标函数对整体变量的导数，其中由对每个误差项的导数 $F_{ij}$ 和 $R_{ij}$ 组合而成

于是我们又回到了经典的增量线性方程
$$H\Delta x=g$$
以高斯牛顿法为例，将变量归类为位姿和空间点两种，雅可比矩阵可分块为
$$J=[FE]$$
$H$ 矩阵为
$$H=J^{T}J=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ E^{T}F& E^{T}E\end{array}\right]$$
因为考虑了所有的优化变量，这个线性方程的维度将非常大，如果直接对 $H$ 求逆的话计算起来会十分困难。幸运的是，这个矩阵是有特殊结构的，可以借此简化运算

稀疏化和边缘化

已知每个误差项只依赖于相机位姿和路标点，因此雅可比矩阵 $J$ 是高度稀疏的，而继承于 $J$ 的$H$ 矩阵更是增强了这种稀疏性

误差项只描述了在 $T_i$ 观测到 $p_j$ 的情况，只涉及到第 $i$ 个相机位姿和第 $j$ 个路标点，ss对其余部分的导数为 0 ，所以该误差项对应的雅可比可写为
$$J_{ij}(x)=\left(0_{2\times 6},\cdots 0_{2\times 6},\frac{\partial e_{ij}}{\partial T_i},0_{2\times 6},\cdots 0_{2\times 3},\cdots 0_{2\times 3},\frac{\partial e_{ij}}{\partial p_j},0_{2\times 3},\cdots 0_{2\times 3}\right)$$

可以看出，除了表示相机姿态的偏导 $\frac{\partial e_{ij}}{\partial T_i}$ 维度为 $2\times 6$ ，对路标点的偏导 $\frac{\partial e_{ij}}{\partial p_j}$ 维度为 $2\times 3$ ，其余地方均为 0

如图，如果相机分别在位姿 $C_1$ 和 $C_2$ 处，观察到的特征点情况为：

那么雅可比矩阵和 $H$ 矩阵如下图所示：

对于这种稀疏结构的 $H$ 矩阵，我们可以利用 $\text{Schur}$ 消元求解。如下图，我们将 $H$ 矩阵分为四块，$B$ 和 $C$ 为对角块矩阵，分别与相机位姿和路标的维度相同

此时对应的增量方程可以由 $H\Delta x=g$ 变为：
$$\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v\\ \omega \end{array}\right]$$
其中：

• $B=F^{T}F$ ：相机-相机块，维度为 $6\times 6$
• $C=E^{T}E$ ：路标-路标块，维度为 $3\times 3$
• $E=F^{T}E$ ：相机-路标耦合块，维度为 $6\times 3$

由于一般矩阵求逆的难度远远大于对角块矩阵，所以我们要对一般矩阵 $E$ 进行高斯消元
$$\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$
整理得到：
$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}\omega \\ \omega \end{array}\right]$$
消元之后，第一行就变成了和 $\Delta x_p$ 无关的项，单独提取出来我们就得到了关于位姿部分的增量方程：
$$\left[\begin{array}{c}B-E{C}^{-1}{E}^T\end{array}\right]\Delta x_c=v-E{C}^{-1}\omega$$
该方程没有什么特殊的结构了，就只是一个线性方程

求解该方程得到 $\Delta x_c$ ，再代回去即可得到 $\Delta x_p$

回到位姿的增量方程，我们定义
$$S=\left[\begin{array}{c}B-E{C}^{-1}{E}^T\end{array}\right]$$
$S$ 矩阵的稀疏性是不规则的，从图中可以看出，有色部分即为两个位置有着共同的观测点，白色则表示没有

鲁棒核函数

在前面的 BA 问题中，我们定义目标函数为最小化误差项的二次项范数平方和，但是如果存在较大的误差项的话，其二范数增长速度会很快，从而让收敛速度慢很多，甚至可能导致正确结果偏移。因此我们需要引入核函数，核函数通过将误差的二范数度量替换成其他增长速度没那么快的函数，同时保证其光滑性质。

以常见的 Huber 核为例：
$$H(e)=\{\begin{array}{l}\frac{1}{2}{e}^2|e|\le \delta \\ \delta (|e|-\frac{1}{2}\delta )\text{otherwise}\end{array}$$

代码实现

BAL 数据集格式

这里利用了 BAL 数据集，可以通过 https://grail.cs.washington.edu/projects/bal/index.html 获取，这里以 problem-16-22106-pre.txt 为例

对于数据集，其格式为：

• 第一行：相机数量，三维点数量，二维观测数量
• 第二行~第(2+num_observations-1)行：相机索引，三维点索引，x，y
• 第(2+num_observations-1)行~第(2+num_observations+num_cameras-1)行：外参（旋转+平移），焦距，畸变系数共 9 个参数
• 最后 num_points 行：三维点的X，Y，Z

Ceres BA 代码

代码中 BALProblem 类的定义和实现可以在ceres库里面的example获取，直接加入工作空间即可使用

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <iostream>
#include "bal_problem.h"
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

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
    {
        T prediction[2];
        CamProjectionWithDistortion(camera, point, prediction);
        residuals[0] = prediction[0] - T(observed_x);
        residuals[1] = prediction[1] - T(observed_y);

        return true;
    }

    // camera : 9 维数组：[0-2] 为旋转向量，[3-5] 为平移向量，[6-8] 为相机内参:6为焦距(设fx=fy)，8、9为二阶和四阶径向畸变参数
    // point : 3D 位置
    // predictions : 以图像中心为原点的 2D 预测点
    template <typename T>
    static bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions)
    {
        // 罗德里格斯公式 // p = R * p
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);

        // 平移向量      // p = p + t
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // 归一化平面
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];

        // 获取二阶和四阶径向畸变参数
        const T& l1 = camera[7];
        const T& l2 = camera[8];

        // 计算畸变后的坐标
        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;
        predictions[1] = focal * distortion * yp;

        return true;
    }

    static ceres::CostFunction* Create(const double observed_x, const double observed_y)
    {
        return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(  // 2 表示残差维度，9 表示相机参数维度，3 表示点的维度
            new SnavelyReprojectionError(observed_x, observed_y)));
    }

private:
    double observed_x;
    double observed_y;
};

class OptimizationMonitor : public ceres::IterationCallback
{
public:
    OptimizationMonitor()
    {
        start_time_ = std::chrono::steady_clock::now();
    }

    ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary)
    {
        auto current_time = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - start_time_);

        // 记录每次迭代的信息
        iterations_.push_back(summary.iteration);
        costs_.push_back(summary.cost);
        times_.push_back(duration.count() / 1000.0);
        cost_changes_.push_back(summary.cost_change);
        gradient_norms_.push_back(summary.gradient_norm);

        std::cout << "iteration: " << summary.iteration
                  << ", cost: " << summary.cost
                  << ", cost_change: " << summary.cost_change
                  << ", time: " << times_.back() << "s" << std::endl;

        return ceres::SOLVER_CONTINUE;
    }

    // matplotlib 可视化
    void plotResults()
    {
        try
        {
            // 创建子图布局
            plt::figure_size(1200, 800);

            // 子图1：代价函数下降曲线
            plt::subplot(2, 2, 1);
            plt::semilogy(iterations_, costs_);
            plt::title("Cost Function Convergence");
            plt::xlabel("Iterations");
            plt::ylabel("Cost (log scale)");
            plt::grid(true);

            // 子图2：每次迭代时间
            plt::subplot(2, 2, 2);
            plt::plot(iterations_, times_);
            plt::title("Cumulative Time");
            plt::xlabel("Iterations");
            plt::ylabel("Time (s)");
            plt::grid(true);

            // 子图3：代价变化
            plt::subplot(2, 2, 3);
            plt::semilogy(iterations_, cost_changes_);
            plt::title("Cost Change");
            plt::xlabel("Iterations");
            plt::ylabel("Cost Change (log scale)");
            plt::grid(true);

            // 子图4：梯度范数
            plt::subplot(2, 2, 4);
            plt::semilogy(iterations_, gradient_norms_);
            plt::title("Gradient Norm");
            plt::xlabel("Iterations");
            plt::ylabel("Gradient Norm (log scale)");
            plt::grid(true);

            // 显示所有子图
            plt::tight_layout();
            plt::show();
        }
        catch (const std::exception& e)
        {
            std::cerr << "Error: " << e.what() << std::endl;
        }
    }

private:
    std::chrono::steady_clock::time_point start_time_;
    std::vector<int> iterations_;
    std::vector<double> costs_;
    std::vector<double> times_;
    std::vector<double> cost_changes_;
    std::vector<double> gradient_norms_;
};

void SolveBA(ceres::examples::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 是一个大小为 2 * num_observations 的数组，其中每两个元素表示一个观测
    const double* observations = bal_problem.observations();

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

        // 每个残差块以一个点和一个相机作为输入，输出一个二维残差
        cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]);

        ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);

        // 获取参数块的指针：起始指针 + 参数块大小 * 对应索引
        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);
    }

    std::cout << "Solving ceres BA ..." << std::endl;
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::LinearSolverType::SPARSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.max_num_iterations = 100;

    // 记录优化过程中的信息
    OptimizationMonitor monitor;
    options.callbacks.push_back(&monitor);
    options.update_state_every_iteration = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << summary.FullReport() << "\n";

    // 绘制优化结果
    monitor.plotResults();
}

int main()
{
    ceres::examples::BALProblem bal_problem("../test/data", false);
    SolveBA(bal_problem);
    return 0;
}

可视化结果如图：