【ORB_SLAM2源码解读】针孔鱼眼相机camera和惯导IMU标定方法汇总以及详细标定过程

Camera标定原理

Camera标定原理一般都是张正友标定原理，流程是两个camera通过ROS将图片image以话题的形式发布，ROS再调用标定程序接收图片，标定程序输出两个Camera的内外参数。有了Camera的内外参数，我们就可以对Camera采集到的image进行畸变矫正，然后进一步应用SLAM的原理获得深度信息，进而建图，导航等。

张正友标定法的步骤
1、打印一张模板并贴在一个平面上；
2、从不同角度拍摄若干张模板图像；
3、检测出图像中的特征点；
4、求出摄像机的外参数（单应性矩阵）和内参数（最大似然估计） ；
5、求出畸变系数；
6、优化求精。

建立相机成像几何模型计算机视觉的首要任务就是要通过拍摄到的图像信息获取到物体在真实三维世界里相对应的信息，于是，建立物体从三维世界映射到相机成像平面这一过程中的几何模型就显得尤为重要，而这一过程最关键的部分就是要得到相机的内参和外参。相机标定的参数可以实现三维世界点和二维图像点之间一一对应（映射）。我们所处的世界是三维的，而照片是二维的，这样我们可以把相机认为是一个函数，输入量是一个场景，输出量是一幅灰度图。这个从三维到二维的过程的函数是不可逆的。

相机标定找一个合适的数学模型，求出这个模型的参数，这样我们能够近似这个三维到二维的过程，使这个三维到二维的过程的函数找到反函数。建立相机成像几何模型并矫正透镜畸变，相机标定的目的之一是为了建立物体从三维世界到成像平面上各坐标点的对应关系。

世界坐标系 Xw、Yw、Zw
相机坐标系 Xc、Yc、Zc
图像坐标系 x、y
像素坐标系 u、v

世界坐标系(world coordinate system)用户定义的三维世界的坐标系，为了描述目标物在真实世界里的位置而被引入，单位为m。由于像机可安放在环境中的任意位置，在环境中选择一个基准坐标系来描述像机的位置，并用它描述环境中任何物体的位置，该坐标系称为世界坐标系。像机坐标系与世界坐标系之间的关系可以用旋转矩阵与平移向量来描述。与此相关的是图像坐标系和像机坐标系，所以世界坐标系的原点在任意位置都可以。

相机坐标系(camera coordinate system)在相机上建立的坐标系，为了从相机的角度描述物体位置而定义，作为沟通世界坐标系、图像和像素坐标系的中间一环，单位为m。相机坐标系原点在光心，这就解释了为什么ORB_SLAM2要求光心。

图像坐标系(image coordinate system)为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标，单位为m。图像坐标系原点与焦点重合，理论上焦点位于焦平面，焦平面是最理想的成像平面。

像素坐标系(pixel coordinate system)为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系，单位为个（像素数目）。像素坐标系原点位于图中左上角（之所以这么定义，目的是从存储信息的首地址开始读写）。

单应性变换（Homography）可以简单的理解为用来描述物体在世界坐标系和像素坐标系之间的位置映射关系，对应的变换矩阵称为单应性矩阵。实现了棋盘格和棋盘格拍摄的图片的对应。单应矩阵包含了相机的内参Ｋ和外参Ｒ。求解Ｈ可以解出Ｒ和ｔ(通过一系列优化方法)。非线性优化问题通常用Levenberg-Marquardt（LM）算法进行迭代求解，优化算法部分的顺序是：梯度下降、牛顿法、高斯牛顿法、LM（Levenberg-Marquardt）法。

相机的外参决定传感器和外部某个坐标系的转换关系，比如姿态参数（旋转和平移6自由度）。要还原摄像头成像的物体在真实世界的位置就需要知道世界中的物体到计算机图像平面是如何变换的，为了搞清楚这种变换关系，求解内外参数矩阵。从世界坐标系到相机坐标系 借助相机的外参变换矩阵 假设已经知道变换矩阵。

相机的内参决定传感器内部的映射关系，比如摄像头的焦距，偏心和像素横纵比（+畸变系数）。

相机的畸变参数摄像机的透视投影有个很大的问题－－畸变。求解畸变系数，然后用于图像矫正。矫正透镜畸变我们最开始接触到的成像方面的知识应该是有关小孔成像的，但是由于这种成像方式只有小孔部分能透过光线就会导致物体的成像亮度很低，于是聪明的人类发明了透镜。虽然亮度问题解决了，但是新的问题又来了，由于透镜的制造工艺，会使成像产生多种形式的畸变，于是为了去除畸变（使成像后的图像与真实世界的景象保持一致），人们计算并利用畸变系数来矫正这种像差。虽然理论上可以设计出不产生畸变的透镜，但其制造工艺相对于球面透镜会复杂很多，所以相对于复杂且高成本的制造工艺，人们更喜欢用数学来解决问题。

对于针孔相机从相机坐标系到理想图像坐标系不考虑畸变借助缩放 考虑畸变借助缩放和5个畸变参数（k1、k2、k3、p1和p2 ）
成像过程如下图所示：针孔面（相机坐标系）在图像平面（图像坐标系）和物点平面（棋盘平面）之间，所成图像为倒立实像。

但是为了在数学上更方便描述，我们将相机坐标系和图像坐标系位置对调，变成如下图所示的布置方式（没有实际的物理意义，只是方便计算）

此时，假设相机坐标系中有一点M，则在理想图像坐标系下（无畸变）的成像点P的坐标为（可由相似三角形原则得出）：
从实际图像坐标系到像素坐标系 内参矩阵

像素坐标系和世界坐标系下的坐标映射关系：

其中u、v表示像素坐标系中的坐标，s表示尺度因子，fx、fy、u0、v0、γ（由于制造误差产生的两个坐标轴偏斜参数，通常很小）表示5个相机内参，R,t表示相机外参，Xw、Yw、Zw（假设标定棋盘位于世界坐标系中Zw=0的平面）表示世界坐标系中的坐标。

我们在这里引入一个新的概念：单应性（Homography）变换。可以简单的理解为它用来描述物体在世界坐标系和像素坐标系之间的位置映射关系。对应的变换矩阵称为单应性矩阵。在上述式子中，单应性矩阵定义为：

其中，M是内参矩阵

从单应矩阵定义式子来看，它同时包含了相机内参和外参。在进一步介绍相机标定知识之前，我们重点来了解一下单应性，这有助于深入理解相机标定。因为在计算机视觉领域，单应性是一个非常重要的概念。

dx和dy表示：x方向和y方向的一个像素分别占多少长度单位，即一个像素代表的实际物理值的大小，其是实现图像物理坐标系与像素坐标系转换的关键。

u0，v0表示图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数。

相机的外参数是6个：三个轴的旋转参数分别为（ω、δ、 θ）,然后把每个轴的33旋转矩阵进行组合（即先矩阵之间相乘），得到集合三个轴旋转信息的R，其大小还是33；T的三个轴的平移参数（Tx、Ty、Tz）。

畸变参数是：k1,k2,k3径向畸变系数，p1,p2是切向畸变系数。径向畸变发生在相机坐标系转图像物理坐标系的过程中。而切向畸变是发生在相机制作过程，其是由于感光元平面跟透镜不平行。

为更好的理解，现以NiKon D700相机为例进行求解其内参数矩阵：
焦距 f = 35mm 最高分辨率：4256×2832 传感器尺寸：36.0×23.9 mm
根据以上定义可以有：
u0= 4256/2 = 2128 v0= 2832/2 = 1416 dx = 36.0/4256 dy = 23.9/2832
fx = f/dx = 4137.8 fy = f/dy = 4147.3

鱼眼模型

https://zhuanlan.zhihu.com/p/71234078

VINS-mono的鱼眼相机模型Unified Camera Model

发布图像

uvc_cam

ROS 教程之 vision : 用各种摄像头获取图像

usb_cam

usb_cam是ROS的功能包Package

mkdir -p usb_cam_ws/src
cd usb_cam_ws/src
git clone https://github.com/ros-drivers/usb_cam.git
cd usb_cam_ws/
catkin_make

检验一下是否安装成功
source devel/setup.bash 
roslaunch usb_cam usb_cam-test.launch

查看摄像头
ls /dev/video*
video0 video1 代表不同的摄像头更改可以打开指定的摄像头
gedit /usb_cam_ws/src/usb_cam/launch/usb_cam-test.launch
<param name="video_device" value="/dev/video1" />

更多launch文件具体参数含义参考下面文章和图示
ROS Launch 启动文件语法和例子超级详细讲解

检查是否成功发布了图片

rostopic list
/usb_cam/image_raw

image_view查看图像

rosrun image_view image_view image:=/usb_cam/image_raw

用rqt_image_view节点检查

rqt_image_view image:=/usb_cam/image_raw

RVIZ

rviz
增加image然后将Image Topic的值更改为/usb_cam/image_raw

录制数据包

rosbag record -O 21mm /usb_cam/image_raw/compressed
rosbag info 21mm.bag
rosbag play --pause 21mm.bag

安装 kalibr

mkdir -p kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
cd kalibr_ws
catkin_make

可能遇到的问题以以及解决方法

sudo apt install libv4l-dev
sudo apt install python-igraph

ImportError: No module named pyx
sudo apt-get install python-pyx

eta 1m 52s c++: internal compiler error: 已杀死 (program cc1plus)
增加交换空间大小重新编译

如何在Ubuntu 16.04上添加交换空间

标定单目眼相机（针孔模型、鱼眼模型）

ROS

单目：http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration
参考：https://blog.csdn.net/heyijia0327/article/details/43538695

kalibr

准备标定板 公众号"小秋SLAM笔记"可以获取下载好的文件
下载标定板
标定版介绍

Grids
downloaded
created using script

kalibr_create_target_pdf --h

Aprilgrids

简介 https://blog.csdn.net/yjy728/article/details/78524814
downloaded
created using script

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

aprilgrid.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[-]

Checkerboard

checkerboard.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]
Circlegrid

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]

也可以终端生成

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

根据你实际标定版的格式大小，更改下面参数。

yaml文件

# aprilgrid.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]
tagSize: 0.024
#tagSpacing: 0.3          #ratio of space between tags to tagSize
                         #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
tagSpacing: 0.2917

准备用于标定的ros bag数据

rostopic list 

可以查看发布了那些话题

录制数据

rosbag record -O fisheye_data /usb_cam/image_raw

-O参数将数据保存到名为fisheye_data.bag的文件中
rosbag record录制/usb_cam/image_raw话题
按Ctrl+C退出rosbag record录制命令

此时打开rviz添加图像，选择话题可以看到录制的图像，录制标定数据的时候不要快速移动，让标定版尽可能在图像当中，上下左右缓慢移动。

查看录制数据包的信息

rosbag info fisheye_data.bag  

播放录制好的数据包

rosbag play fisheye_data.bag

暂停数据集播放

rosbag play  --pause

开始运行bag时不发布数据，直到你按下空格键它才开始发布，同样你再按空格键rosbag又会暂停发布

标定程序的输入参数

cd kalibr_ws
source devel/setup.bash 
kalibr_calibrate_cameras --bag /home/q/packages/usb_cam_ws/2020-07-21-14-11-52.bag --models eucm-none --topics /usb_cam/image_raw --target /home/q/packages/usb_cam_ws/aprilgrid.yaml

cd kalibr_ws
source devel/setup.bash 
kalibr_calibrate_cameras --bag /home/q/packages/usb_cam_ws/21mm.bag --models pinhole-radtan --topics /usb_cam/image_raw --target /home/q/packages/kalibr_ws/21mm-fisheye-data/aprilgrid-shinan.yaml

支持的相机模型
Supported models
pinhole是针孔相机模型

畸变模型
关注公众号"小秋SLAM笔记"可以获取

可能出现的问题

packages/kalibr_ros/src/kalibr/Schweizer-Messer/sm_python/python/sm/PlotCollection.py:from matplotlib.backends.backend_wxagg import NavigationToolbar2Wx as Toolbar

ImportError: cannot import name NavigationToolbar2Wx 
修改源码　NavigationToolbar2Wx 换成NavigationToolbar2WxAgg 重新编译

ImportError: No module named igraph
sudo apt-get install python2.7-igraph

[ WARN] [1594103515.384115]: getTargetPoseGuess: solvePnP failed with solution: <sm.libsm_python.Transformation object at 0x7f69e359e320>
The quaternion must be a unit vector to represent a rotation

这个问题偶尔出现，暂时忽略

标定结果的参数含义

不用操作就可以，自动保存三个文件
标定评估重投影误差在 0.1~0.2 以内，标定结果较好，如下所示。

图片进行畸变矫正

image_pipeline: camera_calibration | depth_image_proc | image_proc | image_publisher | image_rotate | image_view | stereo_image_proc

检查标定结果
cameracheck.py

http://wiki.ros.org/image_proc

http://wiki.ros.org/stereo_image_proc?distro=kinetic

http://wiki.ros.org/depth_image_proc?distro=kinetic

标定双目相机（针孔模型、鱼眼模型）

ROS

双目：http://wiki.ros.org/camera_calibration/Tutorials/StereoCalibration
参考：https://blog.csdn.net/heyijia0327/article/details/43538695

发布图像

roslaunch stereo_usb_cam_stream_publisher.launch
video_device_right:=/dev/video1
video_device_left:=/dev/video2
image_width:=640
image_height:=480
left_camera_name:=left
right_camera_name:=right
camera_info:=true
camera_info_path:=stereo_camera_info

开始标定

rosrun camera_calibration cameracalibrator.py
–size 8x6
–square 0.108
right:=/stereo/right/image_raw
left:=/stereo/left/image_raw
right_camera:=/stereo/right
left_camera:=/stereo/left
–no-service-check
–approximate=0.1

官方参数

rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 8x6 --square 0.108 right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw right_camera:=/my_stereo/right left_camera:=/my_stereo/left

我的参数

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0513 --no-service-check --approximate=0.1 right:=stereo_camera/right/image_raw left:=stereo_camera/left/image_raw right_camera:=stereo_camera/right left_camera:=stereo_camera/left

标定界面图标含义
x：表示标定板在视野中的左右位置。
y：表示标定板在视野中的上下位置。
size：标定板在占视野的尺寸大小，也可以理解为标定板离摄像头的远近。
skew：标定板在视野中的倾斜位置。

Kalibr

uvc_camera发布双目图像话题

https://github.com/xiaoqiuslambiji/camera_umd

usb_cam发布双目图像话题
https://github.com/xiaoqiuslambiji/ROS_stereo_camera

roslaunch stereo_usb_cam_stream_publisher.launch 
video_device_right:=/dev/video1 
video_device_left:=/dev/video2 
image_width:=640 
image_height:=480 
left_camera_name:=left 
right_camera_name:=right 
camera_info:=true 
camera_info_path:=stereo_camera_info

查看是否有对应摄像头的参数

ls /dev/video*

检查是否成功发布了left and right image_raw图片

rostopic list
/stereo/left/camera_info
/stereo/left/image_raw
/stereo/right/camera_info
/stereo/right/image_raw

开始标定

rosrun camera_calibration cameracalibrator.py 
--size 8x6 
--square 0.108 
right:=/stereo/right/image_raw 
left:=/stereo/left/image_raw 
right_camera:=/stereo/right 
left_camera:=/stereo/left  
--no-service-check 
--approximate=0.1

标定界面图标含义
x：表示标定板在视野中的左右位置。
y：表示标定板在视野中的上下位置。
size：标定板在占视野的尺寸大小，也可以理解为标定板离摄像头的远近。
skew：标定板在视野中的倾斜位置。

realsense D455 双目标定关掉双目散板投射

标定多目相机（针孔模型、鱼眼模型）

Multiple camera calibration

四个相机的标定

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​​

验证标定结果
相机的标定完成之后可以使用caliibration_validator进行标定的验证
原理是对重投影误差进行量化分析
使用相机的标定结果以及标定板的yaml文件

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

降帧率
录制完成，可以用以下命令进行降帧率操作

rosrun topic_tools throttle messages /usb_cam150/image_raw 4.0 /usb_cam

/usb_cam/image_raw 是原topic
/usb_cam 是新tiopic
4.0是帧率
先运行这个命令，然后record这个新的topic，最后播放原包

可能出现的错误

Spline Coefficient Buffer Exceeded. Set larger buffer margins!

在标定命令行添加：–timeoffset-padding　0.05　（代码中默认为0.03）

单IMU标定

imu_utils

安装d435i的sdk和ros warpper
安装realsense的SDK
安装realsense的ros warpper

rs_camera.launch文件默认没有发布imu话题

sudo gedit /opt/ros/kinetic/share/realsense2_camera/launch/rs_camera.launch
<arg name="unite_imu_method"          default=""/>
<arg name="enable_gyro"         default="false"/>
<arg name="enable_accel"        default="false"/>

通过设置参数发布imu话题

roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
    enable_accel:=true

查看话题：rostopic echo /camera/imu

header: 
  seq: 4942
  stamp: 
    secs: 1647847359
    nsecs: 799555779
  frame_id: "camera_imu_optical_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00174532923847
  y: 0.0
  z: -0.00349065847695
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: 0.470719188452
  y: -8.99785895848
  z: 0.770079927684
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

录制imu数据bag进行标定，让IMU静止不动两个小时，录制IMU的bag，Ctrl+C完成录制

roslaunch realsense2_camera rs_camera.launch \
    align_depth:=true \
    unite_imu_method:="linear_interpolation" \
    enable_gyro:=true \
    enable_accel:=true
rosbag record /camera/imu -O d455_imu.bag
[ INFO] [1647847683.100932270]: Subscribing to /camera/imu
[ INFO] [1647847683.103357507]: Recording to d455_imu.bag.

然后安装ceres库，code_utils依赖ceres，由于imu_utils 依赖 code_utils，所以先把code_utils放在工作空间的src下面，进行编译，最后再将imu_utils放到src下面，再编译。

安装Ceres

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev  
git clone https://github.com/ceres-solver/ceres-solver
cd ceres-solver
mkdir build
cd build
cmake ..  
make
sudo make install 
Ceres库的头文件安装在"/usr/local/include/ceres/"目录下
Ceres库的库文件安装在"/usr/local/lib/"目录下

先安装code_utils

https://github.com/gaowenliang/code_utils
Eigen3Config.cmake
eigen3-config.cmake
set(EIGEN3_INCLUDE_DIRS /usr/local/include/eigen3)
include_directories(${EIGEN3_INCLUDE_DIRS})

fatal error: backward.hpp: No such file or directory
sumpixel_test.cpp，
#include "backward.hpp"
#include "code_utils/backward.hpp"

fatal error: elfutils/libdw.h: No such file or directory
sudo apt-get install libdw-dev

再安装imu_utils

https://github.com/gaowenliang/imu_utils
Eigen3Config.cmake
eigen3-config.cmake
set(EIGEN3_INCLUDE_DIRS /usr/local/include/eigen3)
include_directories(${EIGEN3_INCLUDE_DIRS})

保存imu_d455.launch在imu_utils下的launch中

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu"/> # 相机发布imu topic的名字
        <param name="imu_name" type="string" value= "d455"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/d455"/>
        <param name="max_time_min" type="int" value= "60"/> # 录制数据的时长 单位是分钟
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

标定IMU

rosbag play -r 200 d455_imu.bag
roslaunch imu_utils imu_d455.launch

也可以让IMU静止不动两个小时标定

roslaunch realsense2_camera rs_camera.launch
roslaunch imu_utils imu_d455.launch

等待imu_d455.launch中设定的时间后,imu的参数将会被保存在data_save_path的路径下文件imu.yaml中

多IMU标定

Multi IMU and IMU intrinsic calibration
https://github.com/ethz-asl/kalibr/wiki/Multi-IMU-and-IMU-intrinsic-calibration

https://blog.csdn.net/shenshikexmu/article/details/80013444
https://blog.csdn.net/shenshikexmu/category_6446413.html

利用pyrealsense2进行六面标定法标定加速度计
https://www.cnblogs.com/buxiaoyi/p/7541974.html

标定 Camera-IMU（针孔、鱼眼）

Kalibr

利用kalibr标定IMU和相机
https://github.com/ethz-asl/kalibr/wiki/Camera-IMU-calibration#2-collect-images
https://blog.csdn.net/heyijia0327/article/details/83583360

标定 RealSense cameras D435i

gedit rs_camera.launch
<arg name="color_width"         default="640"/>
<arg name="color_height"        default="480"/>
图像大小为640*480

<arg name="color_fps"           default="30"/>
帧率为30

rosrun camera_calibration cameracalibrator.py  --size 8x6 --square 0.05 image:=/camera/color/image_raw

roslaunch realsense2_camera rs_camera.launch
rosrun image_view image_view image:=/camera/color/image_raw
realsense-viewer
rosbag record -O realsense_data /camera/color/image_raw
rosbag info realsense_data.bag
rosbag play --pause realsense_data.bag

kalibr_calibrate_cameras --bag /home/q/packages/realsense_ros_ws/realsense_data_cal.bag --models pinhole-radtan --topics /camera/color/image_raw --target /home/q/packages/kalibr_ws/aprilgrid.yaml

kalibr_calibrate_cameras --bag /home/q/packages/realsense_ros_ws/realsense_data_cal.bag --models pinhole-fov --topics /camera/color/image_raw --target /home/q/packages/kalibr_ws/aprilgrid.yaml

kalibr_calibrate_cameras --bag /home/q/packages/realsense_ros_ws/realsense_data_cal.bag --models pinhole-equi --topics /camera/color/image_raw --target /home/q/packages/kalibr_ws/aprilgrid.yaml

camchain.yaml

bag包格式
https://github.com/ethz-asl/Kalibr/wiki/bag-format
标定需要的图像以及相对应的imu数据。
图片格式是：19位时间（精度为ns，精度不够可以补0）.png。
IMU文件格式是：19位时间戳（精确到ns，精度不够可以补0），角速度，含重力的加速度
需要的文件
camera-IMU标定需要输入的四个文件：
分别是：包含图形和imu数据的bag文件、相机参数文件、IMU参数文件和标定板参数文件。
1、.bag:包含有图片信息和IMU数据的ROS包
2、camchain.yaml: 包含相机的内参、畸变参数的文件，如果是双目的话，还包含两个相机的位置转换矩阵；
3、IMU.yaml: 包含IMU的噪声密度、随机游走；
4、target.yaml:标定目标板的参数
所以在进行camera-IMU 标定前，我们分别要对camera内参和IMU进行标定得到相应的camchain.yaml和IMU.yaml文件
输出的结果
IMU和相机坐标系的相对位姿矩阵T和重投影误差（或者像素误差，Pixel Error（像素误差）指的是the standard deviation of the reprojection error (in Pixel) in both x and y directions respectivly（在x和y方向上以像素为单位的重投影误差的标准差。根据优化的准则我们知道重投影误差越小，就说相机标定的精度越高）。在估计出内参之后，会进行优化迭代操作。
如果是多相机标定，在完成内参标定的同时，也会完成具有交叉视野相机外参的的标定。初始估计步骤也会进行多相机基线距离的估计，用作后续的迭代优化。如果某一时刻有多个相机同时观测靶标，则先求取视野内看见角点最多的那个相机的外参，然后根据预估计出的基线距离得到靶标到第一个相机的变换矩阵。然后计算第一个相机的重投影误差，构造优化函数。
我们进行camera-IMU标定的目的是为了得到IMU和相机坐标系的相对位姿矩阵T和相对时间延时t_shift(t_imu=t_cam + t_shift)。
准备工作

target.yaml
https://github.com/ethz-asl/kalibr/wiki/calibration-targets

tagCols 为标定格每行个数
tagRows 为标定格每列个数
tagSize 为标定格二维码边长，单位m

提前准备好camchain.yaml（包含相机的内参、畸变参数的文件，如果是双目的话，还包含两个相机的位置转换矩阵）、IMU.yaml（包含IMU的噪声密度、随机游走）、target.yaml（标定目标板的参数），并制作.bag文件。

启动realsense相机和imu，所以也要先设置好rs_camera.launch文件中的相关参数；

gedit rs_camera.launch
<arg name="enable_sync" default="true"/> 
第一参数保证相机数据和ＩＭＵ数据的同步
<arg name="unite_imu_method" default="copy"/>
第二个参数保证输出同步的ＩＭＵ数据
roslaunch realsense2_camera rs_camera.launch

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

将标定板打印出来，置于平稳，光照充足的地方，注意要保持平整。使得相机与IMU位置相对固定，移动camera-IMU系统让IMU三个轴都被激活（不能只有转动，最好在转动的同时移动，确保加速度计也有输出）采集数据。录制数据包时标定板不要超出画面，尽量保证图像清晰，不要剧烈移动，采集数据的起始和结束阶段注意别晃动太大，如从桌子上拿起或者放下。如果有这样的动作，在标定阶段应该跳过bag数据集的首尾的数据。同时尽可能地激活IMU的各个角度，各个方向，可以先依次绕各个轴运动，运动完后来个在空中画8字之类的操作，当然也要注意别运动太剧烈。
录制包的时候要注意激活所有的轴，同时不要将camera距离标定板太近，否则会出现focalength无法初始化的错误；推荐是将camera固定，然后移动标定板，这样可以提高标定的稳定性；
多相机的时候尤其注意：kalibr推荐的是频率保持在4HZ，这样降低采样率之后可以减少计算量，但是可能会遇到一个问题就是两个相机的采样图像并不同步，我使用的是ros中的usb_cam_node ，没有对像个相机做同步，所以设置成4HZ采集好bag之后，进行标定会出现 Cameras are not connected through mutual observations 的提醒，改成20HZ之后解决；虽然包很大，也有很多图像，但是最后有用的只有有限组。为相机和IMU录制数据包，保证在录制数据包的时候标定板不会超出画面，录制的时候尽量保证图像清晰，不要剧烈移动，同时尽可能地激活IMU的各个角度，各个方向。

rosrun image_view image_view image:=/camera/color/image_raw 
rosbag record /camera/color/image_raw  /camera/imu -O rgb_imu.bag​ 

进行标定：参数分别是包名称(不用加.bag)，相机参数文件，imu参数文件， 标定板文件

kalibr_calibrate_imu_camera 
--bag rgb_imu.bag​
--bag-from-to 5 45
--cam cam.yaml 
--imu imu.yaml 
--target aprilgrid.yaml 
--time—calibration可以标定IMU相对于camera的延时。

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

ImportError: No module named scipy.optimize
sudo apt-get install python-scipy

标定结果
会得到几个输出文件
report-imucam-%BAGNAME%.pdf: Report in PDF format. Contains all plots for documentation.
results-imucam-%BAGNAME%.txt: Result summary as a text file.
camchain-imucam-%BAGNAME%.yaml: 这个文件是在输入文件camchain.yaml基础上增加了标定后的cam-imu信息的结果文件。
我们想要的T_cam_imu矩阵就在这里。

标定结果 target.yaml

Timeshift表示的就是IMU相对于camera的延时；

重投影误差，大部分在1说明很准确。一个数值代表差一个像素点。