|
|
@@ -580,6 +580,299 @@ POS* LocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO
|
|
|
return p;
|
|
|
}
|
|
|
|
|
|
+std::shared_ptr<POS> LocateAlgorithm::Pos(std::shared_ptr<ReceiveDataMap> pRdm)
|
|
|
+{
|
|
|
+ if (pRdm->size() < 3)
|
|
|
+ {
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ std::shared_ptr<POS> p = std::make_shared<POS>();
|
|
|
+
|
|
|
+ //2D定位
|
|
|
+ std::vector<double> vtk; //保存ki
|
|
|
+ std::vector<double> vtd; //保存di
|
|
|
+ std::vector<_coordinate> vtc; //保存xi,1 yi,1
|
|
|
+ std::vector<_point> vtp; //保存三角形的顶点坐标
|
|
|
+ std::vector<int> vts;
|
|
|
+
|
|
|
+ vtk.resize(0);
|
|
|
+ vtd.resize(0);
|
|
|
+ vtc.resize(0);
|
|
|
+ vts.resize(0);
|
|
|
+
|
|
|
+ _coordinate tc;
|
|
|
+
|
|
|
+ int i = 0;
|
|
|
+ unsigned long long time_stamp = 0; //保存第一次的插值时间戳
|
|
|
+ for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end()&&i<3;++it)
|
|
|
+ {
|
|
|
+ double k = 0 ;
|
|
|
+ k = pow(it->second->x,2) + pow(it->second->y,2) ;//+ pow(it->second->z,2)
|
|
|
+ vtk.push_back(k);
|
|
|
+
|
|
|
+ if (i == 0 )
|
|
|
+ {
|
|
|
+ time_stamp = it->second->rec_time_stamp;
|
|
|
+ tc.x = it->second->x;
|
|
|
+ tc.y = it->second->y;
|
|
|
+ //tc.z = it->second->z;
|
|
|
+ }
|
|
|
+ long long diff_time = it->second->rec_time_stamp - time_stamp;
|
|
|
+ double d = 0;
|
|
|
+ d = CFunctions::getDistance(diff_time,CFunctions::TDOA);
|
|
|
+ if (i>0)
|
|
|
+ {
|
|
|
+ if (d>0)
|
|
|
+ {
|
|
|
+ vts.push_back(1);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ vts.push_back(-1);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ vtd.push_back(d);
|
|
|
+
|
|
|
+ _coordinate dtc;
|
|
|
+ dtc.x = it->second->x - tc.x;
|
|
|
+ dtc.y = it->second->y - tc.y;
|
|
|
+ //dtc.z = it->second->z - tc.z;
|
|
|
+ vtc.push_back(dtc);
|
|
|
+
|
|
|
+ _point p;
|
|
|
+ p.x = it->second->x;
|
|
|
+ p.y = it->second->y;
|
|
|
+ vtp.push_back(p);
|
|
|
+ i++;
|
|
|
+ }
|
|
|
+
|
|
|
+ double a[20] = {0};
|
|
|
+ double dt = 0;
|
|
|
+
|
|
|
+ dt = vtd[1]*vtc[2].y - vtd[2]*vtc[1].y;
|
|
|
+ if (fabs(dt) < 1E-5)
|
|
|
+ {
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ a[0] = (vtd[2]*vtc[1].x - vtd[1]*vtc[2].x)/dt;
|
|
|
+ a[1] = (-1)*((vtk[1] - vtk[0] - pow(vtd[1],2))*vtd[2] - (vtk[2] - vtk[0] - pow(vtd[2],2))*vtd[1])*0.5/dt;
|
|
|
+
|
|
|
+ if (fabs(vtd[1]) < 1E-5)
|
|
|
+ {
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ a[2] = 0.5*(vtk[1] - vtk[0] - pow(vtd[1],2) - 2*vtc[1].y*a[1])/vtd[1];
|
|
|
+ a[3] = -1*(vtc[1].x + vtc[1].y*a[0])/vtd[1];
|
|
|
+
|
|
|
+ double A = 0, B = 0, C = 0;
|
|
|
+ A = pow(a[3],2) - 1 - pow(a[0],2);
|
|
|
+ B = 2*(a[2]*a[3] + tc.x + a[0]*(tc.y - a[1]));
|
|
|
+ C = pow(a[2],2) - pow(tc.x,2) - pow(tc.y-a[1],2);
|
|
|
+
|
|
|
+ _point pos[MAX_READER_TDOA_PATH_NUMS];
|
|
|
+ double delta = 0.0;
|
|
|
+ int count = 0;
|
|
|
+
|
|
|
+ delta = pow(B,2) - 4*A*C;
|
|
|
+ if (delta > 0)
|
|
|
+ {
|
|
|
+ pos[0].x = ((-1)*B + sqrt(delta))/(2*A);
|
|
|
+ pos[0].y = a[0]*pos[0].x + a[1];
|
|
|
+ //TRACE(_T("x1: %.2f , y1: %.2f \r\n"),pos[0].x,pos[0].y);
|
|
|
+ pos[1].x = ((-1)*B - sqrt(delta))/(2*A);
|
|
|
+ pos[1].y = a[0]*pos[1].x + a[1];
|
|
|
+ //TRACE(_T("x2: %.2f , y2: %.2f \r\n"),pos[1].x,pos[1].y);
|
|
|
+ count = 2;
|
|
|
+ }else{
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+
|
|
|
+ int idx = -1;
|
|
|
+ int points = 0;
|
|
|
+ for (int i=0;i<count;i++)
|
|
|
+ {
|
|
|
+ if (IsInTriangle(vtp,pos[i]))
|
|
|
+ {
|
|
|
+ //TRACE(_T("point is in triangle. the index is : %d \r\n"),i);
|
|
|
+ bool condition[2] = {false,false};
|
|
|
+ double d1 = 0,d2 = 0;
|
|
|
+
|
|
|
+ //对两解进行判断，需要同时满足两个条件
|
|
|
+ //1.解到点1和点2的距离差的方向性和之前的参数相同
|
|
|
+ //2.解到点1和点3的距离差的方向性和之前的参数相同
|
|
|
+ d1 = sqrt(pow(vtp[1].x - pos[i].x,2) + pow(vtp[1].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));
|
|
|
+ d2 = sqrt(pow(vtp[2].x - pos[i].x,2) + pow(vtp[2].y - pos[i].y,2)) - sqrt(pow(vtp[0].x - pos[i].x,2) + pow(vtp[0].y - pos[i].y,2));
|
|
|
+
|
|
|
+ if ((d1<0&&vts[0]==-1)||(d1>0&&vts[0] == 1))
|
|
|
+ {
|
|
|
+ condition[0] = true;
|
|
|
+ }
|
|
|
+ if ((d2<0&&vts[1]==-1)||(d2>0&&vts[1] == 1))
|
|
|
+ {
|
|
|
+ condition[1] =true;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (condition[0]&&condition[1])
|
|
|
+ {
|
|
|
+ idx = i;
|
|
|
+ }
|
|
|
+
|
|
|
+ //idx = i;
|
|
|
+ }else{
|
|
|
+ //TRACE(_T("point is not in triangle. the index is : %d \r\n"),i);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (points == 2)
|
|
|
+ {
|
|
|
+ idx = -1;
|
|
|
+ //TRACE(_T("There hava 2 point.\r\n"));
|
|
|
+ }
|
|
|
+
|
|
|
+ if (idx != -1)
|
|
|
+ {
|
|
|
+ //TRACE(_T("the idx is : %d \r\n"),idx);
|
|
|
+ p->posx = pos[idx].x;
|
|
|
+ p->posy = pos[idx].y;
|
|
|
+ }else{
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+
|
|
|
+ //算法一
|
|
|
+ //ReceiveDataMap::iterator first = pRdm->begin();
|
|
|
+
|
|
|
+ ////////解
|
|
|
+ //Eigen::VectorXd x(3);
|
|
|
+ ////需要设置初始坐标
|
|
|
+ //x[0] = 43; //500,500,100 //4300,200,90
|
|
|
+ //x[1] = 2;
|
|
|
+ //x[2] = 0.9;
|
|
|
+
|
|
|
+ //Eigen::MatrixXd data(3,4),pos(1,3);
|
|
|
+
|
|
|
+ //pos(0,0) = first->second->x;
|
|
|
+ //pos(0,1) = first->second->y;
|
|
|
+ //pos(0,2) = first->second->z;
|
|
|
+
|
|
|
+ //int i = 0;
|
|
|
+ //ReceiveDataMap::iterator it = pRdm->begin();
|
|
|
+ //it++;
|
|
|
+ //for (;it!=pRdm->end();++it)
|
|
|
+ //{
|
|
|
+ // data(i,0) = it->second->x;
|
|
|
+ // data(i,1) = it->second->y;
|
|
|
+ // data(i,2) = it->second->z;
|
|
|
+
|
|
|
+ // data(i,3) = (it->second->rec_time_stamp - first->second->rec_time_stamp) * 15.65 * 2.99702547*0.0001; //0.01
|
|
|
+ // i++;
|
|
|
+ //}
|
|
|
+
|
|
|
+ //TdoaAlgo::TDOAFunctor functor(data,pos);
|
|
|
+ //Eigen::NumericalDiff<TdoaAlgo::TDOAFunctor> num_diff(functor);
|
|
|
+ //Eigen::LevenbergMarquardt<Eigen::NumericalDiff<TdoaAlgo::TDOAFunctor>,double> lm(num_diff);
|
|
|
+ //lm.parameters.maxfev = 2000;
|
|
|
+ //lm.parameters.xtol = 1.0e-10;
|
|
|
+
|
|
|
+ //int ret = lm.minimize(x);
|
|
|
+ //if (ret > 0)
|
|
|
+ //{
|
|
|
+ // p->posx = x[0];
|
|
|
+ // p->posy = x[1];
|
|
|
+ // p->posz = x[2];
|
|
|
+ // TRACE(_T("x: %.2f , y: %.2f , z: %.2f"),p->posx,p->posy,p->posz);
|
|
|
+ //}
|
|
|
+
|
|
|
+ //算法二
|
|
|
+ //std::vector<double> vtk;
|
|
|
+ //std::vector<double> vtd;
|
|
|
+ //vtk.resize(0);
|
|
|
+ //vtd.resize(0);
|
|
|
+
|
|
|
+ //_coordinate tc;
|
|
|
+ //std::vector<_coordinate> vtc;
|
|
|
+ //vtc.resize(0);
|
|
|
+
|
|
|
+ //int i = 0;
|
|
|
+ //unsigned long long time_stamp = 0; //保存第一次的插值时间戳
|
|
|
+ //for (ReceiveDataMap::iterator it = pRdm->begin();it != pRdm->end();++it)
|
|
|
+ //{
|
|
|
+ // double k = 0 ;
|
|
|
+ // k = pow(it->second->x,2) + pow(it->second->y,2) + pow(it->second->z,2);
|
|
|
+ // vtk.push_back(k);
|
|
|
+
|
|
|
+ // if (i == 0 )
|
|
|
+ // {
|
|
|
+ // time_stamp = it->second->rec_time_stamp;
|
|
|
+ // tc.x = it->second->x;
|
|
|
+ // tc.y = it->second->y;
|
|
|
+ // tc.z = it->second->z;
|
|
|
+ // }
|
|
|
+ // long long diff_time = it->second->rec_time_stamp - time_stamp;
|
|
|
+ // double d = 0;
|
|
|
+ // d = CFunctions::getDistance(diff_time,CFunctions::TDOA);
|
|
|
+ // vtd.push_back(d);
|
|
|
+
|
|
|
+ // _coordinate dtc;
|
|
|
+ // dtc.x = it->second->x - tc.x;
|
|
|
+ // dtc.y = it->second->y - tc.y;
|
|
|
+ // dtc.z = it->second->z - tc.z;
|
|
|
+ // vtc.push_back(dtc);
|
|
|
+ // i++;
|
|
|
+ //}
|
|
|
+
|
|
|
+ //double a[20] = {0};
|
|
|
+
|
|
|
+ //a[0] = vtk[1] - pow(vtd[1],2) - vtk[0];
|
|
|
+ //a[1] = vtk[2] - pow(vtd[2],2) - vtk[0];
|
|
|
+ //a[2] = vtk[3] - pow(vtd[3],2) - vtk[0];
|
|
|
+ //double dt = vtc[2].y*vtd[1] - vtc[1].y*vtd[2];
|
|
|
+ //if (fabs(dt) < 1E-5)
|
|
|
+ //{
|
|
|
+ // return nullptr;
|
|
|
+ //}
|
|
|
+ //a[3] = (vtc[1].x*vtd[2] - vtc[2].x*vtd[1])/dt;
|
|
|
+ //a[4] = (vtc[1].z*vtd[2] - vtc[2].z*vtd[1])/dt;
|
|
|
+ //a[5] = 0.5*(a[1]*vtd[1] - a[0]*vtd[2])/dt;
|
|
|
+ //
|
|
|
+ //dt = vtc[3].y*vtd[2] - vtc[2].y*vtd[3];
|
|
|
+ //if (fabs(dt) < 1E-5)
|
|
|
+ //{
|
|
|
+ // return nullptr;
|
|
|
+ //}
|
|
|
+ //a[6] = (vtc[2].x*vtd[3] - vtc[3].x*vtd[2])/dt;
|
|
|
+ //a[7] = (vtc[2].z*vtd[3] - vtc[3].z*vtd[2])/dt;
|
|
|
+ //a[8] = 0.5*(a[2]*vtd[2] - a[1]*vtd[3])/dt;
|
|
|
+ //a[9] = (a[3] - a[6])/(a[7] - a[4]);
|
|
|
+ //a[10] = (a[5] - a[8])/(a[7] - a[4]);
|
|
|
+ //a[11] = a[7] + a[8]*a[10];
|
|
|
+ //a[12] = a[7]*a[10];
|
|
|
+ //a[13] = (-1)*(vtc[1].x/vtd[1] + vtc[1].y*a[11]/vtd[1] + vtc[1].z*a[9]/vtd[1]);
|
|
|
+ //a[14] = 0.5*a[0]/vtd[1] - vtc[1].y*a[12]/vtd[1] - vtc[1].z*a[10]/vtd[1];
|
|
|
+ //
|
|
|
+ //double A = 0, B = 0, C = 0;
|
|
|
+ //A = 1 + pow(a[11],2) + pow(a[9],2) - pow(a[13],2);
|
|
|
+ //B = (-2)*(tc.x + a[11]*(tc.y - a[12]) + a[9]*(tc.z - a[10]) + a[13]*a[14]);
|
|
|
+ //C = pow(tc.x,2) + pow(tc.y - a[12],2) + pow(tc.z - a[10],2) - pow(a[14],2);
|
|
|
+
|
|
|
+ //double x1 = 0,x2 = 0,y1 = 0, y2 = 0, z1 = 0, z2 = 0;
|
|
|
+ //double delta = 0;
|
|
|
+ //delta = pow(B,2) - 4*A*C;
|
|
|
+ //if (delta > 0)
|
|
|
+ //{
|
|
|
+ // x1 = ((-1)*B + sqrt(delta))/(2*A);
|
|
|
+ // y1 = a[11]*x1 + a[12];
|
|
|
+ // z1 = a[9]*x1 + a[10];
|
|
|
+ // x2 = ((-1)*B - sqrt(delta))/(2*A);
|
|
|
+ // y2 = a[11]*x2 + a[12];
|
|
|
+ // z2 = a[9]*x2 + a[10];
|
|
|
+ //}else{
|
|
|
+ // return nullptr;
|
|
|
+ //}
|
|
|
+
|
|
|
+ return p;
|
|
|
+}
|
|
|
+
|
|
|
SOLUTION* LocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i)
|
|
|
{
|
|
|
double x1 = 0.0;
|
|
|
@@ -1418,6 +1711,39 @@ bool LocateAlgorithm::IsOnMap(std::shared_ptr<POS> pos,std::shared_ptr<TDOAReade
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
+double LocateAlgorithm::GetTriangleArea(_point p0,_point p1,_point p2)
|
|
|
+{
|
|
|
+ _point ab,bc;
|
|
|
+
|
|
|
+ ab.x = p1.x - p0.x;
|
|
|
+ ab.y = p1.y - p0.y;
|
|
|
+
|
|
|
+ bc.x = p2.x - p1.x;
|
|
|
+ bc.y = p2.y - p1.y;
|
|
|
+
|
|
|
+ return abs(ab.x*bc.y - ab.y*bc.x)/2.0;
|
|
|
+}
|
|
|
+
|
|
|
+bool LocateAlgorithm::IsInTriangle(std::vector<_point> vtp,_point p)
|
|
|
+{
|
|
|
+ double sabc = 0,sadb = 0,sbdc = 0,sadc = 0;
|
|
|
+ sabc = GetTriangleArea(vtp[0],vtp[1],vtp[2]);
|
|
|
+ sadb = GetTriangleArea(vtp[0],p,vtp[1]);
|
|
|
+ sbdc = GetTriangleArea(vtp[1],p,vtp[2]);
|
|
|
+ sadc = GetTriangleArea(vtp[0],p,vtp[2]);
|
|
|
+
|
|
|
+ double sum = 0.0;
|
|
|
+ sum = sadb + sbdc + sadc;
|
|
|
+ if ((sabc - sum) > -1E-5 && (sabc - sum) < 1E-5)
|
|
|
+ {
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
bool LocateAlgorithm::CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale)
|
|
|
{
|
|
|
if(dScale <= 0.0){
|
