小觅双目+IMU联合标定

安装Kalibr

Kalibr工程有两种，一种是已经编译好的包，叫CDE package,另一种是未经编译的源文件。
选择源码安装

安装依赖项

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 pip installpython-igraph --upgrade

创建工作空间

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

下载并编译

cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
 
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4

设置环境

source ~/kalibr_workspace/devel/setup.bash

mynteye标定

get_img_params获得：

获取相机标定参数

./samples/_output/bin/tutorials/get_img_params   

I/get_img_params.cc:44 Intrinsics left: {pinhole, width: 752, height: 480, fx: 362.20059643202876032, fy: 363.50065250745848289, cx: 406.58699068023440759, cy: 234.35161110061483214, model: 0, coeffs: [-0.25034765682756088, 0.05057939920289762, -0.00070536676161976, -0.00852554513070338, 0.00000000000000000]}
I/get_img_params.cc:45 Intrinsics right: {pinhole, width: 752, height: 480, fx: 365.14014888558477878, fy: 365.13385298966960590, cx: 389.32395100630907336, cy: 234.95160212312546832, model: 0, coeffs: [-0.30377346762098512, 0.07992969367399984, 0.00005154751753072, -0.00067345903740579, 0.00000000000000000]}
I/get_img_params.cc:46 Extrinsics right to left: {rotation: [0.99867908939669447, -0.00634455661374854, 0.05098845950961969, 0.00598903163893333, 0.99995670037792639, 0.00712242018683670, -0.05103144032669509, -0.00680764060926713, 0.99867384471984544], translation: [-120.02489764113249748, -1.17826374090507469, -5.20582051599965379]}
ROSMsgInfoPair:
left:
width: 752, height: 480
distortion_model: PINHOLE
D: -0.250348,0.0505794,-0.000705367,-0.00852555,0,
K: 362.201,0,363.501,0,406.587,234.352,0,0,1,
R: 0.995541,0.00317344,0.0942805,-0.00348417,0.999989,0.00313134,-0.0942695,-0.00344587,0.995541,
P: 326.239,0,340.971,0,0,326.239,235.709,0,0,0,1,0,

right:
width: 752, height: 480
distortion_model: PINHOLE
D: -0.303773,0.0799297,5.15475e-05,-0.000673459,0,
K: 365.14,0,365.134,0,389.324,234.952,0,0,1,
R: 0.999013,0.00980714,0.04333,-0.00966439,0.999947,-0.00350257,-0.0433621,0.00308036,0.999055,
P: 326.239,0,340.971,-39195.5,0,326.239,235.709,0,0,0,1,0,

相机投影参数：fx、fy、cx、cy

相机畸变参数：coeffs[5]=[k1,k2,p1,p2,k3]

获取IMU标定参数

./samples/_output/bin/tutorials/get_imu_params

I/get_imu_params.cc:23 Motion intrinsics: {accel: {scale: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000], assembly: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000], drift: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], noise: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], bias: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], x: [0.00000000000000000, 0.00000000000000000], y: [0.00000000000000000, 0.00000000000000000], z: [0.00000000000000000, 0.00000000000000000]}, gyro: {scale: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000], assembly: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000], drift: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], noise: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], bias: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000], x: [0.00000000000000000, 0.00000000000000000], y: [0.00000000000000000, 0.00000000000000000], z: [0.00000000000000000, 0.00000000000000000]}}
I/get_imu_params.cc:24 Motion extrinsics left to imu: {rotation: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000, 0.00000000000000000], translation: [0.00000000000000000, 0.00000000000000000, 0.00000000000000000]}

Camera与IMU参数：

Each camera has the following parameters:

camera_model
camera projection type (pinhole / omni)
intrinsics
vector containing the intrinsic parameters for the given projection type. elements are as follows:
pinhole: [fu fv pu pv]
omni: [xi fu fv pu pv]
ds: [xi alpha fu fv pu pv]
eucm: [alpha beta fu fv pu pv]
see Supported models for more information
distortion_model
lens distortion type (radtan / equidistant)
distortion_coeffs
parameter vector for the distortion model
see Supported models for more information
T_cn_cnm1
camera extrinsic transformation, always with respect to the last camera in the chain
(e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to cam1 coordinates)
T_cam_imu
IMU extrinsics: transformation from IMU to camera coordinates (T_c_i)
timeshift_cam_imu
timeshift between camera and IMU timestamps in seconds (t_imu = t_cam + shift)
rostopic
topic of the camera’s image stream
resolution
camera resolution [width,height]

Kalibr 标定

1. 标定 Camera

参考：Multiple camera calibration
mynt相机发布topic

source wrappers/ros/devel/setup.bash
roslaunch mynt_eye_ros_wrapper display.launch

数据采集
通过 ROS 发布 image 消息的节点, 我们只需要使用 rosbag record 工具将拍摄到的标定板图像制作成 bag 文件。

建议在捕获校准数据时将摄像机流的频率降低到4 Hz左右。这减少了数据集中的冗余信息，从而降低了校准的运行时间。

ROS 提供了改变 topic 发布频率的节点throttle, 指令如下 :

rosrun topic_tools throttle messages /mynteye/left/image_raw 4.0 

rosrun topic_tools throttle messages /mynteye/right/image_raw 4.0 

rosbag record -O stereo_calibra.bag 

标定

集好数据集以后, 就可以用已经安装好的 Kalibr 标定双目相机了, 指令如下:

标定方式：

kalibr_calibrate_cameras --bag [filename.bag] --topics [TOPIC_0 ... TOPIC_N] --models [MODEL_0 ... MODEL_N] --target [target.yaml]

• bag filename.bag：包含数据的ROS包

• topics TOPIC_0 … TOPIC_N：相机节点

• models MODEL_0 … MODEL_N：list of camera/distortion models to be fitted
  (see Supported models)

• target target.yaml：the calibration target configuration

采集好数据集以后, 就可以用已经安装好的 Kalibr 标定双目相机了, 指令如下:

source ros_ws/kalibr/devel/setup.bash
kalibr_calibrate_cameras --bag /home/xiaohu/bagfiles/stereo_calibra.bag  --topics /mynteye/left/image_raw /mynteye/right/image_raw --models pinhole-radtan pinhole-radtan --target /home/xiaohu/bagfiles/april_6x6_80x80cm_A0.yaml 

输出cam_chain.yaml

2. 标定 Camera-IMU

参考：Camera-IMU calibration
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]

camera_model//相机模型
camera projection type (pinhole / omni)
intrinsics//相机内参
vector containing the intrinsic parameters for the given projection type. elements are as follows:
pinhole: [fu fv pu pv]
omni: [xi fu fv pu pv]
distortion_model//畸变模型
lens distortion type (radtan / equidistant)
distortion_coeffs//畸变参数
parameter vector for the distortion model
T_cn_cnm1//左右摄像头的相对位姿
camera extrinsic transformation, always with respect to the last camera in the chain
(e.g. cam1: T_cn_cnm1 = T_c1_c0, takes cam0 to cam1 coordinates)
timeshift_cam_imu//在捕捉数据时，imu数据和图像时间偏移
timeshift between camera and IMU timestamps in seconds (t_imu = t_cam + shift)
rostopic
topic of the camera’s image stream
resolution
camera resolution [width,height]

#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 之间的外参数了. 这次采用同样的标定板, 采集数据的最佳频率为图像20Hz, IMU 200 Hz 。

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

注意事项：

• 采集数据的起始和结束阶段注意别晃动太大，如从桌子上拿起或者放下。如果有这样的动作，在标定阶段应该跳过bag数据集的首尾的数据.
• 采集数据的时候应该给imu各个轴足够的激励，如先依次绕各个轴运动，运动完后来个在空中画8字之类的操作，当然也要注意别运动太剧烈，图像都模糊了。

标定

 kalibr_calibrate_imu_camera --target /home/xiaohu/bagfiles/april_6x6_80x80cm_A0.yaml --cam /home/xiaohu/bagfiles/camchain-stereo_calibra.yaml --imu /home/xiaohu/bagfiles/imu_mynteye.yaml --bag /home/xiaohu/bagfiles/stereo_imu_calibra.bag --bag-from-to 2 37