public HDMapReader(UTM utm, PointQ gpsCoordinate_previous) { //Reading HDMap.bin binary file from MapDatabase _mapDatabase = MapDatabase.LoadFromFile("HDMap.bin"); //Extract Road Surfaces form MapDatabase System.Collections.ObjectModel.ReadOnlyCollection <RoadSurface> rsurf = new System.Collections.ObjectModel.ReadOnlyCollection <RoadSurface>(_mapDatabase.GetRoadSurfaces()); //Initialize Pedestrian GPS coordinates of type PoinQ from Recorded Stream PointQ gpsCoordinates = new PointQ { X = utm.X, Y = utm.Y }; Console.WriteLine(); foreach (RoadSurface rs in rsurf) { //Initialize Polygon List List <PointQ> polygon = new List <PointQ>(); //Initialize a list of type PointQ with all possible end points/coordinates List <PointQ> allEndPoints = new List <PointQ>(); //Extract Accessors from Road Surfaces System.Collections.ObjectModel.ReadOnlyCollection <Accessor> accessors = new System.Collections.ObjectModel.ReadOnlyCollection <Accessor>(rs.GetAccessors()); foreach (Accessor a in accessors) { //Console.WriteLine("Id: " + a.Id.ToString()); // Console.WriteLine("LeftRoadSurface: " + a.LeftRoadSurface.ToString()); // Console.WriteLine("Line: " + a.Line.ToString()); // Console.WriteLine("Point A:" + a.Line.Begin.ToString()); // Console.WriteLine("Point B:" + a.Line.End.ToString()); // Console.WriteLine("RightRoadSurface: " + a.RightRoadSurface); // Console.WriteLine("StartingConnections: " + a.StartingConnections.ToString()); //Creat Road surface points using Accessors as boundary points PointQ strt = new PointQ(); strt.X = a.Line.Begin.X; strt.Y = a.Line.Begin.Y; PointQ finish = new PointQ(); finish.X = a.Line.End.X; finish.Y = a.Line.End.Y; polygon.Add(strt); polygon.Add(finish); //Extract Lanes Types System.Collections.Generic.IList <Lane> lane = new List <Lane>(rs.GetLanes()); foreach (Lane l in lane) { } } //Extract Borders System.Collections.Generic.IList <Border> border = new List <Border>(rs.GetBorders()); System.Collections.Generic.IList <IParametricCurve> outline = new List <IParametricCurve>(rs.GetOutline()); //Extract Connections System.Collections.Generic.IList <TUC.Data.HDMap.Connection> connections = new List <TUC.Data.HDMap.Connection>(rs.GetConnections()); foreach (Connection c in connections) { // Console.WriteLine("Connection 1 Begin Point: " + ((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).Begin.ToString()); // Console.WriteLine("Connection 1 End Point: " + ((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).End.ToString()); //Inialize Connection Points PointQ begin = new PointQ(); begin.X = ((QuinticHermiteCurve)c.CenterCurve).Begin.X; begin.Y = ((QuinticHermiteCurve)c.CenterCurve).Begin.Y; PointQ end = new PointQ(); end.X = ((QuinticHermiteCurve)c.CenterCurve).End.X; end.Y = ((QuinticHermiteCurve)c.CenterCurve).End.Y; //List<PointQ> allBeginPoints = new List<PointQ>(); // List<PointQ> allEndPoints = new List<PointQ>(); allEndPoints.Add(begin); allEndPoints.Add(end); // Console.WriteLine("All Points:" + allEndPoints.ToString()); //allEndPoints.Add(Connection.endpoint); // add all connection end points like this //AlgorithProbabailty(gpsCoordinates, allPoints); // allEndPoints.Add((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).Begin; // allEndPoints.Add((QuinticHermiteCurve)c.CenterCurve).Begin; // allEndPoints.Add((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve). } // bool x = IsPointInPolygon(polygon, new PointQ { X = utm.X, Y = utm.Y }); if (x == true) { Console.WriteLine("Pedestriian is Inside of Surface ID: " + rs.Id.ToString()); Console.ReadLine(); predictTrajectory(gpsCoordinates, allEndPoints, gpsCoordinate_previous); } else { Console.WriteLine("Pedestrian is outside of surface ID: " + rs.Id.ToString()); Console.ReadLine(); } // TrajectoryPredictor tp = new TrajectoryPredictor(); // tp.predictTrajectory(gpsCoordinates, allEndPoints); // tp.gpsCoordinate_previous = gpsCoordinates; } }