B 样条曲线用于生成光滑、多阶可导的曲线。
Kalibr 使用 B 样条曲线进行相机与 IMU 的时间对齐。
本文旨在通过对角速度的理解，理解如何将 6 维的 SE(3) 轨迹输入到 B 样条中，利用 B 样条对轨迹进行求导，输出轨迹上任何一点处的角速度。本文不对 B 样条曲线进行介绍，只将 B 样条曲线当做一个求导的黑盒。
角速度的理解请参考 Rotation Kinematics，本文是这篇文章的后续。
1. Kalibr 对 B 样条的使用
Kalibr 此行代码 构建 6*1 向量的数组，之后将此数组输入到 B 样条中，构建 6 维的 B 样条曲线。从 B 样条曲线取出角速度，求角速度的 norm，此 norm 应与在同一刚体上的 imu 测量得到的角速度 norm 相同，从而对齐 imu 与 camera 的时间。
Kalibr 构建 6*1 向量的数组代码如下：
curve = np.matrix([ pose.transformationToCurveValue( np.dot(obs.T_t_c().T(), T_c_b) ) for obs in self.targetObservations]).T此处的 T() 函数是指 Transformation，而不是 Transpose。于是可以知道输入的 Transformation 方向是 T_t_c * T_c_b = T_t_b，这个 Transformation 是将 body 坐标系（imu 坐标系）下的坐标转化为 target 坐标系（标定板坐标系）下的坐标（现在暂且假定标定板完全放置水平，认为 target 坐标系与世界 inertial 坐标系重合）。pose.transformationToCurveValue() 的定义在此。然而，此代码调用的是 rotation_->rotationMatrixToParameters，此处使用的 rotation_ 是 RotationVector，于是可以找到详细的将 RotationMatrix 转化为 3 维变量的代码。对于这个转换过程，可以参考维基百科Rotation_matrix#Conversion_from_and_to_axis–angle，代码与公式完全对应，可以确认这个转换过程是转换成轴角，并且没有转变旋转的方向。
Kalibr 是使用 T_t_b 构建 B 样条，与 IMU 测量值对齐，实际上完全没必要做 T_c_b 的转换，可以直接使用 T_t_c 构建 B 样条，因为在同一个刚体上的各个坐标系的角速度的模相等。这个结论可以参考 [1] 6.4.4 Inertial Measurement Unit (6.154)，公式中的 \(\mathbf{\omega}\) 是指 imu 的角速度测量值，\(\mathbf{\omega}_v^{vi}\) 是 vehicle（可以当做是相机 c）的角速度，因为旋转矩阵不改变向量坐标的模长，所以两者的模长相等。
为了下文讨论方便，正式定义本文使用的坐标系，i 表示 inertial frame，s 表示 sensor frame（与上篇博客保持一致，也是与 [1] 保持一致）。主要参考[1]的6.2.4 Rotational Kinematics。注意，在 kalibr 中输入到 B 样条的 Transformation 方向是 \(\mathbf{T}_{is}\)，所以主要以这个方向讨论。
2. 轴角时间变化率与角速度的关系
在 Rotation Kinematics 中已经得出角速度的结论，如下式。
\[\begin{align} \dot{\mathbf{C}_{si}(t)} = \mathbf{C}_{si}(t){\mathbf{\omega}_s^{si}}^{\wedge} \end{align}\]
IMU 的角速度测量值 \(\mathbf{\omega}_s^{si}\) 是 s 相对 i 的角速度，在 s 下观测到的结果。（所谓在 s 下观测，是指将这个角速度对应的向量投影到 s 坐标系下，取其坐标值。）
现在要寻找角速度 \(\mathbf{\omega}_s^{si}\) 与轴角时间变化率 \(\dot{\mathbf{\phi}_{is}}\) 之间的关系。因为 B 样条与 SE(3) 无关，B 样条并不理解业务，B 样条只能做一条曲线拟合输进去的数字，所以从 B 样条得到的是轴角的时间变化率 \(\dot{\mathbf{\phi}_{is}}\)。（当然，B 样条作为一条曲线，它是一个参数方程，这个参数也是输入进去的，是时间 t，所以此处是时间变化率）。
轴角变化率 \(\dot{\mathbf{\phi}_{is}}\) 蕴含在 \(\dot{\mathbf{C}_{si}(t)}\) 中。
先给出 3 个下面推导会用到的结论。
- [2] 公式 (10) \(\mathbf{R}\text{Exp}(\mathbf{\phi})\mathbf{R}^T=\exp(\mathbf{R}\mathbf{\phi}^{\wedge}\mathbf{R}^T)=\text{Exp}(\mathbf{R}\mathbf{\phi})\)，注意 \(\text{Exp}(\cdot)\) 与 \(\exp(\cdot)\) 的区别。
- 参考 [1] (7.75)，\(\text{Exp}(\mathbf{\phi} + \Delta\mathbf{\phi}) = \text{Exp}(\mathbf{\phi})\text{Exp}(J_r(\mathbf{\phi})\Delta\mathbf{\phi})\)。
- 参考 [3]，\((\mathbf{R}\mathbf{v})^\wedge = \mathbf{R}\mathbf{v}^\wedge\mathbf{R}^T\)。
如下推导，得到轴角变化率与角速度的关系。
\[\begin{align} {\mathbf{\omega}_s^{si}}^{\wedge} &= {\mathbf{C}_{si}(t)}^T\dot{\mathbf{C}_{si}(t)} \notag \\ &= {\mathbf{C}_{is}(t)}\dot{\mathbf{C}_{is}(t)}^T \notag \\ &= -\mathbf{C}_{is}(t) {\mathbf{C}_{is}(t)}^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\mathbf{C}_{is}(t+\Delta t) - \mathbf{C}_{is}(t) \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{is}+\Delta\mathbf{\phi}_{is}) - \text{Exp}(\mathbf{\phi}_{is}) \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{is})\text{Exp}(J_r(\mathbf{\phi}_{is})\Delta\mathbf{\phi}_{is}) - \text{Exp}(\mathbf{\phi}_{is}) \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{is})[\mathbf{I}+(J_r(\mathbf{\phi}_{is})\Delta\mathbf{\phi}_{is})^\wedge] - \text{Exp}(\mathbf{\phi}_{is}) \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{is})(J_r(\mathbf{\phi}_{is})\Delta\mathbf{\phi}_{is})^\wedge \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\text{Exp}(\mathbf{\phi}_{is}) \lim_{\Delta t \to 0}\{ {J_r(\mathbf{\phi}_{is})(\Delta\mathbf{\phi}_{is})^\wedge J_r^T(\mathbf{\phi}_{is}) \over \Delta t} \} \mathbf{C}_{is}(t)^T \notag \\ &= -\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is}) \lim_{\Delta t \to 0}\{ {(\Delta\mathbf{\phi}_{is})^\wedge \over \Delta t} \} J_r^T(\mathbf{\phi}_{is}) \mathbf{C}_{is}(t)^T \notag \\ &= -\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is}) (\lim_{\Delta t \to 0}\{ {\Delta\mathbf{\phi}_{is} \over \Delta t} \})^\wedge J_r^T(\mathbf{\phi}_{is}) \mathbf{C}_{is}(t)^T \notag \\ &= -\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is}) (\dot{\mathbf{\phi}_{is}})^\wedge J_r^T(\mathbf{\phi}_{is}) \mathbf{C}_{is}(t)^T \notag \\ &= -(\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}})^\wedge \\ \mathbf{\omega}_s^{si} &= -\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \end{align}\]
另外，顺便写出 \(\mathbf{\omega}_i^{is}\)，参考 [1] Page233 (7.78) (7.80)。
\[\begin{align} \mathbf{\omega}_i^{is} &= -\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \notag \\ &= \mathbf{C}_{is}(t)^T J_r(-\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \notag \\ &= \mathbf{C}_{is}(t)^T J_l(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \notag \\ &= \mathbf{C}_{is}(t)^T \mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \notag \\ &= J_r(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \end{align}\]
3. Kalibr 代码验证
代码提供两种角速度。对于这两种角速度angularVelocity, angularVelocityBodyFrame，注释里面有写他们的含义。
// \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
Eigen::Vector3d BSplinePose::angularVelocity(double tk) const
// \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const所以 angularVelocity 对应 \(\mathbf{\omega}_i^{si} = -\mathbf{\omega}_i^{is}\)，angularVelocityBodyFrame 对应 \(\mathbf{\omega}_s^{is} = -\mathbf{\omega}_s^{si}\)。推导，可以验证代码与以上得到的结论匹配。
可以证明这两个函数中使用的 rotation_->parametersToSMatrix(r.tail<3>()) 与我以上写的公式 \(J_r(\mathbf{\phi}_{is})\) 对应，\(J_r(\mathbf{\phi}_{is})\) 由 [1] Page233 (7.77a) 给出，在证明中需要参考 [1] Page221 (7.24)。令 \(\mathbf{\phi}=\theta\mathbf{a}\) 为轴角，\(\theta\) 为角，\(\mathbf{a}\) 为轴。证明如下。
\[\begin{align} S(\mathbf{\phi}) &= \mathbf{I} + (-2 {\sin^2({\theta \over 2}) \over \theta}\mathbf{a}^{\wedge}) + {\theta - \sin\theta \over \theta}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge} \notag \\ &= \mathbf{I} + {\cos\theta - 1 \over \theta}\mathbf{a}^{\wedge} + ({\theta - \sin\theta\over \theta})(\mathbf{a}^T\mathbf{a}^T - \mathbf{I}) \notag \\ &= \mathbf{I} + {\cos\theta - 1 \over \theta}\mathbf{a}^{\wedge} + (\mathbf{I}-{\sin\theta\over \theta})(\mathbf{a}^T\mathbf{a}^T - \mathbf{I}) \notag \\ &= \mathbf{I} + {\cos\theta - 1 \over \theta}\mathbf{a}^{\wedge} + \mathbf{a}^T\mathbf{a}^T - \mathbf{I} - {\sin\theta\over \theta}\mathbf{a}^T\mathbf{a}^T + {\sin\theta\over \theta}\mathbf{I} \notag \\ &= {\sin\theta\over \theta}\mathbf{I} + (1 - {\sin\theta\over \theta})\mathbf{a}^T\mathbf{a}^T - {1 - \cos\theta \over \theta}\mathbf{a}^{\wedge} \end{align}\]
Reference
[1] Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.