realsense d435i + kimera slam，包括realsense双目+imu标定
简单阅读了一下kimera源码，里面明确表示不支持kitti类型数据集，同时发现代码工程量巨大，把imu部分摘除不现实，故采用realsense相机作为视觉+imu传感器进行实验。
kimera中配置有realsense启动的launch，但是采用默认相机参数飘逸很大，且在realsense topic 的 info中未查到合适的cam to imu mat 所以首先对相机进行imu+cam标定。
一、标定
主要参考：https://blog.csdn.net/neptune4751/article/details/111505667
1、imu标定
主要是测加计和陀螺仪的3轴静态误差，原理很简单，只要能保存出数据，自己写也都可以，没什么难度。
不想写就按照参考执行即可。
注：1）先安装code_utils，再安装imu_utils
2）由于rosbag回放频率很高，max_time_min参数可以再小一些；或者直接开着相机，不需要rosbag录制。
3）搞完看一下结果的数据量级是否对的上。（防止d435i 在采集数据过程中抽风）
2、cam标定
要双目内参，建议是先bag，再执行标定程序
注：1）sudo pip install python-igraph --upgrade 报错。采用apt-get命令安装可解决，安装0.71版本
2）生成标定板：这部分参考教程描述的不好。
Kalibr提供3种不同的标定板，其中checkerboard是棋盘格，也可以采用其他种类的板子，可采用如下方式进行生成
kalibr_create_target_pdf --type <标定板种类> <参数>生成以后，建立一个yaml文件，记录你所用板子的参数，如：（如果是已经有标定板，直接给参数就行）（注意给的是内部角点个数，而非方块个数）
target_type: 'checkerboard' #gridtype
tagCols: 11 #number of internal chessboard corners
tagRows: 8 #number of internal chessboard corners
tagSize: 0.02 #size of one chessboard square [m]
tagSpacing: 0.02 #size of one chessboard square [m]
3）双目标定 需要读取infra，并且关掉realsense的红外。
copy rs_camera.launch文件，为rs_camera_imu.launch
更改rs_camera_imu.launch参数：
4）节点启动为
roscore
roslaunch realsense2_camera rs_camera_imu.launch
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4 /left_cam
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4 /right_cam
rosbag record -o stereo.bag /left_cam /right_cam
rviz打开rviz，看一下相机图像（topic：/left_cam），不用录太长时间，频率可以改低一点。在终端当前目录下生成stereo_<时间戳>.bag
5）检测：
需要给bag路径，topic，models和标定板参数。
rosrun kalibr kalibr_calibrate_cameras --bag ~/softwaves/kalibr_workspace/src/stereo.bag --topics /left_cam /right_cam --models pinhole-radtan pinhole-radtan --target ~/softwaves/kalibr_workspace/src/checkerboard.yamlValueError: max must be larger than min in range parameter. 就是标定失败，再录一遍。
最后会输出3个文件，camchain*.yaml，记录内参，参数都写的很明白。以输入的第一个topic为参考坐标系
results-cam*.txt 记录置信度
3、stereo+imu
图像采集参考https://www.youtube.com/watch?app=desktop&v=puNXsnrYWTY
简单的说就是相机对着标定板，6个自由度，每个自由度运动3次，然后在随便晃晃就可以了
注：1）imu的数据要复制按照需求格式保存
2）cam和imu的yaml文件中话题要和rosbag的话题对应
最后得到 results-imucam*.txt，camchain-imucam*.yaml等文件
二、kimera-vio-ros → stereo+imu
1、配置矫正参数
打开/Kimera-VIO/params/RealSenseIR/LeftCameraParams.yaml 文件 把cam0参数复制进去
T_BS.data：results-imucam*.txt中的cam0 to imu
distortion_model: radtan
其他参数：camchain.yaml
打开/Kimera-VIO/params/RealSenseIR/RightCameraParams.yaml 文件 把cam1参数复制进去
打开/Kimera-VIO/params/RealSenseIR/ImuParams.yaml 文件 把静态测量的4个误差放进去，以及n_gravity按照results-imucam*.txt中的写入。其他不用改
也可以使用 src/Kimera-VIO/scripts/kalibr/kalibr_params_to_kimera_vio_params.py 文件生成对应文件
python
使用后<输出文件夹>下生成3个配置文件
注：1）使用前把107行 with np.set_printoptions(floatmode="fixed", suppress=True)： 注销掉，同时把它下面部分的缩进取消（反正它会稀里糊涂得出错，又没啥用，那就把它干掉。）
2）152行 load_kalibr_information(result_directory) 揭示了它怎么找文件的，要求文件名是怎样的
2、修改rviz
复制src/Kimera-VIO-ROS/rviz/kimera_vio_euroc.rviz 到同目录 kimera_vio_realsense.rviz
改掉第一个 rviz/Image的话题
3、修改roslaunch kimera_vio_ros kimera_vio_ros_realsense_IR.launch 文件
把里面的话题改成realsense输出的话题
4、运行kimera-vio-ros
参考：https://github.com/MIT-SPARK/Kimera-VIO-ROS/blob/master/docs/hardware_setup.md
roscore
roslaunch realsense2_camera rs_camera_imu.launch
roslaunch kimera_vio_ros kimera_vio_ros_realsense_IR.launch
rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_realsense.rviz
由于在标定的时候新建了一个catkin的空间，运行kimera前，需要打开source文件，把Kalibr所在ros空间注销掉。
5、不要开回环检测，开了就差不多运行20s挂。
三、kimera-semantic-ros（无语义）
roscore
rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_euroc.rviz
rviz -d $(rospack find kimera_semantics_ros)/rviz/kimera_realsense_metric.rviz
roslaunch kimera_semantics_ros kimera_metric_realsense.launch register_depth:=true
roslaunch kimera_vio_ros kimera_vio_ros_realsense_IR.launch
roslaunch realsense2_camera rs_camera_imu.launch