IMU与camera标定、Kalibr

目前的研究方向可以总结为在滤波算法中实现高精度，在优化算法中追求实时性．当加入IMU后，研究方向分为松耦合和紧耦合，松耦合分别单独计算出IMU测量得到的状态和视觉里程计得到的状态然后融合，紧耦合则将IMU测量和视觉约束信息放在一个非线性优化函数中去优化．紧耦合的框架使得IMU数据可以对视觉里程计进行矫正，同时视觉里程计信息也可以矫正IMU的零偏，因此一般认为紧耦合的定位精度较高．个人认为松耦合和滤波融合的方法类似，紧耦合则主要基于非线性优化．

IMU融合后可以处理视觉失效的情况，例如光照变化，遮挡，模糊，快速运动；同时视觉也可以对IMU的本质误差零偏（漂移）进行很好的估计．

视觉和IMU融合目前主要基于三类方法，在设计时，主要考虑精确度和计算量的平衡．

1. 滤波

• 状态向量只包含当前状态．由于线性化误差和计算能力的限制，通常只能构建很少的landmark/mappoint．或者创建structureless的状态向量（将landmark/mappoint边缘化），典型代表为MSCKF.
• 边缘化是将旧状态融合进当前状态的先验中，滤波方法主要的缺陷也就存在于边缘化过程中：首先当前测量的structure信息需要延迟处理，降低当前状态的更新精度，其次边缘化对线性化近似和外点（outlier）都比较敏感，容易造成滤波器状态失准．（这里有很多坑，还不太理解）．

2. Fixed-Lag Smoothing

• 也就是滑动窗口优化（Sliding Window Optimization）．状态向量包含随时间滑动的窗口内多个状态．但是也需要将旧状态边缘化到高斯先验中．因此在滤波算法中存在的边缘化问题，这里都存在．
• 但是由于采用了多个状态的窗口，状态估计更精确，同时可以建立鲁棒的优化代价函数，降低外点对状态估计的影响．
• 状态向量中可以加入测量的structure，但是太多strcuture会造成高斯先验矩阵是稠密的，通常会想办法减少structure的数量．

3. Full Smoothing

• 也就是batch non-linear least-squares或者全局优化，状态向量包含所有相机位姿和strcuture信息，做大规模的非线性优化．显然是最精确的方法，虽说稀疏性降低了计算量，但是依然比滤波大．主要工作集中于降低计算量．
• 可以采用只优化关键帧，以及将优化放在独立线程中．
• 最新的研究采用因子图，增量式地根据测量更新部分状态，典型代表为iSAM．
• 当引入IMU后，由于IMU频率通常在100Hz-1kHz量级，无法每次IMU测量都更新状态向量．通常的做法是在关键帧之间对IMU进行积分，初始状态由上一帧状态提供，根据IMU的微分运动方程，积分出当前状态，再将视觉测量引入更新．
• 然而，由于优化过程中上一帧的状态是会变化的，这样积分的初始状态改变了，需要重新算积分，为了避免每一次优化调整后都需要重复计算IMU积分，利用预积分preintegration将相对运动约束再参数化．

双目相机与IMU camera IMU 联合标定工具箱使用方法——Kalibr

https://github.com/ethz-asl/kalibr

1.安装

Kalibr 提供两种使用方式。第一种为CDE下直接使用，作者将所需的库文件打包好，省去了配置dependency的步骤，可以直接从

https://github.com/ethz-asl/kalibr/wiki/downloads

下载， 注意需要科学浏览。

下载之后直接用

./cde-exec+命令 即可使用对应的工具。

例如如果我们想使用kalibr内的kalibr_calibrate_cameras功能，

使用

./ced-exec kalibr_calibrate_cameras --target april_6x6.yaml --bag static.bag --models pinhole-equi pinhole-equi omni-radtan omni-radtan --topics /cam0/image_raw /cam1/image_raw /cam2/image_raw /cam3/image_raw

即可。

但是此方法不能使用其中的某些功能，例如我们想使用calibration validator，在CDE环境下是无法使用的。

所以作者建议我们build from source。

根据https://github.com/ethz-asl/kalibr/wiki/installation的part B中的描述进行安装。

在安装过程中，可能会遇到wget相关的错误，比如说在安装到suitesparse库的时候，对应的cmakelists中会通过wget 下载压缩包，若无法下载则整个kalibr都无法成功安装，那么我们需要进行一番修改。

当然你也可以自己编写cmakelists，不过个人觉得有点麻烦。

首先打开对应的cmakelists。

找到DOWNLOAD_COMMAND那一行，添加host agent 信息，将语句替换类似如下

DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget "http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-${VERSION}.tar.gz" --user-agent="Mozilla/5.0 (Ubuntu;U; Linux x86_64; rv:53.0) Gecko/20100101 Firefox/53.0"

即可，若terminal提示无法解析你的host agent部分，可以将此行注释掉，因为文件已经下载，加入一行

DOWNLOAD_COMMAND wget "https://www.baidu.com"

即可进行下面的步骤。

到了这里应该没有问题了，等待半个小时左右即可build完毕。

 

ps: 在非CDE环境下使用需要注意python版本为2.7时可以正常使用。使用anaconda或者其他python版本的需要将python版本切换。

 

2.开始标定

Kalibr不仅提供了IMU以及camera的联合标定工具箱，也提供了camera标定工具箱。

在此我们先使用camera标定工具对相机进行标定。

首先我们需要制作标定板，在此推荐使用april tag，可以使用kalibr自带工具自定义生成不同大小的标定板，命令如下

kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]

将生成的pdf打印出来，置于平稳，光照充足的地方，注意要保持平整。

Kalibr作者推荐将camera固定，然后移动标定板，这样可以提高标定的稳定性，但是鉴于我使用场景受限，我使用的是晃动camera方法。

之后就可以开始录制bag了，使用

rosbag record /cam0/image_raw /cam1/image_raw /imu0

标定时注意激活所有的轴，即可录制包含三个topic的rosbag了。

注意录制过程中camera不要距离标定板太近，否则会出现无法初始化focal length的错误。

Kalibr是一个十分强大的工具箱，提供了很多工具。我们可以使用calibration validator进行标定的验证，原理是对重投影误差进行量化分析。大家可以自行验证。工具使用上一步骤的camera标定结果以及标定板的yaml文件，具体命令如下：

kalibr_camera_validator --cam camchain.yaml --target target.yaml

若对相机标定结果满意我们可以继续进行camera以及imu的联合标定步骤，此步使用了如下信息：

1. 标定板yaml

2. 相机标定结果yaml

3. imu内参yaml

4. 前面使用的对应bag

有了如上材料即可进行联合标定，具体如下

kalibr_calibrate_imu_camera --target aprilgrid6x4.yaml --cam camchain-2017-06-14-09-13-29.yaml --imu imu.yaml --bag 2017-06-14-09-13-29.bag --bag-from-to 5 45

最后的--bag_from_to是选取地5-45s的bag数据，去除了拾取防止设备产生的抖动部分影响。我们建议进行多组标定之后将标定结果进行最佳无偏估计，不建议对R部分直接取平均值，可以将多组数据的R部分转换成四元数。之后进行处理。

到此就将联合标定简单介绍完了。