refactor merging of segregated roads

adjust to generalFindMaximum function
moved parallel detection to ratio/absolute based regression testing
considerably improved detection quality using normalised regression lines
only follow initial direction/narrow turns for parallel detection

include/util/bearing.hpp

@@ -91,9 +91,8 @@ inline bool CheckInBounds(const int A, const int B, const int range)
         return normalized_B - range <= normalized_A && normalized_A <= normalized_B + range;
     }
 }
-} // namespace bearing
 
-inline double reverseBearing(const double bearing)
+inline double reverse(const double bearing)
 {
     if (bearing >= 180)
         return bearing - 180.;
@@ -119,8 +118,9 @@ inline double reverseBearing(const double bearing)
 // % 360;
 // All other cases are handled by first rotating both bearings to an
 // entry_bearing of 0.
-inline double angleBetweenBearings(const double entry_bearing, const double exit_bearing)
+inline double angleBetween(const double entry_bearing, const double exit_bearing)
 {
+    // transform bearing from cw into ccw order
     const double offset = 360 - entry_bearing;
     const double rotated_exit = [](double bearing, const double offset) {
         bearing += offset;
@@ -131,13 +131,39 @@ inline double angleBetweenBearings(const double entry_bearing, const double exit
     return angle >= 360 ? angle - 360 : angle;
 }
 
-// minimal difference between two angles/bearings going left or right
-inline double angularDeviation(const double angle, const double from)
+} // namespace bearing
+
+// compute the minimum distance in degree between two angles/bearings
+inline double angularDeviation(const double angle_or_bearing, const double from)
 {
-    const double deviation = std::abs(angle - from);
+    const double deviation = std::abs(angle_or_bearing - from);
     return std::min(360 - deviation, deviation);
 }
 
+/* Angles in OSRM are expressed in the range of [0,360). During calculations, we might violate
+ * this range via offsets. This function helps to ensure the range is kept. */
+inline double restrictAngleToValidRange(const double angle)
+{
+    if (angle < 0)
+        return restrictAngleToValidRange(angle + 360.);
+    else if (angle > 360)
+        return restrictAngleToValidRange(angle - 360.);
+    else
+        return angle;
+}
+
+// finds the angle between two angles, based on the minum difference between the two
+inline double angleBetween(const double lhs, const double rhs)
+{
+    const auto difference = std::abs(lhs - rhs);
+    const auto is_clockwise_difference = difference <= 180;
+    const auto angle_between_candidate = .5 * (lhs + rhs);
+    if (is_clockwise_difference)
+        return angle_between_candidate;
+    else
+        return restrictAngleToValidRange(angle_between_candidate + 180);
+}
+
 } // namespace util
 } // namespace osrm
 

include/util/coordinate_calculation.hpp

@@ -3,9 +3,12 @@
 
 #include "util/coordinate.hpp"
 
+#include <boost/math/constants/constants.hpp>
 #include <boost/optional.hpp>
 
 #include <algorithm>
+#include <cmath>
+#include <numeric>
 #include <utility>
 #include <vector>
 
@@ -23,6 +26,18 @@ const constexpr long double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
 // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
 // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
 const constexpr long double EARTH_RADIUS = 6372797.560856;
+
+inline double degToRad(const double degree)
+{
+    using namespace boost::math::constants;
+    return degree * (pi<double>() / 180.0);
+}
+
+inline double radToDeg(const double radian)
+{
+    using namespace boost::math::constants;
+    return radian * (180.0 * (1. / pi<double>()));
+}
 }
 
 //! Takes the squared euclidean distance of the input coordinates. Does not return meters!
@@ -33,22 +48,8 @@ double haversineDistance(const Coordinate first_coordinate, const Coordinate sec
 double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
 
 // get the length of a full coordinate vector, using one of our basic functions to compute distances
-template <class BinaryOperation>
-double getLength(const std::vector<Coordinate> &coordinates, BinaryOperation op)
-{
-    if (coordinates.empty())
-        return 0.;
-
-    double result = 0;
-    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
-        result += op(lhs, rhs);
-        return false;
-    };
-    // side-effect find adding up distances
-    std::adjacent_find(coordinates.begin(), coordinates.end(), functor);
-
-    return result;
-}
+template <class BinaryOperation, typename iterator_type>
+double getLength(iterator_type begin, const iterator_type end, BinaryOperation op);
 
 // Find the closest distance and location between coordinate and the line connecting source and
 // target:
@@ -59,41 +60,35 @@ double getLength(const std::vector<Coordinate> &coordinates, BinaryOperation op)
 // returns x as well as the distance between source and x as ratio ([0,1])
 inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                 const FloatCoordinate &target,
-                                                                const FloatCoordinate &coordinate)
-{
-    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
-    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
-    // dot product of two un-normed vectors
-    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
-                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
-    // squared length of the slope vector
-    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
-                                static_cast<double>(slope_vector.lat * slope_vector.lat);
+                                                                const FloatCoordinate &coordinate);
 
-    if (squared_length < std::numeric_limits<double>::epsilon())
-    {
-        return {0, source};
-    }
+// find the closest distance between a coordinate and a segment
+// O(1)
+double findClosestDistance(const Coordinate coordinate,
+                           const Coordinate segment_begin,
+                           const Coordinate segment_end);
 
-    const double normed_ratio = unnormed_ratio / squared_length;
-    double clamped_ratio = normed_ratio;
-    if (clamped_ratio > 1.)
-    {
-        clamped_ratio = 1.;
-    }
-    else if (clamped_ratio < 0.)
-    {
-        clamped_ratio = 0.;
-    }
+// find the closest distance between a coordinate and a set of coordinates
+// O(|coordinates|)
+template <typename iterator_type>
+double findClosestDistance(const Coordinate coordinate,
+                           const iterator_type begin,
+                           const iterator_type end);
 
-    return {clamped_ratio,
-            {
-                FloatLongitude{1.0 - clamped_ratio} * source.lon +
-                    target.lon * FloatLongitude{clamped_ratio},
-                FloatLatitude{1.0 - clamped_ratio} * source.lat +
-                    target.lat * FloatLatitude{clamped_ratio},
-            }};
-}
+// find the closes distance between two sets of coordinates
+// O(|lhs| * |rhs|)
+template <typename iterator_type>
+double findClosestDistance(const iterator_type lhs_begin,
+                           const iterator_type lhs_end,
+                           const iterator_type rhs_begin,
+                           const iterator_type rhs_end);
+
+// checks if two sets of coordinates describe a parallel set of ways
+template <typename iterator_type>
+bool areParallel(const iterator_type lhs_begin,
+                 const iterator_type lhs_end,
+                 const iterator_type rhs_begin,
+                 const iterator_type rhs_end);
 
 double perpendicularDistance(const Coordinate segment_source,
                              const Coordinate segment_target,
@@ -136,8 +131,252 @@ bool isCCW(const Coordinate first_coordinate,
            const Coordinate second_coordinate,
            const Coordinate third_coordinate);
 
-std::pair<util::Coordinate, util::Coordinate>
-leastSquareRegression(const std::vector<util::Coordinate> &coordinates);
+template <typename iterator_type>
+std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
+                                                        const iterator_type end);
+
+// rotates a coordinate around the point (0,0). This function can be used to normalise a few
+// computations around regression vectors
+Coordinate rotateCCWAroundZero(Coordinate coordinate, double angle_in_radians);
+
+// compute the difference vector of two coordinates lhs - rhs
+Coordinate difference(const Coordinate lhs, const Coordinate rhs);
+
+// TEMPLATE/INLINE DEFINITIONS
+inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
+                                                                const FloatCoordinate &target,
+                                                                const FloatCoordinate &coordinate)
+{
+    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
+    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
+    // dot product of two un-normed vectors
+    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
+                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
+    // squared length of the slope vector
+    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
+                                static_cast<double>(slope_vector.lat * slope_vector.lat);
+
+    if (squared_length < std::numeric_limits<double>::epsilon())
+    {
+        return {0, source};
+    }
+
+    const double normed_ratio = unnormed_ratio / squared_length;
+    double clamped_ratio = normed_ratio;
+    if (clamped_ratio > 1.)
+    {
+        clamped_ratio = 1.;
+    }
+    else if (clamped_ratio < 0.)
+    {
+        clamped_ratio = 0.;
+    }
+
+    return {clamped_ratio,
+            {
+                FloatLongitude{1.0 - clamped_ratio} * source.lon +
+                    target.lon * FloatLongitude{clamped_ratio},
+                FloatLatitude{1.0 - clamped_ratio} * source.lat +
+                    target.lat * FloatLatitude{clamped_ratio},
+            }};
+}
+
+template <class BinaryOperation, typename iterator_type>
+double getLength(iterator_type begin, const iterator_type end, BinaryOperation op)
+{
+    double result = 0;
+    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
+        result += op(lhs, rhs);
+        return false;
+    };
+    // side-effect find adding up distances
+    std::adjacent_find(begin, end, functor);
+
+    return result;
+}
+
+template <typename iterator_type>
+double
+findClosestDistance(const Coordinate coordinate, const iterator_type begin, const iterator_type end)
+{
+    double current_min = std::numeric_limits<double>::max();
+
+    // comparator updating current_min without ever finding an element
+    const auto compute_minimum_distance = [&current_min, coordinate](const Coordinate lhs,
+                                                                     const Coordinate rhs) {
+        current_min = std::min(current_min, findClosestDistance(coordinate, lhs, rhs));
+        return false;
+    };
+
+    std::adjacent_find(begin, end, compute_minimum_distance);
+    return current_min;
+}
+
+template <typename iterator_type>
+double findClosestDistance(const iterator_type lhs_begin,
+                           const iterator_type lhs_end,
+                           const iterator_type rhs_begin,
+                           const iterator_type rhs_end)
+{
+    double current_min = std::numeric_limits<double>::max();
+
+    const auto compute_minimum_distance_in_rhs = [&current_min, rhs_begin, rhs_end](
+        const Coordinate coordinate) {
+        current_min = std::min(current_min, findClosestDistance(coordinate, rhs_begin, rhs_end));
+        return false;
+    };
+
+    std::find_if(lhs_begin, lhs_end, compute_minimum_distance_in_rhs);
+    return current_min;
+}
+
+template <typename iterator_type>
+std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
+                                                        const iterator_type end)
+{
+    // following the formulas of https://faculty.elgin.edu/dkernler/statistics/ch04/4-2.html
+    const auto number_of_coordinates = std::distance(begin, end);
+    BOOST_ASSERT(number_of_coordinates >= 2);
+    const auto extract_lon = [](const Coordinate coordinate) {
+        return static_cast<double>(toFloating(coordinate.lon));
+    };
+
+    const auto extract_lat = [](const Coordinate coordinate) {
+        return static_cast<double>(toFloating(coordinate.lat));
+    };
+
+    double min_lon = extract_lon(*begin);
+    double max_lon = extract_lon(*begin);
+    double min_lat = extract_lat(*begin);
+    double max_lat = extract_lat(*begin);
+
+    for (auto coordinate_iterator = begin; coordinate_iterator != end; ++coordinate_iterator)
+    {
+        const auto c = *coordinate_iterator;
+        const auto lon = extract_lon(c);
+        min_lon = std::min(min_lon, lon);
+        max_lon = std::max(max_lon, lon);
+        const auto lat = extract_lat(c);
+        min_lat = std::min(min_lat, lat);
+        max_lat = std::max(max_lat, lat);
+    }
+    // very small difference in longitude -> would result in inaccurate calculation, check if lat is
+    // better
+    if ((max_lat - min_lat) > 2 * (max_lon - min_lon))
+    {
+        std::vector<util::Coordinate> rotated_coordinates(number_of_coordinates);
+        // rotate all coordinates to the right
+        std::transform(begin, end, rotated_coordinates.begin(), [](const auto coordinate) {
+            return rotateCCWAroundZero(coordinate, detail::degToRad(-90));
+        });
+        const auto rotated_regression =
+            leastSquareRegression(rotated_coordinates.begin(), rotated_coordinates.end());
+        return {rotateCCWAroundZero(rotated_regression.first, detail::degToRad(90)),
+                rotateCCWAroundZero(rotated_regression.second, detail::degToRad(90))};
+    }
+
+    const auto make_accumulate = [](const auto extraction_function) {
+        return [extraction_function](const double sum_so_far, const Coordinate coordinate) {
+            return sum_so_far + extraction_function(coordinate);
+        };
+    };
+
+    const auto accumulated_lon = std::accumulate(begin, end, 0., make_accumulate(extract_lon));
+
+    const auto accumulated_lat = std::accumulate(begin, end, 0., make_accumulate(extract_lat));
+
+    const auto mean_lon = accumulated_lon / number_of_coordinates;
+    const auto mean_lat = accumulated_lat / number_of_coordinates;
+    const auto make_variance = [](const auto mean, const auto extraction_function) {
+        return [extraction_function, mean](const double sum_so_far, const Coordinate coordinate) {
+            const auto difference = extraction_function(coordinate) - mean;
+            return sum_so_far + difference * difference;
+        };
+    };
+
+    // using the unbiased version, we divide by num_samples - 1 (see
+    // http://mathworld.wolfram.com/SampleVariance.html)
+    const auto sample_variance_lon =
+        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lon, extract_lon)) /
+                  (number_of_coordinates - 1));
+
+    // if we don't change longitude, return the vertical line as is
+    if (std::abs(sample_variance_lon) <
+        std::numeric_limits<decltype(sample_variance_lon)>::epsilon())
+        return {*begin, *(end - 1)};
+
+    const auto sample_variance_lat =
+        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lat, extract_lat)) /
+                  (number_of_coordinates - 1));
+
+    if (std::abs(sample_variance_lat) <
+        std::numeric_limits<decltype(sample_variance_lat)>::epsilon())
+        return {*begin, *(end - 1)};
+    const auto linear_correlation =
+        std::accumulate(begin,
+                        end,
+                        0.,
+                        [&](const auto sum_so_far, const auto current_coordinate) {
+                            return sum_so_far +
+                                   (extract_lon(current_coordinate) - mean_lon) *
+                                       (extract_lat(current_coordinate) - mean_lat) /
+                                       (sample_variance_lon * sample_variance_lat);
+                        }) /
+        (number_of_coordinates - 1);
+
+    const auto slope = linear_correlation * sample_variance_lat / sample_variance_lon;
+    const auto intercept = mean_lat - slope * mean_lon;
+
+    const auto GetLatAtLon = [intercept,
+                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
+        return {intercept + slope * static_cast<double>((longitude))};
+    };
+
+    const double offset = 0.00001;
+    const Coordinate regression_first = {
+        toFixed(util::FloatLongitude{min_lon - offset}),
+        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - offset})))};
+    const Coordinate regression_end = {
+        toFixed(util::FloatLongitude{max_lon + offset}),
+        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + offset})))};
+
+    return {regression_first, regression_end};
+}
+
+template <typename iterator_type>
+bool areParallel(const iterator_type lhs_begin,
+                 const iterator_type lhs_end,
+                 const iterator_type rhs_begin,
+                 const iterator_type rhs_end)
+{
+    const auto regression_lhs = leastSquareRegression(lhs_begin, lhs_end);
+    const auto regression_rhs = leastSquareRegression(rhs_begin, rhs_end);
+
+    const auto null_island = Coordinate(FixedLongitude{0}, FixedLatitude{0});
+    const auto difference_lhs = difference(regression_lhs.first, regression_lhs.second);
+    const auto difference_rhs = difference(regression_rhs.first, regression_rhs.second);
+
+    // we normalise the left slope to be zero, so we rotate the coordinates around 0,0 to match 90
+    // degrees
+    const auto bearing_lhs = bearing(null_island, difference_lhs);
+
+    // we rotate to have one of the lines facing horizontally to the right (bearing 90 degree)
+    const auto rotation_angle_radians = detail::degToRad(bearing_lhs - 90);
+    const auto rotated_difference_rhs = rotateCCWAroundZero(difference_rhs, rotation_angle_radians);
+
+    const auto get_slope = [](const Coordinate from, const Coordinate to) {
+        const auto diff_lat = static_cast<int>(from.lat) - static_cast<int>(to.lat);
+        const auto diff_lon = static_cast<int>(from.lon) - static_cast<int>(to.lon);
+        if (diff_lon == 0)
+            return std::numeric_limits<double>::max();
+        return static_cast<double>(diff_lat) / static_cast<double>(diff_lon);
+    };
+
+    const auto slope_rhs = get_slope(null_island, rotated_difference_rhs);
+    // the left hand side has a slope of `0` after the rotation. We can check the slope of the right
+    // hand side to ensure we only considering slight slopes
+    return std::abs(slope_rhs) < 0.20; // twenty percent incline at the most
+}
 
 } // ns coordinate_calculation
 } // ns util

include/util/guidance/name_announcements.hpp

@@ -22,7 +22,6 @@ namespace util
 {
 namespace guidance
 {
-
 // Name Change Logic
 // Used both during Extraction as well as during Post-Processing
 
@@ -157,25 +156,31 @@ inline bool requiresNameAnnounced(const NameID from_name_id,
                                   const util::NameTable &name_table,
                                   const extractor::SuffixTable &suffix_table)
 {
-    return requiresNameAnnounced(name_table.GetNameForID(from_name_id),
-                                 name_table.GetRefForID(from_name_id),
-                                 name_table.GetPronunciationForID(from_name_id),
-                                 name_table.GetNameForID(to_name_id),
-                                 name_table.GetRefForID(to_name_id),
-                                 name_table.GetPronunciationForID(to_name_id),
-                                 suffix_table);
+    if (from_name_id == to_name_id)
+        return false;
+    else
+        return requiresNameAnnounced(name_table.GetNameForID(from_name_id),
+                                     name_table.GetRefForID(from_name_id),
+                                     name_table.GetPronunciationForID(from_name_id),
+                                     name_table.GetNameForID(to_name_id),
+                                     name_table.GetRefForID(to_name_id),
+                                     name_table.GetPronunciationForID(to_name_id),
+                                     suffix_table);
 }
 
 inline bool requiresNameAnnounced(const NameID from_name_id,
                                   const NameID to_name_id,
                                   const util::NameTable &name_table)
 {
-    return requiresNameAnnounced(name_table.GetNameForID(from_name_id),
-                                 name_table.GetRefForID(from_name_id),
-                                 name_table.GetPronunciationForID(from_name_id),
-                                 name_table.GetNameForID(to_name_id),
-                                 name_table.GetRefForID(to_name_id),
-                                 name_table.GetPronunciationForID(to_name_id));
+    if (from_name_id == to_name_id)
+        return false;
+    else
+        return requiresNameAnnounced(name_table.GetNameForID(from_name_id),
+                                     name_table.GetRefForID(from_name_id),
+                                     name_table.GetPronunciationForID(from_name_id),
+                                     name_table.GetNameForID(to_name_id),
+                                     name_table.GetRefForID(to_name_id),
+                                     name_table.GetPronunciationForID(to_name_id));
 }
 
 } // namespace guidance