1 // Copyright (c) 2021 by Rockchip Electronics Co., Ltd. All Rights Reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14
15 #include "postprocess.h"
16
17 #include <math.h>
18 #include <stdint.h>
19 #include <stdio.h>
20 #include <stdlib.h>
21 #include <string.h>
22 #include <sys/time.h>
23
24 #include <set>
25 #include <vector>
26 #define LABEL_NALE_TXT_PATH "./model/coco_80_labels_list.txt"
27
28 static char* labels[OBJ_CLASS_NUM];
29
30 const int anchor0[6] = {10, 13, 16, 30, 33, 23};
31 const int anchor1[6] = {30, 61, 62, 45, 59, 119};
32 const int anchor2[6] = {116, 90, 156, 198, 373, 326};
33
clamp(float val,int min,int max)34 inline static int clamp(float val, int min, int max) { return val > min ? (val < max ? val : max) : min; }
35
readLine(FILE * fp,char * buffer,int * len)36 char* readLine(FILE* fp, char* buffer, int* len)
37 {
38 int ch;
39 int i = 0;
40 size_t buff_len = 0;
41
42 buffer = (char*)malloc(buff_len + 1);
43 if (!buffer)
44 return NULL; // Out of memory
45
46 while ((ch = fgetc(fp)) != '\n' && ch != EOF) {
47 buff_len++;
48 void* tmp = realloc(buffer, buff_len + 1);
49 if (tmp == NULL) {
50 free(buffer);
51 return NULL; // Out of memory
52 }
53 buffer = (char*)tmp;
54
55 buffer[i] = (char)ch;
56 i++;
57 }
58 buffer[i] = '\0';
59
60 *len = buff_len;
61
62 // Detect end
63 if (ch == EOF && (i == 0 || ferror(fp))) {
64 free(buffer);
65 return NULL;
66 }
67 return buffer;
68 }
69
readLines(const char * fileName,char * lines[],int max_line)70 int readLines(const char* fileName, char* lines[], int max_line)
71 {
72 FILE* file = fopen(fileName, "r");
73 char* s;
74 int i = 0;
75 int n = 0;
76
77 if (file == NULL) {
78 printf("Open %s fail!\n", fileName);
79 return -1;
80 }
81
82 while ((s = readLine(file, s, &n)) != NULL) {
83 lines[i++] = s;
84 if (i >= max_line)
85 break;
86 }
87 fclose(file);
88 return i;
89 }
90
loadLabelName(const char * locationFilename,char * label[])91 int loadLabelName(const char* locationFilename, char* label[])
92 {
93 printf("loadLabelName %s\n", locationFilename);
94 readLines(locationFilename, label, OBJ_CLASS_NUM);
95 return 0;
96 }
97
CalculateOverlap(float xmin0,float ymin0,float xmax0,float ymax0,float xmin1,float ymin1,float xmax1,float ymax1)98 static float CalculateOverlap(float xmin0, float ymin0, float xmax0, float ymax0, float xmin1, float ymin1, float xmax1,
99 float ymax1)
100 {
101 float w = fmax(0.f, fmin(xmax0, xmax1) - fmax(xmin0, xmin1) + 1.0);
102 float h = fmax(0.f, fmin(ymax0, ymax1) - fmax(ymin0, ymin1) + 1.0);
103 float i = w * h;
104 float u = (xmax0 - xmin0 + 1.0) * (ymax0 - ymin0 + 1.0) + (xmax1 - xmin1 + 1.0) * (ymax1 - ymin1 + 1.0) - i;
105 return u <= 0.f ? 0.f : (i / u);
106 }
107
nms(int validCount,std::vector<float> & outputLocations,std::vector<int> classIds,std::vector<int> & order,int filterId,float threshold)108 static int nms(int validCount, std::vector<float>& outputLocations, std::vector<int> classIds, std::vector<int>& order,
109 int filterId, float threshold)
110 {
111 for (int i = 0; i < validCount; ++i) {
112 if (order[i] == -1 || classIds[i] != filterId) {
113 continue;
114 }
115 int n = order[i];
116 for (int j = i + 1; j < validCount; ++j) {
117 int m = order[j];
118 if (m == -1 || classIds[i] != filterId) {
119 continue;
120 }
121 float xmin0 = outputLocations[n * 4 + 0];
122 float ymin0 = outputLocations[n * 4 + 1];
123 float xmax0 = outputLocations[n * 4 + 0] + outputLocations[n * 4 + 2];
124 float ymax0 = outputLocations[n * 4 + 1] + outputLocations[n * 4 + 3];
125
126 float xmin1 = outputLocations[m * 4 + 0];
127 float ymin1 = outputLocations[m * 4 + 1];
128 float xmax1 = outputLocations[m * 4 + 0] + outputLocations[m * 4 + 2];
129 float ymax1 = outputLocations[m * 4 + 1] + outputLocations[m * 4 + 3];
130
131 float iou = CalculateOverlap(xmin0, ymin0, xmax0, ymax0, xmin1, ymin1, xmax1, ymax1);
132
133 if (iou > threshold) {
134 order[j] = -1;
135 }
136 }
137 }
138 return 0;
139 }
140
quick_sort_indice_inverse(std::vector<float> & input,int left,int right,std::vector<int> & indices)141 static int quick_sort_indice_inverse(std::vector<float>& input, int left, int right, std::vector<int>& indices)
142 {
143 float key;
144 int key_index;
145 int low = left;
146 int high = right;
147 if (left < right) {
148 key_index = indices[left];
149 key = input[left];
150 while (low < high) {
151 while (low < high && input[high] <= key) {
152 high--;
153 }
154 input[low] = input[high];
155 indices[low] = indices[high];
156 while (low < high && input[low] >= key) {
157 low++;
158 }
159 input[high] = input[low];
160 indices[high] = indices[low];
161 }
162 input[low] = key;
163 indices[low] = key_index;
164 quick_sort_indice_inverse(input, left, low - 1, indices);
165 quick_sort_indice_inverse(input, low + 1, right, indices);
166 }
167 return low;
168 }
169
sigmoid(float x)170 static float sigmoid(float x) { return 1.0 / (1.0 + expf(-x)); }
171
unsigmoid(float y)172 static float unsigmoid(float y) { return -1.0 * logf((1.0 / y) - 1.0); }
173
__clip(float val,float min,float max)174 inline static int32_t __clip(float val, float min, float max)
175 {
176 float f = val <= min ? min : (val >= max ? max : val);
177 return f;
178 }
179
qnt_f32_to_affine(float f32,int32_t zp,float scale)180 static int8_t qnt_f32_to_affine(float f32, int32_t zp, float scale)
181 {
182 float dst_val = (f32 / scale) + zp;
183 int8_t res = (int8_t)__clip(dst_val, -128, 127);
184 return res;
185 }
186
deqnt_affine_to_f32(int8_t qnt,int32_t zp,float scale)187 static float deqnt_affine_to_f32(int8_t qnt, int32_t zp, float scale) { return ((float)qnt - (float)zp) * scale; }
188
process(int8_t * input,int * anchor,int grid_h,int grid_w,int height,int width,int stride,std::vector<float> & boxes,std::vector<float> & objProbs,std::vector<int> & classId,float threshold,int32_t zp,float scale)189 static int process(int8_t* input, int* anchor, int grid_h, int grid_w, int height, int width, int stride,
190 std::vector<float>& boxes, std::vector<float>& objProbs, std::vector<int>& classId, float threshold,
191 int32_t zp, float scale)
192 {
193 int validCount = 0;
194 int grid_len = grid_h * grid_w;
195 float thres = unsigmoid(threshold);
196 int8_t thres_i8 = qnt_f32_to_affine(thres, zp, scale);
197 for (int a = 0; a < 3; a++) {
198 for (int i = 0; i < grid_h; i++) {
199 for (int j = 0; j < grid_w; j++) {
200 int8_t box_confidence = input[(PROP_BOX_SIZE * a + 4) * grid_len + i * grid_w + j];
201 if (box_confidence >= thres_i8) {
202 int offset = (PROP_BOX_SIZE * a) * grid_len + i * grid_w + j;
203 int8_t* in_ptr = input + offset;
204 float box_x = sigmoid(deqnt_affine_to_f32(*in_ptr, zp, scale)) * 2.0 - 0.5;
205 float box_y = sigmoid(deqnt_affine_to_f32(in_ptr[grid_len], zp, scale)) * 2.0 - 0.5;
206 float box_w = sigmoid(deqnt_affine_to_f32(in_ptr[2 * grid_len], zp, scale)) * 2.0;
207 float box_h = sigmoid(deqnt_affine_to_f32(in_ptr[3 * grid_len], zp, scale)) * 2.0;
208 box_x = (box_x + j) * (float)stride;
209 box_y = (box_y + i) * (float)stride;
210 box_w = box_w * box_w * (float)anchor[a * 2];
211 box_h = box_h * box_h * (float)anchor[a * 2 + 1];
212 box_x -= (box_w / 2.0);
213 box_y -= (box_h / 2.0);
214
215 int8_t maxClassProbs = in_ptr[5 * grid_len];
216 int maxClassId = 0;
217 for (int k = 1; k < OBJ_CLASS_NUM; ++k) {
218 int8_t prob = in_ptr[(5 + k) * grid_len];
219 if (prob > maxClassProbs) {
220 maxClassId = k;
221 maxClassProbs = prob;
222 }
223 }
224 if (maxClassProbs>thres_i8){
225 objProbs.push_back(sigmoid(deqnt_affine_to_f32(maxClassProbs, zp, scale))* sigmoid(deqnt_affine_to_f32(box_confidence, zp, scale)));
226 classId.push_back(maxClassId);
227 validCount++;
228 boxes.push_back(box_x);
229 boxes.push_back(box_y);
230 boxes.push_back(box_w);
231 boxes.push_back(box_h);
232 }
233 }
234 }
235 }
236 }
237 return validCount;
238 }
239
post_process(int8_t * input0,int8_t * input1,int8_t * input2,int model_in_h,int model_in_w,float conf_threshold,float nms_threshold,float scale_w,float scale_h,std::vector<int32_t> & qnt_zps,std::vector<float> & qnt_scales,detect_result_group_t * group)240 int post_process(int8_t* input0, int8_t* input1, int8_t* input2, int model_in_h, int model_in_w, float conf_threshold,
241 float nms_threshold, float scale_w, float scale_h, std::vector<int32_t>& qnt_zps,
242 std::vector<float>& qnt_scales, detect_result_group_t* group)
243 {
244 static int init = -1;
245 if (init == -1) {
246 int ret = 0;
247 ret = loadLabelName(LABEL_NALE_TXT_PATH, labels);
248 if (ret < 0) {
249 return -1;
250 }
251
252 init = 0;
253 }
254 memset(group, 0, sizeof(detect_result_group_t));
255
256 std::vector<float> filterBoxes;
257 std::vector<float> objProbs;
258 std::vector<int> classId;
259
260 // stride 8
261 int stride0 = 8;
262 int grid_h0 = model_in_h / stride0;
263 int grid_w0 = model_in_w / stride0;
264 int validCount0 = 0;
265 validCount0 = process(input0, (int*)anchor0, grid_h0, grid_w0, model_in_h, model_in_w, stride0, filterBoxes, objProbs,
266 classId, conf_threshold, qnt_zps[0], qnt_scales[0]);
267
268 // stride 16
269 int stride1 = 16;
270 int grid_h1 = model_in_h / stride1;
271 int grid_w1 = model_in_w / stride1;
272 int validCount1 = 0;
273 validCount1 = process(input1, (int*)anchor1, grid_h1, grid_w1, model_in_h, model_in_w, stride1, filterBoxes, objProbs,
274 classId, conf_threshold, qnt_zps[1], qnt_scales[1]);
275
276 // stride 32
277 int stride2 = 32;
278 int grid_h2 = model_in_h / stride2;
279 int grid_w2 = model_in_w / stride2;
280 int validCount2 = 0;
281 validCount2 = process(input2, (int*)anchor2, grid_h2, grid_w2, model_in_h, model_in_w, stride2, filterBoxes, objProbs,
282 classId, conf_threshold, qnt_zps[2], qnt_scales[2]);
283
284 int validCount = validCount0 + validCount1 + validCount2;
285 // no object detect
286 if (validCount <= 0) {
287 return 0;
288 }
289
290 std::vector<int> indexArray;
291 for (int i = 0; i < validCount; ++i) {
292 indexArray.push_back(i);
293 }
294
295 quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);
296
297 std::set<int> class_set(std::begin(classId), std::end(classId));
298
299 for (auto c : class_set) {
300 nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
301 }
302
303 int last_count = 0;
304 group->count = 0;
305 /* box valid detect target */
306 for (int i = 0; i < validCount; ++i) {
307 if (indexArray[i] == -1 || last_count >= OBJ_NUMB_MAX_SIZE) {
308 continue;
309 }
310 int n = indexArray[i];
311
312 float x1 = filterBoxes[n * 4 + 0];
313 float y1 = filterBoxes[n * 4 + 1];
314 float x2 = x1 + filterBoxes[n * 4 + 2];
315 float y2 = y1 + filterBoxes[n * 4 + 3];
316 int id = classId[n];
317 float obj_conf = objProbs[i];
318
319 group->results[last_count].box.left = (int)(clamp(x1, 0, model_in_w) / scale_w);
320 group->results[last_count].box.top = (int)(clamp(y1, 0, model_in_h) / scale_h);
321 group->results[last_count].box.right = (int)(clamp(x2, 0, model_in_w) / scale_w);
322 group->results[last_count].box.bottom = (int)(clamp(y2, 0, model_in_h) / scale_h);
323 group->results[last_count].prop = obj_conf;
324 char* label = labels[id];
325 strncpy(group->results[last_count].name, label, OBJ_NAME_MAX_SIZE);
326
327 // printf("result %2d: (%4d, %4d, %4d, %4d), %s\n", i, group->results[last_count].box.left,
328 // group->results[last_count].box.top,
329 // group->results[last_count].box.right, group->results[last_count].box.bottom, label);
330 last_count++;
331 }
332 group->count = last_count;
333
334 return 0;
335 }
336
deinitPostProcess()337 void deinitPostProcess()
338 {
339 for (int i = 0; i < OBJ_CLASS_NUM; i++) {
340 if (labels[i] != nullptr) {
341 free(labels[i]);
342 labels[i] = nullptr;
343 }
344 }
345 }
346