public node(int value)
 {
     data = value;
     next = null;
 }
Beispiel #2
0
        /// <summary>
        /// find if point is elligable to be a junction point based on the area of the triangle
        /// formed by threee consecutive points
        /// </summary>
        /// <param name="node1"> </param>
        /// <param name="node2"></param>
        /// <param name="node3"></param>
        /// <returns></returns>
        public void SegmentOsmRoads(Dictionary <way, List <node> > wayNodesDict)
        {
            IList <double> filteredValues = new List <double>();
            OpenDRIVE      od             = new OpenDRIVE();
            int            p = wayNodesDict.Count();

            od.road = new OpenDRIVERoad[p];
            int c = 0;

            foreach (KeyValuePair <way, List <node> > road in wayNodesDict)
            {
                // segmentation works on a set of points, which are 3 or more than 3 consecutive points, if otherwise found
                // it will be considered as line.
                if (road.Value.Count > 3 && road.Key.tag.Find(highWayTag => highWayTag.k == "highway") != null)
                {
                    node     nextNode                 = GetNext <node>(road.Value, road.Value[0]);
                    var      headingsAndDistances     = ApplyLSG(road.Value);
                    double[] curvatureVector          = new double[road.Value.Count];
                    double[] curvatureVector_filtered = new double[road.Value.Count];
                    double[] distanceVector           = new double[road.Value.Count];
                    double[] headingVector            = new double[road.Value.Count];
                    double[] emaValues                = new double[road.Value.Count];
                    double   numberOfnodes            = road.Value.Count;


                    //od.road[c] = odRoad;


                    for (int x = 0; x < headingsAndDistances.Count(); x++)
                    {
                        //TODO: replace distances with headings, it is reversed.
                        curvatureVector[x] = (headingsAndDistances[x].distance);
                        Console.WriteLine("Radius of curvature for point {0} is {1}", x, curvatureVector[x]);
                    }
                    //curvature/ distance curve will suffer from some data inhereted inaccuricies
                    //we need to apply smoothing filter to enhance the curve accuracy. Exponential
                    //moving avarage filter will be used. [SMA : Simple moving avarage for first point only]
                    //[EMA: Exponential moving avarage], filter periods will be 2.

                    //EMA Multiplier
                    double k = 2 / curvatureVector.Count();
                    for (int i = 0; i < curvatureVector.Count(); i++)
                    {
                        distanceVector[i] = headingsAndDistances[i].heading;
                    }
                    //filter data, exponential moving average filter
                    var paramterHandler = new getParamter();
                    paramterHandler.setAlpha(0.5);
                    double alpha         = paramterHandler.getAlpha();
                    int    curvatureIndx = 0;
                    foreach (double curvatureVal in curvatureVector)
                    {
                        if (curvatureIndx == 0)
                        {
                            curvatureVector_filtered[0] = curvatureVal;
                        }
                        else
                        {
                            curvatureVector_filtered[curvatureIndx] = alpha * curvatureVector[curvatureIndx] + (1 - alpha) * curvatureVector_filtered[curvatureIndx - 1];
                        }
                        curvatureIndx++;
                    }

                    SegmentationHelper segmentationHandler = new SegmentationHelper(distanceVector, curvatureVector_filtered);
                    Tuple <List <Osm2Od.Point>, List <int> > junctionPoints = SegmentationHelper.DouglasPeuckerReduction(segmentationHandler.curvaturDistanceDomain, Math.Pow(.007, 1));
                    double[] junction_Points_x = new double[junctionPoints.Item2.Count()];
                    double[] junction_Points_y = new double[junctionPoints.Item2.Count()];
                    for (int i = 0; i < junctionPoints.Item1.Count(); i++)
                    {
                        junction_Points_y[i] = junctionPoints.Item1[i].Y;
                        junction_Points_x[i] = junctionPoints.Item1[i].X;
                    }
                    Console.WriteLine("Road {0} junction points calculated, {1} found", road.Key.id, junctionPoints.Item1.Count());
                    //segment the sections according to line deviataion, if line stills withing our given tolerance, with curvature value
                    //==0 or near zero, it will be considered a line, if curvature!=0, it is a curve, if != constant value it is a spiral
                    List <double> slopes = getRoadPrimativeGeometries(junction_Points_x, junction_Points_y);
                    OpenDRIVERoad odRoad = convertRoadsFromOsmToOd(road, distanceVector, curvatureVector_filtered, junction_Points_x, junction_Points_y);
                    odRoad.planView = new OpenDRIVERoadGeometry[slopes.Count()];
                    //create the geometry attribute of the road
                    for (int i = 0; i < slopes.Count(); i++)
                    {
                        OpenDRIVERoadGeometry roadGeometry = new OpenDRIVERoadGeometry();
                        roadGeometry.Items  = new object[1];
                        roadGeometry.s      = distanceVector[junctionPoints.Item2[i]];
                        roadGeometry.x      = MercatorProjection.lonToX(road.Value[junctionPoints.Item2[i]].lon) - MercatorProjection.lonToX(minLon);
                        roadGeometry.y      = MercatorProjection.latToY(road.Value[junctionPoints.Item2[i]].lat) - MercatorProjection.latToY(minLat);
                        roadGeometry.hdg    = headingVector[junctionPoints.Item2[i]];
                        roadGeometry.length = distanceVector[junctionPoints.Item2[i + 1]];

                        //TODO: SET CURVATURE TOLERANCE HER
                        paramterHandler.setCurvatureTol(.00018);
                        double curvatureTol = paramterHandler.getcurvatureTol();
                        if (Math.Abs(slopes[i]) <= curvatureTol)
                        {
                            if (Math.Abs((curvatureVector_filtered[junctionPoints.Item2[i + 1]] + curvatureVector_filtered[junctionPoints.Item2[i]]) / 2) <= .005)
                            {
                                OpenDRIVERoadGeometryLine line = new OpenDRIVERoadGeometryLine();
                                roadGeometry.Items[0] = line;
                            }
                            else
                            {
                                OpenDRIVERoadGeometryArc arc = new OpenDRIVERoadGeometryArc();
                                arc.curvature         = curvatureVector_filtered[junctionPoints.Item2[i]];
                                roadGeometry.Items[0] = arc;
                            }
                        }
                        if (Math.Abs(slopes[i]) > curvatureTol)
                        {
                            OpenDRIVERoadGeometrySpiral spiral = new OpenDRIVERoadGeometrySpiral();
                            spiral.curvStart      = curvatureVector_filtered[junctionPoints.Item2[i]];
                            spiral.curvEnd        = curvatureVector_filtered[junctionPoints.Item2[i + 1]];
                            roadGeometry.Items[0] = spiral;
                        }
                        odRoad.planView[i] = roadGeometry;
                    }
                    od.road[c] = odRoad;
                    CurvatureDistanceChart curvatureDistanceChart = new CurvatureDistanceChart(curvatureVector, curvatureVector_filtered, distanceVector, junction_Points_x, junction_Points_y);
                }
                else
                {
                    Console.WriteLine("[Segmentation] object {0} is a simple line or has no highway tag", road.Key.id);
                }
                Console.WriteLine("Heading/distance calculated, transforming to curvature/distance domain");
                c++;
            }
            //deserilize to OpenDrive.xodr.
            XmlSerializer odDeserilizer = new XmlSerializer(typeof(OpenDRIVE));
            var           xml           = "";

            using (var sww = new System.IO.StringWriter())
            {
                using (System.Xml.XmlWriter writer = System.Xml.XmlWriter.Create(sww))
                {
                    odDeserilizer.Serialize(writer, od);
                    xml = sww.ToString();
                }
            }
        }