From 3a13e465a8ad9a42fef8367732c3980adf476f37 Mon Sep 17 00:00:00 2001 From: gmoryes Date: Thu, 16 Jan 2020 14:51:26 +0300 Subject: [PATCH] [routing] Fix matching to fake segments --- map/routing_manager.cpp | 3 +- routing/base/followed_polyline.cpp | 62 +++++++++++++++--------------- routing/base/followed_polyline.hpp | 39 +++++++------------ routing/route.cpp | 21 +++++----- routing/route.hpp | 10 +---- routing/routing_session.cpp | 7 +--- 6 files changed, 61 insertions(+), 81 deletions(-) diff --git a/map/routing_manager.cpp b/map/routing_manager.cpp index c9d22a9342..80f56db05f 100644 --- a/map/routing_manager.cpp +++ b/map/routing_manager.cpp @@ -1159,8 +1159,9 @@ void RoutingManager::MatchLocationToRoute(location::GpsInfo & location, location::RouteMatchingInfo RoutingManager::GetRouteMatchingInfo(location::GpsInfo & info) { - location::RouteMatchingInfo routeMatchingInfo; CheckLocationForRouting(info); + + location::RouteMatchingInfo routeMatchingInfo; MatchLocationToRoute(info, routeMatchingInfo); return routeMatchingInfo; } diff --git a/routing/base/followed_polyline.cpp b/routing/base/followed_polyline.cpp index 4c936461d1..810c94589e 100644 --- a/routing/base/followed_polyline.cpp +++ b/routing/base/followed_polyline.cpp @@ -109,6 +109,11 @@ void FollowedPolyline::Update() m_current = Iter(m_poly.Front(), 0); } +bool FollowedPolyline::IsFakeSegment(size_t index) const +{ + return binary_search(m_fakeSegmentIndexes.begin(), m_fakeSegmentIndexes.end(), index); +} + template Iter FollowedPolyline::GetBestProjection(m2::RectD const & posRect, DistanceFn const & distFn) const @@ -128,32 +133,36 @@ Iter FollowedPolyline::GetBestProjection(m2::RectD const & posRect, return GetClosestProjectionInInterval(posRect, distFn, hoppingBorderIdx, m_nextCheckpointIndex); } -FollowedPolyline::UpdatedProjection FollowedPolyline::GetBestMatchingProjection(m2::RectD const & posRect) const +Iter FollowedPolyline::GetBestMatchingProjection(m2::RectD const & posRect) const { CHECK_EQUAL(m_segProj.size() + 1, m_poly.GetSize(), ()); // At first trying to find a projection to two closest route segments of route which is close // enough to |posRect| center. If |m_current| is right before intermediate point we can get |closestIter| // right after intermediate point (in next subroute). size_t const hoppingBorderIdx = min(m_segProj.size(), m_current.m_ind + 3); - auto const res = GetClosestMatchingProjectionInInterval(posRect, m_current.m_ind, hoppingBorderIdx); - if (res.m_iter.IsValid() || res.m_closerToUnmatching) - return UpdatedProjection{res.m_iter, res.m_closerToUnmatching}; + auto const iter = GetClosestMatchingProjectionInInterval(posRect, m_current.m_ind, hoppingBorderIdx); + if (iter.IsValid()) + return iter; // If a projection to the 3 closest route segments is not found tries to find projection to other route // segments of current subroute. return GetClosestMatchingProjectionInInterval(posRect, hoppingBorderIdx, m_nextCheckpointIndex); } -FollowedPolyline::UpdatedProjectionInfo FollowedPolyline::UpdateMatchingProjection(m2::RectD const & posRect) +bool FollowedPolyline::UpdateMatchingProjection(m2::RectD const & posRect) { ASSERT(m_current.IsValid(), ()); ASSERT_LESS(m_current.m_ind, m_poly.GetSize() - 1, ()); - auto const res = GetBestMatchingProjection(posRect); + auto const iter = GetBestMatchingProjection(posRect); - if (res.m_iter.IsValid()) - m_current = res.m_iter; - return UpdatedProjectionInfo{res.m_iter.IsValid(), res.m_closerToUnmatching}; + if (iter.IsValid()) + { + m_current = iter; + return true; + } + + return false; } Iter FollowedPolyline::UpdateProjection(m2::RectD const & posRect) @@ -173,10 +182,10 @@ Iter FollowedPolyline::UpdateProjection(m2::RectD const & posRect) return res; } -void FollowedPolyline::SetUnmatchingSegmentIndexes(vector && unmatchingSegmentIndexes) +void FollowedPolyline::SetFakeSegmentIndexes(vector && fakeSegmentIndexes) { - ASSERT(is_sorted(unmatchingSegmentIndexes.cbegin(), unmatchingSegmentIndexes.cend()), ()); - m_unmatchingSegmentIndexes = move(unmatchingSegmentIndexes); + ASSERT(is_sorted(fakeSegmentIndexes.cbegin(), fakeSegmentIndexes.cend()), ()); + m_fakeSegmentIndexes = move(fakeSegmentIndexes); } double FollowedPolyline::GetDistFromCurPointToRoutePointMerc() const @@ -209,14 +218,14 @@ void FollowedPolyline::GetCurrentDirectionPoint(m2::PointD & pt, double toleranc pt = point; } -FollowedPolyline::UpdatedProjection FollowedPolyline::GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, - size_t startIdx, size_t endIdx) const +Iter FollowedPolyline::GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, + size_t startIdx, size_t endIdx) const { CHECK_LESS_OR_EQUAL(endIdx, m_segProj.size(), ()); CHECK_LESS_OR_EQUAL(startIdx, endIdx, ()); + Iter nearestIter; double minDist = std::numeric_limits::max(); - double minDistUnmatching = minDist; m2::PointD const currPos = posRect.Center(); @@ -227,25 +236,14 @@ FollowedPolyline::UpdatedProjection FollowedPolyline::GetClosestMatchingProjecti if (!posRect.IsPointInside(pt)) continue; - Iter it(pt, i); - double const dp = mercator::DistanceOnEarth(it.m_pt, currPos); - if (dp >= minDistUnmatching && dp >= minDist) + double const dp = mercator::DistanceOnEarth(pt, currPos); + if (dp >= minDist) continue; - if (!std::binary_search(m_unmatchingSegmentIndexes.begin(), m_unmatchingSegmentIndexes.end(), it.m_ind)) - { - if (minDist > dp) // Overwrites the best matching for matched segment. - { - minDist = dp; - nearestIter = it; - } - } - else - { - if (minDistUnmatching > dp) - minDistUnmatching = dp; - } + nearestIter = Iter(pt, i); + minDist = dp; } - return UpdatedProjection{nearestIter, minDistUnmatching < minDist /* m_closerToUnmatching */}; + + return nearestIter; } } // namespace routing diff --git a/routing/base/followed_polyline.hpp b/routing/base/followed_polyline.hpp index fe4f9b46cc..e89a9252b4 100644 --- a/routing/base/followed_polyline.hpp +++ b/routing/base/followed_polyline.hpp @@ -59,39 +59,26 @@ public: struct Iter { - m2::PointD m_pt; - size_t m_ind; - static size_t constexpr kInvalidIndex = std::numeric_limits::max(); + Iter() = default; Iter(m2::PointD pt, size_t ind) : m_pt(pt), m_ind(ind) {} - Iter() : m_ind(kInvalidIndex) {} + bool IsValid() const { return m_ind != kInvalidIndex; } + + m2::PointD m_pt; + size_t m_ind = kInvalidIndex; }; - struct UpdatedProjection - { - // Iterator to the projection point. - Iter m_iter; - // True if nearest point is on an unmatched segment. - bool m_closerToUnmatching; - }; - - struct UpdatedProjectionInfo - { - bool m_updatedProjection; - bool m_closerToUnmatching; - }; - - const Iter GetCurrentIter() const { return m_current; } + Iter GetCurrentIter() const { return m_current; } double GetDistanceM(Iter const & it1, Iter const & it2) const; /// \brief Sets indexes of all unmatching segments on route. - void SetUnmatchingSegmentIndexes(std::vector && unmatchingSegmentIndexes); + void SetFakeSegmentIndexes(std::vector && fakeSegmentIndexes); /// \brief Updates projection to the closest matched segment if it's possible. - UpdatedProjectionInfo UpdateMatchingProjection(m2::RectD const & posRect); + bool UpdateMatchingProjection(m2::RectD const & posRect); Iter UpdateProjection(m2::RectD const & posRect); @@ -134,8 +121,10 @@ public: return res; } -UpdatedProjection GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, - size_t startIdx, size_t endIdx) const; + Iter GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, size_t startIdx, + size_t endIdx) const; + + bool IsFakeSegment(size_t index) const; private: /// \returns iterator to the best projection of center of |posRect| to the |m_poly|. @@ -145,13 +134,13 @@ private: template Iter GetBestProjection(m2::RectD const & posRect, DistanceFn const & distFn) const; - UpdatedProjection GetBestMatchingProjection(m2::RectD const & posRect) const; + Iter GetBestMatchingProjection(m2::RectD const & posRect) const; void Update(); m2::PolylineD m_poly; /// Indexes of all unmatching segments on route. - std::vector m_unmatchingSegmentIndexes; + std::vector m_fakeSegmentIndexes; /// Iterator with the current position. Position sets with UpdateProjection methods. Iter m_current; diff --git a/routing/route.cpp b/routing/route.cpp index d67b4af856..57e749ff50 100644 --- a/routing/route.cpp +++ b/routing/route.cpp @@ -247,16 +247,16 @@ void Route::SetFakeSegmentsOnPolyline() fakeSegmentIndexes.push_back(i); } - m_poly.SetUnmatchingSegmentIndexes(move(fakeSegmentIndexes)); + m_poly.SetFakeSegmentIndexes(move(fakeSegmentIndexes)); } -Route::MovedIteratorInfo Route::MoveIteratorToReal(location::GpsInfo const & info) +bool Route::MoveIterator(location::GpsInfo const & info) { m2::RectD const rect = mercator::MetersToXY( info.m_longitude, info.m_latitude, max(m_routingSettings.m_matchingThresholdM, info.m_horizontalAccuracy)); - auto const resUpdate = m_poly.UpdateMatchingProjection(rect); - return MovedIteratorInfo{resUpdate.m_updatedProjection, resUpdate.m_closerToUnmatching}; + + return m_poly.UpdateMatchingProjection(rect); } double Route::GetPolySegAngle(size_t ind) const @@ -285,17 +285,20 @@ void Route::MatchLocationToRoute(location::GpsInfo & location, location::RouteMa if (m_poly.IsValid()) { auto const & iter = m_poly.GetCurrentIter(); - m2::PointD const locationMerc = mercator::FromLatLon(location.m_latitude, location.m_longitude); - double const distFromRouteM = mercator::DistanceOnEarth(iter.m_pt, locationMerc); - if (distFromRouteM < m_routingSettings.m_matchingThresholdM) + + auto const locationMerc = mercator::FromLatLon(location.m_latitude, location.m_longitude); + auto const distFromRouteM = mercator::DistanceOnEarth(iter.m_pt, locationMerc); + + if (!m_poly.IsFakeSegment(iter.m_ind) && + distFromRouteM < m_routingSettings.m_matchingThresholdM) { location.m_latitude = mercator::YToLat(iter.m_pt.y); location.m_longitude = mercator::XToLon(iter.m_pt.x); if (m_routingSettings.m_matchRoute) location.m_bearing = location::AngleToBearing(GetPolySegAngle(iter.m_ind)); - - routeMatchingInfo.Set(iter.m_pt, iter.m_ind, GetMercatorDistanceFromBegin()); } + + routeMatchingInfo.Set(iter.m_pt, iter.m_ind, GetMercatorDistanceFromBegin()); } } diff --git a/routing/route.hpp b/routing/route.hpp index e5fd976e20..7ee958fe28 100644 --- a/routing/route.hpp +++ b/routing/route.hpp @@ -195,14 +195,6 @@ public: size_t m_endSegmentIdx = 0; }; - struct MovedIteratorInfo - { - // Indicator of setting the iterator to one of real segments. - bool m_movedIterator; - // Indicator of the presence of the fake segment which is the nearest to the given point. - bool m_closerToFake; - }; - /// \brief For every subroute some attributes are kept in the following structure. struct SubrouteSettings final { @@ -334,7 +326,7 @@ public: void GetCurrentDirectionPoint(m2::PointD & pt) const; - MovedIteratorInfo MoveIteratorToReal(location::GpsInfo const & info); + bool MoveIterator(location::GpsInfo const & info); void MatchLocationToRoute(location::GpsInfo & location, location::RouteMatchingInfo & routeMatchingInfo) const; diff --git a/routing/routing_session.cpp b/routing/routing_session.cpp index 68c8aedd51..ba9ed389b9 100644 --- a/routing/routing_session.cpp +++ b/routing/routing_session.cpp @@ -284,9 +284,7 @@ SessionState RoutingSession::OnLocationPositionChanged(GpsInfo const & info) m_turnNotificationsMgr.SetSpeedMetersPerSecond(info.m_speedMpS); - auto const iteratorAction = m_route->MoveIteratorToReal(info); - - if (iteratorAction.m_movedIterator) + if (m_route->MoveIterator(info)) { m_moveAwayCounter = 0; m_lastDistance = 0.0; @@ -306,7 +304,6 @@ SessionState RoutingSession::OnLocationPositionChanged(GpsInfo const & info) else { SetState(SessionState::OnRoute); - m_speedCameraManager.OnLocationPositionChanged(info); } @@ -316,7 +313,7 @@ SessionState RoutingSession::OnLocationPositionChanged(GpsInfo const & info) return m_state; } - if (!iteratorAction.m_closerToFake && m_state != SessionState::RouteNeedRebuild && m_state != SessionState::RouteRebuilding) + if (m_state != SessionState::RouteNeedRebuild && m_state != SessionState::RouteRebuilding) { // Distance from the last known projection on route // (check if we are moving far from the last known projection).