ultra96v2でVitis 2020.2を使用してORB_SLAM2のFASTとDistirbuteOctTree処理をカーネル化していく話

fastとcomputeDistributeOctTreeを一つにしたfast_and_distカーネルを作っていく話。
アクセラレータにDistribution機能を移すに際して、アルゴリズムを変更し、動作確認を行った。

環境

• ubuntu:20.04
• vitis:2020.2
• yocto:zeus(https://github.com/akira-nishiyama/petalinux-ros2-manifests feature-v2020.2ブランチ)
• board:ultra96v2(https://github.com/akira-nishiyama/ultra96v2_4z.git feature-v2020.2ブランチ)

変更内容

現状のcomputeDistributeOctTreeは全ての特徴点を分割したノードに割付ていき、
分割の限界となった場合には、各々のノードで最も明確な特徴点を残し、残りは削除して処理を終了する。
または、各ノードに一つの特徴点が配置された状態となった場合に処理を終了する。
(https://www.acri.c.titech.ac.jp/wordpress/archives/7776)

これをHW処理しやすいようにノードサイズをFASTの抽出単位と揃えて固定し、そのノードを更に4分割し、
4分割ノードそれぞれで最も明確な特徴点を選出して間引くこととした。
最小ノードのサイズが同じであれば、ほぼ同じ結果が得られることが期待される。

考え方はおおよそACRiの記事と同じ。
一方でそのままだとultra96v2には乗らないので、改変する。
アルゴリズムは細かく書くと次の通りとなる。

(1)フレームを規定のサイズに分割する。(とりあえずは40x40)
(2)各フレームのFAST特徴点をiniThFASTのしきい値で抽出する。
(3)特徴点が見つからない場合にはminThFASTのしきい値で抽出する。
(4)得られた特徴点はフレームを4分割したサブフレームに割付け、サブフレームで最も明確な特徴点を一つ選び出す。

試してみるとtum-datasetでは同一頂点が抽出されるので問題無く使用できると考えられる。２つのしきい値を同時にチェックする方法もHWコストの割に高速化に効きそうなので考えておく。

build

まずはカーネルを使用せずに評価してみる。
-DDISABLE_NEW_DISTRIBUTION=OFF -DENABLE_KRNL_CHECK=ONとすると変更前後を比較して結果を標準出力するようになる。
また、-DDISABLE_NEW_DISTRIBUTION=ON -DENABLE_KRNL_CHECK=OFFとすると、オリジナルコードに近い状態で動作する。  

hashは2d04944a46ee3d2c71e5c108a376b92a5655770cで確認。

2021/5/20:比較が間違っていたので修正版を作成中

git clone https://github.com/akira-nishiyama/orb_slam_2_ros.git -b feature-fpga
cd orb_slam_2_ros
mkdir build
cd build
source <vitis-installation-path>/settings64.sh
source <sdk-installation-path>/environment-setup-aarch64-xilinx-linux
export AMENT_PREFIX_PATH=$OECORE_NATIVE_SYSROOT/usr:$OECORE_TARGET_SYSROOT/usr
export PYTHONPATH=$OECORE_NATIVE_SYSROOT/usr/lib/python3.7/site-packages:$OECORE_TARGET_SYSROOT/usr/lib/python3.7/site-packages
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TESTING=OFF -DCMAKE_NO_SYSTEM_FROM_IMPORTED=1 -GNinja -DCMAKE_INSTALL_PREFIX=<path-to-sd-root> -DPLATFORM_COMPONENTS_PATH=<path-to-components> -DBUILD_TEST_TARGETS=OFF -DBUILD_TARGET=hw -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=ON -DBUILD_FAST_TEST=OFF -DBUILD_FAST_AND_DIST_TEST=ON -DDISABLE_RESIZE_KRNL=ON -DDISABLE_FAST_KRNL=ON -DDISABLE_NEW_DISTRIBUTION=OFF

result

変更後

root@ultra96v2-zynqmp:~# time ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt /home/root/tum-dataset

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.

OpenCV version : 3.4.3
Major version : 3
Minor version : 4
Subminor version : 3
Input sensor was set to: Monocular

Loading ORB Vocabulary.
Vocabulary loaded!


Camera Parameters: 
- fx: 517.306
- fy: 516.469
- cx: 318.643
- cy: 255.314
- k1: 0.262383
- k2: -0.953104
- k3: 0.002628
- p1: 1.16331
- p2: -0.005358
- fps: 30
- bf: 0
- color order: RGB (ignored if grayscale)
Found Platform
Platform Name: Xilinx
INFO: Device found - edge
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'
file size:5634881
actual read size:5634881
program xclbin.
program xclbin Done.
create resize_accel kernel.
create resize_accel kernel Done.
Found Platform
Platform Name: Xilinx
INFO: Device found - edge
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'
file size:5634881
actual read size:5634881
program xclbin.
program xclbin Done.
create resize_accel kernel.
create resize_accel kernel Done.

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

-------
Start processing sequence ...
Images in the sequence: 798

-------

median tracking time: 0.0999473
mean tracking time: 0.0991993

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    1m45.135s
user    1m54.849s
sys 0m5.194s

real    1m43.408s
user    1m55.425s
sys 0m4.890s

オリジナル

実行時間だけ書いておく。
少し早くなっているかな？

real    2m1.869s
user    2m13.626s
sys 0m5.523s

real    2m0.575s
user    2m13.668s
sys 0m5.611s