pastk/organicmaps
1
Fork 0
forked from organicmaps/organicmaps
Code Pull requests Activity

[pedestrian] Dispose of M2M in favour of P2P.

Browse source
This commit is contained in:
Maxim Pimenov 2015-04-17 17:35:12 +03:00 • committed by Alex Zolotarev
parent 29e003b1e5
commit 651d788c1f
11 changed files with 128 additions and 89 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.cpp
View file

@ -44,19 +44,17 @@ void TestTwoPoints(uint32_t featureIdStart, uint32_t segIdStart, uint32_t featur
pair<m2::PointD, m2::PointD> startBounds =
GetPointsAroundSeg(index, id, featureIdStart, segIdStart);
vector<routing::RoadPos> startPos = {{featureIdStart, true, segIdStart, startBounds.second},
{featureIdStart, false, segIdStart, startBounds.first}};
routing::RoadPos startPos(featureIdStart, true, segIdStart, startBounds.first);
pair<m2::PointD, m2::PointD> finalBounds =
GetPointsAroundSeg(index, id, featureIdFinal, segIdFinal);
vector<routing::RoadPos> finalPos = {{featureIdFinal, true, segIdFinal, finalBounds.second},
{featureIdFinal, false, segIdFinal, finalBounds.first}};
routing::RoadPos finalPos(featureIdFinal, true, segIdFinal, finalBounds.first);
vector<routing::RoadPos> 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"));

18
routing/astar_router.cpp
View file

@ -6,8 +6,8 @@
namespace routing
{
IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector<RoadPos> const & startPos,
vector<RoadPos> const & finalPos,
IRouter::ResultCode AStarRouter::CalculateRouteP2P(RoadPos const & startPos,
RoadPos const & finalPos,
vector<RoadPos> & route)
{
RoadGraph graph(*m_roadGraph);
@ -22,16 +22,10 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector<RoadPos> 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;

2
routing/astar_router.hpp
View file

@ -19,7 +19,7 @@ public:
void ClearState() override { Reset(); }
// RoadGraphRouter overrides:
ResultCode CalculateRouteM2M(vector<RoadPos> const & startPos, vector<RoadPos> const & finalPos,
ResultCode CalculateRouteP2P(RoadPos const & startPos, RoadPos const & finalPos,
vector<RoadPos> & route) override;
// my::Cancellable overrides:

77
routing/base/astar_algorithm.hpp
View file

@ -51,10 +51,9 @@ public:
AStarAlgorithm() : m_graph(nullptr) {}
Result FindPath(vector<TVertexType> const & startPos, vector<TVertexType> const & finalPos,
Result FindPath(TVertexType const & startVertex, TVertexType const & finalVertex,
vector<TVertexType> & path) const;
Result FindPathBidirectional(vector<TVertexType> const & startPos,
vector<TVertexType> const & finalPos,
Result FindPathBidirectional(TVertexType const & startVertex, TVertexType const & finalVertex,
vector<TVertexType> & 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<TVertexType> const & startPos,
vector<TVertexType> 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<TVertexType> const & startPos;
vector<TVertexType> const & finalPos;
TVertexType const & startVertex;
TVertexType const & finalVertex;
TGraph const & graph;
priority_queue<State, vector<State>, greater<State>> queue;
@ -163,24 +162,17 @@ double const AStarAlgorithm<TGraph>::kEpsilon = 1e-6;
// The edges of the graph are of type PossibleTurn.
template <typename TGraph>
typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
vector<TVertexType> const & startPos, vector<TVertexType> const & finalPos,
TVertexType const & startVertex, TVertexType const & finalVertex,
vector<TVertexType> & path) const
{
ASSERT(m_graph, ());
ASSERT(!startPos.empty(), ());
ASSERT(!finalPos.empty(), ());
vector<TVertexType> sortedStartPos(startPos.begin(), startPos.end());
sort(sortedStartPos.begin(), sortedStartPos.end());
map<TVertexType, double> bestDistance;
priority_queue<State, vector<State>, greater<State>> queue;
map<TVertexType, TVertexType> 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<TGraph>::Result AStarAlgorithm<TGraph>::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<TGraph>::Result AStarAlgorithm<TGraph>::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<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
return Result::NoPath;
}
/// @todo This may work incorrectly if (startPos.size() > 1) or (finalPos.size() > 1).
template <typename TGraph>
typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirectional(
vector<TVertexType> const & startPos, vector<TVertexType> const & finalPos,
TVertexType const & startVertex, TVertexType const & finalVertex,
vector<TVertexType> & 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

11
routing/features_road_graph.cpp
View file

@ -113,6 +113,17 @@ void FeaturesRoadGraph::LoadFeature(uint32_t featureId, FeatureType & ft)
void FeaturesRoadGraph::GetNearestTurns(RoadPos const & pos, vector<PossibleTurn> & 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);

1
routing/road_graph.cpp
View file

@ -1,4 +1,5 @@
#include "routing/road_graph.hpp"
#include "routing/road_graph_router.hpp"
#include "routing/route.hpp"

25
routing/road_graph.hpp
View file

@ -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<PossibleTurn> m_startVicinityTurns;
vector<PossibleTurn> m_finalVicinityTurns;
};
// A class which represents an edge used by RoadGraph.

52
routing/road_graph_router.cpp
View file

@ -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<RoadPos> const & vicinity,
vector<PossibleTurn> & 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<RoadPos> 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<RoadPos> 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<RoadPos> startPos;
mwmID = GetRoadPos(startPoint, startPos);
vector<RoadPos> startVicinity;
mwmID = GetRoadPos(startPoint, startVicinity);
if (startPos.empty() || !IsMyMWM(mwmID))
if (startVicinity.empty() || !IsMyMWM(mwmID))
return StartPointNotFound;
my::Timer timer;
timer.Reset();
vector<RoadPos> 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<RoadPos> 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));

5
routing/road_graph_router.hpp
View file

@ -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<RoadPos> const & startPos,
vector<RoadPos> const & finalPos,
virtual ResultCode CalculateRouteP2P(RoadPos const & startPos, RoadPos const & finalPos,
vector<RoadPos> & route) = 0;
virtual void SetRoadGraph(unique_ptr<IRoadGraph> && 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<RoadPos> & pos);
bool IsMyMWM(size_t mwmID) const;
unique_ptr<IRoadGraph> m_roadGraph;

6
routing/routing_tests/astar_algorithm_test.cpp
View file

@ -51,13 +51,11 @@ void TestAStar(UndirectedGraph const & graph, vector<unsigned> const & expectedR
TAlgorithm algo;
algo.SetGraph(graph);
vector<unsigned> actualRoute;
TEST_EQUAL(TAlgorithm::Result::OK,
algo.FindPath(vector<unsigned>{0}, vector<unsigned>{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<unsigned>{0}, vector<unsigned>{4}, actualRoute), ());
TEST_EQUAL(TAlgorithm::Result::OK, algo.FindPathBidirectional(0u, 4u, actualRoute), ());
TEST_EQUAL(expectedRoute, actualRoute, ());
}

12
routing/routing_tests/astar_router_test.cpp
View file

@ -15,7 +15,7 @@
using namespace routing;
using namespace routing_test;
void TestAStarRouterMock(vector<RoadPos> const & startPos, vector<RoadPos> const & finalPos,
void TestAStarRouterMock(RoadPos const & startPos, RoadPos const & finalPos,
m2::PolylineD const & expected)
{
AStarRouter router;
@ -25,7 +25,7 @@ void TestAStarRouterMock(vector<RoadPos> const & startPos, vector<RoadPos> const
router.SetRoadGraph(move(graph));
}
vector<RoadPos> 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<RoadPos> startPos = {RoadPos(1, true /* forward */, 0, m2::PointD(0, 0))};
vector<RoadPos> 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<RoadPos> startPos = {RoadPos(7, false /* forward */, 0, m2::PointD(80, 55))};
vector<RoadPos> 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)};