SLERP Jacobians (left perturb)
看了 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), \alpha\in[0,1]\)。
\[\begin{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 \\
&= \text{Exp}(\alpha \text{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, ...
令 \(\bm{\tau}=\text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T)\)，则。
\[\begin{align}
{}^{Ci}\mathbf{R}_W &= \text{Exp}(\alpha \bm{\tau}) {}^{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。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} + \text{Exp}(\alpha \bm{\tau}) {\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 \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C1}\mathbf{R}_W} \\
{\partial {}^{Ci}\mathbf{R}_W \over \partial \alpha} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \alpha}
\end{align}\]
以上的公式 (\(\ref{eq:wrong}\)) 是错误的，但是后面按照这个公式计算出来的 \({\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W}\) 结果 (\(\ref{eq:wrong1}\)) 却是能够通过代码验证（以下4.1，得到的结果与数值方法的结果一致）。本来是想按照 chain rule 写，即 \((f(x)g(x))^{\prime} = f(x)^{\prime} g(x) + f(x)g(x)^{\prime}\)，但是中间出了错，应该是写成如下。结果歪打正着，得到了可以通过代码验证的结果。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &= {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} {}^{C0}\mathbf{R}_W + \text{Exp}(\alpha \bm{\tau}) {\partial {}^{C0}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W}
\end{align}\]
为什么 (\(\ref{eq:wrong}\)) 是错误的？因为导数的乘法法则 \((f(x)g(x))^{\prime} = f(x)^{\prime} g(x) + f(x)g(x)^{\prime}\) 是在实数域上成立，并没有在 Manifold 上成立。按照 Manifold 上的导数定义 (44*)，应当如下计算，然而这种计算是没有结果的。
\[\begin{align}
h(\mathcal{X}) &= f(\mathcal{X}) \circ g(\mathcal{X}) \\
{\partial h(\mathcal{X}) \over \partial \mathcal{X}} &=^{(44^*)} \lim_{\delta\bm{\tau} \to \mathbf{0}} {\text{Log}[h(\delta\bm{\tau}\oplus\mathcal{X})\circ h(\mathcal{X})^{-1}] \over \delta\bm{\tau}} \\
&= \lim_{\delta\bm{\tau} \to \mathbf{0}} {\text{Log}[(f(\delta\bm{\tau}\oplus\mathcal{X}) \circ g(\delta\bm{\tau}\oplus\mathcal{X}))\circ (f(\mathcal{X}) \circ g(\mathcal{X}))^{-1}] \over \delta\bm{\tau}} \\
&= \lim_{\delta\bm{\tau} \to \mathbf{0}} {\text{Log}[f(\delta\bm{\tau}\oplus\mathcal{X}) \circ g(\delta\bm{\tau}\oplus\mathcal{X}) \circ g(\mathcal{X})^{-1} \circ f(\mathcal{X})^{-1}] \over \delta\bm{\tau}} \\
&=^{(45^*)(27^*)} \lim_{\delta\bm{\tau} \to \mathbf{0}} {\text{Log}[\text{Exp}({\partial f(\mathcal{X}) \over \partial \mathcal{X}} \delta\bm{\tau})f(\mathcal{X}) \text{Exp}({\partial g(\mathcal{X}) \over \partial \mathcal{X}} \delta\bm{\tau})g(\mathcal{X}) g(\mathcal{X})^{-1} f(\mathcal{X})^{-1}] \over \delta\bm{\tau}} \\
&=^{(20^*)(31^*)} \lim_{\delta\bm{\tau} \to \mathbf{0}} {\text{Log}[\text{Exp}({\partial f(\mathcal{X}) \over \partial \mathcal{X}} \delta\bm{\tau}) \text{Exp}(\mathbf{Ad}_{f(\mathcal{X})}{\partial g(\mathcal{X}) \over \partial \mathcal{X}} \delta\bm{\tau})] \over \delta\bm{\tau}} \\
\end{align}\]
那么正确的写法是什么？将 (\(\ref{eq:origin}\)) 用另一种方式写。
\[\begin{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 \\
&= \text{Exp}((1-\alpha) \text{Log}({}^{C0}\mathbf{R}_W {}^{C1}\mathbf{R}_W^T)) {}^{C1}\mathbf{R}_W \\
&= \text{Exp}((\alpha - 1) \bm{\tau}) {}^{C1}\mathbf{R}_W \\
-\bm{\tau}&=\text{Log}({}^{C0}\mathbf{R}_W {}^{C1}\mathbf{R}_W^T)
\end{align}\]
这种方式避免 \({}^{C0}\mathbf{R}_W\) 出现在最外层矩阵乘法的两项中。于是可以方便使用 chain rule。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}((\alpha - 1) \bm{\tau})} {\partial \text{Exp}((\alpha - 1) \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} \label{eq:right0}
\end{align}\]
之所以会出现 (\(\ref{eq:wrong}\)) 从 product rule 上思考是错误的，但是得到的结果能够通过代码检查，原因是实际上使用的是 “多元复合函数求导法则”。认为 \({}^{C0}\mathbf{R}_W\) 对 \({}^{Ci}\mathbf{R}_W\) 的影响有两条路径，一条路径是通过 \({}^{Ci}\mathbf{R}_W\) 公式最外层矩阵乘法中的 \({}^{C0}\mathbf{R}_W\)，一条路径是通过 \(\bm{\tau}\)。可以写作。（微积分常识需要巩固。）
\[\begin{align}
{D {}^{Ci}\mathbf{R}_W \over D {}^{C0}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} + {\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} \\
&= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} + \text{Exp}(\alpha \bm{\tau})
\end{align}\]
3.1. \({\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W}\)
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} &= {\partial \text{Exp}(\alpha \bm{\tau}) {}^{C0}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})}\\
&=^{(44^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}[(\text{Exp}(\delta\bm{\tau})\text{Exp}(\alpha \bm{\tau}) {}^{C0}\mathbf{R}_W)(\text{Exp}(\alpha \bm{\tau}) {}^{C0}\mathbf{R}_W)^{-1}]\over \delta\bm{\tau}} \\
&= \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}[(\text{Exp}(\delta\bm{\tau}))]\over \delta\bm{\tau}} \\
&= \mathbf{I} \\
{\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} &=^{(44^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}[\text{Exp}(\alpha (\bm{\tau} + \delta\bm{\tau}))\text{Exp}(\alpha \bm{\tau})^{-1}]\over \delta\bm{\tau}} \\
&=^{(72^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}[\text{Exp}(\mathbf{J}_l(\alpha\bm{\tau})\alpha\delta\bm{\tau})\text{Exp}(\alpha \bm{\tau})\text{Exp}(\alpha \bm{\tau})^{-1}]\over \delta\bm{\tau}} \\
&= \alpha\mathbf{J}_l(\alpha\bm{\tau}) \\
{\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} &= {\partial \text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T) \over \partial {}^{C0}\mathbf{R}_W} \\
&=^{(44^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}({}^{C1}\mathbf{R}_W (\text{Exp}(\delta\bm{\tau}){}^{C0}\mathbf{R}_W)^T) - \text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T)\over \delta\bm{\tau}} \\
&= \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T\text{Exp}(-\delta\bm{\tau})) - \text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T)\over \delta\bm{\tau}} \\
&=^{(70^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\bm{\tau} + \mathbf{J}_r^{-1}(\bm{\tau})(-\delta\bm{\tau}) - \bm{\tau}\over \delta\bm{\tau}} \\
&= -\mathbf{J}_r^{-1}(\bm{\tau}) \\
{\partial {}^{C0}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &=^{(44^*)} \mathbf{I}
\end{align}\]
注意以上以公式 (44*) 为基础，要注意 (44*) 中的 \(\oplus, \ominus\) 不完全由 (27*) 与 (28*) 定义。如 \({\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}}\) 的 \(\oplus\) 由分数线下方的 \(\bm{\tau} \in \mathbb{R}^3\) 定义，是 vector space 的 \(\oplus\)，写作 \(+\)。如 \({\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W}\) 的 \(\ominus\) 由分数线上方的 \(\bm{\tau} \in \mathbb{R}^3\) 定义，是 vector space 的 \(\ominus\)，写作 \(-\)。
于是有（按照错误的 (\(\ref{eq:wrong}\))）。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} + \text{Exp}(\alpha \bm{\tau}) {\partial {}^{C0}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} \\
&= \mathbf{I} \alpha\mathbf{J}_l(\alpha\bm{\tau}) (-\mathbf{J}_r^{-1}(\bm{\tau})) + \text{Exp}(\alpha \bm{\tau}) \mathbf{I} \\
&= - \alpha\mathbf{J}_l(\alpha\bm{\tau}) \mathbf{J}_r^{-1}(\bm{\tau}) + \text{Exp}(\alpha \bm{\tau}) \label{eq:wrong1}
\end{align}\]
同理，按照正确的 (\(\ref{eq:right0}\))，得到的结果如下。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C0}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}((\alpha - 1) \bm{\tau})} {\partial \text{Exp}((\alpha - 1) \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C0}\mathbf{R}_W} \\
&= \mathbf{I} (\alpha - 1)\mathbf{J}_l((\alpha - 1)\bm{\tau}) (-\mathbf{J}_r^{-1}(\bm{\tau})) \\
&= -(\alpha - 1)\mathbf{J}_l((\alpha - 1)\bm{\tau}) \mathbf{J}_r^{-1}(\bm{\tau}) \label{eq:right1}
\end{align}\]
3.2. \({\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C1}\mathbf{R}_W}\)
\[\begin{align}
{\partial \bm{\tau} \over \partial {}^{C1}\mathbf{R}_W} &= {\partial \text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T) \over \partial {}^{C0}\mathbf{R}_W} \\
&=^{(44^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\text{Log}(\text{Exp}(\delta\bm{\tau}){}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T) - \text{Log}({}^{C1}\mathbf{R}_W {}^{C0}\mathbf{R}_W^T)\over \delta\bm{\tau}} \\
&=^{(74^*)} \lim_{\delta\bm{\tau}\to\mathbf{0}} {\bm{\tau} + \mathbf{J}_l^{-1}(\bm{\tau})\delta\bm{\tau} - \bm{\tau}\over \delta\bm{\tau}} \\
&= \mathbf{J}_l^{-1}(\bm{\tau})
\end{align}\]
于是有。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial {}^{C1}\mathbf{R}_W} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \bm{\tau}} {\partial \bm{\tau} \over \partial {}^{C1}\mathbf{R}_W} \\
&= \mathbf{I} \alpha\mathbf{J}_l(\alpha\bm{\tau}) \mathbf{J}_l^{-1}(\bm{\tau}) \\
&= \alpha\mathbf{J}_l(\alpha\bm{\tau}) \mathbf{J}_l^{-1}(\bm{\tau})
\end{align}\]
3.3. \({\partial {}^{Ci}\mathbf{R}_W \over \partial \alpha}\)
\[\begin{align}
{\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \alpha} &=^{(44^*)} \lim_{\delta\alpha\to\mathbf{0}} {\text{Log}[\text{Exp}((\alpha + \delta\alpha) \bm{\tau}) \text{Exp}(\alpha \bm{\tau})^{-1}]\text{}\over \delta\alpha} \\
&=^{(72^*)} \lim_{\delta\alpha\to\mathbf{0}} {\text{Log}[\text{Exp}(\mathbf{J}_l(\alpha\bm{\tau})\delta\alpha\bm{\tau}) \text{Exp}(\alpha \bm{\tau}) \text{Exp}(\alpha \bm{\tau})^{-1}]\text{}\over \delta\alpha} \\
&= \mathbf{J}_l(\alpha\bm{\tau})\bm{\tau}
\end{align}\]
于是有。
\[\begin{align}
{\partial {}^{Ci}\mathbf{R}_W \over \partial \alpha} &= {\partial {}^{Ci}\mathbf{R}_W \over \partial \text{Exp}(\alpha \bm{\tau})} {\partial \text{Exp}(\alpha \bm{\tau}) \over \partial \alpha} \\
&= \mathbf{I} \mathbf{J}_l(\alpha\bm{\tau})\bm{\tau} \\
&= \mathbf{J}_l(\alpha\bm{\tau})\bm{\tau}
\end{align}\]
4. 代码验证
用 ceres 的 AutoDiff 与 NumericDiff 工具验证。Analytic 的结果就是公式推导的结果。
比较以下两段代码的 if(_jacobians[0] != nullptr)。
4.1. 按照 (\(\ref{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:\n" << c0_q_w.toRotationMatrix() << std::endl;
std::cout << "c1_R_w:\n" << c1_q_w.toRotationMatrix() << std::endl;
std::cout << "alpha:\n" << 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:\n"
<< 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:\n"
<< 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:\n" << 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:\n"
<< 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:\n"
<< 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:\n" << 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:\n"
<< jacobian_0.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_1:\n"
<< jacobian_1.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_2:\n" << jacobian_2 << std::endl;
}
return 0;
}
4.2. 按照 (\(\ref{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:\n" << c0_q_w.toRotationMatrix() << std::endl;
std::cout << "c1_R_w:\n" << c1_q_w.toRotationMatrix() << std::endl;
std::cout << "alpha:\n" << 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:\n" << 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:\n"
<< 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:\n"
<< 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:\n" << 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:\n"
<< 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:\n"
<< 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:\n" << 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:\n"
<< jacobian_0.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_1:\n"
<< jacobian_1.block(0, 0, 3, 3) << std::endl;
std::cout << "analytic jacobian_2:\n" << jacobian_2 << std::endl;
}
return 0;
}
