zoukankan      html  css  js  c++  java
  • 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).

  • 相关阅读:
    CTF_论剑场-web26
    Bugku-never give up
    Bugku-你必须让他停下来
    Bugku-域名解析
    bugku-web3
    请允许我成为你的夏季——shiro、jdbcInsertall
    HTTP请求/响应报文结构
    SQL常用命令
    dialogs打开对话框选定文件夹,getopenfilename获取文件名
    fso文件夹操作用法实操
  • 原文地址:https://www.cnblogs.com/JingeTU/p/13526943.html
Copyright © 2011-2022 走看看