From 9c033ff4613b6cc273d576c881c10b0ef6992b01 Mon Sep 17 00:00:00 2001 From: Michael Krasnyk Date: Mon, 20 Nov 2017 17:20:49 +0100 Subject: [PATCH] Free functions for guidance intersections analysis --- .../extractor/guidance/turn_lane_types.hpp | 10 +- .../intersection/intersection_analysis.hpp | 50 +++ .../intersection/intersection_edge.hpp | 42 +++ include/util/bearing.hpp | 12 - src/extractor/edge_based_graph_factory.cpp | 62 ++++ src/extractor/guidance/motorway_handler.cpp | 1 + .../intersection/intersection_analysis.cpp | 317 ++++++++++++++++++ 7 files changed, 478 insertions(+), 16 deletions(-) create mode 100644 include/extractor/intersection/intersection_analysis.hpp create mode 100644 include/extractor/intersection/intersection_edge.hpp create mode 100644 src/extractor/intersection/intersection_analysis.cpp diff --git a/include/extractor/guidance/turn_lane_types.hpp b/include/extractor/guidance/turn_lane_types.hpp index 7c5404164..eebdd5b25 100644 --- a/include/extractor/guidance/turn_lane_types.hpp +++ b/include/extractor/guidance/turn_lane_types.hpp @@ -100,8 +100,10 @@ typedef util::ConcurrentIDMap LaneDescriptionMap; -inline std::tuple, std::vector> -transformTurnLaneMapIntoArrays(const LaneDescriptionMap &turn_lane_map) +using TurnLanesIndexedArray = + std::tuple, std::vector>; + +inline TurnLanesIndexedArray transformTurnLaneMapIntoArrays(const LaneDescriptionMap &turn_lane_map) { // could use some additional capacity? To avoid a copy during processing, though small data so // probably not that important. @@ -111,8 +113,7 @@ transformTurnLaneMapIntoArrays(const LaneDescriptionMap &turn_lane_map) // // turn lane offsets points into the locations of the turn_lane_masks array. We use a standard // adjacency array like structure to store the turn lane masks. - std::vector turn_lane_offsets(turn_lane_map.data.size() + - 2); // empty ID + sentinel + std::vector turn_lane_offsets(turn_lane_map.data.size() + 1); // + sentinel for (auto entry = turn_lane_map.data.begin(); entry != turn_lane_map.data.end(); ++entry) turn_lane_offsets[entry->second + 1] = entry->first.size(); @@ -125,6 +126,7 @@ transformTurnLaneMapIntoArrays(const LaneDescriptionMap &turn_lane_map) std::copy(entry->first.begin(), entry->first.end(), turn_lane_masks.begin() + turn_lane_offsets[entry->second]); + return std::make_tuple(std::move(turn_lane_offsets), std::move(turn_lane_masks)); } diff --git a/include/extractor/intersection/intersection_analysis.hpp b/include/extractor/intersection/intersection_analysis.hpp new file mode 100644 index 000000000..529e6ba32 --- /dev/null +++ b/include/extractor/intersection/intersection_analysis.hpp @@ -0,0 +1,50 @@ +#ifndef OSRM_EXTRACTOR_INTERSECTION_INTERSECTION_ANALYSIS_HPP +#define OSRM_EXTRACTOR_INTERSECTION_INTERSECTION_ANALYSIS_HPP + +#include "extractor/compressed_edge_container.hpp" +#include "extractor/guidance/turn_lane_types.hpp" +#include "extractor/intersection/intersection_edge.hpp" +#include "extractor/restriction_index.hpp" + +#include "util/coordinate.hpp" +#include "util/node_based_graph.hpp" + +#include +#include + +namespace osrm +{ +namespace extractor +{ +namespace intersection +{ + +IntersectionEdges getIncomingEdges(const util::NodeBasedDynamicGraph &graph, + const NodeID intersection); + +IntersectionEdges getOutgoingEdges(const util::NodeBasedDynamicGraph &graph, + const NodeID intersection); + +IntersectionEdgeBearings +getIntersectionBearings(const util::NodeBasedDynamicGraph &graph, + const extractor::CompressedEdgeContainer &compressed_geometries, + const std::vector &node_coordinates, + const NodeID intersection); + +bool isTurnAllowed(const util::NodeBasedDynamicGraph &graph, + const EdgeBasedNodeDataContainer &node_data_container, + const RestrictionMap &restriction_map, + const std::unordered_set &barrier_nodes, + const IntersectionEdgeBearings &bearings, + const guidance::TurnLanesIndexedArray &turn_lanes_data, + const IntersectionEdge &from, + const IntersectionEdge &to); + +double computeTurnAngle(const IntersectionEdgeBearings &bearings, + const IntersectionEdge &from, + const IntersectionEdge &to); +} +} +} + +#endif diff --git a/include/extractor/intersection/intersection_edge.hpp b/include/extractor/intersection/intersection_edge.hpp new file mode 100644 index 000000000..4529494e9 --- /dev/null +++ b/include/extractor/intersection/intersection_edge.hpp @@ -0,0 +1,42 @@ +#ifndef OSRM_EXTRACTOR_INTERSECTION_INTERSECTION_EDGE_HPP +#define OSRM_EXTRACTOR_INTERSECTION_INTERSECTION_EDGE_HPP + +#include "util/typedefs.hpp" + +#include + +namespace osrm +{ +namespace extractor +{ +namespace intersection +{ + +// IntersectionEdge is an alias for incoming and outgoing node-based graph edges of an intersection +struct IntersectionEdge +{ + NodeID node; + EdgeID edge; + + bool operator<(const IntersectionEdge &other) const + { + return std::tie(node, edge) < std::tie(other.node, other.edge); + } +}; + +using IntersectionEdges = std::vector; + +struct IntersectionEdgeBearing +{ + EdgeID edge; + float bearing; + + bool operator<(const IntersectionEdgeBearing &other) const { return edge < other.edge; } +}; + +using IntersectionEdgeBearings = std::vector; +} +} +} + +#endif diff --git a/include/util/bearing.hpp b/include/util/bearing.hpp index fa68f0d17..c046bf417 100644 --- a/include/util/bearing.hpp +++ b/include/util/bearing.hpp @@ -144,18 +144,6 @@ inline double restrictAngleToValidRange(const double angle) 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 diff --git a/src/extractor/edge_based_graph_factory.cpp b/src/extractor/edge_based_graph_factory.cpp index ba4c4a814..a04fdecb6 100644 --- a/src/extractor/edge_based_graph_factory.cpp +++ b/src/extractor/edge_based_graph_factory.cpp @@ -7,6 +7,8 @@ #include "extractor/scripting_environment.hpp" #include "extractor/suffix_table.hpp" +#include "extractor/intersection/intersection_analysis.hpp" + #include "extractor/serialization.hpp" #include "storage/io.hpp" @@ -444,6 +446,8 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges( bearing_class_by_node_based_node.resize(m_node_based_graph.GetNumberOfNodes(), std::numeric_limits::max()); + const auto &turn_lanes_data = transformTurnLaneMapIntoArrays(lane_description_map); + // FIXME these need to be tuned in pre-allocated size std::vector turn_weight_penalties; std::vector turn_duration_penalties; @@ -661,6 +665,53 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges( ++node_at_center_of_intersection) { + int new_turns = 0, old_turns = 0; + + std::cout << "=== node_at_center_of_intersection " + << node_at_center_of_intersection << "\n"; + const auto &incoming_edges = intersection::getIncomingEdges( + m_node_based_graph, node_at_center_of_intersection); + const auto &outgoing_edges = intersection::getOutgoingEdges( + m_node_based_graph, node_at_center_of_intersection); + const auto &edge_bearings = + intersection::getIntersectionBearings(m_node_based_graph, + m_compressed_edge_container, + m_coordinates, + node_at_center_of_intersection); + + std::cout << "=== new turns \n"; + for (const auto &incoming_edge : incoming_edges) + { + for (const auto &outgoing_edge : outgoing_edges) + { + const auto turn_angle = intersection::computeTurnAngle( + edge_bearings, incoming_edge, outgoing_edge); + + std::cout << incoming_edge.node << "," << incoming_edge.edge << " -> " + << outgoing_edge.node << "," << outgoing_edge.edge << " -> " + << m_node_based_graph.GetTarget(outgoing_edge.edge) + << " is allowed " + << intersection::isTurnAllowed(m_node_based_graph, + m_edge_based_node_container, + node_restriction_map, + m_barrier_nodes, + edge_bearings, + turn_lanes_data, + incoming_edge, + outgoing_edge) + << " angle " << turn_angle << "\n"; + + new_turns += intersection::isTurnAllowed(m_node_based_graph, + m_edge_based_node_container, + node_restriction_map, + m_barrier_nodes, + edge_bearings, + turn_lanes_data, + incoming_edge, + outgoing_edge); + } + } + // We capture the thread-local work in these objects, then flush // them in a controlled manner at the end of the parallel range @@ -685,6 +736,8 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges( // From the flags alone, we cannot determine which nodes are connected to // `b` by an outgoing edge. Therefore, we have to search all connected edges for // edges entering `b` + std::cout << "=== old turns \n"; + for (const EdgeID outgoing_edge : m_node_based_graph.GetAdjacentEdgeRange(node_at_center_of_intersection)) { @@ -747,6 +800,11 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges( if (!turn.entry_allowed) continue; + old_turns += 1; + std::cout << node_along_road_entering << " -> " + << node_at_center_of_intersection << " -> " + << m_node_based_graph.GetTarget(turn.eid) << "\n"; + // In case a way restriction starts at a given location, add a turn onto // every artificial node eminating here. // @@ -895,6 +953,10 @@ void EdgeBasedGraphFactory::GenerateEdgeExpandedEdges( } } } + + std::cout << "new_turns " << new_turns << " old_turns " << old_turns << "\n"; + OSRM_ASSERT(new_turns == old_turns, + m_coordinates[node_at_center_of_intersection]); } return buffer; diff --git a/src/extractor/guidance/motorway_handler.cpp b/src/extractor/guidance/motorway_handler.cpp index 4689286f1..95ff22507 100644 --- a/src/extractor/guidance/motorway_handler.cpp +++ b/src/extractor/guidance/motorway_handler.cpp @@ -278,6 +278,7 @@ Intersection MotorwayHandler::fromMotorway(const EdgeID via_eid, Intersection in via_eid, isThroughStreet(1, intersection), intersection[1]); + // TODO: no coverage by feature test cases intersection[0].entry_allowed = false; // UTURN on the freeway } else if (exiting_motorways == 2) diff --git a/src/extractor/intersection/intersection_analysis.cpp b/src/extractor/intersection/intersection_analysis.cpp new file mode 100644 index 000000000..5944a17b0 --- /dev/null +++ b/src/extractor/intersection/intersection_analysis.cpp @@ -0,0 +1,317 @@ +#include "extractor/intersection/intersection_analysis.hpp" + +#include "util/bearing.hpp" +#include "util/coordinate_calculation.hpp" + +namespace osrm +{ +namespace extractor +{ +namespace intersection +{ + +IntersectionEdges getIncomingEdges(const util::NodeBasedDynamicGraph &graph, + const NodeID intersection_node) +{ + IntersectionEdges result; + + for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node)) + { + const auto from_node = graph.GetTarget(outgoing_edge); + const auto incoming_edge = graph.FindEdge(from_node, intersection_node); + + if (!graph.GetEdgeData(incoming_edge).reversed) + { + result.push_back({from_node, incoming_edge}); + } + } + + // Enforce ordering of incoming edges + std::sort(result.begin(), result.end()); + return result; +} + +IntersectionEdges getOutgoingEdges(const util::NodeBasedDynamicGraph &graph, + const NodeID intersection_node) +{ + IntersectionEdges result; + + for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node)) + { + if (!graph.GetEdgeData(outgoing_edge).reversed) + { + result.push_back({intersection_node, outgoing_edge}); + } + } + + // Enforce ordering of outgoing edges + std::sort(result.begin(), result.end()); + return result; +} + +std::vector +getEdgeCoordinates(const extractor::CompressedEdgeContainer &compressed_geometries, + const std::vector &node_coordinates, + const NodeID from_node, + const EdgeID edge, + const NodeID to_node) +{ + if (!compressed_geometries.HasEntryForID(edge)) + return {node_coordinates[from_node], node_coordinates[to_node]}; + + BOOST_ASSERT(from_node < node_coordinates.size()); + BOOST_ASSERT(to_node < node_coordinates.size()); + + // extracts the geometry in coordinates from the compressed edge container + std::vector result; + const auto &geometry = compressed_geometries.GetBucketReference(edge); + result.reserve(geometry.size() + 2); + + result.push_back(node_coordinates[from_node]); + std::transform(geometry.begin(), + geometry.end(), + std::back_inserter(result), + [&node_coordinates](const auto &compressed_edge) { + return node_coordinates[compressed_edge.node_id]; + }); + result.push_back(node_coordinates[to_node]); + + // filter duplicated coordinates + result.erase(std::unique(result.begin(), result.end()), result.end()); + return result; +} + +IntersectionEdgeBearings +getIntersectionBearings(const util::NodeBasedDynamicGraph &graph, + const extractor::CompressedEdgeContainer &compressed_geometries, + const std::vector &node_coordinates, + const NodeID intersection_node) +{ + IntersectionEdgeBearings result; + + for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node)) + { + const auto remote_node = graph.GetTarget(outgoing_edge); + const auto incoming_edge = graph.FindEdge(remote_node, intersection_node); + + const auto &geometry = getEdgeCoordinates( + compressed_geometries, node_coordinates, intersection_node, outgoing_edge, remote_node); + + // TODO: add MergableRoadDetector logic + const auto outgoing_bearing = + util::coordinate_calculation::bearing(geometry[0], geometry[1]); + + result.push_back({outgoing_edge, static_cast(outgoing_bearing)}); + result.push_back( + {incoming_edge, static_cast(util::bearing::reverse(outgoing_bearing))}); + + for (auto x : geometry) + std::cout << x << ", "; + std::cout << "\n"; + } + + for (auto x : result) + std::cout << x.edge << "," << x.bearing << "; "; + std::cout << "\n"; + + // Enforce ordering of edges + std::sort(result.begin(), result.end()); + return result; +} + +auto findEdgeBearing(const IntersectionEdgeBearings &bearings, const EdgeID &edge) +{ + const auto it = std::lower_bound( + bearings.begin(), bearings.end(), edge, [](const auto &edge_bearing, const auto edge) { + return edge_bearing.edge < edge; + }); + BOOST_ASSERT(it != bearings.end() && it->edge == edge); + return it->bearing; +} + +double computeTurnAngle(const IntersectionEdgeBearings &bearings, + const IntersectionEdge &from, + const IntersectionEdge &to) +{ + return util::bearing::angleBetween(findEdgeBearing(bearings, from.edge), + findEdgeBearing(bearings, to.edge)); +} + +template +bool isTurnRestricted(const RestrictionsRange &restrictions, const NodeID to) +{ + // Check turn restrictions to find a node that is the only allowed target when coming from a + // node to an intersection + // d + // | + // a - b - c and `only_straight_on ab | bc would return `c` for `a,b` + const auto is_only = std::find_if(restrictions.first, + restrictions.second, + [](const auto &pair) { return pair.second->is_only; }); + if (is_only != restrictions.second) + return is_only->second->AsNodeRestriction().to != to; + + // Check if explicitly forbidden + const auto no_turn = + std::find_if(restrictions.first, restrictions.second, [&to](const auto &restriction) { + return restriction.second->AsNodeRestriction().to == to; + }); + + return no_turn != restrictions.second; +} + +bool isTurnAllowed(const util::NodeBasedDynamicGraph &graph, + const EdgeBasedNodeDataContainer &node_data_container, + const RestrictionMap &restriction_map, + const std::unordered_set &barrier_nodes, + const IntersectionEdgeBearings &bearings, + const guidance::TurnLanesIndexedArray &turn_lanes_data, + const IntersectionEdge &from, + const IntersectionEdge &to) +{ + BOOST_ASSERT(graph.GetTarget(from.edge) == to.node); + + const auto intersection_node = to.node; + const auto destination_node = graph.GetTarget(to.edge); + auto const &restrictions = restriction_map.Restrictions(from.node, intersection_node); + + // Check if turn is explicitly restricted by a turn restriction + if (isTurnRestricted(restrictions, destination_node)) + return false; + + // Precompute reversed bearing of the `from` edge + const auto from_edge_reversed_bearing = + util::bearing::reverse(findEdgeBearing(bearings, from.edge)); + + // Collect some information about the intersection + // 1) number of allowed exits and adjacent bidirectional edges + std::uint32_t allowed_exits = 0, bidirectional_edges = 0; + // 2) edge IDs of roundabouts edges + EdgeID roundabout_from = SPECIAL_EDGEID, roundabout_to = SPECIAL_EDGEID; + double roundabout_from_angle = 0., roundabout_to_angle = 0.; + + for (const auto eid : graph.GetAdjacentEdgeRange(intersection_node)) + { + const auto &edge_data = graph.GetEdgeData(eid); + const auto &edge_class = edge_data.flags; + const auto to_node = graph.GetTarget(eid); + const auto reverse_edge = graph.FindEdge(to_node, intersection_node); + BOOST_ASSERT(reverse_edge != SPECIAL_EDGEID); + + const auto is_exit_edge = !edge_data.reversed && !isTurnRestricted(restrictions, to_node); + const auto is_bidirectional = !graph.GetEdgeData(reverse_edge).reversed; + allowed_exits += is_exit_edge; + bidirectional_edges += is_bidirectional; + + if (edge_class.roundabout || edge_class.circular) + { + if (edge_data.reversed) + { + // "Linked Roundabouts" is an example of tie between two linked roundabouts + // A tie breaker for that maximizes ∠(roundabout_from_bearing, ¬from_edge_bearing) + const auto angle = util::bearing::angleBetween( + findEdgeBearing(bearings, reverse_edge), from_edge_reversed_bearing); + if (angle > roundabout_from_angle) + { + roundabout_from = reverse_edge; + roundabout_from_angle = angle; + } + } + else + { + // a tie breaker that maximizes ∠(¬from_edge_bearing, roundabout_to_bearing) + const auto angle = util::bearing::angleBetween(from_edge_reversed_bearing, + findEdgeBearing(bearings, eid)); + if (angle > roundabout_to_angle) + { + roundabout_to = eid; + roundabout_to_angle = angle; + } + } + } + } + + // 3) if the intersection has a barrier + const bool is_barrier_node = barrier_nodes.find(intersection_node) != barrier_nodes.end(); + + // Check a U-turn + if (from.node == destination_node) + { + // Allow U-turns before barrier nodes + if (is_barrier_node) + return true; + + // Allow U-turns at dead-ends + if (graph.GetAdjacentEdgeRange(intersection_node).size() == 1) + return true; + + // Allow U-turns at dead-ends if there is at most one bidirectional road at the intersection + // The condition allows a U-turns d→a→d and c→b→c ("Bike - Around the Block" test) + // a→b + // ↕ ↕ + // d↔c + if (allowed_exits == 1 || bidirectional_edges <= 1) + return true; + + // Allow U-turn if the incoming edge has a U-turn lane + const auto &incoming_edge_annotation_id = graph.GetEdgeData(from.edge).annotation_data; + const auto lane_description_id = static_cast( + node_data_container.GetAnnotation(incoming_edge_annotation_id).lane_description_id); + if (lane_description_id != INVALID_LANE_DESCRIPTIONID) + { + const auto &turn_lane_offsets = std::get<0>(turn_lanes_data); + const auto &turn_lanes = std::get<1>(turn_lanes_data); + BOOST_ASSERT(lane_description_id + 1 < turn_lane_offsets.size()); + + if (std::any_of(turn_lanes.begin() + turn_lane_offsets[lane_description_id], + turn_lanes.begin() + turn_lane_offsets[lane_description_id + 1], + [](const auto &lane) { return lane & guidance::TurnLaneType::uturn; })) + return true; + } + + // Don't allow U-turns on usual intersections + return false; + } + + // Don't allow turns via barriers for not U-turn maneuvers + if (is_barrier_node) + return false; + + // Check for roundabouts exits in the opposite direction of roundabout flow + if (roundabout_from != SPECIAL_EDGEID && roundabout_to != SPECIAL_EDGEID) + { + // Get bearings of edges + const auto roundabout_from_bearing = findEdgeBearing(bearings, roundabout_from); + const auto roundabout_to_bearing = findEdgeBearing(bearings, roundabout_to); + const auto to_bearing = findEdgeBearing(bearings, to.edge); + + // Get angles from the roundabout edge to three other edges + const auto roundabout_angle = + util::bearing::angleBetween(roundabout_from_bearing, roundabout_to_bearing); + const auto roundabout_from_angle = + util::bearing::angleBetween(roundabout_from_bearing, from_edge_reversed_bearing); + const auto roundabout_to_angle = + util::bearing::angleBetween(roundabout_from_bearing, to_bearing); + + // Restrict turning over a roundabout if `roundabout_to_angle` is in + // a sector between `roundabout_from_bearing` to `from_bearing` + // + // 150° 150° + // v░░░░░░ ░░░░░░░░░v + // v░░░░░░░ ░░░░░░░░v + // 270° <-ooo- v -ttt-> 90° 270° <-ttt- v -ooo-> 90° + // ^░░░░░░░ ░░░░░░░^ + // r░░░░░░░ ░░░░░░░r + // r░░░░░░░ ░░░░░░░r + if ((roundabout_from_angle < roundabout_angle && + roundabout_to_angle < roundabout_from_angle) || + (roundabout_from_angle > roundabout_angle && + roundabout_to_angle > roundabout_from_angle)) + return false; + } + + return true; +} +} +} +}