使用realsense拍摄数据运行msckf-vio（转载）
他这里是修改MSCKF_VIO里面的launch文件里订阅的话题名称，也就是IMU的和两个摄像头 图像的话题名称由他们自己原本的（可能是小觅的）改为D435i发布的IMU的和两个摄像头 图像的话题名称（当然注意不止一个launch文件要修改，可以仔细看这篇博文内容），看来换成D435i并不麻烦？当然D435i的launch文件也修改了一些东西
原本msckf-vio订阅的topic为/imu0,/cam0/image_raw,/cam1/image_raw
下面作者用D435i之后改为如下
另一处修改可能就是标定，这个应该必不可少。
转载自：https://blog.csdn.net/xiaoxiaoyikesu/article/details/105684652
使用realsense拍摄数据运行msckf-vio
小小一粒粟 2020-04-24 17:34:18 649 收藏 11
分类专栏： slam msckf-vio
版权
使用realsense拍摄数据运行msckf-vio
- 0、 环境准备
- 1、运行分析原版msckf-vio
-
- （1）运行原版msckf-vio
- （2）分析msckf-vio订阅topic
- 2、标定realsense的imu+双目
-
- （1）标定过程
- （2）使用标定参数
- 3、使用realsense录制bag数据包
- 4、使用realsense录制数据运行msckf-vio
- 5、参考资料
- 6、最终使用的配置文件
0、 环境准备
1、运行分析原版msckf-vio
（1）运行原版msckf-vio
新建终端，运行
roscore
新建终端窗口
cd msckf-vio catkin工作空间
source devel/setup.bash
roslaunch msckf_vio msckf_vio_euroc.launch
再新建一个终端
cd /catkin_ws/src/msckf_vio(msckf-vio源代码根路径)
rosrun rviz rviz -d rviz/rviz_euroc_config.rviz
再新建一个终端
cd EuROC数据集所在路径
rosbag play MH_04_difficult.bag
结果如下图：
（2）分析msckf-vio订阅topic
- step 1：在数据集运行过程中再次打开新终端
rostopic list -v
可以得到以下内容
- step 2: 打开msckf-vio launch文件夹的启动文件msckf_vio_euroc.launch可以看到如下内容：
以及
同时打开image_processor_euroc.launch，可以看到如下内容：
综上可知：msckf-vio订阅的topic为/imu0,/cam0/image_raw,/cam1/image_raw
- step 3: 分析以上三个topic
在终端运行
rostopic hz /imu0
得到结果
可知imu话题的频率约为200Hz
在终端运行
rostopic hz /cam0/image_raw
得到结果
在终端运行
rostopic hz /cam1/image_raw
得到结果：
可知左右双目图像数据发布频率为20Hz
2、标定realsense的imu+双目
（1）标定过程
参考：https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064
（2）使用标定参数
- step1：复制msckf_vio中的camchain-imucam-euroc.yaml，并重命名为realsense-d435i-imu-stereo.yaml，并且按照标定得到的结果更改相应的参数，最终结果如下所示：
# The modifications of the output file from Kalibr:
# 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix.
# 2. Add the T_imu_body at the end of the calibration file (usually set to identity).
cam0:
T_cam_imu:
[0.9997871124599239, -0.02052600946290517, -0.0021006414676627023, 0.010014245693363543,
0.020504239757627662, 0.9997406760624286, -0.009907410267038301, 0.00808001607594771,
0.0023034563179399944, 0.00986222904654026, 0.9999487139484826, 0.021095445353456775,
0, 0, 0, 1.000000000000000]
camera_model: pinhole
distortion_coeffs: [0.30555440741044954, 0.7285476316461339, -4.250916487767546, 9.140999701786525]
distortion_model: equidistant
intrinsics: [452.88832599321535, 452.4578637656529, 307.9476959421152, 238.59721508283187]
resolution: [640, 480]
timeshift_cam_imu: 0.006807558830179361
cam1:
T_cam_imu:
[0.9997863209650709, -0.020649307313993077, -0.0009583937502405984, -0.03954039252561855,
0.020633867129875697, 0.9996889467954134, -0.014009039308385943, 0.00794371663363066,
0.0012473725966460005, 0.013986270501085675, 0.9999014092894738, 0.020753389841910594,
0, 0, 0, 1.000000000000000]
T_cn_cnm1:
[0.9999993400335837, -0.00013459882276698756, 0.0011409713212246289, -0.04957761334742124,
0.00013927762366911574, 0.9999915785800182, -0.004101630259879458, -5.110044044384557e-05,
-0.0011404096380215392, 0.004101786464715525, 0.9999909373657629, -0.000363586489509635,
0, 0, 0, 1.000000000000000]
camera_model: pinhole
distortion_coeffs: [0.42382909642879546, -2.1832609268771774, 19.76450405922213, -56.648879181035106]
distortion_model: equidistant
intrinsics: [451.6120316820647, 451.1613490589376, 307.501676883884, 239.56379942726517]
resolution: [640, 480]
timeshift_cam_imu: 0.00682500425867652
T_imu_body:
[1.0000, 0.0000, 0.0000, 0.0000,
0.0000, 1.0000, 0.0000, 0.0000,
0.0000, 0.0000, 1.0000, 0.0000,
0.0000, 0.0000, 0.0000, 1.0000]
- step2: 复制image_processor_euroc.launch，重命名为image_processor_realsenseD435i.launch，部分参数是通过实验调整过的
- step3:复制msckf_vio_euroc.launch，重命令为msckf_vio_realsenseD435i.launch，这里的imu参数我是参考了vins-fusion里面的config，应该直接用标定的结果，飘的很厉害修改后内容为：
3、使用realsense录制bag数据包
- step 1:启动realsense
先复制realsense-ros中的rs_camera.launch，重命名为rs_imu_stereo.launch，并更改两处内容，一处为
这可以使得有imu这个话题发布
第二处为
这可以使imu和双目数据进行同步时间
而后运行
roslaunch realsense2_camera rs_imu_stereo.launch
- step 2：关闭结构光，新建终端
rosrun rqt_reconfigure rqt_reconfigure
打开后设置emitter_enabled如下：
- step 3: 打开rviz
rviz
并添加imu和双目话题，结果展示如下：
- step 4：（可省略）更改话题发布频率
通过rostopic list -v可知对应的三个topic为：
* /camera/imu [sensor_msgs/Imu] 1 publisher
* /camera/infra1/image_rect_raw [sensor_msgs/Image] 1 publisher
* /camera/infra2/image_rect_raw [sensor_msgs/Image] 1 publisher
通过rostopic hz 查看频率可知：imu发布频率为400Hz，双目图像发布频率为30Hz
通过
rosrun topic_tools throttle messages 更改发布频率
分别新建三个终端运行以下命令
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我们可以发现/imu的频率约为15Hz，/infra_left和/infra_right的频率约为150Hz，为了保证频率的准确性，这一步可以不做。
- step5: 录制bag数据集
rosbag record -O imu_stereo5.bag /tf /tf_static /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/imu
最终可以得到imu_stereo.bag数据集
注意：为了运行msckf-vio，前面200帧应该静止
- step6: 通过以下步骤查看bag数据
新建终端
roscore
再建终端
rviz
再建终端
cd bag数据包位置
rosbag play bag数据包名
然后在rviz中添加imu和infra_left、infra_right就可以查看数据
4、使用realsense录制数据运行msckf-vio
- step1:（可省略）新建终端，运行
roscore
- step2: 新建终端窗口
cd msckf-vio catkin工作空间
source devel/setup.bash
roslaunch msckf_vio msckf_vio_realsenseD435i.launch
- step3: 再新建一个终端
cd /catkin_ws/src/msckf_vio(msckf-vio源代码根路径)
rosrun rviz rviz -d rviz/rviz_euroc_config.rviz
- step4: 再新建一个终端
cd EuROC数据集所在路径
rosbag play 录制的bag包
注意：如果出现飘的很厉害的情况，尝试更改image_processor_realsenseD435i.launch为：
5、参考资料
（1）运行msckf-vio相关：https://zhuanlan.zhihu.com/p/76347723
（2）realsense相关：https://github.com/IntelRealSense/realsense-ros
（3）用indemind采集的数据运行msckf-vio:https://gitee.com/sholmes/msckf_indemind
6、最终使用的配置文件
https://download.csdn.net/download/xiaoxiaoyikesu/12359335