要求
要运行ROVIOLI,需要以下标定文件:
- Camera calibration, example file for the Euroc datasets
- IMU parameters maplab, example file for the Euroc datasets
- IMU parameters Rovio, example file for the Euroc datasets
这三个标定文件都在~/maplab_ws/src/maplab/applications/rovioli/share
路径下面。
从rosbag建立地图
到Euroc dataset website下载Machine Hall 01。
使用脚本:~/maplab_ws/src/maplab/applications/rovioli/scripts/tutorials/tutorial_euroc
#!/usr/bin/env bash
LOCALIZATION_MAP_OUTPUT=$1
ROSBAG=$2
NCAMERA_CALIBRATION="$ROVIO_CONFIG_DIR/ncamera-euroc.yaml"
IMU_PARAMETERS_MAPLAB="$ROVIO_CONFIG_DIR/imu-adis16488.yaml"
IMU_PARAMETERS_ROVIO="$ROVIO_CONFIG_DIR/imu-sigmas-rovio.yaml"
REST=$@
rosrun rovioli rovioli \
--alsologtostderr=1 \
--v=2 \
--ncamera_calibration=$NCAMERA_CALIBRATION \
--imu_parameters_maplab=$IMU_PARAMETERS_MAPLAB \
--imu_parameters_rovio=$IMU_PARAMETERS_ROVIO \
--datasource_type="rosbag" \
--save_map_folder="$LOCALIZATION_MAP_OUTPUT" \
--optimize_map_to_localization_map=false \
--map_builder_save_image_as_resources=false \
--datasource_rosbag=$ROSBAG $REST
打开roscore,运行数据集:
roscore
source ~/maplab_ws/devel/setup.bash
rosrun rovioli tutorial_euroc save_folder MH_01_easy.bag
注意:save_folder
和MH_01_easy.bag
要为绝对路径。
完成 ROVIOLI 并保存地图后,save_folder
应有如下结构:
$ tree save_folder
save_folder/
├── metadata
├── resource_info
├── resources
└── vi_map
├── edges
├── landmark_index
├── missions
├── optional_sensor_data
├── sensors.yaml
├── vertices0
├── vertices1
├── vertices2
├── vertices3
├── vertices4
├── vertices5
├── vertices6
├── vertices7
├── vertices8
└── vertices9
从一个rostopic建立一个地图
使用脚本:~/maplab_ws/src/maplab/applications/rovioli/scripts/tutorials/tutorial_euroc_live
。
使用Euroc数据集,调用:
roscore
source ~/maplab_ws/devel/setup.bash
rosrun rovioli tutorial_euroc_live save_folder
然后,打开另一个终端,播放bag:
rosbag play MH_01_easy.bag
bag播放完后,需要按ctrl+c退出,才能保存地图。
输出
默认情况下,位姿估计值由以下rostopics发布:
- /maplab_rovio/T_G_I
- /maplab_rovio/T_G_M
- /maplab_rovio/T_M_I
- /maplab_rovio/bias_acc
- /maplab_rovio/bias_gyro
- /maplab_rovio/velocity_I
可视化
下载Rviz配置文件,打开rviz
,导入配置文件,添加topic
。
如果想可视化更多的topic,需要在脚本(~/maplab_ws/src/maplab/applications/rovioli/scripts/tutorials/tutorial_euroc_live
)中添加相应的标志(flag)。
我在默认的脚本最后添加了几个使能的标志位:
#!/usr/bin/env bash
# Script to run ROVIOLI from a Euroc live data source (e.g., Euroc bag file with rosbag play).
# Usage: tutorial_euroc <output save folder> [<additional rovioli flags>]
LOCALIZATION_MAP_OUTPUT=$1
NCAMERA_CALIBRATION="$ROVIO_CONFIG_DIR/ncamera-euroc.yaml"
IMU_PARAMETERS_MAPLAB="$ROVIO_CONFIG_DIR/imu-adis16488.yaml"
IMU_PARAMETERS_ROVIO="$ROVIO_CONFIG_DIR/imu-sigmas-rovio.yaml"
REST=$@
rosrun rovioli rovioli \
--alsologtostderr=1 \
--v=2 \
--ncamera_calibration=$NCAMERA_CALIBRATION \
--imu_parameters_maplab=$IMU_PARAMETERS_MAPLAB \
--imu_parameters_rovio=$IMU_PARAMETERS_ROVIO \
--datasource_type="rostopic" \
--save_map_folder="$LOCALIZATION_MAP_OUTPUT" \
--map_builder_save_image_as_resources=false \
--optimize_map_to_localization_map=false $REST \
--detection_visualize_keypoints=true \
--feature_tracker_visualize_feature_tracks=true \
--feature_tracker_visualize_keypoint_matches=true \
--feature_tracker_visualize_keypoints=true \
--feature_tracker_visualize_keypoints_individual_frames=true \
--publish_debug_markers=true \
--rovioli_visualize_map=true
这里不再赘述,详细信息请切换到这里。
运行截图如下: