VSLAM 테스트는 총 세가지 ORB_SLAM2, OpenVSLAM, RTABMAP을 진행하였다. 관련 영상 링크는 아래에 있다.
https://youtu.be/WZDmh8JAfcU
위 같은 내용을 진행하기 위해 필요한 세팅에 대해 알아보자.
1. ORB_SLAM2, OpenVSLAM, RTABMAP install in jetson agx xavier
1-1. ORB_SLAM2 ros install
original ORB_SLAM2는 ros를 지원하긴 하지만 pcl이 pangolin viewer에서만 나오고 특별히 ros를 이용해서 할 수 있는 기능이 거의 없었던 걸로 기억한다. 또한 build시 따로 추가해야하는 헤더가 많아 귀찮았던걸로 기억한다. ros에 특화된 패키지는
https://github.com/appliedAI-Initiative/orb_slam_2_ros 이 링크의 패키지이며, 설치 방법 또한 쉽다.
1-2. OpenVSLAM install
https://openvslam-community.readthedocs.io/en/latest/installation.html 해당링크에서 install 하는 방법을 따라가면 된다. 참고로 opencv 4.1.1로도 실행가능하니 설치된 opencv를 삭제하지는 말자. 또한 Pangolin Viewer를 사용할지 말지를 기준으로 build하게 되는데, socket통신보다는 pangolin viewer가 훨씬 보기 편하다. 필자는 docker에서 설치하겠다고 거기있는 설치방법 그대로 가면 설치에 실패했다. 몇가지 설정을 바꾸어서 docker에서 설치를 성공했어도 결국 pangolin viewer가 제대로 작동하지 않아 base에 설치했다.
1-3. RTAB MAP install
https://github.com/introlab/rtabmap_ros 해당링크에서 install하는 방법을 따라가면 된다. nvidia jetson에서는 다음과 같은 것을 주의해서 설치하면 되겠다. https://github.com/introlab/rtabmap/issues/427#issuecomment-608052821
2. pkg에 필요한 내용.
3 패키지 모두 realsense, zed를 지원해서 두 카메라를 사용할 때는 특별히 추가할 것이 없다. 하지만 만일 본인만의 카메라를 사용한다고 하면 두가지 모두 camera tf를 추가해주자. tf를 추가하는 방법은 다음 차시에 설명할 것이다.
간단하게 launch 파일을 조작해보자.
2-1. ORB_SLAM2 customizing launch file
본인이 monocamera를 사용한다면 orb_slam2_mynteye_s_mono.launch 를 복사해서 이름을 바꾸고 본인것으로 사용해보자.
<launch>
<node name="orb_slam2_mono" pkg="orb_slam2_ros"
type="orb_slam2_ros_mono" output="screen">
<remap from="/camera/image_raw" to="/mynteye/left_rect/image_rect" />
<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="camera_link" />
<param name="min_num_kf_in_map" type="int" value="5" />
<!-- ORB parameters -->
<param name="/ORBextractor/nFeatures" type="int" value="1200" />
<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="false" />
<!-- Camera calibration and distortion parameters (OpenCV) -->
<param name="camera_fx" type="double" value="332.97713134460906" />
<param name="camera_fy" type="double" value="332.97713134460906" />
<param name="camera_cx" type="double" value="398.9270935058594" />
<param name="camera_cy" type="double" value="252.28187370300293" />
<!-- Camera calibration and distortion parameters (OpenCV) -->
<param name="camera_k1" type="double" value="0.0" />
<param name="camera_k2" type="double" value="0.0" />
<param name="camera_p1" type="double" value="0.0" />
<param name="camera_p2" type="double" value="0.0" />
<param name="camera_k3" type="double" value="0.0" />
</node>
</launch>바꿔야 할 부분은 본인의 camera calibration 값을 넣고, camera/iamge_raw를 본인의 토픽으로 바꾸면 되겠다. 또한 scaleFactor를 본인 카메라에 맞게 넣어주면 되겠지만, 알다시피 monocamera의 scaleFactor는 알아내기 까다로우므로 일단 존재하는 것만 사용해도 무리가 없다.
<param name="camera_baseline" type="double" value="47.90639384423901" />만일 stereo 카메라라면 base line부분을 추가로 바꿔주면 되겠다.
ORB_SLAM2의 trajectroy, 즉 Path를 보려면 다음과 같은 것을 활용해야 한다.
PoseStamped
PoseStamped geometry_msgs에서 보자. http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html
rviz상에서 보면 PoseStamped는 화살표 방향의 것으로만 알 수 있지, 시각적으로 보이는 것은 아니다. 그러므로 우리는 이것을 보기 위해 tf를 추가함과 동시에 nav_msgs path 쪽으로 만들어줄 필요가 있다. 하지만 trajectory를 시각적으로 보는 것이 아닌, 데이터만 저장하고 싶다면 이는 불필요한 내용이다.
Python: PoseStamped to Path
~~ 내용 추가 ~~같은 이유로 OpenVSLAM도 동일한 과정을 겪는다. 하지만 개인적인 경험으로는 loopclosure를 경험하기 힘들었다. 두 슬램 모두에게서 말이다. 이는 loop closing이 얼마나 힘든 것인지 알 수 있다.
2-2. OpenVSLAM customizing launch file
https://openvslam-community.readthedocs.io/en/latest/ros_package.html#installation
OpenVSLAM ros 패키지이다. 위 과정을 통해 설치하면 되지만, 특별한 launch 파일은 존재하지 않는다. 그래서 원한다면 추가로 만들어줘야 한다.
run_slam.launch
rosrun image_transport republish \
raw in:=/usb_cam/image_raw raw out:=/camera/image_raw
<launch>
<node name="image_transport" pkg="image_transport" type="republish">
<param name="in" default="/usb_cam/image_raw" value="/usb_cam/image_raw"/>
<param name="out" default="/camera/image_raw" value="/camera/image_raw"/>
</node>
<node name="openvslam_slam" pkg="openvslam_ros" type="run_slam" output="screen">
<arg name="-v" default="/path/to/orb_vocab.fbow" value="/path/to/orb_vocab.fbow"/>
<arg name="-c" default="/path/to/config.yaml" value="/path/to/config.yaml"/>
</node>
</launch>run_localization.launch
<launch>
<node name="openvslam_slam" pkg="openvslam_ros" type="run_localization" output="screen">
<arg name="-v" default="/path/to/orb_vocab.fbow" value="/path/to/orb_vocab.fbow"/>
<arg name="-c" default="/path/to/config.yaml" value="/path/to/config.yaml"/>
<arg name="--map-db" default="/path/to/mag.msg" value="/path/to/mag.msg"/>
</node>
</launch>위에서 config 파일이나 fbow의 경로는 기존 openvslam 패키지의 경로를 의미한다. 정 원한다면 ros 패키지 안에 따로 만들어 놓아도 되겠다.
config file
config.yaml 파일을 본인이 원하는 내용으로 작성하도록 하자. 나는 camera intrinsic을 제외하곤 바꾸지 않았다.
# EuRoC monocular model
#==============#
# Camera Model #
#==============#
Camera:
name: "EuRoC monocular"
setup: "monocular"
model: "perspective"
fx: 458.654
fy: 457.296
cx: 367.215
cy: 248.375
k1: -0.28340811
k2: 0.07395907
p1: 0.00019359
p2: 1.76187114e-05
k3: 0.0
fps: 20.0
cols: 752
rows: 480
color_order: "Gray"
#=====================#
# Tracking Parameters #
#=====================#
Tracking:
max_num_keypoints: 1000
ini_max_num_keypoints: 2000
#================#
# ORB Parameters #
#================#
Feature:
scale_factor: 1.2
num_levels: 8
ini_fast_threshold: 20
min_fast_threshold: 7
#====================#
# Mapping Parameters #
#====================#
Mapping:
baseline_dist_thr_ratio: 0.02
redundant_obs_ratio_thr: 0.9
#===========================#
# PangolinViewer Parameters #
#===========================#
PangolinViewer:
keyframe_size: 0.07
keyframe_line_width: 1
graph_line_width: 1
point_size: 2
camera_size: 0.08
camera_line_width: 3
viewpoint_x: 0
viewpoint_y: -0.65
viewpoint_z: -1.9
viewpoint_f: 400stereo 또한 동일하게 바꿔주면 되겠다.
OpenVSLAM은 rviz상에서 확인하기 위해 camera pose를 바꿔줘야 한다.
camera_pose
Python: camera pose to path
~~ 내용 추가 ~~2-3. RTAB MAP launchfile customizing??
https://doongdoongeee.tistory.com/105
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
RTAB MAP의 경우 정말 잘 되있어서 따로 custom 하기 보단 위 링크에서 명령어를 복사하여 사용해보도록 하자. 정말 사용하는데 문제가 없다.
FeedBack
Pure Rotation for monocular
OpenVSLAM, ORB_SLAM2 monocular slam의 경우 모두 제자리 회전을 할 시 translation이 너무 작아서, 0에 근접해서 essential matrix를 구하는데 문제가 생겼었다(E=t^R). 그래서 본인의 trajectorty를 잃었다는 error가 계속해서 나오기도 했다.
하지만 stereo camera를 사용한 경우에는 그러한 문제가 없었다.
Velocity
너무 빠른 속도로 움직였을 때는 정면의 feature를 잃기도 했다. 물론 실험 당시에는 turtlebot 적정속도로 해서 문제는 없었지만 만일 카메라가 측면을 바라보며 빠른 속도로 달린다면 주변 feature를 쉽게 잃어버릴 것이다. 이는 적절한 속도를 생각하는 필요성도 있다는 것을 암시한다.
단순 비교
위 영상 링크에서 다른 영상들을 보면 3가지 slam을 비교하는 것을 볼 수 있다. 하지만 2d grid map을 만들 수 있던 것은 rtab map 뿐이었고, 나머지는 3d pcl을 추가로 처리해야
만들 수 있다는 것을 알았다. 하지만 아직 나에게는 너무나 어려운 일이였기에 성공하진 못했다. 또한 좁은 장소에서 해서 그런가 loop closing을 거의 못해냈다. 특히 ORB_SLAM2, OpenVSLAM의 경우 실내 공간을 한바퀴 돌았을 때 사각형을 그려야 하는데 그러지 못하는 모습을 보고 많이 실망했다. 물론 내가 한 실험방법이 틀린 걸 수 도 있다. 특히 sota를 받은걸 생각하면, 좀 더 공부하고 만져야 한다는 것이다. 아직은 dbow나 fbow를 파악하지 못했으므로 이것과 관련된 것만 더 공부한다면 앞으로 더욱 정진할 수 있을 것 같다.
실험시 힘든 점
ssh 로 pangolin viewer를 볼 수 없다는 점이 상당히 짜증났다. 그런걸 고려하면 socket통신으로 build하여 보는 것도 나쁘지 않다고 생각한다.
