介紹

著名的開源計(jì)算機(jī)視覺庫OpenCV宣布了其將首次舉行由英特爾贊助的空間人工智能競賽。

隨著OAK-D(OpenCV-AI-Kit With Depth)模塊的發(fā)布,OpenCV呼吁參與者使用其模塊解決現(xiàn)實(shí)世界中的問題。OAK-D模塊有內(nèi)置立體攝像頭和RGB攝像頭,同時(shí)還配備了一個(gè)強(qiáng)大的視覺處理單元(來自英特爾的Myriad X),可以進(jìn)行深度神經(jīng)網(wǎng)絡(luò)推斷。

我們對這種傳感器的潛力感到興奮,于是決定在7月份提交一份競賽項(xiàng)目建議書。我們會從我們的235個(gè)提案中選出了32個(gè)!并在journal上發(fā)布一系列的博客文章,分享我們在整個(gè)開發(fā)過程中的進(jìn)展、想法和挑戰(zhàn)。

我們的工作:使用空間人工智能來檢測室內(nèi)地標(biāo)(indoor Landmarks),并使用基于圖的姿態(tài)地標(biāo)(Graph-Based Pose-Landmark)SLAM

OAK-D不僅能夠檢測物體,還能夠提供物體的相對三維位置,這使得解我們的問題成為了可能。一般來說,姿態(tài)圖SLAM問題是一個(gè)優(yōu)化問題,它使用多個(gè)移動代理的姿態(tài)估計(jì)值,并通過調(diào)整這些估計(jì)值來最小化這些估計(jì)值之間的誤差。這種優(yōu)化允許我們在其環(huán)境中定位代理,同時(shí)允許我們縫合機(jī)器人的傳感器測量值以創(chuàng)建環(huán)境的地圖表示。

大多數(shù)室內(nèi)SLAM的實(shí)現(xiàn)使用以下兩種姿態(tài)估計(jì)來源:

里程計(jì)(Wheel odometry)

2D激光雷達(dá)

在我們的實(shí)踐中,首先里程計(jì)是我們必備的來源,然后使用OAK-D來代替2D激光雷達(dá)。上圖顯示了我們建議的實(shí)現(xiàn)流程圖。OAK-D能夠唯一地解決這些問題:

1．姿態(tài)-姿態(tài)(pose-pose)約束的來源:通常,來自2D激光雷達(dá)的激光掃描可以通過使用諸如迭代最近點(diǎn)(ICP)等對準(zhǔn)算法來估計(jì)位姿圖中的位姿約束。而另外一種考慮ICP的非正式方式——如果你從兩個(gè)不同的位置得到兩個(gè)激光掃描,你需要如何移動和旋轉(zhuǎn)來使這兩個(gè)激光掃描對齊,這個(gè)運(yùn)動+旋轉(zhuǎn)給了我們一個(gè)pose-pose約束的估計(jì)。

在使用OAK-D時(shí),我們想使用類似的方法,為此,我們需要利用OAK-D深度圖(depth maps)來創(chuàng)建點(diǎn)云,然后將它們與ICP對齊,這將給我們更豐富和更準(zhǔn)確的pose-pose約束,因?yàn)槲覀兛梢岳��?D信息。

2．姿態(tài)-地標(biāo)(pose-landmark)約束的來源:使用OAK-D的主要好處是,它可以直接在邊緣進(jìn)行3D對象檢測,這允許我們使用基于深度學(xué)習(xí)的對象檢測器來檢測對象,并將它們作為姿態(tài)圖優(yōu)化問題中的地標(biāo)。

在多次OAK-D測量中觀察到的環(huán)境中任何靜止的物體都可以作為地標(biāo),因?yàn)樗�在世界坐�?biāo)系中的位置是不變的,而代理相對于地標(biāo)的相對位置是不斷進(jìn)化的。

對同一對象的觀察允許我們創(chuàng)建pose-landmark約束,其中地標(biāo)的位置被認(rèn)為是固定的。這些地標(biāo)可以作為整個(gè)室內(nèi)地圖的“錨點(diǎn)”。

有了這些源,我們就能夠創(chuàng)建一個(gè)既包含姿態(tài)-姿態(tài)約束又包含姿態(tài)-地標(biāo)約束的圖形,然后我們將使用優(yōu)化技術(shù)來優(yōu)化這些姿態(tài)的位置。固定好姿態(tài)后,我們只需縫合OAK-D觀測值(可以是RGB圖像、深度圖或點(diǎn)云)就能來創(chuàng)建室內(nèi)地圖。

機(jī)器人平臺設(shè)置- Jetson Nano + TurtleBot3 Waffle + OAK-D

接下來我們將提供一些執(zhí)行細(xì)節(jié)和我們迄今為止取得的成就。我們首先需要的是一個(gè)室內(nèi)移動機(jī)器人,我們可以在上面安裝OAK-D。該機(jī)器人必須滿足以下條件:

它提供一個(gè)里程計(jì)的來源:雖然我們可以自己添加這個(gè),但由于這不是本項(xiàng)目的主要任務(wù),所以最好使用一個(gè)已經(jīng)有馬達(dá)和編碼器的平臺。

支持機(jī)器人操作系統(tǒng)(Robot Operating System,ROS):我們的目標(biāo)是使用ROS作為本項(xiàng)目的主干,因此必須有一個(gè)支持ROS的機(jī)器人。

它應(yīng)該能夠?yàn)镺AK-D和單板計(jì)算機(jī)(SBC)供電。

為了滿足這些要求,我們修改了TurtleBot3 Waffle。我們選擇它的原因很簡單——這基本就是我們需要的!以下是它提供的功能和我們需要修改的東西:

TB3有里程計(jì):TB3配備了DynamicXel馬達(dá),內(nèi)置編碼器提供里程表。

TB3配備了OpenCR嵌入式平臺,可以為我們選擇的SBC(NVIDIA Jetson Nano)以及OAK-D提供足夠的功率。

默認(rèn)情況下,TB3帶有樹莓派3B+,但是,我們希望使用Jetson Nano作為SBC,這是因?yàn)槲覀兿Ｍ�有更高的性能余量來解決這個(gè)問題。

我們移除了TB3附帶的2D激光掃描儀。

我們將在TB3平臺上安裝OAK-D。

下圖顯示了改進(jìn)后的機(jī)器人平臺,該平臺已經(jīng)安裝并配置為與TB3一起運(yùn)行。我們還沒有安裝和外部校準(zhǔn)OAK-D。

用Jetson Nano作為TurtleBot3的單板機(jī)

如前所述,TurtleBot3默認(rèn)附帶一個(gè)樹莓派3b+,而且設(shè)置Jetson Nano作為TB3的SBC不是很難。為了讓Jetson Nano和OpenCR板通過ROS進(jìn)行通信,需要執(zhí)行以下步驟。

在OpenCR板上安裝/更新固件

1．OpenCR嵌入式平臺板負(fù)責(zé)為DYNAMIXEL電機(jī)供電并與之通信,這意味著它從馬達(dá)讀取編碼器讀數(shù)并生成里程表信息,同時(shí)接收速度指令并將其發(fā)送給這些馬達(dá),這兩個(gè)過程對這個(gè)項(xiàng)目都至關(guān)重要。我們按照這里提供的說明更新OpenCR板的固件。

在Jetson Nano上安裝TurtleBot ROS

1．由于這款Jetson Nano已經(jīng)用于其他機(jī)器人項(xiàng)目,我們已經(jīng)在上面安裝了ROS Melodic。

但是為了讓它與OpenCR板一起工作,我們需要把這個(gè)板設(shè)置成TurtleBot3的大腦,為此,我們在Jetson Nano的新ROS工作區(qū)中設(shè)置了tb3 ROS,這可以簡單地通過sudo apt install ros-melodic-turtlebot3來實(shí)現(xiàn)�，F(xiàn)在,我們可以通過micro-USB將Jetson Nano連接到OpenCR板了。

2．使用roslaunch turtlebot3_bringup turtlebot3_robot．launch啟動TB3機(jī)器人�，F(xiàn)在我們已經(jīng)準(zhǔn)備好接收里程表,并從Jetson Nano向TB3發(fā)送速度指令。

接收里程表并發(fā)送速度指令

1．我們可以啟動遠(yuǎn)程操作節(jié)點(diǎn)來控制馬達(dá)并查看里程表的更新,這可以使用roslaunch turtlebot3_teleop turtlebot3_teleop_key．launch來啟動

2．為了查看流式里程表的數(shù)值,我們使用了rostopic echo /odom。里程計(jì)信息示例如下所示:

---
header:
 seq: 987
 stamp:
   secs: 1597031283
   nsecs: 652199083
 frame_id: "odom"
child_frame_id: "base_footprint"
pose:
 pose:
   position:
     x: -0．147986590862
     y: -0．0244562737644
     z: 0．0
   orientation:
     x: 0．0
     y: 0．0
     z: 0．179800972342
     w: 0．983703017235
 covariance: [0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0]
twist:
 twist:
   linear:
     x: -0．11722844094
     y: 0．0
     z: 0．0
   angular:
     x: 0．0
     y: 0．0
     z: 0．217905953526
 covariance: [0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0, 0．0]
---

這樣,SBC和TB3的設(shè)置就完成了。下一步是將OAK-D添加到這個(gè)設(shè)置中,這將在以后的文章中討論。

從OAK-D獲取深度圖

OAK-D模塊由三個(gè)攝像頭組成:兩個(gè)單色全局快門式攝像頭,用于提供深度感知,以及一個(gè)RGB攝像頭。OAK-D提供了兩種不同的方法來估計(jì)深度。

第一種方法使用半全局匹配(SGBM)算法計(jì)算深度圖,使用左右攝像機(jī)以及在攝像機(jī)(左、右或RGB)上運(yùn)行神經(jīng)推理來執(zhí)行目標(biāo)檢測,然后使用深度圖在攝影機(jī)的參照系中定位檢測到的對象。

第二種方法在左右兩個(gè)攝像頭上并行運(yùn)行神經(jīng)網(wǎng)絡(luò)推理,來估計(jì)兩幅圖像之間的差異。檢測到特征之后利用相機(jī)的內(nèi)在特性計(jì)算它們的三維位置。在這個(gè)項(xiàng)目中,我們將利用第一種方法,因?yàn)樗�同時(shí)為我們提供了一個(gè)密集的深度圖(用于創(chuàng)建點(diǎn)云)和三維物體檢測(在SLAM問題中用作地標(biāo))。

圖3:OAK-D模塊帶有兩個(gè)用于深度的黑白相機(jī),中間有一個(gè)RGB相機(jī)。

圖4:OAK-D相機(jī)的樣本深度圖。如圖所示,它可以檢測對象(這里是一個(gè)人),還可以報(bào)告它們在相機(jī)坐標(biāo)系中的三維位置

在我們開始流式傳輸來自O(shè)AK-D的深度圖之前,我們首先需要獲得內(nèi)在的攝像機(jī)校準(zhǔn)矩陣,這對于將深度圖轉(zhuǎn)換為點(diǎn)云至關(guān)重要(如下一節(jié)所述)。我們能夠通過遵循官方depthaiapi for OAK-D的立體聲校準(zhǔn)教程獲得OAK-D的固有參數(shù)。

DepthAI API支持Python 3和C++,可以用于從OAK-D中流數(shù)據(jù)。由于我們的ROS版本(MyDoice)不支持Python 3,所以我們決定使用C++ API。

我們遵循這里提供的示例,經(jīng)過一些嘗試和錯(cuò)誤之后,在DepthAI開發(fā)人員的幫助下,我們構(gòu)建一個(gè)能夠從OAK-D傳輸深度圖的程序。這些深度圖將通過ROS發(fā)布,以供下游節(jié)點(diǎn)使用。在接下來的幾周里,我們將致力于提高這些深度圖的質(zhì)量。

將深度圖轉(zhuǎn)換為點(diǎn)云

使用OAK-D的好處之一是它能夠生成原始深度圖。深度圖的每個(gè)像素都包含估計(jì)深度信息。通常,當(dāng)我們知道攝像機(jī)的固有校準(zhǔn)矩陣(我們稱之為K)之后,我們可以將場景中的3D點(diǎn)投影到2D圖像平面上,這個(gè)過程稱為透視投影。

3x3固有校準(zhǔn)矩陣由4個(gè)不同的參數(shù)組成。

1．f?和f?是水平和垂直焦距,它們是從相機(jī)中心(針孔)到圖像平面的距離測量,以像素為單位。2．o?和o?是表示像面光學(xué)中心的主要點(diǎn)。給定一個(gè)三維點(diǎn),在相機(jī)坐標(biāo)系中有坐標(biāo)(X,Y,Z),當(dāng)投影到圖像平面上時(shí),我們可以使用以下公式檢索像素位置:

上面的方程告訴我們,如果我們知道像素的固有校準(zhǔn)矩陣和深度,或者Z坐標(biāo),我們也可以將坐標(biāo)(u,v)的2D像素反投影到3D空間中。求解X和Y,我們得到以下方程:

為了構(gòu)造由N個(gè)點(diǎn)組成的點(diǎn)云(其中N等于2D深度圖中的像素?cái)?shù)),你可以簡單地訪問深度圖中的每個(gè)像素并計(jì)算該點(diǎn)的(X,Y,Z)逆投影,但是使用雙重循環(huán)訪問每個(gè)像素可能會非常消耗資源的,因此我們可以使用線性代數(shù)以更具計(jì)算優(yōu)化的方式來解決這一問題。

利用上面的方程,我們只需要找到內(nèi)部校準(zhǔn)矩陣的逆,然后進(jìn)行矩陣乘法。根據(jù)上面的方程,我們可以導(dǎo)出K??內(nèi)部的元素。

這個(gè)過程使我們能夠?qū)�來自O(shè)AK-D的深度圖轉(zhuǎn)換成點(diǎn)云。上圖顯示了使用此過程生成的示例點(diǎn)云,這個(gè)點(diǎn)云沒有進(jìn)行二次采樣或清理,也沒有從原始深度圖生成,這就是為什么質(zhì)量不高的原因,這將在將來得到改進(jìn)。

我們使用迭代最近點(diǎn)(ICP)算法進(jìn)行點(diǎn)云注冊過程將在以后的博客文章中討論。

團(tuán)隊(duì)成員Deepak Talwar目前是圣何塞州立大學(xué)計(jì)算機(jī)工程專業(yè)最后一年的碩士研究生,專注于機(jī)器人技術(shù)和人工智能。他曾在FarmWise實(shí)驗(yàn)室實(shí)習(xí),目前在DeepMap公司實(shí)習(xí)。Seung Won Lee最近從圣何塞州立大學(xué)獲得計(jì)算機(jī)工程碩士學(xué)位,在那里他完成了各種與計(jì)算機(jī)視覺相關(guān)的項(xiàng)目,特別是在自動駕駛方面。他的碩士論文“Enhancing Point Cloud Density using Stereo Images”采用了規(guī)則點(diǎn)云和偽激光雷達(dá)點(diǎn)云相結(jié)合的方法來提高三維目標(biāo)的檢測能力。Sachin Guruswamy最近從圣何塞州立大學(xué)獲得了計(jì)算機(jī)工程碩士學(xué)位,主攻機(jī)器人技術(shù)和計(jì)算機(jī)視覺,他目前在Depth．a(chǎn)i產(chǎn)品上工作。