one to many routing speed optimization

3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h

@@ -0,0 +1,267 @@
+/*
+
+Copyright (c) 2014, Project OSRM, Dennis Luxen, others
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+*/
+
+#ifndef NMMANY_TO_MANY_ROUTING_H
+#define NMMANY_TO_MANY_ROUTING_H
+
+#include "BasicRoutingInterface.h"
+#include "../DataStructures/SearchEngineData.h"
+#include "../typedefs.h"
+
+#include <boost/assert.hpp>
+
+#include <limits>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+template <class DataFacadeT> class NMManyToManyRouting final : public BasicRoutingInterface<DataFacadeT>
+{
+    using super = BasicRoutingInterface<DataFacadeT>;
+    using QueryHeap = SearchEngineData::QueryHeap;
+    SearchEngineData &engine_working_data;
+
+    struct NodeBucket
+    {
+        unsigned target_id; // essentially a row in the distance matrix
+        EdgeWeight distance;
+        NodeBucket(const unsigned target_id, const EdgeWeight distance)
+            : target_id(target_id), distance(distance)
+        {
+        }
+    };
+    using SearchSpaceWithBuckets = std::unordered_map<NodeID, std::vector<NodeBucket>>;
+
+  public:
+    NMManyToManyRouting(DataFacadeT *facade, SearchEngineData &engine_working_data)
+        : super(facade), engine_working_data(engine_working_data)
+    {
+    }
+
+    ~NMManyToManyRouting() {}
+
+    std::shared_ptr<std::vector<EdgeWeight>> operator()(const PhantomNodeArray &phantom_sources_nodes_array, const PhantomNodeArray &phantom_targets_nodes_array)
+        const
+    {
+        const unsigned number_of_sources = static_cast<unsigned>(phantom_sources_nodes_array.size());
+        const unsigned number_of_targets = static_cast<unsigned>(phantom_targets_nodes_array.size());
+        std::shared_ptr<std::vector<EdgeWeight>> result_table =
+            std::make_shared<std::vector<EdgeWeight>>(number_of_sources * number_of_targets,
+                                                      std::numeric_limits<EdgeWeight>::max());
+
+        engine_working_data.InitializeOrClearFirstThreadLocalStorage(
+            super::facade->GetNumberOfNodes());
+
+        QueryHeap &query_heap = *(engine_working_data.forwardHeap);
+
+        SearchSpaceWithBuckets search_space_with_buckets;
+
+        unsigned target_id = 0;
+        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_targets_nodes_array)
+        {
+            query_heap.Clear();
+            // insert target(s) at distance 0
+
+            for (const PhantomNode &phantom_node : phantom_node_vector)
+            {
+                if (SPECIAL_NODEID != phantom_node.forward_node_id)
+                {
+                    query_heap.Insert(phantom_node.forward_node_id,
+                                      phantom_node.GetForwardWeightPlusOffset(),
+                                      phantom_node.forward_node_id);
+                }
+                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
+                {
+                    query_heap.Insert(phantom_node.reverse_node_id,
+                                      phantom_node.GetReverseWeightPlusOffset(),
+                                      phantom_node.reverse_node_id);
+                }
+            }
+
+            // explore search space
+            while (!query_heap.Empty())
+            {
+                BackwardRoutingStep(target_id, query_heap, search_space_with_buckets);
+            }
+
+            ++target_id;
+        }
+
+        // for each source do forward search
+        unsigned source_id = 0;
+        for (const std::vector<PhantomNode> &phantom_node_vector : phantom_sources_nodes_array)
+        {
+            query_heap.Clear();
+            for (const PhantomNode &phantom_node : phantom_node_vector)
+            {
+                // insert sources at distance 0
+                if (SPECIAL_NODEID != phantom_node.forward_node_id)
+                {
+                    query_heap.Insert(phantom_node.forward_node_id,
+                                      -phantom_node.GetForwardWeightPlusOffset(),
+                                      phantom_node.forward_node_id);
+                }
+                if (SPECIAL_NODEID != phantom_node.reverse_node_id)
+                {
+                    query_heap.Insert(phantom_node.reverse_node_id,
+                                      -phantom_node.GetReverseWeightPlusOffset(),
+                                      phantom_node.reverse_node_id);
+                }
+            }
+
+            // explore search space
+            while (!query_heap.Empty())
+            {
+                ForwardRoutingStep(source_id,
+                                   number_of_targets,
+                                   query_heap,
+                                   search_space_with_buckets,
+                                   result_table);
+
+            }
+
+            ++source_id;
+        }
+        //BOOST_ASSERT(source_id == target_id);
+        return result_table;
+    }
+
+    void ForwardRoutingStep(const unsigned source_id,
+                            const unsigned number_of_locations,
+                            QueryHeap &query_heap,
+                            const SearchSpaceWithBuckets &search_space_with_buckets,
+                            std::shared_ptr<std::vector<EdgeWeight>> result_table) const
+    {
+        const NodeID node = query_heap.DeleteMin();
+        const int source_distance = query_heap.GetKey(node);
+
+        // check if each encountered node has an entry
+        const auto bucket_iterator = search_space_with_buckets.find(node);
+        // iterate bucket if there exists one
+        if (bucket_iterator != search_space_with_buckets.end())
+        {
+            const std::vector<NodeBucket> &bucket_list = bucket_iterator->second;
+            for (const NodeBucket &current_bucket : bucket_list)
+            {
+                // get target id from bucket entry
+                const unsigned target_id = current_bucket.target_id;
+                const int target_distance = current_bucket.distance;
+                const EdgeWeight current_distance =
+                    (*result_table)[source_id * number_of_locations + target_id];
+                // check if new distance is better
+                const EdgeWeight new_distance = source_distance + target_distance;
+                if (new_distance >= 0 && new_distance < current_distance)
+                {
+                    (*result_table)[source_id * number_of_locations + target_id] =
+                        (source_distance + target_distance);
+                }
+            }
+        }
+        if (StallAtNode<true>(node, source_distance, query_heap))
+        {
+            return;
+        }
+        RelaxOutgoingEdges<true>(node, source_distance, query_heap);
+    }
+
+    void BackwardRoutingStep(const unsigned target_id,
+                             QueryHeap &query_heap,
+                             SearchSpaceWithBuckets &search_space_with_buckets) const
+    {
+        const NodeID node = query_heap.DeleteMin();
+        const int target_distance = query_heap.GetKey(node);
+
+        // store settled nodes in search space bucket
+        search_space_with_buckets[node].emplace_back(target_id, target_distance);
+
+        if (StallAtNode<false>(node, target_distance, query_heap))
+        {
+            return;
+        }
+
+        RelaxOutgoingEdges<false>(node, target_distance, query_heap);
+    }
+
+    template <bool forward_direction>
+    inline void
+    RelaxOutgoingEdges(const NodeID node, const EdgeWeight distance, QueryHeap &query_heap) const
+    {
+        for (auto edge : super::facade->GetAdjacentEdgeRange(node))
+        {
+            const auto &data = super::facade->GetEdgeData(edge, node);
+            const bool direction_flag = (forward_direction ? data.forward : data.backward);
+            if (direction_flag)
+            {
+                const NodeID to = super::facade->GetTarget(edge);
+                const int edge_weight = data.distance;
+
+                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
+                const int to_distance = distance + edge_weight;
+
+                // New Node discovered -> Add to Heap + Node Info Storage
+                if (!query_heap.WasInserted(to))
+                {
+                    query_heap.Insert(to, to_distance, node);
+                }
+                // Found a shorter Path -> Update distance
+                else if (to_distance < query_heap.GetKey(to))
+                {
+                    // new parent
+                    query_heap.GetData(to).parent = node;
+                    query_heap.DecreaseKey(to, to_distance);
+                }
+            }
+        }
+    }
+
+    // Stalling
+    template <bool forward_direction>
+    inline bool StallAtNode(const NodeID node, const EdgeWeight distance, QueryHeap &query_heap)
+        const
+    {
+        for (auto edge : super::facade->GetAdjacentEdgeRange(node))
+        {
+            const auto &data = super::facade->GetEdgeData(edge, node);
+            const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
+            if (reverse_flag)
+            {
+                const NodeID to = super::facade->GetTarget(edge);
+                const int edge_weight = data.distance;
+                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
+                if (query_heap.WasInserted(to))
+                {
+                    if (query_heap.GetKey(to) + edge_weight < distance)
+                    {
+                        return true;
+                    }
+                }
+            }
+        }
+        return false;
+    }
+};
+#endif

routing/osrm_router.cpp

@@ -20,11 +20,12 @@
 #include "../coding/reader_wrapper.hpp"
 
 #include "../base/logging.hpp"
+#include "../base/timer.hpp"
 
 #include "../3party/osrm/osrm-backend/DataStructures/SearchEngineData.h"
 #include "../3party/osrm/osrm-backend/Descriptors/DescriptionFactory.h"
 #include "../3party/osrm/osrm-backend/RoutingAlgorithms/ShortestPathRouting.h"
-#include "../3party/osrm/osrm-backend/RoutingAlgorithms/ManyToManyRouting.h"
+#include "../3party/osrm/osrm-backend/RoutingAlgorithms/NToMManyToManyRouting.h"
 
 
 namespace routing
@@ -514,48 +515,36 @@ bool IsRouteExist(RawRouteData const & r)
 
 }
 
-MultiroutingTaskPointT::const_iterator OsrmRouter::FilterWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets,
-                                    std::vector<EdgeWeight> &weightMatrix)
-{
-  for (size_t i = 0; i < sources.size(); ++i)
-    for (size_t j = 0; j < targets.size(); ++j)
-      if (weightMatrix[i*sources.size() + j] < INVALID_EDGE_WEIGHT)
-      {
-        weightMatrix.erase(weightMatrix.begin(), weightMatrix.begin() + i*targets.size());
-        weightMatrix.erase(weightMatrix.begin() + targets.size(), weightMatrix.end());
-        ASSERT(weightMatrix.size() == targets.size(), ());
-        return sources.cbegin() + i;
-      }
-
-  return sources.cend();
-}
-
 void OsrmRouter::FindWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets,
                                     RawDataFacadeT &facade, std::vector<EdgeWeight> &result)
 {
   SearchEngineData engineData;
-  ManyToManyRouting<RawDataFacadeT> pathFinder(&facade, engineData);
+  NMManyToManyRouting<RawDataFacadeT> pathFinder(&facade, engineData);
 
-  size_t const row_size = sources.size() + targets.size();
-  size_t const result_row_size = targets.size();
-  PhantomNodeArray taskVector(row_size);
+  PhantomNodeArray sourcesTaskVector(sources.size());
+  PhantomNodeArray targetsTaskVector(targets.size());
   for (int i = 0; i < sources.size(); ++i)
   {
-    taskVector[i].push_back(sources[i].m_node);
+    sourcesTaskVector[i].push_back(sources[i].m_node);
   }
   for (int i = 0; i < targets.size(); ++i)
   {
-    taskVector[sources.size() + i].push_back(targets[i].m_node);
+    targetsTaskVector[i].push_back(targets[i].m_node);
   }
 
-  std::shared_ptr<std::vector<EdgeWeight>> resultTable = pathFinder(taskVector);
+  my::HighResTimer timer1(true);
+  std::shared_ptr<std::vector<EdgeWeight>> resultTable = pathFinder(sourcesTaskVector, targetsTaskVector);
+  LOG(LINFO, ("Pure one-to many", timer1.ElapsedNano()));
+  timer1.Reset();
 
-  result.resize(sources.size() * result_row_size);
+  /*result.resize(sources.size() * result_row_size);
   auto const start_iter = resultTable->begin()+sources.size();
   for (int i = 0; i < sources.size(); ++i)
   {
     std::copy_n(start_iter + i*row_size, result_row_size, result.begin() + result_row_size*i);
-  }
+  }*/
+  ASSERT_EQUAL(resultTable->size(), sources.size() * targets.size(), ());
+  result.swap(*resultTable);
 }
 
 bool OsrmRouter::FindSingleRoute(FeatureGraphNodeVecT const & source, FeatureGraphNodeVecT const & target, DataFacadeT &facade,
@@ -758,31 +747,28 @@ public:
   LastCrossFinder(RoutingMappingPtrT & mapping, FeatureGraphNodeVecT const & targetTask):m_targetContext(mapping->m_dataFacade.getRoutingContext()), m_mwmName(mapping->GetName())
   {
     auto income_iterators = m_targetContext.GetIngoingIterators();
-    MultiroutingTaskPointT targets;
+    MultiroutingTaskPointT targets(1);
     m_sources.resize(distance(income_iterators.first, income_iterators.second));
 
     for (auto i = income_iterators.first; i < income_iterators.second; ++i)
       OsrmRouter::GenerateRoutingTaskFromNodeId(*i, m_sources[distance(income_iterators.first, i)]);
-    for (auto t : targetTask)
-      targets.push_back(t);
 
     vector<EdgeWeight> weights;
-    OsrmRouter::FindWeightsMatrix(m_sources, targets, mapping->m_dataFacade, weights);
-
-    for (size_t i = 0; i < targets.size(); ++i)
-      for (size_t j = 0; j < m_sources.size(); ++j)
-        if (weights[j * m_sources.size() + i] < INVALID_EDGE_WEIGHT)
+    for (auto t : targetTask)
+    {
+      targets[0] = t;
+      OsrmRouter::FindWeightsMatrix(m_sources, targets, mapping->m_dataFacade, weights);
+      for (auto weight : weights)
+      {
+        if (weight < INVALID_EDGE_WEIGHT)
         {
-          for (int k = m_sources.size() - 1; k > -1; --k)
-          {
-            weights.erase(weights.begin() + k*targets.size(), weights.begin() + k*targets.size() + i);
-            weights.erase(weights.begin() + k*targets.size() + i + 1, weights.begin() + (k + 1) * targets.size());
-          }
-          ASSERT(weights.size() == m_sources.size(), ());
-          m_target = targets[i];
+          ASSERT_EQUAL(weights.size(), m_sources.size(), ());
+          m_target = t;
           m_weights.swap(weights);
           return;
         }
+      }
+    }
     throw OsrmRouter::EndPointNotFound;
   }
 
@@ -810,6 +796,7 @@ public:
 
 OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, Route & route)
 {
+  my::HighResTimer timer(true);
   RoutingMappingPtrT startMapping = m_indexManager.GetMappingByPoint(startPt, m_pIndex);
   RoutingMappingPtrT targetMapping = m_indexManager.GetMappingByPoint(finalPt, m_pIndex);
 
@@ -822,6 +809,8 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
   MappingGuard finalMappingGuard(targetMapping);
   UNUSED_VALUE(startMappingGuard);
   UNUSED_VALUE(finalMappingGuard);
+  LOG(LINFO, ("Loading first MWM", timer.ElapsedNano()));
+  timer.Reset();
 
   // 3. Find start/end nodes.
   FeatureGraphNodeVecT startTask;
@@ -843,6 +832,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
   if (m_requestCancel)
     return Cancelled;
 
+  LOG(LINFO, ("Find start/stop points", timer.ElapsedNano()));
+  timer.Reset();
+
   // 4. Find route.
   RawRoutingResultT routingResult;
 
@@ -878,24 +870,38 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
   else //4.2 Different mwm case
   {
     LOG(LINFO, ("Different mwm routing case"));
-    if (!m_additionalFeatures)
-      return PointsInDifferentMWM;
+    //if (!m_additionalFeatures)
+    //  return PointsInDifferentMWM;
 
     // Load source data
     CrossRoutingContext const & startContext = startMapping->m_dataFacade.getRoutingContext();
     auto out_iterators = startContext.GetOutgoingIterators();
-    MultiroutingTaskPointT sources, targets(distance(out_iterators.first, out_iterators.second));
+    MultiroutingTaskPointT sources(1), targets(distance(out_iterators.first, out_iterators.second));
 
     for (auto i = out_iterators.first; i < out_iterators.second; ++i)
       OsrmRouter::GenerateRoutingTaskFromNodeId(i->first, targets[distance(out_iterators.first, i)]);
-    for (auto t : startTask)
-      sources.push_back(t);
     vector<EdgeWeight> weights;
-    FindWeightsMatrix(sources, targets, startMapping->m_dataFacade, weights);
+    for (auto t : startTask)
+    {
+      sources[0] = t;
+      bool found = false;
+      LOG(LINFO, ("Start case"));
+      FindWeightsMatrix(sources, targets, startMapping->m_dataFacade, weights);
+      for (auto weight : weights)
+      {
+        if (weight < INVALID_EDGE_WEIGHT)
+        {
+          found = true;
+          break;
+        }
+      }
+      if (found)
+        break;
+      else
+        weights.clear();
+    }
 
-    auto sourceNodeIter = FilterWeightsMatrix(sources, targets,weights);
-
-    if (sourceNodeIter == sources.cend())
+    if (!weights.size())
       return StartPointNotFound;
 
     if (m_requestCancel)
@@ -910,6 +916,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
     RoutingTaskQueueT crossTasks;
     CheckedOutsT checkedOuts;
 
+    LOG(LINFO, ("Make one-to-many crosses", timer.ElapsedNano()));
+    timer.Reset();
+
     //Submit tasks from source
     for (size_t j = 0; j < targets.size(); ++j)
     {
@@ -931,7 +940,7 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
           continue;
         checkedOuts.insert(make_pair(tNodeId, startMapping->GetName()));
         CheckedPathT tmpPath;
-        tmpPath.push_back({startMapping->GetName(), *sourceNodeIter, targets[j], weights[j]});
+        tmpPath.push_back({startMapping->GetName(), sources[0], targets[j], weights[j]});
         if (nextMwm != targetMapping->GetName())
         {
 
@@ -1043,9 +1052,17 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
       }
     }
 
+    LOG(LINFO, ("Finding cross path", timer.ElapsedNano()));
+    timer.Reset();
+
     // 5. Make generate answer
     if (finalWeight < INVALID_EDGE_WEIGHT)
-      return MakeRouteFromCrossesPath(finalPath, route);
+    {
+      auto code = MakeRouteFromCrossesPath(finalPath, route);
+      LOG(LINFO, ("Make final route", timer.ElapsedNano()));
+      timer.Reset();
+      return code;
+    }
     else
       return OsrmRouter::RouteNotFound;
   }

routing/osrm_router.hpp

@@ -292,9 +292,6 @@ private:
 
   typedef priority_queue<CheckedPathT, vector<CheckedPathT>, pathChecker> RoutingTaskQueueT;
 
-  MultiroutingTaskPointT::const_iterator FilterWeightsMatrix(MultiroutingTaskPointT const & sources, MultiroutingTaskPointT const & targets,
-                                      std::vector<EdgeWeight> & weightMatrix);
-
   /*!
    * \brief Makes route (points turns and other annotations) and submits it to @route class
    * \param path vector of pathes through mwms