在仿真運行rtabmap算法后凸丸，在此基礎(chǔ)上繼續(xù)進行算法包仿真實踐。從此前的實驗來看，基本上在gazebo上仿真SLAM算法的要點大致是和ROS 的運行機制相似，就是topic 的訂閱和發(fā)布機制捷泞。原理上就是將gazebo仿真的topic和算法所訂閱的topic一樣即可（指名稱以及topic中消息的類型）。因此在此思路基礎(chǔ)上寿谴，進行對ORB_SLAM2的運行實驗锁右。
安裝配置ORB_SLAM2
主要步驟follow官網(wǎng)即可：https://github.com/raulmur/ORB_SLAM2
其中有兩個大坑，參考連接：https://blog.csdn.net/li528405176/article/details/81164637
即可解決讶泰。
基于gazebo世界的ORB_SLAM2運行實驗
進入正題咏瑟，畢竟ORB_SLAM2號稱單目，雙目以及深度相機都支持的算法痪署，所以先來實施單目相機的實驗码泞。
Mono 相機實驗
參考官網(wǎng)（上面的網(wǎng)站），得出最為重要的信息惠桃，算法訂閱的主題：
orb_slam2_mono_web.png
訂閱主題是"/camera/image_raw"浦夷，因此對現(xiàn)有的機器人進行修改并保存為新模型：robot_monoCam_imu.xacro和robot_monoCam_imu.gazebo。
根據(jù)第一篇文章辜王，建立機器人模型，這里主要是相機插件發(fā)生變化罐孝，因此改動gazebo中的相機插件即可：
<!-- mono camera -->
<gazebo reference="camera_link">
<sensor type="camera" name="camera1">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
這個是單目相機的gazebo插件呐馆，用其替換掉原來的深度相機插件即可。另外莲兢，這個模型還有一個不同的傳感器是IMU（原本打算仿真VINS-Mono算法的汹来，由于未知問題尚未解決，決定先擱置）：
<!-- imu -->
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu0</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>100.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
這里需要注意的是改艇，相機發(fā)布的topic完整是由cameraName加上相應(yīng)的topic（也可以將cameraName置空收班，topic名稱寫完整也可以）。
這樣機器人就完成配置了, 用此前的命令啟動：
roslaunch robot1_gazebo gazebo_wg.launch model:="`rospack find robot1_description`/urdf/robot_monoCam_imu.xacro" world:="custom_world.launch"
再啟動ORB_SLAM2節(jié)點：
rosrun ORB_SLAM2 Mono ~/dev/catkin_ws/src/ORB_SLAM2-master/Vocabulary/ORBvoc.txt ~/dev/catkin_ws/src/ORB_SLAM2-master/Examples/Monocular/TUM3.yaml
這里需要說明一下谒兄，ORBvoc.txt是ORB_SLAM2的作者生成的圖片字典文件摔桦，對室內(nèi)和室外的效果都很好，因此一般不改變；TUM3.yaml 是相機的參數(shù)的配置文件邻耕，焦距鸥咖，畸變參數(shù)等等，根據(jù)真實相機標(biāo)定后的參數(shù)來設(shè)定兄世。在這里用的是算法包自帶的文件啼辣。
運行結(jié)果：
ORB_SLAM2_mono_1.png
ORB_SLAM2_mono_2.png
一直顯示嘗試初始化，猜測原因如下：
- 在上面的相機參數(shù)文件yaml設(shè)定參數(shù)不恰當(dāng)御滩，因為在gazebo世界中鸥拧，硬件是仿真出來的，因此可設(shè)定的參數(shù)不多削解，而且應(yīng)該也不需要標(biāo)定富弦。
- 在gazebo世界中，特征過少钠绍，默認的特征數(shù)是100才會初始化成功舆声。
RGB-D相機實驗
步驟也很相似，在官網(wǎng)得知算法訂閱的topic 后柳爽，由于在rtabmap中媳握，也是用深度相機即RGBD相機，所以在不修改機器人模型的基礎(chǔ)上磷脯，將算法要求的topic 重命名：
新建launch文件：
orb_slam2_rgbd.launch
<launch>
<arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/vocabulary_files/ORBvoc.txt"/>
<arg name="PATH_TO_SETTINGS_FILE" value="$(find ORB_SLAM2)/setting_files/Zdzn.yaml"/>
<node name="RGBD" pkg="ORB_SLAM2" type="RGBD" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE)">
<remap from="/camera/depth_registered/image_raw" to="/depth/image_raw" />
<remap from="/camera/rgb/image_raw" to="/rgb/image_raw" />
</node>
</launch>
標(biāo)簽remap的作用就是重命名蛾找，將原來算法訂閱的topic，"/camera/depth_registered/image_raw" 和 "/camera/rgb/image_raw" 重命名為機器人中定義的topic名稱赵誓。
啟動命令：
roslaunch ORB_SLAM2 orbslam2_rgbd.launch
這里需要主要的是打毛，launch 文件需要放在rosbag對應(yīng)的路徑下，才能被使用俩功。在這里路徑是：/home/lxq/dev/catkin_ws/src/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2
運行結(jié)果：
orb_slam2_rgbd.png
視頻連接：http://www.iqiyi.com/w_19s9zc5gu9.html
Stereo相機實驗
和上面一樣幻枉，先建立機器人模型，因為這是雙目相機诡蜓，因此需要在gazebo文件中替換為雙目的鏡頭：
<!-- stereo camera -->
<gazebo reference="camera_link">
<sensor type="multicamera" name="stereo_camera">
<update_rate>30.0</update_rate>
<camera name="left">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">
<pose>0 -0.07 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
然后通過官網(wǎng)查看需要發(fā)布什么topic熬甫，并在插件中定義。
在修改了一下相機參數(shù)文件蔓罚，保存為Stereo_setting.yaml：
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 277.19135641132203
Camera.fy: 277.19135641132203
Camera.cx: 160.5
Camera.cy: 120.5
Camera.k1: 0.0
Camera.k2: 0.0
Camera.k3: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.width: 752
Camera.height: 480
# Camera frames per second
Camera.fps: 20.0
# stereo baseline times fx
Camera.bf: 47.90639384423901
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 35
#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 480
LEFT.width: 752
LEFT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]
RIGHT.height: 480
RIGHT.width: 752
RIGHT.D: !!opencv-matrix
rows: 1
cols: 5
dt: d
data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]
RIGHT.K: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1]
RIGHT.R: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644]
RIGHT.P: !!opencv-matrix
rows: 3
cols: 4
dt: d
data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0]
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1200
# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2
# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500
編寫launch文件：
orbslam2_stereo.launch:
<launch>
<arg name="PATH_TO_VOCABULARY" value="$(find ORB_SLAM2)/vocabulary_files/ORBvoc.txt"/>
<arg name="PATH_TO_SETTINGS_FILE" value="$(find ORB_SLAM2)/setting_files/Stereo_setting.yaml"/>
<node name="Stereo" pkg="ORB_SLAM2" type="Stereo" args="$(arg PATH_TO_VOCABULARY) $(arg PATH_TO_SETTINGS_FILE) false">
</node>
</launch>
在這里要說明一下ORB_SLAM2的節(jié)點啟動椿肩，最后有一個false，是代表是否需要雙目校正豺谈，在這里填ture的話郑象，反而會出現(xiàn)嚴重的畸變，因此推測茬末，仿真出來的相機鏡頭是不需要標(biāo)定和校正的厂榛。
啟動命令：
roslaunch ORB_SLAM2 orbslam2_stereo.launch
運行結(jié)果：
ORB_SLAM2_stereo.png
視頻連接：http://www.iqiyi.com/w_19s9yursvx.html
