使用realsense拍摄数据运行msckf-vio
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可以看到如下内容:
<remap from="~imu" to="/imu0"/>
以及
<include file="$(find msckf_vio)/launch/image_processor_euroc.launch">
同时打开image_processor_euroc.launch,可以看到如下内容:
<remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/image_raw"/>
综上可知: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,部分参数是通过实验调整过的
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/realsense-d435i-imu-stereo.yaml"/>
<!-- Image Processor Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="image_processor"
args="standalone msckf_vio/ImageProcessorNodelet"
output="screen">
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="4"/>
<param name="grid_col" value="5"/>
<param name="grid_min_feature_num" value="2"/>
<param name="grid_max_feature_num" value="3"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="10"/>
<param name="max_iteration" value="8"/>
<param name="track_precision" value="0.01"/>
<param name="ransac_threshold" value="1"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/camera/imu"/>
<remap from="~cam0_image" to="/camera/infra1/image_rect_raw"/>
<remap from="~cam1_image" to="/camera/infra2/image_rect_raw"/>
</node>
</group>
</launch>
- step3:复制msckf_vio_euroc.launch,重命令为msckf_vio_realsenseD435i.launch,这里的imu参数我是参考了vins-fusion里面的config,应该直接用标定的结果,飘的很厉害修改后内容为:
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="fixed_frame_id" default="world"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/realsense-d435i-imu-stereo.yaml"/>
<!-- Image Processor Nodelet -->
<include file="$(find msckf_vio)/launch/image_processor_realsenseD435i.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="calibration_file" value="$(arg calibration_file)"/>
</include>
<!-- Msckf Vio Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="vio"
args='standalone msckf_vio/MsckfVioNodelet'
output="screen">
<!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="publish_tf" value="true"/>
<param name="frame_rate" value="30"/>
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
<param name="child_frame_id" value="odom"/>
<param name="max_cam_state_size" value="20"/>
<param name="position_std_threshold" value="8.0"/>
<param name="rotation_threshold" value="0.2618"/>
<param name="translation_threshold" value="0.4"/>
<param name="tracking_rate_threshold" value="0.5"/>
<!-- Feature optimization config -->
<param name="feature/config/translation_threshold" value="-1.0"/>
<!-- These values should be standard deviation -->
<param name="noise/gyro" value="0.005"/>
<param name="noise/acc" value="0.05"/>
<param name="noise/gyro_bias" value="0.001"/>
<param name="noise/acc_bias" value="0.01"/>
<param name="noise/feature" value="0.035"/>
<param name="initial_state/velocity/x" value="0.0"/>
<param name="initial_state/velocity/y" value="0.0"/>
<param name="initial_state/velocity/z" value="0.0"/>
<!-- These values should be covariance -->
<param name="initial_covariance/velocity" value="0.25"/>
<param name="initial_covariance/gyro_bias" value="0.01"/>
<param name="initial_covariance/acc_bias" value="0.01"/>
<param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
<remap from="~imu" to="/camera/imu"/>
<remap from="~features" to="image_processor/features"/>
</node>
</group>
</launch>
3、使用realsense录制bag数据包
- step 1:启动realsense
先复制realsense-ros中的rs_camera.launch,重命名为rs_imu_stereo.launch,并更改两处内容,一处为
<arg name="unite_imu_method" default="linear_interpolation"/>
这可以使得有imu这个话题发布
第二处为
<arg name="enable_sync" default="true"/>
这可以使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为:
<launch>
<arg name="robot" default="firefly_sbx"/>
<arg name="calibration_file"
default="$(find msckf_vio)/config/camchain-imucam-indemind.yaml"/>
<!-- Image Processor Nodelet -->
<group ns="$(arg robot)">
<node pkg="nodelet" type="nodelet" name="image_processor"
args="standalone msckf_vio/ImageProcessorNodelet"
output="screen">
<rosparam command="load" file="$(arg calibration_file)"/>
<param name="grid_row" value="10"/>
<param name="grid_col" value="10"/>
<param name="grid_min_feature_num" value="10"/>
<param name="grid_max_feature_num" value="15"/>
<param name="pyramid_levels" value="3"/>
<param name="patch_size" value="15"/>
<param name="fast_threshold" value="3"/>
<param name="max_iteration" value="30"/>
<param name="track_precision" value="0.01"/>
<param name="ransac_threshold" value="3"/>
<param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/indemind/imu"/>
<remap from="~cam0_image" to="/indemind/image_left"/>
<remap from="~cam1_image" to="/indemind/image_right"/>
</node>
</group>
</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