基于ORB特征的三維定位與地圖構(gòu)建算法

這文章相當(dāng)于被追屁股后面寫的東西了，拿到地平線的板子一直準(zhǔn)備做點(diǎn)什么東西出來。

不辜負(fù)是不辜負(fù)，有個(gè)雷達(dá)能干嘛，裝頭上嗎？

自到我看見了這個(gè)塵封已久的小飛機(jī)

dji專屬教育小飛機(jī)，嘖，可以編程哪種

寫過無數(shù)了，感興趣的大家自己搜索一下

x3很牛，TT也牛，但是我不牛，我得讓他牛，所以我決定搞個(gè)SLAM使用。TT有一個(gè)單目的相機(jī)，還有一個(gè)IMU，以及開放的接口。所以有這個(gè)潛力，其次也能學(xué)一手ROS2.事實(shí)上是我太年輕了，后面再說。

在SLAM框架上面我肯定自己寫不了，這里研究了一下午，使用了ORB-SLAM。

ORB-SLAM是一種基于ORB特征的三維定位與地圖構(gòu)建算法（SLAM）[1]。該算法由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年發(fā)表在IEEE Transactions on Robotics。ORB-SLAM基于PTAM架構(gòu)，增加了地圖初始化和閉環(huán)檢測的功能，優(yōu)化了關(guān)鍵幀選取和地圖構(gòu)建的方法，在處理速度、追蹤效果和地圖精度上都取得了不錯(cuò)的效果。要注意ORB-SLAM構(gòu)建的地圖是稀疏的。

ORB-SLAM一開始基于monocular camera，后來擴(kuò)展到Stereo和RGB-D sensor上。作者好像還會做Semi-dense mapping的擴(kuò)展。作者的開源代碼都在GIT上。

ORB-SLAM算法的一大特點(diǎn)是在所有步驟統(tǒng)一使用圖像的ORB特征。ORB特征是一種非?？焖俚奶卣魈崛》椒ǎ哂行D(zhuǎn)不變性，并可以利用金字塔構(gòu)建出尺度不變性。使用統(tǒng)一的ORB特征有助于SLAM算法在特征提取與追蹤、關(guān)鍵幀選取、三維重建、閉環(huán)檢測等步驟具有內(nèi)生的一致性。

架構(gòu)圖

ORB-SLAM利用三個(gè)線程分別進(jìn)行追蹤、地圖構(gòu)建和閉環(huán)檢測。

一、追蹤

ORB特征提取

初始姿態(tài)估計(jì)（速度估計(jì)）

姿態(tài)優(yōu)化（Track local map，利用鄰近的地圖點(diǎn)尋找更多的特征匹配，優(yōu)化姿態(tài)）

選取關(guān)鍵幀

二、地圖構(gòu)建

加入關(guān)鍵幀（更新各種圖）

驗(yàn)證最近加入的地圖點(diǎn)（去除Outlier）

生成新的地圖點(diǎn)（三角法）

局部Bundle adjustment（該關(guān)鍵幀和鄰近關(guān)鍵幀，去除Outlier）

驗(yàn)證關(guān)鍵幀（去除重復(fù)幀）

三、閉環(huán)檢測

選取相似幀（bag of words）

檢測閉環(huán)（計(jì)算相似變換（3D<->3D，存在尺度漂移，因此是相似變換），RANSAC計(jì)算內(nèi)點(diǎn)數(shù)）

融合三維點(diǎn)，更新各種圖

圖優(yōu)化（傳導(dǎo)變換矩陣），更新地圖所有點(diǎn)。

先別看這么高大上，需要做修改的。這個(gè)ORB_SLAM2 庫不用Pangolin可視化，而是通過 ROS2 topcis 發(fā)布所有數(shù)據(jù)。

而且這里也需要對TT飛機(jī)來一套完整的ROS封裝。下篇文章講封裝，在這之前還需要對X3上面的ROS2進(jìn)行一個(gè)熟悉。

這里使用的編輯器是VSCode，記得密碼是root

提前看好自己的IP

ssh root@ip

官方的安裝文檔很好了，這里不寫，記得運(yùn)行前必須加一下，當(dāng)然了我這里建議是可以一勞永逸的寫好

就這樣，下面是我的SLAM庫

但是我也不理解，寫了就好像沒有寫一樣

等等我找到了！

tros。。。地平線自己改了，不過注意后面錄還是不對，沒有foxy了

對了在VSCode里面推薦安裝這些插件

Python的話是安裝這些

而且可以使用Jupyter，記得pip3 install jupyter

運(yùn)行的都可以的

PIP也是可以的

自動補(bǔ)全的速度還行

第一個(gè)就是我們的tros了

不過這個(gè)好像就不管用了

這些說的都對

但是這里指出的是源應(yīng)該是墻外的，所以這里需要全局的安排

接下來試試攝像頭：

應(yīng)該是插入一個(gè)攝像頭了，但是看源文件

在tos的文件夾里面

/opt/tros/share/hobot_usb_cam/launch/hobot_usb_cam.launch.py

源碼的位置

參數(shù)是dev下面的

應(yīng)該是要這個(gè)

這里啟動失敗，是不是是因?yàn)槲业臋?quán)限不夠

報(bào)錯(cuò)是沒有？？？

再開一個(gè)terimal，看看列表

也知道，節(jié)點(diǎn)都是Python的程序，ros的命令是腳本。

在tros下面的share是存放公開節(jié)點(diǎn)的地方

就像這樣

包括我們的demo也是在這里的

工作溫度：-25°Cto65°C穩(wěn)定工作溫度：20°Cto60°C生產(chǎn)工藝：SMT(ROSH)物距：60CM-150CM解析度：800LW/PH(Center)芯片型號：JX_F37芯片尺寸：1/3最大分辨率：1920*1080像素點(diǎn)大小：2.7um*2.7um最大圖像傳輸速率：30fps(1920*1080)信噪比：36db動態(tài)范圍：79db芯片型號：JX_F37芯片尺寸：1/3最大分辨率：1920*1080像素點(diǎn)大?。?.7um*2.7um最大圖像傳輸速率：30fps(1920*1080)信噪比：36db動態(tài)范圍：79db

這個(gè)就是我們的MIPI的相機(jī)

https://developer.horizon.ai/sunrise

售賣的位置，感謝大佬還給了個(gè)攝像頭：

害，白嫖笑嘻嘻

source /opt/tros/setup.bash  ros2 topic list

我個(gè)人是習(xí)慣看看話題的情況的

# 配置 TogetherROS 環(huán)境：source /opt/tros/local_setup.bash# launch 方式啟動ros2 launch mipi_cam mipi_cam.launch.py

開啟一個(gè)新的終端，開始把攝像頭開啟

攝像頭開啟了一個(gè)新的線程

這個(gè)就是發(fā)布的信息

可以對比的看

# 一個(gè)終端編碼source /opt/tros/local_setup.bashros2 run hobot_codec hobot_codec_republish --ros-args -p channel:=1 -p in_mode:=shared_mem -p in_format:=nv12 -p out_mode:=ros -p out_format:=jpeg -p sub_topic:=/hbmem_img -p pub_topic:=/image_jpeg

我們的數(shù)據(jù)在發(fā)送給屏幕顯示的時(shí)候，需要編碼壓縮，所以一個(gè)新的終端

這個(gè)就是mjpeg的壓縮節(jié)點(diǎn)

命令

# 再起一個(gè)終端source /opt/tros/local_setup.bash# 啟動nginx，nginx只需啟動一次，如前面已啟動過nginx，則無需再次啟動cd /opt/tros/lib/websocket/webservice && chmod +x ./sbin/nginx &&./sbin/nginx -p .# 啟動websocketros2 run websocket websocket --ros-args -p image_topic:=/image_jpeg -p image_type:=mjpeg -p only_show_image:=true

在起一個(gè)節(jié)點(diǎn)來把我們的mjpeg的信息通過服務(wù)器發(fā)給瀏覽器

然后再瀏覽器打開就行

結(jié)果

我們可以看到各個(gè)節(jié)點(diǎn)的情況

先把硬件打開

把原生的raw數(shù)據(jù)來發(fā)送，并且解碼成mjpeg

接著就是把數(shù)據(jù)轉(zhuǎn)發(fā)到瀏覽器

事已至此我們在繼續(xù)玩

把一個(gè)Mac連入我的熱點(diǎn)，打開IP就可以播放（我在吃西紅柿）

這里充分的展示了ROS的方便之處。

打開我的DELL，里面也有ROS，來個(gè)全節(jié)點(diǎn)之間的使用

首先是可以讀取發(fā)送的節(jié)點(diǎn)信息

但是rviz2打不開

定位到Cmake,是有這個(gè)庫的

在這里‘

修了半天也玩不來。

把ROS2卸載了重裝看看怎么樣？（好了）

卸載了重裝了一次，記得最后加一個(gè)--fixmissing的東西

板子上面運(yùn)行這種東西也沒有什么毛病

之后我會給安裝腳本，但sudo就可以的

對了在VSCode里面打開新的目錄是使用這個(gè)命令

打開后你記得選擇信任

抱歉了，本來想無梯子的，不地不掛了，有的文件有點(diǎn)難搞

這個(gè)是編譯的TT ROS包，大概一分鐘吧，DELL 幾秒鐘

救命。。。連個(gè)git也沒有

這個(gè)是噩夢的開始，慢不說，內(nèi)存還爆了

看不懂？

就是板子編譯東西的時(shí)候，沒內(nèi)存維持正常運(yùn)行就宕機(jī)了

兩位兄臺的建議很好，但是太慢了，我不準(zhǔn)備在X3上面編譯了，我試了腳本為1，但是好慢。

目前TT ROS搞好了，就差SLAM了