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 中已经得出角速度的结论，如下式。

[egin{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} ext{Exp}(mathbf{phi})mathbf{R}^T=exp(mathbf{R}mathbf{phi}^{wedge}mathbf{R}^T)= ext{Exp}(mathbf{R}mathbf{phi}))，注意 ( ext{Exp}(cdot)) 与 (exp(cdot)) 的区别。
2. 参考 [1] (7.75)，( ext{Exp}(mathbf{phi} + Deltamathbf{phi}) = ext{Exp}(mathbf{phi}) ext{Exp}(J_r(mathbf{phi})Deltamathbf{phi}))。
3. 参考 [3]，((mathbf{R}mathbf{v})^wedge = mathbf{R}mathbf{v}^wedgemathbf{R}^T)。

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

[egin{align} {mathbf{omega}_s^{si}}^{wedge} &= {mathbf{C}_{is}(t)}^Tdot{mathbf{C}_{is}(t)} otag \ &= -dot{mathbf{C}_{si}(t)} {mathbf{C}_{si}(t)}^T otag \ &= -lim_{Delta t o 0}{ {mathbf{C}_{si}(t+Delta t) - mathbf{C}_{si}(t) over Delta t} } mathbf{C}_{si}(t)^T otag \ &= -lim_{Delta t o 0}{ { ext{Exp}(mathbf{phi}_{si}+Deltamathbf{phi}_{si}) - ext{Exp}(mathbf{phi}_{si}) over Delta t} } mathbf{C}_{si}(t)^T otag \ &= -lim_{Delta t o 0}{ { ext{Exp}(mathbf{phi}_{si}) ext{Exp}(J_r(mathbf{phi}_{si})Deltamathbf{phi}_{si}) - ext{Exp}(mathbf{phi}_{si}) over Delta t} } mathbf{C}_{si}(t)^T otag \ &= -lim_{Delta t o 0}{ { ext{Exp}(mathbf{phi}_{si})[mathbf{I}+(J_r(mathbf{phi}_{si})Deltamathbf{phi}_{si})^wedge] - ext{Exp}(mathbf{phi}_{si}) over Delta t} } mathbf{C}_{si}(t)^T otag \ &= -lim_{Delta t o 0}{ { ext{Exp}(mathbf{phi}_{si})(J_r(mathbf{phi}_{si})Deltamathbf{phi}_{si})^wedge over Delta t} } mathbf{C}_{si}(t)^T otag \ &= - ext{Exp}(mathbf{phi}_{si}) lim_{Delta t o 0}{ {J_r(mathbf{phi}_{si})(Deltamathbf{phi}_{si})^wedge J_r^T(mathbf{phi}_{si}) over Delta t} } mathbf{C}_{si}(t)^T otag \ &= -mathbf{C}_{si}(t) J_r(mathbf{phi}_{si}) lim_{Delta t o 0}{ {(Deltamathbf{phi}_{si})^wedge over Delta t} } J_r^T(mathbf{phi}_{si}) mathbf{C}_{si}(t)^T otag \ &= -mathbf{C}_{si}(t) J_r(mathbf{phi}_{si}) (lim_{Delta t o 0}{ {Deltamathbf{phi}_{si} over Delta t} })^wedge J_r^T(mathbf{phi}_{si}) mathbf{C}_{si}(t)^T otag \ &= -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 otag \ &= -(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)。

[egin{align} {mathbf{omega}_i^{is}}^{wedge} &= -mathbf{C}_{is}(t) J_r(mathbf{phi}_{is})dot{mathbf{phi}_{is}} otag \ &= mathbf{C}_{is}(t) J_r(-mathbf{phi}_{si})dot{mathbf{phi}_{si}} otag \ &= mathbf{C}_{is}(t) J_l(mathbf{phi}_{si})dot{mathbf{phi}_{si}} otag \ &= mathbf{C}_{is}(t) mathbf{C}_{si}(t) J_r(mathbf{phi}_{si})dot{mathbf{phi}_{si}} otag \ &= 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}= hetamathbf{a}) 为轴角，( heta) 为角，(mathbf{a}) 为轴。证明如下。

[egin{align} S(mathbf{phi}) &= mathbf{I} + (-2 {sin^2({ heta over 2}) over heta}mathbf{a}^{wedge}) + { heta - sin heta over heta}mathbf{a}^{wedge}mathbf{a}^{wedge} otag \ &= mathbf{I} + {cos heta - 1 over heta}mathbf{a}^{wedge} + ({ heta - sin hetaover heta})(mathbf{a}mathbf{a}^T - mathbf{I}) otag \ &= mathbf{I} + {cos heta - 1 over heta}mathbf{a}^{wedge} + (mathbf{I}-{sin hetaover heta})(mathbf{a}mathbf{a}^T - mathbf{I}) otag \ &= mathbf{I} + {cos heta - 1 over heta}mathbf{a}^{wedge} + mathbf{a}mathbf{a}^T - mathbf{I} - {sin hetaover heta}mathbf{a}mathbf{a}^T + {sin hetaover heta}mathbf{I} otag \ &= {sin hetaover heta}mathbf{I} + (1 - {sin hetaover heta})mathbf{a}mathbf{a}^T - {1 - cos heta over heta}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'.