zed2i相机内参标定

参考：

https://blog.csdn.net/yanpeng_love/article/details/107166922
https://blog.csdn.net/weixin_41954990/article/details/127928852

参考以上连接先安装kalibr。

注意：

• python包装不上，换成：pip install
• 出现pyx找不到的错误：sudo apt-get install python3-pyx

1. 生成标定板

可以自己定制特定尺寸的标定板：

source ~/桌面/liweidong/kalibr_ws/devel/setup.bash
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 4 --ny 4 --tsize 0.035 --tspace 0.2

其中，tsize为方块的边长，tspace为方块边长/方块间距离（上述参数刚好可以用A4纸张打印出来）。
同时，生成相应的配置文件aprilgrid.yaml：

target_type: 'aprilgrid' #gridtype
tagCols: 4               #number of apriltags
tagRows: 4               #number of apriltags
tagSize: 0.035           #size of apriltag, edge to edge [m]
tagSpacing: 0.2          #ratio of space between tags to tagSize

根据张正友标定法，标本板需要为平面，采用纸张可能不够平整，最好用特制的标定板。实验室的标定板如下，规格为297x210mm：

生成的配置文件如下：

target_type: 'aprilgrid' #gridtype
tagCols: 7               #number of apriltags
tagRows: 5               #number of apriltags
tagSize: 0.03           #size of apriltag, edge to edge [m]
tagSpacing: 0.2          #ratio of space between tags to tagSize

（很疑惑，我用购买的标定板的标定质量反而不如打印的A4纸）

2. 开启zed2i相机

source ~/桌面/zed/ros/catkin_ws/devel/setup.bash
roslaunch zed_wrapper zed2i.launch

3. 显示图片

开两个终端，分别输入以下命令，用以观察相机左右目图像

rqt_image_view

两个窗口分别订阅话题：
/zed2i/zed_node/left/image_rect_color和/zed2i/zed_node/right/image_rect_color

4. 录制bag

相机对准标定板

rosbag record /zed2i/zed_node/imu/data /zed2i/zed_node/left/image_rect_color /zed2i/zed_node/right/image_rect_color 

要求：

• 相机的6个自由度都要运动到，且运动要比较缓慢
• 标定板应当占据图像比较大的视野
  可以参考以下视频：
  https://youtu.be/puNXsnrYWTY?t=57

5. 计算相机参数

将录制的bag以及aprilgrid.yaml放到同一目录下

rosrun kalibr kalibr_calibrate_cameras --bag xxx.bag --topics /zed2i/zed_node/left/image_rect_color /zed2i/zed_node/right/image_rect_color --models pinhole-radtan pinhole-radtan --target aprilgrid.yaml

如果依赖出现错误：

pip install -U wxPython
pip uninstall numpy
pip install numpy==1.23.5

如果焦距初始化有问题：

Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.
[ERROR] [1668944382.174500]: initialization of focal length for cam with topic /color failed

先输入export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1，然后再输入标定命令，会再次出现上述错误，然后手动输入焦距初始值，比如513，不需要太准确。

之后需要等待一段时间，然后会生成标定报告

6. 标定报告

标定结果的质量衡量主要靠重投影误差，我标定的平均重投影误差如下：

reprojection error: [-0.000013, -0.000000] +- [0.439647, 0.382278]
reprojection error: [0.000014, -0.000000] +- [0.446480, 0.360552]

得到的内参为：

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.007672411193999486, -0.0018797608319184925, 0.00010455529940025013, 0.003016299846222382]
  distortion_model: radtan
  intrinsics: [513.9484564548267, 514.419647214949, 655.6025133839975, 355.2192952629164]
  resolution: [1280, 720]
  rostopic: /zed2i/zed_node/left/image_rect_color
cam1:
  T_cn_cnm1:
  - [0.999984579215046, -0.0002977357189092575, -0.005545510395196279, -0.1198602703850598]
  - [0.00029918561625662975, 0.9999999212808846, 0.00026062652575081026, 0.00013190712977596532]
  - [0.005545432360832611, -0.0002622816436302485, 0.9999845895753918, -0.0028534346937582433]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.00881952628646651, -0.0009790074643185275, 6.896618449740411e-06, 0.0021108942872241914]
  distortion_model: radtan
  intrinsics: [512.4235378267127, 511.74360543501797, 658.7869197242953, 354.8151090771085]
  resolution: [1280, 720]
  rostopic: /zed2i/zed_node/right/image_rect_color