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

更新：1、增加航迹质量信息输出；

Browse Source 
2、航迹起始时计算初始航迹质量；
3、航迹关联时更新航迹质量；

Signed-off-by: waiwaylee <waiwaylee@foxmail.com>
This commit is contained in:
waiwaylee
2026-10-19 15:03:00 +08:00
parent 1b6cf62115
commit 1ae0cf2512
5 changed files with 86 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_process_class_dll/track_asso.cpp
+17 -1
View File
@@ -100,6 +100,7 @@ int Track_Asso:: track_asso_process(std::vector<PointRecv> *point_
Trust_Track_Output[*Trust_track_num_Output-1][0].GNSS_time=(*trust_track)[i].GNSS_time;
Trust_Track_Output[*Trust_track_num_Output-1][0].Flag_Point = (*trust_track)[i].point_flag;
Trust_Track_Output[*Trust_track_num_Output-1][0].z=static_cast<float>((*trust_track)[i].Height+Work_Parameter.Height);
Trust_Track_Output[*Trust_track_num_Output-1][0].Quality = static_cast<float>((*trust_track)[i].Quality * TRACK_QUALITY_MAX);
double Direction_Angle;
Direction_Angle=atan2((*trust_track)[i].X[4], (*trust_track)[i].X[1]);
@@ -656,6 +657,14 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*trust_track)[track_index-1].P3[ii][jj]=P3_filter[ii][jj];
}
//计算航迹质量
double d_tmp = exp(-associated_info[min_index-1].d_min);
double snr_tmp = 1/(1+exp(-(point_process[point_index-1].SNR - 16) / 1.2));
double len_tmp = 1/(1+exp(-((*trust_track)[track_index-1].Valid_point_sum - 4) / 1.2)) * 0.5 + (1 - (*trust_track)[track_index-1].Valid_point_sum / (*trust_track)[track_index-1].Point_sum) * 0.5;
double tq_tmp = TRACK_QUALITY_D_WEIGHT * d_tmp + TRACK_QUALITY_SNR_WEIGHT * snr_tmp + TRACK_QUALITY_LEN_WEIGHT * len_tmp;
(*trust_track)[track_index-1].Quality = (*trust_track)[track_index-1].Quality * TRACK_QUALITY_ITER_COEFF + tq_tmp * (1 - TRACK_QUALITY_ITER_COEFF);
//更新航迹时间
(*trust_track)[track_index-1].T_track = point_process[point_index-1].CPI_time;
(*trust_track)[track_index-1].GNSS_time = point_process[point_index-1].GNSS_time;
@@ -688,6 +697,8 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*trust_track)[track_index-1].point_flag=1; //实点
(*trust_track)[track_index-1].associate_point_number = (*trust_track)[track_index-1].associate_point_number+1; //关联点数+1
(*trust_track)[track_index-1].Track_Update_Flag = 1; //航迹更新标志
(*trust_track)[track_index-1].Point_sum = (*trust_track)[track_index-1].Point_sum + 1;
(*trust_track)[track_index-1].Valid_point_sum = (*trust_track)[track_index-1].Valid_point_sum + 1;
}
//删除associated_info中关联上的航迹 点迹信息
@@ -778,6 +789,7 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
memcpy((*trust_track)[i].P2,P2_pred,6*6*sizeof(double));
memcpy((*trust_track)[i].P3,P3_pred,6*6*sizeof(double));
(*trust_track)[i].circle_id++;
(*trust_track)[i].T_track = static_cast<long long>((*trust_track)[i].T_track+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].GNSS_time = static_cast<long long>((*trust_track)[i].GNSS_time+delta_T*1000.0); //更新航迹时间
(*trust_track)[i].Extrapolate_round =(*trust_track)[i].Extrapolate_round+1; //连续未用实点更新时间
@@ -788,9 +800,13 @@ void Track_Asso:: model_filter(std::vector <Trust_Track> *trust_track,struct Ra
(*trust_track)[i].point_flag=0; //虚点
(*trust_track)[i].Track_Update_Flag = 0; //航迹更新标志
}
//航迹质量递减
(*trust_track)[i].Quality -= (*trust_track)[i].Extrapolate_round * TRACK_QUALITY_DECREASE_COEFF;
(*trust_track)[i].Quality = (*trust_track)[i].Quality < 0.0 ? 0.0 : (*trust_track)[i].Quality;
(*trust_track)[i].Point_sum = (*trust_track)[i].Point_sum + 1;
}
}
}
void Track_Asso:: model_output(std::vector <Trust_Track> *trust_track)