forked from ros-perception/image_pipeline
/
xf_rectify_accel_streamlined.cpp
134 lines (108 loc) · 5.11 KB
/
xf_rectify_accel_streamlined.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
/*
____ ____
/ /\/ /
/___/ \ / Copyright (c) 2021, Xilinx®.
\ \ \/ Author: Víctor Mayoral Vilches <victorma@xilinx.com>
\ \
/ / Licensed under the Apache License, Version 2.0 (the "License");
/___/ /\ you may not use this file except in compliance with the License.
\ \ / \ You may obtain a copy of the License at
\___\/\___\ http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
// #include "xf_remap_config.h"
#include "hls_stream.h"
#include "ap_axi_sdata.h"
#include "ap_int.h"
#include <vitis_common/common/xf_common.hpp>
#include <vitis_common/common/xf_utility.hpp>
#include <vitis_common/imgproc/xf_remap.hpp>
#define XF_INTERPOLATION_TYPE XF_INTERPOLATION_BILINEAR // interpolation type
#define XF_USE_URAM false // Enable to map some structures to UltraRAM instead of BRAM
// #define PTR_IMG_WIDTH 8 // for GRAY
// #define TYPE XF_8UC1
// #define CHANNELS 1
#define PTR_IMG_WIDTH 32 // if RGB
#define TYPE XF_8UC3
#define CHANNELS 3
#define TYPE_XY XF_32FC1
#define PTR_MAP_WIDTH 32
#define NPC XF_NPPC1 // Number of pixels to be processed per cycle
#define XF_WIN_ROWS 50 // Number of input image rows to be buffered inside
#define HEIGHT 480 // 480 with default RealSense drivers
#define WIDTH 640 // 640 with default RealSense drivers
#define XF_CAMERA_MATRIX_SIZE 9
#define XF_DIST_COEFF_SIZE 5
#define DWIDTH 24
const int dwidth_tripcount = WIDTH/NPC;
const int height_tripcount = HEIGHT;
extern "C" {
void rectify_accel_streamlined(
ap_uint<PTR_IMG_WIDTH>* img_in,
float* map_x,
float* map_y,
hls::stream<ap_axiu<DWIDTH, 0, 0, 0>>& img_out,
// hls::stream<xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC>>& img_out,
// ap_uint<PTR_IMG_WIDTH>* img_out,
// xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC>& img_out,
int rows,
int cols) {
#pragma HLS INTERFACE m_axi port=img_in offset=slave bundle=gmem0
#pragma HLS INTERFACE m_axi port=map_x offset=slave bundle=gmem1
#pragma HLS INTERFACE m_axi port=map_y offset=slave bundle=gmem2
// #pragma HLS INTERFACE m_axi port=img_out offset=slave bundle=gmem3
#pragma HLS INTERFACE s_axilite port=rows
#pragma HLS INTERFACE s_axilite port=cols
#pragma HLS INTERFACE s_axilite port=return
// Get arguments for remap operation (remap)
xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC> imgInput(rows, cols);
xf::cv::Mat<TYPE_XY, HEIGHT, WIDTH, NPC> mapX(rows, cols);
xf::cv::Mat<TYPE_XY, HEIGHT, WIDTH, NPC> mapY(rows, cols);
xf::cv::Mat<TYPE, HEIGHT, WIDTH, NPC> imgOutput(rows, cols);
const int HEIGHT_WIDTH_LOOPCOUNT = HEIGHT * WIDTH / XF_NPIXPERCYCLE(NPC);
for (unsigned int i = 0; i < rows * cols; ++i) {
#pragma HLS LOOP_TRIPCOUNT min=1 max=HEIGHT_WIDTH_LOOPCOUNT
#pragma HLS PIPELINE II=1
// clang-format on
float map_x_val = map_x[i];
float map_y_val = map_y[i];
mapX.write_float(i, map_x_val);
mapY.write_float(i, map_y_val);
}
#pragma HLS STREAM variable=imgInput.data depth=2
#pragma HLS STREAM variable=mapX.data depth=2
#pragma HLS STREAM variable=mapY.data depth=2
#pragma HLS STREAM variable=imgOutput.data depth=2
#pragma HLS DATAFLOW
// Retrieve xf::cv::Mat objects from img_in data:
xf::cv::Array2xfMat<PTR_IMG_WIDTH, TYPE, HEIGHT, WIDTH, NPC>(img_in, imgInput);
// // obtain a mapping between the distorted image and an undistorted one
// xf::cv::InitUndistortRectifyMapInverse<XF_CAMERA_MATRIX_SIZE, XF_DIST_COEFF_SIZE, XF_32FC1, HEIGHT, WIDTH,
// XF_NPPC1>(K_binned_fix, distC_l_fix, irA_l_fix, mapxLMat, mapyLMat,
// _cm_size, _dc_size);
// remap accordingly
xf::cv::remap<XF_WIN_ROWS, XF_INTERPOLATION_TYPE, TYPE, TYPE_XY, TYPE, HEIGHT, WIDTH, NPC, XF_USE_URAM>(
imgInput, imgOutput, mapX, mapY);
// img_out.write(imgOutput); // doesn't work out
int readindex = 0, writeindex = 0;
Row_Loop:
for (auto row = 0; row < HEIGHT; row++) {
#pragma HLS LOOP_TRIPCOUNT min=height_tripcount max=height_tripcount
#pragma HLS LOOP_FLATTEN off
Col_Loop:
for (auto col = 0; col < WIDTH; col++) {
#pragma HLS LOOP_TRIPCOUNT min=dwidth_tripcount max=dwidth_tripcount
#pragma HLS pipeline
ap_uint<DWIDTH> aux = imgOutput.read(readindex++);
ap_axiu<DWIDTH, 0, 0, 0> stream_out;
stream_out.data = aux;
img_out.write(stream_out);
}
}
return;
} // End of kernel
} // End of extern C