249 lines
7.9 KiB
C++
249 lines
7.9 KiB
C++
#include "dot_coh.h"
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <vector>
|
|
using namespace std;
|
|
|
|
int Dot_Coh::dot_coh_process(std::vector <PointRecv> *data_input,
|
|
std::vector <PointRecv> *point_recv,
|
|
struct RadarPara Work_Parameter)
|
|
{
|
|
|
|
if(data_input->size()>1)
|
|
{
|
|
|
|
for (unsigned int loop_of_point=0; loop_of_point<data_input->size()-1;loop_of_point++ )
|
|
{
|
|
if( (*data_input)[loop_of_point].Use_Flag!=1)
|
|
{
|
|
float point_0_R=static_cast<float>((*data_input)[loop_of_point].Range);
|
|
float point_0_V=static_cast<float>((*data_input)[loop_of_point].Velocity);
|
|
float point_0_F=static_cast<float>((*data_input)[loop_of_point].Azimuth);
|
|
float point_0_A=static_cast<float>((*data_input)[loop_of_point].Amplitude);
|
|
for (unsigned int i=loop_of_point+1;i<data_input->size();i++)
|
|
{
|
|
if((*data_input)[loop_of_point].Use_Flag!=1 && (*data_input)[i].Use_Flag!=1)
|
|
{
|
|
float point_1_R=static_cast<float>((*data_input)[i].Range);
|
|
float point_1_V=static_cast<float>((*data_input)[i].Velocity);
|
|
float point_1_F=static_cast<float>((*data_input)[i].Azimuth);
|
|
float point_1_A=static_cast<float>((*data_input)[i].Amplitude);
|
|
|
|
//凝聚条件： 距离、方位接近
|
|
if(Work_Parameter.work_mode == 0)
|
|
{
|
|
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
|
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
|
&& point_0_A <point_1_A )//
|
|
{
|
|
point_0_R=static_cast<float>((*data_input)[i].Range);
|
|
point_0_V=static_cast<float>((*data_input)[i].Velocity);
|
|
point_0_A=static_cast<float>((*data_input)[i].Amplitude);
|
|
point_0_F=static_cast<float>((*data_input)[i].Azimuth);
|
|
(*data_input)[loop_of_point].Use_Flag=1;
|
|
}
|
|
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
|
&& point_0_A >= point_1_A)//
|
|
{
|
|
(*data_input)[i].Use_Flag=1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
|
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
|
&& point_0_A <point_1_A )//
|
|
{
|
|
point_0_R=static_cast<float>((*data_input)[i].Range);
|
|
point_0_V=static_cast<float>((*data_input)[i].Velocity);
|
|
point_0_A=static_cast<float>((*data_input)[i].Amplitude);
|
|
point_0_F=static_cast<float>((*data_input)[i].Azimuth);
|
|
(*data_input)[loop_of_point].Use_Flag=1;
|
|
}
|
|
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
|
&& point_0_A >= point_1_A)//
|
|
{
|
|
(*data_input)[i].Use_Flag=1;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//删除凝聚点 和 量程范围外点
|
|
std::vector <PointRecv>::iterator Iter;
|
|
for (Iter=data_input->begin(); Iter!=data_input->end();)
|
|
{
|
|
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
|
|
{
|
|
data_input->erase(Iter);
|
|
Iter=data_input->begin();
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
}
|
|
|
|
for (size_t i=0;i<data_input->size();i++) //data_buffer_2 ---> point_recv
|
|
{
|
|
(*point_recv).push_back((*data_input)[i]);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int Dot_Coh::dot_coh_process_buff( std::vector <PointRecv> *data_input, //输入的点迹
|
|
std::vector <PointRecv> *point_recv, //输出点迹
|
|
struct RadarPara Work_Parameter //工作参数
|
|
)
|
|
{
|
|
|
|
//1. data_input与data_input_buff进行凝聚
|
|
|
|
//1.1 data_input、data_input_buff中的数据放在一起
|
|
std::vector <PointRecv> data_tmp;
|
|
for (size_t i=0;i<data_input_buff.size();i++)
|
|
{
|
|
data_tmp.push_back(data_input_buff[i]);
|
|
data_tmp[data_tmp.size()-1].point_section_asso = 1;
|
|
|
|
}
|
|
|
|
for (size_t i=0;i<data_input->size();i++)
|
|
{
|
|
data_tmp.push_back((*data_input)[i]);
|
|
data_tmp[data_tmp.size()-1].point_section_asso = 2;
|
|
}
|
|
|
|
//1.2 把data_input、data_input_buff清空
|
|
|
|
std::vector<PointRecv>().swap(data_input_buff);
|
|
std::vector<PointRecv>().swap(*data_input);
|
|
|
|
//1.3 对data_tmp进行凝聚
|
|
if(data_tmp.size()>1)
|
|
{
|
|
|
|
for (unsigned int loop_of_point=0; loop_of_point<data_tmp.size()-1;loop_of_point++ )
|
|
{
|
|
if( data_tmp[loop_of_point].Use_Flag!=1)
|
|
{
|
|
float point_0_R=static_cast<float>(data_tmp[loop_of_point].Range);
|
|
float point_0_V=static_cast<float>(data_tmp[loop_of_point].Velocity);
|
|
float point_0_F=static_cast<float>(data_tmp[loop_of_point].Azimuth);
|
|
float point_0_A=static_cast<float>(data_tmp[loop_of_point].Amplitude);
|
|
for (unsigned int i=loop_of_point+1;i<data_tmp.size();i++)
|
|
{
|
|
if(data_tmp[loop_of_point].Use_Flag!=1 && data_tmp[i].Use_Flag!=1)
|
|
{
|
|
float point_1_R=static_cast<float>(data_tmp[i].Range);
|
|
float point_1_V=static_cast<float>(data_tmp[i].Velocity);
|
|
float point_1_F=static_cast<float>(data_tmp[i].Azimuth);
|
|
float point_1_A=static_cast<float>(data_tmp[i].Amplitude);
|
|
|
|
//凝聚条件： 距离、方位接近
|
|
if(Work_Parameter.work_mode == 0)
|
|
{
|
|
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
|
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
|
&& point_0_A <point_1_A )//
|
|
{
|
|
point_0_R=static_cast<float>(data_tmp[i].Range);
|
|
point_0_V=static_cast<float>(data_tmp[i].Velocity);
|
|
point_0_A=static_cast<float>(data_tmp[i].Amplitude);
|
|
point_0_F=static_cast<float>(data_tmp[i].Azimuth);
|
|
data_tmp[loop_of_point].Use_Flag=1;
|
|
}
|
|
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI && fabs(point_0_V-point_1_V)<=DOT_COH_V)//
|
|
&& point_0_A >= point_1_A)//
|
|
{
|
|
data_tmp[i].Use_Flag=1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float delta_F = fabs(point_0_F-point_1_F)<2*PI-fabs(point_0_F-point_1_F) ? fabs(point_0_F-point_1_F) :2*PI-fabs(point_0_F-point_1_F);
|
|
if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
|
&& point_0_A <point_1_A )//
|
|
{
|
|
point_0_R=static_cast<float>(data_tmp[i].Range);
|
|
point_0_V=static_cast<float>(data_tmp[i].Velocity);
|
|
point_0_A=static_cast<float>(data_tmp[i].Amplitude);
|
|
point_0_F=static_cast<float>(data_tmp[i].Azimuth);
|
|
data_tmp[loop_of_point].Use_Flag=1;
|
|
}
|
|
else if( (fabs(point_0_R-point_1_R)<=DOT_COH_RANGE && delta_F <=DOT_COH_AZI/180.0*PI )//
|
|
&& point_0_A >= point_1_A)//
|
|
{
|
|
data_tmp[i].Use_Flag=1;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//1.4 删除凝聚点 和 量程范围外点
|
|
std::vector <PointRecv>::iterator Iter;
|
|
for (Iter=data_tmp.begin(); Iter!=data_tmp.end();)
|
|
{
|
|
if((*Iter).Use_Flag==1 || (*Iter).Range <R_MIN || (*Iter).Range > R_MAX )
|
|
{
|
|
data_tmp.erase(Iter);
|
|
Iter=data_tmp.begin();
|
|
}
|
|
else
|
|
{
|
|
Iter++;
|
|
}
|
|
}
|
|
|
|
//1.5 将data_tmp中凝聚后的点再分到 data_input_buff和data_input中
|
|
for (size_t i=0;i<data_tmp.size();i++)
|
|
{
|
|
if(data_tmp[i].point_section_asso==1)
|
|
{
|
|
data_input_buff.push_back(data_tmp[i]);
|
|
}
|
|
|
|
if(data_tmp[i].point_section_asso==2)
|
|
{
|
|
(*data_input).push_back(data_tmp[i]);
|
|
}
|
|
}
|
|
std::vector<PointRecv>().swap(data_tmp);
|
|
|
|
//2.data_input_buff数据输出给point_recv
|
|
|
|
for (size_t i=0;i<data_input_buff.size();i++)
|
|
{
|
|
(*point_recv).push_back(data_input_buff[i]);
|
|
}
|
|
|
|
std::vector<PointRecv>().swap(data_input_buff);
|
|
|
|
//3.data_input数据输出给data_input_buff
|
|
for (size_t i=0;i<data_input->size();i++)
|
|
{
|
|
data_input_buff.push_back((*data_input)[i]);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
Dot_Coh::Dot_Coh(void)
|
|
{
|
|
|
|
}
|
|
|
|
void Dot_Coh::reset()
|
|
{
|
|
data_input_buff.clear();
|
|
}
|