Merge pull request #4277 from ygorshenin/fix-routingmsaa

[routing] Fixed fake edges construction.

9 files changed, 685 insertions, 199 deletions

diff --git a/routing/directions_engine.cpp b/routing/directions_engine.cpp
index 6b162e5d51..53d5ed602a 100644
--- a/routing/directions_engine.cpp
+++ b/routing/directions_engine.cpp
@@ -13,7 +13,7 @@ void IDirectionsEngine::CalculateTimes(IRoadGraph const & graph, vector<Junction
                                        Route::TTimes & times) const
 {
   times.clear();
-  if (path.size() <= 1)
+  if (path.size() < 1)
     return;
 
   // graph.GetMaxSpeedKMPH() below is used on purpose.
diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp
index c7ed2612c4..db72d0bb2c 100644
--- a/routing/road_graph.cpp
+++ b/routing/road_graph.cpp
@@ -6,9 +6,11 @@
 #include "geometry/mercator.hpp"
 
 #include "geometry/distance_on_sphere.hpp"
+#include "geometry/segment2d.hpp"
 
 #include "base/assert.hpp"
 #include "base/math.hpp"
+#include "base/stl_helpers.hpp"
 
 #include "std/limits.hpp"
 #include "std/sstream.hpp"
@@ -17,50 +19,19 @@ namespace routing
 {
 namespace
 {
-vector<Edge>::const_iterator FindEdgeContainingPoint(vector<Edge> const & edges, m2::PointD const & pt)
+bool OnEdge(Junction const & p, Edge const & ab)
 {
-  // A           P               B
-  // o-----------x---------------o
-
-  auto const liesOnEdgeFn = [&pt](Edge const & e)
-  {
-    // Point P lies on edge AB if:
-    // - P corresponds to edge's start point A or edge's end point B
-    // - angle between PA and PB is 180 degrees
-
-    m2::PointD const v1 = e.GetStartJunction().GetPoint() - pt;
-    m2::PointD const v2 = e.GetEndJunction().GetPoint() - pt;
-    if (my::AlmostEqualAbs(v1, m2::PointD::Zero(), kPointsEqualEpsilon)
-        || my::AlmostEqualAbs(v2, m2::PointD::Zero(), kPointsEqualEpsilon))
-    {
-      // Point p corresponds to the start or the end of the edge.
-      return true;
-    }
-
-    // If an angle between two vectors is 180 degrees then dot product is negative and cross product is 0.
-    if (m2::DotProduct(v1, v2) < 0.0)
-    {
-        double constexpr kEpsilon = 1e-9;
-        if (my::AlmostEqualAbs(m2::CrossProduct(v1, v2), 0.0, kEpsilon))
-        {
-            // Point p lies on edge.
-            return true;
-        }
-    }
-
-    return false;
-  };
-
-  return find_if(edges.begin(), edges.end(), liesOnEdgeFn);
+  auto const & a = ab.GetStartJunction();
+  auto const & b = ab.GetEndJunction();
+  return m2::IsPointOnSegment(p.GetPoint(), a.GetPoint(), b.GetPoint());
 }
 
-/// \brief Reverses |edges| starting from index |beginIdx| and upto the end of |v|.
-void ReverseEdges(size_t beginIdx, IRoadGraph::TEdgeVector & edges)
+void SplitEdge(Edge const & ab, Junction const & p, vector<Edge> & edges)
 {
-  ASSERT_LESS_OR_EQUAL(beginIdx, edges.size(), ("Index too large."));
-
-  for (size_t i = beginIdx; i < edges.size(); ++i)
-    edges[i] = edges[i].GetReverseEdge();
+  auto const & a = ab.GetStartJunction();
+  auto const & b = ab.GetEndJunction();
+  edges.push_back(Edge::MakeFake(a, p));
+  edges.push_back(Edge::MakeFake(p, b));
 }
 }  // namespace
 
@@ -74,7 +45,7 @@ Junction::Junction(m2::PointD const & point, feature::TAltitude altitude)
 string DebugPrint(Junction const & r)
 {
   ostringstream ss;
-  ss << "Junction{point:" << DebugPrint(r.m_point) << ", altitude:}" << r.GetAltitude();
+  ss << "Junction{point:" << DebugPrint(r.m_point) << ", altitude:" << r.GetAltitude() << "}";
   return ss.str();
 }
 
@@ -199,172 +170,56 @@ void IRoadGraph::GetRegularIngoingEdges(Junction const & junction, TEdgeVector &
 
 void IRoadGraph::GetFakeOutgoingEdges(Junction const & junction, TEdgeVector & edges) const
 {
-  auto const itr = m_outgoingEdges.find(junction);
-  if (itr == m_outgoingEdges.cend())
-    return;
-
-  edges.reserve(edges.size() + itr->second.size());
-  edges.insert(edges.end(), itr->second.begin(), itr->second.end());
+  auto const it = m_fakeOutgoingEdges.find(junction);
+  if (it != m_fakeOutgoingEdges.cend())
+    edges.insert(edges.end(), it->second.cbegin(), it->second.cend());
 }
 
 void IRoadGraph::GetFakeIngoingEdges(Junction const & junction, TEdgeVector & edges) const
 {
-  size_t const oldSize = edges.size();
-  GetFakeOutgoingEdges(junction, edges);
-  ReverseEdges(oldSize, edges);
+  auto const it = m_fakeIngoingEdges.find(junction);
+  if (it != m_fakeIngoingEdges.cend())
+    edges.insert(edges.end(), it->second.cbegin(), it->second.cend());
 }
 
 void IRoadGraph::ResetFakes()
 {
-  m_outgoingEdges.clear();
+  m_fakeOutgoingEdges.clear();
+  m_fakeIngoingEdges.clear();
 }
+
 void IRoadGraph::AddFakeEdges(Junction const & junction,
                               vector<pair<Edge, Junction>> const & vicinity)
 {
   for (auto const & v : vicinity)
   {
-    Edge const & closestEdge = v.first;
+    Edge const & ab = v.first;
     Junction const p = v.second;
 
-    if (p == closestEdge.GetStartJunction() || p == closestEdge.GetEndJunction())
-    {
-      // The point is mapped on the start junction of the edge or on the end junction of the edge:
-      //        o M                                          o M
-      //        ^                                            ^
-      //        |                                            |
-      //        |                                            |
-      //  (P) A o--------------->o B  or  A o--------------->o B (P)  (the feature is A->B)
-      // Here AB is a feature, M is a junction, which is projected to A (where P is projection),
-      // P is the closest junction of the feature to the junction M.
-
-      // Add outgoing edges for M.
-      TEdgeVector & edgesM = m_outgoingEdges[junction];
-      edgesM.push_back(Edge::MakeFake(junction, p));
-
-      // Add outgoing edges for P.
-      TEdgeVector & edgesP = m_outgoingEdges[p];
-      GetRegularOutgoingEdges(p, edgesP);
-      edgesP.push_back(Edge::MakeFake(p, junction));
-    }
-    else
-    {
-      Edge edgeToSplit = closestEdge;
-
-      vector<Edge> splittingToFakeEdges;
-      if (HasBeenSplitToFakes(closestEdge, splittingToFakeEdges))
-      {
-        // Edge AB has already been split by some point Q and this point P
-        // should split AB one more time
-        //            o M
-        //            ^
-        //            |
-        //            |
-        // A o<-------x--------------x------------->o B
-        //            P              Q
-
-        auto const itr = FindEdgeContainingPoint(splittingToFakeEdges, p.GetPoint());
-        CHECK(itr != splittingToFakeEdges.end(), ());
-
-        edgeToSplit = *itr;
-      }
-      else
-      {
-        // The point P is mapped in the middle of the feature AB
-
-        TEdgeVector & edgesA = m_outgoingEdges[edgeToSplit.GetStartJunction()];
-        if (edgesA.empty())
-          GetRegularOutgoingEdges(edgeToSplit.GetStartJunction(), edgesA);
-
-        TEdgeVector & edgesB = m_outgoingEdges[edgeToSplit.GetEndJunction()];
-        if (edgesB.empty())
-          GetRegularOutgoingEdges(edgeToSplit.GetEndJunction(), edgesB);
-      }
-
-      //            o M
-      //            ^
-      //            |
-      //            |
-      // A o<-------x------->o B
-      //            P
-      // Here AB is the edge to split, M is a junction and P is the projection of M on AB,
-
-      // Edge AB is split into two fake edges AP and PB (similarly BA edge has been split into two
-      // fake edges BP and PA). In the result graph edges AB and BA are redundant, therefore edges AB and BA are
-      // replaced by fake edges AP + PB and BP + PA.
-
-      Edge const ab = edgeToSplit;
-
-      Edge const pa(ab.GetFeatureId(), false /* forward */, ab.GetSegId(), p, ab.GetStartJunction());
-      Edge const pb(ab.GetFeatureId(), true /* forward */, ab.GetSegId(), p, ab.GetEndJunction());
-      Edge const pm = Edge::MakeFake(p, junction);
-
-      // Add outgoing edges to point P.
-      TEdgeVector & edgesP = m_outgoingEdges[p];
-      edgesP.push_back(pa);
-      edgesP.push_back(pb);
-      edgesP.push_back(pm);
-
-      // Add outgoing edges for point M.
-      m_outgoingEdges[junction].push_back(pm.GetReverseEdge());
-
-      // Replace AB edge with AP edge.
-      TEdgeVector & edgesA = m_outgoingEdges[pa.GetEndJunction()];
-      Edge const ap = pa.GetReverseEdge();
-      edgesA.erase(remove_if(edgesA.begin(), edgesA.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(ap); }), edgesA.end());
-      edgesA.push_back(ap);
-
-      // Replace BA edge with BP edge.
-      TEdgeVector & edgesB = m_outgoingEdges[pb.GetEndJunction()];
-      Edge const bp = pb.GetReverseEdge();
-      edgesB.erase(remove_if(edgesB.begin(), edgesB.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(bp); }), edgesB.end());
-      edgesB.push_back(bp);
-    }
-  }
-
-  // m_outgoingEdges may contain duplicates. Remove them.
-  for (auto & m : m_outgoingEdges)
-  {
-    TEdgeVector & edges = m.second;
-    sort(edges.begin(), edges.end());
-    edges.erase(unique(edges.begin(), edges.end()), edges.end());
-  }
-}
+    vector<Edge> edges;
+    SplitEdge(ab, p, edges);
+    edges.push_back(Edge::MakeFake(junction, p));
+    edges.push_back(Edge::MakeFake(p, junction));
 
-bool IRoadGraph::HasBeenSplitToFakes(Edge const & edge, vector<Edge> & fakeEdges) const
-{
-  vector<Edge> tmp;
-
-  if (m_outgoingEdges.find(edge.GetStartJunction()) == m_outgoingEdges.end() ||
-      m_outgoingEdges.find(edge.GetEndJunction()) == m_outgoingEdges.end())
-    return false;
-
-  auto i = m_outgoingEdges.end();
-  Junction junction = edge.GetStartJunction();
-  while (m_outgoingEdges.end() != (i = m_outgoingEdges.find(junction)))
-  {
-    auto const & edges = i->second;
+    ForEachFakeEdge([&](Edge const & uv)
+                    {
+                      if (OnEdge(p, uv))
+                        SplitEdge(uv, p, edges);
+                    });
 
-    auto const j = find_if(edges.begin(), edges.end(), [&edge](Edge const & e) { return e.SameRoadSegmentAndDirection(edge); });
-    if (j == edges.end())
+    for (auto const & uv : edges)
     {
-      ASSERT(fakeEdges.empty(), ());
-      return false;
+      auto const & u = uv.GetStartJunction();
+      auto const & v = uv.GetEndJunction();
+      m_fakeOutgoingEdges[u].push_back(uv);
+      m_fakeIngoingEdges[v].push_back(uv);
     }
-
-    tmp.push_back(*j);
-
-    junction = j->GetEndJunction();
-    if (junction == edge.GetEndJunction())
-      break;
   }
-  if (i == m_outgoingEdges.end())
-    return false;
 
-  if (tmp.empty())
-    return false;
-
-  fakeEdges.swap(tmp);
-  return true;
+  for (auto & m : m_fakeIngoingEdges)
+    my::SortUnique(m.second);
+  for (auto & m : m_fakeOutgoingEdges)
+    my::SortUnique(m.second);
 }
 
 double IRoadGraph::GetSpeedKMPH(Edge const & edge) const
@@ -382,6 +237,15 @@ void IRoadGraph::GetEdgeTypes(Edge const & edge, feature::TypesHolder & types) c
     GetFeatureTypes(edge.GetFeatureId(), types);
 }
 
+string DebugPrint(IRoadGraph::Mode mode)
+{
+  switch (mode)
+  {
+    case IRoadGraph::Mode::ObeyOnewayTag: return "ObeyOnewayTag";
+    case IRoadGraph::Mode::IgnoreOnewayTag: return "IgnoreOnewayTag";
+  }
+}
+
 IRoadGraph::RoadInfo MakeRoadInfoForTesting(bool bidirectional, double speedKMPH,
                                             initializer_list<m2::PointD> const & points)
 {
diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp
index 880377dc71..b93dac3203 100644
--- a/routing/road_graph.hpp
+++ b/routing/road_graph.hpp
@@ -145,9 +145,11 @@ public:
       {
         if (!my::AlmostEqualAbs(m_cross.GetPoint(), roadInfo.m_junctions[i].GetPoint(),
                                 kPointsEqualEpsilon))
+        {
           continue;
+        }
 
-        if (i < roadInfo.m_junctions.size() - 1)
+        if (i + 1 < roadInfo.m_junctions.size())
         {
           // Head of the edge.
           // m_cross
@@ -256,13 +258,28 @@ private:
   /// \brief Finds all ingoing fake edges for junction.
   void GetFakeIngoingEdges(Junction const & junction, TEdgeVector & edges) const;
 
-  /// Determines if the edge has been split by fake edges and if yes returns these fake edges.
-  bool HasBeenSplitToFakes(Edge const & edge, vector<Edge> & fakeEdges) const;
+  template <typename Fn>
+  void ForEachFakeEdge(Fn && fn)
+  {
+    for (auto const & m : m_fakeIngoingEdges)
+    {
+      for (auto const & e : m.second)
+        fn(e);
+    }
 
-  // Map of outgoing edges for junction
-  map<Junction, TEdgeVector> m_outgoingEdges;
+    for (auto const & m : m_fakeOutgoingEdges)
+    {
+      for (auto const & e : m.second)
+        fn(e);
+    }
+  }
+
+  map<Junction, TEdgeVector> m_fakeIngoingEdges;
+  map<Junction, TEdgeVector> m_fakeOutgoingEdges;
 };
 
+string DebugPrint(IRoadGraph::Mode mode);
+
 IRoadGraph::RoadInfo MakeRoadInfoForTesting(bool bidirectional, double speedKMPH,
                                             initializer_list<m2::PointD> const & points);
 
diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp
index 2ce3321978..ef8cc95587 100644
--- a/routing/road_graph_router.cpp
+++ b/routing/road_graph_router.cpp
@@ -96,22 +96,38 @@ bool CheckGraphConnectivity(IRoadGraph const & graph, Junction const & junction,
 void FindClosestEdges(IRoadGraph const & graph, m2::PointD const & point,
                       vector<pair<Edge, Junction>> & vicinity)
 {
-  // WARNING: Take only one vicinity
-  // It is an oversimplification that is not as easily
-  // solved as tuning up this constant because if you set it too high
-  // you risk to find a feature that you cannot in fact reach because of
-  // an obstacle.  Using only the closest feature minimizes (but not
-  // eliminates) this risk.
-
+  // WARNING: Take only one vicinity, with, maybe, its inverse.  It is
+  // an oversimplification that is not as easily solved as tuning up
+  // this constant because if you set it too high you risk to find a
+  // feature that you cannot in fact reach because of an obstacle.
+  // Using only the closest feature minimizes (but not eliminates)
+  // this risk.
   vector<pair<Edge, Junction>> candidates;
   graph.FindClosestEdges(point, kMaxRoadCandidates, candidates);
 
   vicinity.clear();
   for (auto const & candidate : candidates)
   {
-    if (CheckGraphConnectivity(graph, candidate.first.GetStartJunction(), kTestConnectivityVisitJunctionsLimit))
+    auto const & edge = candidate.first;
+    if (CheckGraphConnectivity(graph, edge.GetStartJunction(), kTestConnectivityVisitJunctionsLimit))
     {
       vicinity.emplace_back(candidate);
+
+      // Need to add a reverse edge, if exists, because fake edges
+      // must be added for reverse edge too.
+      IRoadGraph::TEdgeVector revEdges;
+      graph.GetOutgoingEdges(edge.GetEndJunction(), revEdges);
+      for (auto const & revEdge : revEdges)
+      {
+        if (revEdge.GetFeatureId() == edge.GetFeatureId() &&
+            revEdge.GetEndJunction() == edge.GetStartJunction() &&
+            revEdge.GetSegId() == edge.GetSegId())
+        {
+          vicinity.emplace_back(revEdge, candidate.second);
+          break;
+        }
+      }
+
       break;
     }
   }
diff --git a/routing/routing_benchmarks/bicycle_routing_tests.cpp b/routing/routing_benchmarks/bicycle_routing_tests.cpp
new file mode 100644
index 0000000000..5bab41676e
--- /dev/null
+++ b/routing/routing_benchmarks/bicycle_routing_tests.cpp
@@ -0,0 +1,47 @@
+#include "testing/testing.hpp"
+
+#include "routing/routing_benchmarks/helpers.hpp"
+
+#include "routing/bicycle_directions.hpp"
+#include "routing/bicycle_model.hpp"
+#include "routing/road_graph.hpp"
+
+#include "geometry/mercator.hpp"
+
+#include "std/set.hpp"
+#include "std/string.hpp"
+
+namespace
+{
+// Test preconditions: files from the kMapFiles set with '.mwm'
+// extension must be placed in omim/data folder.
+set<string> const kMapFiles = {"Russia_Moscow"};
+
+class BicycleTest : public RoutingTest
+{
+public:
+  BicycleTest() : RoutingTest(routing::IRoadGraph::Mode::ObeyOnewayTag, kMapFiles) {}
+
+protected:
+  // RoutingTest overrides:
+  unique_ptr<routing::IDirectionsEngine> CreateDirectionsEngine() override
+  {
+    unique_ptr<routing::IDirectionsEngine> engine(new routing::BicycleDirectionsEngine(m_index));
+    return engine;
+  }
+
+  unique_ptr<routing::IVehicleModelFactory> CreateModelFactory() override
+  {
+    unique_ptr<routing::IVehicleModelFactory> factory(
+        new SimplifiedModelFactory<routing::BicycleModel>());
+    return factory;
+  }
+};
+
+UNIT_CLASS_TEST(BicycleTest, Smoke)
+{
+  m2::PointD const start = MercatorBounds::FromLatLon(55.79181, 37.56513);
+  m2::PointD const final = MercatorBounds::FromLatLon(55.79369, 37.56054);
+  TestRouters(start, final);
+}
+}  // namespace
diff --git a/routing/routing_benchmarks/helpers.cpp b/routing/routing_benchmarks/helpers.cpp
new file mode 100644
index 0000000000..139a3d7e09
--- /dev/null
+++ b/routing/routing_benchmarks/helpers.cpp
@@ -0,0 +1,144 @@
+#include "routing/routing_benchmarks/helpers.hpp"
+
+#include "testing/testing.hpp"
+
+#include "routing/base/astar_algorithm.hpp"
+#include "routing/features_road_graph.hpp"
+#include "routing/route.hpp"
+#include "routing/router_delegate.hpp"
+
+#include "indexer/classificator_loader.hpp"
+#include "indexer/mwm_set.hpp"
+
+#include "platform/local_country_file.hpp"
+#include "platform/local_country_file_utils.hpp"
+#include "platform/platform.hpp"
+
+#include "geometry/polyline2d.hpp"
+
+#include "base/logging.hpp"
+#include "base/math.hpp"
+#include "base/timer.hpp"
+
+#include "std/limits.hpp"
+
+namespace
+{
+void TestRouter(routing::IRouter & router, m2::PointD const & startPos,
+                m2::PointD const & finalPos, routing::Route & foundRoute)
+{
+  routing::RouterDelegate delegate;
+  LOG(LINFO, ("Calculating routing ...", router.GetName()));
+  routing::Route route("");
+  my::Timer timer;
+  routing::IRouter::ResultCode const resultCode = router.CalculateRoute(
+      startPos, m2::PointD::Zero() /* startDirection */, finalPos, delegate, route);
+  double const elapsedSec = timer.ElapsedSeconds();
+  TEST_EQUAL(routing::IRouter::NoError, resultCode, ());
+  TEST(route.IsValid(), ());
+  m2::PolylineD const & poly = route.GetPoly();
+  TEST(my::AlmostEqualAbs(poly.Front(), startPos, routing::kPointsEqualEpsilon), ());
+  TEST(my::AlmostEqualAbs(poly.Back(), finalPos, routing::kPointsEqualEpsilon), ());
+  LOG(LINFO, ("Route polyline size:", route.GetPoly().GetSize()));
+  LOG(LINFO, ("Route distance, meters:", route.GetTotalDistanceMeters()));
+  LOG(LINFO, ("Elapsed, seconds:", elapsedSec));
+  foundRoute.Swap(route);
+}
+
+m2::PointD GetPointOnEdge(routing::Edge const & e, double posAlong)
+{
+  if (posAlong <= 0.0)
+    return e.GetStartJunction().GetPoint();
+  if (posAlong >= 1.0)
+    return e.GetEndJunction().GetPoint();
+  m2::PointD const d = e.GetEndJunction().GetPoint() - e.GetStartJunction().GetPoint();
+  return e.GetStartJunction().GetPoint() + d * posAlong;
+}
+}  // namespace
+
+RoutingTest::RoutingTest(routing::IRoadGraph::Mode mode, set<string> const & neededMaps)
+  : m_mode(mode)
+{
+  classificator::Load();
+
+  Platform & platform = GetPlatform();
+  m_cig = storage::CountryInfoReader::CreateCountryInfoReader(platform);
+
+  vector<platform::LocalCountryFile> localFiles;
+  platform::FindAllLocalMapsAndCleanup(numeric_limits<int64_t>::max(), localFiles);
+
+  set<string> registeredMaps;
+  for (auto const & localFile : localFiles)
+  {
+    auto const & name = localFile.GetCountryName();
+    if (neededMaps.count(name) == 0)
+      continue;
+
+    UNUSED_VALUE(m_index.RegisterMap(localFile));
+
+    auto const & countryFile = localFile.GetCountryFile();
+    TEST(m_index.IsLoaded(countryFile), ());
+    MwmSet::MwmId const id = m_index.GetMwmIdByCountryFile(countryFile);
+    TEST(id.IsAlive(), ());
+
+    registeredMaps.insert(name);
+  }
+
+  if (registeredMaps != neededMaps)
+  {
+    for (auto const & file : neededMaps)
+    {
+      if (registeredMaps.count(file) == 0)
+        LOG(LERROR, ("Can't find map:", file));
+    }
+    TEST(false, ("Some maps can't be found. See error messages above."));
+  }
+}
+
+void RoutingTest::TestRouters(m2::PointD const & startPos, m2::PointD const & finalPos)
+{
+  // Find route by A*-bidirectional algorithm.
+  routing::Route routeFoundByAstarBidirectional("");
+  {
+    auto router =
+        CreateRouter<routing::AStarBidirectionalRoutingAlgorithm>("test-astar-bidirectional");
+    TestRouter(*router, startPos, finalPos, routeFoundByAstarBidirectional);
+  }
+
+  // Find route by A* algorithm.
+  routing::Route routeFoundByAstar("");
+  {
+    auto router = CreateRouter<routing::AStarRoutingAlgorithm>("test-astar");
+    TestRouter(*router, startPos, finalPos, routeFoundByAstar);
+  }
+
+  double constexpr kEpsilon = 1e-6;
+  TEST(my::AlmostEqualAbs(routeFoundByAstar.GetTotalDistanceMeters(),
+                           routeFoundByAstarBidirectional.GetTotalDistanceMeters(), kEpsilon),
+        ());
+}
+
+void RoutingTest::TestTwoPointsOnFeature(m2::PointD const & startPos, m2::PointD const & finalPos)
+{
+  vector<pair<routing::Edge, routing::Junction>> startEdges;
+  GetNearestEdges(startPos, startEdges);
+  TEST(!startEdges.empty(), ());
+
+  vector<pair<routing::Edge, routing::Junction>> finalEdges;
+  GetNearestEdges(finalPos, finalEdges);
+  TEST(!finalEdges.empty(), ());
+
+  m2::PointD const startPosOnFeature =
+      GetPointOnEdge(startEdges.front().first, 0.0 /* the start point of the feature */);
+  m2::PointD const finalPosOnFeature =
+      GetPointOnEdge(finalEdges.front().first, 1.0 /* the end point of the feature */);
+
+  TestRouters(startPosOnFeature, finalPosOnFeature);
+}
+
+void RoutingTest::GetNearestEdges(m2::PointD const & pt,
+                                  vector<pair<routing::Edge, routing::Junction>> & edges)
+{
+  routing::FeaturesRoadGraph graph(m_index, m_mode, CreateModelFactory());
+  graph.FindClosestEdges(pt, 1 /* count */, edges);
+}
diff --git a/routing/routing_benchmarks/helpers.hpp b/routing/routing_benchmarks/helpers.hpp
new file mode 100644
index 0000000000..bf3a385e7a
--- /dev/null
+++ b/routing/routing_benchmarks/helpers.hpp
@@ -0,0 +1,92 @@
+#pragma once
+
+#include "routing/road_graph.hpp"
+#include "routing/router.hpp"
+#include "routing/vehicle_model.hpp"
+#include "routing/road_graph_router.hpp"
+
+#include "indexer/index.hpp"
+
+#include "storage/country_info_getter.hpp"
+
+#include "geometry/point2d.hpp"
+
+#include "std/set.hpp"
+#include "std/shared_ptr.hpp"
+#include "std/string.hpp"
+#include "std/unique_ptr.hpp"
+#include "std/utility.hpp"
+#include "std/vector.hpp"
+
+class RoutingTest
+{
+public:
+  RoutingTest(routing::IRoadGraph::Mode mode, set<string> const & neededMaps);
+
+  virtual ~RoutingTest() = default;
+
+  void TestRouters(m2::PointD const & startPos, m2::PointD const & finalPos);
+  void TestTwoPointsOnFeature(m2::PointD const & startPos, m2::PointD const & finalPos);
+
+protected:
+  virtual unique_ptr<routing::IDirectionsEngine> CreateDirectionsEngine() = 0;
+  virtual unique_ptr<routing::IVehicleModelFactory> CreateModelFactory() = 0;
+
+  template <typename Algorithm>
+  unique_ptr<routing::IRouter> CreateRouter(string const & name)
+  {
+    auto getter = [&](m2::PointD const & pt) { return m_cig->GetRegionCountryId(pt); };
+    unique_ptr<routing::IRoutingAlgorithm> algorithm(new Algorithm());
+    unique_ptr<routing::IRouter> router(
+        new routing::RoadGraphRouter(name, m_index, getter, m_mode, CreateModelFactory(),
+                                     move(algorithm), CreateDirectionsEngine()));
+    return router;
+  }
+
+  void GetNearestEdges(m2::PointD const & pt,
+                       vector<pair<routing::Edge, routing::Junction>> & edges);
+
+  routing::IRoadGraph::Mode const m_mode;
+  Index m_index;
+  unique_ptr<storage::CountryInfoGetter> m_cig;
+};
+
+template <typename Model>
+class SimplifiedModelFactory : public routing::IVehicleModelFactory
+{
+public:
+  // Since for test purposes we compare routes lengths to check
+  // algorithms consistency, we should use simplified vehicle model,
+  // where all available edges have max speed
+  class SimplifiedModel : public Model
+  {
+  public:
+    // IVehicleModel overrides:
+    //
+    // SimplifiedModel::GetSpeed() filters features and returns zero
+    // speed if feature is not allowed by the base model, or otherwise
+    // some speed depending of road type (0 <= speed <= maxSpeed).  For
+    // tests purposes for all allowed features speed must be the same as
+    // max speed.
+    double GetSpeed(FeatureType const & f) const override
+    {
+      double const speed = Model::GetSpeed(f);
+      if (speed <= 0.0)
+        return 0.0;
+      return Model::GetMaxSpeed();
+    }
+  };
+
+  SimplifiedModelFactory() : m_model(make_shared<SimplifiedModel>()) {}
+
+  // IVehicleModelFactory overrides:
+  shared_ptr<routing::IVehicleModel> GetVehicleModel() const override { return m_model; }
+  shared_ptr<routing::IVehicleModel> GetVehicleModelForCountry(
+      string const & /*country*/) const override
+  {
+    return m_model;
+  }
+
+private:
+  shared_ptr<SimplifiedModel> const m_model;
+};
diff --git a/routing/routing_benchmarks/pedestrian_routing_tests.cpp b/routing/routing_benchmarks/pedestrian_routing_tests.cpp
new file mode 100644
index 0000000000..144688c857
--- /dev/null
+++ b/routing/routing_benchmarks/pedestrian_routing_tests.cpp
@@ -0,0 +1,283 @@
+#include "testing/testing.hpp"
+
+#include "routing/routing_benchmarks/helpers.hpp"
+
+#include "routing/pedestrian_directions.hpp"
+#include "routing/pedestrian_model.hpp"
+#include "routing/road_graph.hpp"
+
+#include "std/set.hpp"
+#include "std/string.hpp"
+
+namespace
+{
+// Test preconditions: files from the kMapFiles set with '.mwm'
+// extension must be placed in omim/data folder.
+set<string> const kMapFiles =
+{
+  "UK_England_East Midlands",
+  "UK_England_East of England_Essex",
+  "UK_England_East of England_Norfolk",
+  "UK_England_Greater London",
+  "UK_England_North East England",
+  "UK_England_North West England_Lancaster",
+  "UK_England_North West England_Manchester",
+  "UK_England_South East_Brighton",
+  "UK_England_South East_Oxford",
+  "UK_England_South West England_Bristol",
+  "UK_England_South West England_Cornwall",
+  "UK_England_West Midlands",
+  "UK_England_Yorkshire and the Humber"
+};
+
+class PedestrianTest : public RoutingTest
+{
+public:
+  PedestrianTest() : RoutingTest(routing::IRoadGraph::Mode::IgnoreOnewayTag, kMapFiles) {}
+
+protected:
+  // RoutingTest overrides:
+  unique_ptr<routing::IDirectionsEngine> CreateDirectionsEngine() override
+  {
+    unique_ptr<routing::IDirectionsEngine> engine(new routing::PedestrianDirectionsEngine());
+    return engine;
+  }
+
+  unique_ptr<routing::IVehicleModelFactory> CreateModelFactory() override
+  {
+    unique_ptr<routing::IVehicleModelFactory> factory(
+        new SimplifiedModelFactory<routing::PedestrianModel>());
+    return factory;
+  }
+};
+}  // namespace
+
+// Tests on features -------------------------------------------------------------------------------
+UNIT_CLASS_TEST(PedestrianTest, UK_Long1)
+{
+  TestTwoPointsOnFeature(m2::PointD(-1.88798, 61.90292), m2::PointD(-2.06025, 61.82824));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Long2)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.20434, 60.27445), m2::PointD(0.06962, 60.33909));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Long3)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.07706, 60.42876), m2::PointD(-0.11058, 60.20991));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Long4)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.48574, 60.05082), m2::PointD(-0.45973, 60.56715));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Long5)
+{
+  TestTwoPointsOnFeature(m2::PointD(0.11646, 60.57330), m2::PointD(0.05767, 59.93019));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Long6)
+{
+  TestTwoPointsOnFeature(m2::PointD(0.02771, 60.49348), m2::PointD(0.06533, 59.93155));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Medium1)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.10461, 60.29721), m2::PointD(-0.07532, 60.35180));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Medium2)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.17925, 60.06331), m2::PointD(-0.09959, 60.06880));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Medium3)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.26440, 60.16831), m2::PointD(-0.20113, 60.20884));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Medium4)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.25296, 60.46539), m2::PointD(-0.10975, 60.43955));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Medium5)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.03115, 60.31819), m2::PointD(0.07400, 60.33662));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Short1)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.10461, 60.29721), m2::PointD(-0.11905, 60.29747));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Short2)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.11092, 60.27172), m2::PointD(-0.08159, 60.27623));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Short3)
+{
+  TestTwoPointsOnFeature(m2::PointD(-0.09449, 60.25051), m2::PointD(-0.06520, 60.26647));
+}
+
+// Tests on points ---------------------------------------------------------------------------------
+UNIT_CLASS_TEST(PedestrianTest, UK_Test1)
+{
+  TestRouters(m2::PointD(-0.23371, 60.18821), m2::PointD(-0.27958, 60.25155));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test2)
+{
+  TestRouters(m2::PointD(-0.23204, 60.22073), m2::PointD(-0.25325, 60.34312));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test3)
+{
+  TestRouters(m2::PointD(-0.13493, 60.21329), m2::PointD(-0.07502, 60.38699));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test4)
+{
+  TestRouters(m2::PointD(0.07362, 60.24965), m2::PointD(0.06262, 60.30536));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test6)
+{
+  TestRouters(m2::PointD(0.12973, 60.28698), m2::PointD(0.16166, 60.32989));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test7)
+{
+  TestRouters(m2::PointD(0.24339, 60.22193), m2::PointD(0.30297, 60.47235));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test9)
+{
+  TestRouters(m2::PointD(0.01390, 60.24852), m2::PointD(-0.01102, 60.29319));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test10)
+{
+  TestRouters(m2::PointD(-1.26084, 60.68840), m2::PointD(-1.34027, 60.37865));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test11)
+{
+  TestRouters(m2::PointD(-1.26084, 60.68840), m2::PointD(-1.34027, 60.37865));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test14)
+{
+  TestRouters(m2::PointD(-0.49921, 60.50093), m2::PointD(-0.42539, 60.46021));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test15)
+{
+  TestRouters(m2::PointD(-0.35293, 60.38324), m2::PointD(-0.27232, 60.48594));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test16)
+{
+  TestRouters(m2::PointD(-0.24521, 60.41771), m2::PointD(0.052673, 60.48102));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test17)
+{
+  TestRouters(m2::PointD(0.60492, 60.36565), m2::PointD(0.59411, 60.31529));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test19)
+{
+  TestRouters(m2::PointD(-0.42411, 60.22511), m2::PointD(-0.44178, 60.37796));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test20)
+{
+  TestRouters(m2::PointD(0.08776, 60.05433), m2::PointD(0.19336, 60.38398));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test21)
+{
+  TestRouters(m2::PointD(0.23038, 60.43846), m2::PointD(0.18335, 60.46692));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test22)
+{
+  TestRouters(m2::PointD(-0.33907, 60.691735), m2::PointD(-0.17824, 60.478512));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test23)
+{
+  TestRouters(m2::PointD(-0.02557, 60.41371), m2::PointD(0.05972, 60.31413));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test24)
+{
+  TestRouters(m2::PointD(-0.12511, 60.23813), m2::PointD(-0.27656, 60.05896));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test25)
+{
+  TestRouters(m2::PointD(-0.12511, 60.23813), m2::PointD(-0.27656, 60.05896));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test26)
+{
+  TestRouters(m2::PointD(-3.04538, 63.44428), m2::PointD(-2.98887, 63.47582));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test27)
+{
+  TestRouters(m2::PointD(-2.94653, 63.61187), m2::PointD(-2.83215, 63.51525));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test28)
+{
+  TestRouters(m2::PointD(-2.85275, 63.42478), m2::PointD(-2.88245, 63.38932));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test29)
+{
+  TestRouters(m2::PointD(-2.35266, 63.59979), m2::PointD(-2.29857, 63.54677));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test30)
+{
+  TestRouters(m2::PointD(-2.22043, 63.41066), m2::PointD(-2.29619, 63.65305));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test31)
+{
+  TestRouters(m2::PointD(-2.28078, 63.66735), m2::PointD(-2.25378, 63.62744));
+}
+
+// This is very slow pedestrian tests (more than 20 minutes).
+#if defined(SLOW_TESTS)
+UNIT_CLASS_TEST(PedestrianTest, UK_Test5)
+{
+  TestRouters(m2::PointD(0.07362, 60.24965), m2::PointD(0.06262, 60.30536));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test8)
+{
+  TestRouters(m2::PointD(-0.09007, 59.93887), m2::PointD(-0.36591, 60.38306));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test12)
+{
+  TestRouters(m2::PointD(-0.41581, 60.05507), m2::PointD(-0.00499, 60.55921));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test13)
+{
+  TestRouters(m2::PointD(-0.00847, 60.17501), m2::PointD(-0.38291, 60.48435));
+}
+
+UNIT_CLASS_TEST(PedestrianTest, UK_Test18)
+{
+  TestTwoPoints(m2::PointD(0.57712, 60.31156), m2::PointD(-1.09911, 59.24341));
+}
+#endif
diff --git a/routing/routing_benchmarks/routing_benchmarks.pro b/routing/routing_benchmarks/routing_benchmarks.pro
new file mode 100644
index 0000000000..e1034ba9e0
--- /dev/null
+++ b/routing/routing_benchmarks/routing_benchmarks.pro
@@ -0,0 +1,23 @@
+TARGET = routing_benchmarks
+CONFIG += console warn_on
+CONFIG -= app_bundle
+TEMPLATE = app
+
+ROOT_DIR = ../../
+DEPENDENCIES = map routing search storage indexer platform editor geometry coding base \
+               osrm jansson protobuf tomcrypt stats_client succinct pugixml
+
+macx-*: LIBS *= "-framework IOKit"
+
+include($$ROOT_DIR/common.pri)
+
+QT *= core
+
+SOURCES += \
+  ../../testing/testingmain.cpp \
+  bicycle_routing_tests.cpp \
+  helpers.cpp \
+  pedestrian_routing_tests.cpp \
+
+HEADERS += \
+  helpers.hpp \