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
投稿者 時刻:

0 件のコメント:

コメントを投稿

登録: コメントの投稿 (Atom)