⾼位合成によるSLAM技術のFPGA実装

株式会社アイヴィス 松本茂樹

2020.6.24

目次

FPGAとは

⾼位合成とは

背景・目的

SLAM技術のFPGA実装（概要）

共同研究：株式会社アイヴィス，株式会社セック，九州工業大学 田向研究室

SLAM技術のFPGA実装（詳細）

HW化した処理

元アルゴリズムの改造

まとめ

2

FPGAとは

FPGA: Field Programmable Gate Array

内部の論理回路構造を何度も再構成できる半導体チップ

エッジコンピューティング技術を中心に，近年注目を集めつつある補助演算プロセッサ

小型・省電力でランニングコストが低い

Intel(Altera)と，XILINXが市場を二分している

3

Intel Arria 10 XILINX ZYNQ

FPGAのアーキティクチャ

論理ブロックの配列（Logic Block），データ入出力のための I/O pad，配線用チャンネル(Routing Channel)から構成される．

FPGA回路デザインデータを書き換えることで，回路の再現に必要な要素間を連結して回路を作り出す．

4

図の出典：https://ja.wikipedia.org/wiki/FPGA

⾼位合成：回路デザインが手軽に！

5

従来のFPGA開発

HDLでコーディング

論理合成

配置・配線

FPGA回路デザインデータ(バイナリ)

OpenCLによるFPGA開発

OpenCL Cでコーディング__kernel void matrixMult( // Input and output matrices

__global float *restrict C, __global float *restrict A,__global float *restrict B, int A_width, int B_width)

{for (int a = a_start, b = b_start; a <= a_end; a += BSIZE, b += (BSIZE * B_width)){

A_local[local_y][local_x] = A[a + A_width * local_y + local_x];B_local[local_x][local_y] = B[b + B_width * local_y + local_x];for (int k = 0; k < BSIZE; ++k) {

running_sum += A_local[local_y][k] * B_local[local_x][k];}

C[get_global_id(1) * get_global_size(0) + get_global_id(0)] = running_sum;}

⾼位合成

背景・目的

元来FPGAはS/W技術者には敷居が⾼かったが，C/C++で記述可能な⾼位合成技術の発達により敷居が下がっている．

SLAM(Simultaneous Localization and Mapping)に応用

オープンソースのORB-SLAMをFPGA用に⾼位合成し，ORB-SLAMの処理⾼速化にFPGAが有効であることを示す．

6

消費電力，排熱，スペースに制限がある場合に有効な補助演算プロセッサ

FPGA適用

SLAM技術のFPGA実装（概要）

7

ORB-SLAM

カメラ映像を用いるSLAMをvSLAM(Visual SLAM)と呼ぶ.

比較的安価にSLAMを行える．

画像の輝度を直接用いる直接法と，特徴点を用いる間接法がある．

ORB-SLAMは，代表的な間接法のvSLAM

３つのスレッドが協調的に動作

8

[1] Raul Mur-Artal, Juan D. Tardos:ORB-SLAM, "An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras",

IEEE Transactions on Robotics ( Volume: 33 , Issue: 5 , 1255—1262, Oct. 2017 )

[1] ORB-SLAMの構成

Tracking:・フレームから特徴点を抽出・フレームを照合して追跡・地図情報にキーフレーム追加

Local mapping:Trackingでキーフレームが追加されたとき，局所的に地図情報を最適化

Loop closing:周回した経路を最適化

Trackingの特徴点抽出処理をFPGA化

既存の地図情報があればTrackingだけで自己位置推定可能

特徴点抽出処理に着目

画像処理が多く，並列化しやすい

Tracking全体の処理時間を，FPGA利用により約9.5倍高速化！

目標とする10FPS(100ms)以内の処理を達成

9

[1] Raul Mur-Artal, Juan D. Tardos:ORB-SLAM, "An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras",

IEEE Transactions on Robotics ( Volume: 33 , Issue: 5 , 1255—1262, Oct. 2017 )

[1] ORB-SLAMの構成

特徴点抽出処理

「高位合成によるORB-SLAMのFPGA実装と評価」 ，岩渕 甲誠，松本 茂樹，松尾 幸治，石田 裕太郎，廣瀬 尚三，長瀬 雅之，田向 権，ロボティクス・メカトロニクス講演会2019 in

Hiroshima，1AI-F06，2019年6月5-8日(6)，広島，広島国際会議場．

※1

※1

次のステップ： Local Mapping

Tracking⾼速化に伴い，他のスレッドも⾼速化させる

Local Mappingは、曲がり角などでは，ほぼ毎フレーム動作し，地図を作成することになる．

Tracking⾼速化に対してLocal Mappingの処理が追い付かず，ロストする可能性がある

10

[1] Raul Mur-Artal, Juan D. Tardos:ORB-SLAM, "An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras",

IEEE Transactions on Robotics ( Volume: 33 , Issue: 5 , 1255—1262, Oct. 2017 )

[1] ORB-SLAMの構成

Local mapping

Tracking:ORB-SLAMで想定している、10FPSの処理をクリア

Local Mappingの⾼速化の検討

SearchInNeighborsはソフトウェア側の改造で⾼速化可能

FPGA化の候補:CreateNewMapPoints,LocalBundleAdjustment

11

No 処理名 処理時間割合(ms)

処理概要

改造前 改造後

1 ProcessNewKeyFrame 13.8 10.6 KeyFrameのBoWやMapPointの法線を計算

2 MapPointCulling 0.1 0.2 最近追加された不要なMapPointの削除

3 CreateNewMapPoints 58.3 62.2 近隣のKeyFrameと新しいMapPointの作成

4 SearchInNeighbors 58.2 17.1 近隣のKeyFrameを用いてMapPointの統合

5 LocalBundleAdjustment 102.2 114.7 周辺KeyFrameとMapPointで最適化

6 KeyFrameCulling 4.9 7.7 不要なKeyFrameの削除

合計 237.5 212.5

Local Mappingの処理一覧（ソフトウェア側での改造を実施）

SLAM技術のFPGA実装（詳細）

12

FPGA開発環境

XILINX社製Zynq-UltraScale+ MPSoC ZCU102を用いる．

FPGAとARMが同梱されたSoC (System on Chip)形式

CPU :Arm Cortex-A53 クアッドコア，最大クロック 1.5GHz

FPGA :ロジックセル600KブロックRAM 32.1Mbit

⾼位合成ツールとしてXILINX社製SDSoC を用いる．

CPUで動作するソフトウェアとFPGAに生成する回路を同時に開発する事が可能

13

ORB-SLAMに沿ったFPGAデータフロー

14

画像データがパイプライン処理されるよう処理を並べる．

内部メモリに画像データを保存しないため，前段・後段それぞれに画像データ転送

8個の同じ回路を作成し，ピラミッド画像を並列に処理

以下の箇所で，line bufferを設ける

4 line buffer: ピラミッド画像作成

7 line buffer: FASTでの特徴点検出

31 line buffer: 特徴量計算画像データ

4 line buffer

補足：ラインバッファによる⾼速化

FPGAから専用メモリへのアクセス回数は少ないほど良い

画像データの場合、FPGA内にラインバッファを構成するとストリーム化できる場合がある（アクセス回数削減）

Burst Read/Writeという機能を用いれば，順次アクセスに限り，データ転送はより効率的になる

15

FPGA用メモリ

FPGA回路

メモリアクセス:多 ⇒ 処理時間:大

画像データ

画像データは、ストリーム化してFPGA回路に送りたい

畳込みカーネルサイズ： 3×3（シフトレジスタ）

2行のラインバッファ (横：画像の横サイズ)

出力画像

入力画像

入力画素のストリーム

出力画素のストリーム

例：畳込み演算への適用

FASTでの特徴点検出の改造

16

FASTでは，画像に写る構造物のコーナーを特徴点として検出する．

パイプライン化のためにランダムアクセスを排除

元実装より多く検出されるが，次の処理で間引く

画像の一部に

ランダムアクセス

が発生！

元実装：画像を等分割し，異なる閾値で最大2回検出

改造：画像全体に対して低い閾値で1回検出

特徴点削減処理の改造

17

FASTの特徴点検出の結果は，細かい構造部分に特徴点が集中するので，それを間引く

ループ依存性排除，固定ループ化するよう改造

元実装：画像を4分木構造で分割 改造：画像を固定サイズで分割

実験

18

実施内容

ARMとFPGAの処理時間を比較

改造ORB-SLAMの処理をすべてARM上で動作

改造部分をFPGAで動作

KITTI00データセット（右図）を利用

縦376ピクセル，横1241ピクセル，4541フレーム，10fps

測定する処理

(1)フレーム読み込み間隔の実時間（Tracking処理全体を含む）

(2)Tracking処理全体

(3)ピラミッド画像作成

(4)ピラミッド画像作成

＋Gaussian Blur

(5)8並列部分前段

(6)8並列部分後段

(3)

(4)

(5)

(6)

FPGA化

実験結果(4541フレームでの平均)

19

FPGA化した個別の処理(3)～(6)の処理時間は，(6)を除いて約10数倍⾼速化

ARM上での処理も含むTracking全体は約9.5倍⾼速化

フレーム読込み間隔は105.34 msで，10FPSの実時間処理をほぼ達成

処理内容 処理時間 ms 高速化倍率ARM FPGA

+ARM

(1)フレーム読込み間隔の実時間 634.42 105.34 6.0

(2)Tracking全体 579.38 61.21 9.5

(3)ピラミッド画像作成 114.23 9.56 11.9

(4)ピラミッド画像作成+ GaussianBlur 143.64 9.59 15.0

(5)8並列部分前段 196.20 12.87 15.2

(6)8並列部分後段 120.51 24.78 4.8

KITTI00データセットでの移動軌跡

軌跡のスケールに問題はあるが，軌跡の形状はほぼ同じ

改造版のARM,FPGA＋ARMともに1060フレームでロスト

⇒アルゴリズム簡約化部分の見直し

⇒変数のbit長の検討(消費リソース量を下げるために小さくした)

SLAMの精度改善が課題

20

(フレーム1~1000)

元の軌跡 ground truthのスケールに合わせた軌跡

改造ORB-SLAMでロスト

Local MappingのS/W⾼速化に関して

⾼速化の検討(S/W側の工夫)Eigenライブラリ用データ型への統一

Eigen(行列計算ライブラリ)は，g2oのグラフ最適化で利用

g2o処理の前に、OpenCV形式⇒ Eigen形式などのデータ変換が行われている．

⇒ 形式を統一すれば，変換処理の負荷が減らせる．

⇒ Eigenの行列計算処理は精錬されたS/Wである．

⇒ データ形式統一は，FPGA化を視野に入れた処置でもある．

変更箇所（Local Mapping以外も含む）最適化対象のCameraPose、MapPoint及び関連パラメータ

範囲：Trackingスレッド，LocalMappingスレッド，LoopClosingスレッド

ORB特徴点のDescriptor (BRIEF記述子)の処理 範囲：Trackingスレッド，LocalMappingスレッド

DBoW2の処理 範囲：Trackingスレッド，LocalMappingスレッド

21

Local Mappingの⾼速化の検討（※スライド11の再掲）

FPGA化の候補:CreateNewMapPoints,LocalBundleAdjustment

22

No 処理名 処理時間割合(ms)

処理概要

改造前 改造後

1 ProcessNewKeyFrame 13.8 10.6 KeyFrameのBoWやMapPointの法線を計算

2 MapPointCulling 0.1 0.2 最近追加された不要なMapPointの削除

3 CreateNewMapPoints 58.3 62.2 近隣のKeyFrameと新しいMapPointの作成

4 SearchInNeighbors 58.2 17.1 近隣のKeyFrameを用いてMapPointの統合

5 LocalBundleAdjustment 102.2 114.7 周辺KeyFrameとMapPointで最適化

6 KeyFrameCulling 4.9 7.7 不要なKeyFrameの削除

合計 237.5 212.5

Local Mappingの処理一覧（ソフトウェア側での改造を実施）

計算量が大きな部分でFPGA化で⾼速化できれば効果は大きいアルゴリズムの大きな改造が必要になるため，今回FPGA化は見送り（今後の課題）

まとめ

23

OSSを元にしたORB-SLAMのFPGA実装を行った．

FPGAに効率的な処理をさせるには，以下の改造が重要

既存ソフトウェアの処理フローを整理した上で，FPGAで効率的に処理できるよう再設計する．

メモリのランダムアクセス，ループ依存性，可変ループを排除し，パイプライン処理できるようにアルゴリズムを変更する．

ORB-SLAMのTracking処理を約10倍⾼速化！

Local MappingのFPGA化対象の候補を絞った．⇒CreateNewMapPoints，LocalBundleAdjustment