從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

前期準備

從零開始跑ORB_SLAM2(零) 安裝Ubuntu16.04 LTS

從零開始跑ORB_SLAM2(一) 前期準備與環境配置

從零開始跑ORB_SLAM2(二) 配置ROS Kinetic環境并測試Realsense D435相機

工作環境

PC：i5-8265U 8GRAM 核顯弟弟CPU

相機：Intel-Realsense D435

環境：Ubuntu16.04 LTS+ ROS Kinetic

創造ROS工作空間

國際慣例，GitHub的官方文檔發一波:ROS Wrapper for Intel® RealSense™ Devices

如果懶得看英文也可以跟我一步步走。

在開始這一步之前，需要完成的事情分别是:

安裝ROS Kinetic
安裝Realsense SDK

毫無疑問我們已經完成了這兩步，為了安裝基于ROS使用RealSense的包，現在開始我們要創造一個ROS的工作空間并且配置它。

Ctrl + Alt + T 建立一個指令行：

建立一個檔案夾命名為catkin_ws，在目錄下建立一個檔案夾src并進入:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src/

将catkin_ws/src設定為工作空間:

catkin_init_workspace

這個工作空間設定完了之後最神奇的地方在于，雖然裡面什麼也沒有，但是仍然可以進行編譯:

catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install

編譯完畢之後你可以在根目錄下看見build等檔案。

将最新的ntel® RealSense™ ROS包git到src下: 注意這一步，直接git不一定能得到最新的(雖然我也不知道為什麼)，我比較建議你們進入Github官網直接把包download到本地。

cd src
git clone https://github.com/IntelRealSense/realsense-ros.git
cd realsense-ros/
git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1`
cd ..

現在需要對工作空間再進行一次編譯，但是一部分人會出現編譯失敗(也包括我…)，請看一條報錯:

missing: ddynamic_reconfigure_DIR

缺少了ddynamic_reconfigure這個依賴包，進入Github下的Readme中查閱到:

Make sure all dependent packages are installed. You can check .travis.yml file for reference.

Specifically, make sure that the ros package ddynamic_reconfigure is installed. If ddynamic_reconfigure cannot be installed using APT, you may clone it into your workspace ‘catkin_ws/src/’ from here (Version 0.2.0)

是以需要點選此處下載下傳依賴包，解壓後将其提取到src下，再重新編譯一次:

catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install

如無意外這次可以成功。在src下不需要放其他的東西，ORB_SLAM2的包放在catkin_ws下就行了，如下圖。(我的catkin_ws寫成了catkin_s，不要介意細節)

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

主目錄下文結構:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

至此，ROS工作空間的創造就完成了，如果對工作空間感興趣可移步這裡了解更多資訊。

下面配置相機節點并測試。

運作ORB_SLAM2

終于到令人興奮的實戰環節了!其實到這一步就已經沒什麼坑了，按照官網的步驟基本都可以成功，但有些細節需要注意，我大概講一講。

首先要在工作空間内放置一個Pangolin優化庫，這就是我在之前的部落格中提到的Pangolin可能需要重新安裝的原因，你可以把之前的先解除安裝了。

在任意目錄下安裝一些依賴:

sudo apt-get install libglew-dev
sudo apt-get install libboost-dev libboost-thread-dev libboost-filesystem-dev
sudo apt-get install libpython2.7-dev
sudo apt-get install build-essential

cd到catkin_ws/ORBSLAM2下，克隆Pangolin代碼到本地倉庫并編譯:

git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake -DCPP11_NO_BOOST=1 ..
make -j

Ctrl + Alt + T 建立一個指令行：

克隆ORB_SLAM2代碼到本地倉庫:

cd catkin_ws
git clone https://github.com/raulmur/ORB_SLAM2

修改訂閱topic話題:進入

catkin_ws/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src

路徑下，找到

ros_rgbd.cc

，找到以下語句:

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);

将其修改為:

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

如下圖:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

老規矩，别忘了在bashrc下添加ORB_SLAM2的工作路徑:

用gedit或vim打開後在裡面自行添加:
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/(使用者名)/catkin_ws/ORB_SLAM2/Examples/ROS

現在你的bashrc應該總共有了以下新的路徑資訊:

source /opt/ros/kinetic/setup.bash
source ~/catkin_ws/devel/setup.sh
source ~/catkin_ws/devel/setup.bash
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/(使用者名)/catkin_ws/ORB_SLAM2/Examples/ROS
export ROS_PACKAGE_PATH=~/catkin_s:/home/(使用者名)/catkin_ws/src:/opt/ros/kinetic/share

開始編譯，回到catkin_ws/ORB_SLAM2下:

ls

你會發現有兩個可運作腳本，一個是build.sh，一個是build_ros.sh，但是都處于不能運作狀态。

賦予編譯權限:

chmod +x build.sh
chmod +x build_ros.sh

現在如果運作

ls

指令，則發現兩個檔案已經高亮标注，可以運作了。

運作第一個腳本:

./build.sh

這同樣是一個漫長的過程，如果你的電腦因為運作記憶體不足而溢出報錯(原因請看我之前的部落格)，請在gedit或者vim中修改build.sh的最後一行代碼将

make -j

改為

make -j2

或

make

；你也可以釋放更多的虛拟記憶體來防止溢出。後面的build_ros.sh同理。

編譯完後繼續編譯ros版本腳本:

./build_ros.sh

下面内容來自其他部落客，我沒有遇到這個錯誤，供參考:

在/Examples/ROS/ORB-SLAM2/CMakeLists.txt檔案下修改加-lboost_system(沒加這個的時候會有關于stl的錯誤)

set(LIBS

${OpenCV_LIBS}

${EIGEN3_LIBS}

${Pangolin_LIBRARIES}

${PROJECT_SOURCE_DIR}/…/…/…/Thirdparty/DBoW2/lib/libDBoW2.so

${PROJECT_SOURCE_DIR}/…/…/…/Thirdparty/g2o/lib/libg2o.so

${PROJECT_SOURCE_DIR}/…/…/…/lib/libORB_SLAM2.so

-lboost_system #此處

)

編譯結束後啟動相機節點:

roslaunch realsense2_camera rs_rgbd.launch

Ctrl + Alt + T 建立一個指令行：

我們先用一個yaml檔案試一下能不能運作

cd catkin_ws/ORB_SLAM2
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

一個常見的錯誤:

find: 探測到檔案系統循環；
`/opt/ros/kinetic/share/ORB_SLAM2/ORB_SLAM2' 是與
 `/opt/ros/kinetic/share/ORB_SLAM2' 相同的檔案系統循環的一部分。
[ERROR] [1570687975.286079105]: 
[registerPublisher] Failed to contact master at [localhost:11311].  Retrying...

這是由于你忘了打開相機節點的緣故。

請看一條不常見的報錯:

[email protected]:~/catkin_s$ rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/D435.yaml
段錯誤 (核心已轉儲)

這是因為在catkin_ws下你有兩個或更多的ORB_SLAM2包的緣故，請隻留下已經編譯完的版本。這個錯誤在以後使用改進版(with_pointcloud版本)的時候可能還會再冒出來一次。

再來看一條罕見的報錯:

[rospack] Error: package 'ORB_SLAM2' not found

請進入檔案路徑

/opt/ros/kinetic/share

，查找關鍵詞

ORB

。

如果你的結果是:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

證明因為某種操作你的工作路徑對應的連結丢失了，我們用

sudo ln-s

指令鏡像一個新的:(類似于超連結)

sudo ln -s /home/ushio/catkin_s/ORB_SLAM2/Examples/ROS/ORB_SLAM2 /opt/ros/kinetic/share/ORB_SLAM2

此時對應路徑下應該如下所示:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

此時再執行

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

應該不會有問題。

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

運作結果如下:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

成功!

我測試的是一個小樓梯，很明顯可以看出邊緣存在明顯的畸變，這是沒有使用自己的yaml的緣故，你需要标定自己的相機，并制作自己的yaml檔案，yaml中存儲的是你的相機内參。标定相機步驟略過。

制作yaml檔案擷取内參

國際慣例貼上Intel Realsense-D435的官方說明書，有興趣可自己琢磨。不過我還是講一下它在說什麼。

首先確定相機已經連上電腦并打開相機節點，Ctrl + Alt + T 建立一個指令行：

rostopic echo /camera/color/camera_info

此時可以看見指令行在不停地重新整理一些資訊，大概長這樣:

---
header: 
  seq: 108
  stamp: 
    secs: 1548140470
    nsecs: 392033543
  frame_id: "camera_color_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [614.4114379882812, 0.0, 324.2138671875, 0.0, 614.7125244140625, 236.86329650878906, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [614.4114379882812, 0.0, 324.2138671875, 0.0, 0.0, 614.7125244140625, 236.86329650878906, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

你應該還記得在運作

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

指令的時候，指令行界面顯示的是:

ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: RGB-D

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!


Camera Parameters: 
- fx: 613.42
- fy: 613.439
- cx: 327.191
- cy: 244.335
- k1: 0
- k2: 0
- k3: 1.04
- p1: 0
- p2: 0
- fps: 30
- color order: RGB (ignored if grayscale)

ORB Extractor Parameters: 
- Number of Features: 1000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

Depth Threshold (Close/Far Points): 2.5
New map created with 546 points

我們關注這一行代碼:

K就是相機的内參矩陣寫成行向量後的結果，四個非零數值依次對應運作

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml

指令後的這四行數值:

- fx: 613.42
- fy: 613.439
- cx: 327.191
- cy: 244.335

此時我們可以建立一個yaml字尾檔案，以yaml1為模闆修改這四個數值。

打開yaml1，另一個需要關心的數值如下:

# IR projector baseline times fx (aprox.)
Camera.bf: 30.720571899

翻閱到官方說明書第33頁:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

我們想要的資訊已經出現了，但是要确認一下。翻閱到51頁:

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

D435的Baseline基線長度為50毫米。

Camera.bf = ybaseline (in meters) * fx

。

根據此條公式計算出D435的bf值，寫入你的yaml中。

最後檔案可命名為D435.yaml

我還是貼上我的yaml，但是與你們的可能存在不同:

%YAML:D435-1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 613.420166015625
Camera.fy: 613.4390258789062
Camera.cx: 327.191462728515625
Camera.cy: 244.33473205566406

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 1.04

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 30.6710083

# 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: 50.0

# Deptmap values factor 
DepthMapFactor: 1000.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# 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

一切都完成後，確定相機插入、重新啟動相機節點并在ORB_SLAM2下運作:

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/D435.yaml

現在可以開始SLAM建圖了!

一些運作的結果

樓梯特征點比對，對于邊緣對比度高的地方，還是很容易比對的。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

建了一個兩層樓的地圖，基本沒有回環，下面測試。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

這是測試了三層樓+兩個很長的走廊的結果，可以看見上方紅色的地方已回環。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

ORB_SLAM2的回環速度和精度都蠻好的，我走到同一條路後大概兩秒它就檢測出回環了。下圖是回環成功和正在運作全局Bundle Adjustment的提示。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

可以很勉強看出來這裡有一個樓梯，畢竟是稀疏點陣，沒法做稠密的。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

地圖全貌，雖然壓根看不出是個地圖。。。

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

至此使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣就結束了。

待解決的問題

筆者在做的項目暫時在别的子產品出現了問題，是以這個SLAM就中止了，隻做到這一步。暫時的問題是:

生成點雲地圖，并儲存稀疏點陣和稠密點雲的地圖
如何在儲存地圖後計算房間的中心點
有沒有更好的開源方案可以實施?比如港科大的VINS-Mono

以後再來填坑!

從零開始跑ORB_SLAM2(三) 使用Realsense D435相機跑ORB_SLAM2生成稀疏點陣

前期準備

工作環境

創造ROS工作空間

相關配置

運作ORB_SLAM2

制作yaml檔案擷取内參

一些運作的結果

待解決的問題

繼續閱讀

ROS中工作空間的建立

國内外LiDAR SLAM實驗室總結☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆☆

基于ORB特征點比對的對極幾何限制實作（源碼+講解）

在Ubuntu16.04上提取相鄰序列圖像之間的ORB的特征點，并用暴力方法找到比對點并連線一、什麼是ORB特征二、什麼是暴力比對三、實作代碼四、運作方法

【ROS】移動機器人導航仿真（3）——定位（amcl）和路徑規劃（move_base）

ROS Navigation-----amcl簡介

Turtlebot :" where am I? " -——amcl的原理

C++實作RVIZ 2D POSE ESTIMATE 功能設定機器人初始坐标1. 首先檢視設定初始坐标的話題為 /INTIALPOSE ，檢視消息類型和格式進而決定怎麼給它發資料2.監聽RVIZ發出的資料格式3.仿照RVIZ消息格式4.編寫釋出節點程式

ROS2(二) - nav2_amcl節點概況

amcl ROS1.算法2.舉例3.節點3.1.6Transforms

ROS Navigation之amcl源碼解析（完全詳解）0. 寫在最前面1. amcl是幹什麼的2. 總體情況3. amcl_node.cpp參考

or frame [laser]: No transform to fixed frame [map]

ros安裝-Ubuntu14.04

ROS Stage學習

視覺傳感器在無人駕駛領域取得技術突破

SLAM主要步驟