241 lines
8.3 KiB
C
241 lines
8.3 KiB
C
#include "zf_common_headfile.h"
|
||
#include "math.h"
|
||
#include "gl_headfile.h"
|
||
|
||
|
||
|
||
|
||
const int32_t direction_front[4][2] = {{0, -1},{1, 0},{0, 1},{-1, 0}};
|
||
const int32_t direction_frontleft[4][2] = {{-1, -1},{1, -1},{1, 1},{-1, 1}};
|
||
const int32_t direction_frontright[4][2] ={{1, -1},{1, 1},{-1, 1},{-1, -1}};
|
||
|
||
|
||
void SearchLineAdaptive_Left(uint8_t img_gray[], int32_t block_size, int32_t down_value, int32_t h, int32_t w, int32_t pts[][2], int32_t* line_num)
|
||
{
|
||
const int32_t half = block_size / 2;
|
||
int step = 0;
|
||
int dir = 0;
|
||
int turn = 0;
|
||
while (step < *line_num && half < w && w < IMAGE_W - half -1 && half < h && h < IMAGE_H - half -1 && turn < 4) {
|
||
int local_thres = 0;
|
||
for (int dh = -1 * half; dh <= half; dh++) {
|
||
for (int dw = -1 * half; dw <= half; dw++) {
|
||
local_thres += GET_PIX_1C(img_gray, h + dh, w + dw);
|
||
}
|
||
}
|
||
local_thres /= block_size * block_size;
|
||
local_thres -= down_value;
|
||
|
||
int front_value = GET_PIX_1C(img_gray, h+direction_front[dir][1], w + direction_front[dir][0]);
|
||
int frontleft_value = GET_PIX_1C(img_gray, h + direction_frontleft[dir][1], w + direction_frontleft[dir][0]);
|
||
if (front_value < local_thres) {
|
||
dir = (dir + 1) % 4;
|
||
turn++;
|
||
} else if (frontleft_value < local_thres) {
|
||
w += direction_front[dir][0];
|
||
h += direction_front[dir][1];
|
||
pts[step][1] = w;
|
||
pts[step][0] = h;
|
||
step++;
|
||
turn = 0;
|
||
} else {
|
||
|
||
w += direction_frontleft[dir][0];
|
||
h += direction_frontleft[dir][1];
|
||
dir = (dir + 3) % 4;
|
||
pts[step][1] = w;
|
||
pts[step][0] = h;
|
||
step++;
|
||
turn = 0;
|
||
|
||
}
|
||
|
||
|
||
}
|
||
*line_num = step;
|
||
}
|
||
|
||
void SearchLineAdaptive_Right(uint8_t img_gray[], int32_t block_size, int32_t down_value, int32_t h, int32_t w, int32_t pts[][2], int32_t* line_num)
|
||
{
|
||
int half = block_size / 2;
|
||
int step = 0, dir = 0, turn = 0;
|
||
while (step < *line_num && 0 < w && w < IMAGE_W - 1 && 0 < h && h < IMAGE_H -1 && turn < 4) {
|
||
int local_thres = 0;
|
||
for (int dh = -half; dh <= half; dh++) {
|
||
for (int dw = -half; dw <= half; dw++) {
|
||
local_thres += GET_PIX_1C(img_gray, h + dh, w + dw);
|
||
}
|
||
}
|
||
local_thres /= block_size * block_size;
|
||
local_thres -= down_value;
|
||
|
||
|
||
int front_value = GET_PIX_1C(img_gray, h + direction_front[dir][1], w + direction_front[dir][0]);
|
||
int frontright_value = GET_PIX_1C(img_gray, h + direction_frontright[dir][1], w + direction_frontright[dir][0]);
|
||
if (front_value < local_thres) {
|
||
dir = (dir + 3) % 4;
|
||
turn++;
|
||
} else if (frontright_value < local_thres) {
|
||
w += direction_front[dir][0];
|
||
h += direction_front[dir][1];
|
||
pts[step][1] = w;
|
||
pts[step][0] = h;
|
||
step++;
|
||
turn = 0;
|
||
|
||
|
||
} else {
|
||
w += direction_frontright[dir][0];
|
||
h += direction_frontright[dir][1];
|
||
dir = (dir + 1) % 4;
|
||
pts[step][1] = w;
|
||
pts[step][0] = h;
|
||
step++;
|
||
turn = 0;
|
||
|
||
|
||
}
|
||
|
||
}
|
||
*line_num = step;
|
||
}
|
||
|
||
|
||
|
||
void GetLinesFilter(float pts_in[][2], int32_t pts_in_count, float pts_out[][2], int32_t kernelsize)
|
||
{
|
||
int half = kernelsize / 2;
|
||
for (int i = 0; i < pts_in_count; i++) {
|
||
pts_out[i][0] = 0;
|
||
pts_out[i][1] = 0;
|
||
for (int j = -half; j <= half; j++) {
|
||
pts_out[i][0] += pts_in[clip(i + j, 0, pts_in_count - 1)][0] * (half + 1 - abs(j));
|
||
pts_out[i][1] += pts_in[clip(i + j, 0, pts_in_count - 1)][1] * (half + 1 - abs(j));
|
||
}
|
||
pts_out[i][0] /= (2 * half + 2) * (half + 1) / 2;
|
||
pts_out[i][1] /= (2 * half + 2) * (half + 1) / 2;
|
||
}
|
||
}
|
||
|
||
void GetLinesResample(float pts_in[][2], int32_t num1, float pts_out[][2], int32_t* num2, float dist)
|
||
{
|
||
if (num1 < 0) {
|
||
*num2 = 0;
|
||
return;
|
||
}
|
||
pts_out[0][0] = pts_in[0][0];
|
||
pts_out[0][1] = pts_in[0][1];
|
||
int len = 1;
|
||
for (int i = 0; i < num1 - 1 && len < *num2; i++) {
|
||
float x0 = pts_in[i][1];
|
||
float y0 = pts_in[i][0];
|
||
float x1 = pts_in[i + 1][1];
|
||
float y1 = pts_in[i + 1][0];
|
||
|
||
do {
|
||
float x = pts_out[len - 1][1];
|
||
float y = pts_out[len - 1][0];
|
||
|
||
float dx0 = x0 - x;
|
||
float dy0 = y0 - y;
|
||
float dx1 = x1 - x;
|
||
float dy1 = y1 - y;
|
||
|
||
float dist0 = Q_sqrt(dx0 * dx0 + dy0 * dy0);
|
||
float dist1 = Q_sqrt(dx1 * dx1 + dy1 * dy1);
|
||
|
||
float r0 = (dist1 - dist) / (dist1 - dist0);
|
||
float r1 = 1 - r0;
|
||
|
||
if (r0 < 0 || r1 < 0) break;
|
||
x0 = x0 * r0 + x1 * r1;
|
||
y0 = y0 * r0 + y1 * r1;
|
||
pts_out[len][1] = x0;
|
||
pts_out[len][0] = y0;
|
||
len++;
|
||
} while (len < *num2);
|
||
|
||
}
|
||
*num2 = len;
|
||
}
|
||
|
||
//*************************** 函数中x,y的位置似乎反了 *************************//
|
||
void local_angle_points(float pts_in[][2], int num, float angle_out[], int dist) {
|
||
for (int i = 0; i < num; i++) {
|
||
if (i <= 0 || i >= num - 1) {
|
||
angle_out[i] = 0;
|
||
continue;
|
||
}
|
||
float dx1 = pts_in[i][0] - pts_in[clip(i - dist, 0, num - 1)][0];
|
||
float dy1 = pts_in[i][1] - pts_in[clip(i - dist, 0, num - 1)][1];
|
||
float dn1 = sqrtf(dx1 * dx1 + dy1 * dy1);
|
||
float dx2 = pts_in[clip(i + dist, 0, num - 1)][0] - pts_in[i][0];
|
||
float dy2 = pts_in[clip(i + dist, 0, num - 1)][1] - pts_in[i][1];
|
||
float dn2 = sqrtf(dx2 * dx2 + dy2 * dy2);
|
||
float c1 = dx1 / dn1;
|
||
float s1 = dy1 / dn1;
|
||
float c2 = dx2 / dn2;
|
||
float s2 = dy2 / dn2;
|
||
angle_out[i] = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1); //得到弧度 类似angle_out = 0.3491
|
||
}
|
||
}
|
||
|
||
void nms_angle(float angle_in[], int num, float angle_out[], int kernel) {
|
||
|
||
int half = kernel / 2;
|
||
for (int i = 0; i < num; i++) {
|
||
angle_out[i] = angle_in[i];
|
||
for (int j = -half; j <= half; j++) {
|
||
if (fabs(angle_in[clip(i + j, 0, num - 1)]) > fabs(angle_out[i])) {
|
||
angle_out[i] = 0;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//*********************************** 交叉相加减，dx接近与0导致y基本不变；dy接近于1导致x的值变得较多 *********************************//
|
||
void GetMidLine_Left(float pts_left[][2], int32_t pts_left_count, float mid_left[][2], int32_t approx_num, float dist)
|
||
{
|
||
for (int i = 0; i < pts_left_count; i++) {
|
||
float dx = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][1] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][1];
|
||
float dy = pts_left[clip(i + approx_num, 0, pts_left_count - 1)][0] - pts_left[clip(i - approx_num, 0, pts_left_count - 1)][0];
|
||
float dn = Q_sqrt(dx * dx + dy * dy);
|
||
dx /= dn;
|
||
dy /= dn;
|
||
mid_left[i][0] = pts_left[i][0] + dx * dist;
|
||
mid_left[i][1] = pts_left[i][1] - dy * dist;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
void GetMidLine_Right(float pts_right[][2], int32_t pts_right_count, float mid_right[][2], int32_t approx_num, float dist)
|
||
{
|
||
for (int i = 0; i < pts_right_count; i++) {
|
||
float dx = pts_right[clip(i + approx_num, 0, pts_right_count -1)][1] - pts_right[clip(i - approx_num, 0, pts_right_count -1 )][1];
|
||
float dy = pts_right[clip(i + approx_num, 0, pts_right_count -1)][0] - pts_right[clip(i - approx_num, 0, pts_right_count -1)][0];
|
||
float dn = Q_sqrt(dx * dx + dy * dy);
|
||
dx /= dn;//sin
|
||
dy /= dn;//cos
|
||
mid_right[i][0] = pts_right[i][0] - dx * dist;
|
||
mid_right[i][1] = pts_right[i][1] + dy * dist;
|
||
|
||
|
||
}
|
||
}
|
||
|
||
|
||
int is_curve(float angle[], int n, float threshold) {
|
||
for (int i = 1; i < n - 1; i++) {
|
||
float da = fabs(angle[i] - angle[i-3]);
|
||
float db = fabs(angle[i+3] - angle[i]);
|
||
if (da > threshold && db > threshold) {
|
||
return 1; // 是弯道
|
||
}
|
||
}
|
||
return 0; // 不是弯道
|
||
}
|
||
|
||
|