private double MeasurementProbability(Point p, Edge e, double miu_o, double sigma_o, double miu_b, double sigma_b) { //observation probability double p_lng, p_lat; p_lng = p.X; p_lat = p.Y; Point p_prj = e.projectFrom(p); double p_prj_lng, p_prj_lat; p_prj_lng = p_prj.X; p_prj_lat = p_prj.Y; double great_circle_dis = p.DistanceFrom(p_prj); double measurementProb1, measurementProb2 = 0; double var1 = 1.0 / (Math.Sqrt(2 * Math.PI) * sigma_o); double var2 = (great_circle_dis - miu_o) / sigma_o; //direction analysis //double theta = Geometry.getAngle(e); double derta = Geometry.getAnglediff(p.Direction, e); double var11 = 1.0 / (Math.Sqrt(2 * Math.PI) * sigma_b); double var22 = (derta - miu_b) / sigma_b; measurementProb1 = var1 * Math.Exp(-0.5 * Math.Pow(var2, 2)); measurementProb2 = var11 * Math.Exp(-0.5 * Math.Pow(var22, 2)); //Console.WriteLine("{0},{1}",great_circle_dis,measurementProb); return(measurementProb1 * measurementProb2 * 100); }
public double getLength(Graph g, Point p1, Point p2) { path_edges = this.getPath_EdgeList(g); Edge e1 = path_edges[0]; Edge e11 = path_edges[1]; Edge e2 = path_edges[path_edges.Count - 2]; Edge e22 = path_edges[path_edges.Count - 1]; Point p1_prj = e1.projectFrom(p1); Point p2_prj = e22.projectFrom(p2); double length = RouteDistance(p1_prj, e1.getConnectedV(e11), p2_prj, e2.getConnectedV(e22), g, e1.ID == e2.ID); return(length); }
/// <summary> /// calculate route distance by search the shortest route between the from vertex of the two edges /// this method may be not accurate. /// <returns></returns> private Edgespp RouteDistance(Point p1, Edge e1, Point p2, Edge e2, Graph g) { double routeDis = 0; Point p1_prj = e1.projectFrom(p1); Vertex e1_to = e1.To; double dis1 = p1_prj.DistanceFrom(e1_to.toPoint()); Point p2_prj = e2.projectFrom(p2); Vertex e2_from = e2.From; double dis2 = p2_prj.DistanceFrom(e2_from.toPoint()); if (e1 == e2) { return(new Edgespp(p1_prj.DistanceFrom(p2_prj), new List <Edge>() { e1 })); } else if (e1_to.ID == e2_from.ID) { routeDis = dis1 + dis2; return(new Edgespp(routeDis, new List <Edge>() { e1, e2 })); } else { Dijkstra dij = new Dijkstra(g); LinkedList <Vertex> shortestPath_vertices = dij.ShortestPath_ScoreValue(e1_to, e2_from, _distance_limit); //Route rs = getminRoute(F, e1.ID, e2.ID, p1, p2); if (shortestPath_vertices == null) { return(null); } List <Edge> shortestPath_edges = dij.MakeShortestPath_EdgeList(); //List<Edge> shortestPath_edges = rs.getPath_EdgeList(m_graph); for (int i = 0; i < shortestPath_edges.Count; i++) { routeDis += shortestPath_edges[i].Length; } if (shortestPath_edges.Contains(e1)) { routeDis -= dis1; shortestPath_edges.Remove(e1); } else { routeDis += dis1; } if (shortestPath_edges.Contains(e2)) { routeDis -= dis2; shortestPath_edges.Remove(e2); } else { routeDis += dis2; } return(new Edgespp(routeDis, shortestPath_edges)); } }