公式推导

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}$就是测量值。

 

 

 

 

 

 

 

 

转载：https://blog.csdn.net/weixin_42361804/article/details/104504313