SLERP 导数

看了 Joan Solà 的论文 [1]，写了博客 《A Micro Lie Theory 论文理解》。自认为对论文理解程度已经可以实用。

找一个以前解决得不好的过程用现在学到的理论处理。SLERP(Spherical Linear intERPolation)过程，在 Joan Solà 的论文 [2] Section 2.7 中有介绍 3 种方法，此处选择 Method 1。为了保证问题描述清晰，所以以下定义问题的方法不会与 [2] 中完全一致。

以下正文中的公式引用使用两种形式：本文的公式，如(1)；[1] 的公式，如(1*)。

1. 问题定义

假设相机 C 相对世界坐标系 W 的姿态可以用 ({}^{C}mathbf{R}_W) 表示。在 (t_0) 时刻，姿态为 ({}^{C0}mathbf{R}_W)，在 (t_1) 时刻，姿态为 ({}^{C1}mathbf{R}_W)，(t_0 < t_1)。那么在 (t_0) 时刻到 (t_1) 时刻中间的某个时刻 (t_i) 相机的姿态 ({}^{Ci}mathbf{R}_W) 如何用 ({}^{C0}mathbf{R}_W, {}^{C1}mathbf{R}_W) 表达？

令 (t_i = t_0 + alpha (t_1 - t_0), alphain[0,1])。

[egin{align} {}^{Ci}mathbf{R}_W &= {}^{Ci}mathbf{R}_{C0} {}^{C0}mathbf{R}_W \ &= ({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)^alpha {}^{C0}mathbf{R}_W \ &= ext{Exp}(alpha ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)) {}^{C0}mathbf{R}_W label{eq:origin} end{align}]

求 ({}^{Ci}mathbf{R}_W) 对 ({}^{C1}mathbf{R}_W, {}^{C0}mathbf{R}_W, alpha) 的导数。前两个导数是 (mathbb{R}^3) 对 (mathbb{R}^3) 的导数，最后一个导数是 (mathbb{R}^3) 对 (mathbb{R}) 的导数。

2. 选择

所以，我们需要求什么形式的导数？是扰动还是右扰动？为了方便代码验证，按照我前面一篇博客 《[ceres-solver] From QuaternionParameterization to LocalParameterization》 所述，ceres::QuaternionParameterization 默认是左扰动。依此定下左扰动的 convention。

这个问题是否与 Quaternion 的 Hamilton 或 JPL 有关？公式推导与其无关，公式推导只涉及 SO(3), so(3)，以及 so(3) 对应的 (mathbb{R}^3)。

3. 求导

因为是使用左扰动形式，所以对应到论文 [1] 中，主要使用公式 (27*), (28*), (44*)。

以下求导会大量使用 chain rule，不一定对，但是总是可以试一下，最后使用代码验证公式是否正确。使用 chain rule 需要谨记 [1] (59*) 之前的一句话。

Notice that when mixing right, left and corssed Jacobians, we need to chain also the reference frames, ...

令 (m{ au}= ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T))，则。

[egin{align} {}^{Ci}mathbf{R}_W &= ext{Exp}(alpha m{ au}) {}^{C0}mathbf{R}_W end{align}]

对需要计算的三个导数 ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W}, {partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W}, {partial {}^{Ci}mathbf{R}_W over partial alpha}) （虽然导数这么写不严谨，但是也就这样了）应用 chain rule。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} + ext{Exp}(alpha m{ au}) {partial {}^{C0}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} label{eq:wrong} \ {partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C1}mathbf{R}_W} \ {partial {}^{Ci}mathbf{R}_W over partial alpha} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial alpha} end{align}]

以上的公式 (( ef{eq:wrong})) 是错误的，但是后面按照这个公式计算出来的 ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W}) 结果 (( ef{eq:wrong1})) 却是能够通过代码验证（以下4.1，得到的结果与数值方法的结果一致）。本来是想按照 chain rule 写，即 ((f(x)g(x))^{prime} = f(x)^{prime} g(x) + f(x)g(x)^{prime})，但是中间出了错，应该是写成如下。结果歪打正着，得到了可以通过代码验证的结果。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &= {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} {}^{C0}mathbf{R}_W + ext{Exp}(alpha m{ au}) {partial {}^{C0}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} end{align}]

为什么 (( ef{eq:wrong})) 是错误的？因为导数的乘法法则 ((f(x)g(x))^{prime} = f(x)^{prime} g(x) + f(x)g(x)^{prime}) 是在实数域上成立，并没有在 Manifold 上成立。按照 Manifold 上的导数定义 (44*)，应当如下计算，然而这种计算是没有结果的。

[egin{align} h(mathcal{X}) &= f(mathcal{X}) circ g(mathcal{X}) \ {partial h(mathcal{X}) over partial mathcal{X}} &=^{(44^*)} lim_{deltam{ au} o mathbf{0}} { ext{Log}[h(deltam{ au}oplusmathcal{X})circ h(mathcal{X})^{-1}] over deltam{ au}} \ &= lim_{deltam{ au} o mathbf{0}} { ext{Log}[(f(deltam{ au}oplusmathcal{X}) circ g(deltam{ au}oplusmathcal{X}))circ (f(mathcal{X}) circ g(mathcal{X}))^{-1}] over deltam{ au}} \ &= lim_{deltam{ au} o mathbf{0}} { ext{Log}[f(deltam{ au}oplusmathcal{X}) circ g(deltam{ au}oplusmathcal{X}) circ g(mathcal{X})^{-1} circ f(mathcal{X})^{-1}] over deltam{ au}} \ &=^{(45^*)(27^*)} lim_{deltam{ au} o mathbf{0}} { ext{Log}[ ext{Exp}({partial f(mathcal{X}) over partial mathcal{X}} deltam{ au})f(mathcal{X}) ext{Exp}({partial g(mathcal{X}) over partial mathcal{X}} deltam{ au})g(mathcal{X}) g(mathcal{X})^{-1} f(mathcal{X})^{-1}] over deltam{ au}} \ &=^{(20^*)(31^*)} lim_{deltam{ au} o mathbf{0}} { ext{Log}[ ext{Exp}({partial f(mathcal{X}) over partial mathcal{X}} deltam{ au}) ext{Exp}(mathbf{Ad}_{f(mathcal{X})}{partial g(mathcal{X}) over partial mathcal{X}} deltam{ au})] over deltam{ au}} \ end{align}]

那么正确的写法是什么？将 (( ef{eq:origin})) 用另一种方式写。

[egin{align} {}^{Ci}mathbf{R}_W &= {}^{Ci}mathbf{R}_{C1} {}^{C1}mathbf{R}_W \ &= {}^{C0}mathbf{R}_{C1}^{1-alpha} {}^{C1}mathbf{R}_W \ &= ({}^{C0}mathbf{R}_W {}^{C1}mathbf{R}_W^T)^{1-alpha} {}^{C1}mathbf{R}_W \ &= ext{Exp}((1-alpha) ext{Log}({}^{C0}mathbf{R}_W {}^{C1}mathbf{R}_W^T)) {}^{C1}mathbf{R}_W \ &= ext{Exp}((alpha - 1) m{ au}) {}^{C1}mathbf{R}_W \ -m{ au}&= ext{Log}({}^{C0}mathbf{R}_W {}^{C1}mathbf{R}_W^T) end{align}]

这种方式避免 ({}^{C0}mathbf{R}_W) 出现在最外层矩阵乘法的两项中。于是可以方便使用 chain rule。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}((alpha - 1) m{ au})} {partial ext{Exp}((alpha - 1) m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} label{eq:right0} end{align}]

之所以会出现 (( ef{eq:wrong})) 从 product rule 上思考是错误的，但是得到的结果能够通过代码检查，原因是实际上使用的是 “多元复合函数求导法则”。认为 ({}^{C0}mathbf{R}_W) 对 ({}^{Ci}mathbf{R}_W) 的影响有两条路径，一条路径是通过 ({}^{Ci}mathbf{R}_W) 公式最外层矩阵乘法中的 ({}^{C0}mathbf{R}_W)，一条路径是通过 (m{ au})。可以写作。（微积分常识需要巩固。）

[egin{align} {D {}^{Ci}mathbf{R}_W over D {}^{C0}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} + {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} \ &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} + ext{Exp}(alpha m{ au}) end{align}]

3.1. ({partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W})

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} &= {partial ext{Exp}(alpha m{ au}) {}^{C0}mathbf{R}_W over partial ext{Exp}(alpha m{ au})}\ &=^{(44^*)} lim_{deltam{ au} omathbf{0}} { ext{Log}[( ext{Exp}(deltam{ au}) ext{Exp}(alpha m{ au}) {}^{C0}mathbf{R}_W)( ext{Exp}(alpha m{ au}) {}^{C0}mathbf{R}_W)^{-1}]over deltam{ au}} \ &= lim_{deltam{ au} omathbf{0}} { ext{Log}[( ext{Exp}(deltam{ au}))]over deltam{ au}} \ &= mathbf{I} \ {partial ext{Exp}(alpha m{ au}) over partial m{ au}} &=^{(44^*)} lim_{deltam{ au} omathbf{0}} { ext{Log}[ ext{Exp}(alpha (m{ au} + deltam{ au})) ext{Exp}(alpha m{ au})^{-1}]over deltam{ au}} \ &=^{(72^*)} lim_{deltam{ au} omathbf{0}} { ext{Log}[ ext{Exp}(mathbf{J}_l(alpham{ au})alphadeltam{ au}) ext{Exp}(alpha m{ au}) ext{Exp}(alpha m{ au})^{-1}]over deltam{ au}} \ &= alphamathbf{J}_l(alpham{ au}) \ {partial m{ au} over partial {}^{C0}mathbf{R}_W} &= {partial ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T) over partial {}^{C0}mathbf{R}_W} \ &=^{(44^*)} lim_{deltam{ au} omathbf{0}} { ext{Log}({}^{C1}mathbf{R}_W ( ext{Exp}(deltam{ au}){}^{C0}mathbf{R}_W)^T) - ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)over deltam{ au}} \ &= lim_{deltam{ au} omathbf{0}} { ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T ext{Exp}(-deltam{ au})) - ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)over deltam{ au}} \ &=^{(70^*)} lim_{deltam{ au} omathbf{0}} {m{ au} + mathbf{J}_r^{-1}(m{ au})(-deltam{ au}) - m{ au}over deltam{ au}} \ &= -mathbf{J}_r^{-1}(m{ au}) \ {partial {}^{C0}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &=^{(44^*)} mathbf{I} end{align}]

注意以上以公式 (44*) 为基础，要注意 (44*) 中的 (oplus, ominus) 不完全由 (27*) 与 (28*) 定义。如 ({partial ext{Exp}(alpha m{ au}) over partial m{ au}}) 的 (oplus) 由分数线下方的 (m{ au} in mathbb{R}^3) 定义，是 vector space 的 (oplus)，写作 (+)。如 ({partial m{ au} over partial {}^{C0}mathbf{R}_W}) 的 (ominus) 由分数线上方的 (m{ au} in mathbb{R}^3) 定义，是 vector space 的 (ominus)，写作 (-)。

于是有（按照错误的 (( ef{eq:wrong}))）。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} + ext{Exp}(alpha m{ au}) {partial {}^{C0}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} \ &= mathbf{I} alphamathbf{J}_l(alpham{ au}) (-mathbf{J}_r^{-1}(m{ au})) + ext{Exp}(alpha m{ au}) mathbf{I} \ &= - alphamathbf{J}_l(alpham{ au}) mathbf{J}_r^{-1}(m{ au}) + ext{Exp}(alpha m{ au}) label{eq:wrong1} end{align}]

同理，按照正确的 (( ef{eq:right0}))，得到的结果如下。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C0}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}((alpha - 1) m{ au})} {partial ext{Exp}((alpha - 1) m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C0}mathbf{R}_W} \ &= mathbf{I} (alpha - 1)mathbf{J}_l((alpha - 1)m{ au}) (-mathbf{J}_r^{-1}(m{ au})) \ &= -(alpha - 1)mathbf{J}_l((alpha - 1)m{ au}) mathbf{J}_r^{-1}(m{ au}) label{eq:right1} end{align}]

3.2. ({partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W})

[egin{align} {partial m{ au} over partial {}^{C1}mathbf{R}_W} &= {partial ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T) over partial {}^{C0}mathbf{R}_W} \ &=^{(44^*)} lim_{deltam{ au} omathbf{0}} { ext{Log}( ext{Exp}(deltam{ au}){}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T) - ext{Log}({}^{C1}mathbf{R}_W {}^{C0}mathbf{R}_W^T)over deltam{ au}} \ &=^{(74^*)} lim_{deltam{ au} omathbf{0}} {m{ au} + mathbf{J}_l^{-1}(m{ au})deltam{ au} - m{ au}over deltam{ au}} \ &= mathbf{J}_l^{-1}(m{ au}) end{align}]

于是有。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial {}^{C1}mathbf{R}_W} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial m{ au}} {partial m{ au} over partial {}^{C1}mathbf{R}_W} \ &= mathbf{I} alphamathbf{J}_l(alpham{ au}) mathbf{J}_l^{-1}(m{ au}) \ &= alphamathbf{J}_l(alpham{ au}) mathbf{J}_l^{-1}(m{ au}) end{align}]

3.3. ({partial {}^{Ci}mathbf{R}_W over partial alpha})

[egin{align} {partial ext{Exp}(alpha m{ au}) over partial alpha} &=^{(44^*)} lim_{deltaalpha omathbf{0}} { ext{Log}[ ext{Exp}((alpha + deltaalpha) m{ au}) ext{Exp}(alpha m{ au})^{-1}] ext{}over deltaalpha} \ &=^{(72^*)} lim_{deltaalpha omathbf{0}} { ext{Log}[ ext{Exp}(mathbf{J}_l(alpham{ au})deltaalpham{ au}) ext{Exp}(alpha m{ au}) ext{Exp}(alpha m{ au})^{-1}] ext{}over deltaalpha} \ &= mathbf{J}_l(alpham{ au})m{ au} end{align}]

于是有。

[egin{align} {partial {}^{Ci}mathbf{R}_W over partial alpha} &= {partial {}^{Ci}mathbf{R}_W over partial ext{Exp}(alpha m{ au})} {partial ext{Exp}(alpha m{ au}) over partial alpha} \ &= mathbf{I} mathbf{J}_l(alpham{ au})m{ au} \ &= mathbf{J}_l(alpham{ au})m{ au} end{align}]

4. 代码验证

用 ceres 的 AutoDiff 与 NumericDiff 工具验证。Analytic 的结果就是公式推导的结果。

比较以下两段代码的 if(_jacobians[0] != nullptr)。

4.1. 按照 (( ef{eq:wrong1})) 的错误示范

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/internal/autodiff.h>
#include <ceres/internal/numeric_diff.h>
#include <cstdlib>
#include <ctime>

/// use numeric diff and auto diff to check my analytic diff.

class QuaternionCostFunctor {
public:
  QuaternionCostFunctor(const Eigen::Quaterniond &_ci_q_w) : ci_q_w_{_ci_q_w} {}

  template <typename T>
  bool operator()(const T *const _c0_q_w, const T *const _c1_q_w,
                  const T *const _alpha, T *_e) const {
    const Eigen::Quaternion<T> c0_q_w(_c0_q_w);
    const Eigen::Quaternion<T> c1_q_w(_c1_q_w);

    const Eigen::Quaternion<T> delta_qua = c1_q_w * c0_q_w.inverse();
    Eigen::AngleAxis<T> delta_aa(delta_qua);
    delta_aa.angle() *= _alpha[0];

    const Eigen::Quaternion<T> ci_q_w_p =
        Eigen::Quaternion<T>(delta_aa) * c0_q_w;

    const Eigen::Quaternion<T> ci_q_w(
        static_cast<T>(ci_q_w_.w()), static_cast<T>(ci_q_w_.x()),
        static_cast<T>(ci_q_w_.y()), static_cast<T>(ci_q_w_.z()));

    const Eigen::Quaternion<T> e_q = ci_q_w_p * ci_q_w.inverse();

    const Eigen::AngleAxis<T> e_aa(e_q);

    Eigen::Map<Eigen::Matrix<T, 3, 1>> e(_e);

    e = e_aa.axis() * e_aa.angle();

    return true;
  }

  void evaluateAnalytically(const double *const _c0_q_w,
                            const double *const _c1_q_w,
                            const double *const _alpha, double *_e,
                            double **_jacobians) const {
    const Eigen::Quaternion<double> c0_q_w(_c0_q_w);
    const Eigen::Quaternion<double> c1_q_w(_c1_q_w);

    const Eigen::Quaternion<double> delta_qua = c1_q_w * c0_q_w.inverse();
    Eigen::AngleAxis<double> delta_aa(delta_qua);
    delta_aa.angle() *= _alpha[0];

    const Eigen::Quaternion<double> ci_q_w_p =
        Eigen::Quaternion<double>(delta_aa) * c0_q_w;

    const Eigen::Quaternion<double> e_q = ci_q_w_p * ci_q_w_.inverse();

    const Eigen::AngleAxis<double> e_aa(e_q);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> e(_e);

    e = e_aa.axis() * e_aa.angle();

    if (_jacobians != nullptr) {
      const Eigen::Vector3d tau_axis = delta_aa.axis();
      const double tau_angle = delta_aa.angle() / _alpha[0];
      if (_jacobians[0] != nullptr) { /// c0_q_w
        Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
            _jacobians[0]);
        J.setZero();
        J.block(0, 0, 3, 3) = -_alpha[0] *
                                  J_l(_alpha[0] * tau_angle, tau_axis) *
                                  J_r_inv(tau_angle, tau_axis) +
                              delta_aa.toRotationMatrix();
      }
      if (_jacobians[1] != nullptr) { /// c1_q_w
        Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
            _jacobians[1]);
        J.setZero();
        J.block(0, 0, 3, 3) = _alpha[0] * J_l(_alpha[0] * tau_angle, tau_axis) *
                              J_l_inv(tau_angle, tau_axis);
      }
      if (_jacobians[2] != nullptr) { /// alpha
        Eigen::Map<Eigen::Matrix<double, 3, 1>> J(_jacobians[2]);
        J = J_l(_alpha[0] * tau_angle, tau_axis) * tau_angle * tau_axis;
      }
    }
  }

  static bool GlobalToLocal(const double *x, double *jacobian) {
    const double qw = x[3];
    const double qx = x[0];
    const double qy = x[1];
    const double qz = x[2];
    jacobian[0] = qw, jacobian[1] = qz, jacobian[2] = -qy;
    jacobian[3] = -qz, jacobian[4] = qw, jacobian[5] = qx;
    jacobian[6] = qy, jacobian[7] = -qx, jacobian[8] = qw;
    jacobian[9] = -qx, jacobian[10] = -qy, jacobian[11] = -qz;
    return true;
  }

  static inline Eigen::Matrix3d skew(const Eigen::Vector3d &_v) {
    Eigen::Matrix3d res;
    res.setZero();
    res(0, 1) = -_v[2], res(0, 2) = _v[1], res(1, 2) = -_v[0];
    res(1, 0) = _v[2], res(2, 0) = -_v[1], res(2, 1) = _v[0];
    return res;
  }

  static Eigen::Matrix3d J_l(const double _angle,
                             const Eigen::Vector3d &_axis) {
    /// (145)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() +
        (1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
        (_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_l_inv(const double _angle,
                                 const Eigen::Vector3d &_axis) {
    /// (146)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() - 0.5 * skew(_angle * _axis) +
        (1 / (_angle * _angle) -
         (1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_r(const double _angle,
                             const Eigen::Vector3d &_axis) {
    // return J_l(_angle, _axis).transpose();
    /// (143)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() -
        (1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
        (_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_r_inv(const double _angle,
                                 const Eigen::Vector3d &_axis) {
    // return J_l_inv(_angle, _axis).transpose();
    /// (144)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() + 0.5 * skew(_angle * _axis) +
        (1 / (_angle * _angle) -
         (1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

private:
  const Eigen::Quaterniond ci_q_w_;
};

Eigen::Quaterniond getRandomQuaternion() {
  const double range = 1.;

  Eigen::Vector3d axis(std::rand() / double(RAND_MAX) * 2 * range + (-range),
                       std::rand() / double(RAND_MAX) * 2 * range + (-range),
                       std::rand() / double(RAND_MAX) * 2 * range + (-range));
  axis.normalize();
  const double angle = std::rand() / double(RAND_MAX) * 2 * M_PI;
  Eigen::AngleAxisd aa(angle, axis);

  return Eigen::Quaterniond(aa);
}

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

  std::srand(std::time(NULL));

  std::srand(0);

  Eigen::Quaterniond c0_q_w = getRandomQuaternion();
  Eigen::Quaterniond c1_q_w = getRandomQuaternion();
  double alpha = std::rand() / double(RAND_MAX);

  std::cout << "c0_R_w:
" << c0_q_w.toRotationMatrix() << std::endl;

  std::cout << "c1_R_w:
" << c1_q_w.toRotationMatrix() << std::endl;

  std::cout << "alpha:
" << alpha << std::endl;

  const Eigen::Quaterniond delta_qua = c1_q_w * c0_q_w.inverse();
  Eigen::AngleAxisd delta_aa(delta_qua);
  delta_aa.angle() *= alpha;

  const Eigen::Quaterniond ci_q_w = Eigen::Quaterniond(delta_aa) * c0_q_w;

  QuaternionCostFunctor functor(ci_q_w);

  double residuals[3];
  double *parameters[3] = {c0_q_w.coeffs().data(), c1_q_w.coeffs().data(),
                           &alpha};
  double **jacobians = new double *[3];
  for (int i = 0; i < 2; ++i)
    jacobians[i] = new double[12];
  jacobians[2] = new double[3];

  {
    ceres::internal::AutoDiff<QuaternionCostFunctor, double, 4, 4,
                              1>::Differentiate(functor, parameters,
                                                3, /// residual num
                                                residuals, jacobians);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
    QuaternionCostFunctor::GlobalToLocal(parameters[0],
                                         global_to_local_0.data());

    std::cout << "autodiff jacobian_0:
"
              << 0.5 * jacobian_0 * global_to_local_0 << std::endl;

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
    QuaternionCostFunctor::GlobalToLocal(parameters[1],
                                         global_to_local_1.data());

    std::cout << "autodiff jacobian_1:
"
              << 0.5 * jacobian_1 * global_to_local_1 << std::endl;

    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "autodiff jacobian_2:
" << jacobian_2 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 0,
        4>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              0, /// block index
                                              4, /// block size
                                              parameters, jacobians[0]);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
    QuaternionCostFunctor::GlobalToLocal(parameters[0],
                                         global_to_local_0.data());

    std::cout << "numdiff jacobian_0:
"
              << 0.5 * jacobian_0 * global_to_local_0 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 1,
        4>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              1, /// block index
                                              4, /// block size
                                              parameters, jacobians[1]);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
    QuaternionCostFunctor::GlobalToLocal(parameters[1],
                                         global_to_local_1.data());

    std::cout << "numdiff jacobian_1:
"
              << 0.5 * jacobian_1 * global_to_local_1 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 2,
        1>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              2, /// block index
                                              1, /// block size
                                              parameters, jacobians[2]);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "numdiff jacobian_2:
" << jacobian_2 << std::endl;
  }

  {
    functor.evaluateAnalytically(parameters[0], parameters[1], parameters[2],
                                 residuals, jacobians);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);
    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);
    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "analytic jacobian_0:
"
              << jacobian_0.block(0, 0, 3, 3) << std::endl;
    std::cout << "analytic jacobian_1:
"
              << jacobian_1.block(0, 0, 3, 3) << std::endl;
    std::cout << "analytic jacobian_2:
" << jacobian_2 << std::endl;
  }

  return 0;
}

4.2. 按照 (( ef{eq:right1})) 的正确写法

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/internal/autodiff.h>
#include <ceres/internal/numeric_diff.h>
#include <cstdlib>
#include <ctime>

/// use numeric diff and auto diff to check my analytic diff.

class QuaternionCostFunctor {
public:
  QuaternionCostFunctor(const Eigen::Quaterniond &_ci_q_w) : ci_q_w_{_ci_q_w} {}

  template <typename T>
  bool operator()(const T *const _c0_q_w, const T *const _c1_q_w,
                  const T *const _alpha, T *_e) const {
    const Eigen::Quaternion<T> c0_q_w(_c0_q_w);
    const Eigen::Quaternion<T> c1_q_w(_c1_q_w);

    const Eigen::Quaternion<T> delta_qua = c1_q_w * c0_q_w.inverse();
    Eigen::AngleAxis<T> delta_aa(delta_qua);
    delta_aa.angle() *= _alpha[0];

    const Eigen::Quaternion<T> ci_q_w_p =
        Eigen::Quaternion<T>(delta_aa) * c0_q_w;

    const Eigen::Quaternion<T> ci_q_w(
        static_cast<T>(ci_q_w_.w()), static_cast<T>(ci_q_w_.x()),
        static_cast<T>(ci_q_w_.y()), static_cast<T>(ci_q_w_.z()));

    const Eigen::Quaternion<T> e_q = ci_q_w_p * ci_q_w.inverse();

    const Eigen::AngleAxis<T> e_aa(e_q);

    Eigen::Map<Eigen::Matrix<T, 3, 1>> e(_e);

    e = e_aa.axis() * e_aa.angle();

    return true;
  }

  void evaluateAnalytically(const double *const _c0_q_w,
                            const double *const _c1_q_w,
                            const double *const _alpha, double *_e,
                            double **_jacobians) const {
    const Eigen::Quaternion<double> c0_q_w(_c0_q_w);
    const Eigen::Quaternion<double> c1_q_w(_c1_q_w);

    const Eigen::Quaternion<double> delta_qua = c1_q_w * c0_q_w.inverse();
    Eigen::AngleAxis<double> delta_aa(delta_qua);
    delta_aa.angle() *= _alpha[0];

    const Eigen::Quaternion<double> ci_q_w_p =
        Eigen::Quaternion<double>(delta_aa) * c0_q_w;

    const Eigen::Quaternion<double> e_q = ci_q_w_p * ci_q_w_.inverse();

    const Eigen::AngleAxis<double> e_aa(e_q);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> e(_e);

    e = e_aa.axis() * e_aa.angle();

    if (_jacobians != nullptr) {
      const Eigen::Vector3d tau_axis = delta_aa.axis();
      const double tau_angle = delta_aa.angle() / _alpha[0];
      if (_jacobians[0] != nullptr) { /// c0_q_w
        Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
            _jacobians[0]);
        J.setZero();
        J.block(0, 0, 3, 3) = -(_alpha[0] - 1) *
                              J_l((_alpha[0] - 1) * tau_angle, tau_axis) *
                              J_r_inv(tau_angle, tau_axis);
      }
      if (_jacobians[1] != nullptr) { /// c1_q_w
        Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> J(
            _jacobians[1]);
        J.setZero();
        J.block(0, 0, 3, 3) = _alpha[0] * J_l(_alpha[0] * tau_angle, tau_axis) *
                              J_l_inv(tau_angle, tau_axis);
      }
      if (_jacobians[2] != nullptr) { /// alpha
        Eigen::Map<Eigen::Matrix<double, 3, 1>> J(_jacobians[2]);
        J = J_l(_alpha[0] * tau_angle, tau_axis) * tau_angle * tau_axis;
      }
    }
  }

  static bool GlobalToLocal(const double *x, double *jacobian) {
    const double qw = x[3];
    const double qx = x[0];
    const double qy = x[1];
    const double qz = x[2];
    jacobian[0] = qw, jacobian[1] = qz, jacobian[2] = -qy;
    jacobian[3] = -qz, jacobian[4] = qw, jacobian[5] = qx;
    jacobian[6] = qy, jacobian[7] = -qx, jacobian[8] = qw;
    jacobian[9] = -qx, jacobian[10] = -qy, jacobian[11] = -qz;
    return true;
  }

  static inline Eigen::Matrix3d skew(const Eigen::Vector3d &_v) {
    Eigen::Matrix3d res;
    res.setZero();
    res(0, 1) = -_v[2], res(0, 2) = _v[1], res(1, 2) = -_v[0];
    res(1, 0) = _v[2], res(2, 0) = -_v[1], res(2, 1) = _v[0];
    return res;
  }

  static Eigen::Matrix3d J_l(const double _angle,
                             const Eigen::Vector3d &_axis) {
    /// (145)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() +
        (1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
        (_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_l_inv(const double _angle,
                                 const Eigen::Vector3d &_axis) {
    /// (146)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() - 0.5 * skew(_angle * _axis) +
        (1 / (_angle * _angle) -
         (1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_r(const double _angle,
                             const Eigen::Vector3d &_axis) {
    return J_l(_angle, _axis).transpose();
    /// (143)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() -
        (1 - std::cos(_angle)) / (_angle * _angle) * skew(_angle * _axis) +
        (_angle - std::sin(_angle)) / (_angle * _angle * _angle) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

  static Eigen::Matrix3d J_r_inv(const double _angle,
                                 const Eigen::Vector3d &_axis) {
    return J_l_inv(_angle, _axis).transpose();
    /// (144)
    Eigen::Matrix3d res =
        Eigen::Matrix3d::Identity() + 0.5 * skew(_angle * _axis) +
        (1 / (_angle * _angle) -
         (1 + std::cos(_angle)) / (2 * _angle * std::sin(_angle))) *
            skew(_angle * _axis) * skew(_angle * _axis);
    return res;
  }

private:
  const Eigen::Quaterniond ci_q_w_;
};

Eigen::Quaterniond getRandomQuaternion() {
  const double range = 1.;

  Eigen::Vector3d axis(std::rand() / double(RAND_MAX) * 2 * range + (-range),
                       std::rand() / double(RAND_MAX) * 2 * range + (-range),
                       std::rand() / double(RAND_MAX) * 2 * range + (-range));
  axis.normalize();
  const double angle = std::rand() / double(RAND_MAX) * 2 * M_PI;
  Eigen::AngleAxisd aa(angle, axis);

  return Eigen::Quaterniond(aa);
}

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

  std::srand(std::time(NULL));

  std::srand(0);

  Eigen::Quaterniond c0_q_w = getRandomQuaternion();
  Eigen::Quaterniond c1_q_w = getRandomQuaternion();
  double alpha = std::rand() / double(RAND_MAX);

  std::cout << "c0_R_w:
" << c0_q_w.toRotationMatrix() << std::endl;

  std::cout << "c1_R_w:
" << c1_q_w.toRotationMatrix() << std::endl;

  std::cout << "alpha:
" << alpha << std::endl;

  const Eigen::Quaterniond delta_qua = c1_q_w * c0_q_w.inverse();
  Eigen::AngleAxisd delta_aa(delta_qua);
  delta_aa.angle() *= alpha;

  const Eigen::Quaterniond ci_q_w = Eigen::Quaterniond(delta_aa) * c0_q_w;

  std::cout << "ci_R_w:
" << ci_q_w.toRotationMatrix() << std::endl;

  QuaternionCostFunctor functor(ci_q_w);

  double residuals[3];
  double *parameters[3] = {c0_q_w.coeffs().data(), c1_q_w.coeffs().data(),
                           &alpha};
  double **jacobians = new double *[3];
  for (int i = 0; i < 2; ++i)
    jacobians[i] = new double[12];
  jacobians[2] = new double[3];

  {
    ceres::internal::AutoDiff<QuaternionCostFunctor, double, 4, 4,
                              1>::Differentiate(functor, parameters,
                                                3, /// residual num
                                                residuals, jacobians);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
    QuaternionCostFunctor::GlobalToLocal(parameters[0],
                                         global_to_local_0.data());

    std::cout << "autodiff jacobian_0:
"
              << 0.5 * jacobian_0 * global_to_local_0 << std::endl;

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
    QuaternionCostFunctor::GlobalToLocal(parameters[1],
                                         global_to_local_1.data());

    std::cout << "autodiff jacobian_1:
"
              << 0.5 * jacobian_1 * global_to_local_1 << std::endl;

    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "autodiff jacobian_2:
" << jacobian_2 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 0,
        4>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              0, /// block index
                                              4, /// block size
                                              parameters, jacobians[0]);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_0;
    QuaternionCostFunctor::GlobalToLocal(parameters[0],
                                         global_to_local_0.data());

    std::cout << "numdiff jacobian_0:
"
              << 0.5 * jacobian_0 * global_to_local_0 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 1,
        4>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              1, /// block index
                                              4, /// block size
                                              parameters, jacobians[1]);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);

    Eigen::Matrix<double, 4, 3, Eigen::RowMajor> global_to_local_1;
    QuaternionCostFunctor::GlobalToLocal(parameters[1],
                                         global_to_local_1.data());

    std::cout << "numdiff jacobian_1:
"
              << 0.5 * jacobian_1 * global_to_local_1 << std::endl;
  }

  {
    ceres::internal::NumericDiff<
        QuaternionCostFunctor, ceres::NumericDiffMethodType::CENTRAL, 3, 4, 4,
        1, 0, 0, 0, 0, 0, 0, 0, 2,
        1>::EvaluateJacobianForParameterBlock(&functor, residuals,
                                              ceres::NumericDiffOptions(),
                                              3, /// residual num
                                              2, /// block index
                                              1, /// block size
                                              parameters, jacobians[2]);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "numdiff jacobian_2:
" << jacobian_2 << std::endl;
  }

  {
    functor.evaluateAnalytically(parameters[0], parameters[1], parameters[2],
                                 residuals, jacobians);

    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_0(
        jacobians[0]);
    Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian_1(
        jacobians[1]);
    Eigen::Map<Eigen::Matrix<double, 3, 1>> jacobian_2(jacobians[2]);

    std::cout << "analytic jacobian_0:
"
              << jacobian_0.block(0, 0, 3, 3) << std::endl;
    std::cout << "analytic jacobian_1:
"
              << jacobian_1.block(0, 0, 3, 3) << std::endl;
    std::cout << "analytic jacobian_2:
" << jacobian_2 << std::endl;
  }

  return 0;
}

Reference

[1] Sola, Joan, Jeremie Deray, and Dinesh Atchuthan. "A micro Lie theory for state estimation in robotics." arXiv preprint arXiv:1812.01537 (2018).

[2] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).