Kalibr标定工具：Camera+IMU联合标定（MYNTEYE相机）

一、安装Kalibr

1.1 安装依赖项

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-devlibboost-all-dev doxygen libopencv-dev ros-indigo-vision-opencvros-indigo-image-transport-plugins ros-indigo-cmake-modulespython-software-properties software-properties-common libpoco-devpython-matplotlib python-scipy python-git python-pip ipython libtbb-devlibblas-dev liblapack-dev python-catkin-tools libv4l-dev

sudo -i
pip install python_igraph

1.2 创建工作空间

mkdir -p~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/indigo/setup.bash
catkin init
catkin config --extend /opt/ros/indigo
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

1.3 下载并编译

cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git

cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4

1.4 设置环境变量

source ~/kalibr_workspace/devel/setup.bash

二、准备标定所需文件

2.1 标定板及其参数文件

标定板可以用 kalibr 提供的pdf, 里面有三种类型的标定板(Aprilgrid, Checkerboard, Circlegrid).具体信息及参数文件下载https://github.com/ethz-asl/kalibr/wiki/downloads（下载需要科学上网），我们这里采用的是Aprilgrid标定板。注意事项：打印标定板一定要打印全，不能为了追求实际尺寸只打印部分，否则会导致标定时初始化失败。打印完实际测量标定板参数，然后修改标定板参数文件。

2.2 Camera参数文件

Camera与IMU参数文件的标准格式参考https://github.com/ethz-asl/kalibr/wiki/yaml-formats

官网例子：Example chain.yaml

cam0:
  camera_model: pinhole
  intrinsics: [461.629, 460.152, 362.680, 246.049]
  distortion_model: radtan
  distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /cam0/image_raw
  resolution: [752, 480]
cam1:
  camera_model: omni
  intrinsics: [0.80065662, 833.006, 830.345, 373.850, 253.749]
  distortion_model: radtan
  distortion_coeffs: [-0.33518750, 0.13211436, 0.00055967, 0.00057686]
  T_cn_cnm1:
  - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785]
  - [-0.00221074, 0.99992702, 0.01187697, 0.00045792]
  - [-0.00424598,-0.01188627, 0.99992034,-0.00064487]
  - [0.0, 0.0, 0.0, 1.0]
  T_cam_imu:
  - [ 0.01567142, 0.99978002,-0.01393948,-0.03997419]
  - [-0.99966203, 0.01595569, 0.02052137,-0.01735854]
  - [ 0.02073927, 0.01361317, 0.99969223, 0.00326019]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.681e-05
  rostopic: /cam1/image_raw
  resolution: [752, 480]

注意事项：1、 pinhole是针孔相机模型，radtan径向畸变，MYNTEYE相机选这两个 ；2、T_cam_imu在做联合标定时不需要写；3、MYNTEYE相机可以通过get_img_params获得，但是更推荐自己标定，因为我暂时用这组参数做的时候最后优化失败了，发散了，猜测和初始参数有关，明天验证一下。4、疑问：get_img_params获得的左右相机转换矩阵是right转到left下的，而T_cn_cnm1是cam0转到cam1中，好像是反的，是不是需要求逆？？不知道理解的对不对。

更新：用kalibr工具箱先进行相机的标定，得到的标定结果修改topic后用于联合标定，结果会比较准确。相机标定的过程可以参考https://blog.csdn.net/heyijia0327/article/details/83583360。如果相机标定过程中focal length初始化失败，可能的原因是标定板打印不全或者录制数据包时相机距离标定板太近了，推荐距离0.8m。

2.3 IMU参数文件 

官网例子：imu.yaml

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

rostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

同样建议自己标定IMU参数，如果不标定，MYNTEYE相机的IMU参数只能通过技术支持给的参数文件获得。另外别的教程给的推荐频率是相机20Hz，IMU 200Hz。 

注意事项：rostopic必须与bag文件中文件夹的路径相同，例如用MYNTEYE-SDK2-Sample中的rosbag记录数据，这里就是mynteye/left/image_raw，mynteye/right/image_raw，mynteye/imu/data_raw。

2.4 bag文件：包括左右图像数据和IMU数据

MYNTEYE相机制作bag文件有两种方法:用MYNTEYE-SDK2-Sample中的rosbag记录数据，或者用MYNTEYE-Okvis中的okvis_app_getcameraimucalibdataset记录数据。（为什么不用MYNTEYE-SDK2-Sample中的record？因为这里IMU数据记录的时间戳必须是ns单位的，但是record记录的时间戳单位是0.01ms，转换难度有点大，不推荐使用）

2.4.1 用MYNTEYE-SDK2-Sample中的rosbag记录数据（强烈推荐）

ros下启动MYNTEYE相机，推荐使用display.launch，这样可以观察录制数据时标定板是否在双目视野中。用以下指令记录数据：

rosbag record -O stereo_imu_calibra.bag/mynteye/left/image_raw/mynteye/right/image_raw /mynteye/imu/data_raw

 2.4.2 用MYNTEYE-Okvis中的okvis_app_getcameraimucalibdataset记录数据

cd mynt-eye-okvis-sample/build
./okvis_app_getcameraimucalibdataset 0 ./cameraimu/cam0/ ./cameraimu/cam1/ ./cameraimu/imu0.csv

./cameraimu/cam0/等是数据保存的路径，需要提前在mynt-eye-okvis-sample/build中建立空文件夹。

用kalibr自带工具制作bag包，dataset-dir是数据输入路径：

kalibr_bagcreater--folder dataset-dir --output-bag awsome.bag

注意事项：

采集数据的起始和结束阶段注意别晃动太大，如从桌子上拿起或者放下。如果有这样的动作，在标定阶段应该跳过bag数据集的首尾的数据。

采集数据的时候应该给imu各个轴足够的激励，如先依次绕各个轴运动，运动完后来个在空中画8字之类的操作，当然也要注意别运动太剧烈，图像都模糊了。

三、标定

cd ~/kalibr_workspace

kalibr_calibrate_imu_camera --target april_6x6_50x50cm.yaml --cam mynteye.yaml --imu imu.yaml --bag stereo_imu_calibra.bag

四、遇到的问题

1、如果kalibr_calibrate_cameras command not found，则需要设置环境;

source ~/kalibr_workspace/devel/setup.bash

2、 提示：“Spline Coefficient Buffer Exceeded. Set larger buffer margins”

在标定命令最后添加 --timeoffset-padding 0.1

3、图像和IMU过多导致计算量巨大，相机标定时可以降低图像topic发布的频率，而联合标定时不太需要处理，我做实验时近2000张图片，近20000条imu数据计算时间也不是很长。

4、优化失败，结果发散。猜测原因：bag包录制有问题，Camera与IMU参数文件中初始值不准确。

五、标定实验结果（部分截图）

PS：欢迎大家指正，提问与交流，后面还会随时更新进度。