diff --git a/openlr/CMakeLists.txt b/openlr/CMakeLists.txt index ea59cba7c6..6040f4f468 100644 --- a/openlr/CMakeLists.txt +++ b/openlr/CMakeLists.txt @@ -29,6 +29,13 @@ set( road_info_getter.hpp router.cpp router.hpp + score_candidate_paths_getter.cpp + score_candidate_paths_getter.hpp + score_candidate_points_getter.cpp + score_candidate_points_getter.hpp + score_paths_connector.cpp + score_paths_connector.hpp + score_types.hpp stats.hpp way_point.hpp ) diff --git a/openlr/candidate_paths_getter.cpp b/openlr/candidate_paths_getter.cpp index 1b68db3437..5f96b25e31 100644 --- a/openlr/candidate_paths_getter.cpp +++ b/openlr/candidate_paths_getter.cpp @@ -27,15 +27,6 @@ namespace int const kNumBuckets = 256; double const kAnglesInBucket = 360.0 / kNumBuckets; -m2::PointD PointAtSegmentM(m2::PointD const & p1, m2::PointD const & p2, double const distanceM) -{ - auto const v = p2 - p1; - auto const l = v.Length(); - auto const L = MercatorBounds::DistanceOnEarth(p1, p2); - auto const delta = distanceM * l / L; - return PointAtSegment(p1, p2, delta); -} - uint32_t Bearing(m2::PointD const & a, m2::PointD const & b) { auto const angle = location::AngleToBearing(base::RadToDeg(ang::AngleTo(a, b))); @@ -43,65 +34,9 @@ uint32_t Bearing(m2::PointD const & a, m2::PointD const & b) CHECK_GREATER_OR_EQUAL(angle, 0, ("Angle should be greater than or equal to 0")); return base::clamp(angle / kAnglesInBucket, 0.0, 255.0); } - -// This class is used to get correct points for further bearing calculations. -// Depending on |isLastPoint| it either calculates those points straightforwardly -// or reverses directions and then calculates. -class BearingPointsSelector -{ -public: - BearingPointsSelector(uint32_t const bearDistM, bool const isLastPoint) - : m_bearDistM(bearDistM), m_isLastPoint(isLastPoint) - { - } - - m2::PointD GetBearingStartPoint(Graph::Edge const & e) const - { - return m_isLastPoint ? e.GetEndPoint() : e.GetStartPoint(); - } - - m2::PointD GetBearingEndPoint(Graph::Edge const & e, double const distanceM) - { - if (distanceM < m_bearDistM && m_bearDistM <= distanceM + EdgeLength(e)) - { - auto const edgeLen = EdgeLength(e); - auto const edgeBearDist = min(m_bearDistM - distanceM, edgeLen); - ASSERT_LESS_OR_EQUAL(edgeBearDist, edgeLen, ()); - return m_isLastPoint ? PointAtSegmentM(e.GetEndPoint(), e.GetStartPoint(), - static_cast(edgeBearDist)) - : PointAtSegmentM(e.GetStartPoint(), e.GetEndPoint(), - static_cast(edgeBearDist)); - } - return m_isLastPoint ? e.GetStartPoint() : e.GetEndPoint(); - } - -private: - double m_bearDistM; - bool m_isLastPoint; -}; } // namespace // CandidatePathsGetter::Link ---------------------------------------------------------------------- -bool CandidatePathsGetter::Link::operator<(Link const & o) const -{ - if (m_distanceM != o.m_distanceM) - return m_distanceM < o.m_distanceM; - - if (m_edge != o.m_edge) - return m_edge < o.m_edge; - - if (m_parent == o.m_parent) - return false; - - if (m_parent && o.m_parent) - return *m_parent < *o.m_parent; - - if (!m_parent) - return true; - - return false; -} - Graph::Edge CandidatePathsGetter::Link::GetStartEdge() const { auto * start = this; @@ -135,7 +70,7 @@ bool CandidatePathsGetter::GetLineCandidatesForPoints( if (i != points.size() - 1 && points[i].m_distanceToNextPoint == 0) { LOG(LINFO, ("Distance to next point is zero. Skipping the whole segment")); - ++m_stats.m_dnpIsZero; + ++m_stats.m_zeroDistToNextPointCount; return false; } @@ -249,7 +184,7 @@ void CandidatePathsGetter::GetBestCandidatePaths( BearingPointsSelector pointsSelector(bearDistM, isLastPoint); for (auto const & l : allPaths) { - auto const bearStartPoint = pointsSelector.GetBearingStartPoint(l->GetStartEdge()); + auto const bearStartPoint = pointsSelector.GetStartPoint(l->GetStartEdge()); auto const startPointsDistance = MercatorBounds::DistanceOnEarth(bearStartPoint, startPoint); // Number of edges counting from the last one to check bearing on. Accorfing to OpenLR spec @@ -273,7 +208,7 @@ void CandidatePathsGetter::GetBestCandidatePaths( --traceBackIterationsLeft; auto const bearEndPoint = - pointsSelector.GetBearingEndPoint(part->m_edge, part->m_distanceM); + pointsSelector.GetEndPoint(part->m_edge, part->m_distanceM); auto const bearing = Bearing(bearStartPoint, bearEndPoint); auto const bearingDiff = AbsDifference(bearing, requiredBearing); diff --git a/openlr/candidate_paths_getter.hpp b/openlr/candidate_paths_getter.hpp index c89ea30718..164e5a1632 100644 --- a/openlr/candidate_paths_getter.hpp +++ b/openlr/candidate_paths_getter.hpp @@ -47,7 +47,6 @@ private: { } - bool operator<(Link const & o) const; bool IsJunctionInPath(routing::Junction const & j) const; Graph::Edge GetStartEdge() const; diff --git a/openlr/candidate_points_getter.cpp b/openlr/candidate_points_getter.cpp index c9fe6c615a..f9db6ee245 100644 --- a/openlr/candidate_points_getter.cpp +++ b/openlr/candidate_points_getter.cpp @@ -12,8 +12,8 @@ using namespace routing; namespace openlr { -void CandidatePointsGetter::GetJunctionPointCandidates(m2::PointD const & p, - vector & candidates) +void CandidatePointsGetter::FillJunctionPointCandidates(m2::PointD const & p, + vector & candidates) { // TODO(mgsergio): Get optimal value using experiments on a sample. // Or start with small radius and scale it up when there are too few points. diff --git a/openlr/candidate_points_getter.hpp b/openlr/candidate_points_getter.hpp index 3b1e7de5f6..02c16c0b87 100644 --- a/openlr/candidate_points_getter.hpp +++ b/openlr/candidate_points_getter.hpp @@ -27,12 +27,12 @@ public: void GetCandidatePoints(m2::PointD const & p, std::vector & candidates) { - GetJunctionPointCandidates(p, candidates); + FillJunctionPointCandidates(p, candidates); EnrichWithProjectionPoints(p, candidates); } private: - void GetJunctionPointCandidates(m2::PointD const & p, std::vector & candidates); + void FillJunctionPointCandidates(m2::PointD const & p, std::vector & candidates); void EnrichWithProjectionPoints(m2::PointD const & p, std::vector & candidates); size_t const m_maxJunctionCandidates; diff --git a/openlr/graph.cpp b/openlr/graph.cpp index 36b763137d..6b76b6e418 100644 --- a/openlr/graph.cpp +++ b/openlr/graph.cpp @@ -67,11 +67,6 @@ void Graph::FindClosestEdges(m2::PointD const & point, uint32_t const count, m_graph.FindClosestEdges(point, count, vicinities); } -void Graph::AddFakeEdges(Junction const & junction, vector> const & vicinities) -{ - m_graph.AddFakeEdges(junction, vicinities); -} - void Graph::AddIngoingFakeEdge(Edge const & e) { m_graph.AddIngoingFakeEdge(e); @@ -81,4 +76,9 @@ void Graph::AddOutgoingFakeEdge(Edge const & e) { m_graph.AddOutgoingFakeEdge(e); } + +void Graph::GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const +{ + m_graph.GetFeatureTypes(featureId, types); +} } // namespace openlr diff --git a/openlr/graph.hpp b/openlr/graph.hpp index 9da97dee03..483ead2921 100644 --- a/openlr/graph.hpp +++ b/openlr/graph.hpp @@ -5,11 +5,14 @@ #include "routing_common/car_model.hpp" +#include "indexer/feature_data.hpp" + #include "geometry/point2d.hpp" #include #include #include +#include #include #include @@ -42,14 +45,13 @@ public: void FindClosestEdges(m2::PointD const & point, uint32_t const count, std::vector> & vicinities) const; - void AddFakeEdges(Junction const & junction, - std::vector> const & vicinities); - void AddIngoingFakeEdge(Edge const & e); void AddOutgoingFakeEdge(Edge const & e); void ResetFakes() { m_graph.ResetFakes(); } + void GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const; + private: routing::FeaturesRoadGraph m_graph; std::map m_outgoingCache; diff --git a/openlr/helpers.cpp b/openlr/helpers.cpp index 06df202162..1cca7b8670 100644 --- a/openlr/helpers.cpp +++ b/openlr/helpers.cpp @@ -6,12 +6,18 @@ #include "geometry/mercator.hpp" +#include #include #include #include +#include "boost/optional.hpp" + namespace { +using namespace openlr; +using namespace std; + openlr::FunctionalRoadClass HighwayClassToFunctionalRoadClass(ftypes::HighwayClass const & hwClass) { switch (hwClass) @@ -25,10 +31,86 @@ openlr::FunctionalRoadClass HighwayClassToFunctionalRoadClass(ftypes::HighwayCla default: return openlr::FunctionalRoadClass::FRC7; } } + +/// \returns boost::none if |e| doesn't conform to |functionalRoadClass| and score otherwise. +boost::optional GetFrcScore(Graph::Edge const & e, FunctionalRoadClass functionalRoadClass, + RoadInfoGetter & infoGetter) +{ + CHECK(!e.IsFake(), ()); + Score constexpr kMaxScoreForFrc = 25; + + if (functionalRoadClass == FunctionalRoadClass::NotAValue) + return boost::none; + + auto const hwClass = infoGetter.Get(e.GetFeatureId()).m_hwClass; + + switch (functionalRoadClass) + { + case FunctionalRoadClass::FRC0: + // Note. HighwayClass::Trunk means motorway, motorway_link, trunk or trunk_link. + return hwClass == ftypes::HighwayClass::Trunk ? boost::optional(kMaxScoreForFrc) + : boost::none; + + case FunctionalRoadClass::FRC1: + return (hwClass == ftypes::HighwayClass::Trunk || hwClass == ftypes::HighwayClass::Primary) + ? boost::optional(kMaxScoreForFrc) + : boost::none; + + case FunctionalRoadClass::FRC2: + case FunctionalRoadClass::FRC3: + if (hwClass == ftypes::HighwayClass::Secondary || hwClass == ftypes::HighwayClass::Tertiary) + return boost::optional(kMaxScoreForFrc); + + return hwClass == ftypes::HighwayClass::Primary || hwClass == ftypes::HighwayClass::LivingStreet + ? boost::optional(0) + : boost::none; + + case FunctionalRoadClass::FRC4: + if (hwClass == ftypes::HighwayClass::LivingStreet || hwClass == ftypes::HighwayClass::Service) + return boost::optional(kMaxScoreForFrc); + + return hwClass == ftypes::HighwayClass::Tertiary ? boost::optional(0) : boost::none; + + case FunctionalRoadClass::FRC5: + case FunctionalRoadClass::FRC6: + case FunctionalRoadClass::FRC7: + return hwClass == ftypes::HighwayClass::LivingStreet || hwClass == ftypes::HighwayClass::Service + ? boost::optional(kMaxScoreForFrc) + : boost::none; + + case FunctionalRoadClass::NotAValue: + UNREACHABLE(); + } + UNREACHABLE(); +} } // namespace namespace openlr { +// BearingPointsSelector --------------------------------------------------------------------------- +BearingPointsSelector::BearingPointsSelector(uint32_t bearDistM, bool isLastPoint) + : m_bearDistM(bearDistM), m_isLastPoint(isLastPoint) +{ +} + +m2::PointD BearingPointsSelector::GetStartPoint(Graph::Edge const & e) const +{ + return m_isLastPoint ? e.GetEndPoint() : e.GetStartPoint(); +} + +m2::PointD BearingPointsSelector::GetEndPoint(Graph::Edge const & e, double distanceM) +{ + if (distanceM < m_bearDistM && m_bearDistM <= distanceM + EdgeLength(e)) + { + auto const edgeLen = EdgeLength(e); + auto const edgeBearDist = min(m_bearDistM - distanceM, edgeLen); + CHECK_LESS_OR_EQUAL(edgeBearDist, edgeLen, ()); + return m_isLastPoint ? PointAtSegmentM(e.GetEndPoint(), e.GetStartPoint(), edgeBearDist) + : PointAtSegmentM(e.GetStartPoint(), e.GetEndPoint(), edgeBearDist); + } + return m_isLastPoint ? e.GetStartPoint() : e.GetEndPoint(); +} + bool PointsAreClose(m2::PointD const & p1, m2::PointD const & p2) { double const kMwmRoadCrossingRadiusMeters = routing::GetRoadCrossingRadiusMeters(); @@ -47,11 +129,11 @@ bool EdgesAreAlmostEqual(Graph::Edge const & e1, Graph::Edge const & e2) PointsAreClose(e1.GetEndPoint(), e2.GetEndPoint()); } -std::string LogAs2GisPath(Graph::EdgeVector const & path) +string LogAs2GisPath(Graph::EdgeVector const & path) { CHECK(!path.empty(), ("Paths should not be empty")); - std::ostringstream ost; + ostringstream ost; ost << "https://2gis.ru/moscow?queryState="; auto ll = MercatorBounds::ToLatLon(path.front().GetStartPoint()); @@ -69,9 +151,9 @@ std::string LogAs2GisPath(Graph::EdgeVector const & path) return ost.str(); } -std::string LogAs2GisPath(Graph::Edge const & e) { return LogAs2GisPath(Graph::EdgeVector({e})); } +string LogAs2GisPath(Graph::Edge const & e) { return LogAs2GisPath(Graph::EdgeVector({e})); } -bool PassesRestriction(Graph::Edge const & e, FunctionalRoadClass restriction, FormOfWay fow, +bool PassesRestriction(Graph::Edge const & e, FunctionalRoadClass restriction, FormOfWay formOfWay, int frcThreshold, RoadInfoGetter & infoGetter) { if (e.IsFake() || restriction == FunctionalRoadClass::NotAValue) @@ -81,6 +163,22 @@ bool PassesRestriction(Graph::Edge const & e, FunctionalRoadClass restriction, F return static_cast(frc) <= static_cast(restriction) + frcThreshold; } +bool PassesRestrictionV3(Graph::Edge const & e, FunctionalRoadClass functionalRoadClass, + FormOfWay formOfWay, RoadInfoGetter & infoGetter, Score & score) +{ + CHECK(!e.IsFake(), ("Edges should not be fake:", e)); + auto const frcScore = GetFrcScore(e, functionalRoadClass, infoGetter); + if (frcScore == boost::none) + return false; + + score = frcScore.get(); + Score constexpr kScoreForFormOfWay = 25; + if (formOfWay == FormOfWay::Roundabout && infoGetter.Get(e.GetFeatureId()).m_isRoundabout) + score += kScoreForFormOfWay; + + return true; +} + bool ConformLfrcnp(Graph::Edge const & e, FunctionalRoadClass lowestFrcToNextPoint, int frcThreshold, RoadInfoGetter & infoGetter) { @@ -90,4 +188,45 @@ bool ConformLfrcnp(Graph::Edge const & e, FunctionalRoadClass lowestFrcToNextPoi auto const frc = HighwayClassToFunctionalRoadClass(infoGetter.Get(e.GetFeatureId()).m_hwClass); return static_cast(frc) <= static_cast(lowestFrcToNextPoint) + frcThreshold; } + +bool ConformLfrcnpV3(Graph::Edge const & e, FunctionalRoadClass lowestFrcToNextPoint, + RoadInfoGetter & infoGetter) +{ + return GetFrcScore(e, lowestFrcToNextPoint, infoGetter) != boost::none; +} + +size_t IntersectionLen(Graph::EdgeVector a, Graph::EdgeVector b) +{ + sort(a.begin(), a.end()); + sort(b.begin(), b.end()); + return set_intersection(a.begin(), a.end(), b.begin(), b.end(), CounterIterator()).GetCount(); +} + +bool SuffixEqualsPrefix(Graph::EdgeVector const & a, Graph::EdgeVector const & b, size_t len) +{ + CHECK_LESS_OR_EQUAL(len, a.size(), ()); + CHECK_LESS_OR_EQUAL(len, b.size(), ()); + return equal(a.end() - len, a.end(), b.begin()); +} + +// Returns a length of the longest suffix of |a| that matches any prefix of |b|. +// Neither |a| nor |b| can contain several repetitions of any edge. +// Returns -1 if |a| intersection |b| is not equal to some suffix of |a| and some prefix of |b|. +int32_t PathOverlappingLen(Graph::EdgeVector const & a, Graph::EdgeVector const & b) +{ + auto const len = IntersectionLen(a, b); + if (SuffixEqualsPrefix(a, b, len)) + return base::checked_cast(len); + + return -1; +} + +m2::PointD PointAtSegmentM(m2::PointD const & p1, m2::PointD const & p2, double const distanceM) +{ + auto const v = p2 - p1; + auto const l = v.Length(); + auto const L = MercatorBounds::DistanceOnEarth(p1, p2); + auto const delta = distanceM * l / L; + return PointAtSegment(p1, p2, delta); +} } // namespace openlr diff --git a/openlr/helpers.hpp b/openlr/helpers.hpp index 6d51538e33..f0bdd51bd1 100644 --- a/openlr/helpers.hpp +++ b/openlr/helpers.hpp @@ -2,15 +2,32 @@ #include "openlr/graph.hpp" #include "openlr/openlr_model.hpp" +#include "openlr/score_types.hpp" #include "geometry/point2d.hpp" +#include +#include #include namespace openlr { class RoadInfoGetter; +// This class is used to get points for further bearing calculations. +class BearingPointsSelector +{ +public: + BearingPointsSelector(uint32_t bearDistM, bool isLastPoint); + + m2::PointD GetStartPoint(Graph::Edge const & e) const; + m2::PointD GetEndPoint(Graph::Edge const & e, double distanceM); + +private: + double m_bearDistM; + bool m_isLastPoint; +}; + bool PointsAreClose(m2::PointD const & p1, m2::PointD const & p2); double EdgeLength(Graph::Edge const & e); @@ -28,12 +45,29 @@ std::common_type_t AbsDifference(T const a, U const b) return a >= b ? a - b : b - a; } -bool PassesRestriction(Graph::Edge const & e, FunctionalRoadClass restriction, FormOfWay fow, +bool PassesRestriction(Graph::Edge const & e, FunctionalRoadClass restriction, FormOfWay formOfWay, int frcThreshold, RoadInfoGetter & infoGetter); +/// \returns true if |e| conforms |functionalRoadClass| and |formOfWay| and false otherwise. +/// \note If the method returns true |score| should be considered next. +bool PassesRestrictionV3(Graph::Edge const & e, FunctionalRoadClass functionalRoadClass, + FormOfWay formOfWay, RoadInfoGetter & infoGetter, Score & score); /// \returns true if edge |e| conforms Lowest Functional Road Class to Next Point. /// \note frc means Functional Road Class. Please see openlr documentation for details: /// http://www.openlr.org/data/docs/whitepaper/1_0/OpenLR-Whitepaper_v1.0.pdf bool ConformLfrcnp(Graph::Edge const & e, FunctionalRoadClass lowestFrcToNextPoint, int frcThreshold, RoadInfoGetter & infoGetter); +bool ConformLfrcnpV3(Graph::Edge const & e, FunctionalRoadClass lowestFrcToNextPoint, + RoadInfoGetter & infoGetter); + +size_t IntersectionLen(Graph::EdgeVector a, Graph::EdgeVector b); + +bool SuffixEqualsPrefix(Graph::EdgeVector const & a, Graph::EdgeVector const & b, size_t len); + +// Returns a length of the longest suffix of |a| that matches any prefix of |b|. +// Neither |a| nor |b| can contain several repetitions of any edge. +// Returns -1 if |a| intersection |b| is not equal to some suffix of |a| and some prefix of |b|. +int32_t PathOverlappingLen(Graph::EdgeVector const & a, Graph::EdgeVector const & b); + +m2::PointD PointAtSegmentM(m2::PointD const & p1, m2::PointD const & p2, double const distanceM); } // namespace openlr diff --git a/openlr/openlr_decoder.cpp b/openlr/openlr_decoder.cpp index 95d33b6519..db6ab13fe3 100644 --- a/openlr/openlr_decoder.cpp +++ b/openlr/openlr_decoder.cpp @@ -10,6 +10,10 @@ #include "openlr/paths_connector.hpp" #include "openlr/road_info_getter.hpp" #include "openlr/router.hpp" +#include "openlr/score_candidate_paths_getter.hpp" +#include "openlr/score_candidate_points_getter.hpp" +#include "openlr/score_paths_connector.hpp" +#include "openlr/score_types.hpp" #include "openlr/way_point.hpp" #include "routing/features_road_graph.hpp" @@ -17,11 +21,11 @@ #include "routing_common/car_model.hpp" +#include "storage/country_info_getter.hpp" + #include "indexer/classificator.hpp" #include "indexer/data_source.hpp" -#include "storage/country_info_getter.hpp" - #include "platform/country_file.hpp" #include "geometry/mercator.hpp" @@ -163,12 +167,13 @@ void ExpandFakes(DataSource const & dataSource, Graph & g, Graph::EdgeVector & p // Returns an iterator pointing to the first edge that should not be cut off. // Offsets denote a distance in meters one should travel from the start/end of the path -// to some point alog that path and drop everything form the start to that point or from +// to some point along that path and drop everything form the start to that point or from // that point to the end. template -InputIterator CutOffset(InputIterator start, InputIterator const stop, double const offset) +InputIterator CutOffset(InputIterator start, InputIterator stop, double offset, + bool keepEnd) { - if (offset == 0) + if (offset == 0.0) return start; for (double distance = 0.0; start != stop; ++start) @@ -176,8 +181,8 @@ InputIterator CutOffset(InputIterator start, InputIterator const stop, double co auto const edgeLen = EdgeLength(*start); if (distance <= offset && offset < distance + edgeLen) { - // Throw out this edge if (offest - distance) is greater than edgeLength / 2. - if (2 * (offset - distance) >= edgeLen) + // Throw out this edge if (offset - distance) is greater than edgeLength / 2. + if (!keepEnd && offset - distance >= edgeLen / 2.0) ++start; break; } @@ -188,21 +193,23 @@ InputIterator CutOffset(InputIterator start, InputIterator const stop, double co } template -void CopyWithoutOffsets(InputIterator const start, InputIterator const stop, OutputIterator out, - uint32_t const positiveOffset, uint32_t const negativeOffset) +void CopyWithoutOffsets(InputIterator start, InputIterator stop, OutputIterator out, + uint32_t positiveOffset, uint32_t negativeOffset, bool keepEnds) { auto from = start; auto to = stop; if (distance(start, stop) > 1) { - from = CutOffset(start, stop, positiveOffset); + from = CutOffset(start, stop, positiveOffset, keepEnds); // |to| points past the last edge we need to take. to = CutOffset(reverse_iterator(stop), reverse_iterator(start), - negativeOffset) - .base(); + negativeOffset, keepEnds).base(); } + if (!keepEnds) + CHECK(from <= to, ("From iterator is less or equal than to.")); + if (from >= to) return; @@ -288,9 +295,9 @@ public: bool DecodeSegment(LinearSegment const & segment, DecodedPath & path, v2::Stats & stat) { - double const kPathLengthTolerance = 0.30; - uint32_t const kMaxJunctionCandidates = 10; - uint32_t const kMaxProjectionCandidates = 5; + double constexpr kPathLengthTolerance = 0.30; + uint32_t constexpr kMaxJunctionCandidates = 10; + uint32_t constexpr kMaxProjectionCandidates = 5; m_graph.ResetFakes(); @@ -302,8 +309,8 @@ public: lineCandidates.reserve(points.size()); LOG(LDEBUG, ("Decoding segment:", segment.m_segmentId, "with", points.size(), "points")); - CandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates, m_dataSource, - m_graph); + CandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates, + m_dataSource, m_graph); CandidatePathsGetter pathsGetter(pointsGetter, m_graph, m_infoGetter, stat); if (!pathsGetter.GetLineCandidatesForPoints(points, lineCandidates)) @@ -346,7 +353,99 @@ public: ExpandFakes(m_dataSource, m_graph, route); ASSERT(none_of(begin(route), end(route), mem_fn(&Graph::Edge::IsFake)), (segment.m_segmentId)); CopyWithoutOffsets(begin(route), end(route), back_inserter(path.m_path), positiveOffsetM, - negativeOffsetM); + negativeOffsetM, false /* keep ends */); + + if (path.m_path.empty()) + { + ++stat.m_wrongOffsets; + LOG(LINFO, ("Path is empty after offsets cutting. segmentId:", segment.m_segmentId)); + return false; + } + + return true; + } + +private: + DataSource const & m_dataSource; + Graph m_graph; + RoadInfoGetter m_infoGetter; +}; + +// The idea behind the third version of matching algorithm is to collect a lot of candidates (paths) +// which correspond an openlr edges with some score. And on the final stage to decide which +// candidate is better. +class SegmentsDecoderV3 +{ +public: + SegmentsDecoderV3(DataSource const & dataSource, unique_ptr carModelFactory) + : m_dataSource(dataSource), m_graph(dataSource, move(carModelFactory)), m_infoGetter(dataSource) + { + } + + bool DecodeSegment(LinearSegment const & segment, DecodedPath & path, v2::Stats & stat) + { + LOG(LDEBUG, ("DecodeSegment(...) seg id:", segment.m_segmentId, ", point num:", segment.GetLRPs().size())); + + uint32_t constexpr kMaxJunctionCandidates = 10; + uint32_t constexpr kMaxProjectionCandidates = 5; + + path.m_segmentId.Set(segment.m_segmentId); + + auto const & points = segment.GetLRPs(); + CHECK_GREATER(points.size(), 1, ("A segment cannot consist of less than two points")); + vector lineCandidates; + lineCandidates.reserve(points.size()); + LOG(LDEBUG, ("Decoding segment:", segment.m_segmentId, "with", points.size(), "points")); + + ScoreCandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates, + m_dataSource, m_graph); + ScoreCandidatePathsGetter pathsGetter(pointsGetter, m_graph, m_infoGetter, stat); + + if (!pathsGetter.GetLineCandidatesForPoints(points, lineCandidates)) + return false; + + vector resultPath; + ScorePathsConnector connector(m_graph, m_infoGetter, stat); + if (!connector.FindBestPath(points, lineCandidates, resultPath)) + { + LOG(LINFO, ("Connections not found:", segment.m_segmentId)); + return false; + } + + Graph::EdgeVector route; + for (auto const & part : resultPath) + route.insert(route.end(), part.begin(), part.end()); + + double requiredRouteDistanceM = 0.0; + // Sum up all distances between points. Last point's m_distanceToNextPoint + // should be equal to zero, but let's skip it just in case. + CHECK(!points.empty(), ()); + for (auto it = points.begin(); it != prev(points.end()); ++it) + requiredRouteDistanceM += it->m_distanceToNextPoint; + + double actualRouteDistanceM = 0.0; + for (auto const & e : route) + actualRouteDistanceM += EdgeLength(e); + + auto const scale = actualRouteDistanceM / requiredRouteDistanceM; + LOG(LDEBUG, ("actualRouteDistance:", actualRouteDistanceM, + "requiredRouteDistance:", requiredRouteDistanceM, "scale:", scale)); + + if (segment.m_locationReference.m_positiveOffsetMeters + + segment.m_locationReference.m_negativeOffsetMeters >= + requiredRouteDistanceM) + { + ++stat.m_wrongOffsets; + LOG(LINFO, ("Wrong offsets for segment:", segment.m_segmentId)); + return false; + } + + auto const positiveOffsetM = segment.m_locationReference.m_positiveOffsetMeters * scale; + auto const negativeOffsetM = segment.m_locationReference.m_negativeOffsetMeters * scale; + + CHECK(none_of(route.begin(), route.end(), mem_fn(&Graph::Edge::IsFake)), (segment.m_segmentId)); + CopyWithoutOffsets(route.begin(), route.end(), back_inserter(path.m_path), positiveOffsetM, + negativeOffsetM, true /* keep ends */); if (path.m_path.empty()) { @@ -419,6 +518,12 @@ void OpenLRDecoder::DecodeV2(vector const & segments, uint32_t co Decode(segments, numThreads, paths); } +void OpenLRDecoder::DecodeV3(vector const & segments, uint32_t numThreads, + vector & paths) +{ + Decode(segments, numThreads, paths); +} + template void OpenLRDecoder::Decode(vector const & segments, uint32_t const numThreads, vector & paths) @@ -450,6 +555,7 @@ void OpenLRDecoder::Decode(vector const & segments, } }; + base::Timer timer; vector stats(numThreads); vector workers; for (size_t i = 1; i < numThreads; ++i) @@ -464,5 +570,6 @@ void OpenLRDecoder::Decode(vector const & segments, allStats.Add(s); allStats.Report(); + LOG(LINFO, ("Matching tool:", timer.ElapsedSeconds(), "seconds.")); } } // namespace openlr diff --git a/openlr/openlr_decoder.hpp b/openlr/openlr_decoder.hpp index 7faa4e0fc5..61d6ebfccc 100644 --- a/openlr/openlr_decoder.hpp +++ b/openlr/openlr_decoder.hpp @@ -50,6 +50,9 @@ public: void DecodeV2(std::vector const & segments, uint32_t const numThreads, std::vector & paths); + void DecodeV3(std::vector const & segments, uint32_t numThreads, + std::vector & paths); + private: template void Decode(std::vector const & segments, uint32_t const numThreads, diff --git a/openlr/openlr_stat/openlr_stat.cpp b/openlr/openlr_stat/openlr_stat.cpp index 3bf197c628..d630744049 100644 --- a/openlr/openlr_stat/openlr_stat.cpp +++ b/openlr/openlr_stat/openlr_stat.cpp @@ -110,8 +110,8 @@ bool ValidateLimit(char const * flagname, int32_t value) { if (value < -1) { - printf("Invalid value for --%s: %d, must be greater or equal to -1\n", flagname, - static_cast(value)); + LOG(LINFO, ("Valid value for --", std::string(flagname), ":", value, + "must be greater or equal to -1.")); return false; } @@ -122,9 +122,8 @@ bool ValidateNumThreads(char const * flagname, int32_t value) { if (value < kMinNumThreads || value > kMaxNumThreads) { - printf("Invalid value for --%s: %d, must be between %d and %d inclusively\n", flagname, - static_cast(value), static_cast(kMinNumThreads), - static_cast(kMaxNumThreads)); + LOG(LINFO, ("Valid value for --", std::string(flagname), ":", value, "must be between", + kMinNumThreads, "and", kMaxNumThreads)); return false; } @@ -135,7 +134,7 @@ bool ValidateMwmPath(char const * flagname, std::string const & value) { if (value.empty()) { - printf("--%s should be specified\n", flagname); + LOG(LINFO, ("--", std::string(flagname), "should be specified.")); return false; } @@ -146,13 +145,13 @@ bool ValidateVersion(char const * flagname, int32_t value) { if (value == 0) { - printf("--%s should be specified\n", flagname); + LOG(LINFO, ("--", std::string(flagname), "should be specified.")); return false; } - if (value != 1 && value != 2) + if (value != 1 && value != 2 && value != 3) { - printf("--%s should be one of 1 or 2\n", flagname); + LOG(LINFO, ("--", std::string(flagname), "should be one of 1, 2 or 3.")); return false; } @@ -282,7 +281,8 @@ int main(int argc, char * argv[]) { case 1: decoder.DecodeV1(segments, numThreads, paths); break; case 2: decoder.DecodeV2(segments, numThreads, paths); break; - default: ASSERT(false, ("There should be no way to fall here")); + case 3: decoder.DecodeV3(segments, numThreads, paths); break; + default: CHECK(false, ("Wrong algorithm version.")); } SaveNonMatchedIds(FLAGS_non_matched_ids, paths); diff --git a/openlr/paths_connector.cpp b/openlr/paths_connector.cpp index 10bc697eec..84cbde6b5f 100644 --- a/openlr/paths_connector.cpp +++ b/openlr/paths_connector.cpp @@ -1,4 +1,5 @@ #include "openlr/paths_connector.hpp" + #include "openlr/helpers.hpp" #include "base/checked_cast.hpp" @@ -15,31 +16,6 @@ namespace openlr { namespace { -size_t IntersectionLen(Graph::EdgeVector a, Graph::EdgeVector b) -{ - sort(begin(a), end(a)); - sort(begin(b), end(b)); - return set_intersection(begin(a), end(a), begin(b), end(b), CounterIterator()).GetCount(); -} - -bool PrefEqualsSuff(Graph::EdgeVector const & a, Graph::EdgeVector const & b, size_t const len) -{ - ASSERT_LESS_OR_EQUAL(len, a.size(), ()); - ASSERT_LESS_OR_EQUAL(len, b.size(), ()); - return equal(end(a) - len, end(a), begin(b)); -} - -// Returns a length of the longest suffix of |a| that matches any prefix of |b|. -// Neither |a| nor |b| can contain several repetitions of any edge. -// Returns -1 if |a| intersection |b| is not equal to some suffix of |a| and some prefix of |b|. -int32_t PathOverlappingLen(Graph::EdgeVector const & a, Graph::EdgeVector const & b) -{ - auto const len = IntersectionLen(a, b); - if (PrefEqualsSuff(a, b, len)) - return base::checked_cast(len); - return -1; -} - bool ValidatePath(Graph::EdgeVector const & path, double const distanceToNextPoint, double const pathLengthTolerance) diff --git a/openlr/road_info_getter.cpp b/openlr/road_info_getter.cpp index a54b52177b..26e7f029b7 100644 --- a/openlr/road_info_getter.cpp +++ b/openlr/road_info_getter.cpp @@ -13,6 +13,7 @@ RoadInfoGetter::RoadInfo::RoadInfo(FeatureType & ft) : m_hwClass(ftypes::GetHighwayClass(feature::TypesHolder(ft))) , m_link(ftypes::IsLinkChecker::Instance()(ft)) , m_oneWay(ftypes::IsOneWayChecker::Instance()(ft)) + , m_isRoundabout(ftypes::IsRoundAboutChecker::Instance()(ft)) { } diff --git a/openlr/road_info_getter.hpp b/openlr/road_info_getter.hpp index 94fe377b56..a5601e28b6 100644 --- a/openlr/road_info_getter.hpp +++ b/openlr/road_info_getter.hpp @@ -27,6 +27,7 @@ public: ftypes::HighwayClass m_hwClass = ftypes::HighwayClass::Undefined; bool m_link = false; bool m_oneWay = false; + bool m_isRoundabout = false; }; explicit RoadInfoGetter(DataSource const & dataSource); diff --git a/openlr/router.cpp b/openlr/router.cpp index 9152929977..3fbe51aa8b 100644 --- a/openlr/router.cpp +++ b/openlr/router.cpp @@ -41,83 +41,44 @@ uint32_t Bearing(m2::PointD const & a, m2::PointD const & b) CHECK_GREATER_OR_EQUAL(angle, 0, ("Angle should be greater than or equal to 0")); return base::clamp(angle / kAnglesInBucket, 0.0, 255.0); } - -class Score final -{ -public: - // A weight for total length of true fake edges. - static const int kTrueFakeCoeff = 10; - - // A weight for total length of fake edges that are parts of some - // real edges. - static constexpr double kFakeCoeff = 0.001; - - // A weight for passing too far from pivot points. - static const int kIntermediateErrorCoeff = 3; - - // A weight for excess of distance limit. - static const int kDistanceErrorCoeff = 3; - - // A weight for deviation from bearing. - static const int kBearingErrorCoeff = 5; - - void AddDistance(double p) { m_distance += p; } - - void AddFakePenalty(double p, bool partOfReal) - { - m_penalty += (partOfReal ? kFakeCoeff : kTrueFakeCoeff) * p; - } - - void AddIntermediateErrorPenalty(double p) { m_penalty += kIntermediateErrorCoeff * p; } - - void AddDistanceErrorPenalty(double p) { m_penalty += kDistanceErrorCoeff * p; } - - void AddBearingPenalty(int expected, int actual) - { - ASSERT_LESS(expected, kNumBuckets, ()); - ASSERT_GREATER_OR_EQUAL(expected, 0, ()); - - ASSERT_LESS(actual, kNumBuckets, ()); - ASSERT_GREATER_OR_EQUAL(actual, 0, ()); - - int const diff = abs(expected - actual); - double angle = base::DegToRad(std::min(diff, kNumBuckets - diff) * kAnglesInBucket); - m_penalty += kBearingErrorCoeff * angle * kBearingDist; - } - - double GetDistance() const { return m_distance; } - double GetPenalty() const { return m_penalty; } - double GetScore() const { return m_distance + m_penalty; } - - bool operator<(Score const & rhs) const - { - auto const ls = GetScore(); - auto const rs = rhs.GetScore(); - if (ls != rs) - return ls < rs; - - if (m_distance != rhs.m_distance) - return m_distance < rhs.m_distance; - return m_penalty < rhs.m_penalty; - } - - bool operator>(Score const & rhs) const { return rhs < *this; } - - bool operator==(Score const & rhs) const - { - return m_distance == rhs.m_distance && m_penalty == rhs.m_penalty; - } - - bool operator!=(Score const & rhs) const { return !(*this == rhs); } - -private: - // Reduced length of path in meters. - double m_distance = 0.0; - - double m_penalty = 0.0; -}; } // namespace +// Router::Vertex::Score --------------------------------------------------------------------------- +void Router::Vertex::Score::AddFakePenalty(double p, bool partOfReal) +{ + m_penalty += (partOfReal ? kFakeCoeff : kTrueFakeCoeff) * p; +} + +void Router::Vertex::Score::AddBearingPenalty(int expected, int actual) +{ + ASSERT_LESS(expected, kNumBuckets, ()); + ASSERT_GREATER_OR_EQUAL(expected, 0, ()); + + ASSERT_LESS(actual, kNumBuckets, ()); + ASSERT_GREATER_OR_EQUAL(actual, 0, ()); + + int const diff = abs(expected - actual); + double angle = base::DegToRad(std::min(diff, kNumBuckets - diff) * kAnglesInBucket); + m_penalty += kBearingErrorCoeff * angle * kBearingDist; +} + +bool Router::Vertex::Score::operator<(Score const & rhs) const +{ + auto const ls = GetScore(); + auto const rs = rhs.GetScore(); + if (ls != rs) + return ls < rs; + + if (m_distance != rhs.m_distance) + return m_distance < rhs.m_distance; + return m_penalty < rhs.m_penalty; +} + +bool Router::Vertex::Score::operator==(Score const & rhs) const +{ + return m_distance == rhs.m_distance && m_penalty == rhs.m_penalty; +} + // Router::Vertex ---------------------------------------------------------------------------------- Router::Vertex::Vertex(routing::Junction const & junction, routing::Junction const & stageStart, double stageStartDistance, size_t stage, bool bearingChecked) @@ -245,18 +206,19 @@ bool Router::Init(std::vector const & points, double positiveOffsetM, bool Router::FindPath(std::vector & path) { - using State = std::pair; + using State = std::pair; std::priority_queue, greater> queue; - std::map scores; + std::map scores; Links links; - auto pushVertex = [&queue, &scores, &links](Vertex const & u, Vertex const & v, Score const & sv, - Edge const & e) { - if ((scores.count(v) == 0 || scores[v].GetScore() > sv.GetScore() + kEps) && u != v) + auto const pushVertex = [&queue, &scores, &links](Vertex const & u, Vertex const & v, + Vertex::Score const & vertexScore, + Edge const & e) { + if ((scores.count(v) == 0 || scores[v].GetScore() > vertexScore.GetScore() + kEps) && u != v) { - scores[v] = sv; + scores[v] = vertexScore; links[v] = make_pair(u, e); - queue.emplace(sv, v); + queue.emplace(vertexScore, v); } }; @@ -264,7 +226,7 @@ bool Router::FindPath(std::vector & path) false /* bearingChecked */); CHECK(!NeedToCheckBearing(s, 0 /* distance */), ()); - scores[s] = Score(); + scores[s] = Vertex::Score(); queue.emplace(scores[s], s); double const piS = GetPotential(s); @@ -274,7 +236,7 @@ bool Router::FindPath(std::vector & path) auto const p = queue.top(); queue.pop(); - Score const & su = p.first; + Vertex::Score const & su = p.first; Vertex const & u = p.second; if (su != scores[u]) @@ -314,7 +276,7 @@ bool Router::FindPath(std::vector & path) { Vertex v = u; - Score sv = su; + Vertex::Score sv = su; if (u.m_junction != u.m_stageStart) { int const expected = m_points[stage].m_bearing; @@ -332,7 +294,7 @@ bool Router::FindPath(std::vector & path) false /* bearingChecked */); double const piV = GetPotential(v); - Score sv = su; + Vertex::Score sv = su; sv.AddDistance(std::max(piV - piU, 0.0)); sv.AddIntermediateErrorPenalty( MercatorBounds::DistanceOnEarth(v.m_junction.GetPoint(), m_points[v.m_stage].m_point)); @@ -353,7 +315,7 @@ bool Router::FindPath(std::vector & path) double const piV = GetPotential(v); - Score sv = su; + Vertex::Score sv = su; double const w = GetWeight(edge); sv.AddDistance(std::max(w + piV - piU, 0.0)); diff --git a/openlr/router.hpp b/openlr/router.hpp index 89ad635d4a..67f864f2fc 100644 --- a/openlr/router.hpp +++ b/openlr/router.hpp @@ -31,6 +31,47 @@ public: private: struct Vertex final { + class Score final + { + public: + // A weight for total length of true fake edges. + static const int kTrueFakeCoeff = 10; + + // A weight for total length of fake edges that are parts of some + // real edges. + static constexpr double kFakeCoeff = 0.001; + + // A weight for passing too far from pivot points. + static const int kIntermediateErrorCoeff = 3; + + // A weight for excess of distance limit. + static const int kDistanceErrorCoeff = 3; + + // A weight for deviation from bearing. + static const int kBearingErrorCoeff = 5; + + void AddDistance(double p) { m_distance += p; } + void AddFakePenalty(double p, bool partOfReal); + void AddIntermediateErrorPenalty(double p) { m_penalty += kIntermediateErrorCoeff * p; } + void AddDistanceErrorPenalty(double p) { m_penalty += kDistanceErrorCoeff * p; } + void AddBearingPenalty(int expected, int actual); + + double GetDistance() const { return m_distance; } + double GetPenalty() const { return m_penalty; } + double GetScore() const { return m_distance + m_penalty; } + + bool operator<(Score const & rhs) const; + bool operator>(Score const & rhs) const { return rhs < *this; } + bool operator==(Score const & rhs) const; + bool operator!=(Score const & rhs) const { return !(*this == rhs); } + + private: + // Reduced length of path in meters. + double m_distance = 0.0; + + double m_penalty = 0.0; + }; + Vertex() = default; Vertex(routing::Junction const & junction, routing::Junction const & stageStart, double stageStartDistance, size_t stage, bool bearingChecked); diff --git a/openlr/score_candidate_paths_getter.cpp b/openlr/score_candidate_paths_getter.cpp new file mode 100644 index 0000000000..a035d892d6 --- /dev/null +++ b/openlr/score_candidate_paths_getter.cpp @@ -0,0 +1,288 @@ +#include "openlr/score_candidate_paths_getter.hpp" + +#include "openlr/graph.hpp" +#include "openlr/helpers.hpp" +#include "openlr/openlr_model.hpp" +#include "openlr/score_candidate_points_getter.hpp" + +#include "routing/road_graph.hpp" + +#include "platform/location.hpp" + +#include "geometry/angles.hpp" +#include "geometry/mercator.hpp" + +#include "base/logging.hpp" +#include "base/stl_helpers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace routing; +using namespace std; + +namespace openlr +{ +namespace +{ +int constexpr kNumBuckets = 256; +double constexpr kAnglesInBucket = 360.0 / kNumBuckets; + +double ToAngleInDeg(uint32_t angleInBuckets) +{ + CHECK_GREATER_OR_EQUAL(angleInBuckets, 0, ()); + CHECK_LESS_OR_EQUAL(angleInBuckets, 255, ()); + return base::clamp(kAnglesInBucket * static_cast(angleInBuckets), 0.0, 360.0); +} + +uint32_t BearingInDeg(m2::PointD const & a, m2::PointD const & b) +{ + auto const angle = location::AngleToBearing(base::RadToDeg(ang::AngleTo(a, b))); + CHECK_LESS_OR_EQUAL(angle, 360.0, ()); + CHECK_GREATER_OR_EQUAL(angle, 0.0, ()); + return angle; +} + +double DifferenceInDeg(double a1, double a2) +{ + auto const diff = 180.0 - abs(abs(a1 - a2) - 180.0); + CHECK_LESS_OR_EQUAL(diff, 180.0, ()); + CHECK_GREATER_OR_EQUAL(diff, 0.0, ()); + return diff; +} +} // namespace + +// ScoreCandidatePathsGetter::Link ---------------------------------------------------------------------- +Graph::Edge ScoreCandidatePathsGetter::Link::GetStartEdge() const +{ + auto * start = this; + while (start->m_parent) + start = start->m_parent.get(); + + return start->m_edge; +} + +bool ScoreCandidatePathsGetter::Link::IsJunctionInPath(Junction const & j) const +{ + for (auto * l = this; l; l = l->m_parent.get()) + { + if (l->m_edge.GetEndJunction() == j) + { + LOG(LDEBUG, ("A loop detected, skipping...")); + return true; + } + } + + return false; +} + +// ScoreCandidatePathsGetter ---------------------------------------------------------------------------- +bool ScoreCandidatePathsGetter::GetLineCandidatesForPoints( + vector const & points, vector & lineCandidates) +{ + CHECK_GREATER(points.size(), 1, ()); + + for (size_t i = 0; i < points.size(); ++i) + { + if (i != points.size() - 1 && points[i].m_distanceToNextPoint == 0) + { + LOG(LINFO, ("Distance to next point is zero. Skipping the whole segment")); + ++m_stats.m_zeroDistToNextPointCount; + return false; + } + + lineCandidates.emplace_back(); + auto const isLastPoint = i == points.size() - 1; + double const distanceToNextPointM = + (isLastPoint ? points[i - 1] : points[i]).m_distanceToNextPoint; + + ScoreEdgeVec edgesCandidates; + m_pointsGetter.GetEdgeCandidates(MercatorBounds::FromLatLon(points[i].m_latLon), + isLastPoint, edgesCandidates); + + GetLineCandidates(points[i], isLastPoint, distanceToNextPointM, edgesCandidates, + lineCandidates.back()); + + if (lineCandidates.back().empty()) + { + LOG(LINFO, ("No candidate lines found for point", points[i].m_latLon, "Giving up")); + ++m_stats.m_noCandidateFound; + return false; + } + } + + CHECK_EQUAL(lineCandidates.size(), points.size(), ()); + return true; +} + +void ScoreCandidatePathsGetter::GetAllSuitablePaths(ScoreEdgeVec const & startLines, + bool isLastPoint, double bearDistM, + FunctionalRoadClass functionalRoadClass, + FormOfWay formOfWay, + vector> & allPaths) +{ + queue> q; + + for (auto const & e : startLines) + { + Score roadScore = 0; // Score based on functional road class and form of way. + if (!PassesRestrictionV3(e.m_edge, functionalRoadClass, formOfWay, m_infoGetter, roadScore)) + continue; + + q.push( + make_shared(nullptr /* parent */, e.m_edge, 0 /* distanceM */, e.m_score, roadScore)); + } + + // Filling |allPaths| staring from |startLines| which have passed functional road class + // and form of way restrictions. All paths in |allPaths| are shorter then |bearDistM| plus + // one segment length. + while (!q.empty()) + { + auto const u = q.front(); + q.pop(); + + auto const & currentEdge = u->m_edge; + auto const currentEdgeLen = EdgeLength(currentEdge); + + if (u->m_distanceM + currentEdgeLen >= bearDistM) + { + allPaths.emplace_back(move(u)); + continue; + } + + CHECK_LESS(u->m_distanceM + currentEdgeLen, bearDistM, ()); + + Graph::EdgeVector edges; + if (!isLastPoint) + m_graph.GetOutgoingEdges(currentEdge.GetEndJunction(), edges); + else + m_graph.GetIngoingEdges(currentEdge.GetStartJunction(), edges); + + for (auto const & e : edges) + { + CHECK(!e.IsFake(), ()); + + if (EdgesAreAlmostEqual(e.GetReverseEdge(), currentEdge)) + continue; + + CHECK(currentEdge.HasRealPart(), ()); + + Score roadScore = 0; + if (!PassesRestrictionV3(e, functionalRoadClass, formOfWay, m_infoGetter, roadScore)) + continue; + + if (u->IsJunctionInPath(e.GetEndJunction())) + continue; + + // Road score for a path is minimum value of score of segments based on functional road class + // of the segments and form of way of the segments. + q.emplace(make_shared(u, e, u->m_distanceM + currentEdgeLen, u->m_pointScore, + min(roadScore, u->m_minRoadScore))); + } + } +} + +void ScoreCandidatePathsGetter::GetBestCandidatePaths( + vector> const & allPaths, bool isLastPoint, uint32_t requiredBearing, + double bearDistM, m2::PointD const & startPoint, ScorePathVec & candidates) +{ + CHECK_GREATER_OR_EQUAL(requiredBearing, 0, ()); + CHECK_LESS_OR_EQUAL(requiredBearing, 255, ()); + + multiset> candidatePaths; + + BearingPointsSelector pointsSelector(static_cast(bearDistM), isLastPoint); + for (auto const & link : allPaths) + { + auto const bearStartPoint = pointsSelector.GetStartPoint(link->GetStartEdge()); + + // Number of edges counting from the last one to check bearing on. According to OpenLR spec + // we have to check bearing on a point that is no longer than 25 meters traveling down the path. + // But sometimes we may skip the best place to stop and generate a candidate. So we check several + // edges before the last one to avoid such a situation. Number of iterations is taken + // by intuition. + // Example: + // o -------- o { Partners segment. } + // o ------- o --- o { Our candidate. } + // ^ 25m + // ^ This one may be better than + // ^ this one. + // So we want to check them all. + uint32_t traceBackIterationsLeft = 3; + for (auto part = link; part; part = part->m_parent) + { + if (traceBackIterationsLeft == 0) + break; + + --traceBackIterationsLeft; + + auto const bearEndPoint = pointsSelector.GetEndPoint(part->m_edge, part->m_distanceM); + + auto const bearingDeg = BearingInDeg(bearStartPoint, bearEndPoint); + double const requiredBearingDeg = ToAngleInDeg(requiredBearing); + double const angleDeviationDeg = DifferenceInDeg(bearingDeg, requiredBearingDeg); + + // If the bearing according to osm segments (|bearingDeg|) is significantly different + // from the bearing set in openlr (|requiredBearingDeg|) the candidate should be skipped. + double constexpr kMinAngleDeviationDeg = 50.0; + if (angleDeviationDeg > kMinAngleDeviationDeg) + continue; + + double constexpr kMaxScoreForBearing = 60.0; + double constexpr kAngleDeviationFactor = 4.3; + auto const bearingScore = static_cast( + kMaxScoreForBearing / (1.0 + angleDeviationDeg / kAngleDeviationFactor)); + candidatePaths.emplace(part, part->m_pointScore, part->m_minRoadScore, bearingScore); + } + } + + size_t constexpr kMaxCandidates = 7; + vector paths; + copy_n(candidatePaths.begin(), min(static_cast(kMaxCandidates), candidatePaths.size()), + back_inserter(paths)); + + for (auto const & path : paths) + { + Graph::EdgeVector edges; + for (auto * p = path.m_path.get(); p; p = p->m_parent.get()) + edges.push_back(p->m_edge); + + if (!isLastPoint) + reverse(edges.begin(), edges.end()); + + candidates.emplace_back(path.GetScore(), move(edges)); + } +} + +void ScoreCandidatePathsGetter::GetLineCandidates(openlr::LocationReferencePoint const & p, + bool isLastPoint, + double distanceToNextPointM, + ScoreEdgeVec const & edgeCandidates, + ScorePathVec & candidates) +{ + double constexpr kDefaultBearDistM = 25.0; + double const bearDistM = min(kDefaultBearDistM, distanceToNextPointM); + + ScoreEdgeVec const & startLines = edgeCandidates; + LOG(LDEBUG, ("Listing start lines:")); + for (auto const & e : startLines) + LOG(LDEBUG, (LogAs2GisPath(e.m_edge))); + + auto const startPoint = MercatorBounds::FromLatLon(p.m_latLon); + + vector> allPaths; + GetAllSuitablePaths(startLines, isLastPoint, bearDistM, p.m_functionalRoadClass, p.m_formOfWay, + allPaths); + + GetBestCandidatePaths(allPaths, isLastPoint, p.m_bearing, bearDistM, startPoint, candidates); + // Sorting by increasing order. + sort(candidates.begin(), candidates.end(), + [](ScorePath const & s1, ScorePath const & s2) { return s1.m_score > s2.m_score; }); + LOG(LDEBUG, (candidates.size(), "Candidate paths found for point:", p.m_latLon)); +} +} // namespace openlr diff --git a/openlr/score_candidate_paths_getter.hpp b/openlr/score_candidate_paths_getter.hpp new file mode 100644 index 0000000000..cef4b1ee4c --- /dev/null +++ b/openlr/score_candidate_paths_getter.hpp @@ -0,0 +1,115 @@ +#pragma once + +#include "openlr/graph.hpp" +#include "openlr/openlr_model.hpp" +#include "openlr/road_info_getter.hpp" +#include "openlr/score_types.hpp" +#include "openlr/stats.hpp" + +#include "geometry/point2d.hpp" + +#include "base/assert.hpp" + +#include +#include +#include +#include + +namespace openlr +{ +class ScoreCandidatePointsGetter; + +class ScoreCandidatePathsGetter +{ +public: + ScoreCandidatePathsGetter(ScoreCandidatePointsGetter & pointsGetter, Graph & graph, + RoadInfoGetter & infoGetter, v2::Stats & stat) + : m_pointsGetter(pointsGetter), m_graph(graph), m_infoGetter(infoGetter), m_stats(stat) + { + } + + bool GetLineCandidatesForPoints(std::vector const & points, + std::vector & lineCandidates); + +private: + struct Link + { + Link(std::shared_ptr const & parent, Graph::Edge const & edge, double distanceM, + Score pointScore, Score rfcScore) + : m_parent(parent) + , m_edge(edge) + , m_distanceM(distanceM) + , m_pointScore(pointScore) + , m_minRoadScore(rfcScore) + { + CHECK(!edge.IsFake(), ("Edge should not be fake:", edge)); + } + + bool IsJunctionInPath(routing::Junction const & j) const; + + Graph::Edge GetStartEdge() const; + + std::shared_ptr const m_parent; + Graph::Edge const m_edge; + double const m_distanceM; + Score const m_pointScore; + // Minimum score of segments of the path going along |m_parent| based on functional road class + // and form of way. + Score const m_minRoadScore; + }; + + struct CandidatePath + { + CandidatePath() = default; + + CandidatePath(std::shared_ptr const path, Score pointScore, Score rfcScore, Score bearingScore) + : m_path(path) + , m_pointScore(pointScore) + , m_roadScore(rfcScore) + , m_bearingScore(bearingScore) + { + } + + bool operator>(CandidatePath const & o) const { return GetScore() > o.GetScore(); } + + Score GetScore() const { return m_pointScore + m_roadScore + m_bearingScore; } + + std::shared_ptr m_path = nullptr; + Score m_pointScore = 0; + Score m_roadScore = 0; + Score m_bearingScore = 0; + }; + + // Note: In all methods below if |isLastPoint| is true than algorithm should + // calculate all parameters (such as bearing, distance to next point, etc.) + // relative to the last point. + // o ----> o ----> o <---- o. + // 1 2 3 4 + // ^ isLastPoint = true. + // To calculate bearing for points 1 to 3 one have to go beardist from + // previous point to the next one (eg. from 1 to 2 and from 2 to 3). + // For point 4 one have to go from 4 to 3 reversing directions. And + // distance-to-next point is taken from point 3. You can learn more in + // TomTom OpenLR spec. + + /// \brief Fills |allPaths| with paths near start or finish point starting from |startLines|. + /// To extract a path from |allPaths| a item from |allPaths| should be taken, + /// then should be taken the member |m_parent| of the item and so on till the beginning. + void GetAllSuitablePaths(ScoreEdgeVec const & startLines, bool isLastPoint, + double bearDistM, FunctionalRoadClass functionalRoadClass, + FormOfWay formOfWay, std::vector> & allPaths); + + void GetBestCandidatePaths(std::vector> const & allPaths, bool isLastPoint, + uint32_t requiredBearing, double bearDistM, + m2::PointD const & startPoint, ScorePathVec & candidates); + + void GetLineCandidates(openlr::LocationReferencePoint const & p, bool isLastPoint, + double distanceToNextPointM, ScoreEdgeVec const & edgeCandidates, + ScorePathVec & candidates); + + ScoreCandidatePointsGetter & m_pointsGetter; + Graph & m_graph; + RoadInfoGetter & m_infoGetter; + v2::Stats & m_stats; +}; +} // namespace openlr diff --git a/openlr/score_candidate_points_getter.cpp b/openlr/score_candidate_points_getter.cpp new file mode 100644 index 0000000000..c6bafdb08b --- /dev/null +++ b/openlr/score_candidate_points_getter.cpp @@ -0,0 +1,135 @@ +#include "openlr/score_candidate_points_getter.hpp" + +#include "openlr/helpers.hpp" + +#include "routing/road_graph.hpp" +#include "routing/routing_helpers.hpp" + +#include "storage/country_info_getter.hpp" + +#include "indexer/feature.hpp" +#include "indexer/scales.hpp" + +#include "geometry/mercator.hpp" + +#include "base/assert.hpp" +#include "base/stl_helpers.hpp" + +#include +#include +#include + +using namespace routing; + +namespace +{ +// Ends of segments and intermediate points of segments are considered only within this radius. +double const kRadius = 30.0; +} // namespace + +namespace openlr +{ +void ScoreCandidatePointsGetter::GetJunctionPointCandidates(m2::PointD const & p, bool isLastPoint, + ScoreEdgeVec & edgeCandidates) +{ + ScorePointVec pointCandidates; + auto const selectCandidates = [&p, &pointCandidates, this](FeatureType & ft) { + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + ft.ForEachPoint( + [&p, &pointCandidates, this](m2::PointD const & candidate) { + if (MercatorBounds::DistanceOnEarth(p, candidate) < kRadius) + pointCandidates.emplace_back(GetScoreByDistance(p, candidate), candidate); + }, + scales::GetUpperScale()); + }; + + m_dataSource.ForEachInRect(selectCandidates, + MercatorBounds::RectByCenterXYAndSizeInMeters(p, kRadius), + scales::GetUpperScale()); + + base::SortUnique(pointCandidates); + std::reverse(pointCandidates.begin(), pointCandidates.end()); + + pointCandidates.resize(min(m_maxJunctionCandidates, pointCandidates.size())); + + for (auto const & pc : pointCandidates) + { + Graph::EdgeVector edges; + if (!isLastPoint) + m_graph.GetOutgoingEdges(Junction(pc.m_point, 0 /* altitude */), edges); + else + m_graph.GetIngoingEdges(Junction(pc.m_point, 0 /* altitude */), edges); + + for (auto const & e : edges) + edgeCandidates.emplace_back(pc.m_score, e); + } +} + +void ScoreCandidatePointsGetter::EnrichWithProjectionPoints(m2::PointD const & p, + ScoreEdgeVec & edgeCandidates) +{ + m_graph.ResetFakes(); + + vector> vicinities; + m_graph.FindClosestEdges(p, static_cast(m_maxProjectionCandidates), vicinities); + for (auto const & v : vicinities) + { + auto const & edge = v.first; + auto const & proj = v.second; + + CHECK(edge.HasRealPart(), ()); + CHECK(!edge.IsFake(), ()); + + if (MercatorBounds::DistanceOnEarth(p, proj.GetPoint()) >= kRadius) + continue; + + edgeCandidates.emplace_back(GetScoreByDistance(p, proj.GetPoint()), edge); + } +} + +bool ScoreCandidatePointsGetter::IsJunction(m2::PointD const & p) +{ + Graph::EdgeVector outgoing; + m_graph.GetRegularOutgoingEdges(Junction(p, 0 /* altitude */), outgoing); + + Graph::EdgeVector ingoing; + m_graph.GetRegularIngoingEdges(Junction(p, 0 /* altitude */), ingoing); + + // Note. At mwm borders the size of |ids| may be bigger than two in case of straight + // road because of road feature duplication at borders. + std::set> ids; + for (auto const & e : outgoing) + ids.insert(std::make_pair(e.GetFeatureId().m_index, e.GetSegId())); + + for (auto const & e : ingoing) + ids.insert(std::make_pair(e.GetFeatureId().m_index, e.GetSegId())); + + // Size of |ids| is number of different pairs of (feature id, segment id) starting from + // |p| plus going to |p|. The size 0, 1 or 2 is considered |p| is not a junction of roads. + // If the size is 3 or more it means |p| is an intersection of 3 or more roads. + return ids.size() >= 3; +} + +Score ScoreCandidatePointsGetter::GetScoreByDistance(m2::PointD const & point, + m2::PointD const & candidate) +{ + // Maximum possible score for the distance between an openlr segment ends and an osm segments. + Score constexpr kMaxScoreForDist = 70; + // If the distance between an openlr segments end and an osm segments end is less or equal + // |kMaxScoreDistM| the point gets |kMaxScoreForDist| score. + double constexpr kMaxScoreDistM = 5.0; + // According to the standard openlr edge should be started from a junction. Despite the fact + // that openlr and osm are based on different graphs, the score of junction should be increased. + double const junctionFactor = IsJunction(candidate) ? 1.1 : 1.0; + + double const distM = MercatorBounds::DistanceOnEarth(point, candidate); + double const score = + distM <= kMaxScoreDistM + ? kMaxScoreForDist * junctionFactor + : static_cast(kMaxScoreForDist) * junctionFactor / (1.0 + distM - kMaxScoreDistM); + + CHECK_GREATER_OR_EQUAL(score, 0.0, ()); + CHECK_LESS_OR_EQUAL(score, static_cast(kMaxScoreForDist) * junctionFactor, ()); + return static_cast(score); +} +} // namespace openlr diff --git a/openlr/score_candidate_points_getter.hpp b/openlr/score_candidate_points_getter.hpp new file mode 100644 index 0000000000..3c8c964ec3 --- /dev/null +++ b/openlr/score_candidate_points_getter.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include "openlr/graph.hpp" +#include "openlr/score_types.hpp" +#include "openlr/stats.hpp" + +#include "indexer/data_source.hpp" + +#include "geometry/point2d.hpp" + +#include +#include +#include + +namespace openlr +{ +class ScoreCandidatePointsGetter +{ +public: + ScoreCandidatePointsGetter(size_t maxJunctionCandidates, size_t maxProjectionCandidates, + DataSource const & dataSource, Graph & graph) + : m_maxJunctionCandidates(maxJunctionCandidates) + , m_maxProjectionCandidates(maxProjectionCandidates) + , m_dataSource(dataSource) + , m_graph(graph) + { + } + + void GetEdgeCandidates(m2::PointD const & p, bool isLastPoint, ScoreEdgeVec & edges) + { + GetJunctionPointCandidates(p, isLastPoint, edges); + EnrichWithProjectionPoints(p, edges); + } + +private: + void GetJunctionPointCandidates(m2::PointD const & p, bool isLastPoint, + ScoreEdgeVec & edgeCandidates); + void EnrichWithProjectionPoints(m2::PointD const & p, ScoreEdgeVec & edgeCandidates); + + /// \returns true if |p| is a junction and false otherwise. + bool IsJunction(m2::PointD const & p); + Score GetScoreByDistance(m2::PointD const & point, m2::PointD const & candidate); + + size_t const m_maxJunctionCandidates; + size_t const m_maxProjectionCandidates; + + DataSource const & m_dataSource; + Graph & m_graph; +}; +} // namespace openlr diff --git a/openlr/score_paths_connector.cpp b/openlr/score_paths_connector.cpp new file mode 100644 index 0000000000..bf2b427062 --- /dev/null +++ b/openlr/score_paths_connector.cpp @@ -0,0 +1,341 @@ +#include "openlr/score_paths_connector.hpp" + +#include "openlr/helpers.hpp" + +#include "indexer/feature_data.hpp" +#include "indexer/ftypes_matcher.hpp" + +#include "base/assert.hpp" +#include "base/checked_cast.hpp" +#include "base/stl_helpers.hpp" +#include "base/stl_iterator.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace openlr +{ +namespace +{ +class EdgeContainer +{ +public: + explicit EdgeContainer(Graph const & graph) : m_graph(graph) {} + + void ProcessEdge(Graph::Edge const & edge) + { + CHECK(!edge.IsFake(), ()); + + feature::TypesHolder types; + m_graph.GetFeatureTypes(edge.GetFeatureId(), types); + + ftypes::HighwayClass const hwClass = ftypes::GetHighwayClass(types); + if (m_minHwClass == ftypes::HighwayClass::Undefined) + { + m_minHwClass = hwClass; + m_maxHwClass = m_minHwClass; + m_oneWay.m_field = ftypes::IsOneWayChecker::Instance()(types); + m_roundabout.m_field = ftypes::IsRoundAboutChecker::Instance()(types); + m_link.m_field = ftypes::IsLinkChecker::Instance()(types); + } + else + { + m_minHwClass = static_cast( + min(base::Underlying(m_minHwClass), base::Underlying(hwClass))); + m_maxHwClass = static_cast( + max(base::Underlying(m_maxHwClass), base::Underlying(hwClass))); + + if (m_oneWay.m_allEqual && m_oneWay.m_field != ftypes::IsOneWayChecker::Instance()(types)) + m_oneWay.m_allEqual = false; + + if (m_roundabout.m_allEqual && m_roundabout.m_field != ftypes::IsRoundAboutChecker::Instance()(types)) + m_roundabout.m_allEqual = false; + + if (m_link.m_allEqual && m_link.m_field != ftypes::IsLinkChecker::Instance()(types)) + m_link.m_allEqual = false; + } + } + + uint8_t GetHighwayClassDiff() const + { + CHECK_NOT_EQUAL(m_minHwClass, ftypes::HighwayClass::Undefined, ()); + CHECK_GREATER_OR_EQUAL(m_maxHwClass, m_minHwClass, ()); + + uint8_t const hwClassDiff = static_cast(m_maxHwClass) - static_cast(m_minHwClass); + return hwClassDiff; + } + + bool AreAllOneWaysEqual() const { return m_oneWay.m_allEqual; } + bool AreAllRoundaboutEqual() const { return m_roundabout.m_allEqual; } + bool AreAllLinksEqual() const { return m_link.m_allEqual; } + +private: + struct Field + { + bool m_field = false; + bool m_allEqual = true; + }; + + Graph const & m_graph; + + ftypes::HighwayClass m_minHwClass = ftypes::HighwayClass::Undefined; + ftypes::HighwayClass m_maxHwClass = ftypes::HighwayClass::Undefined; + Field m_oneWay; + Field m_roundabout; + Field m_link; +}; + +/// \returns true if |path| may be used as a candidate. In that case |lenScore| is filled +/// with score of this candidate based on length. The closer length of the |path| to +/// |distanceToNextPoint| the more score. +bool ValidatePathByLength(Graph::EdgeVector const & path, double distanceToNextPoint, + Score & lenScore) +{ + CHECK(!path.empty(), ()); + Score const kMaxScoreForRouteLen = 110; + + double pathLen = 0.0; + for (auto const & e : path) + pathLen += EdgeLength(e); + + // 0 <= |pathDiffRatio| <= 1. The more pathDiffRatio the closer |distanceToNextPoint| and |pathLen|. + double const pathDiffRatio = + 1.0 - abs(distanceToNextPoint - pathLen) / max(distanceToNextPoint, pathLen); + + bool const shortPath = path.size() <= 2; + double constexpr kMinValidPathDiffRation = 0.6; + if (pathDiffRatio <= kMinValidPathDiffRation && !shortPath) + return false; + + lenScore = static_cast(kMaxScoreForRouteLen * (pathDiffRatio - kMinValidPathDiffRation) / + (1.0 - kMinValidPathDiffRation)); + + return true; +} +} // namespace + +ScorePathsConnector::ScorePathsConnector(Graph & graph, RoadInfoGetter & infoGetter, v2::Stats & stat) + : m_graph(graph), m_infoGetter(infoGetter), m_stat(stat) +{ +} + +bool ScorePathsConnector::FindBestPath(vector const & points, + vector> const & lineCandidates, + vector & resultPath) +{ + CHECK_GREATER_OR_EQUAL(points.size(), 2, ()); + + resultPath.resize(points.size() - 1); + + for (size_t i = 1; i < points.size(); ++i) + { + // @TODO It's possible that size of |points| is more then two. In that case two or more edges + // should be approximated with routes. If so only |toCandidates| which may be reached from + // |fromCandidates| should be used for the next edge. + + auto const & point = points[i - 1]; + auto const distanceToNextPoint = static_cast(point.m_distanceToNextPoint); + auto const & fromCandidates = lineCandidates[i - 1]; + auto const & toCandidates = lineCandidates[i]; + auto & resultPathPart = resultPath[i - 1]; + + vector result; + for (size_t fromInd = 0; fromInd < fromCandidates.size(); ++fromInd) + { + for (size_t toInd = 0; toInd < toCandidates.size(); ++toInd) + { + Graph::EdgeVector path; + if (!ConnectAdjacentCandidateLines(fromCandidates[fromInd].m_path, + toCandidates[toInd].m_path, point.m_lfrcnp, + distanceToNextPoint, path)) + { + continue; + } + + Score pathLenScore = 0; + if (!ValidatePathByLength(path, distanceToNextPoint, pathLenScore)) + continue; + + auto const score = pathLenScore + GetScoreForUniformity(path) + + fromCandidates[fromInd].m_score + toCandidates[toInd].m_score; + result.emplace_back(score, move(path)); + } + } + + for (auto const & p : result) + CHECK(!p.m_path.empty(), ()); + + if (result.empty()) + { + LOG(LINFO, ("No shortest path found")); + ++m_stat.m_noShortestPathFound; + resultPathPart.clear(); + return false; + } + + auto const it = max_element( + result.cbegin(), result.cend(), + [](ScorePath const & o1, ScorePath const & o2) { return o1.m_score < o2.m_score; }); + + Score constexpr kMinValidScore = 240; + if (it->m_score < kMinValidScore) + { + LOG(LINFO, ("The shortest path found but it is no good.")); + return false; + } + + resultPathPart = it->m_path; + LOG(LDEBUG, ("Best score:", it->m_score, "resultPathPart.size():", resultPathPart.size())); + } + + CHECK_EQUAL(resultPath.size(), points.size() - 1, ()); + + return true; +} + +bool ScorePathsConnector::FindShortestPath(Graph::Edge const & from, Graph::Edge const & to, + FunctionalRoadClass lowestFrcToNextPoint, + uint32_t maxPathLength, Graph::EdgeVector & path) +{ + double constexpr kLengthToleranceFactor = 1.1; + uint32_t constexpr kMinLengthTolerance = 20; + uint32_t const lengthToleranceM = + max(static_cast(kLengthToleranceFactor * maxPathLength), kMinLengthTolerance); + + struct State + { + State(Graph::Edge const & e, uint32_t const s) : m_edge(e), m_score(s) {} + + bool operator>(State const & o) const + { + return tie(m_score, m_edge) > tie(o.m_score, o.m_edge); + } + + Graph::Edge m_edge; + uint32_t m_score; + }; + + CHECK(from.HasRealPart() && to.HasRealPart(), ()); + + priority_queue, greater<>> q; + map scores; + map links; + + q.emplace(from, 0); + scores[from] = 0; + + while (!q.empty()) + { + auto const state = q.top(); + q.pop(); + + auto const & stateEdge = state.m_edge; + // TODO(mgsergio): Unify names: use either score or distance. + auto const stateScore = state.m_score; + + if (stateScore > maxPathLength + lengthToleranceM) + continue; + + if (stateScore > scores[stateEdge]) + continue; + + if (stateEdge == to) + { + for (auto edge = stateEdge; edge != from; edge = links[edge]) + path.emplace_back(edge); + path.emplace_back(from); + reverse(begin(path), end(path)); + return true; + } + + Graph::EdgeVector edges; + m_graph.GetOutgoingEdges(stateEdge.GetEndJunction(), edges); + for (auto const & edge : edges) + { + if (!ConformLfrcnpV3(edge, lowestFrcToNextPoint, m_infoGetter)) + continue; + + CHECK(!stateEdge.IsFake(), ()); + CHECK(!edge.IsFake(), ()); + + auto const it = scores.find(edge); + auto const eScore = stateScore + EdgeLength(edge); + if (it == end(scores) || it->second > eScore) + { + scores[edge] = eScore; + links[edge] = stateEdge; + q.emplace(edge, eScore); + } + } + } + + return false; +} + +bool ScorePathsConnector::ConnectAdjacentCandidateLines(Graph::EdgeVector const & from, + Graph::EdgeVector const & to, + FunctionalRoadClass lowestFrcToNextPoint, + double distanceToNextPoint, + Graph::EdgeVector & resultPath) +{ + CHECK(!to.empty(), ()); + + if (auto const skip = PathOverlappingLen(from, to)) + { + if (skip == -1) + return false; + + resultPath.insert(resultPath.end(), from.cbegin(), from.cend()); + resultPath.insert(resultPath.end(), to.cbegin() + skip, to.cend()); + return true; + } + + CHECK_NOT_EQUAL(from, to, ()); + + Graph::EdgeVector shortestPath; + auto const found = + FindShortestPath(from.back(), to.front(), lowestFrcToNextPoint, + static_cast(ceil(distanceToNextPoint)), shortestPath); + if (!found) + return false; + + CHECK_EQUAL(from.back(), shortestPath.front(), ()); + resultPath.insert(resultPath.end(), from.cbegin(), prev(from.cend())); + resultPath.insert(resultPath.end(), shortestPath.cbegin(), shortestPath.cend()); + + CHECK_EQUAL(to.front(), shortestPath.back(), ()); + resultPath.insert(resultPath.end(), next(to.begin()), to.end()); + + return !resultPath.empty(); +} + +Score ScorePathsConnector::GetScoreForUniformity(Graph::EdgeVector const & path) +{ + EdgeContainer edgeContainer(m_graph); + for (auto const & edge : path) + edgeContainer.ProcessEdge(edge); + + auto const hwClassDiff = edgeContainer.GetHighwayClassDiff(); + Score constexpr kScoreForTheSameHwClass = 40; + Score constexpr kScoreForNeighboringHwClasses = 15; + Score const hwClassScore = hwClassDiff == 0 + ? kScoreForTheSameHwClass + : hwClassDiff == 1 ? kScoreForNeighboringHwClasses : 0; + + Score constexpr kScoreForOneWayOnly = 17; + Score constexpr kScoreForRoundaboutOnly = 18; + Score constexpr kScoreForLinkOnly = 10; + Score const allEqualScore = + (edgeContainer.AreAllOneWaysEqual() ? kScoreForOneWayOnly : 0) + + (edgeContainer.AreAllRoundaboutEqual() ? kScoreForRoundaboutOnly : 0) + + (edgeContainer.AreAllLinksEqual() ? kScoreForLinkOnly : 0); + + return hwClassScore + allEqualScore; +} +} // namespace openlr diff --git a/openlr/score_paths_connector.hpp b/openlr/score_paths_connector.hpp new file mode 100644 index 0000000000..7f29b5650f --- /dev/null +++ b/openlr/score_paths_connector.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "openlr/graph.hpp" +#include "openlr/openlr_model.hpp" +#include "openlr/road_info_getter.hpp" +#include "openlr/score_types.hpp" +#include "openlr/stats.hpp" + +#include +#include +#include + +namespace openlr +{ +class ScorePathsConnector +{ +public: + ScorePathsConnector(Graph & graph, RoadInfoGetter & infoGetter, v2::Stats & stat); + + /// \brief Connects |lineCandidates| and fills |resultPath| with the path with maximum score + /// if there's a good enough. + /// \returns true if the best path is found and false otherwise. + bool FindBestPath(std::vector const & points, + std::vector> const & lineCandidates, + std::vector & resultPath); + +private: + bool FindShortestPath(Graph::Edge const & from, Graph::Edge const & to, + FunctionalRoadClass lowestFrcToNextPoint, uint32_t maxPathLength, + Graph::EdgeVector & path); + + bool ConnectAdjacentCandidateLines(Graph::EdgeVector const & from, Graph::EdgeVector const & to, + FunctionalRoadClass lowestFrcToNextPoint, + double distanceToNextPoint, Graph::EdgeVector & resultPath); + + Score GetScoreForUniformity(Graph::EdgeVector const & path); + + Graph & m_graph; + RoadInfoGetter & m_infoGetter; + v2::Stats & m_stat; +}; +} // namespace openlr diff --git a/openlr/score_types.hpp b/openlr/score_types.hpp new file mode 100644 index 0000000000..128dbdef3d --- /dev/null +++ b/openlr/score_types.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include "routing/road_graph.hpp" + +#include "geometry/point2d.hpp" + +#include +#include +#include + +namespace openlr +{ +using Edge = routing::Edge; +using EdgeVector = routing::RoadGraphBase::TEdgeVector; + +using Score = uint32_t; + +struct ScorePoint +{ + ScorePoint() = default; + ScorePoint(Score score, m2::PointD const & point) : m_score(score), m_point(point) {} + + bool operator<(ScorePoint const & o) const + { + return std::tie(m_score, m_point) < std::tie(o.m_score, o.m_point); + } + + bool operator==(ScorePoint const & o) const + { + return m_score == o.m_score && m_point == o.m_point; + } + + Score m_score = 0; + m2::PointD m_point; +}; + +using ScorePointVec = std::vector; + +struct ScoreEdge +{ + ScoreEdge(Score score, Edge const & edge) : m_score(score), m_edge(edge) {} + + Score m_score; + Edge m_edge; +}; + +using ScoreEdgeVec = std::vector; + +struct ScorePath +{ + ScorePath(Score score, EdgeVector && path) : m_score(score), m_path(move(path)) {} + + Score m_score; + EdgeVector m_path; +}; + +using ScorePathVec = std::vector; +} // namespace openlr diff --git a/openlr/stats.hpp b/openlr/stats.hpp index a5f0a0e5ac..2461198388 100644 --- a/openlr/stats.hpp +++ b/openlr/stats.hpp @@ -20,7 +20,7 @@ struct alignas(kCacheLineSize) Stats m_noCandidateFound += s.m_noCandidateFound; m_noShortestPathFound += s.m_noShortestPathFound; m_wrongOffsets += s.m_wrongOffsets; - m_dnpIsZero += s.m_dnpIsZero; + m_zeroDistToNextPointCount += s.m_zeroDistToNextPointCount; } void Report() const @@ -28,7 +28,7 @@ struct alignas(kCacheLineSize) Stats LOG(LINFO, ("Total routes handled:", m_routesHandled)); LOG(LINFO, ("Failed:", m_routesFailed)); LOG(LINFO, ("No candidate lines:", m_noCandidateFound)); - LOG(LINFO, ("Wrong distance to next point:", m_dnpIsZero)); + LOG(LINFO, ("Wrong distance to next point:", m_zeroDistToNextPointCount)); LOG(LINFO, ("Wrong offsets:", m_wrongOffsets)); LOG(LINFO, ("No shortest path:", m_noShortestPathFound)); } @@ -39,7 +39,7 @@ struct alignas(kCacheLineSize) Stats uint32_t m_noShortestPathFound = 0; uint32_t m_wrongOffsets = 0; // Number of zeroed distance-to-next point values in the input. - uint32_t m_dnpIsZero = 0; + uint32_t m_zeroDistToNextPointCount = 0; }; } // namespace V2 } // namespace openlr diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index 41c3213635..84297e0919 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -15,6 +15,7 @@ #include #include +#include #include #include diff --git a/xcode/openlr/openlr.xcodeproj/project.pbxproj b/xcode/openlr/openlr.xcodeproj/project.pbxproj index 57a9c4dd6f..18e542888f 100644 --- a/xcode/openlr/openlr.xcodeproj/project.pbxproj +++ b/xcode/openlr/openlr.xcodeproj/project.pbxproj @@ -7,6 +7,25 @@ objects = { /* Begin PBXBuildFile section */ + 56B1DDC2225E04A100F88086 /* graph.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDAF225E04A000F88086 /* graph.hpp */; }; + 56B1DDC3225E04A100F88086 /* score_types.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDB0225E04A000F88086 /* score_types.hpp */; }; + 56B1DDC4225E04A100F88086 /* graph.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDB1225E04A000F88086 /* graph.cpp */; }; + 56B1DDC5225E04A100F88086 /* candidate_paths_getter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDB2225E04A000F88086 /* candidate_paths_getter.cpp */; }; + 56B1DDC6225E04A100F88086 /* paths_connector.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDB3225E04A000F88086 /* paths_connector.hpp */; }; + 56B1DDC7225E04A100F88086 /* score_candidate_paths_getter.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDB4225E04A000F88086 /* score_candidate_paths_getter.hpp */; }; + 56B1DDC8225E04A100F88086 /* candidate_points_getter.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDB5225E04A100F88086 /* candidate_points_getter.hpp */; }; + 56B1DDC9225E04A100F88086 /* candidate_paths_getter.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDB6225E04A100F88086 /* candidate_paths_getter.hpp */; }; + 56B1DDCA225E04A100F88086 /* candidate_points_getter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDB7225E04A100F88086 /* candidate_points_getter.cpp */; }; + 56B1DDCB225E04A100F88086 /* paths_connector.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDB8225E04A100F88086 /* paths_connector.cpp */; }; + 56B1DDCC225E04A100F88086 /* helpers.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDB9225E04A100F88086 /* helpers.cpp */; }; + 56B1DDCD225E04A100F88086 /* score_candidate_points_getter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDBA225E04A100F88086 /* score_candidate_points_getter.cpp */; }; + 56B1DDCE225E04A100F88086 /* score_candidate_points_getter.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDBB225E04A100F88086 /* score_candidate_points_getter.hpp */; }; + 56B1DDCF225E04A100F88086 /* score_paths_connector.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDBC225E04A100F88086 /* score_paths_connector.cpp */; }; + 56B1DDD0225E04A100F88086 /* score_paths_connector.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDBD225E04A100F88086 /* score_paths_connector.hpp */; }; + 56B1DDD1225E04A100F88086 /* cache_line_size.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDBE225E04A100F88086 /* cache_line_size.hpp */; }; + 56B1DDD2225E04A100F88086 /* stats.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDBF225E04A100F88086 /* stats.hpp */; }; + 56B1DDD3225E04A100F88086 /* score_candidate_paths_getter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56B1DDC0225E04A100F88086 /* score_candidate_paths_getter.cpp */; }; + 56B1DDD4225E04A100F88086 /* helpers.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56B1DDC1225E04A100F88086 /* helpers.hpp */; }; 671E79101E6A502200B2859B /* openlr_model.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 671E79011E6A502200B2859B /* openlr_model.cpp */; }; 671E79111E6A502200B2859B /* openlr_model.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 671E79021E6A502200B2859B /* openlr_model.hpp */; }; 671E79181E6A502200B2859B /* road_info_getter.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 671E79091E6A502200B2859B /* road_info_getter.cpp */; }; @@ -23,6 +42,25 @@ /* End PBXBuildFile section */ /* Begin PBXFileReference section */ + 56B1DDAF225E04A000F88086 /* graph.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = graph.hpp; sourceTree = ""; }; + 56B1DDB0225E04A000F88086 /* score_types.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = score_types.hpp; sourceTree = ""; }; + 56B1DDB1225E04A000F88086 /* graph.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = graph.cpp; sourceTree = ""; }; + 56B1DDB2225E04A000F88086 /* candidate_paths_getter.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = candidate_paths_getter.cpp; sourceTree = ""; }; + 56B1DDB3225E04A000F88086 /* paths_connector.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = paths_connector.hpp; sourceTree = ""; }; + 56B1DDB4225E04A000F88086 /* score_candidate_paths_getter.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = score_candidate_paths_getter.hpp; sourceTree = ""; }; + 56B1DDB5225E04A100F88086 /* candidate_points_getter.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = candidate_points_getter.hpp; sourceTree = ""; }; + 56B1DDB6225E04A100F88086 /* candidate_paths_getter.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = candidate_paths_getter.hpp; sourceTree = ""; }; + 56B1DDB7225E04A100F88086 /* candidate_points_getter.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = candidate_points_getter.cpp; sourceTree = ""; }; + 56B1DDB8225E04A100F88086 /* paths_connector.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = paths_connector.cpp; sourceTree = ""; }; + 56B1DDB9225E04A100F88086 /* helpers.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = helpers.cpp; sourceTree = ""; }; + 56B1DDBA225E04A100F88086 /* score_candidate_points_getter.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = score_candidate_points_getter.cpp; sourceTree = ""; }; + 56B1DDBB225E04A100F88086 /* score_candidate_points_getter.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = score_candidate_points_getter.hpp; sourceTree = ""; }; + 56B1DDBC225E04A100F88086 /* score_paths_connector.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = score_paths_connector.cpp; sourceTree = ""; }; + 56B1DDBD225E04A100F88086 /* score_paths_connector.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = score_paths_connector.hpp; sourceTree = ""; }; + 56B1DDBE225E04A100F88086 /* cache_line_size.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = cache_line_size.hpp; sourceTree = ""; }; + 56B1DDBF225E04A100F88086 /* stats.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = stats.hpp; sourceTree = ""; }; + 56B1DDC0225E04A100F88086 /* score_candidate_paths_getter.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = score_candidate_paths_getter.cpp; sourceTree = ""; }; + 56B1DDC1225E04A100F88086 /* helpers.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = helpers.hpp; sourceTree = ""; }; 671E78F31E6A4FE400B2859B /* libopenlr.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; includeInIndex = 0; path = libopenlr.a; sourceTree = BUILT_PRODUCTS_DIR; }; 671E79011E6A502200B2859B /* openlr_model.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = openlr_model.cpp; sourceTree = ""; }; 671E79021E6A502200B2859B /* openlr_model.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = openlr_model.hpp; sourceTree = ""; }; @@ -73,6 +111,25 @@ 671E78F51E6A4FE400B2859B /* openlr */ = { isa = PBXGroup; children = ( + 56B1DDBE225E04A100F88086 /* cache_line_size.hpp */, + 56B1DDB2225E04A000F88086 /* candidate_paths_getter.cpp */, + 56B1DDB6225E04A100F88086 /* candidate_paths_getter.hpp */, + 56B1DDB7225E04A100F88086 /* candidate_points_getter.cpp */, + 56B1DDB5225E04A100F88086 /* candidate_points_getter.hpp */, + 56B1DDB1225E04A000F88086 /* graph.cpp */, + 56B1DDAF225E04A000F88086 /* graph.hpp */, + 56B1DDB9225E04A100F88086 /* helpers.cpp */, + 56B1DDC1225E04A100F88086 /* helpers.hpp */, + 56B1DDB8225E04A100F88086 /* paths_connector.cpp */, + 56B1DDB3225E04A000F88086 /* paths_connector.hpp */, + 56B1DDC0225E04A100F88086 /* score_candidate_paths_getter.cpp */, + 56B1DDB4225E04A000F88086 /* score_candidate_paths_getter.hpp */, + 56B1DDBA225E04A100F88086 /* score_candidate_points_getter.cpp */, + 56B1DDBB225E04A100F88086 /* score_candidate_points_getter.hpp */, + 56B1DDBC225E04A100F88086 /* score_paths_connector.cpp */, + 56B1DDBD225E04A100F88086 /* score_paths_connector.hpp */, + 56B1DDB0225E04A000F88086 /* score_types.hpp */, + 56B1DDBF225E04A100F88086 /* stats.hpp */, E92EE07E1F98E8EB00B57D20 /* decoded_path.cpp */, E92EE07D1F98E8EB00B57D20 /* decoded_path.hpp */, E92EE0811F98E8EC00B57D20 /* openlr_decoder.cpp */, @@ -99,12 +156,23 @@ buildActionMask = 2147483647; files = ( 671E791D1E6A502200B2859B /* router.hpp in Headers */, + 56B1DDC6225E04A100F88086 /* paths_connector.hpp in Headers */, + 56B1DDC7225E04A100F88086 /* score_candidate_paths_getter.hpp in Headers */, + 56B1DDC3225E04A100F88086 /* score_types.hpp in Headers */, + 56B1DDC2225E04A100F88086 /* graph.hpp in Headers */, E92EE0821F98E8EC00B57D20 /* openlr_model_xml.hpp in Headers */, 671E79111E6A502200B2859B /* openlr_model.hpp in Headers */, 671E79191E6A502200B2859B /* road_info_getter.hpp in Headers */, + 56B1DDD2225E04A100F88086 /* stats.hpp in Headers */, + 56B1DDD0225E04A100F88086 /* score_paths_connector.hpp in Headers */, + 56B1DDD4225E04A100F88086 /* helpers.hpp in Headers */, + 56B1DDC8225E04A100F88086 /* candidate_points_getter.hpp in Headers */, E92EE0851F98E8EC00B57D20 /* openlr_decoder.hpp in Headers */, E92EE0831F98E8EC00B57D20 /* decoded_path.hpp in Headers */, + 56B1DDC9225E04A100F88086 /* candidate_paths_getter.hpp in Headers */, 671E791E1E6A502200B2859B /* way_point.hpp in Headers */, + 56B1DDD1225E04A100F88086 /* cache_line_size.hpp in Headers */, + 56B1DDCE225E04A100F88086 /* score_candidate_points_getter.hpp in Headers */, ); runOnlyForDeploymentPostprocessing = 0; }; @@ -148,6 +216,7 @@ developmentRegion = English; hasScannedForEncodings = 0; knownRegions = ( + English, en, ); mainGroup = 671E78EA1E6A4FE400B2859B; @@ -165,12 +234,20 @@ isa = PBXSourcesBuildPhase; buildActionMask = 2147483647; files = ( + 56B1DDCF225E04A100F88086 /* score_paths_connector.cpp in Sources */, + 56B1DDCD225E04A100F88086 /* score_candidate_points_getter.cpp in Sources */, + 56B1DDD3225E04A100F88086 /* score_candidate_paths_getter.cpp in Sources */, 671E79101E6A502200B2859B /* openlr_model.cpp in Sources */, + 56B1DDCC225E04A100F88086 /* helpers.cpp in Sources */, + 56B1DDC5225E04A100F88086 /* candidate_paths_getter.cpp in Sources */, + 56B1DDCA225E04A100F88086 /* candidate_points_getter.cpp in Sources */, E92EE0861F98E8EC00B57D20 /* openlr_model_xml.cpp in Sources */, E92EE0841F98E8EC00B57D20 /* decoded_path.cpp in Sources */, 671E79181E6A502200B2859B /* road_info_getter.cpp in Sources */, + 56B1DDCB225E04A100F88086 /* paths_connector.cpp in Sources */, 671E791C1E6A502200B2859B /* router.cpp in Sources */, E92EE0871F98E8EC00B57D20 /* openlr_decoder.cpp in Sources */, + 56B1DDC4225E04A100F88086 /* graph.cpp in Sources */, ); runOnlyForDeploymentPostprocessing = 0; };