public node(int value)
{
data = value;
next = null;
}
/// <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();
}
}
}
