431 lines
18 KiB
C++
431 lines
18 KiB
C++
#include "extractor/guidance/intersection_normalizer.hpp"
|
||
#include "util/bearing.hpp"
|
||
#include "util/coordinate_calculation.hpp"
|
||
|
||
#include <tuple>
|
||
#include <utility>
|
||
|
||
using osrm::util::angularDeviation;
|
||
|
||
namespace osrm
|
||
{
|
||
namespace extractor
|
||
{
|
||
namespace guidance
|
||
{
|
||
|
||
IntersectionNormalizer::IntersectionNormalizer(
|
||
const util::NodeBasedDynamicGraph &node_based_graph,
|
||
const EdgeBasedNodeDataContainer &node_data_container,
|
||
const std::vector<util::Coordinate> &coordinates,
|
||
const util::NameTable &name_table,
|
||
const SuffixTable &street_name_suffix_table,
|
||
const IntersectionGenerator &intersection_generator)
|
||
: node_based_graph(node_based_graph), intersection_generator(intersection_generator),
|
||
mergable_road_detector(node_based_graph,
|
||
node_data_container,
|
||
coordinates,
|
||
intersection_generator,
|
||
intersection_generator.GetCoordinateExtractor(),
|
||
name_table,
|
||
street_name_suffix_table)
|
||
{
|
||
}
|
||
|
||
IntersectionNormalizer::NormalizationResult IntersectionNormalizer::
|
||
operator()(const NodeID node_at_intersection, IntersectionShape intersection) const
|
||
{
|
||
const auto intersection_copy = intersection;
|
||
auto merged_shape_and_merges =
|
||
MergeSegregatedRoads(node_at_intersection, std::move(intersection));
|
||
merged_shape_and_merges.normalized_shape = AdjustBearingsForMergeAtDestination(
|
||
node_at_intersection, std::move(merged_shape_and_merges.normalized_shape));
|
||
return merged_shape_and_merges;
|
||
}
|
||
|
||
bool IntersectionNormalizer::CanMerge(const NodeID intersection_node,
|
||
const IntersectionShape &intersection,
|
||
std::size_t fist_index_in_ccw,
|
||
std::size_t second_index_in_ccw) const
|
||
{
|
||
BOOST_ASSERT(((fist_index_in_ccw + 1) % intersection.size()) == second_index_in_ccw);
|
||
|
||
// don't merge on degree two, since it's most likely a bollard/traffic light or a round way
|
||
if (intersection.size() <= 2)
|
||
return false;
|
||
|
||
const auto can_merge = mergable_road_detector.CanMergeRoad(
|
||
intersection_node, intersection[fist_index_in_ccw], intersection[second_index_in_ccw]);
|
||
|
||
/*
|
||
* Merging should never depend on order/never merge more than two roads. To ensure that we don't
|
||
* merge anything that is impacted by neighboring roads (e.g. three roads of the same name as in
|
||
* parking lots/border checkpoints), we check if the neigboring roads would be merged as well.
|
||
* In that case, we cannot merge, since we would end up merging multiple items together
|
||
*/
|
||
const auto is_distinct = [&]() {
|
||
const auto next_index_in_ccw = (second_index_in_ccw + 1) % intersection.size();
|
||
const auto distinct_to_next_in_ccw = mergable_road_detector.IsDistinctFrom(
|
||
intersection[second_index_in_ccw], intersection[next_index_in_ccw]);
|
||
const auto prev_index_in_ccw =
|
||
(fist_index_in_ccw + intersection.size() - 1) % intersection.size();
|
||
const auto distinct_to_prev_in_ccw = mergable_road_detector.IsDistinctFrom(
|
||
intersection[prev_index_in_ccw], intersection[fist_index_in_ccw]);
|
||
return distinct_to_next_in_ccw && distinct_to_prev_in_ccw;
|
||
};
|
||
|
||
// use lazy evaluation to check only if mergable
|
||
return can_merge && is_distinct();
|
||
}
|
||
|
||
IntersectionNormalizationOperation
|
||
IntersectionNormalizer::DetermineMergeDirection(const IntersectionShapeData &lhs,
|
||
const IntersectionShapeData &rhs) const
|
||
{
|
||
if (node_based_graph.GetEdgeData(lhs.eid).reversed)
|
||
return {lhs.eid, rhs.eid};
|
||
else
|
||
return {rhs.eid, lhs.eid};
|
||
}
|
||
|
||
IntersectionShapeData IntersectionNormalizer::MergeRoads(const IntersectionShapeData &into,
|
||
const IntersectionShapeData &from) const
|
||
{
|
||
// we only merge small angles. If the difference between both is large, we are looking at a
|
||
// bearing leading north. Such a bearing cannot be handled via the basic average. In this
|
||
// case we actually need to shift the bearing by half the difference.
|
||
const auto aroundZero = [](const double first, const double second) {
|
||
return (std::max(first, second) - std::min(first, second)) >= 180;
|
||
};
|
||
|
||
// find the angle between two other angles
|
||
const auto combineAngles = [aroundZero](const double first, const double second) {
|
||
if (!aroundZero(first, second))
|
||
return .5 * (first + second);
|
||
else
|
||
{
|
||
const auto offset = angularDeviation(first, second);
|
||
auto new_angle = std::max(first, second) + .5 * offset;
|
||
if (new_angle >= 360)
|
||
return new_angle - 360;
|
||
return new_angle;
|
||
}
|
||
};
|
||
|
||
auto result = into;
|
||
BOOST_ASSERT(!node_based_graph.GetEdgeData(into.eid).reversed);
|
||
result.bearing = combineAngles(into.bearing, from.bearing);
|
||
BOOST_ASSERT(0 <= result.bearing && result.bearing < 360.0);
|
||
return result;
|
||
}
|
||
|
||
IntersectionShapeData
|
||
IntersectionNormalizer::MergeRoads(const IntersectionNormalizationOperation direction,
|
||
const IntersectionShapeData &lhs,
|
||
const IntersectionShapeData &rhs,
|
||
const double opposite_bearing) const
|
||
{
|
||
// In some intersections, turning roads can introduce artificial turns if we merge here.
|
||
// Consider a scenario like:
|
||
//
|
||
// a . g - f
|
||
// | .
|
||
// | .
|
||
// |.
|
||
// d-b--------e
|
||
// |
|
||
// c
|
||
//
|
||
// Merging `bgf` and `be` would introduce an angle, even though d-b-e is perfectly straight
|
||
// We don't change the angle, if such an opposite road exists
|
||
if (direction.merged_eid == lhs.eid)
|
||
{
|
||
// change the angle only if the opposite direction is not nearly straight
|
||
if (angularDeviation(opposite_bearing, rhs.bearing) >
|
||
(STRAIGHT_ANGLE - MAXIMAL_ALLOWED_NO_TURN_DEVIATION))
|
||
return rhs;
|
||
else
|
||
return MergeRoads(rhs, lhs);
|
||
}
|
||
else
|
||
{
|
||
if (angularDeviation(opposite_bearing, lhs.bearing) >
|
||
(STRAIGHT_ANGLE - MAXIMAL_ALLOWED_NO_TURN_DEVIATION))
|
||
return lhs;
|
||
else
|
||
return MergeRoads(lhs, rhs);
|
||
}
|
||
}
|
||
|
||
/*
|
||
* Segregated Roads often merge onto a single intersection.
|
||
* While technically representing different roads, they are
|
||
* often looked at as a single road.
|
||
* Due to the merging, turn Angles seem off, wenn we compute them from the
|
||
* initial positions.
|
||
*
|
||
* b<b<b<b(1)<b<b<b
|
||
* aaaaa-b
|
||
* b>b>b>b(2)>b>b>b
|
||
*
|
||
* Would be seen as a slight turn going fro a to (2). A Sharp turn going from
|
||
* (1) to (2).
|
||
*
|
||
* In cases like these, we megre this segregated roads into a single road to
|
||
* end up with a case like:
|
||
*
|
||
* aaaaa-bbbbbb
|
||
*
|
||
* for the turn representation.
|
||
* Anything containing the first u-turn in a merge affects all other angles
|
||
* and is handled separately from all others.
|
||
*/
|
||
IntersectionNormalizer::NormalizationResult
|
||
IntersectionNormalizer::MergeSegregatedRoads(const NodeID intersection_node,
|
||
IntersectionShape intersection) const
|
||
{
|
||
const auto getRight = [&](std::size_t index) {
|
||
return (index + intersection.size() - 1) % intersection.size();
|
||
};
|
||
|
||
// This map stores for all edges that participated in a merging operation in which edge id they
|
||
// end up in the end. We only store what we have merged into other edges.
|
||
std::vector<IntersectionNormalizationOperation> merging_map;
|
||
const auto merge = [this, &merging_map](const IntersectionShapeData &first,
|
||
const IntersectionShapeData &second,
|
||
const double opposite_bearing) {
|
||
|
||
const auto direction = DetermineMergeDirection(first, second);
|
||
BOOST_ASSERT(
|
||
std::find_if(merging_map.begin(), merging_map.end(), [direction](const auto pair) {
|
||
return pair.merged_eid == direction.merged_eid;
|
||
}) == merging_map.end());
|
||
merging_map.push_back(direction);
|
||
return MergeRoads(direction, first, second, opposite_bearing);
|
||
};
|
||
|
||
if (intersection.size() <= 1)
|
||
return {intersection, merging_map};
|
||
|
||
const auto intersection_copy = intersection;
|
||
const auto opposite_bearing = [this, intersection_copy](const IntersectionShapeData &lhs,
|
||
const IntersectionShapeData &rhs) {
|
||
if (node_based_graph.GetEdgeData(lhs.eid).reversed)
|
||
{
|
||
return intersection_copy.FindClosestBearing(util::bearing::reverse(rhs.bearing))
|
||
->bearing;
|
||
}
|
||
else
|
||
{
|
||
BOOST_ASSERT(node_based_graph.GetEdgeData(rhs.eid).reversed);
|
||
return intersection_copy.FindClosestBearing(util::bearing::reverse(lhs.bearing))
|
||
->bearing;
|
||
}
|
||
};
|
||
// check for merges including the basic u-turn
|
||
// these result in an adjustment of all other angles. This is due to how these angles are
|
||
// perceived. Considering the following example:
|
||
//
|
||
// c b
|
||
// Y
|
||
// a
|
||
//
|
||
// coming from a to b (given a road that splits at the fork into two one-ways), the turn is not
|
||
// considered as a turn but rather as going straight.
|
||
// Now if we look at the situation merging:
|
||
//
|
||
// a b
|
||
// \ /
|
||
// e - + - d
|
||
// |
|
||
// c
|
||
//
|
||
// With a,b representing the same road, the intersection itself represents a classif for way
|
||
// intersection so we handle it like
|
||
//
|
||
// (a),b
|
||
// |
|
||
// e - + - d
|
||
// |
|
||
// c
|
||
//
|
||
// To be able to consider this adjusted representation down the line, we merge some roads.
|
||
// If the merge occurs at the u-turn edge, we need to adjust all angles, though, since they are
|
||
// with respect to the now changed perceived location of a. If we move (a) to the left, we add
|
||
// the difference to all angles. Otherwise we subtract it.
|
||
// these result in an adjustment of all other angles
|
||
if (CanMerge(intersection_node, intersection, intersection.size() - 1, 0))
|
||
{
|
||
// moving `a` to the left
|
||
const auto opposite = opposite_bearing(intersection.front(), intersection.back());
|
||
intersection[0] = merge(intersection.front(), intersection.back(), opposite);
|
||
// FIXME if we have a left-sided country, we need to switch this off and enable it
|
||
// below
|
||
intersection.pop_back();
|
||
}
|
||
else if (CanMerge(intersection_node, intersection, 0, 1))
|
||
{
|
||
const auto opposite = opposite_bearing(intersection.front(), intersection[1]);
|
||
intersection[0] = merge(intersection.front(), intersection[1], opposite);
|
||
intersection.erase(intersection.begin() + 1);
|
||
}
|
||
|
||
// a merge including the first u-turn requires an adjustment of the turn angles
|
||
// therefore these are handled prior to this step
|
||
for (std::size_t index = 2; index < intersection.size(); ++index)
|
||
{
|
||
if (CanMerge(intersection_node, intersection, getRight(index), index))
|
||
{
|
||
const auto opposite =
|
||
opposite_bearing(intersection[getRight(index)], intersection[index]);
|
||
intersection[getRight(index)] =
|
||
merge(intersection[getRight(index)], intersection[index], opposite);
|
||
intersection.erase(intersection.begin() + index);
|
||
--index;
|
||
}
|
||
}
|
||
return {intersection, merging_map};
|
||
}
|
||
|
||
// OSM can have some very steep angles for joining roads. Considering the following intersection:
|
||
// x
|
||
// |
|
||
// v __________c
|
||
// /
|
||
// a ---d
|
||
// \ __________b
|
||
//
|
||
// with c->d as a oneway
|
||
// and d->b as a oneway, the turn von x->d is actually a turn from x->a. So when looking at the
|
||
// intersection coming from x, we want to interpret the situation as
|
||
// x
|
||
// |
|
||
// a __ d __ v__________c
|
||
// |
|
||
// |_______________b
|
||
//
|
||
// Where we see the turn to `d` as a right turn, rather than going straight.
|
||
// We do this by adjusting the local turn angle at `x` to turn onto `d` to be reflective of this
|
||
// situation, where `v` would be the node at the intersection.
|
||
IntersectionShape
|
||
IntersectionNormalizer::AdjustBearingsForMergeAtDestination(const NodeID node_at_intersection,
|
||
IntersectionShape intersection) const
|
||
{
|
||
// nothing to do for dead ends
|
||
if (intersection.size() <= 1)
|
||
return intersection;
|
||
|
||
// we don't adjust any road that is longer than 30 meters (between centers of intersections),
|
||
// since the road is probably too long otherwise to impact perception.
|
||
const double constexpr PRUNING_DISTANCE = 30;
|
||
// never adjust u-turns
|
||
for (std::size_t index = 0; index < intersection.size(); ++index)
|
||
{
|
||
auto &road = intersection[index];
|
||
// only consider roads that are close
|
||
if (road.segment_length > PRUNING_DISTANCE)
|
||
continue;
|
||
|
||
// to find out about the above situation, we need to look at the next intersection (at d in
|
||
// the example). If the initial road can be merged to the left/right, we are about to adjust
|
||
// the angle.
|
||
const auto next_intersection_along_road = intersection_generator.ComputeIntersectionShape(
|
||
node_based_graph.GetTarget(road.eid), node_at_intersection);
|
||
|
||
if (next_intersection_along_road.size() <= 1)
|
||
continue;
|
||
|
||
const auto node_at_next_intersection = node_based_graph.GetTarget(road.eid);
|
||
|
||
const auto adjustAngle = [](double angle, double offset) {
|
||
angle += offset;
|
||
if (angle > 360)
|
||
return angle - 360.;
|
||
else if (angle < 0)
|
||
return angle + 360.;
|
||
return angle;
|
||
};
|
||
|
||
const auto range = node_based_graph.GetAdjacentEdgeRange(node_at_next_intersection);
|
||
if (range.size() <= 1)
|
||
continue;
|
||
|
||
// the order does not matter
|
||
const auto get_offset = [](const IntersectionShapeData &lhs,
|
||
const IntersectionShapeData &rhs) {
|
||
return 0.5 * angularDeviation(lhs.bearing, rhs.bearing);
|
||
};
|
||
|
||
// When offsetting angles in our turns, we don't want to get past the next turn. This
|
||
// function simply limits an offset to be at most half the distance to the next turn in the
|
||
// offfset direction
|
||
const auto get_corrected_offset = [](
|
||
const double offset,
|
||
const IntersectionShapeData &road,
|
||
const IntersectionShapeData &next_road_in_offset_direction) {
|
||
const auto offset_limit =
|
||
angularDeviation(road.bearing, next_road_in_offset_direction.bearing);
|
||
// limit the offset with an additional buffer
|
||
return (offset + MAXIMAL_ALLOWED_NO_TURN_DEVIATION > offset_limit) ? 0.5 * offset_limit
|
||
: offset;
|
||
};
|
||
|
||
// only if straighmost angles get smaller, we consider it an improvement
|
||
auto const improves_straightmost = [&](auto const index, auto const offset) {
|
||
const auto itr = next_intersection_along_road.FindClosestBearing(
|
||
util::bearing::reverse(next_intersection_along_road[index].bearing));
|
||
const auto angle = util::bearing::angleBetween(
|
||
util::bearing::reverse(itr->bearing), next_intersection_along_road[index].bearing);
|
||
|
||
return util::angularDeviation(angle, STRAIGHT_ANGLE) >
|
||
util::angularDeviation(angle + offset, STRAIGHT_ANGLE);
|
||
};
|
||
|
||
// check if the u-turn edge at the next intersection could be merged to the left/right. If
|
||
// this is the case and the road is not far away (see previous distance check), if
|
||
// influences the perceived angle.
|
||
if (CanMerge(node_at_next_intersection, next_intersection_along_road, 0, 1))
|
||
{
|
||
const auto offset =
|
||
get_offset(next_intersection_along_road[0], next_intersection_along_road[1]);
|
||
|
||
if (improves_straightmost(0, -offset) && improves_straightmost(1, offset))
|
||
{
|
||
const auto corrected_offset = get_corrected_offset(
|
||
offset,
|
||
road,
|
||
intersection[(intersection.size() + index - 1) % intersection.size()]);
|
||
// at the target intersection, we merge to the right, so we need to shift the
|
||
// current
|
||
// angle to the left
|
||
road.bearing = adjustAngle(road.bearing, corrected_offset);
|
||
}
|
||
}
|
||
else if (CanMerge(node_at_next_intersection,
|
||
next_intersection_along_road,
|
||
next_intersection_along_road.size() - 1,
|
||
0))
|
||
{
|
||
const auto offset =
|
||
get_offset(next_intersection_along_road[0],
|
||
next_intersection_along_road[next_intersection_along_road.size() - 1]);
|
||
|
||
if (improves_straightmost(0, offset) &&
|
||
improves_straightmost(next_intersection_along_road.size() - 1, -offset))
|
||
{
|
||
const auto corrected_offset = get_corrected_offset(
|
||
offset, road, intersection[(index + 1) % intersection.size()]);
|
||
// at the target intersection, we merge to the left, so we need to shift the current
|
||
// angle to the right
|
||
road.bearing = adjustAngle(road.bearing, -corrected_offset);
|
||
}
|
||
}
|
||
}
|
||
|
||
return intersection;
|
||
}
|
||
|
||
} // namespace guidance
|
||
} // namespace extractor
|
||
} // namespace osrm
|