waiwaylee/radar_data_process
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新：1、使用VS code+cmake重新编译，编译器保持不变；

Browse Source 
2、修复若干逻辑bug，具体参考BUG_FIX_REPORT.md

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-08-18 11:26:19 +08:00
parent 60ad5c136f
commit 01d28e0d28
49 changed files with 1452 additions and 957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_init.cpp
+75 -112
View File
@@ -1,17 +1,16 @@
#include "track_init.h"
#include "kalman.h"
#include "coor_trans.h"
#include <qmath.h>
#include "memory.h"
#include <QVector>
#include <cmath>
#include <cstring>
#include <vector>
#include <iostream>
#include<iomanip>
using namespace std;
int Track_Init::track_init_process_logic( QVector <PointRecv> *point_recv, //输入点迹
QVector<Trust_Track> *trust_track, //可靠航迹
QVector <QVector<Temp_track>> *temp_track,
int Track_Init::track_init_process_logic( std::vector <PointRecv> *point_recv, //输入点迹
std::vector<Trust_Track> *trust_track, //可靠航迹
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter
@@ -25,7 +24,7 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// || (*point_recv)[i].CPI_Time == 30682132 || (*point_recv)[i].CPI_Time == 30688692
// || (*point_recv)[i].CPI_Time == 30691692 || (*point_recv)[i].CPI_Time == 30691880)
// {
// qDebug() << "azi=" << (*point_recv)[i].Azimuth
// std::cout << "azi=" << (*point_recv)[i].Azimuth
// << "dis=" << (*point_recv)[i].Range
// << "h=" << (*point_recv)[i].Height
// << "v=" << (*point_recv)[i].Velocity
@@ -34,13 +33,10 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// }
// }
//取出待起航的点迹数据
for (int i=0;i<(*point_recv).size();i++)
point_process.push_back((*point_recv)[i]);
//临时航迹的buff_round+1
for (int i=0;i<temp_track->size();i++)
{
@@ -59,7 +55,7 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
point_track_head_asso(temp_track,Work_Parameter);
//删除关联上的点迹
QVector <PointRecv>::iterator Iter;
std::vector <PointRecv>::iterator Iter;
for (Iter=point_process.begin(); Iter!=point_process.end();)
{
if((*Iter).Use_Flag==1)
@@ -74,15 +70,14 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
}
//剩余点重新存入点迹
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap((*point_recv));
for (int i=0;i<point_process.size();i++)
(*point_recv).push_back(point_process[i]);
//剩余点转为航迹头
for (unsigned int i=0;i<point_process.size();i++)
{
Temp_track temp_track_tmp;
Temp_track temp_track_tmp = {};
temp_track_tmp.r = point_process[i].Range;
temp_track_tmp.azi = point_process[i].Azimuth;
temp_track_tmp.height = point_process[i].Height;
@@ -102,14 +97,13 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
temp_track_tmp.GNSS_time = point_process[i].GNSS_time;
temp_track_tmp.buff_round = 1;
// temp_track_tmp.Temp_track_section_idx = floor(point_process[i].beam_index/BEAM_NUM_DOT_SECTION)+1;
temp_track->push_back( QVector <Temp_track> ());
temp_track->push_back( std::vector <Temp_track> ());
int n=temp_track->size();
(*temp_track)[n-1].push_back(temp_track_tmp);
// if(point_process[i].CPI_Time == 30675948 || point_process[i].CPI_Time == 30682320)
// {
// qDebug() << "point_process_azi=" << point_process[i].Azimuth
// std::cout << "point_process_azi=" << point_process[i].Azimuth
// << "point_process_dis=" << point_process[i].Range
// << "point_process_h=" << point_process[i].Height
// << "point_process_v=" << point_process[i].Velocity
@@ -120,28 +114,23 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
}
//清空点迹
QVector<PointRecv>().swap(point_process);
QVector<PointRecv>().swap((*point_recv));
std::vector<PointRecv>().swap(point_process);
std::vector<PointRecv>().swap((*point_recv));
//临时航迹满足起始长度 转为可靠航迹
tmp_track_to_trust_track(trust_track,temp_track,Trust_Track_Output, Trust_track_num_Output,Work_Parameter);
}
//消亡临时航迹
tmp_track_die(temp_track);
// qDebug() << "-----------------------------------------------";
// std::cout << "-----------------------------------------------";
// for(int i=0;i<temp_track->size();i++)
// {
// for(int j=0;j<(*temp_track)[i].size();j++)
// {
// qDebug() << "temp_track_azi=" << (*temp_track)[i][j].azi
// std::cout << "temp_track_azi=" << (*temp_track)[i][j].azi
// << "temp_track_dis=" << (*temp_track)[i][j].r
// << "temp_track_h=" << (*temp_track)[i][j].height
// << "temp_track_v=" << (*temp_track)[i][j].vr
@@ -149,18 +138,14 @@ int Track_Init::track_init_process_logic( QVector <PointRecv> *p
// }
// }
return 0;
}
void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_track,
void Track_Init::point_temp_track_asso(std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
//关联信息
struct Asso_info
{
@@ -168,7 +153,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
int point_idx;
double d;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
for ( int i=0;i<point_process.size();i++)
{
@@ -219,11 +204,9 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
double y2=Z[0]*sin(Z[1]);
double alpha = alpha_cal_track_init(x0,y0,x1,y1,x2,y2);
// if(T_track_head == 33855816 && T_point == 33859004 )
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "r_temp_track=" << r_temp_track
@@ -235,9 +218,6 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
// }
if(d*d<=TRACK_START_THRESHOLD*TRACK_START_THRESHOLD && alpha<ALPHA_START && vr_point*v_temp_track>0)// && fabs(h_point-h_temp_track )<= r_point*SIGMA_E&& abs(vr_point-v_temp_track)/abs(v_temp_track)<0.8
{
struct Asso_info asso_info_tmp;
@@ -247,7 +227,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][L-1].asso_flag = 1;
point_process[i].Use_Flag = 1;
//qDebug() << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point;
//std::cout << "v_temp_track : " << v_temp_track << "vr_point : "<< vr_point;
}
}
}
@@ -257,7 +237,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//前L个点
int L = (*temp_track)[asso_info[i].track_idx-1].size();
@@ -268,7 +248,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
}
//关联上的点
Temp_track asso_track_info_tmp;
Temp_track asso_track_info_tmp = {};
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height;
@@ -300,9 +280,8 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
(*temp_track)[(*temp_track).size()-1].push_back(asso_track_info_tmp);
}
//temp_track中删除关联上的临时航迹
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter).size()>=2)
@@ -324,11 +303,7 @@ void Track_Init::point_temp_track_asso(QVector <QVector<Temp_track>> *temp_t
}
}
void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_track,
void Track_Init::point_track_head_asso( std::vector <std::vector<Temp_track>> *temp_track,
struct RadarPara Work_Parameter)
{
@@ -338,8 +313,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
int track_idx;
int point_idx;
};
QVector <Asso_info> asso_info;
std::vector <Asso_info> asso_info;
//关联
for ( int i=0;i<point_process.size();i++)
@@ -351,7 +325,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
if((*temp_track)[j].size()==1 && (*temp_track)[j][0].buff_round >= 2)
{
//点迹信息
double x_point, y_point, vr_point;
coor_trans Coor_trans;
@@ -371,7 +344,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -382,8 +355,8 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_point == 30682320)
// {
// qDebug() <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// qDebug() << "r_point=" << r_point
// std::cout <<"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!";
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -392,7 +365,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// << "T_track_head=" << T_track_head;
// }
//距离差
double dis = sqrt(pow(x_point-x_track_head,2)+pow(y_point-y_track_head,2));
@@ -408,8 +380,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
double delta_T = (T_point - T_track_head)/1000.0;
//满足关联条件的点航
// if( ( (r_point>=1000 && dis<=vmax*delta_T && dis>=V_MIN*delta_T) || (r_point<1000 && dis<=vmax*delta_T/2.0 && dis>=V_MIN*delta_T/2.0) )
// && vr_point*v_track_head>0 )//&& fabs(vr_point-v_track_head)/fabs(v_track_head)<0.2 && fabs(h_point-h_track_head)<=r_point*SIGMA_E
@@ -422,7 +392,7 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
// if(T_track_head == 30675948)
// {
// qDebug() << "r_point=" << r_point
// std::cout << "r_point=" << r_point
// << "h_point=" << h_point
// << "T_point=" << T_point
// << "x_track_head=" << x_track_head
@@ -437,27 +407,24 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
asso_info.push_back(asso_info_tmp);
(*temp_track)[j][0].asso_flag = 1;
point_process[i].Use_Flag = 1;
//qDebug() << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
//std::cout << "v_track_head: " << v_track_head << "vr_point: " << vr_point;
}
}
}
}
// temp_track中加入新关联上的临时航迹
for (int i = 0 ; i<asso_info.size();i++ )
{
(*temp_track).push_back(QVector <Temp_track> ());
(*temp_track).push_back(std::vector <Temp_track> ());
//第一个点
(*temp_track)[(*temp_track).size()-1].push_back((*temp_track)[asso_info[i].track_idx-1][0]);
(*temp_track)[(*temp_track).size()-1][0].asso_flag = 0;
//第二个点
struct Temp_track asso_track_info_tmp;
struct Temp_track asso_track_info_tmp = {};
asso_track_info_tmp.r = point_process[asso_info[i].point_idx-1].Range;
asso_track_info_tmp.azi = point_process[asso_info[i].point_idx-1].Azimuth;
asso_track_info_tmp.height = point_process[asso_info[i].point_idx-1].Height;
@@ -489,10 +456,8 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
}
//temp_track中删除关联上的航迹头
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
if((*Iter)[0].asso_flag==1)
@@ -509,8 +474,6 @@ void Track_Init::point_track_head_asso( QVector <QVector<Temp_track>> *temp_
}
////////////////////////////////////////////计算三点间的夹角////////////////////////////////////////////////
double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,double x2,double y2)
{
@@ -520,23 +483,19 @@ double Track_Init::alpha_cal_track_init(double x0,double y0,double x1,double y1,
return alpha;
}
void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
QVector <QVector<Temp_track>> *temp_track,
void Track_Init::tmp_track_to_trust_track(std::vector<Trust_Track> *trust_track,
std::vector <std::vector<Temp_track>> *temp_track,
struct Track Trust_Track_Output[MAX_TRACK_NUM][10],
int *Trust_track_num_Output,
struct RadarPara Work_Parameter)
{
QVector <QVector<Temp_track>> Track_to_start;
std::vector <std::vector<Temp_track>> Track_to_start;
// 1. 将temp_track中满足条件的航迹取出, 放到Track_to_start中
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int L=(*Iter).size();
@@ -556,7 +515,7 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
if( ( L==Work_Parameter.track_start_point_num)&& track_init_prohibit(range,azi,Work_Parameter)==0 && snr_flag) //按长度查找TRUST_TRACK_POINT
{
//加入Track_to_start中
Track_to_start.push_back(QVector <Temp_track> ());
Track_to_start.push_back(std::vector <Temp_track> ());
for (int i = 0; i<L; i++)
{
Track_to_start[Track_to_start.size()-1].push_back((*Iter)[i]);
@@ -570,16 +529,21 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
if (Track_to_start.empty())
return;
//2.两两比较Track_to_start中的航迹信息，删除重复的航迹
int L = Work_Parameter.track_start_point_num;
for (int i=0; i<Track_to_start.size()-1;i++)
for (size_t i=0; i+1<Track_to_start.size();i++)
{
if(Track_to_start[i][0].asso_flag!=2)
{
for (int j=i+1;j<Track_to_start.size();j++)
for (size_t j=i+1;j<Track_to_start.size();j++)
{
if(Track_to_start[j][0].asso_flag!=2)
{
if (Track_to_start[i].size() < 3 || Track_to_start[j].size() < 3)
continue;
if( (Track_to_start[i][0].T == Track_to_start[j][0].T && Track_to_start[i][0].r == Track_to_start[j][0].r)
|| (Track_to_start[i][1].T == Track_to_start[j][1].T && Track_to_start[i][1].r == Track_to_start[j][1].r)
@@ -595,17 +559,14 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Track_to_start[i][0].asso_flag=2;
}
}
}
}
}
}
QVector <QVector<Temp_track>>::iterator Iter1;
std::vector <std::vector<Temp_track>>::iterator Iter1;
for (Iter1=Track_to_start.begin(); Iter1!=Track_to_start.end();)
{
if( (*Iter1)[0].asso_flag == 2 )
@@ -619,15 +580,10 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
//3.Track_to_start中剩余的航迹起始为可靠航迹
for (int i=0; i<Track_to_start.size();i++)
for (size_t i=0; i<Track_to_start.size();i++)
{
if (Track_to_start[i].size() < 3) continue;
if(trust_track->size()<MAX_TRACK_INDEX)
{
int index = track_index_mangement.track_ind_get(trust_track);
@@ -645,7 +601,7 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
kalman Kalman;
Kalman.kalman_filter_init_3dots(Z0, Z1, Z2, T1,T2, X ,P);
Trust_Track trust_track_tmp;
Trust_Track trust_track_tmp = {};
memcpy(trust_track_tmp.X, X, 6*sizeof(double));
memcpy(trust_track_tmp.P, P, 6*6*sizeof(double));
@@ -684,7 +640,6 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
(*trust_track).push_back(trust_track_tmp);
//输出航迹更新信息
*Trust_track_num_Output=*Trust_track_num_Output+1;
for (int j=0;j<L;j++)
@@ -694,14 +649,18 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Trust_Track_Output[*Trust_track_num_Output-1][j].Range=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].Azimuth=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Elevation=asin(elev_ratio)/PI*180;
}
Trust_Track_Output[*Trust_track_num_Output-1][j].Range_V=sqrt(pow(Track_to_start[i][j].X[1],2)+pow(Track_to_start[i][j].X[3],2));
Trust_Track_Output[*Trust_track_num_Output-1][j].z=Track_to_start[i][j].height;
Trust_Track_Output[*Trust_track_num_Output-1][j].Amplitude=Track_to_start[i][j].Amp;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_snr = Track_to_start[i][j].snr;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_rcs = Track_to_start[i][j].RCS;
Trust_Track_Output[*Trust_track_num_Output-1][j].pitch_num = Track_to_start[i][j].pitch_num;
std::memcpy(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim, Track_to_start[i][j].speed_dim, sizeof(Trust_Track_Output[*Trust_track_num_Output-1][j].speed_dim));
@@ -712,8 +671,6 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Direction_Angle=Direction_Angle+2*PI;
Trust_Track_Output[*Trust_track_num_Output-1][j].Direction_Angle=Direction_Angle/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].track_time=(Track_to_start[i][j].T)/1000.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].GNSS_time=Track_to_start[i][j].GNSS_time;
@@ -724,7 +681,12 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
Trust_Track_Output[*Trust_track_num_Output-1][j].range_point=Track_to_start[i][j].r;
Trust_Track_Output[*Trust_track_num_Output-1][j].azi_point=Track_to_start[i][j].azi/PI*180;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(Track_to_start[i][j].height/Track_to_start[i][j].r)/PI*180;
{
double elev_ratio = (Track_to_start[i][j].r > 0.0) ? Track_to_start[i][j].height / Track_to_start[i][j].r : 0.0;
if (elev_ratio > 1.0) elev_ratio = 1.0;
if (elev_ratio < -1.0) elev_ratio = -1.0;
Trust_Track_Output[*Trust_track_num_Output-1][j].elev_point=asin(elev_ratio)/PI*180;
}
Trust_Track_Output[*Trust_track_num_Output-1][j].vr_point=Track_to_start[i][j].vr;
Trust_Track_Output[*Trust_track_num_Output-1][j].point_type=0;
Trust_Track_Output[*Trust_track_num_Output-1][j].Flag_Point=1;
@@ -735,20 +697,23 @@ void Track_Init::tmp_track_to_trust_track(QVector<Trust_Track> *trust_track,
}
}
QVector <QVector<Temp_track>>().swap(Track_to_start);
std::vector <std::vector<Temp_track>>().swap(Track_to_start);
}
void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
void Track_Init::tmp_track_die(std::vector <std::vector<Temp_track>> *temp_track)
{
QVector <QVector<Temp_track>>::iterator Iter;
std::vector <std::vector<Temp_track>>::iterator Iter;
for (Iter=temp_track->begin(); Iter!=temp_track->end();)
{
int n=(*Iter).size();
if (n <= 0)
{
temp_track->erase(Iter);
Iter=temp_track->begin();
continue;
}
if( (*Iter)[n-1].buff_round >2 || n>=10)
{
@@ -762,13 +727,9 @@ void Track_Init::tmp_track_die(QVector <QVector<Temp_track>> *temp_track)
}
};
int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara Work_Parameter)
{
for (int i=0;i<Work_Parameter.track_prohibite_area_num;i++)
{
if(r<Work_Parameter.R_max_track_prohibited[i] && r>Work_Parameter.R_min_track_prohibited[i] && azi<Work_Parameter.Azimuth_max_track_prohibited[i] && azi>Work_Parameter.Azimuth_min_track_prohibited[i])
@@ -779,13 +740,15 @@ int Track_Init::track_init_prohibit(double r, double azi, struct RadarPara
return 0;
}
Track_Init::Track_Init()
{
}
void Track_Init::reset()
{
point_process.clear();
track_index_mangement.reset();
}