// 根据强信号点推理干扰源，多点定位
private void strongLoc()
{
List <List <int> > id = getStrongPtMult(); // 获得多点
MulPtLoc solve = new MulPtLoc((leftBound + rightBound) / 2, (downBound + upBound) / 2, 6);
for (int i = 0; i < id.Count; i++)
{
List <List <double> > X = new List <List <double> >(); // 测量点位置
List <double> P = new List <double>(); // 测量点功率
for (int j = 0; j < id[i].Count; j++)
{
int k = id[i][j];
X.Add(new List <double> {
gx[k], gy[k]
});
P.Add(pwrDbm[k]);
}
double[] loc = solve.solve(ref X, ref P);
Pt3D pt = new Pt3D(loc[0], loc[1], 0);
if (inBound(ref pt))
{
StrongResult.Add(pt);
}
}
}
bool inBound(ref Pt3D pt)
{
if (pt.x < rightBound && pt.y < upBound && pt.x > leftBound && pt.y > downBound)
{
return(true);
}
return(false);
}
// 根据强弱相间点对栅格打分
private Result strongWeakLoc()
{
Pt.Sort(new StrongWeakPtCmp());
// 加载加速结构
RayTracing inter = new RayTracing();
inter.SinglePrepare(leftBound, rightBound, downBound, upBound);
// 从每个点发出360*360条射线，经过的格子分值加1，分数最高的格子为可能位置
Dictionary <string, int> dic = new Dictionary <string, int>();
for (int i = 0; i < Pt.Count && i < 20; i++)
{
for (int j = 0; j < 360; j++)
{
for (int h = 3; h < 100; h += 3)
{
double angle = (double)(j) / 180.0 * Math.PI;
LTE.Geometric.Point endPoint = new Geometric.Point(Pt[i].strong.X + Math.Cos(angle), Pt[i].strong.Y + Math.Sin(angle), (double)h);
LTE.Geometric.Vector3D dir = LTE.Geometric.Vector3D.constructVector(Pt[i].strong, endPoint);
inter.SingleRayJudge(Pt[i].strong, dir, ref dic, true);
LTE.Geometric.Point endPoint1 = new Geometric.Point(Pt[i].weak.X + Math.Cos(angle), Pt[i].weak.Y + Math.Sin(angle), (double)h);
LTE.Geometric.Vector3D dir1 = LTE.Geometric.Vector3D.constructVector(Pt[i].weak, endPoint1);
inter.SingleRayJudge(Pt[i].weak, dir1, ref dic, false);
}
}
}
if (dic.Count == 0)
{
return(new Result(false, "强弱点对未推出合理结果"));
}
// 挑选分数最高的格子
var maxKey = (from d in dic orderby d.Value descending select d.Key).First();
var allMaxKey = (from d in dic where d.Value == dic[maxKey] select d.Key).ToArray();
int cnt = 0;
double X = 0, Y = 0, Z = 0;
if (allMaxKey.Count() > 5)
{
foreach (string a in allMaxKey) // 有多个分数最高的格子
{
string[] num = a.Split(',');
GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), Convert.ToInt32(num[2]), ref X, ref Y, ref Z);
Pt3D pt = new Pt3D(X, Y, Z);
if (inBound(ref pt))
{
StrongWeakResult.Add(pt);
}
}
}
else
{
var dicSort = from objDic in dic orderby objDic.Value descending select objDic;
foreach (KeyValuePair <string, int> kvp in dicSort)
{
string[] num = kvp.Key.Split(',');
GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z);
++cnt;
Pt3D pt = new Pt3D(X, Y, Z);
if (inBound(ref pt))
{
StrongWeakResult.Add(pt);
}
if (cnt > 5)
{
break;
}
}
}
return(new Result(true));
}
// 根据单调性对栅格打分
private Result monotoneLoc()
{
int agMaxX = 0, agMaxY = 0, agMinX = 0, agMinY = 0, agZ = 0;
GridHelper.getInstance().XYZToAccGrid(leftBound, downBound, 0, ref agMinX, ref agMinY, ref agZ);
GridHelper.getInstance().XYZToAccGrid(rightBound, upBound, 0, ref agMaxX, ref agMaxY, ref agZ);
Dictionary <string, int> dicMon = new Dictionary <string, int>();
Regress regress1 = new Regress();
for (int div = 0; div < roadDivide.Count - 1; div++)
{
List <double> pwrDbmSub = new List <double>();
List <double> x = new List <double>();
x.Add(0);
for (int j = roadDivide[div]; j < roadDivide[div + 1]; j++)
{
pwrDbmSub.Add(pwrDbm[j]);
x.Add(j - roadDivide[div] + 1);
}
#region 提取信号强度突变点
Divide divide = new Divide(100, 0.8, ref pwrDbmSub);
divide.run();
#endregion
#region 对每段进行线性拟合
for (int i = 0; i < divide.posV.Count - 1; i++)
{
int id1 = divide.posV[i];
int id2 = divide.posV[i + 1] - 1;
if (id2 < 0)
{
id2 = 0;
}
double a2 = 0, b2 = 0, err2 = 0;
regress1.CalcRegress(ref x, divide.m, id1, id2, out a2, out b2, out err2); // y = ax + b
double x1, y1, x2, y2; // (x1, y1) 单调上升的尽头
if (a2 < 0)
{
x1 = gx[id1 + roadDivide[div]];
y1 = gy[id1 + roadDivide[div]];
x2 = gx[id2 + roadDivide[div]];
y2 = gy[id2 + roadDivide[div]];
}
else
{
x1 = gx[id2 + roadDivide[div]];
y1 = gy[id2 + roadDivide[div]];
x2 = gx[id1 + roadDivide[div]];
y2 = gy[id1 + roadDivide[div]];
}
double k = (x1 - x2) / (y2 - y1);
double b = y1 - (x1 - x2) / (y2 - y1) * x1;
double y2Tmp = k * x2 + b;
if (y2 < y2Tmp)
{
for (int tx = agMinX + 1; tx < agMaxX - 1; tx++)
{
for (int ty = agMinY + 1; ty < agMaxY - 1; ty++)
{
double cx = 0, cy = 0, cz = 0;
GridHelper.getInstance().AccGridToXYZ(tx, ty, 0, ref cx, ref cy, ref cz);
if (cy > k * cx + b)
{
string key = string.Format("{0},{1}", tx, ty);
if (dicMon.Keys.Contains(key))
{
dicMon[key]++;
}
else
{
dicMon[key] = 0;
}
}
}
}
}
else
{
for (int tx = agMinX + 1; tx < agMaxX - 1; tx++)
{
for (int ty = agMinY + 1; ty < agMaxY - 1; ty++)
{
double cx = 0, cy = 0, cz = 0;
GridHelper.getInstance().AccGridToXYZ(tx, ty, 0, ref cx, ref cy, ref cz);
if (cy < k * cx + b)
{
string key = string.Format("{0},{1}", tx, ty);
if (dicMon.Keys.Contains(key))
{
dicMon[key]++;
}
else
{
dicMon[key] = 0;
}
}
}
}
}
}
#endregion
}
// 挑选分数最高的格子
if (dicMon.Count == 0)
{
return(new Result(false, "单调性未推出合理结果"));
}
var maxKeyMon = (from d in dicMon orderby d.Value descending select d.Key).First();
var allMaxKeyMon = (from d in dicMon where d.Value == dicMon[maxKeyMon] select d.Key).ToArray();
double X = 0, Y = 0, Z = 0;
int cnt = 0;
if (allMaxKeyMon.Count() > 5)
{
foreach (string a in allMaxKeyMon) // 有多个分数最高的格子
{
string[] num = a.Split(',');
GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z);
Pt3D pt = new Pt3D(X, Y, 0);
if (inBound(ref pt))
{
MonotoneResult.Add(pt);
}
}
}
else
{
var dicSort = from objDic in dicMon orderby objDic.Value descending select objDic;
foreach (KeyValuePair <string, int> kvp in dicSort)
{
string[] num = kvp.Key.Split(',');
GridHelper.getInstance().AccGridToXYZ(Convert.ToInt32(num[0]), Convert.ToInt32(num[1]), 0, ref X, ref Y, ref Z);
++cnt;
MonotoneResult.Add(new Pt3D(X, Y, 0));
if (cnt > 5)
{
break;
}
}
}
return(new Result(true));
}
