2021年5月28日金曜日

enqueueMigrateMemObjectを使ってみたときのメモ

使い方

1.cl::Bufferを宣言する

hostが書き込む場合はCL_MEM_READ_ONLYのflagを指定する。
hostが読み込む場合はCL_MEM_WRITE_ONLYのflagを指定する。
exampleには無いが、ドキュメントではCL_MEM_ALLOC_HOST_PTRを使う方が良いらしい。

cl::Buffer *imageToDevice_fast;
cl::Buffer *buffer_vec;
imageToDevice_fast = new cl::Buffer(*context, CL_MEM_READ_ONLY | CL_MEM_ALLOC_HOST_PTR, fast_and_dist_in_size_bytes, NULL, &err);
buffer_vec = new cl::Buffer(*context, CL_MEM_WRITE_ONLY | CL_MEM_ALLOC_HOST_PTR, vector_size, NULL, &err);

2.Kernelにcl::Bufferを紐付ける

cl::Kernel *fast_krnl;
fast_krnl = new cl::Kernel(program,"fast_and_dist_accel", &err);
fast_krnl->setArg(0, *imageToDevice_fast);
fast_krnl->setArg(4, *buffer_vec);

3.cl::Bufferのポインタを取得する

unsigned char *host_write_ptr_fast;
host_write_ptr_fast = (unsigned char *)q->enqueueMapBuffer(*imageToDevice_fast,CL_TRUE,CL_MAP_WRITE,0,rows_fast*cols_fast*sizeof(unsigned char),nullptr,nullptr,&err);
kernel_write_ptr_fast = (unsigned int *)q->enqueueMapBuffer(*buffer_vec,CL_TRUE,CL_MAP_READ,0,vector_size,nullptr,nullptr,&err);

4.書き込む場合は取得したポインタを使用してcl::Bufferを操作する

for(int k=0; k < rows_fast; ++k){
    memcpy( (host_write_ptr_fast + k*buffer_row_pitch),
            (img_ptr + (k+(unsigned int)iniY)*host_row_pitch + (unsigned int)iniX),
            buffer_row_pitch);
}

5.enqueueMigrateMemObjectsでcl::Bufferを指定する。

q->enqueueMigrateMemObjects({*imageToDevice_fast},0,nullptr, &write_event);

6.読み出す場合はenqueueMigrateMemObjectsの実行後にポインタを介してデータ取得する。

if((kernel_write_ptr_fast[ii] & 0x80000000) != 0){
    // statements
}

感想

KernelにBufferを紐付けるのは最後でも良い気がする。そうすれば前処理したデータをcl::Bufferとしてたくさん持っておいて、これを逐次切り替えながらkernelを動作させるということも可能そう。

2021年5月21日金曜日

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

fastとcomputeDistributeOctTreeを一つにしたfast_and_distカーネルを作っていく話その3。

入力画像をcloneしてenqueueWriteBufferに突っ込んでいる所がすごく気になっていたので改善方法探ったら、
enqueueWriteBufferRectのメソッドがあったのでこれを使ってみたところ少し改善した。
また、同時にカーネル側も改造し、フィルタ機能を追加した。

変更内容

hostプログラムの変更

2021/5/26 下記は初回の転送だけしか行われないよう。テストしたつもりだったが、その後は動作確認できないので、結局はenqueueMigrateMemObjectを使った方法に変更することとなった。

buffer_originはFPGAに確保したメモリ上の転送先を{x,y,z}の配列で指定する。
カーネルの転送先は原点を指定したいので{0,0,0}

host_originはホストメモリ上の転送元を{x,y,z}の配列で指定する。
hostにある解析対象画像のFASTによって特徴点抽出した上で間引き処理したい部分の開始地点を指定する。
2次元の場合はz=0を指定する必要がある。

regionは矩形領域のサイズを{x,y,z}の配列で指定する。
2次元の場合はz=1を指定する必要がある。

buffer_row_pitchはカーネルの1行の長さを指定する。
buffer_slice_pitchは3次元の場合に使う模様。今回は2次元なので0。

~~host_row_pitchは元画像の１行の長さを指定する。 host_slice_pitchも同様。こちらも0。~~

~~ptrはhost上のメモリにある画像のポインタを指定する。~~


array<size_t,3> host_origin = {	(((unsigned int)iniX) * sizeof(unsigned char)),
										(((unsigned int)iniY) * sizeof(unsigned char)),
										0}; // should be 0 in 2d.
array<size_t,3> region = {	cols_fast * sizeof(unsigned char),
									rows_fast * sizeof(unsigned char),
                                    1};
size_t buffer_row_pitch = cols_fast * sizeof(unsigned char);
size_t host_row_pitch = mvImagePyramid[level].cols * sizeof(unsigned char);

err = q->enqueueWriteBufferRect(	*imageToDevice_fast,        // buffer on the FPGA
									CL_TRUE,                    // blocking call
                                    {0,0,0},                    // size_t<3> buffer offset in bytes
                                    host_origin,                // size_t<3> host offset in bytes
                                    region,                     // size_t<3> region
                                    buffer_row_pitch,           // size_t buffer_row_pitch
                                    0,                          // size_t buffer_slice_pitch
                                    host_row_pitch,             // size_t host_row_pitch
                                    0,                          // size_t host_slice_pitch
                                    mvImagePyramid[level].data, // Pointer to the data to copy
                                    nullptr, &event);

カーネルの変更

フレームを4分割したサブフレームでの最大値を探索して、最後に結果を出力する。
メモリアクセスのコスト下げるためにコーナースコアは最終ワードにまとめた。
気休めだと思うけどレジスタに配置されるよう個別に宣言して全てif文で比較する。

コードは以下の通り。

#include "xf_fast_and_dist_config.h"

template <int PW, int SRC_T, int ROWS, int COLS, int NPC>
void fastResult2Vec(xf::cv::Mat<SRC_T, ROWS, COLS, NPC>& in_img, ap_uint<32>* vec_out){

    int read_index = 0;
    ap_uint<32> vec0 = 0;
    ap_uint<32> vec_resp0 = 0;
    ap_uint<32> vec1 = 0;
    ap_uint<32> vec_resp1 = 0;
    ap_uint<32> vec2 = 0;
    ap_uint<32> vec_resp2 = 0;
    ap_uint<32> vec3 = 0;
    ap_uint<32> vec_resp3 = 0;

    for (int row = 0; row < in_img.rows; row++) {
#pragma HLS PIPELINE
        for (int col = 0; col < in_img.cols; col++) {
            ap_uint<8> temp = in_img.read(read_index++);
            if(temp != 0){
                if ((col <= in_img.cols/2) && (row <= in_img.rows/2) && (temp > vec_resp0)){vec0 = 0x80000000 + (row << 15) + col; vec_resp0 = temp;}
                if ((col >  in_img.cols/2) && (row <= in_img.rows/2) && (temp > vec_resp1)){vec1 = 0x80000000 + (row << 15) + col; vec_resp1 = temp;}
                if ((col <= in_img.cols/2) && (row >  in_img.rows/2) && (temp > vec_resp2)){vec2 = 0x80000000 + (row << 15) + col; vec_resp2 = temp;}
                if ((col >  in_img.cols/2) && (row >  in_img.rows/2) && (temp > vec_resp3)){vec3 = 0x80000000 + (row << 15) + col; vec_resp3 = temp;}
            }
        }
    }
#ifndef __SYNTHESIS__
    std::cout << std::dec;
    std::cout << "rows,cols:" << in_img.rows << "," << in_img.cols << std::endl;
    std::cout << "vec0:" << std::hex << vec0 << std::endl;
    std::cout << "vec1:" << std::hex << vec1 << std::endl;
    std::cout << "vec2:" << std::hex << vec2 << std::endl;
    std::cout << "vec3:" << std::hex << vec3 << std::endl;
    std::cout << "vec4:" << std::hex << ((vec_resp3 << 24) + (vec_resp2 << 16) + (vec_resp1 << 8) + vec_resp0) << std::endl;
#endif
    vec_out[0] = vec0;
    vec_out[1] = vec1;
    vec_out[2] = vec2;
    vec_out[3] = vec3;
    vec_out[4] = (vec_resp3 << 24) + (vec_resp2 << 16) + (vec_resp1 << 8) + vec_resp0;
    return;
}

extern "C" {

void fast_and_dist_accel(   ap_uint<PTR_WIDTH>* img_in, unsigned char threshold, /*ap_uint<PTR_WIDTH>* img_out,*/ int rows, int cols,
                            ap_uint<32>* vec_out) {
// clang-format off
    #pragma HLS INTERFACE m_axi      port=img_in        offset=slave  bundle=gmem0
    
//    #pragma HLS INTERFACE m_axi      port=img_out       offset=slave  bundle=gmem1
    #pragma HLS INTERFACE m_axi      port=vec_out       offset=slave  bundle=gmem0
    #pragma HLS INTERFACE s_axilite  port=rows
    #pragma HLS INTERFACE s_axilite  port=cols
    #pragma HLS INTERFACE s_axilite  port=threshold
    #pragma HLS INTERFACE s_axilite  port=return
    // clang-format on

    xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC1> imgInput(rows, cols);
    xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC1> imgOutput(rows, cols);
// clang-format off
    #pragma HLS DATAFLOW
    // clang-format on

    // Retrieve xf::cv::Mat objects from img_in data:
    xf::cv::Array2xfMat<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(img_in, imgInput);

    // Run xfOpenCV kernel:
    xf::cv::fast<NMS, TYPE, HEIGHT, WIDTH, NPC1>(imgInput, imgOutput, threshold);

    // Convert _dst xf::cv::Mat object to output array:
    //xf::cv::xfMat2Array<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(imgOutput, img_out);

    // Convert _dst xf::cv::Mat objecto to output vector
    fastResult2Vec<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(imgOutput, vec_out);

    return;
} // End of kernel

} // End of extern C

result

https://github.com/akira-nishiyama/orb_slam_2_ros.gitのfeature-fpgaブランチのhash:47ca7053e207f37d705f47d570360982ccc0743fをビルド・実行すると、以下の結果が得られる。
少し改善したが、劇的には変わらない。

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'
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'
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.170544
mean tracking time: 0.170566

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    2m42.178s
user    2m5.889s
sys 0m25.341s

2021年5月14日金曜日

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

続けて間引き処理をカーネルに追加していく。

フレームを4分割したサブフレームでの最大値を探索して、最後に結果を出力する。
メモリアクセスのコスト下げるためにコーナースコアは最終ワードにまとめた。
気休めだと思うけどレジスタに配置されるよう個別に宣言して全てif文で比較する。
カーネルの探索範囲を40x40の領域に限定すると32bitに全ての情報載せられそうだけど、改善は微々たるものと思うのでとりあえず保留。

カーネル部分のコードは以下の通り。

#include "xf_fast_and_dist_config.h"

template <int PW, int SRC_T, int ROWS, int COLS, int NPC>
void fastResult2Vec(xf::cv::Mat<SRC_T, ROWS, COLS, NPC>& in_img, ap_uint<32>* vec_out){

    int read_index = 0;
    ap_uint<32> vec0 = 0;
    ap_uint<8> vec_resp0 = 0;
    ap_uint<32> vec1 = 0;
    ap_uint<8> vec_resp1 = 0;
    ap_uint<32> vec2 = 0;
    ap_uint<8> vec_resp2 = 0;
    ap_uint<32> vec3 = 0;
    ap_uint<8> vec_resp3 = 0;

    for (int row = 0; row < in_img.rows; row++) {
#pragma HLS PIPELINE
        for (int col = 0; col < in_img.cols; col++) {
            ap_uint<8> temp = in_img.read(read_index++);
            if(temp != 0){
                if ((col <= COLS/2) && (row <= ROWS/2) && (temp > vec0)){vec0 = 0x80000000 + (row << 15) + col; vec_resp0 = temp;}
                if ((col >  COLS/2) && (row <= ROWS/2) && (temp > vec1)){vec1 = 0x80000000 + (row << 15) + col; vec_resp1 = temp;}
                if ((col <= COLS/2) && (row >  ROWS/2) && (temp > vec2)){vec2 = 0x80000000 + (row << 15) + col; vec_resp2 = temp;}
                if ((col >  COLS/2) && (row >  ROWS/2) && (temp > vec3)){vec3 = 0x80000000 + (row << 15) + col; vec_resp3 = temp;}
            }
        }
    }
    vec_out[0] = vec0;
    vec_out[1] = vec1;
    vec_out[2] = vec2;
    vec_out[3] = vec3;
    vec_out[4] = (vec_resp3 << 24) + (vec_resp2 << 16) + (vec_resp1 << 8) + vec_resp0;
    return;
}

extern "C" {

void fast_and_dist_accel(   ap_uint<PTR_WIDTH>* img_in, unsigned char threshold, /*ap_uint<PTR_WIDTH>* img_out,*/ int rows, int cols,
                            ap_uint<32>* vec_out) {
// clang-format off
    #pragma HLS INTERFACE m_axi      port=img_in        offset=slave  bundle=gmem0
    
//    #pragma HLS INTERFACE m_axi      port=img_out       offset=slave  bundle=gmem1
    #pragma HLS INTERFACE m_axi      port=vec_out       offset=slave  bundle=gmem1
    #pragma HLS INTERFACE s_axilite  port=rows
    #pragma HLS INTERFACE s_axilite  port=cols
    #pragma HLS INTERFACE s_axilite  port=threshold
    #pragma HLS INTERFACE s_axilite  port=return
    // clang-format on

    xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC1> imgInput(rows, cols);
    xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC1> imgOutput(rows, cols);
// clang-format off
    #pragma HLS DATAFLOW
    // clang-format on

    // Retrieve xf::cv::Mat objects from img_in data:
    xf::cv::Array2xfMat<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(img_in, imgInput);

    // Run xfOpenCV kernel:
    xf::cv::fast<NMS, TYPE, HEIGHT, WIDTH, NPC1>(imgInput, imgOutput, threshold);

    // Convert _dst xf::cv::Mat object to output array:
    //xf::cv::xfMat2Array<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(imgOutput, img_out);

    // Convert _dst xf::cv::Mat objecto to output vector
    fastResult2Vec<PTR_WIDTH, TYPE, HEIGHT, WIDTH, NPC1>(imgOutput, vec_out);

    return;
} // End of kernel

} // End of extern C

ビルドは以下の通り。hashはfcf6cdc2661a448b21190a7dd9b710e9b507f545

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=OFF -DBUILD_FAST_TEST=OFF -DBUILD_FAST_AND_DIST_TEST=ON -DDISABLE_RESIZE_KRNL=ON -DDISABLE_FAST_KRNL=OFF

実行結果は次の通り。
時間はそこそこ短くなったけど全然及ばないなぁ。
hostプログラムのclone処理が悪いと思うが。。。

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:5625745
actual read size:5625745
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:5625745
actual read size:5625745
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.188161
mean tracking time: 0.188687

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    2m56.512s
user    2m35.888s
sys 0m55.475s

2021年5月13日木曜日

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

2021年5月5日水曜日

ultra96v2でVitis 2020.2を使用してcv::fastをxf::cv::fastに置き換えて見た話

xf::cv::fastはコーナ部分をMatで固定値255として出力してくる。ORB_SLAM2ではコーナスコアを使用して特徴点の数を間引くので、そのままだと不都合がある。

そこでcv::fastで出力している特徴点リストと、明確さ(CornerScore)をxf::cv::fastから出力するように変更する。直接変更座標を出力するようにしたほうがリソースと速度の面から有利かもしれないので、後の改善項目として検討する。

この後は間引き処理もカーネルに取り込み、全体の並列動作を調整していく。

環境

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ブランチ)

変更点

デフォルトではコーナースコアが出力されないので、以下のように変更する。

diff --git a/hw/hw_kernel/vision/include/features/xf_fast.hpp b/hw/hw_kernel/vision/include/features/xf_fast.hpp
index a7aa1b1..935c0a2 100644
--- a/hw/hw_kernel/vision/include/features/xf_fast.hpp
+++ b/hw/hw_kernel/vision/include/features/xf_fast.hpp
@@ -201,7 +201,8 @@ void xFfastProc(XF_PTNAME(DEPTH) OutputValues[XF_NPIXPERCYCLE(NPC)],
         // NMS Score Computation
         if (iscorner) {
             xFCoreScore(flag_d[i], _threshold, &core);
-            pack_corners.range(ix + 7, ix) = 255;
+            //pack_corners.range(ix + 7, ix) = 255;
+            pack_corners.range(ix + 7, ix) = core;
         } else
             pack_corners.range(ix + 7, ix) = 0;
         ix += 8;
@@ -570,7 +571,8 @@ void xFnmsProc(XF_PTNAME(DEPTH) OutputValues[XF_NPIXPERCYCLE(NPC)],
         if ((src_buf[1][1] > src_buf[1][0]) && (src_buf[1][1] > src_buf[1][2]) && (src_buf[1][1] > src_buf[0][0]) &&
             (src_buf[1][1] > src_buf[0][1]) && (src_buf[1][1] > src_buf[0][2]) && (src_buf[1][1] > src_buf[2][0]) &&
             (src_buf[1][1] > src_buf[2][1]) && (src_buf[1][1] > src_buf[2][2])) {
-            pix = 255;
+            //pix = 255;
+            pix = src_buf[1][1];
         } else {
             pix = 0;
         }

sw_emu

build

OpenCVの結果と比較するテストプログラムで確認する。 hashは85f0af60868b7a0ba831647bcfd453f4b6b19c6c

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=/media/akira/usr -DPLATFORM_COMPONENTS_PATH=/home/akira/work/ultra96v2 -DBUILD_TEST_TARGETS=OFF -DBUILD_TARGET=sw_emu -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=OFF -DBUILD_FAST_TEST=OFF -DBUILD_FAST_AND_DIST_TEST=ON -DDISABLE_RESIZE_KRNL=ON -DDISABLE_FAST_KRNL=OFF
ninja -j4

run

cd hw/hw_link/package_sw_emu/
source <vitis-installation-path>/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 2222 22

ターミナルで以下の通り実行する。

cd /mnt/sd-mmcblk0p1
export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
export XCL_EMULATION_MODE=sw_emu
export XILINX_XRT=/usr
export XILINX_VITIS=/mnt/sd-mmcblk0p1
./xf_fast_and_dist_tb 128x128_1.png

result

============ xf_fast_and_dist test starts ============
INFO: Running OpenCL section.
Found Platform
Platform Name: Xilinx
INFO: Device found - avnet_com_av_ULTRA96V2_1_0
XCLBIN File Name: krnl_fast
INFO: Importing xclbin/krnl_fast.xclbin
Loading: 'xclbin/krnl_fast.xclbin'
ocv_result:511
hls_result:511
Test Passed
The maximum depth reached by any of the 2 hls::stream() instances in the design is 16384

2021年5月1日土曜日

orb_slam2のcv::fastをxf::cv::fastで置き換えてみた話

orb_slam2のcv::fastをxf::cv::fastで置き換えてみた話。
単純な置き換えで並列化もしてないために更に遅くなった。
間引き処理まで含めて一度最適化してみようかとは思う。

xf::cv::fastは8の倍数の列数をカーネルに指定しないと応答が帰ってこなくなる模様。
XF_NPPC1をNPSに指定していれば大丈夫と考えていたが、実際に動作させてみるとenqueueReadBufferか、 enqueueTaskのイベントが完了しなくなる事象を確認した。
8の倍数を指定すると正常にイベントが完了するのか、処理が先に進む。面倒なので原因は追って無いが、enqueueTasの完了条件満たせなくなるのだろうと思っている。
なので、オリジナルの実装から少し変更し、固定サイズセルに分割するように変更した。この辺はACRiの記事と同じかな。

また、orb_slam2ではcv::fast処理を行って得られたkeypointのresponse値を使用して特徴点の間引きを行っているが、 xf::cv::fastはkeypointが得られず、response値も得られない実装となっている。とりあえずは動作速度の確認のために検出したコーナをkeypoint化し、response値は埋めないように実装して試した。

keypointの差はどうやって埋めようかなぁ。

環境

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ブランチ)

環境

省略。

sw_emu

インターフェースのデバッグにはvitis -debugが有効だった。
setArgは変更毎に実行しないとカーネルに設定されないことはこれで見つけた。
以下に実行した方法を示しておく。

build

~~hashは50ac9c83379795185fd49166ca9d1c074b37dc23。~~
hashはb35f2dde90e18fd94b08c4ce903f16866f9797dd。

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=/media/akira/usr -DPLATFORM_COMPONENTS_PATH=/home/akira/work/ultra96v2 -DBUILD_TEST_TARGETS=OFF -DBUILD_TARGET=sw_emu -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=ON -DBUILD_FAST_TEST=ON -DDISABLE_RESIZE_KRNL=ON -DDISABLE_FAST_KRNL=OFF
ninja -j4

run

-forward-portオプションは１つのポートしか指定できない模様(v2020.2では)。なので、sshのポートにビルドPC側の2222番ポートでアクセスできるようにしておく。

cd hw/hw_link/package_sw_emu/
source <vitis-installation-path>/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 2222 22

エミュレータ起動したらtcfが1534番のポートで待ち受けているので、ビルドPC側の1440番ポートで接続できるようにsshでtargetに接続する。

ssh -L 1440:localhost:1534 localhost -p 2222 -l root

テスト対象を転送する。(リポジトリのルートからの実行) orb_slam2_ros_mono_tum_testは転送しなくてもvitisがやってくれるかも。

ssh -p 2222 root@localhost "mkdir -p /usr/bin/xclbin"
scp -P 2222 build/orb_slam2_ros_mono_tum_test root@localhost:/home/root/
scp -P 2222 orb_slam2/lib/liborb_slam2_ros_core.so root@localhost:/usr/lib/liborb_slam2_ros_core.accel.so
scp -P 2222 build/hw/hw_link/orbslam2_accel_sw_emu.xclbin root@localhost:/usr/bin/xclbin/resize_krnl.xclbin

エミュレータ実行フォルダから次の通り実行する。

vitis -debug -flow embedded_accel -host-exe-file ../../../orb_slam2_ros_mono_tum_test -target sw_emu -kernel-names fast_accel -program-args "/usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt /home/root/tum-dataset" -host 127.0.0.1 -port 1440

result

デバッグ画面表示されるのと、ブレークポイント設定することでカーネル呼び出し時の引数の状態などを確認できる。
ライブラリの中までデバッグしようとした場合にはどうすればよいかはよく分からない。
hostプログラムだけならgdbserverとdebugfsを用意すればできる。

hw

resize_krnlとfast_krnlを両方活かしてみてどうなるかを確認する。

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=/media/akira/usr -DPLATFORM_COMPONENTS_PATH=/home/akira/work/ultra96v2 -DBUILD_TEST_TARGETS=OFF -DBUILD_TARGET=hw -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=OFF -DBUILD_FAST_TEST=ON -DDISABLE_RESIZE_KRNL=OFF -DDISABLE_FAST_KRNL=OFF
ninja -j4

run

テスト対象を転送する。

ssh root@192.168.2.1 "mkdir -p /usr/bin/xclbin"
scp build/orb_slam2_ros_mono_tum_test root@192.168.2.1:/home/root/
scp orb_slam2/lib/liborb_slam2_ros_core.so root@192.168.2.1:/usr/lib/liborb_slam2_ros_core.accel.so
scp build/hw/hw_link/orbslam2_accel_hw.xclbin root@192.168.2.1:/usr/bin/xclbin/resize_krnl.xclbin

root@ultra96v2-zynqmp:~# export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
root@ultra96v2-zynqmp:~# export XILINX_XRT=/usr
root@ultra96v2-zynqmp:~# export XILINX_VITIS=/mnt/sd-mmcblk0p1
root@ultra96v2-zynqmp:~# mv /usr/lib/liborb_slam2_ros_core.so /usr/lib/liborb_slam2_ros_core.soft.so
root@ultra96v2-zynqmp:~# ln -s -f /usr/lib/liborb_slam2_ros_core.accel.so /usr/lib/liborb_slam2_ros_core.so
root@ultra96v2-zynqmp:~# time ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./tum-dataset

result

liborb_slam2_ros_core.soft.soを使用する場合はmedian tracking=0.12、トータル約2分なので、倍くらいの時間がかかるようになった。
置き換えただけじゃ当然そうなるよね。

root@ultra96v2-zynqmp:~# time ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./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'
Found Platform
Platform Name: Xilinx
INFO: Device found - edge
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'

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.259621
mean tracking time: 0.258882

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    3m53.420s
user    3m24.638s
sys 0m45.453s
root@ultra96v2-zynqmp:~#

2021年4月25日日曜日

ultra96v2上でcv::fastをaccelerationするテストを実行した話

cv::fastをaccelerationするテストを実行した話。
nonmaxSupressionは有効とし、Resource Optimizaitonも有効とする。
基本的にはVitis_Librariesのfastをそのまま持ってきて動かしただけ。
他のライブラリも同じようにして導入できそうなことが分かってきた。

準備

前回と同じ

src

vision/L2/examples/fast配下から次のファイルを配置する。

xf_fast_accel.cpp
xf_fast_config.h
xf_fast_tb.cpp
xf_config_params.h

extやdataフォルダの中身はresizeと共通して使用できる。
他のものも同じようにして導入できそう。

sw_emu

build(sw_emu)

hashは3eec9b91c8ceba52e219e4b8d0be5e64c1ec1f30

git clone https://github.com/akira-nishiyama/orb_slam_2_ros.git -b feature-fpga
cd hw
mkdir build
cd build
source <vitis-installation-path>/settings64.sh
source <sdk-installation-path>/environment-setup-aarch64-xilinx-linux
cmake .. -GNinja -DPLATFORM_COMPONENTS_PATH=<path-to-platform-components> -DBUILD_TARGET=sw_emu -DBUILD_FAST_TEST=ON

run(sw_emu)

エミュレータを起動する。次はビルドディレクトリからのコマンド例である。

cd hw/hw_link/package_sw_emu/
source <vitis-installation-path>/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534 -forward-port 2222 22

ターミナルで次の通り実行する。必要なファイルは/mnt/sd-mmcblk0p1にある。

root@ultra96v2-zynqmp:~# cd /mnt/sd-mmcblk0p1/
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XCL_EMULATION_MODE=sw_emu
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XILINX_XRT=/usr
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XILINX_VITIS=/mnt/sd-mmcblk0p1
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# mkdir xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# cp binary_container_1_sw_emu.xclbin xclbin/krnl_fast.xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# ./xf_fast_tb 128x128_1.png

result(sw_emu)

INFO: Running OpenCL section.
Found Platform
Platform Name: Xilinx
INFO: Device found - avnet_com_av_ULTRA96V2_1_0
XCLBIN File Name: krnl_fast
INFO: Importing xclbin/krnl_fast.xclbin
Loading: 'xclbin/krnl_fast.xclbin'
ocvpoints:511=
INFO: Verification results:
    Common = 511
    Success = 100
    Loss = 0
    Gain = 0
Test Passed
The maximum depth reached by any of the 4 hls::stream() instances in the design is 2048

hw

build(hw)

ビルドディレクトリからのコマンドを示す。

source <vitis-installation-path>/settings64.sh
source <sdk-installation-path>/environment-setup-aarch64-xilinx-linux
cmake .. -GNinja -DPLATFORM_COMPONENTS_PATH=<path-to-platform-components> -DBUILD_TARGET=hw -DBUILD_FAST_TEST=ON

run(hw)

sdカードはビルドディレクトリから以下の通り作成可能。 sd-deviceはdmesg等で調べたsdカードのデバイスファイルを指定する。

sudo dd if=hw/hw_link/package_hw/sd_card.img of=<sd-device>

ultra96のwifiに接続し、以下の通りsshアクセスを行う。

ssh -l root 192.168.2.1

ターミナルでは次のようにコマンドを実行する。

root@ultra96v2-zynqmp:~# cd /mnt/sd-mmcblk0p1/
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XILINX_XRT=/usr
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XILINX_VITIS=/mnt/sd-mmcblk0p1
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# mkdir xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# cp binary_container_1_hw.xclbin xclbin/krnl_fast.xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# ./xf_fast_tb 128x128_1.png

result(hw)

INFO: Running OpenCL section.
Found Platform
Platform Name: Xilinx
INFO: Device found - edge
XCLBIN File Name: krnl_fast
INFO: Importing xclbin/krnl_fast.xclbin
Loading: 'xclbin/krnl_fast.xclbin'
ocvpoints:511=
INFO: Verification results:
    Common = 511
    Success = 100
    Loss = 0
    Gain = 0
Test Passed

2021年4月23日金曜日

ultra96v2+orb_slam2のaccelerationでcv::resizeを置き換えてみた話

ultra96v2+orb_slam2のaccelerationでcv::resizeをハードウェアカーネルに置き換えてみた話。単純に置き換えるだけじゃやっぱりだめだよねっていう結果が得られた。

環境

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ブランチ)

準備

前回と同じ

変更内容

とりあえずの対象はACRiの記事で示されているExtractORBとする。
記事に記載の内容を実装してみることとするが、ultra96v2向けにはリソース使用量から考えて記事に記載の回路そのままは入らないと考えられるため並列処理数は減らしていく方向で検討する。
とりあえずはcv::resizeを回路に置き換えてみて様子を見る。
このためにORBExtractor.ccとORBExtractor.hを変更した。
主にOpen CLのランタイム環境設定と、cv::resizeに対してカーネル呼び出しコードの追加を行った。
(参考：https://japan.xilinx.com/html_docs/xilinx2020_2/vitis_doc/devhostapp.html#ariaid-title2)

sw_emu

build

sw_emuでは簡単に動作を見ていく。そのためにENABLE_KRNL_CHECKオプションを有効にする。 ENABLE_KRNL_CHECKオプションはKRNL_CHECKをdefineして必要な処理をORBExtractorに追加している。

hashは9fd1ad11b8982927366b687b527595651a82d8a9。将来変更となると思うので。

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-sdcard>/usr -DPLATFORM_COMPONENTS_PATH=<path-to-platform-components> -DBUILD_TEST_TARGETS=ON -DBUILD_TARGET=sw_emu -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=ON
ninja -j4

run

エミュレータを起動する。次はビルドディレクトリからのコマンド例である。

cd hw/hw_link/package_sw_emu/
source <vitis-installation-path>/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534 -forward-port 2222 22

エミュレータが起動したら必要なファイルを転送する。
リポジトリのトップからの転送コマンドを示す。

ssh -p 2222 root@localhost "mkdir -p /usr/bin/xclbin"
scp -P 2222 build/orb_slam2_ros_mono_tum_test root@localhost:/home/root/
scp -P 2222 orb_slam2/lib/liborb_slam2_ros_core.so root@localhost:/usr/lib/liborb_slam2_ros_core.accel.so
scp -P 2222 build/hw/hw_link/orbslam2_accel_sw_emu.xclbin root@localhost:/usr/bin/xclbin/resize_krnl.xclbin

エミュレータ上でプログラムを実行する。(参考:https://japan.xilinx.com/html_docs/xilinx2020_2/vitis_doc/runemulation1.html#ariaid-title6)
オリジナルのliborb_slam2_ros_core.soはsoft実行用に保存し、新たにacceleration向けのものをリンクしておきこれを実行する。

export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
export XCL_EMULATION_MODE=sw_emu
export XILINX_XRT=/usr
export XILINX_VITIS=/mnt/sd-mmcblk0p1
mv /usr/lib/liborb_slam2_ros_core.so /usr/lib/liborb_slam2_ros_core.soft.so
ln -s -f /usr/lib/liborb_slam2_ros_core.accel.so /usr/lib/liborb_slam2_ros_core.so
cp /mnt/sd-mmcblk0p1/emconfig.json .
./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./tum-dataset

result

一生かかっても終わる気配が無かったので途中で中断している。 opencvと同じ結果が得られているようなのでとりあえずよしとする。 (意図的にfailさせてないのでテストプログラムが正しいかは少し心配)

root@ultra96v2-zynqmp:~# ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./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 - avnet_com_av_ULTRA96V2_1_0
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'
Found Platform
Platform Name: Xilinx
INFO: Device found - avnet_com_av_ULTRA96V2_1_0
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'

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

here! level=1
in_height:480,in_width:640
out_height:400,out_width:533
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=2
in_height:400,in_width:533
out_height:333,out_width:444
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=3
in_height:333,in_width:444
out_height:278,out_width:370
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=4
in_height:278,in_width:370
out_height:231,out_width:309
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=5
in_height:231,in_width:309
out_height:193,out_width:257
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=6
in_height:193,in_width:257
out_height:161,out_width:214
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=7
in_height:161,in_width:214
out_height:134,out_width:179
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=1
in_height:480,in_width:640
out_height:400,out_width:533
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=2
in_height:400,in_width:533
out_height:333,out_width:444
    Minimum error in intensity = 0
    Maximum error in intensity = 0
    Percentage of pixels above error threshold = 0
Check Passed 
here! level=3
in_height:333,in_width:444
out_height:278,out_width:370
^C
root@ultra96v2-zynqmp:~#

hw

build

buildディレクトリで以下のように実行。環境変数設定済の場合はcmake以降を実行すれば良い。

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-sdcard>/usr -DPLATFORM_COMPONENTS_PATH=<path-to-platform-components> -DBUILD_TEST_TARGETS=ON -DBUILD_TARGET=hw -DCMAKE_BUILD_TYPE=Debug -DENABLE_KRNL_CHECK=OFF
ninja -j4

run

ビルドディレクトリから以下の通り実行してsdカードを作成する。

sudo dd if=hw/hw_link/package_hw/sd_card.img of=<path-to-sd-device>

sdをultra96v2に挿して起動させれば良い。 wifiは有効なはずなので、これに接続すれば、次のコマンドでターミナルにアクセスできる。

ssh 192.168.2.1 -l root

sdに直接書いてもいいが、何度かビルドと変更を繰り返すことになると思うのでホストで次のコマンドを実行して必要なファイルを転送する。実行はリポジトリのトップから行っている。

ssh root@192.168.2.1 "mkdir -p /usr/bin/xclbin"
scp build/orb_slam2_ros_mono_tum_test root@192.168.2.1:/home/root/
scp orb_slam2/lib/liborb_slam2_ros_core.so root@192.168.2.1:/usr/lib/liborb_slam2_ros_core.accel.so
scp build/hw/hw_link/orbslam2_accel_sw_emu.xclbin root@192.168.2.1:/usr/bin/xclbin/resize_krnl.xclbin

ultra96v2のターミナルで以下のコマンドを入力してオリジナルの状態を確認する。

root@ultra96v2-zynqmp:~# export LD_LIBRARY_PATH=/mnt/sd-mmcblk0p1:/tmp:$LD_LIBRARY_PATH
root@ultra96v2-zynqmp:~# export XILINX_XRT=/usr
root@ultra96v2-zynqmp:~# export XILINX_VITIS=/mnt/sd-mmcblk0p1
root@ultra96v2-zynqmp:~# mv /usr/lib/liborb_slam2_ros_core.so /usr/lib/liborb_slam2_ros_core.soft.so
root@ultra96v2-zynqmp:~# ln -s -f /usr/lib/liborb_slam2_ros_core.soft.so /usr/lib/liborb_slam2_ros_core.so
root@ultra96v2-zynqmp:~# time ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./tum-dataset

result

liborb_slam2_ros_core.soft.soを実行した場合の結果は次の通りだった。

ORBvoc.txt ./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)

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.129078
mean tracking time: 0.129074

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    2m9.262s
user    2m19.686s
sys 0m5.226s
root@ultra96v2-zynqmp:~# cat KeyFrameTrajectory.txt 
root@ultra96v2-zynqmp:~#

accelerationを使用した場合の結果は次の通りであった。

root@ultra96v2-zynqmp:~# ln -s -f /usr/lib/liborb_slam2_ros_core.accel.so /usr/lib/liborb_slam2_ros_core.so
root@ultra96v2-zynqmp:~# time ./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./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'
Found Platform
Platform Name: Xilinx
INFO: Device found - edge
Loading: '/usr/bin/xclbin/resize_krnl.xclbin'

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.148386
mean tracking time: 0.148938

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

real    2m25.290s
user    2m20.827s
sys 0m5.976s
root@ultra96v2-zynqmp:~# cat KeyFrameTrajectory.txt 
root@ultra96v2-zynqmp:~#

2021年4月14日水曜日

ultra96v2上でorb_slam2をtum datasetで動作させてみた話

ultra96v2上でorb_slam2のaccelerationを行う前に、評価のためのtum datasetを準備して使ってみた話。

環境

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ブランチ)

準備

vivadoプロジェクト(これまでから変更なし)

source <vivado_installation_path>/setup.sh
source ~/vivado_cmake_helper/setup.sh
https://github.com/akira-nishiyama/ultra96v2_4z.git -b feature-v2020.2
cd ultra96v2_4z
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX=~/<xsa-path> -GNinja
cmake --build .
ninja install

petalinux(tum datasetを/home/rootに追加。emulationの簡素化のため)

cd <petalinux-project-path>
repo init -u http://github.com/akira-nishiyama/petalinux-ros2-manifests -b feature-rel-v2020.2
repo sync

local.confを変更してxsaを指定する

HDF_EXT_ultra96v2-zynqmp = "xsa"
HDF_BASE_ultra96v2-zynqmp = "file://"
HDF_PATH_ultra96v2-zynqmp = "<xsa-file-path>/<yourdesign.xsa>"
QB_TAP_OPT_forcevariable = "-netdev tap,id=net0,ifname=tap0"

cd <petalinux-project-path>
source setupsdk
MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2
MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2 -c do_populate_sdk
tmp/deploy/sdk/petalinux-glibc-x86_64-petalinux-image-minimal-ultra96v2ros2-aarch64-ultra96v2-zynqmp-toolchain-2020.2.sh -d <sdk-installation-path>

<sdk-installation-path>/sysroots/x86_64-petalinux-linux/usr/share/cmake/OEToolchainConfig.cmakeを次の通り編集する

set( CMAKE_FIND_ROOT_PATH "\$ENV{OECORE_TARGET_SYSROOT};\$ENV{OECORE_NATIVE_SYSROOT}" )

<sdk-installation-path>sysroots/aarch64-xilinx-linux/usr/share/fastrtps/cmake/fastrtps-targets.cmakeを修正する。

set_target_properties(fastrtps PROPERTIES
  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
  INTERFACE_LINK_LIBRARIES "fastcdr;foonathan_memory;-lpthread;dl;$ENV{OECORE_TARGET_SYSROOT}/usr/lib/libtinyxml2.so;\$<\$<BOOL:1>:OpenSSL::SSL\$<SEMICOLON>OpenSSL::Crypto>;\$<\$<BOOL:>:iphlpapi\$<SEMICOLON>Shlwapi>"
)

sw_emu

build

git clone https://github.com/akira-nishiyama/orb_slam_2_ros.git -b feature-fpga
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-card-root>/usr -DPLATFORM_COMPONENTS_PATH=<path-to-platform-components> -DBUILD_TEST_TARGETS=ON -DBUILD_TARGET=sw_emu
ninja -j4

run

エミュレータを起動する

cd hw/hw_link/package_sw_emu/
source /opt/Xilinx/Vitis/2020.2/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534 -forward-port 2222 22

テストプログラムを/home/rootに転送する

scp -P 2222 ./orb_slam2_ros_mono_tum_test root@localhost:/home/root

ultra96上でテストプログラムを実行する

./orb_slam2_ros_mono_tum_test /usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt ./tum-dataset/

result

ログは以下の通り。遅いせいかKeyFrameが全然認識できていない。実機の評価は別の機会にしてopencl化して改善できるかはこれから試す。

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)

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: 1.5039
mean tracking time: 1.74812

Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!

2021年4月7日水曜日

Vitis 2020.2でVitis LIbraryのresize_accelを試してみた話

resize_accelのテストをしたときのメモ。

環境

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ブランチ)

sw_emu

ビルド

以下のコマンドで必要なデータ生成される。 cmakeの-DBUILD_TARGETをhw_emuにするとハードウェアエミュレーション、hwにすると実機用のデータが生成される。はacceleration platformを作成するのに必要なファイルを保存しているパス。必要に応じて置き換えること。 cmakeを実行するとどのように期待しているかのメッセージが表示されるので参考にすること。

git clone https://github.com/akira-nishiyama/orb_slam_2_ros.git -b feature-resize-accel
source <path-to-sdk>/environment_setup
source /opt/Xilinx/Vitis/2020.2/settings64.sh
cd orb_slam_2_ros/orb_slam2
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TARGET=sw_emu -DPLATFORM_COMPONENTS_PATH=<path-to-components-required-files> -DBUILD_TEST_TARGETS=ON -GNinja
make -j4

実行

cd hw_link/package_sw_emu/
source /opt/Xilinx/Vitis/2020.2/settings64.sh
./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534 -forward-port 2222 22

ターミナル起動後は次のコマンドを実行する。デバッグが必要な場合はvitis -debugでxf_resize_tbを起動する。

root@ultra96v2-zynqmp:~# cd /mnt/sd-mmcblk0p1/
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XCL_EMULATION_MODE=sw_emu
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# mkdir xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# cp binary_container_2_sw_emu.xclbin xclbin/krnl_resize.sw_emu.avnet_com_av_ULTRA96V2_1_0.xclbin

profile結果が得られるようにxrt.iniを作成する。

root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# vi xrt.ini
[Debug] 
profile=true
timeline_trace=true
trace_buffer_size=1M

結果

root@ultra96v2-zynqmp:/media/sd-mmcblk0p1# ./xf_resize_tb 128x128_1.png 64 64
INFO: Running OpenCL section.
Found Platform
Platform Name: Xilinx
INFO: Device found - avnet_com_av_ULTRA96V2_1_0
XCLBIN File Name: krnl_resize
INFO: Importing xclbin/krnl_resize.sw_emu.avnet_com_av_ULTRA96V2_1_0.xclbin
Loading: 'xclbin/krnl_resize.sw_emu.avnet_com_av_ULTRA96V2_1_0.xclbin'
36905.7ms
    Minimum error in intensity = 0
    Maximum error in intensity = 1
    Percentage of pixels above error threshold = 0
Test Passed 
The maximum depth reached by any of the 4 hls::stream() instances in the design is 16384
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1#

プロファイルの結果は次のようにして取得可能。 vitis -debugで実行した場合は自動的に取得される。

mkdir report_sw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/xclbin.run_summary ./report_sw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/profile_summary.csv ./report_sw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/timeline_trace.csv ./report_sw_emu

vitis_analyzer report_sw_emu

hw_emu

ビルド

sw_emu実行後にリポジトリのtopから開始する。

cd orb_slam_2_ros/orb_slam2/build
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TARGET=hw_emu -DPLATFORM_COMPONENTS_PATH=<path-to-components-required-files> -DBUILD_TEST_TARGETS=ON
make -j4

実行

cd hw_link/package_hw_emu/
./launch_hw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534 -forward-port 2222 22

ターミナル起動後は次のコマンドを実行する。デバッグが必要な場合はvitis -debugでxf_resize_tbを起動する。

root@ultra96v2-zynqmp:~# cd /mnt/sd-mmcblk0p1/
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# export XCL_EMULATION_MODE=hw_emu
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# mkdir xclbin
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# cp binary_container_2_hw_emu.xclbin xclbin/krnl_resize.hw_emu.edge.xclbin

profile結果が得られるようにxrt.iniを作成する。

root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# vi xrt.ini
[Debug] 
profile=true
timeline_trace=true
trace_buffer_size=1M

結果

root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1# ./xf_resize_tb 128x128_1.png 64 64
INFO: Running OpenCL section.
Found Platform
Platform Name: Xilinx
[  257.173751] [drm] Pid 1124 opened device
INFO: Device found - edge
XCLBIN File Name: krnl_resize
INFO: Importing xclbin/krnl_resize.hw_emu.edge.xclbin
Loading: 'xclbin/krnl_resize.hw_emu.edge.xclbin'
[  258.218452] [drm] get section DEBUG_IP_LAYOUT err: -22 
[  258.224260] [drm] get section AIE_METADATA err: -22 
[  258.230851] [drm] zocl_xclbin_read_axlf ad3f6288-9ebf-40e2-b7d2-da7e0ef9d155 ret: 0
[  258.329375] [drm] bitstream ad3f6288-9ebf-40e2-b7d2-da7e0ef9d155 locked, ref=1
[  258.339355] [drm] No ERT scheduler on MPSoC, using KDS
[  258.454364] [drm] scheduler config ert(0)
[  258.456924] [drm]   cus(1)
[  258.468834] [drm]   slots(16)
[  258.469260] [drm]   num_cu_masks(1)
[  258.469485] [drm]   cu_shift(16)
[  258.473936] [drm]   cu_base(0x80010000)
[  258.474516] [drm]   polling(0)
[  258.480007] [drm] bitstream ad3f6288-9ebf-40e2-b7d2-da7e0ef9d155 unlocked, ref=0
21509.8ms
    Minimum error in intensity = 0
    Maximum error in intensity = 1
    Percentage of pixels above error threshold = 0
Test Passed 
[  258.647570] [drm] bitstream ad3f6288-9ebf-40e2-b7d2-da7e0ef9d155 locked, ref=1
[  280.899806] [drm] bitstream ad3f6288-9ebf-40e2-b7d2-da7e0ef9d155 unlocked, ref=0
[  281.437319] [drm] Pid 1124 closed device
root@ultra96v2-zynqmp:/mnt/sd-mmcblk0p1#

プロファイルの結果は次のようにして取得可能。 vitis -debugの場合は自動的に取得される。

mkdir report_hw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/binary_container_1_hw_emu.xclbin.run_summary ./report_hw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/profile_summary.csv ./report_hw_emu
scp -P 2222 root@localhost:/mnt/sd-mmcblk0p1/timeline_trace.csv ./report_hw_emu

vitis_analyzer report_hw_emu

hw

ビルド

sw_emu実行後にリポジトリのtopから開始する。

cd orb_slam_2_ros/orb_slam2/build
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TARGET=hw -DPLATFORM_COMPONENTS_PATH=<path-to-components-required-files> -DBUILD_TEST_TARGETS=ON -GNinja
make -j4

実行

引っ越しのために実機は梱包しているので未実施。

2021年1月30日土曜日

Vitis 2020.2でvector additionサンプルをコマンドラインからライブラリとしてビルドできるようにしてデバッグ方法を確認した

vector additionサンプルをコマンドラインからライブラリとしてビルドできるようにしてデバッグ方法を確認した話。

環境

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ブランチ)

ビルド方法

githubにvadd_lib_sampleリポジトリを用意するので、 xsaとboot componentを用意してsdカードのパスを確認して以下の通り実行すればok。なお、サンプルはVitis-Tutorialsを参考にさせてもらっている。

git clone https://github.com/akira-nishiyama/vitis_library_compilation_sample.git
cd vitis_library_compilation_sample

platform設定は用意したxsaとboot componentに合わせて修正が必要。 guiでプラットフォーム作るときと同じようにパスを指定すれば良い。

platform/platform.tcl

platform create -name {ultra96v2_platform}\
-hw {/home/akira/work/ultra96v2/xsa_dir/ultra96v2_4z/ultra96v2_4z.xsa}\
-proc {psu_cortexa53} -os {linux} -arch {64-bit} -no-boot-bsp -fsbl-target {psu_cortexa53_0} -out {.}

platform write
platform active {ultra96v2_platform}
domain config -bif {/home/akira/work/ultra96v2/vitis_args/linux.bif}
platform write
domain config -boot {/home/akira/work/ultra96v2/boot}
platform write
domain config -image {/home/akira/work/ultra96v2/boot}
platform write
domain config -rootfs {/home/akira/work/ultra96v2/petalinux/build/tmp/deploy/images/ultra96v2-zynqmp/petalinux-image-minimal-ultra96v2ros2-ultra96v2-zynqmp.ext4}
platform write
domain config -sysroot {/home/akira/work/ultra96v2/sdk/sysroots/aarch64-xilinx-linux}
platform write
domain config -qemu-data {/home/akira/work/ultra96v2/vitis_args}
platform write
domain config -qemu-data {/home/akira/work/ultra96v2/boot}
platform write
domain config -qemu-args {/home/akira/work/ultra96v2/vitis_args/qemu_args.txt}
platform write
domain config -pmuqemu-args {/opt/Xilinx/Vitis/2020.2/data/emulation/platforms/zynqmp/sw/a53_linux/qemu/pmu_args.txt}
platform write
domain config -pmuqemu-args {/home/akira/work/ultra96v2/vitis_args/pmu_args.txt}
platform write
domain config -qemu-data {/home/akira/work/ultra96v2/petalinux/build/tmp/deploy/images/ultra96v2-zynqmp}
platform write
platform generate

<sd-device>,<sd-device-root>は環境に合わせて指定すること。

source <vitis-installation-path>/settings.sh
source <sdk-installation-path>/environment-setup-aarch64-xilinx-linux
export PATH_TO_SD=<path-to-sd>
mkdir build
cd build
cmake .. -DBUILD_TARGET=hw
sudo dd if=hw_link/package_hw/sd_card.img of=<sd-device>
mkdir rootfs
sudo mount <sd-device-root> rootfs
sudo cp host/test_vecadd rootfs/home/root
sudo cp hw_link/binary_container_2_hw.xclbin rootfs/home/root
sudo cp host/libvecadd.so rootfs/usr/lib

sdをultra96v2に挿して起動後、次のコマンドを実行するとTEST PASSEDの文字が確認できる。

# ./vecadd_test binary_container_2_hw.xclbin 
Loading: 'binary_container_2_hw.xclbin'
TEST PASSED

また、BUILD_TARGETをsw_emu,hw_emuとするとbuild配下のhw_link/package_sw_emuやhw_link/package_hw_emuにエミュレーション用のファイル出来上がるので、それぞれのフォルダでterminalを２つ用意し、次の通り実行するとデバッグ可能。(https://japan.xilinx.com/html_docs/xilinx2020_1/vitis_doc/debugappproj.html#rme1587072841352)

なお、実行に際してはxrt.iniを自分で用意する必要がある。(https://www.xilinx.com/html_docs/xilinx2020_2/vitis_doc/obl1532064985142.html)

[Debug] 
profile=true
timeline_trace=true
trace_buffer_size=1M

ライブラリのパスをどうやって解決しているのかはよくわからない。

sw_emu

build/package_sw_emuに移動して次のコマンドを実行する。

terminal1

./launch_sw_emu.sh -pid-file emulation.pid -no-reboot -forward-port 1440 1534

ログインプロンプトが表示されたら次のコマンドを実行する。デバッグしないなら/mnt/sd-mmcblk0p1においてあるテストプログラムを実行すれば結果が得られる。ただし、export XCL_EMULATION_MODE=sw_emuの実行(xrt.iniに書いても可かも)と、libvecadd.soを/usr/libにコピーする必要がある。

terminal2

vitis -debug -flow embedded_accel -target sw_emu -exe ../../host/vecadd_test -program-args binary_container_2_sw_emu.xclbin -kernels vadd

実行後、vitisが立ち上がるので、vaddにブレークポイントを設定する。

メニューからFile->Open fileしてline break pointを指定しても良い。

実行するとbreak pointにひっかかる。

無事に終了。

profileデータなどはbuild/package_sw_emu/reportに保存されるのでvitis_analyzerで開けば確認できる。

vitis_analyzer reports

hw_emu

build/package_hw_emuに移動して次のコマンドを実行する。 -sim-guiは波形でバッグしない場合は不要。

terminal1

./launch_hw_emu.sh -pid-file emulation.pid -sim-gui -no-reboot -forward-port 1440 1534

ログインプロンプトが表示されたら次のコマンドを実行する。デバッグしないなら/mnt/sd-mmcblk0p1においてあるテストプログラムを実行すれば結果が得られる。

terminal2

vitis -debug -flow embedded_accel -target hw_emu -exe ../../host/vecadd_test -program-args binary_container_2_hw_emu.xclbin -kernels vadd

メニューからFile->Open fileしてline break pointを指定できる。 HWカーネルはCソースレベルではブレークできないようだが、 vivado_simと同じ使い方でブレークさせることは可能そう。

構成

まず、vitisのembedded accelerationはplatform部と、host部と、hw_kernel部と、hw_link部に分けることができる。

platformは必要なファイルを集めて固定のshell部分を作っている。

host部はクロスコンパイラを使用してxilinxopencl,pthread,rt,stdc++,gmp,xrt_coreとリンクすれば良い。インクルードディレクトリはsdkの環境セットアップすれば解決される。通常のC++などと同じに扱えるのでライブラリはそれで作成可能。 (https://japan.xilinx.com/html_docs/xilinx2020_1/vitis_doc/debugappproj.html#rme1587072841352)

hw_kernel部はv++ -cを動作させて.xoを作成している。

hw_link部ではv++ -lで.xoを必要な個数接続している。また、パッケージ化もついでに行っている。

2021年1月19日火曜日

ros2 eloquent + orb_slam2_rosをクロスコンパイルして動かした話

ultra96v2とros2 eloquentでorb_slam2を動かした際のメモ。能力不足なのか、マッピングが動かなかった。

vitisで性能改善させるのにsdkでのクロスコンパイル環境必要かと思ったので整えた。

レシピはここのfeature-v2020.2ブランチから一式取得できる。もしくはここのfeature-v2020.2ブランチにレシピ類がまとめてある。

レシピの変更

クロスコンパイル用のパッケージにはnativesdkをBBCLASSEXTENDに追加する必要がある。そのため、/sources/meta-ros/meta-ros-common/classes/ros_superflore_generated.bbclassを変更する。

# BBCLASSEXTEND_append = "${@bb.utils.contains('ROS_SUPERFLORE_GENERATED_BUILDTOOLS', '${BPN}-native', ' native', '', d)}"
BBCLASSEXTEND_append = "${@bb.utils.contains('ROS_SUPERFLORE_GENERATED_BUILDTOOLS', '${BPN}-native', ' native nativesdk', '', d)}"

次のファイルはFILES_${PN} += "*"を追加する。そのままだとament-cmake向けファイル等がインストールされない模様。
recipes-ros2/ament-index-python/ament-index-python_%.bbappend
recipes-ros2/ament-package/ament-package_%.bbappend
recipes-ros2/domain-coordinator/domain-coordinator_%.bbappend

python3-lark-parserはBsuperfloreのBUILDTOOL登録されていないため、nativesdk化されない。なのでbbappendでBBCLASSEXTEND_appendにnativesdkを追加する。 recipes-ros2/python3-lark-parser/python3-lark-parser_%.bbappend

rviz-ogre-vendorがmesaに依存しているけど、 zynqではmesa-glを使用するので、mesaを削除してmesa-glを追加する。
recipes-ros2/rviz/rviz-ogre-vendor_%.bbappend

ROS_BUILD_DEPENDS_remove = "mesa"
ROS_BUILD_DEPENDS_append = " mesa-gl"
ROS_EXPORT_DEPENDS_remove = "mesa"
ROS_EXPORT_DEPENDS_append = " mesa-gl"
ROS_EXEC_DEPENDS_remove = "mesa"
ROS_EXEC_DEPENDS_append = " mesa-gl"

あとはpackage.xmlを参考にopencv-camとros2-sharedのレシピを作成する。
recipes-ros2/opencv-cam/opencv-cam_git.bb
recipes-ros2/ros2-shared/ros2-shared_git.bb

あとはSDKのためにTOOLCHAIN_TARGET_TASK_appendとTOOLCHAIN_HOST_TASK_appendを recipes-core/images/petalinux-image-minimal-ultra96v2ros2.bbに追加していく。
yoctoは自動的に依存関係収集してくれないようなので、クロスコンパイルしたい対象の依存関係を手動で解決していく。
*-nativeに依存している場合はnativesdk-*をTOOLCHAIN_HOST_TASK_appendに追加する。
無印の場合にはTOOLCHAIN_TARGET_TASK_appendに該当パッケージを追加する。
クロスコンパイル実行時にライブラリが不足などのメッセージが出た場合には-staticdevや-devなどをTOOLCHAIN_TARGET_TASK_appendに追加する。
(で合ってるはず。自信は無い。)

ビルド

MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2
MACHINE=ultra96v2-zynqmp bitbake petalinux-image-minimal-ultra96v2ros2 -c do_populate_sdk

orb-slam2-rosの実行

まずはカメラ画像の確認。 USBのHOSTにwebカメラを接続する。使用するのはlogicoolのc270n。ついでにCPU使用率を確認する。

ssh 192.168.2.1 -l root
root@ultra96v2-zynqmp:~# source ros_setup.sh
root@ultra96v2-zynqmp:~# ros2 run opencv_cam opencv_cam_main --ros-args --param index:=0 --param camera_info_path:=camera-info-logicool-c270n.ini &
root@ultra96v2-zynqmp:~# top

topで動作を確認。

top - 23:11:54 up 11:33,  2 users,  load average: 0.86, 1.13, 1.63
Tasks: 103 total,   3 running, 100 sleeping,   0 stopped,   0 zombie
%Cpu(s):  0.2 us,  2.3 sy,  0.0 ni, 97.5 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
MiB Mem :   1974.4 total,   1682.3 free,    124.8 used,    167.3 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1780.4 avail Mem

大したことはなさそう。

orb-slam2-rosを動かす。

root@ultra96v2-zynqmp:~# ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=/usr/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=/usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt &

同じくtopで確認。1オプションでCPU毎の負荷も確認しておく。

top - 23:50:13 up 12:11,  2 users,  load average: 3.71, 3.16, 2.74
Tasks: 108 total,   2 running, 106 sleeping,   0 stopped,   0 zombie
%Cpu0  : 85.9 us,  2.0 sy,  0.0 ni, 11.8 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
%Cpu1  : 21.5 us,  5.0 sy,  0.0 ni, 73.2 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
%Cpu2  : 48.1 us,  6.5 sy,  0.0 ni, 45.4 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
%Cpu3  :  3.2 us, 26.4 sy,  0.0 ni, 70.4 id,  0.0 wa,  0.0 hi,  0.0 si,  0.0 st
MiB Mem :   1974.4 total,   1089.0 free,    665.8 used,    219.6 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1239.2 avail Mem 

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU  %MEM     TIME+ COMMAND                                                                                 
 1628 root      20   0 1790424 505660  21916 R 121.2  25.0  26:20.67 orb_slam2_ros_m                                                                         
 1277 root      20   0  516600  47812  23240 S  47.4   2.4  12:44.68 opencv_cam_main                                                                         
 4034 root       0 -20       0      0      0 I  31.8   0.0   3:38.79 kworker/u9:2-uvcvideo                                                                   
 1258 root      20   0  415052  37020  15196 S   1.0   1.8   0:24.32 python3                                                                                 
 6384 root      20   0    3320   2024   1564 R   0.7   0.1   0:02.39 top

CPUごとに見ると１つのCPUの負荷が高く、orb_slam2_rosプロセスの負荷が高い。 load averageも高めで能力足りてないと見ればいいかな。

クロスコンパイル

sdkをインストールしたあと

/sysroots/x86_64-petalinux-linux/usr/share/cmake/OEToolchainConfig.cmake を次のように編集する。 $ENV{OECORE_NATIVE_SYSROOT}が無いとnativeツールが発見できないため

set( CMAKE_FIND_ROOT_PATH "\$ENV{OECORE_TARGET_SYSROOT};\$ENV{OECORE_NATIVE_SYSROOT}" )

sysroots/aarch64-xilinx-linux/usr/share/fastrtps/cmake/fastrtps-targets.cmakeの検索パスが間違っているので修正。レシピから修正したほうがいいだろうけど、とりあえずなんとかなるから後回し。

#set_target_properties(fastrtps PROPERTIES
#  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
#  INTERFACE_LINK_LIBRARIES "fastcdr;foonathan_memory;-lpthread;dl;/home/akira/work/ultra96v2/petalinux/build/tmp/work/aarch64-xilinx-linux/fastrtps/1.9.3-2-r0/recipe-sysroot/usr/lib/libtinyxml2.so;\$<\$<BOOL:1>:OpenSSL::SSL\$<SEMICOLON>OpenSSL::Crypto>;\$<\$<BOOL:>:iphlpapi\$<SEMICOLON>Shlwapi>"
#)
set_target_properties(fastrtps PROPERTIES
  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
  INTERFACE_LINK_LIBRARIES "fastcdr;foonathan_memory;-lpthread;dl;$ENV{OECORE_TARGET_SYSROOT}/usr/lib/libtinyxml2.so;\$<\$<BOOL:1>:OpenSSL::SSL\$<SEMICOLON>OpenSSL::Crypto>;\$<\$<BOOL:>:iphlpapi\$<SEMICOLON>Shlwapi>"
)

buildの実行。インストールパスはsd-cardのrootをマウントした場所を指定する。

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
cd <path-to-orb_slam2_ros-repository>
mkdir build
cd build
cmake .. -DCMAKE_INSTALL_SO_NO_EXE=0 -DBUILD_TESTING=OFF -DCMAKE_NO_SYSTEM_FROM_IMPORTED=1 -GNinja -DCMAKE_INSTALL_PREFIX=<sd-card-root>
cmake --build
sudo ninja install

同じようにtopを実行してみると似た結果が得られている。

top - 00:12:13 up 12 min,  2 users,  load average: 3.12, 1.92, 0.98
Tasks: 109 total,   4 running, 105 sleeping,   0 stopped,   0 zombie
%Cpu(s): 45.3 us, 14.8 sy,  0.0 ni, 39.5 id,  0.0 wa,  0.0 hi,  0.3 si,  0.0 st
MiB Mem :   1974.4 total,   1059.6 free,    636.4 used,    278.5 buff/cache
MiB Swap:      0.0 total,      0.0 free,      0.0 used.   1249.7 avail Mem 

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU  %MEM     TIME+ COMMAND                                                                                 
 1212 root      20   0 1785152 499480  21788 R 125.2  24.7   0:15.17 orb_slam2_ros_m                                                                         
  812 root      20   0  516588  47672  22840 S  67.5   2.4   1:58.91 opencv_cam_main                                                                         
  133 root       0 -20       0      0      0 R  46.4   0.0   0:50.66 kworker/u9:0+uvcvideo                                                                   
 1211 root      20   0    3320   2064   1616 R   0.7   0.1   0:00.10 top

また、rviz2でdebug_imageを確認するとorb_slam2_rosから出力されているのでクロスコンパイル版もしっかり動作している模様。

2021年1月12日火曜日

ubuntu 20.04+ros2 foxyでorab_slam2_rosを動作させてみた

ubuntu 20.04とros2 foxyでカメラを動作させてorb_slam2_rosを試してみた際のメモ。カメラで試したのはros2_v4l2_cameraとopencv_cam。ほぼリンク先の通り実行しただけ。

opencvの準備

ubuntu 20.04標準のopencvではバージョン起因と思われるエラーが出たので、 v4.5.0をソースからビルドした。特別問題なくビルドできた。

cd <path-to-opencv-download-dir>
unzip opencv-4.5.0.zip
cd opencv-4.5.0
mkdir build
cd build
cmake .. -GNinja -DCMAKE_INSTALL_PREFIX=<opencv-installation-path>/local
ninja -j4
ninja install

私の環境では既にopencv3.4.12が入っているので、v4.5.0が先に見つかるように環境変数を設定する。 colcon実行前に以下のコマンドが必要。

export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH

ros2_v4l2_cameraの準備

こちらは記載の通りに実行して特に問題なかった。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り

ros2_v4l2_camera:65b072565f8e4520d166b725d91d5f31acd97610
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_transport_plugins:0565f23dc955fa2d56cffcfebc63de5a97cd005f
vision_opencv:2026f6d66833e0830f9ce69004d43c9abd1e5fa4

opencv_camの準備

ここを参考にしてビルドした。 image_pipelineのビルドはros2ブランチのhash:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837を使用する。 foxyっぽいブランチを使用すると2021/1/12時点ではcalibrationを実行する際にros-argsを指定すると実行できない問題がある模様。 (https://github.com/ros-perception/image_pipeline/pull/597) image_commonはros2_v4ls_cameraと同じ。 ~/ros_work/srcにリポジトリをクローンする。

使用したhashは以下の通り。

opencv_cam:f4487b0de651a4323a535f8e8d0e86b147cc81c2
ros2_shared:02433ef4f873876c3dd3ab2925987cf04d224660
image_common:4c5053ecf703a58c25a712d1d4a80012f5076a1f
image_pipeline:b4fbbae6463fb3baaa4b36a0a7517d0b0395e837

ビルド

パスの設定をしてビルドする。

cd ~/ros_work
export PATH=<opencv-installation-path>/local/bin:$PATH
export LD_LIBRARY_PATH=<opencv-installation-path>/local/lib:$LD_LIBRARY_PATH
export PYTHONPATH=<opencv-installation-path>/local/lib/python3.8/dist-packages:$PYTHONPATH
colcon build --symlink-install

videoの出力

cam2imageを試す場合は以下。 /dev/video0のみが出力できる。

source ~/ros_work/install/setup.sh
ros2 run image_tools cam2image

v4l2_cameraを試す場合は以下。 video_deviceのパラメータで指定可能。 videoデバイスが複数合ってどれを使えばいいかわからない場合は、 videoデバイスの情報の調べ方を使って調べられる。 USBなら挿して増えたものでいいとは思う。 eloquent以前のビルドはできそうになかったので、 ultra96v2への採用は見送った。

source ~/ros_work/install/setup.sh
ros2 run v4l2_camera v4l2_camera_node --ros-args -p video_device:=/dev/video14

opencv_camを試す場合は以下。 indexでvideoデバイスファイルの番号を指定できる。

source ~/ros_work/install/setup.sh
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14

videoデバイスの情報の調べ方

v4l2-ctlを使用する。ない場合にはv4l-utilsをインストールすると使用できる。

sudo apt install v4l-utils

logicoolのc270nに試してみた結果は以下の通り。

# v4l2-ctl -d /dev/video14 --all
driver Info:
    Driver name      : uvcvideo
    Card type        : UVC Camera (046d:0825)
    Bus info         : usb-0000:00:14.0-1.1
    Driver version   : 5.8.18
    Capabilities     : 0x84a00001
        Video Capture
        Metadata Capture
        Streaming
        Extended Pix Format
        Device Capabilities
    Device Caps      : 0x04200001
        Video Capture
        Streaming
        Extended Pix Format
Media Driver Info:
    Driver name      : uvcvideo
    Model            : UVC Camera (046d:0825)
    Serial           : 98251310
    Bus info         : usb-0000:00:14.0-1.1
    Media version    : 5.8.18
    Hardware revision: 0x00000012 (18)
    Driver version   : 5.8.18
Interface Info:
    ID               : 0x03000002
    Type             : V4L Video
Entity Info:
    ID               : 0x00000001 (1)
    Name             : UVC Camera (046d:0825)
    Function         : V4L2 I/O
    Flags         : default
    Pad 0x01000007   : 0: Sink
      Link 0x02000019: from remote pad 0x100000a of entity 'Extension 4': Data, Enabled, Immutable
Priority: 2
Video input : 0 (Camera 1: ok)
Format Video Capture:
    Width/Height      : 640/480
    Pixel Format      : 'YUYV' (YUYV 4:2:2)
    Field             : None
    Bytes per Line    : 1280
    Size Image        : 614400
    Colorspace        : sRGB
    Transfer Function : Default (maps to sRGB)
    YCbCr/HSV Encoding: Default (maps to ITU-R 601)
    Quantization      : Default (maps to Limited Range)
    Flags             : 
Crop Capability Video Capture:
    Bounds      : Left 0, Top 0, Width 640, Height 480
    Default     : Left 0, Top 0, Width 640, Height 480
    Pixel Aspect: 1/1
Selection Video Capture: crop_default, Left 0, Top 0, Width 640, Height 480, Flags: 
Selection Video Capture: crop_bounds, Left 0, Top 0, Width 640, Height 480, Flags: 
Streaming Parameters Video Capture:
    Capabilities     : timeperframe
    Frames per second: 30.000 (30/1)
    Read buffers     : 0
                     brightness 0x00980900 (int)    : min=0 max=255 step=1 default=128 value=128
                       contrast 0x00980901 (int)    : min=0 max=255 step=1 default=32 value=32
                     saturation 0x00980902 (int)    : min=0 max=255 step=1 default=32 value=32
 white_balance_temperature_auto 0x0098090c (bool)   : default=1 value=1
                           gain 0x00980913 (int)    : min=0 max=255 step=1 default=64 value=64
           power_line_frequency 0x00980918 (menu)   : min=0 max=2 default=2 value=2
                0: Disabled
                1: 50 Hz
                2: 60 Hz
      white_balance_temperature 0x0098091a (int)    : min=0 max=10000 step=10 default=4000 value=4000 flags=inactive
                      sharpness 0x0098091b (int)    : min=0 max=255 step=1 default=24 value=24
         backlight_compensation 0x0098091c (int)    : min=0 max=1 step=1 default=0 value=0
                  exposure_auto 0x009a0901 (menu)   : min=0 max=3 default=3 value=3
                1: Manual Mode
                3: Aperture Priority Mode
              exposure_absolute 0x009a0902 (int)    : min=1 max=10000 step=1 default=166 value=166 flags=inactive
         exposure_auto_priority 0x009a0903 (bool)   : default=0 value=1

videoの確認

上記でvideoの出力したものはrqt_image_viewで確認できる。 rviz2だとカメラ画像はうまく表示されなかった。(camera_infoの出力が必要とのこと)

ros2 run rqt_image_view rqt_image_view

calibration

カメラのキャリブレーションしないとopencv_camを実行した際にcamera_infoのtopicを出力できない旨のエラーメッセージが出力される。

[ERROR] [1610449586.191007718] [camera_calibration_parsers]: Failed to detect content in .ini file
[ERROR] [1610449586.191095947] [opencv_cam]: cannot get camera info, will not publish

キャリブレーション方法はこちらに記載の内容に従って行った。途中でフリーズしてCalibrateボタンが押せないことが何度かあったが、頑張ってプログラムを完了させた。

#ros2 run camera_calibration cameracalibrator \
  --size=8x6 \
  --square=0.063 \
  --approximate=0.3 \
  --no-service-check \
  --ros-args --remap /image:=/image_raw
*** Added sample 1, p_x = 0.479, p_y = 0.527, p_size = 0.602, skew = 0.051
*** Added sample 2, p_x = 0.790, p_y = 0.645, p_size = 0.634, skew = 0.083
*** Added sample 3, p_x = 0.758, p_y = 0.556, p_size = 0.634, skew = 0.164
*** Added sample 4, p_x = 0.599, p_y = 0.417, p_size = 0.627, skew = 0.155
*** Added sample 5, p_x = 0.529, p_y = 0.281, p_size = 0.622, skew = 0.103
*** Added sample 6, p_x = 0.435, p_y = 0.000, p_size = 0.627, skew = 0.128
*** Added sample 7, p_x = 0.463, p_y = 0.776, p_size = 0.621, skew = 0.057
*** Added sample 8, p_x = 0.630, p_y = 0.905, p_size = 0.627, skew = 0.056
*** Added sample 9, p_x = 0.465, p_y = 0.997, p_size = 0.629, skew = 0.101
*** Added sample 10, p_x = 0.787, p_y = 0.888, p_size = 0.667, skew = 0.104
*** Added sample 11, p_x = 0.336, p_y = 0.914, p_size = 0.647, skew = 0.025
*** Added sample 12, p_x = 0.355, p_y = 0.735, p_size = 0.666, skew = 0.005
*** Added sample 13, p_x = 0.460, p_y = 0.585, p_size = 0.688, skew = 0.012
*** Added sample 14, p_x = 0.255, p_y = 0.577, p_size = 0.683, skew = 0.032
*** Added sample 15, p_x = 0.371, p_y = 0.504, p_size = 0.706, skew = 0.054
*** Added sample 16, p_x = 0.640, p_y = 0.781, p_size = 0.737, skew = 0.073
*** Added sample 17, p_x = 0.402, p_y = 0.322, p_size = 0.719, skew = 0.036
*** Added sample 18, p_x = 0.534, p_y = 0.101, p_size = 0.747, skew = 0.069
*** Added sample 19, p_x = 0.689, p_y = 0.225, p_size = 0.764, skew = 0.129
*** Added sample 20, p_x = 0.632, p_y = 0.539, p_size = 0.733, skew = 0.140
*** Added sample 21, p_x = 0.527, p_y = 0.628, p_size = 0.711, skew = 0.133
*** Added sample 22, p_x = 0.241, p_y = 0.822, p_size = 0.620, skew = 0.040
*** Added sample 23, p_x = 0.524, p_y = 0.801, p_size = 0.745, skew = 0.015
*** Added sample 24, p_x = 0.603, p_y = 0.734, p_size = 0.669, skew = 0.157
*** Added sample 25, p_x = 0.404, p_y = 0.131, p_size = 0.690, skew = 0.103
*** Added sample 26, p_x = 0.301, p_y = 0.048, p_size = 0.690, skew = 0.082
*** Added sample 27, p_x = 0.743, p_y = 0.106, p_size = 0.692, skew = 0.131
*** Added sample 28, p_x = 0.788, p_y = 0.305, p_size = 0.689, skew = 0.128
*** Added sample 29, p_x = 0.423, p_y = 0.621, p_size = 0.631, skew = 0.084
*** Added sample 30, p_x = 0.638, p_y = 0.191, p_size = 0.651, skew = 0.117
*** Added sample 31, p_x = 0.410, p_y = 0.404, p_size = 0.629, skew = 0.059
*** Added sample 32, p_x = 0.700, p_y = 0.411, p_size = 0.678, skew = 0.244
*** Added sample 33, p_x = 0.577, p_y = 0.321, p_size = 0.674, skew = 0.193
*** Added sample 34, p_x = 0.416, p_y = 0.468, p_size = 0.664, skew = 0.171
*** Added sample 35, p_x = 0.348, p_y = 0.709, p_size = 0.778, skew = 0.065
*** Added sample 36, p_x = 0.546, p_y = 0.666, p_size = 0.784, skew = 0.035
*** Added sample 37, p_x = 0.545, p_y = 0.369, p_size = 0.769, skew = 0.006
*** Added sample 38, p_x = 0.640, p_y = 0.493, p_size = 0.598, skew = 0.063
*** Added sample 39, p_x = 0.814, p_y = 0.795, p_size = 0.610, skew = 0.071
*** Added sample 40, p_x = 0.730, p_y = 0.839, p_size = 0.600, skew = 0.166
*** Added sample 41, p_x = 0.587, p_y = 0.679, p_size = 0.570, skew = 0.052
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 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 =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 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 =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000


('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')
D =  [0.45527714279447146, 0.29663576559913546, -0.023494638427063057, -0.03623143280219075, 0.0]
K =  [1594.715058900246, 0.0, 214.64225194625027, 0.0, 1535.171197718767, 174.6268394034711, 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 =  [1628.6416015625, 0.0, 210.49766367948905, 0.0, 0.0, 1574.3720703125, 171.79825469629577, 0.0, 0.0, 0.0, 1.0, 0.0]
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
1594.715059 0.000000 214.642252
0.000000 1535.171198 174.626839
0.000000 0.000000 1.000000

distortion
0.455277 0.296636 -0.023495 -0.036231 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
1628.641602 0.000000 210.497664 0.000000
0.000000 1574.372070 171.798255 0.000000
0.000000 0.000000 1.000000 0.000000

/tmp/calibrationdata.tar.gzに保存されたものを使用する。解凍すると、ost.txtがあり、これがiniファイルとして使用できる。(コンソールの結果も同じ) なので、camera-info-logicool-c270n.iniとして保存した。使うときには以下の通り。ただし、rviz2では表示されなかった。あと何がいるんだ？

ros2 run opencv_cam opencv_cam_main --ros-args --param index:=14 --param camera_info_path:=camera-info-logicool-c270n.ini

orb_slam2実行

以下のコマンドでorb_slam2_rosを実行する。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=install/orb_slam2_ros/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=install/orb_slam2_ros/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

ultra96v2で実行する場合は以下。機能の実装方法は次回。

ros2 run orb_slam2_ros orb_slam2_ros_mono --ros-args --remap /camera/image_raw:=/image_raw --remap /camera/camera_info:=/camera_info --param params_file:=/usr/share/orb_slam2_ros/ros/config/params_d435_mono.yaml --param voc_file:=/usr/share/orb_slam2_ros/orb_slam2/Vocabulary/ORBvoc.txt

以下のような結果が得られた。なんだか微妙な感じだけどキャリブレーションうまくいってないのか？

参考

http://joe.ash.jp/program/ros/tutorial/tutorial_camera.htm
https://medium.com/swlh/raspberry-pi-ros-2-camera-eef8f8b94304
https://jeffzzq.medium.com/ros2-image-pipeline-tutorial-3b18903e7329

登録: 投稿 (Atom)