公式推导

2013 年 BMVC, Steven Lovegrove ,Spline Fusion: A continuous-timerepresentation for visual-inertial fusion with application to rolling shutter cameras.

1、Continuous-time representation

1.1、Camera pose transformations

齐次点从图像帧 a 到图像帧 b 的位姿变换可以表示为：

在\(\Delta {\rm{t}}\)时间内，相机位姿从 Twa 变为 Twb（w 表示世界坐标系），假设在该时间内角速度与线速度是恒定的，则相机运动速度可以表示为：

1.2、 \({{\rm{C}}^2}\)-continuous curves in SE3

该方法核心是找到一种连续轨迹表示法，它要满足：
1）局部控制，可以在线求解并且批量处理。
2）C2 连续性可以预测 IMU 测量值。
3）对最小扭矩轨迹有一个较好的估计。
三次 B 样条插值可以对\({{\rm{R}}^3}\)空间进行很好的表示，为了保证\({{\rm{C}}^2}\)连续性，利用由李代数组成的累积基函数参数化连续轨迹。它不仅保持了\({{\rm{C}}^2}\)连续性，而且具有简单的二阶导数公式。

1.2.1、Representing B-Splines with cumulative basis functions
k-1 阶标准基函数表示的 B 样条曲线如下：

其中，\({{\rm{p}}_i} \in {{\rm{R}}^N}\)是\({t_i}\)，\(i \in [0,...,n]\)时刻的控制点，\({B_{i,k}}(t)\)为基函数，上式可以重写为累加形式：

\({{\tilde B}_{i,k}}(t) = \sum \begin{array}{l}
n\\
j = i
\end{array} {B_{j,k}}(t)\)为累加基函数。

利用之前的对数映射和指数映射，可以用控制位姿之差的对数映射，替换上式中的控制点的差，来描述在 SE3 上的轨迹，上式可以写为：

其中，\({{\rm{T}}_{w,s}}(t) \in {\rm{SE}}(3)\)为沿样条在时间t处的位姿，\({{\rm{T}}_{w,i}} \in {\rm{SE}}(3)\)为控制位姿，w表示世界坐标系。

1.2.2、Cumulative cubic B-Splines

以 4 阶累积三次B样条为例（k=4），假设控制点以同一时间间隔 delta_t 分布，在三次B样条插值中，有四个控制点会影响时刻 t 的样条值，就是说在\(t \in [{t_i},{t_{i + 1}}]\)这一时间段中可以定义\([{t_{i - 1}},{t_i},{t_{i + 1}},{t_{i + 2}}]\)四个控制点，然后利用同一个时间表示\(s(t) = (t - {t_0})/\Delta t\)对函数进行化简，这样就把控制点时间\({t_i}\)转换为同一时间序列\({s_i} \in [0,1,...,n]\)。对于给定一个时间\({s_i} < s(t) < {s_{i + 1}}\)，定义\(u(t) = s(t) - {s_i}\)，利用该时间函数，可以将累积基函数B(u)的矩阵表示以及它对时间的一阶和二阶导数表示如下：

样条轨迹中的位姿表示为：
 

对于轨迹对时间的一阶和二阶导数表示如下：
 

2、Generative model of visual-inertial data
2.1、Parameterization

沿着样条轨迹，从第一个观察到的位姿利用逆深度参数化系统中的路标点，然后将逆深度点投影到相机帧 a 中的像素点\({p_a}\)转换到相机帧 b 下的像素点\({p_b}\)。对应的变换为：

其中，\(\pi ({\rm{P}}){\rm{ = }}\frac{1}{{{{\rm{P}}_2}}}{[{{\rm{P}}_0}{\rm{,}}{{\rm{P}}_1}]^{\rm{T}}}\)是齐次映射函数，\{{\rm{K}}_a}{\rm{,}}{{\rm{K}}_b} \in {{\rm{R}}^{3 \times 3}}\是相机帧 a,b 分别对应的内参矩阵。累积 B 样条参数化可以计算对时间的解析导数，所以可以方便的插值出加速度和陀螺仪的测量值，可以依次在观测到的测量值基础上组成误差。插值计算如下：

\({{{\rm{\dot R}}}_{w,s}}\)和\({{\ddot s}_w}\)分别是\({{{\rm{\dot T}}}_{w,s}}\)和\({{{\rm{\ddot T}}}_{w,s}}\)的子矩阵，\({g_w}\)为重力加速度。

最后，给定一个视觉和惯性数据的产生模型，对相机的重投影误差和惯性误差进行统一处理，综合后的目标函数如下式所示，利用迭代非线性最小二乘进行求解，即可得到样条插值结果：

其中，\({{{\rm{\hat p}}}_m}\)，\({{\hat w}_m}\)，\({{{\rm{\hat a}}}_m}\)就是测量值。