Turn Angles in OSRM were computed using a lookahead of 10 meters.

This PR adds more advanced coordinate extraction, analysing the road
to detect offsets due to OSM way modelling.

In addition it improves the handling of bearings. Right now OSM reports
bearings simply based on the very first coordinate along a way.
With this PR, we store the bearings for a turn correctly, making the
bearings for turns correct.

include/extractor/guidance/constants.hpp

@@ -34,7 +34,7 @@ const int constexpr MAX_SLIPROAD_THRESHOLD = 250;
 // Road priorities give an idea of how obvious a turn is. If two priorities differ greatly (e.g.
 // service road over a primary road, the better priority can be seen as obvious due to its road
 // category).
-const double constexpr PRIORITY_DISTINCTION_FACTOR = 2.0;
+const double constexpr PRIORITY_DISTINCTION_FACTOR = 1.75;
 
 } // namespace guidance
 } // namespace extractor

include/extractor/guidance/coordinate_extractor.hpp

@@ -0,0 +1,203 @@
+#ifndef OSRM_EXTRACTOR_COORDINATE_EXTRACTOR_HPP_
+#define OSRM_EXTRACTOR_COORDINATE_EXTRACTOR_HPP_
+
+#include <vector>
+
+#include "extractor/query_node.hpp"
+#include "util/coordinate.hpp"
+#include "util/coordinate_calculation.hpp"
+
+#include "extractor/compressed_edge_container.hpp"
+#include "util/node_based_graph.hpp"
+
+namespace osrm
+{
+namespace extractor
+{
+namespace guidance
+{
+
+class CoordinateExtractor
+{
+  public:
+    CoordinateExtractor(const util::NodeBasedDynamicGraph &node_based_graph,
+                        const extractor::CompressedEdgeContainer &compressed_geometries,
+                        const std::vector<extractor::QueryNode> &node_coordinates);
+
+    /* Find a interpolated coordinate a long the compressed geometries. The desired coordinate
+     * should be in a certain distance. This method is dedicated to find representative coordinates
+     * at turns.
+     */
+    util::Coordinate GetCoordinateAlongRoad(const NodeID intersection_node,
+                                            const EdgeID turn_edge,
+                                            const bool traversed_in_reverse,
+                                            const NodeID to_node,
+                                            const std::uint8_t number_of_in_lanes) const;
+
+    // instead of finding only a single coordinate, we can also list all coordinates along a road.
+    std::vector<util::Coordinate> GetCoordinatesAlongRoad(const NodeID intersection_node,
+                                                          const EdgeID turn_edge,
+                                                          const bool traversed_in_reverse,
+                                                          const NodeID to_node) const;
+
+    /* When extracting the coordinates, we first extract all coordinates. We don't care about most
+     * of them, though.
+     *
+     * Our very first step trims the coordinates to a saller set, close to the intersection.. The
+     * idea here is to filter all coordinates at the end of the road and consider only the formi
+     * close to the intersection:
+     *
+     * a -------------- v ----------.
+     *                                 .
+     *                                   .
+     *                                   .
+     *                                   b
+     *
+     * For calculating the turn angle for the intersection at `a`, we do not care about the turn
+     * between `v` and `b`. This calculation trims the coordinates to the ones immediately at the
+     * intersection.
+     */
+    std::vector<util::Coordinate> TrimCoordinatesToLength(std::vector<util::Coordinate> coordinates,
+                                                          const double desired_length) const;
+
+    /* when looking at a set of coordinates, this function allows trimming the vector to a smaller,
+     * only containing coordinates up to a given distance along the path. The last coordinate might
+     * be interpolated
+     */
+    std::vector<util::Coordinate>
+    TrimCoordinatesByLengthFront(std::vector<util::Coordinate> coordinates,
+                                 const double desired_length) const;
+
+    /*
+     * to correct for the initial offset, we move the lookahead coordinate close
+     * to the original road. We do so by subtracting the difference between the
+     * turn coordinate and the offset coordinate from the lookahead coordinge:
+     *
+     * a ------ b ------ c
+     *          |
+     *          d
+     *             \
+     *                \
+     *                   e
+     *
+     * is converted to:
+     *
+     * a ------ b ------ c
+     *             \
+     *                \
+     *                   e
+     *
+     * for fixpoint `b`, vector_base `d` and vector_head `e`
+     */
+    util::Coordinate GetCorrectedCoordinate(const util::Coordinate fixpoint,
+                                            const util::Coordinate vector_base,
+                                            const util::Coordinate vector_head) const;
+
+    /* generate a uniform vector of coordinates in same range distances
+     *
+     * Turns:
+     * x ------------ x -- x - x
+     *
+     * Into:
+     * x -- x -- x -- x -- x - x
+     */
+    std::vector<util::Coordinate>
+    SampleCoordinates(const std::vector<util::Coordinate> &coordinates,
+                      const double length,
+                      const double rate) const;
+
+  private:
+    const util::NodeBasedDynamicGraph &node_based_graph;
+    const extractor::CompressedEdgeContainer &compressed_geometries;
+    const std::vector<extractor::QueryNode> &node_coordinates;
+
+    double ComputeInterpolationFactor(const double desired_distance,
+                                      const double distance_to_first,
+                                      const double distance_to_second) const;
+
+    std::pair<util::Coordinate, util::Coordinate>
+    RegressionLine(const std::vector<util::Coordinate> &coordinates) const;
+
+    /* In an ideal world, the road would only have two coordinates if it goes mainly straigt. Since
+     * OSM is operating on noisy data, we have some variations going straight.
+     *
+     *              b                       d
+     * a ---------------------------------------------- e
+     *                           c
+     *
+     * The road from a-e offers a lot of variation, even though it is mostly straight. Here we
+     * calculate the distances of all nodes in between to the straight line between a and e. If the
+     * distances inbetween are small, we assume a straight road. To calculate these distances, we
+     * don't use the coordinates of the road itself but our just calculated regression vector
+     */
+    double GetMaxDeviation(std::vector<util::Coordinate>::const_iterator range_begin,
+                           const std::vector<util::Coordinate>::const_iterator &range_end,
+                           const util::Coordinate straight_begin,
+                           const util::Coordinate straight_end) const;
+
+    /*
+     * the curve is still best described as looking at the very first vector for the turn angle.
+     * Consider:
+     *
+     * |
+     * a - 1
+     * |       o
+     * |         2
+     * |          o
+     * |           3
+     * |           o
+     * |           4
+     *
+     * The turn itself from a-1 would be considered as a 90 degree turn, even though the road is
+     * taking a turn later.
+     * In this situaiton we return the very first coordinate, describing the road just at the
+     * turn.
+     * As an added benefit, we get a straight turn at a curved road:
+     *
+     *            o   b   o
+     *      o                   o
+     *   o                         o
+     *  o                           o
+     *  o                           o
+     *  a                           c
+     *
+     * The turn from a-b to b-c is straight. With every vector we go further down the road, the
+     * turn
+     * angle would get stronger. Therefore we consider the very first coordinate as our best
+     * choice
+     */
+    bool IsCurve(const std::vector<util::Coordinate> &coordinates,
+                 const std::vector<double> &segment_distances,
+                 const double segment_length,
+                 const double considered_lane_width,
+                 const util::NodeBasedEdgeData &edge_data) const;
+
+    /*
+     * If the very first coordinate is within lane offsets and the rest offers a near straight line,
+     * we use an offset coordinate.
+     *
+     * ----------------------------------------
+     *
+     * ----------------------------------------
+     *   a -
+     * ----------------------------------------
+     *        \
+     * ----------------------------------------
+     *           \
+     *             b --------------------c
+     *
+     * Will be considered a very slight turn, instead of the near 90 degree turn we see right here.
+     */
+    bool IsDirectOffset(const std::vector<util::Coordinate> &coordinates,
+                        const std::size_t straight_index,
+                        const double straight_distance,
+                        const double segment_length,
+                        const std::vector<double> &segment_distances,
+                        const std::uint8_t considered_lanes) const;
+};
+
+} // namespace guidance
+} // namespace extractor
+} // namespace osrm
+
+#endif // OSRM_EXTRACTOR_COORDINATE_EXTRACTOR_HPP_

include/extractor/guidance/intersection.hpp

@@ -21,6 +21,7 @@ struct TurnOperation final
 {
     EdgeID eid;
     double angle;
+    double bearing;
     TurnInstruction instruction;
     LaneDataID lane_data_id;
 };

include/extractor/guidance/intersection_generator.hpp

@@ -2,6 +2,7 @@
 #define OSRM_EXTRACTOR_GUIDANCE_INTERSECTION_GENERATOR_HPP_
 
 #include "extractor/compressed_edge_container.hpp"
+#include "extractor/guidance/coordinate_extractor.hpp"
 #include "extractor/guidance/intersection.hpp"
 #include "extractor/query_node.hpp"
 #include "extractor/restriction_map.hpp"
@@ -51,7 +52,7 @@ class IntersectionGenerator
     const RestrictionMap &restriction_map;
     const std::unordered_set<NodeID> &barrier_nodes;
     const std::vector<QueryNode> &node_info_list;
-    const CompressedEdgeContainer &compressed_edge_container;
+    const CoordinateExtractor coordinate_extractor;
 
     // Check for restrictions/barriers and generate a list of valid and invalid turns present at
     // the

include/extractor/guidance/road_classification.hpp

@@ -59,21 +59,24 @@ class RoadClassification
     // (difference <=1), we can see the road as a fork. Else one of the road classes is seen as
     // obvious choice
     RoadPriorityClass::Enum road_priority_class : 5;
+    // the number of lanes in the road
+    std::uint8_t number_of_lanes;
 
   public:
     // default construction
     RoadClassification()
         : motorway_class(0), link_class(0), may_be_ignored(1),
-          road_priority_class(RoadPriorityClass::CONNECTIVITY)
+          road_priority_class(RoadPriorityClass::CONNECTIVITY), number_of_lanes(0)
     {
     }
 
     RoadClassification(bool motorway_class,
                        bool link_class,
                        bool may_be_ignored,
-                       RoadPriorityClass::Enum road_priority_class)
+                       RoadPriorityClass::Enum road_priority_class,
+                       std::uint8_t number_of_lanes)
         : motorway_class(motorway_class), link_class(link_class), may_be_ignored(may_be_ignored),
-          road_priority_class(road_priority_class)
+          road_priority_class(road_priority_class), number_of_lanes(number_of_lanes)
     {
     }
 
@@ -88,6 +91,9 @@ class RoadClassification
     bool IsLowPriorityRoadClass() const { return (0 != may_be_ignored); }
     void SetLowPriorityFlag(const bool new_value) { may_be_ignored = new_value; }
 
+    std::uint8_t GetNumberOfLanes() const { return number_of_lanes; }
+    void SetNumberOfLanes(const std::uint8_t new_value) { number_of_lanes = new_value; }
+
     std::uint32_t GetPriority() const { return static_cast<std::uint32_t>(road_priority_class); }
 
     RoadPriorityClass::Enum GetClass() const { return road_priority_class; }
@@ -112,8 +118,8 @@ class RoadClassification
 #pragma pack(pop)
 
 static_assert(
-    sizeof(RoadClassification) == 1,
-    "Road Classification should fit a byte. Increasing this has a severe impact on memory.");
+    sizeof(RoadClassification) == 2,
+    "Road Classification should fit two bytes. Increasing this has a severe impact on memory.");
 
 inline bool canBeSeenAsFork(const RoadClassification first, const RoadClassification second)
 {

include/extractor/guidance/roundabout_handler.hpp

@@ -2,6 +2,7 @@
 #define OSRM_EXTRACTOR_GUIDANCE_ROUNDABOUT_HANDLER_HPP_
 
 #include "extractor/compressed_edge_container.hpp"
+#include "extractor/guidance/coordinate_extractor.hpp"
 #include "extractor/guidance/intersection.hpp"
 #include "extractor/guidance/intersection_generator.hpp"
 #include "extractor/guidance/intersection_handler.hpp"
@@ -87,6 +88,8 @@ class RoundaboutHandler : public IntersectionHandler
 
     const CompressedEdgeContainer &compressed_edge_container;
     const ProfileProperties &profile_properties;
+
+    const CoordinateExtractor coordinate_extractor;
 };
 
 } // namespace guidance

include/extractor/guidance/toolkit.hpp

@@ -7,12 +7,15 @@
 #include "util/coordinate_calculation.hpp"
 #include "util/guidance/toolkit.hpp"
 #include "util/guidance/turn_lanes.hpp"
+#include "util/node_based_graph.hpp"
 #include "util/typedefs.hpp"
 
 #include "extractor/compressed_edge_container.hpp"
 #include "extractor/query_node.hpp"
 
+#include "extractor/guidance/constants.hpp"
 #include "extractor/guidance/intersection.hpp"
+#include "extractor/guidance/road_classification.hpp"
 #include "extractor/guidance/turn_instruction.hpp"
 
 #include "engine/guidance/route_step.hpp"
@@ -24,6 +27,7 @@
 #include <string>
 #include <unordered_map>
 #include <utility>
+#include <vector>
 
 #include <boost/algorithm/string.hpp>
 #include <boost/functional/hash.hpp>
@@ -44,113 +48,6 @@ using util::guidance::angularDeviation;
 using util::guidance::entersRoundabout;
 using util::guidance::leavesRoundabout;
 
-namespace detail
-{
-const constexpr double DESIRED_SEGMENT_LENGTH = 10.0;
-
-template <typename IteratorType>
-util::Coordinate
-getCoordinateFromCompressedRange(util::Coordinate current_coordinate,
-                                 const IteratorType compressed_geometry_begin,
-                                 const IteratorType compressed_geometry_end,
-                                 const util::Coordinate final_coordinate,
-                                 const std::vector<extractor::QueryNode> &query_nodes)
-{
-    const auto extractCoordinateFromNode =
-        [](const extractor::QueryNode &node) -> util::Coordinate {
-        return {node.lon, node.lat};
-    };
-    double distance_to_current_coordinate = 0;
-    double distance_to_next_coordinate = 0;
-
-    // get the length that is missing from the current segment to reach DESIRED_SEGMENT_LENGTH
-    const auto getFactor = [](const double first_distance, const double second_distance) {
-        BOOST_ASSERT(first_distance < detail::DESIRED_SEGMENT_LENGTH);
-        double segment_length = second_distance - first_distance;
-        BOOST_ASSERT(segment_length > 0);
-        BOOST_ASSERT(second_distance >= detail::DESIRED_SEGMENT_LENGTH);
-        double missing_distance = detail::DESIRED_SEGMENT_LENGTH - first_distance;
-        return std::max(0., std::min(missing_distance / segment_length, 1.0));
-    };
-
-    for (auto compressed_geometry_itr = compressed_geometry_begin;
-         compressed_geometry_itr != compressed_geometry_end;
-         ++compressed_geometry_itr)
-    {
-        const auto next_coordinate =
-            extractCoordinateFromNode(query_nodes[compressed_geometry_itr->node_id]);
-        distance_to_next_coordinate =
-            distance_to_current_coordinate +
-            util::coordinate_calculation::haversineDistance(current_coordinate, next_coordinate);
-
-        // reached point where coordinates switch between
-        if (distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH)
-            return util::coordinate_calculation::interpolateLinear(
-                getFactor(distance_to_current_coordinate, distance_to_next_coordinate),
-                current_coordinate,
-                next_coordinate);
-
-        // prepare for next iteration
-        current_coordinate = next_coordinate;
-        distance_to_current_coordinate = distance_to_next_coordinate;
-    }
-
-    distance_to_next_coordinate =
-        distance_to_current_coordinate +
-        util::coordinate_calculation::haversineDistance(current_coordinate, final_coordinate);
-
-    // reached point where coordinates switch between
-    if (distance_to_current_coordinate < detail::DESIRED_SEGMENT_LENGTH &&
-        distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH)
-        return util::coordinate_calculation::interpolateLinear(
-            getFactor(distance_to_current_coordinate, distance_to_next_coordinate),
-            current_coordinate,
-            final_coordinate);
-    else
-        return final_coordinate;
-}
-} // namespace detail
-
-// Finds a (potentially interpolated) coordinate that is DESIRED_SEGMENT_LENGTH away
-// from the start of an edge
-inline util::Coordinate
-getRepresentativeCoordinate(const NodeID from_node,
-                            const NodeID to_node,
-                            const EdgeID via_edge_id,
-                            const bool traverse_in_reverse,
-                            const extractor::CompressedEdgeContainer &compressed_geometries,
-                            const std::vector<extractor::QueryNode> &query_nodes)
-{
-    const auto extractCoordinateFromNode =
-        [](const extractor::QueryNode &node) -> util::Coordinate {
-        return {node.lon, node.lat};
-    };
-
-    // Uncompressed roads are simple, return the coordinate at the end
-    if (!compressed_geometries.HasZippedEntryForForwardID(via_edge_id) && !compressed_geometries.HasZippedEntryForReverseID(via_edge_id))
-    {
-        return extractCoordinateFromNode(traverse_in_reverse ? query_nodes[from_node]
-                                                             : query_nodes[to_node]);
-    }
-    else
-    {
-        const auto &geometry = compressed_geometries.GetBucketReference(via_edge_id);
-
-        const auto base_node_id = (traverse_in_reverse) ? to_node : from_node;
-        const auto base_coordinate = extractCoordinateFromNode(query_nodes[base_node_id]);
-
-        const auto final_node = (traverse_in_reverse) ? from_node : to_node;
-        const auto final_coordinate = extractCoordinateFromNode(query_nodes[final_node]);
-
-        if (traverse_in_reverse)
-            return detail::getCoordinateFromCompressedRange(
-                base_coordinate, geometry.rbegin(), geometry.rend(), final_coordinate, query_nodes);
-        else
-            return detail::getCoordinateFromCompressedRange(
-                base_coordinate, geometry.begin(), geometry.end(), final_coordinate, query_nodes);
-    }
-}
-
 // To simplify handling of Left/Right hand turns, we can mirror turns and write an intersection
 // handler only for one side. The mirror function turns a left-hand turn in a equivalent right-hand
 // turn and vice versa.
@@ -313,6 +210,78 @@ auto inline lanesToTheRight(const engine::guidance::RouteStep &step)
     return boost::make_iterator_range(description.end() - num_lanes_right, description.end());
 }
 
+inline std::uint8_t getLaneCountAtIntersection(const NodeID intersection_node,
+                                               const util::NodeBasedDynamicGraph &node_based_graph)
+{
+    std::uint8_t lanes = 0;
+    for (const EdgeID onto_edge : node_based_graph.GetAdjacentEdgeRange(intersection_node))
+        lanes = std::max(
+            lanes, node_based_graph.GetEdgeData(onto_edge).road_classification.GetNumberOfLanes());
+    return lanes;
+}
+
+inline bool obviousByRoadClass(const RoadClassification in_classification,
+                               const RoadClassification obvious_candidate,
+                               const RoadClassification compare_candidate)
+{
+    const bool has_high_priority = PRIORITY_DISTINCTION_FACTOR * obvious_candidate.GetPriority() <
+                                   compare_candidate.GetPriority();
+
+    const bool continues_on_same_class = in_classification == obvious_candidate;
+    return (has_high_priority && continues_on_same_class) ||
+           (!obvious_candidate.IsLowPriorityRoadClass() &&
+            !in_classification.IsLowPriorityRoadClass() &&
+            compare_candidate.IsLowPriorityRoadClass());
+}
+
+/* We use the sum of least squares to calculate a linear regression through our
+* coordinates.
+* This regression gives a good idea of how the road can be perceived and corrects for
+* initial and final corrections
+*/
+inline std::pair<util::Coordinate, util::Coordinate>
+leastSquareRegression(const std::vector<util::Coordinate> &coordinates)
+{
+    BOOST_ASSERT(coordinates.size() >= 2);
+    double sum_lon = 0, sum_lat = 0, sum_lon_lat = 0, sum_lon_lon = 0;
+    double min_lon = static_cast<double>(toFloating(coordinates.front().lon));
+    double max_lon = static_cast<double>(toFloating(coordinates.front().lon));
+    for (const auto coord : coordinates)
+    {
+        min_lon = std::min(min_lon, static_cast<double>(toFloating(coord.lon)));
+        max_lon = std::max(max_lon, static_cast<double>(toFloating(coord.lon)));
+        sum_lon += static_cast<double>(toFloating(coord.lon));
+        sum_lon_lon +=
+            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lon));
+        sum_lat += static_cast<double>(toFloating(coord.lat));
+        sum_lon_lat +=
+            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lat));
+    }
+
+    const auto dividend = coordinates.size() * sum_lon_lat - sum_lon * sum_lat;
+    const auto divisor = coordinates.size() * sum_lon_lon - sum_lon * sum_lon;
+    if (std::abs(divisor) < std::numeric_limits<double>::epsilon())
+        return std::make_pair(coordinates.front(), coordinates.back());
+
+    // slope of the regression line
+    const auto slope = dividend / divisor;
+    const auto intercept = (sum_lat - slope * sum_lon) / coordinates.size();
+
+    const auto GetLatAtLon = [intercept,
+                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
+        return {intercept + slope * static_cast<double>((longitude))};
+    };
+
+    const util::Coordinate regression_first = {
+        toFixed(util::FloatLongitude{min_lon - 1}),
+        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - 1})))};
+    const util::Coordinate regression_end = {
+        toFixed(util::FloatLongitude{max_lon + 1}),
+        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + 1})))};
+
+    return {regression_first, regression_end};
+}
+
 } // namespace guidance
 } // namespace extractor
 } // namespace osrm

include/extractor/guidance/turn_classification.hpp

@@ -25,11 +25,7 @@ namespace guidance
 {
 
 std::pair<util::guidance::EntryClass, util::guidance::BearingClass>
-classifyIntersection(NodeID nid,
-                     const Intersection &intersection,
-                     const util::NodeBasedDynamicGraph &node_based_graph,
-                     const extractor::CompressedEdgeContainer &compressed_geometries,
-                     const std::vector<extractor::QueryNode> &query_nodes);
+classifyIntersection(Intersection intersection);
 
 } // namespace guidance
 } // namespace extractor

include/extractor/guidance/turn_instruction.hpp

@@ -6,7 +6,6 @@
 #include <boost/assert.hpp>
 
 #include "extractor/guidance/roundabout_type.hpp"
-#include "util/guidance/turn_lanes.hpp"
 #include "util/typedefs.hpp"
 
 namespace osrm
@@ -68,10 +67,8 @@ const constexpr Enum Sliproad =
 const constexpr Enum MaxTurnType = 27; // Special value for static asserts
 }
 
-// turn angle in 1.40625 degree -> 128 == 180 degree
 struct TurnInstruction
 {
-    using LaneTuple = util::guidance::LaneTuple;
     TurnInstruction(const TurnType::Enum type = TurnType::Invalid,
                     const DirectionModifier::Enum direction_modifier = DirectionModifier::UTurn)
         : type(type), direction_modifier(direction_modifier)