参考:orb_slam2_ros中提供了多款相机的demo例程，
本实验参考orb_slam_2_ros/ros/launch/orb_slam2_r200_mono.launch文件做修改实现单目SLAM和纯定位
实验前准备：
1、一个单目摄像头，笔记本自带的摄像头或USB摄像头都可以，
2、摄像头已经标定完成。如果不清楚标定过程，可以参考摄像头标定-camera_calibration
单目相机SLAM文件准备
为例避免修改orb_slam2_ros源码内容，本节新建了一个bingda_orb_slam功能包用于存放launch等文件。
在功能包中首先创建launch、config和map三个目录
在launch目录中创建mono_camera.launch文件，并写入以下内容。
其中ost.yaml为摄像头标定参数，将其放在config目录中。并创建从base_link到usb_cam的tf变换。
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_name" value="my_camera"/>
<param name="camera_info_url" value="file://$(find bingda_orb_slam)/config/ost.yaml"/>
</node>
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_camera"
args="0 0 0 0 0 0 1 base_link usb_cam" />
</launch>
然后创建orb2_mono_slam.launch文件，写入以下内容
文件主要参考了orb_slam2_r200_mono.launch，主要修改了2个地方
一是对话题名重映射做了调整，
二是将load_calibration_from_cam参数设置为true，即使用camera_info中参数作为标定参数（源文件中是使用launch文件中手动写入参数）
<launch>
<node name="orb_slam2_mono" pkg="orb_slam2_ros"
type="orb_slam2_ros_mono" output="screen">
<remap from="/camera/image_raw" to="/usb_cam/image_raw" />
<remap from="/camera/camera_info" to="/usb_cam/camera_info" />
<param name="publish_pointcloud" type="bool" value="true" />
<param name="publish_pose" type="bool" value="true" />
<param name="localize_only" type="bool" value="false" />
<param name="reset_map" type="bool" value="false" />
<!-- static parameters -->
<param name="load_map" type="bool" value="false" />
<param name="map_file" type="string" value="map.bin" />
<param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" />
<param name="pointcloud_frame_id" type="string" value="map" />
<param name="camera_frame_id" type="string" value="usb_cam" />
<param name="min_num_kf_in_map" type="int" value="5" />
<!-- ORB parameters -->
<param name="/ORBextractor/nFeatures" type="int" value="2000" />
<param name="/ORBextractor/scaleFactor" type="double" value="1.2" />
<param name="/ORBextractor/nLevels" type="int" value="8" />
<param name="/ORBextractor/iniThFAST" type="int" value="20" />
<param name="/ORBextractor/minThFAST" type="int" value="7" />
<!-- Camera parameters -->
<!-- Camera frames per second -->
<param name="camera_fps" type="int" value="30" />
<!-- Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -->
<param name="camera_rgb_encoding" type="bool" value="true" />
<!-- Camera calibration parameters -->
<!--If the node should wait for a camera_info topic to take the camera calibration data-->
<param name="load_calibration_from_cam" type="bool" value="true" />
</node>
</launch>
启动单目相机SLAM
启动摄像头
roslaunch bingda_orb_slam mono_camera.launch启动SLAM
roslaunch bingda_orb_slam orb2_mono_slam.launch 打开rviz，并在rviz中订阅/orb_slam2_mono/debug_image图像话题，增加TF显示，订阅
/orb_slam2_mono/map_points点云数据
rviz初始状态下，rviz中TF也会显示警告，并且图像应显示如下图所示，下方会显示“TRYING TO INITIALIZE”。
这是由于orb2需要若干帧移动的图像用于完成初始定位。
缓慢的晃动摄像头，完成orb2的初始化。完成初始化后，Rviz中将显示TF变换关系，图像中也将会用绿色的方框标记特征点，移动摄像头可以观察到相机的TF坐标会有相对于map坐标的移动。
如果需要查看相机和地图坐标之间的变换关系，可以使用tf_echo查看
rosrun tf tf_echo /map /usb_camranslation为XYZ三轴的线性距离变换，单位为米。（单目中距离变化和物理世界尺度不一致）
Rotation为角度变换，角度变换以四元数、欧拉角（弧度制）和欧拉角（角度值）三种形式显示
终端中将会以欧拉角的形式输出
注意事项
1、由于单目相机不具备深度信息，所以构建出的地图和真实世界的尺度是不匹配的
2、由于orb_slam2使用单目相机所构建的地图是稀疏点云地图，所以无法用于导航等需要稠密点云的场合
3、orb_slam2使用单目相机仅仅依赖图像信息，所以在相机快速旋转等会导致前后两帧图像差异很大的运动下，会导致定位跟踪丢失
保存地图
在建立完场景的地图后，可以将地图保存下来。
调用/orb_slam2_mono/save_map服务将当前地图命名为map.bin
rosservice call /orb_slam2_mono/save_map map.bin保存的地图位于~/.ros目录中，为了方便使用，可以将文件移动到bingda_orb_slam/map目录中
mv ~/.ros/map.bin ~/catkin_ws/src/bingda_orb_slam/map/