【无人机自主导航11 自主导航】硬件篇-相机及IMU联合标定(MATLAB+Kalibr)

发布于 2021-05-04  630 次阅读


在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图。这是因为这两个传感器之间具有互补性:相机在快速运动、光照改变等情况下容易失效。而IMU能够高频地获得机器人内部的运动信息,并且不受周围环境的影响,从而弥补相机的不足;同时,相机能够获得丰富的环境信息,通过视觉匹配完成回环检测与回环校正,从而有效地修正IMU的累计漂移误差。

file

一、相机与IMU外参

足够准确的相机与IMU外参是实现相机与IMU融合的定位与建图的前提。相机与IMU之间的外参包括两部分:

(1)相机与IMU之间的相对位姿
如下图所示,相机与IMU之间的相对位姿值的是相机坐标系和IMU坐标系之间的变换,包括相对旋转角和相对平移量。

图2.png

相机坐标系坐标和IMU坐标系坐标之间满足如下变换关系:

file
将上式展开可以得到分别得到相机坐标系和IMU坐标系之间旋转角和平移量的变换关系:

file

(2)相机与IMU之间的时间差
由于触发延时、传输延时的存在,传感器的采样时间和时间戳的时间不匹配,如下图所示,从而导致相机和IMU之间存在时间差td。

file

td用公式表示为:

公式3.png

将相机的时间戳平移td后,相机与IMU之间实现了同步。

二、为什么在线标定?

如果想投入到实际应用中,我们就需要使用自己的传感器。对于低成本、自己组装的相机与IMU传感器组合,相机与IMU之间的外参是未知的,这就需要我们对相机与IMU之间的外参进行标定。

对于相机与IMU之间的相对位姿,传统的标定方法往往采用离线的形式,需要手持标定板进行一系列操作,费时费力。对于相机与IMU之间的时间差,由于每次运行时间差都不相同,所以只能依靠在线标定的方法。所谓在线标定方法,指的是在系统运行之初或者系统运行过程中完成标定,这种方法解放了双手,也能够保证足够的精度。

四、相对位姿在线标定方法 VINS-Fusion基础上的在线标定
港科大沈劭劼实验室提出了在初始化的同时对相机与IMU之间的外参进行标定的方法[1]。流程如下图所示。首先进行相机与IMU之间相对旋转角的标定,标定完成之后进行初始化和相机与IMU之间相对平移量的标定。标定和初始化完成后进行视觉惯性联合的状态估计,并且把标定量也当做待估计的状态放入联合优化中。此在线标定方法被应用于VINS[2]中。代码实现包含于VINS中,VINS的源码已上传至github:https://github.com/HKUST-Aerial-Robotics/VINS-Mono

file

三、张正友标定法

四、MATLAB标定(推荐)

打开应用程序,找到Camera Calibration工具箱按照软件提示操作即可。

采集拍摄棋盘图的方法有:

(一)手动保存拍摄影像(单镜头可用)

运行realsenser-viewer即可实现实时查看手动保存。

(二)rosbag(各类影像均适用)

手动方式效率过低且采集过程对于双目而言达不到精度要求,无法保证左右影像能够处于同一拍摄时间,经过实验证明,手动保存左右影像误差达到1~5px,误差过大标定精度不足。

推荐采用录制rosbag,随后采用如下代码直接提取拍摄的棋盘影像,根据提取出的影像进行筛选即可。


# coding:utf-8
#!/usr/bin/python

# Extract images from a bag file.
# OpenCV3.0 +
#PKG = 'beginner_tutorials'
import roslib;   #roslib.load_manifest(PKG)
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

# Reading bag filename from command line or roslaunch parameter.
#import os
#import sys

rgb_path = '/home/ganahe/D435i/left/'   #已经建立好的存储rgb彩色图文件的目录
depth_path= '/home/ganahe/D435i/right/' # 已经建立好的存储深度图文件的目录

class ImageCreator():
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/home/ganahe/D435i/d435i_infra_calibr.bag', 'r') as bag:  #要读取的bag文件;
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/infra2/image_rect_raw": #图像的topic;
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8") #"16UC1"depth
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位,可根据精确度需要修改;
                        image_name = timestr+ ".png" #图像命名:时间戳.png
                        cv2.imwrite(depth_path + image_name, cv_image)  #保存;

if __name__ == '__main__':
    #rospy.init_node(PKG)
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

结果:
用于双目提取测试:

├── left
│   ├── 1620371654.657641.png
│   ├── 1620371654.690977.png
│   ├── 1620371654.724313.png
│   ├── 1620371654.757649.png
│   ├── 1620371654.790985.png
│   ├── 1620371654.824321.png
│   ├── 1620371654.857656.png
│   ├── 1620371654.890992.png
├── right
│   ├── 1620371654.657641.png
│   ├── 1620371654.690977.png
│   ├──1620371654.724313.png
│   ├── 1620371654.757649.png
│   ├──1620371654.790985.png
│   ├── 1620371654.824321.png
│   ├── 1620371654.857656.png
│   ├── 1620371654.890992.png

可以根据时间戳找到对应的双目左右影像。

双目标定

步骤:

  • 命令输入stereoCameraCalibrator或在工具箱查找;

(三)标定成效

从整理好的文档取出展示:

file

彩色影像采用手动提取的方式,效率较低,采集到18张后便失去动力。

file

双目采用rosbag录制,随后提取的方式,提取到的帧完全是手动提取的好几倍,通过去除拍摄效果不佳的左右影像对,最终标定平均误差能够在0.3px。

五、 Kalibr

(一)安装配置

由于此前已经安装过ROS,网上大多另见工作空间,没必要,直接放在catkin_ws中编译即可。

(二)标定板配置文件及标定板类型

标定板文件:target.yaml
Kalibr支持三种标定板,分别是Aprilgrid、Checkerboard和Circlegrid。
参数比较简单:见https://github.com/ethz-asl/kalibr/wiki/calibration-targets
target.yaml文件模板:
1)对于april板.yaml文件格式如:

  target_type: 'aprilgrid'   #gridtype
  tagCols: 6      #number of apriltags
  tagRows: 6       #number of apriltags
  tagSize: 0.088     #size of apriltag, edge to edge [m]
  tagSpacing: 0.3    #ratio of space between tags to tagSize,实际上就是小黑块与大黑块的边长之比 example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25

2)对于checkboard板.yaml文件格式如:

  target_type: 'checkerboard'     #gridtype
  targetCols: 6  #number of internal chessboard corners
  targetRows: 7   #number of internal chessboard corners
  rowSpacingMeters: 0.06        #size of one chessboard square [m]
  colSpacingMeters: 0.06      #size of one chessboard square [m]

3)对于circle板.yaml文件格式如:

  target_type: 'circlegrid'     #gridtype
  targetCols: 6       #number of circles (cols)
  targetRows: 7      #number of circles (rows)
  spacingMeters: 0.02     #distance between circles [m]
  asymmetricGrid: False   #use asymmetric grid (opencv) [bool]

(三)标定参数说明

3.1 Camera models

Kalibr支持以下投影成像模型:

针孔相机模型(针孔)
(内在矢量:[fu fv pu pv])
全向摄像机模型(omni)
(内在矢量:[xi fu fv pu pv])
双球摄影机模型(ds)
(内在矢量:[xi alpha fu fv pu pv])
扩展统一摄像机模型(eucm)
(内在矢量:[alpha beta fu fv pu pv])

3.2 intrinsic vector包含模型的所有参数:

fu,fv:焦距
pu,pv:主要点
xi:镜像参数(仅全向)
xi,alpha:双球模型参数(仅ds)
alpha,beta:扩展的统一模型参数(仅eucm)

### 3.3  Distortion models
Kalibr支持以下畸变模型:

```yaml
径向切线(radtan)
(畸变系数:[k1 k2 r1 r2])
等距(equi)
(畸变系数:[k1 k2 k3 k4])
fov(fov)
(畸变系数:[w])
无(无)
(distortion_coeffs:[])</code></pre>
<h2>RGB镜头标定</h2>
<h4>(一)Topic重发布</h4>
<p>将传感器的Topic发送速率“转换”一下,因为在Kalibr的官方文档中,对于相机标定,推荐的速率是每秒4帧左右(FPS=4),而D435i默认是30帧每秒,所以需要转换一下。转换也非常简单,使用ROS自带的topic_tools即可。新打开一个终端,然后在其中输入</p>
<pre><code class="language-bash">rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /cam0/image_raw</code></pre>
<h4>(二)rosbag录制</h4>
<p>将打印出来的标定板拿好,然后在相机前面来回运动即可。如何运动,简单来说就是先绕相机的各自三个轴,每个轴旋转三次,然后再沿着相机的三个轴,每个轴平移三次,基本就可以了。录包我们使用ROS自带的工具,新打开一个终端,输入如下内容</p>
<pre><code class="language-bash">rosbag record /cam0/image_raw
</code></pre>
<h4>(三)配置Kalibr的标定板参数</h4>
<p>根据自己采用的标定板设置,详见上文。</p>
<h4>(四)标定</h4>
<pre><code class="language-bash">kalibr_calibrate_cameras --target april_6x6.yaml --bag 2020-09-25-18-07-59.bag --bag-from-to 5 80 --models pinhole-radtan --topics /cam0/image_raw</code></pre>
<p>Python安装igraph包加载出错问题</p>
<pre><code class="language-bash">DeprecationWarning: To avoid name collision with the igraph project, this visualization library has been renamed to ‘jgraph’. Please upgrade when convenient.</code></pre>
<p>通过pip list发现Python安装的有igraph包有两个:igraph、python-igraph。实际上,只需要安装python-igraph。
2, 卸载包igraph
sudo pip3 uninstall igraph
3,重新安装包python-igraph
安装之前的版本:</p>
<p>sudo apt-get install python-igraph
完成以上操作,可验证Python中能否成功加载igraph包。</p>
<p>from igraph import *</p>
<pre><code class="language-bash">Exception in thread block: [aslam::Exception] /home/ganahe/catkin_ws/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.61918e+09 <= 1.61918e+09 < 1.61918e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1619179362.373183]: Optimization failed!
Traceback (most recent call last):
  File "/home/ganahe/catkin_ws/devel/bin/kalibr_calibrate_imu_camera", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 245, in <module>
    main()
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 208, in main
    iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 179, in optimize
    raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
</code></pre>
<h2>联合标定</h2>
<pre><code class="language-bash">ganahe@ganahe-Nitro-AN515-51:~/catkin_ws$ tree -L 1
.
├── build
├── camchain-color_camera.yaml
├── d435iCheckerboard.yaml
├── devel
├── imu_image.bag
├── imu.yaml
├── install
├── scripts
└── src
</code></pre>
<p>`RuntimeError: Optimization failed!`</p>
<p>当执行到开始联合标定时,也就是如下指令:</p>
<pre><code class="language-bash">kalibr_calibrate_imu_camera --target ./checkerboard_11x8_25x25cm.yaml --cam ./camchain-.camd435i.yaml --imu ./imu_self.yaml --bag ./imu_435i_move.bag --bag-from-to 10 90
1</code></pre>
<p>当初直接执行这种指令,在小觅相机下标定成功,没报错。后面用realsense单目+独立IMU标定就开始报如下错误:</p>
<pre><code class="language-bash">Optimization problem initialized with 7866 design variables and 9486 error terms
The Jacobian matrix is 28304 x 35379
[0.0]: J: 1.59093e+07
Exception in thread block: [aslam::Exception] /home/ganahe/catkin_ws/src/Kalibr/aslam_nonparametric_estimation/aslam_splines/src/BSplineExpressions.cpp:447: toTransformationMatrixImplementation() assert(_bufferTmin <= _time.toScalar() < _bufferTmax) failed [1.61918e+09 <= 1.61918e+09 < 1.61918e+09]: Spline Coefficient Buffer Exceeded. Set larger buffer margins!
[ERROR] [1619179828.521786]: Optimization failed!
Traceback (most recent call last):
  File "/home/ganahe/catkin_ws/devel/bin/kalibr_calibrate_imu_camera", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 246, in <module>
    main()
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 209, in main
    iCal.optimize(maxIterations=parsed.max_iter, recoverCov=parsed.recover_cov)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 179, in optimize
    raise RuntimeError("Optimization failed!")
RuntimeError: Optimization failed!
</code></pre>
<p>一开始查到的资料都说是数据录的有问题,但是我更换相机和imu反复试过多次发现并没有解决问题。而且在这个报错上面可以成功读取自己对应的imu信息和单目信息。所以说数据大概率也没问题。最终让我在kalibr的issues里找到了解决问题办法。
3.解决方法:</p>
<h4>(一)指令修正</h4>
<p>提示:“Spline Coefficient Buffer Exceeded. Set larger buffer margins”</p>
<p>在标定命令最后添加 --timeoffset-padding 3</p>
<h4>(一)直接修改源代码</h4>
<p>请打开kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera.py文件,搜索timeOffsetPadding,修改到3左右即可,太大会极大占用内存导致卡死,仅供参考:</p>
<p>```python
print "Building the problem"</p>
<h1>print(parsed.timeoffset_padding)    #输出看一眼timeOffsetPadding的值</h1>
<pre><code>iCal.buildProblem(splineOrder=6, 
                  poseKnotsPerSecond=100, 
                  biasKnotsPerSecond=50, 
                  doPoseMotionError=False,
                  doBiasMotionError=True,
                  blakeZisserCam=-1,
                  huberAccel=-1,
                  huberGyro=-1,
                  noTimeCalibration=parsed.no_time,
                  noChainExtrinsics=(not parsed.recompute_chain_extrinsics),
                  maxIterations=parsed.max_iter,
                  #timeOffsetPadding=parsed.timeoffset_padding,     #原来的代码
                  timeOffsetPadding=0.3,     #修改后
                  verbose = parsed.verbose)</code></pre>
<pre><code>
```bash
top - 20:50:33 up  1:49,  1 user,  load average: 1.11, 1.23, 1.37
Tasks: 304 total,   4 running, 245 sleeping,   3 stopped,   1 zombie
%Cpu(s): 58.3 us,  8.3 sy,  0.0 ni, 33.3 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
KiB Mem :  8032864 total,   418604 free,  4608520 used,  3005740 buff/cache
KiB Swap:  8000508 total,  7974128 free,    26380 used.  2829908 avail Mem 

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU %MEM     TIME+ COMMAND                                                                              
10340 ganahe    20   0 3144376 1.379g 236404 R 100.0 18.0  38:50.17 kalibr_calibrat                                                                      
 1652 ganahe    20   0 4240872 430772 113200 R  66.7  5.4   7:36.29 gnome-shell                                                                          
 1113 root      20   0  923268 162692 101340 S  33.3  2.0   9:03.46 Xorg                                                                                 
 2500 ganahe    20   0  757704  55100  30860 S  33.3  0.7   0:35.56 gnome-terminal-                                                                      
    1 root      20   0  225664   9260   6592 S   0.0  0.1   0:01.36 systemd                                                                              
    2 root      20   0       0      0      0 S   0.0  0.0   0:00.00 kthreadd                                                                             
    3 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 rcu_gp                                                                               
    4 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 rcu_par_gp                                                                           
    6 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 kworker/0:0H-kb                                                                      
    9 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 mm_percpu_wq                                                                         
   10 root      20   0       0      0      0 S   0.0  0.0   0:00.19 ksoftirqd/0                                                                          
   11 root      20   0       0      0      0 I   0.0  0.0   0:03.61 rcu_sched                                                                            
   12 root      rt   0       0      0      0 S   0.0  0.0   0:00.02 migration/0                                                                          
   13 root     -51   0       0      0      0 S   0.0  0.0   0:00.00 idle_inject/0                                                                        
   14 root      20   0       0      0      0 S   0.0  0.0   0:00.00 cpuhp/0                                                                              
   15 root      20   0       0      0      0 S   0.0  0.0   0:00.00 cpuhp/1                                                                              
   16 root     -51   0       0      0      0 S   0.0  0.0   0:00.00 idle_inject/1                                                                        
   17 root      rt   0       0      0      0 S   0.0  0.0   0:00.13 migration/1                                                                          
   18 root      20   0       0      0      0 S   0.0  0.0   0:00.92 ksoftirqd/1                                                                          
   20 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 kworker/1:0H-kb                                                                      
   21 root      20   0       0      0      0 S   0.0  0.0   0:00.00 cpuhp/2                                                                              
   22 root     -51   0       0      0      0 S   0.0  0.0   0:00.00 idle_inject/2                                                                        
   23 root      rt   0       0      0      0 S   0.0  0.0   0:00.13 migration/2                                                                          
   24 root      20   0       0      0      0 S   0.0  0.0   0:00.14 ksoftirqd/2                                                                          
   25 root      20   0       0      0      0 I   0.0  0.0   0:00.25 kworker/2:0-eve                                                                      
   26 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 kworker/2:0H-kb                                                                      
   27 root      20   0       0      0      0 S   0.0  0.0   0:00.00 cpuhp/3                                                                              
   28 root     -51   0       0      0      0 S   0.0  0.0   0:00.00 idle_inject/3                                                                        
   29 root      rt   0       0      0      0 S   0.0  0.0   0:00.14 migration/3                                                                          
   30 root      20   0       0      0      0 R   0.0  0.0   0:00.17 ksoftirqd/3                                                                          
   32 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 kworker/3:0H-kb                                                                      
   33 root      20   0       0      0      0 S   0.0  0.0   0:00.00 kdevtmpfs                                                                            
   34 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 netns                                                                                
   35 root      20   0       0      0      0 S   0.0  0.0   0:00.00 rcu_tasks_kthre                                                                      
   36 root      20   0       0      0      0 S   0.0  0.0   0:00.00 kauditd                                                                              
   37 root      20   0       0      0      0 S   0.0  0.0   0:00.00 khungtaskd                                                                           
   38 root      20   0       0      0      0 S   0.0  0.0   0:00.00 oom_reaper                                                                           
   39 root       0 -20       0      0      0 I   0.0  0.0   0:00.00 writeback                                                                            
   40 root      20   0       0      0      0 S   0.0  0.0   0:00.00 kcompactd0 

错误2:

ganahe@ganahe-Nitro-AN515-51:~/catkin_ws$ kalibr_calibrate_imu_camera --target d435iCheckerboard.yaml --cam camchain-newRGBCamera.yaml --imu imu.yaml --bag imu_camera.bag --bag-from-to 5 25
importing libraries
Initializing IMUs:
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.00186 
    Noise density (discrete): 0.0263043722601 
    Random walk: 0.000433
  Gyroscope:
    Noise density: 0.000187
    Noise density (discrete): 0.00264457936164 
    Random walk: 2.66e-05
Initializing imu rosbag dataset reader:
    Dataset:          imu_camera.bag
    Topic:            /imu0
bagstart 1619260323.2
baglength 114.318610668
[ WARN] [1619349374.336197]: BagImuDatasetReader: truncated 12215 / 14803 messages.
    Number of messages: 14803
Reading IMU data (/imu0)
  Read 2588 imu readings over 20.0 seconds                   
Initializing calibration target:
  Type: checkerboard
  Rows
    Count: 5
    Distance: 0.028 [m]
  Cols
    Count: 4
    Distance: 0.028 [m]
Initializing camera chain:
Camera chain - cam0:
  Camera model: pinhole
  Focal length: [537.349029104188, 537.44026827034]
  Principal point: [334.6350548809914, 251.29656609968728]
  Distortion model: radtan
  Distortion coefficients: [-0.05779558634089584, 0.9600335120176468, -0.007181471684482495, 0.000664325240977249]
  baseline: no data available
Initializing camera rosbag dataset reader:
    Dataset:          imu_camera.bag
    Topic:            /cam0/image_raw
[ WARN] [1619349375.584788]: BagImageDatasetReader: truncated 1407 / 1707 images.
    Number of images: 1707
Extracting calibration target corners
  Progress 300 / 300     Time remaining:                        
[FATAL] [1619349392.145080]: No corners could be extracted for camera /cam0/image_raw! Check the calibration target configuration and dataset.

Building the problem
    Spline order: 6
    Pose knots per second: 100
    Do pose motion regularization: False
        xddot translation variance: 1000000.000000
        xddot rotation variance: 100000.000000
    Bias knots per second: 50
    Do bias motion regularization: True
    Blake-Zisserman on reprojection errors -1
    Acceleration Huber width (sigma): -1.000000
    Gyroscope Huber width (sigma): -1.000000
    Do time calibration: True
    Max iterations: 30
    Time offset padding: 0.030000
Estimating time shift camera to imu:
Traceback (most recent call last):
  File "/home/ganahe/catkin_ws/devel/bin/kalibr_calibrate_imu_camera", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 246, in <module>
    main()
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera", line 200, in main
    verbose = parsed.verbose)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccCalibrator.py", line 105, in buildProblem
    cam.findTimeshiftCameraImuPrior(self.ImuList[0], verbose)
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py", line 219, in findTimeshiftCameraImuPrior
    poseSpline = self.initPoseSplineFromCamera( timeOffsetPadding=0.0 )
  File "/home/ganahe/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_imu_camera_calibration/IccSensors.py", line 288, in initPoseSplineFromCamera
    times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times, times[-1] + (timeOffsetPadding * 2.0)))
IndexError: index 0 is out of bounds for axis 0 with size 0

解决方案:适当增大时间间隙

双目相机标定

红外相机携带有结构光投影点影响标定,可以通过新打开终端,运行

rosrun rqt_reconfigure rqt_reconfigure

打开后将camera->stereo_module中的emitter_enabled设置为off(0) 将其关闭。
file

开始标定,执行:

kalibr_calibrate_cameras --target checkboard.yaml --bag  stereo_1280_720.bag --models pinhole-equi pinhole-equi --topics /infra_right /infra_left --bag-from-to 5 45

输出:

dji@ganahe-Nitro-AN515-51:~/TianqueROS/Tools/camera_calibration/Kalibr_calibration_scripts$ kalibr_calibrate_cameras --target checkboard.yaml --bag  stereo_1280_720.bag --models pinhole-equi pinhole-equi --topics /infra_right /infra_left --bag-from-to 5 45
------ GanAHE calibration -------
1.Monocular; 2.Stereo; 3.more_camera. 4.imu
--[INFO] Please input camera type by code: >> 2
--[INFO] Stereo calibrating..
importing libraries
Initializing cam0:
    Camera model:     pinhole-equi
    Dataset:          stereo_1280_720.bag
    Topic:            /infra_left
[ WARN] [1622045891.694735]: BagImageDatasetReader: truncated 198 / 348 images.
    Number of images: 150
Extracting calibration target corners
  Extracted corners for 4 images (of 150 images)                              
    Projection initialized to: [ 151.07119141  153.00506353  578.5916559   352.47935061]
    Distortion initialized to: [ 0.46196329 -0.47922491  0.89345569 -0.34662788]
Initializing cam1:
    Camera model:     pinhole-equi
    Dataset:          stereo_1280_720.bag
    Topic:            /infra_right
[ WARN] [1622045929.077886]: BagImageDatasetReader: truncated 197 / 347 images.
    Number of images: 150
Extracting calibration target corners
  Extracted corners for 1 images (of 150 images)                              
    Projection initialized to: [ 588.73994051  588.72536871  639.50054321  359.50065317]
    Distortion initialized to: [ 0.37205771  0.06825367  0.01090866  0.00174418]
...

如报错:

Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
Traceback (most recent call last):
  File "/home/dji/catkin_ws/devel/bin/kalibr_calibrate_cameras", line 15, in <module>
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/dji/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 447, in <module>
    main()
  File "/home/dji/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 204, in main
    graph.plotGraph()
  File "/home/dji/catkin_ws/src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py", line 311, in plotGraph
    edge_label=self.G.es["weight"],
KeyError: 'Attribute does not exist'

两个相机时间不同步导致的,需要调整参数:--approx-sync,根据错误信息提供的时间轴设置:

kalibr_calibrate_cameras --target checkboard.yaml --bag  stereo_1280_720.bag --models pinhole-equi pinhole-equi --topics /infra_right /infra_left --bag-from-to 5 85 --approx-sync 0.5

参考

参考:

[1] Yang Z , Shen S . Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration[J]. IEEE Transactions on Automation Science and Engineering, 2016, 14(1):1-13.

[2] Tong Q, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. IEEE Transactions on Robotics, 2017, PP(99): 1-17.

[3] Huang W, Liu H, Ieee: Online Initialization and Automatic Camera-IMU Extrinsic Calibration for Monocular Visual-Inertial SLAM[C], 2018 Ieee International Conference on Robotics and Automation, 2018: 5182-5189.

[4] Qin T , Shen S . Online Temporal Calibration for Monocular Visual-Inertial Systems[J]. 2018.

[5] Liu J , Gao W , Hu Z, Ieee: Online Temporal Calibration of Camera and IMU using Nonlinear Optimization[C], 2018 Ieee International Conference on Pattern Recognition, 2018: 1761-1766.
[6]realsenseD435i imu+双目标定
[7]shanmo/Kalibr-camera-calibration
[8]J. Kannala and S. Brandt (2006). A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 8, pp. 1335-1340