btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

摄像头循迹——中线测试版本

Browse Source
This commit is contained in:
Glz
2024-01-08 20:13:20 +08:00
parent 83994bbbc2
commit 83f4b5e8d2
7 changed files with 279 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
app/gl_tracking.c
View File

@@ -66,16 +66,16 @@ void MidLineTrack() {
}
// 车轮对应点 (纯跟踪起始点)
float cx = UndistInverseMapW[(int) (IMAGE_H * 0.90f)][78];
float cy = UndistInverseMapH[(int) (IMAGE_H * 0.90f)][78];
float cx = InverseMapW[(int) (IMAGE_H * 0.85f)][94];
float cy = InverseMapH[(int) (IMAGE_H * 0.85f)][94];
// 找最近点 (起始点中线归一化)
float min_dist = 1e10;
int begin_id = -1;
for (int i = 0; i < rpts_num; i++) {
float dx = rpts[i][1] - cx;
float dy = rpts[i][0] - cy;
for (int i = 0; i < mid_track_count; i++) {
float dx = mid_track[i][1] - cx;
float dy = mid_track[i][0] - cy;
float dist = sqrt(dx * dx + dy * dy);
if (dist < min_dist) {
min_dist = dist;
@@ -83,12 +83,12 @@ void MidLineTrack() {
}
}
if (begin_id >= 0 && rpts_num - begin_id >= 3){
if (begin_id >= 0 && mid_track_count - begin_id >= 3){
// 归一化中线
rpts[begin_id][0] = cy;
rpts[begin_id][1] = cx;
mid_track[begin_id][0] = cy;
mid_track[begin_id][1] = cx;
rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
GetLinesResample(rpts + begin_id, rpts_num - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER );
GetLinesResample(mid_track + begin_id, mid_track_count - begin_id, rptsn, &rptsn_num, RESAMPLEDIST * PIXPERMETER );
// 远预锚点位置
int aim_idx = clip(round(aim_distance / RESAMPLEDIST), 0, rptsn_num - 1);
@@ -111,7 +111,7 @@ void MidLineTrack() {
//考虑近点
near_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx_near / dn_near / dn_near) / PI * 180 ;
//考虑远点
pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ;
``` pure_angle = -atanf(PIXPERMETER * 2 * 0.2 * dx/ dn / dn) / PI * 180 ;