多传感器标定

激光雷达和相机标定方法按是否需要特定的标志物分为target-based和targetless两种，前者需要专门的控制场，有代表ground truth的target标志物，比如典型的棋盘格平面板和圆点标定板，标志物通常需要专门定制，无论是棋盘格还是圆点标定板，都是精密仪器加工出来的高精度标定板，标定板各圆点，棋盘格之间的距离精准到丝级。标定板价格也从几十到几千元不等。；后者则可以在自然环境中进行，约束条件少，不需要用专门的target标志物，相对更方便，但是鲁棒性和精度通常差一点；
按照人为参与的程度可以分为手动、半自动和全自动。一般来讲自动化程度越高越方便易用，但是有人工的介入通常会使得标定结果更加鲁棒。

相机与激光雷达标定

autoware_camera_lidar标定工具链是手动、targetless标定方法。具体原理为手动标记图像和点云中的对应点，通过PnP方法首先解线性方程组得到初解(最少需要9对点)，再经由优化算法精细求解。

autoware雷达相机联合标定程序的结果是Camera->Lidar。

本人在autoware的标定程序基础上加入手动动态调参功能，可能人工微调达到理想效果。代码链接

激光雷达与IMU标定

手眼标定：参考文章

浙大LI_Calib

雷达 IMU 标定程序：浙大 Li_Calib 结果为IMU->Lidar
点云的运动畸变校正和数据关联是LIDAR-IMU标定的两个难点。
对于运动畸变校正，大多数现有工作采用短时间内的匀速运动假设或匀加速运动假设进行插值去畸变。
参考链接

港大Lidar_IMU_Init

港大lidar_imu_init：标定结果为Lidar->IMU
使用匀速运动模型去畸变的Fast-lio2得到激光里程计，通过将激光雷达原始帧分割成多个子帧来提高激光里程计的频率。
激光里程计输出每帧扫描结束时刻$t_k$雷达的角速度${\mathbf{\omega }}_{L_k}$和线速度${}^G{\mathbf{v}}_{L_k}$。IMU原始数据提供角速度${\mathbf{\omega }}_{m_i}$和线加速度${\mathbf{a}}_{m_i}$。当满足足够激励的数据收集完毕，就启动初始化模块，输出时间偏移${}^I{t}_L\in \mathbb{R}$，外参${}^I{\mathbf{T}}_L={(}^I{\mathbf{R}}_L{,}^I{\mathbf{p}}_L)\in SE(3)$，IMU偏置${\mathbf{b}}_{\omega },{\mathbf{b}}_a\in {\mathbb{R}}^3$和世界坐标系下的重力向量${}^G\mathbf{g}\in {\mathbb{R}}^3$

• 数据处理部分： 使用non-causal zero phase low-pass filter非因果零相位低通滤波器对激光里程计估计的角速度和线速度以及IMU测量的角速度和加速度进行去噪(主要是高频噪声)
  原始IMU测量：
  $$\begin{array}{rl}{\mathbf{\omega }}_{m_i}& ={\mathbf{\omega }}_i^{\mathrm{gt}}+{\mathbf{b}}_{\omega }+{\mathbf{n}}_{{\omega }_i}\\ {\mathbf{a}}_{m_i}& ={\mathbf{a}}_i^{\mathrm{gt}}+{\mathbf{b}}_a+{\mathbf{n}}_{a_i}\end{array}$$
  去噪IMU测量：
  $$\begin{array}{rl}{\mathbf{\omega }}_{I_i}& ={\mathbf{\omega }}_i^{\mathrm{gt}}+{\mathbf{b}}_{\omega }\\ {\mathbf{a}}_{I_i}& ={\mathbf{a}}_i^{\mathrm{gt}}+{\mathbf{b}}_a\end{array}$$
• 通过non-causal central difference来从LiDAR Odometry的角速度和线速度推算它的角加速度和线性加速度，以及从IMU的角速度差分得到角加速度。
  L k = { ω L k , G v L k , Ω L k , G a L k } I i = { ω I i , Ω I i , a I i } \mathcal{L}_k=\{\bm{\omega}_{L_k},{}^{G}\bold{v}_{L_k},\bold{\Omega}_{L_k},{}^{G}\bold{a}_{L_k}\}\\ \mathcal{I}_i=\{\bm{\omega}_{I_i},\bold{\Omega}_{I_i},\bold{a}_{I_i}\} Lk​={ωLk​​,GvLk​​,ΩLk​​,GaLk​​}Ii​={ωIi​​,ΩIi​​,aIi​​}
• 数据对齐：由于LiDAR和IMU频率不一致，还需要将IMU数据通过插值并降采样得到每个LiDAR odometry时刻$t_k$的IMU数据：
  I k = { ω I k , Ω I k , a I k } \mathcal{I}_k=\{\bm{\omega}_{I_k},\bold{\Omega}_{I_k},\bold{a}_{I_k}\} Ik​={ωIk​​,ΩIk​​,aIk​​}

最大互相关时间初始化

使用最大互相关(Cross-Correlation)得到时间的偏移的粗略估计(以一个子帧的时间间隔$\Delta t$为单位)。
准确的时间偏移${}^I{t}_L=d^{\ast }\Delta t+\delta t$，$d$为整数
经过最大互相关得到最优的$d^{\ast }$

外参旋转和时间标定

参考IMU模型的角速度式：
$$\mathbf{\omega }={}^{\mathrm{s}}{\mathbf{R}}_{\mathrm{v}}{}^{\mathrm{v}}{\mathbf{\omega }}_{\mathrm{vi}}$$
将sensor系对应IMU系$\mathrm{s}\to I$，载具系对应Lidar系$\mathrm{v}\to L$，与文章中变量的对应关系：

• $\mathbf{\omega }\to {\mathbf{\omega }}_I^{\mathrm{gt}}={\bar{\mathbf{\omega }}}_{I_k}-{\mathbf{b}}_{\omega }={\mathbf{\omega }}_I(t_k+d^{\ast }\Delta t+\delta t)-{\mathbf{b}}_{\omega }$
• ${}^{\mathrm{s}}{\mathbf{R}}_{\mathrm{v}}\to {}^I{\mathbf{R}}_L$
• ${}^{\mathrm{v}}{\mathbf{\omega }}_{\mathrm{vi}}\to {\mathbf{\omega }}_{L_k}$

得到
$${\mathbf{\omega }}_I(t_k+d^{\ast }\Delta t+\delta t)-{\mathbf{b}}_{\omega }={}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}$$

假设匀加速运动，在$t_k+d^{\ast }\Delta t$展开
$${\mathbf{\omega }}_I(t_k+d^{\ast }\Delta t+\delta t)\approx {\mathbf{\omega }}_{I_{k+d}}+\delta t{\mathbf{\Omega }}_{I_{k+d}}$$
则有
$${\mathbf{\omega }}_{I_k^{\prime }}+\delta t{\mathbf{\Omega }}_{I_k^{\prime }}={}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\omega }$$
基于上述约束进行优化求解${}^I{\mathbf{R}}_L,{\mathbf{b}}_{\omega },\delta t$：

$$\underset{{}^I{\mathbf{R}}_L,{\mathbf{b}}_{\omega },\delta t}{\mathrm{argmin}}\sum \Vert {}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}+{\mathbf{b}}_{\omega }-{\mathbf{\omega }}_{I_k^{\prime }}-\delta t{\mathbf{\Omega }}_{I_k^{\prime }}{\Vert }^2\text{\hspace{1em}(opt 1)}$$

外参偏移和重力初始化

上一步算出$d^{\ast }$和$\delta t$之后就可以计算出与 ${\mathbf{\omega }}_{L_k}$对应的IMU角速度${\bar{\mathbf{\omega }}}_{I_k}$和${\bar{\mathbf{a}}}_{I_k}$
$$\begin{gathered} {\bar{\mathbf{\omega }}}_{I_k}={\mathbf{\omega }}_I(t_k+d^{\ast }\Delta t+\delta t)\approx {\mathbf{\omega }}_{I_{k+d}}+\delta t{\mathbf{\Omega }}_{I_{k+d}} \\ {\bar{\mathbf{a}}}_{I_k}={\mathbf{a}}_I(t_k+d^{\ast }\Delta t+\delta t)\approx {\mathbf{a}}_{I_{k+d}}+\frac{\delta t}{\Delta t}({\mathbf{a}}_{I_{k+d+1}}-{\mathbf{a}}_{I_{k+d}}) \end{gathered}$$
论文中出现的公式参考IMU模型的acc fixed frame式
$${}^A{\mathbf{R}}_B{\mathbf{a}}_B={\mathbf{a}}_A+{\mathbf{\omega }}_A^{\wedge 2}{}^A{\mathbf{p}}_B+{\mathbf{\Omega }}_A^{\wedge }{}^A{\mathbf{p}}_B$$

• ${\mathbf{a}}_A$是A坐标系相对惯性系的加速度在A坐标系下表示
• ${\mathbf{a}}_B$是B坐标系相对惯性系的加速度在B坐标系下表示
• ${\mathbf{\omega }}_A$是A坐标系相对惯性系的角速度在A坐标系下的表
• ${\mathbf{\Omega }}_A$是A坐标系相对惯性系的角加速度在A坐标系下的表示
• ${}^A{\mathbf{p}}_B$是B坐标系相对于A坐标系的偏移

对于Lidar-inertial system，我们有两个选择A for IMU and B for LiDAR或者相反。由于IMU的角速度和角加速度会被陀螺仪噪声和偏置影响，所以选择A as LiDAR and B as IMU。
LIO得到的雷达的加速度是在全局坐标系描述的${}^G{\mathbf{a}}_{L_k}$，计算其等效的IMU测量加速度(假设有一个相同的IMU安装在雷达处)。
$${\mathbf{a}}_{L_k}={}^L{\mathbf{R}}_L^T\left({}^G{\mathbf{a}}_{L_k}-{}^G\mathbf{g}\right)$$
ps：---------------------------------------------------------------------------------------------------
我觉得论文中的这段公式推导有点不好理解，可以直接参考IMU模型的加速度式：
$$\mathbf{a}={}^{\mathrm{s}}{\mathbf{R}}_{\mathrm{v}}\left[{}^{\mathrm{v}}{\mathbf{R}}_{\mathrm{i}}\left({}^{\mathrm{i}}{\ddot{\mathbf{r}}}_{\mathrm{vi}}-{}^{\mathrm{i}}\mathbf{g}\right)+\left({}^{\mathrm{v}}{\mathbf{\omega }}_{\mathrm{vi}}^{\wedge 2}+{}^{\mathrm{v}}{\dot{\mathbf{\omega }}}_{\mathrm{vi}}^{\wedge }{}^{\mathrm{v}}\right){}^{\mathrm{v}}{\mathbf{r}}_{\mathrm{s}\mathrm{v}}\right]$$
将sensor系对应IMU系$\mathrm{s}\to I$，载具系对应Lidar系$\mathrm{v}\to L$，与文章中变量的对应关系：

• $\mathbf{a}\to {\mathbf{a}}_k^{\mathrm{gt}}={\bar{\mathbf{a}}}_{I_k}-{\mathbf{b}}_a$
• ${}^{\mathrm{s}}{\mathbf{R}}_{\mathrm{v}}\to {}^I{\mathbf{R}}_L$
• ${}^{\mathrm{v}}{\mathbf{R}}_{\mathrm{i}}\to {}^L{\mathbf{R}}_G$
• ${}^{\mathrm{i}}{\ddot{\mathbf{r}}}_{\mathrm{vi}}\to {}^G{\mathbf{a}}_{L_k}$
• ${}^i\mathbf{g}\to {}^G\mathbf{g}$
• ${}^{\mathrm{v}}{\mathbf{\omega }}_{\mathrm{vi}}\to {\mathbf{\omega }}_{L_k}$
• ${}^{\mathrm{v}}{\dot{\mathbf{\omega }}}_{\mathrm{vi}}\to {\mathbf{\Omega }}_{L_k}$
• ${}^{\mathrm{v}}{\mathbf{r}}_{\mathrm{s}\mathrm{v}}\to {}^L{\mathbf{p}}_I$

变换得到等式:
$${}^I{\mathbf{R}}_L^T\left({\bar{\mathbf{a}}}_{I_k}-{\mathbf{b}}_a\right)={}^L{\mathbf{R}}_G\left({}^G{\mathbf{a}}_{L_k}-{}^G\mathbf{g}\right)+\left({\mathbf{\omega }}_{L_k}^{\wedge 2}+{\mathbf{\Omega }}_{L_k}^{\wedge }\right){}^L{\mathbf{p}}_I$$

最后可以得到与论文相同的优化问题：

$$\underset{{}^L{\mathbf{p}}_I,{\mathbf{b}}_a,{}^G\mathbf{g}}{\mathrm{argmin}}\sum \Vert {}^I{\mathbf{R}}_L^T\left({\bar{\mathbf{a}}}_{I_k}-{\mathbf{b}}_a\right)-{}^L{\mathbf{R}}_G\left({}^G{\mathbf{a}}_{L_k}-{}^G\mathbf{g}\right)-\left({\mathbf{\omega }}_{L_k}^{\wedge 2}+{\mathbf{\Omega }}_{L_k}^{\wedge }\right){}^L{\mathbf{p}}_I{\Vert }^2\text{\hspace{1em}(opt 2)}$$
计算得到${}^L{\mathbf{p}}_I,{\mathbf{b}}_a,{}^G\mathbf{g}$

激励评估

初始化需要设备充足的运动激励，那么如何评估激励是否充分了呢？
对于最小二乘优化问题，使用高斯-牛顿法求解增量时：
$$\Delta \mathbf{x}=-(\mathbf{J}(\mathbf{x}{)}^T\mathbf{J}(\mathbf{x}){)}^{-1}\mathbf{J}(\mathbf{x}{)}^T\mathbf{e}(x)$$
需要求解${\mathbf{J}}^T\mathbf{J}$的逆，如果其不可逆或者可逆性质不好(即数值求解逆矩阵误差很大)，则优化效果会大打折扣。

所以理想的方法是评估优化问题$opt1$对于$({}^L{\mathbf{R}}_I,{\mathbf{b}}_{\omega },\delta t)$的Jacobian矩阵和$opt2$对于${}^L{\mathbf{p}}_I,{\mathbf{b}}_a,{}^G\mathbf{g}$的Jacobian矩阵的秩。
但是论文中说只需评估对于外参旋转${}^I{\mathbf{R}}_L,$和外参偏移${}^L{\mathbf{p}}_I,$的Jacobian矩阵足矣。
$${\mathbf{J}}_r=\left[\begin{array}{c}⋮\\ -{}^I{\mathbf{R}}_L{\mathbf{\omega }}_{L_k}^{\wedge }\\ ⋮\end{array}\right],{\mathbf{J}}_t=\left[\begin{array}{c}⋮\\ {\mathbf{\omega }}_{L_k}^{\wedge 2}+{\mathbf{\Omega }}_{L_k}^{\wedge }\\ ⋮\end{array}\right]$$
最后问题转化为求${\mathbf{J}}_r^T{\mathbf{J}}_r=\sum {{\mathbf{\omega }}_{L_k}^{\wedge }}^T{\mathbf{\omega }}_{L_k}^{\wedge }$和${\mathbf{J}}_t^T{\mathbf{J}}_t=\sum {\left({\mathbf{\omega }}_{L_k}^{\wedge 2}+{\mathbf{\Omega }}_{L_k}^{\wedge }\right)}^T\left({\mathbf{\omega }}_{L_k}^{\wedge 2}+{\mathbf{\Omega }}_{L_k}^{\wedge }\right)$的奇异值。最小奇异值要足够大才能保证充分的运动激励。

相机-IMU标定

Kalibr标定原理详解
Kalibr使用参考
遇到问题：ModuleNotFoundError: No module named ‘wx’

标定完的结果检验

autoware标定完的结果是相机相对雷达的坐标变换，求一下逆转换为雷达相对于相机的坐标变换，IMU的标定结果也转换为相机到IMU的坐标变换，然后通过static_transform_publisher发布出去，就可以在rviz中查看标定的外参结果了。

<launch>
  <arg name="model" default="$(find xsens_mti_driver)/urdf/MTi_6xx.urdf"/>
  <arg name="rvizconfig" default="$(find xsens_mti_driver)/rviz/display.rviz" />
  
  <!-- 发布雷达到相机的tf坐标变换 -->
  <node name="tf_pub2" pkg="tf2_ros" type="static_transform_publisher" args="0.0444635 -0.0991606 -0.183957  0.0601806 -1.22453  1.50543 camera rslidar"/>	
  <!-- 发布相机到IMU的tf坐标变换 -->
  <node name="tf_cam2imu" pkg="tf2_ros" type="static_transform_publisher" args="0.162062 0.106803 0.113255 -0.571888 0.402432 -0.410687 0.585167 imu_link camera"/>
  
  <!-- IMU驱动自带的urdf文件，不必要 -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>