From 651d788c1f1fa9e28fd697b1e1b35b964d9e9233 Mon Sep 17 00:00:00 2001 From: Maxim Pimenov Date: Fri, 17 Apr 2015 17:35:12 +0300 Subject: [PATCH] [pedestrian] Dispose of M2M in favour of P2P. --- .../pedestrian_routing_benchmarks.cpp | 8 +- routing/astar_router.cpp | 18 ++--- routing/astar_router.hpp | 2 +- routing/base/astar_algorithm.hpp | 77 ++++++++----------- routing/features_road_graph.cpp | 11 +++ routing/road_graph.cpp | 1 + routing/road_graph.hpp | 25 +++++- routing/road_graph_router.cpp | 52 ++++++++++--- routing/road_graph_router.hpp | 5 +- .../routing_tests/astar_algorithm_test.cpp | 6 +- routing/routing_tests/astar_router_test.cpp | 12 +-- 11 files changed, 128 insertions(+), 89 deletions(-) diff --git a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp index 006af64d4d..e1ad174394 100644 --- a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp +++ b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp @@ -44,19 +44,17 @@ void TestTwoPoints(uint32_t featureIdStart, uint32_t segIdStart, uint32_t featur pair startBounds = GetPointsAroundSeg(index, id, featureIdStart, segIdStart); - vector startPos = {{featureIdStart, true, segIdStart, startBounds.second}, - {featureIdStart, false, segIdStart, startBounds.first}}; + routing::RoadPos startPos(featureIdStart, true, segIdStart, startBounds.first); pair finalBounds = GetPointsAroundSeg(index, id, featureIdFinal, segIdFinal); - vector finalPos = {{featureIdFinal, true, segIdFinal, finalBounds.second}, - {featureIdFinal, false, segIdFinal, finalBounds.first}}; + routing::RoadPos finalPos(featureIdFinal, true, segIdFinal, finalBounds.first); vector route; my::Timer timer; LOG(LINFO, ("Calculating routing...")); - routing::IRouter::ResultCode resultCode = router.CalculateRouteM2M(startPos, finalPos, route); + routing::IRouter::ResultCode resultCode = router.CalculateRouteP2P(startPos, finalPos, route); CHECK_EQUAL(routing::IRouter::NoError, resultCode, ()); LOG(LINFO, ("Route length:", route.size())); LOG(LINFO, ("Elapsed:", timer.ElapsedSeconds(), "seconds")); diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 86c6519ac5..b580e219d3 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -6,8 +6,8 @@ namespace routing { -IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector const & startPos, - vector const & finalPos, +IRouter::ResultCode AStarRouter::CalculateRouteP2P(RoadPos const & startPos, + RoadPos const & finalPos, vector & route) { RoadGraph graph(*m_roadGraph); @@ -22,16 +22,10 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector const & start // RoadPos::m_segEndpoint. Thus, start and final positions // returned by algorithm should be replaced by correct start and // final positions. - for (RoadPos const & sp : startPos) - { - if (route.front() == sp) - route.front() = sp; - } - for (RoadPos const & fp : finalPos) - { - if (route.back() == fp) - route.back() = fp; - } + ASSERT_EQUAL(route.front(), startPos, ()); + route.front() = startPos; + ASSERT_EQUAL(route.back(), finalPos, ()); + route.back() = finalPos; return IRouter::NoError; case TAlgorithm::Result::NoPath: return IRouter::RouteNotFound; diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp index 353916c82f..4808c78f0b 100644 --- a/routing/astar_router.hpp +++ b/routing/astar_router.hpp @@ -19,7 +19,7 @@ public: void ClearState() override { Reset(); } // RoadGraphRouter overrides: - ResultCode CalculateRouteM2M(vector const & startPos, vector const & finalPos, + ResultCode CalculateRouteP2P(RoadPos const & startPos, RoadPos const & finalPos, vector & route) override; // my::Cancellable overrides: diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp index 5305021fa7..4f63264218 100644 --- a/routing/base/astar_algorithm.hpp +++ b/routing/base/astar_algorithm.hpp @@ -51,10 +51,9 @@ public: AStarAlgorithm() : m_graph(nullptr) {} - Result FindPath(vector const & startPos, vector const & finalPos, + Result FindPath(TVertexType const & startVertex, TVertexType const & finalVertex, vector & path) const; - Result FindPathBidirectional(vector const & startPos, - vector const & finalPos, + Result FindPathBidirectional(TVertexType const & startVertex, TVertexType const & finalVertex, vector & path) const; void SetGraph(TGraph const & graph) { m_graph = &graph; } @@ -77,12 +76,12 @@ private: // purpose is to make the code that changes directions more readable. struct BidirectionalStepContext { - BidirectionalStepContext(bool forward, vector const & startPos, - vector const & finalPos, TGraphType const & graph) - : forward(forward), startPos(startPos), finalPos(finalPos), graph(graph) + BidirectionalStepContext(bool forward, TVertexType const & startVertex, + TVertexType const & finalVertex, TGraphType const & graph) + : forward(forward), startVertex(startVertex), finalVertex(finalVertex), graph(graph) { - bestVertex = forward ? startPos[0] : finalPos[0]; - pS = ConsistentHeuristic(startPos[0]); + bestVertex = forward ? startVertex : finalVertex; + pS = ConsistentHeuristic(startVertex); } double TopDistance() const @@ -96,27 +95,27 @@ private: // p_r(v) + p_f(v) = const. Note: this condition is called consistence. double ConsistentHeuristic(TVertexType const & v) const { - double piF = graph.HeuristicCostEstimate(v, finalPos[0]); - double piR = graph.HeuristicCostEstimate(v, startPos[0]); - double const piRT = graph.HeuristicCostEstimate(finalPos[0], startPos[0]); - double const piFS = graph.HeuristicCostEstimate(startPos[0], finalPos[0]); + double piF = graph.HeuristicCostEstimate(v, finalVertex); + double piR = graph.HeuristicCostEstimate(v, startVertex); + double const piRT = graph.HeuristicCostEstimate(finalVertex, startVertex); + double const piFS = graph.HeuristicCostEstimate(startVertex, finalVertex); if (forward) { /// @todo careful: with this "return" here and below in the Backward case /// the heuristic becomes inconsistent but still seems to work. - /// return HeuristicCostEstimate(v.pos, finalPos); + /// return HeuristicCostEstimate(v, finalVertex); return 0.5 * (piF - piR + piRT); } else { - // return HeuristicCostEstimate(v.pos, startPos); + // return HeuristicCostEstimate(v, startVertex); return 0.5 * (piR - piF + piFS); } } bool const forward; - vector const & startPos; - vector const & finalPos; + TVertexType const & startVertex; + TVertexType const & finalVertex; TGraph const & graph; priority_queue, greater> queue; @@ -163,24 +162,17 @@ double const AStarAlgorithm::kEpsilon = 1e-6; // The edges of the graph are of type PossibleTurn. template typename AStarAlgorithm::Result AStarAlgorithm::FindPath( - vector const & startPos, vector const & finalPos, + TVertexType const & startVertex, TVertexType const & finalVertex, vector & path) const { ASSERT(m_graph, ()); - ASSERT(!startPos.empty(), ()); - ASSERT(!finalPos.empty(), ()); - - vector sortedStartPos(startPos.begin(), startPos.end()); - sort(sortedStartPos.begin(), sortedStartPos.end()); map bestDistance; priority_queue, greater> queue; map parent; - for (auto const & rp : finalPos) - { - VERIFY(bestDistance.emplace(rp, 0.0).second, ()); - queue.push(State(rp, 0.0)); - } + + bestDistance[finalVertex] = 0.0; + queue.push(State(finalVertex, 0.0)); uint32_t steps = 0; @@ -196,7 +188,7 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( if (stateV.distance > bestDistance[stateV.vertex]) continue; - if (binary_search(sortedStartPos.begin(), sortedStartPos.end(), stateV.vertex)) + if (stateV.vertex == startVertex) { ReconstructPath(stateV.vertex, parent, path); return Result::OK; @@ -210,8 +202,8 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( if (stateV.vertex == stateW.vertex) continue; double const len = edge.GetWeight(); - double const piV = m_graph->HeuristicCostEstimate(stateV.vertex, sortedStartPos[0]); - double const piW = m_graph->HeuristicCostEstimate(stateW.vertex, sortedStartPos[0]); + double const piV = m_graph->HeuristicCostEstimate(stateV.vertex, startVertex); + double const piW = m_graph->HeuristicCostEstimate(stateW.vertex, startVertex); double const reducedLen = len + piW - piV; CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon)); @@ -231,31 +223,22 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( return Result::NoPath; } -/// @todo This may work incorrectly if (startPos.size() > 1) or (finalPos.size() > 1). template typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirectional( - vector const & startPos, vector const & finalPos, + TVertexType const & startVertex, TVertexType const & finalVertex, vector & path) const { - ASSERT(!startPos.empty(), ()); - ASSERT(!finalPos.empty(), ()); - - BidirectionalStepContext forward(true /* forward */, startPos, finalPos, *m_graph); - BidirectionalStepContext backward(false /* forward */, startPos, finalPos, *m_graph); + BidirectionalStepContext forward(true /* forward */, startVertex, finalVertex, *m_graph); + BidirectionalStepContext backward(false /* forward */, startVertex, finalVertex, *m_graph); bool foundAnyPath = false; double bestPathLength = 0.0; - for (auto const & rp : startPos) - { - VERIFY(forward.bestDistance.emplace(rp, 0.0).second, ()); - forward.queue.push(State(rp, 0.0 /* distance */)); - } - for (auto const & rp : finalPos) - { - VERIFY(backward.bestDistance.emplace(rp, 0.0).second, ()); - backward.queue.push(State(rp, 0.0 /* distance */)); - } + forward.bestDistance[startVertex] = 0.0; + forward.queue.push(State(startVertex, 0.0 /* distance */)); + + backward.bestDistance[finalVertex] = 0.0; + backward.queue.push(State(finalVertex, 0.0 /* distance */)); // To use the search code both for backward and forward directions // we keep the pointers to everything related to the search in the diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 031e8755ea..75bcbf51bb 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -113,6 +113,17 @@ void FeaturesRoadGraph::LoadFeature(uint32_t featureId, FeatureType & ft) void FeaturesRoadGraph::GetNearestTurns(RoadPos const & pos, vector & turns) { + if (pos.IsStart()) + { + turns.insert(turns.end(), m_startVicinityTurns.begin(), m_startVicinityTurns.end()); + return; + } + if (pos.IsFinal()) + { + turns.insert(turns.end(), m_finalVicinityTurns.begin(), m_finalVicinityTurns.end()); + return; + } + uint32_t const featureId = pos.GetFeatureId(); FeatureType ft; RoadInfo const ri = GetCachedRoadInfo(featureId, ft, true); diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index 9f0639e5b8..135ccd6b62 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -1,4 +1,5 @@ #include "routing/road_graph.hpp" +#include "routing/road_graph_router.hpp" #include "routing/route.hpp" diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index 152443a270..d4a8b55cd9 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -16,6 +16,14 @@ class Route; class RoadPos { public: + // Our routers (such as a-star) use RoadPos as vertices and we receive PointD from the user. + // Every time a route is calculated, we create a fake RoadPos with a fake featureId to + // place the real starting point on it (and also do the same for the final point). + // The constant here is used as those fake features' ids. + /// @todo The constant value is taken to comply with an assert in the constructor + /// that is terrible, wrong and to be rewritten in a separate CL. + static constexpr uint32_t FakeFeatureId = 1U << 29; + RoadPos() : m_featureId(0), m_segId(0), m_segEndpoint(0, 0) {} RoadPos(uint32_t featureId, bool forward, size_t segId, m2::PointD const & p = m2::PointD::Zero()); @@ -26,6 +34,9 @@ public: uint32_t GetSegStartPointId() const { return m_segId + (IsForward() ? 0 : 1); } uint32_t GetSegEndPointId() const { return m_segId + (IsForward() ? 1 : 0); } m2::PointD const & GetSegEndpoint() const { return m_segEndpoint; } + bool IsFake() const { return GetFeatureId() == FakeFeatureId; } + bool IsStart() const { return IsFake() && GetSegId() == 0; } + bool IsFinal() const { return IsFake() && GetSegId() == 1; } bool operator==(RoadPos const & r) const { @@ -115,14 +126,22 @@ public: virtual ~IRoadGraph() = default; - /// Construct route by road positions (doesn't include first and last section). - virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route); - /// Finds all nearest feature sections (turns), that route to the /// "pos" section. virtual void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) = 0; virtual double GetSpeedKMPH(uint32_t featureId) = 0; + + /// Construct route by road positions (doesn't include first and last section). + virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route); + + // The way we find edges leading from start/final positions and from all other positions + // differ: for start/final we find positions in some vicinity of the starting + // point; for other positions we extract turns from the road graph. This non-uniformity + // comes from the fact that a start/final position does not necessarily fall on a feature + // (i.e. on a road). + vector m_startVicinityTurns; + vector m_finalVicinityTurns; }; // A class which represents an edge used by RoadGraph. diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 57b8504dd8..d5422e4fb9 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -10,17 +10,37 @@ #include "geometry/distance.hpp" +#include "base/assert.hpp" #include "base/timer.hpp" #include "base/logging.hpp" +namespace routing +{ namespace { size_t const MAX_ROAD_CANDIDATES = 2; double const FEATURE_BY_POINT_RADIUS_M = 100.0; + +/// @todo Code duplication with road_graph.cpp +double TimeBetweenSec(m2::PointD const & v, m2::PointD const & w) +{ + static double const kMaxSpeedMPS = 5000.0 / 3600; + return MercatorBounds::DistanceOnEarth(v, w) / kMaxSpeedMPS; } -namespace routing +void AddFakeTurns(RoadPos const & rp1, vector const & vicinity, + vector & turns) { + for (RoadPos const & rp2 : vicinity) + { + PossibleTurn t; + t.m_pos = rp2; + /// @todo Do we need other fields? Do we even need m_secondsCovered? + t.m_secondsCovered = TimeBetweenSec(rp1.GetSegEndpoint(), rp2.GetSegEndpoint()); + turns.push_back(t); + } +} +} // namespace RoadGraphRouter::~RoadGraphRouter() { @@ -53,26 +73,40 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin m2::PointD const & /* startDirection */, m2::PointD const & finalPoint, Route & route) { - vector finalPos; - size_t mwmID = GetRoadPos(finalPoint, finalPos); - if (!finalPos.empty() && !IsMyMWM(mwmID)) + // Despite adding fake notes and calculating their vicinities on the fly + // we still need to check that startPoint and finalPoint are in the same MWM + // and probably reset the graph. So the checks stay here. + vector finalVicinity; + size_t mwmID = GetRoadPos(finalPoint, finalVicinity); + if (!finalVicinity.empty() && !IsMyMWM(mwmID)) m_roadGraph.reset(new FeaturesRoadGraph(m_pIndex, mwmID)); if (!m_roadGraph) return EndPointNotFound; - vector startPos; - mwmID = GetRoadPos(startPoint, startPos); + vector startVicinity; + mwmID = GetRoadPos(startPoint, startVicinity); - if (startPos.empty() || !IsMyMWM(mwmID)) + if (startVicinity.empty() || !IsMyMWM(mwmID)) return StartPointNotFound; my::Timer timer; timer.Reset(); - vector routePos; + RoadPos startPos(RoadPos::FakeFeatureId, true /* forward */, 0 /* segId */, startPoint); + RoadPos finalPos(RoadPos::FakeFeatureId, true /* forward */, 1 /* segId */, finalPoint); - IRouter::ResultCode resultCode = CalculateRouteM2M(startPos, finalPos, routePos); + AddFakeTurns(startPos, startVicinity, m_roadGraph->m_startVicinityTurns); + AddFakeTurns(finalPos, finalVicinity, m_roadGraph->m_finalVicinityTurns); + + vector routePos; + IRouter::ResultCode resultCode = CalculateRouteP2P(startPos, finalPos, routePos); + + /// @todo They have fake feature ids. Can we still draw them? + ASSERT(routePos.back() == finalPos, ()); + routePos.pop_back(); + ASSERT(routePos.front() == startPos, ()); + routePos.erase(routePos.begin()); LOG(LINFO, ("Route calculation time:", timer.ElapsedSeconds(), "result code:", resultCode)); diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp index 97672974c4..ceede4eb89 100644 --- a/routing/road_graph_router.hpp +++ b/routing/road_graph_router.hpp @@ -21,14 +21,15 @@ public: ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, m2::PointD const & finalPoint, Route & route) override; - virtual ResultCode CalculateRouteM2M(vector const & startPos, - vector const & finalPos, + virtual ResultCode CalculateRouteP2P(RoadPos const & startPos, RoadPos const & finalPos, vector & route) = 0; virtual void SetRoadGraph(unique_ptr && roadGraph) { m_roadGraph = move(roadGraph); } inline IRoadGraph * GetGraph() { return m_roadGraph.get(); } protected: + /// @todo This method fits better in features_road_graph. size_t GetRoadPos(m2::PointD const & pt, vector & pos); + bool IsMyMWM(size_t mwmID) const; unique_ptr m_roadGraph; diff --git a/routing/routing_tests/astar_algorithm_test.cpp b/routing/routing_tests/astar_algorithm_test.cpp index 0f7f6e03f8..a910ea3bde 100644 --- a/routing/routing_tests/astar_algorithm_test.cpp +++ b/routing/routing_tests/astar_algorithm_test.cpp @@ -51,13 +51,11 @@ void TestAStar(UndirectedGraph const & graph, vector const & expectedR TAlgorithm algo; algo.SetGraph(graph); vector actualRoute; - TEST_EQUAL(TAlgorithm::Result::OK, - algo.FindPath(vector{0}, vector{4}, actualRoute), ()); + TEST_EQUAL(TAlgorithm::Result::OK, algo.FindPath(0u, 4u, actualRoute), ()); TEST_EQUAL(expectedRoute, actualRoute, ()); actualRoute.clear(); - TEST_EQUAL(TAlgorithm::Result::OK, - algo.FindPathBidirectional(vector{0}, vector{4}, actualRoute), ()); + TEST_EQUAL(TAlgorithm::Result::OK, algo.FindPathBidirectional(0u, 4u, actualRoute), ()); TEST_EQUAL(expectedRoute, actualRoute, ()); } diff --git a/routing/routing_tests/astar_router_test.cpp b/routing/routing_tests/astar_router_test.cpp index 3a5bc7fb09..7459bd1684 100644 --- a/routing/routing_tests/astar_router_test.cpp +++ b/routing/routing_tests/astar_router_test.cpp @@ -15,7 +15,7 @@ using namespace routing; using namespace routing_test; -void TestAStarRouterMock(vector const & startPos, vector const & finalPos, +void TestAStarRouterMock(RoadPos const & startPos, RoadPos const & finalPos, m2::PolylineD const & expected) { AStarRouter router; @@ -25,7 +25,7 @@ void TestAStarRouterMock(vector const & startPos, vector const router.SetRoadGraph(move(graph)); } vector result; - TEST_EQUAL(IRouter::NoError, router.CalculateRouteM2M(startPos, finalPos, result), ()); + TEST_EQUAL(IRouter::NoError, router.CalculateRouteP2P(startPos, finalPos, result), ()); Route route(router.GetName()); router.GetGraph()->ReconstructPath(result, route); @@ -36,8 +36,8 @@ UNIT_TEST(AStarRouter_Graph2_Simple1) { classificator::Load(); - vector startPos = {RoadPos(1, true /* forward */, 0, m2::PointD(0, 0))}; - vector finalPos = {RoadPos(7, true /* forward */, 0, m2::PointD(80, 55))}; + RoadPos startPos(1, true /* forward */, 0, m2::PointD(0, 0)); + RoadPos finalPos(7, true /* forward */, 0, m2::PointD(80, 55)); m2::PolylineD expected = {m2::PointD(0, 0), m2::PointD(5, 10), m2::PointD(5, 40), m2::PointD(18, 55), m2::PointD(39, 55), m2::PointD(80, 55)}; @@ -46,8 +46,8 @@ UNIT_TEST(AStarRouter_Graph2_Simple1) UNIT_TEST(AStarRouter_Graph2_Simple2) { - vector startPos = {RoadPos(7, false /* forward */, 0, m2::PointD(80, 55))}; - vector finalPos = {RoadPos(0, true /* forward */, 4, m2::PointD(80, 0))}; + RoadPos startPos(7, false /* forward */, 0, m2::PointD(80, 55)); + RoadPos finalPos(0, true /* forward */, 4, m2::PointD(80, 0)); m2::PolylineD expected = {m2::PointD(80, 55), m2::PointD(39, 55), m2::PointD(37, 30), m2::PointD(70, 30), m2::PointD(70, 10), m2::PointD(70, 0), m2::PointD(80, 0)};