IMU标定
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<!--TOPIC名称和上面一致-->
<param name="imu_topic" type="string" value= "/camera/imu"/>
<!--imu_name 无所谓-->
<param name="imu_name" type="string" value= "d435i"/>
<!--标定结果存放路径-->
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<!--数据录制时间-min-->
<param name="max_time_min" type="int" value= "120"/>
<!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为200，为后面的rosbag play播放频率-->
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
原文链接：https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064
参考师姐文章rs相机＋imu内外参标定（1-11）
单目标定
标定步骤：
要尽量保持静止，移动要缓慢
上下三次，左右三次，翻转三次，上下平移三次，左右平移三次，前后三次，随机移动多次（任意）
1.kalibr工具生成标定板,（需要生成棋盘格）：
kalibr_create_target_pdf --type 'apriltag' --nx 6 --ny 6 --tsize 0.08 --tspace 0.3
2.下载&打印标定板
https://github.com/ethz-asl/kalibr/wiki/downloads
https://github.com/ethz-asl/kalibr/wiki/calibration-targets（三个参数chekboard, circlegrid, aprilgrid说明，使用的是aprilgrid）
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0244 #size of apriltag, edge to edge [m]
tagSpacing: 0.00286 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
3.确定适合距离
roslaunch realsense2_camera rs_camera.launch
rviz
左侧Fixed Frame选择camera_link
左下角 add --> By topic --> /camera/color/image_raw/ --> 双击Camera ，找一个适合的能拍到棋盘格的距离
关闭
4.修改相机帧数
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
修改相机帧数到4hz，这里采用了新的话题去发布：/color 第5步录制写到/color话题。
5.录制ROS数据包
rosbag record -O camd455i /color
录一分钟就好了，录的时候对着标定板移动，按下ctrl+c 结束录制，看到当前文件夹会出现camd455i.bag数据包。
6.使用Kalibr进行标定（前面的都在tx2上进行，这一步在笔记本电脑上进行）
在刚刚建立的ROS中的Kalibr的工作空间，执行source devel/setup.sh，执行：
kalibr_calibrate_cameras --target aprilgrid.yaml --bag camd455i.bag --models pinhole-radtan --topics /color --show-extraction
pinhole-radtan指的是相机模型和畸变模型
标定完之后可以发现当前目录下出现三个文件，就是标定的结果：
camchain-…camd435i.yaml
results-cam-…camd435i.txt
report-cam-…camd435i.pdf
拍摄的时候，角度不要变化太大，不然会初始化失败并且优化发散。（[ERROR] Did not converge in maxIterations... restarting...）。因为初始化的时候就必须要得到一个较好的估计，但如果有太大角度的图片，很容易得到错误的初值，所以后面就会优化失败。
标定参数为：
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [-0.13104112755856126, 0.020606715483751877, -0.002771690056311741,
0.001163213320750032]
distortion_model: radtan
intrinsics: [260.9278068132713, 263.94361004788766, 301.7212486187008, 199.01710004608793]
resolution: [640, 480]
rostopic: /color
IMU+单目相机标定
官方教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration
标定完camera，IMU，得到相应的yaml文件（即分别为相机和IMU的参数）
标定步骤：
上下三次，左右三次，翻转三次，上下平移三次，左右平移三次，前后三次，随机移动多次（任意）
1.调节帧率
相机20Hz, IMU200Hz，并分别以/color和/imu为话题名发布
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
2.录制数据包
先把目录移动到刚刚建立的kalibr工作环境目录下。
然后执行source devel/setup.sh
rosbag record -b 4096 -O dynamic /color /imu
3.重写yaml文件
标定需要三个文件，一个是相机的标定文件，一个是IMU的标定文件，一个是录制的数据包
同样是用kalibr标定的，camchain-camd435i.yaml，所以直接使用相机yaml文件即可：
（1）相机标定的yaml文件
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [-0.14627678771168612, 0.031132819617662677, -0.0016199154527738965,
-0.01257776985511912]
distortion_model: radtan
intrinsics: [259.05353479443266, 256.15264741602005, 290.7955146414971, 234.21114661849504]
resolution: [640, 480]
rostopic: /color
（2）新建imu.yaml
在kalibr工作环境目录下新建imu.yaml文件：
#Accelerometers
accelerometer_noise_density: 2.0477290485501922e-02 #Noise density (continuous-time)
accelerometer_random_walk: 4.2308969579290693e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 2.2488785808195085e-03 #Noise density (continuous-time)
gyroscope_random_walk: 1.5385085422768701e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
其中以下IMU标定参数已知，将结果对应地填到以上imu.yaml中去：
Gyr:
avg-axis:
gyr_n: 2.2488785808195085e-03
gyr_w: 1.5385085422768701e-05
Acc:
avg-axis:
acc_n: 2.0477290485501922e-02
acc_w: 4.2308969579290693e-04
4.使用Kalibr进行标定
在Kalibr工作目录下执行 source devel/setup.sh
执行指令：
kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camd455i.yaml --imu imu.yaml --bag dynamic.bag --show-extraction
kalibr_calibrate_imu_camera \
--target aprilgrid.yaml \
--cam camchain-camd455i.yaml \
--imu imu.yaml \
--bag imu_cam.bag \
--show-extraction
可以录制2分钟，生成文件名字为：
camchain-imucam-dynamic.yaml
其中T_cam_imu，就是我们需要的外参。
错误：
ImportError: No module named scipy.optimize
解决办法：
sudo apt-get install python-scipy
做完这些就可以耐心等待结果了。。。
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99579733 0.0648917 0.06462771 -0.01794528]
[-0.06212431 0.99710033 -0.04394886 0.01608132]
[-0.06729222 0.03974921 0.9969412 -0.07784444]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99579733 -0.06212431 -0.06729222 0.01363058]
[ 0.0648917 0.99710033 0.03974921 -0.01177593]
[ 0.06462771 -0.04394886 0.9969412 0.07947285]
[ 0. 0. 0. 1. ]]
双目标定
步骤一：下载打印标定板或购买标定板
到https://github.com/ethz-asl/kalibr/wiki/downloads选择
下载，然后缩放到40%，用A4纸就可以打印出来
原始pdf的格子参数是：
6*6的格子
大格子边长：5.5cm
小格子边长：1.65cm
小格子与大格子边长比例：0.3
调整后的格子参数是：
大格子边长：2.2cm
小格子边长：0.66cm
小格子与大格子边长比例：0.3
但这只是理想情况，实际情况还得实际测量。
新建aprilgrid.yaml文件，格式参考上图的yaml，内容展示如下：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0244 #size of apriltag, edge to edge [m]
tagSpacing: 0.00244 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
千万要自己测量大格子边长，即tagSize，我测出来实际上是0.024，所以我的文件里面tagSize是0.024，请自行更改。
步骤二：启动关闭结构光
默认开始结构光时，双目图像会有很多点，这些点可能对标定有影响，所以使用时需要关闭结构光。
先启动
roslaunch realsense2_camera rs_camera.launch
新打开终端，运行
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled设置为off(0) ，展示如下：
步骤三：确定realsense合适放置位置
新打开终端，运行rviz
rviz
之后在里面add rgb和双目对应的topic，
/camera/color/image_raw、
/camera/infra1/image_rect_raw
/camera/infra2/image_rect_raw
之后对准标定板，尝试移动realsense，同时要确保标定板一直在三个图像当中。录制过程参考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop
步骤四：修改相机帧数（官方推荐是4Hz，尽管实际频率不完全准确，但是不影响结果）
kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1才是对应左目相机
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
注意：这种方式可能导致不同摄像头的时间不同步，如果出现这个问题，可以尝试不做这个操作，不这样做意味着需要更多的处理时间，这样的话后面也要相应的更改。
步骤五：录制ROS数据包
rosbag record -O multicameras_calibration /infra_left /infra_right /color
后面三个topic就是转换频率后的topic
步骤六：使用Kalibr标定
先激活环境变量
source devel/setup.bash
然后
kalibr_calibrate_cameras \
--target aprilgrid.yaml \
--bag multicameras_calibration.bag \
--models pinhole-equi pinhole-equi pinhole-equi \
--topics /infra_left /infra_right /color \
--bag-from-to 10 80 \
--show-extraction \
其中–target aprilgrid.yaml是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。–bag multicameras_calibration.bag是录制的数据包，models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）， --topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题，–bag-from-to 10 100表示处理bag中10-100秒的数据。–show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。
可能出现以下报错：
1、报错：Camera are not connected through mutual observations, please check the dataset, Maybe adjust the approx. sync. tolerance.
解决办法：原因应该是各个摄像头数据不同步，解决办法为在标定命令中添加–approx-sync 0.04，更改后的标定命令为：
kalibr_calibrate_cameras \
--target aprilgrid.yaml \
--bag multicameras_calibration.bag \
--models pinhole-equi pinhole-equi pinhole-equi \
--topics /infra_left /infra_right /color \
--bag-from-to 10 80 \
--show-extraction \
--approx-sync 0.04
其中0.04可以看情况调整到0.1，如果还是不行可能是录制的数据有问题，需要重新录制，确保标定板一直在图像当中，并且运动不要太剧烈。如果还是不行尝试不要调整topic频率
参考https://blog.csdn.net/HUST_lc/article/details/96144499和https://blog.csdn.net/HUST_lc/article/details/96144499评论
3、使用棋格标定板标定时出现如下错误
Did not converge in maxIterations... restarting...
等十几分钟。。。。。。
最终得到的结果为三个文件：
双目+IMU标定（跟单目差不多）
步骤一：编写camchain.yaml
格式参考Kalibr官方教程https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的chain.yaml，具体的参数参考上面得到的yaml文件，没有的参数可以删除，最终结果示例如下：
cam0:
camera_model: pinhole
intrinsics: [425.9022457154398, 425.51203910141476, 320.7722078245714, 234.52032563515024]
distortion_model: equidistant
distortion_coeffs: [0.28012961504219924, 0.776522889083524, -3.555039585283302, 7.122425751506347]
rostopic: /infra_left
resolution: [640, 480]
cam1:
camera_model: pinhole
intrinsics: [430.5354779160687, 429.81266841318336, 318.5487581769465, 232.5397023354142]
distortion_model: equidistant
distortion_coeffs: [0.2913961623966211, 0.7172454259365787, -3.7209659434658295, 7.615803448415106]
T_cn_cnm1:
[ 0.9999886252230528, 7.228145915638569e-05, -0.004769087951971224,0.054624373296219914]
[-6.78405772070143e-05, 0.9999995640055901, 0.0009313357504042449, 0.002552511727883378]
[0.004769153190982554,-0.0009310016189884111, 0.9999881941372211,0.004273862007202206]
[0.0, 0.0, 0.0, 1.0]
rostopic: /infra_right
resolution: [640, 480]
步骤二：编写imu.yaml，
格式参考https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的imu.yaml，具体参数使用之前imu标定得到的参数，示例如下：
#Accelerometers
accelerometer_noise_density: 2.8250053766610776e-02 #Noise density (continuous-time)
accelerometer_random_walk: 7.8925155899657628e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 2.3539521240749008e-03 #Noise density (continuous-time)
gyroscope_random_walk: 2.2003805724014335e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
步骤三：准备好之前的aprilgrid.yaml，示例如下：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0244 #size of apriltag, edge to edge [m]
tagSpacing: 0.00299 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
步骤四：复制realsense-ros包中rs_camera.launch，重命名为rs_imu_stereo.launch，更改内容为
将
<arg name="enable_sync" default="false"/>
改为：
<arg name="enable_sync" default="true"/>
这样来使imu和双目数据时间对齐
将
<arg name="unite_imu_method" default=""/>
改为
<arg name="unite_imu_method" default="linear_interpolation"/>
这样来保证会有imu话题
步骤五：启动realsense
roslaunch realsense2_camera rs_imu_stereo.launch
步骤六：关闭IR结构光，参考上面
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled设置为off(0) ，展示如下：
步骤七：打开rviz，add imu topic和infra1 topic以及infra2 topic，同时调整realsense位置，
要确保双目图像数据一直包含标定板全部内容
步骤八：调整imu和双目topic的发布频率以及以新的topic名发布它们
其中双目图像的发布频率改为20Hz，imu发布频率改为200Hz
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
注意：，这种调整频率的方式只是理想结果，通过rostopic hz topic名可以查看实际的频率，可以发现实际频率和设置的频率并不一定相同，但可以先这样，知道如何调整的还望告知。
步骤九: 开始录制数据包，
录制过程参考[https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop]，同样注意双目图像在整个过程要包含整个标定板，同时运动不能太快，这样会造成图像过于模糊，在前后左右上下方向来回移动，录制大概90秒，前后15秒等下可以不使用。录制命令为：
rosbag record -O imu_stereo.bag /infra_left /infra_right /imu
步骤十：开始进行标定
标定命令为
kalibr_calibrate_imu_camera --bag [filename.bag] --cam [camchain.yaml] --imu [imu.yaml] --target [target.yaml] --bag-from-to 15 75 --show-extraction
kalibr_calibrate_imu_camera --bag imu_stereo.bag --cam camchain.yaml --imu imu.yaml --target aprilgrid.yaml --bag-from-to 15 75 --show-extraction
相应参数需要相应更改，target.yaml对应april_6x6_A4.yaml文件
最终得到的结果为是得打yaml，txt，和pdf文件
飞控IMU+D455标定
D455相机IMU数据：
T_ic: (cam0 to imu0):
[[-0.07683649 -0.05913403 0.99528856 -0.00011369]
[-0.98555081 0.15563515 -0.06683784 -0.00001289]
[-0.1509495 -0.98604303 -0.07023805 0.00004632]
[ 0. 0. 0. 1. ]]
使用realsense-viewer录制并标定rgb相机
步骤一：打开realsense并设置好bag包record保存路径，设置如下：设置好rbg相机的分辨率、采样频率后按record记录数据，录制约90秒
步骤二：使用Kalibr标定
先激活环境变量
source devel/setup.bash
然后
kalibr_calibrate_cameras --target ~/project/calibration/rgb/april_6x6_A4.yaml --bag ~/disk/datasets/realsense-dataset/calibration/rgb/20200526_131810.bag --models pinhole-equi --topics /device_0/sensor_1/Color_0/image/data --bag-from-to 20 80 --show-extraction
其中–target aprilgrid.yaml是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。–bag imu_stereo.bag是录制的数据包，models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）， --topics /device_0/sensor_1/Color_0/image/data表示拍摄的rgb相机数据话题，–bag-from-to 20 80表示处理bag中20-80秒的数据。–show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。
注意：
pinhole-radtan: 最常见的针孔模型+布朗畸变模型, 适用于大多数的角度小于120的相机, 其中畸变参数包含了径向畸变k1,k2和切向畸变p1,p2; 如果相机的畸变情况不是很严重,这个模型基本都可以; 比如我的DOV为150的相机, 也可以用这个且去畸变效果很好;
pinhole-equi:针孔模型＋等距畸变模型，也就是ＫＢ模型所需要选择的类型，该模型的使用范围也很广，大部分的鱼眼镜头也可以，注意８参数的ＫＢ模型的畸变参数为k1,k2,k3,k4，虽然也是四个数，但与前一个模型不同的是，这里只有径向畸变的参数，而没有切向畸变tangential distortion，投影时对应的公式也不同；同时这也是opencv中cv::fisheye使用的模型；
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
