B 样条曲线的 SE(3) 应用

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}_{is}(t)} = \mathbf{C}_{is}(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}_{is}(t)}\) 中。

先给出 3 个下面推导会用到的结论。

1. [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)\) 的区别。
2. 参考 [1] (7.75)，\(\text{Exp}(\mathbf{\phi} + \Delta\mathbf{\phi}) = \text{Exp}(\mathbf{\phi})\text{Exp}(J_r(\mathbf{\phi})\Delta\mathbf{\phi})\)。
3. 参考 [3]，\((\mathbf{R}\mathbf{v})^\wedge = \mathbf{R}\mathbf{v}^\wedge\mathbf{R}^T\)。

如下推导，得到轴角变化率与角速度的关系。

\[\begin{align} {\mathbf{\omega}_s^{si}}^{\wedge} &= {\mathbf{C}_{is}(t)}^T\dot{\mathbf{C}_{is}(t)} \notag \\ &= -\dot{\mathbf{C}_{si}(t)} {\mathbf{C}_{si}(t)}^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\mathbf{C}_{si}(t+\Delta t) - \mathbf{C}_{si}(t) \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{si}+\Delta\mathbf{\phi}_{si}) - \text{Exp}(\mathbf{\phi}_{si}) \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{si})\text{Exp}(J_r(\mathbf{\phi}_{si})\Delta\mathbf{\phi}_{si}) - \text{Exp}(\mathbf{\phi}_{si}) \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{si})[\mathbf{I}+(J_r(\mathbf{\phi}_{si})\Delta\mathbf{\phi}_{si})^\wedge] - \text{Exp}(\mathbf{\phi}_{si}) \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\lim_{\Delta t \to 0}\{ {\text{Exp}(\mathbf{\phi}_{si})(J_r(\mathbf{\phi}_{si})\Delta\mathbf{\phi}_{si})^\wedge \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\text{Exp}(\mathbf{\phi}_{si}) \lim_{\Delta t \to 0}\{ {J_r(\mathbf{\phi}_{si})(\Delta\mathbf{\phi}_{si})^\wedge J_r^T(\mathbf{\phi}_{si}) \over \Delta t} \} \mathbf{C}_{si}(t)^T \notag \\ &= -\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si}) \lim_{\Delta t \to 0}\{ {(\Delta\mathbf{\phi}_{si})^\wedge \over \Delta t} \} J_r^T(\mathbf{\phi}_{si}) \mathbf{C}_{si}(t)^T \notag \\ &= -\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si}) (\lim_{\Delta t \to 0}\{ {\Delta\mathbf{\phi}_{si} \over \Delta t} \})^\wedge J_r^T(\mathbf{\phi}_{si}) \mathbf{C}_{si}(t)^T \notag \\ &= -\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si}) (\dot{\mathbf{\phi}_{si}})^\wedge J_r^T(\mathbf{\phi}_{si}) \mathbf{C}_{si}(t)^T \notag \\ &= -(\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}})^\wedge \\ \mathbf{\omega}_s^{si} &= -\mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \end{align}\]

另外，顺便写出 \(\mathbf{\omega}_i^{is}\)，参考 [1] Page233 (7.78) (7.80)。

\[\begin{align} {\mathbf{\omega}_i^{is}}^{\wedge} &= -\mathbf{C}_{is}(t) J_r(\mathbf{\phi}_{is})\dot{\mathbf{\phi}_{is}} \notag \\ &= \mathbf{C}_{is}(t) J_r(-\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \notag \\ &= \mathbf{C}_{is}(t) J_l(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \notag \\ &= \mathbf{C}_{is}(t) \mathbf{C}_{si}(t) J_r(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \notag \\ &= J_r(\mathbf{\phi}_{si})\dot{\mathbf{\phi}_{si}} \end{align}\]

以上之所以要将轴角写成 \(\mathbf{\phi}_{si}\) 方向，而不是 \(\mathbf{\phi}_{is}\)，是因为要与 Kalibr 对照。

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 与 angularVelocityBodyFrame 中必有一错，因为它们表示的相反的方向，且在不同坐标系下。应当相差一个旋转矩阵与一个负号，但是实际上它们仅相差一个旋转矩阵。

angularVelocity 对应 \(\mathbf{\omega}_i^{si} = -\mathbf{\omega}_i^{is}\)，angularVelocityBodyFrame 对应 \(\mathbf{\omega}_s^{si}\)。angularVelocity 应当去除负号，使得对应 \(\mathbf{\omega}_i^{is}\)，才符合注释的含义，这一点可以查看函数 angularVelocityAndJacobian。

可以证明这两个函数中使用的 rotation_->parametersToSMatrix(r.tail<3>()) 与我以上写的公式 \(J_r(\mathbf{\phi}_{si})\) 对应，\(J_r(\mathbf{\phi}_{si})\) 由 [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}\mathbf{a}^T - \mathbf{I}) \notag \\ &= \mathbf{I} + {\cos\theta - 1 \over \theta}\mathbf{a}^{\wedge} + (\mathbf{I}-{\sin\theta\over \theta})(\mathbf{a}\mathbf{a}^T - \mathbf{I}) \notag \\ &= \mathbf{I} + {\cos\theta - 1 \over \theta}\mathbf{a}^{\wedge} + \mathbf{a}\mathbf{a}^T - \mathbf{I} - {\sin\theta\over \theta}\mathbf{a}\mathbf{a}^T + {\sin\theta\over \theta}\mathbf{I} \notag \\ &= {\sin\theta\over \theta}\mathbf{I} + (1 - {\sin\theta\over \theta})\mathbf{a}\mathbf{a}^T - {1 - \cos\theta \over \theta}\mathbf{a}^{\wedge} \end{align}\]

总结一下，4 个函数——angularVelocity、angularVelocityBodyFrame、angularVelocityAndJacobian、angularVelocityBodyFrameAndJacobian——的“对错”。angularVelocity 错（差一个负号），angularVelocityBodyFrame 对，angularVelocityAndJacobian 对，angularVelocityBodyFrameAndJacobian 对。

angularVelocityBodyFrameAndJacobian 结果对，但变量名错误。函数体中的变量 C_w_b 应当改名为 C_b_w，可以查看获得它的函数 inverseOrientationAndJacobian从 parametersToRotationMatrix 中取出之后进行了 transpose。这个过程中输入 parametersToRotationMatrix 的轴角方向是 \(\mathbf{\phi}_{si}\)，parametersToRotationMatrix 输出的实际上是旋转矩阵的 transpose，即 \(\mathbf{C}_{is}\)。transpose 之后从 inverseOrientationAndJacobian 中输出，则 inverseOrientationAndJacobian 的输出结果是 \(\mathbf{C}_{si}\)。

Reference

[1] Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.

[2] Forster, Christian, et al. "IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation." Georgia Institute of Technology, 2015.

[3] A Proof of (Rv)^ = Rv^R'.