ROS与VSLAM入门教程-rtabmap_ros-双目相机手持建图

ROS与VSLAM入门教程-rtabmap_ros-双目相机手持建图

说明：

• 介绍如何利用rtabmap_ros通过手持双目相机进行建图

配置双目相机：

• 假设坐标系是 header.frame_id是/camera_link，发布如下话题：

  • left/image_raw (sensor_msgs/Image)
  • right/image_raw (sensor_msgs/Image)
  • left/camera_info (sensor_msgs/CameraInfo)
  • right/camera_info (sensor_msgs/CameraInfo)

• 相机已经校准，方法可以参考

• 通过stereo_image_proc处理，生成如下话题：

  • left/image_rect_color (sensor_msgs/Image)
  • right/image_rect_color (sensor_msgs/Image)
  • left/image_rect (sensor_msgs/Image)
  • right/image_rect (sensor_msgs/Image)
  • points2 (sensor_msgs/PointCloud2)

• 消息基于 /camera_link 坐标系(x-axis right, y-axis down and z-axis forward)，RTAB-Map 需要基于 /world frame (x-axis forward, y-axis left, z-axis up)，增加一个 static_transform_publisher 的 /base_link 到/camera_link的变换:

<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link camera_link 100" /> 

• 配置即可启动 RTAB-Map

启动RTAB-Map（使用zed双目相机）：

• 启动

$ export ROS_NAMESPACE=stereo_camera
$ roslaunch zed_wrapper zed_camera.launch publish_tf:=false

Mapping mode/建图模式：

• 启动(rtabmapviz)：

 $ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start"

• 启动Zed camera

 $ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_center 100
 $ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start --Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200" right_image_topic:=/stereo_camera/right/image_rect_color stereo:=true

• 启动 (rviz):

 $ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=false

• 双目消息不同步，增加"approximate_sync:=true"选项

• 使用外部里程数据，增加"visual_odometry:=false odom_topic:=/my_odometry".选项

• 地图数据保存在 ~/.ros/rtabmap.db

• 查看地图数据

$ rtabmap-databaseViewer ~/.ros/rtabmap.db

Localization mode/定位模式：

• 切换模式，在rtabmapviz (GUI)下，点击 "Detection"->"Localization", 切换后rtabmap找到闭环来完成定位
• 或通过 Detection->"Reset odometry" 来重新定位
• 命令方式：

$ rosservice call /rtabmap/reset_odom

• 启动时候启用定位模式
• Launch (rtabmapviz):

 $ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" localization:=true

• Zed camera

 $ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_initial_frame 100
 $ rosrun tf static_transform_publisher 0 0 0 0 0 0 zed_current_frame ZED_left_camera  100
 $ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:=/camera right_image_topic:=/camera/right/image_rect_color frame_id:=camera_link rtabmap_args:=" --Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200" localization:=true

• Launch (rviz):

 $ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:="/stereo_camera" rviz:=true rtabmapviz:=false localization:=true

• 在GUI, 点击Edit->"Download map" 来加载地图