Implementation of algorithm of openlr matching with score algorithm.

1 files changed, 120 insertions, 14 deletions

diff --git a/openlr/openlr_decoder.cpp b/openlr/openlr_decoder.cpp
index 95d33b6519..a521f0eba6 100644
--- a/openlr/openlr_decoder.cpp
+++ b/openlr/openlr_decoder.cpp
@@ -10,6 +10,10 @@
 #include "openlr/paths_connector.hpp"
 #include "openlr/road_info_getter.hpp"
 #include "openlr/router.hpp"
+#include "openlr/score_candidate_paths_getter.hpp"
+#include "openlr/score_candidate_points_getter.hpp"
+#include "openlr/score_paths_connector.hpp"
+#include "openlr/score_types.hpp"
 #include "openlr/way_point.hpp"
 
 #include "routing/features_road_graph.hpp"
@@ -17,11 +21,11 @@
 
 #include "routing_common/car_model.hpp"
 
+#include "storage/country_info_getter.hpp"
+
 #include "indexer/classificator.hpp"
 #include "indexer/data_source.hpp"
 
-#include "storage/country_info_getter.hpp"
-
 #include "platform/country_file.hpp"
 
 #include "geometry/mercator.hpp"
@@ -166,7 +170,8 @@ void ExpandFakes(DataSource const & dataSource, Graph & g, Graph::EdgeVector & p
 // to some point alog that path and drop everything form the start to that point or from
 // that point to the end.
 template <typename InputIterator>
-InputIterator CutOffset(InputIterator start, InputIterator const stop, double const offset)
+InputIterator CutOffset(InputIterator start, InputIterator stop, double offset,
+                        bool keepEnds)
 {
   if (offset == 0)
     return start;
@@ -177,7 +182,7 @@ InputIterator CutOffset(InputIterator start, InputIterator const stop, double co
     if (distance <= offset && offset < distance + edgeLen)
     {
       // Throw out this edge if (offest - distance) is greater than edgeLength / 2.
-      if (2 * (offset - distance) >= edgeLen)
+      if (!keepEnds && 2 * (offset - distance) >= edgeLen)
         ++start;
       break;
     }
@@ -188,21 +193,24 @@ InputIterator CutOffset(InputIterator start, InputIterator const stop, double co
 }
 
 template <typename InputIterator, typename OutputIterator>
-void CopyWithoutOffsets(InputIterator const start, InputIterator const stop, OutputIterator out,
-                        uint32_t const positiveOffset, uint32_t const negativeOffset)
+void CopyWithoutOffsets(InputIterator start, InputIterator stop, OutputIterator out,
+                        uint32_t positiveOffset, uint32_t negativeOffset, bool keepEnds)
 {
   auto from = start;
   auto to = stop;
 
   if (distance(start, stop) > 1)
   {
-    from = CutOffset(start, stop, positiveOffset);
+    from = CutOffset(start, stop, positiveOffset, keepEnds);
     // |to| points past the last edge we need to take.
     to = CutOffset(reverse_iterator<InputIterator>(stop), reverse_iterator<InputIterator>(start),
-                   negativeOffset)
+                   negativeOffset, keepEnds)
              .base();
   }
 
+  if (!keepEnds)
+    CHECK(from <= to, ("From iterator is less or equal than to."));
+
   if (from >= to)
     return;
 
@@ -288,9 +296,9 @@ public:
 
   bool DecodeSegment(LinearSegment const & segment, DecodedPath & path, v2::Stats & stat)
   {
-    double const kPathLengthTolerance = 0.30;
-    uint32_t const kMaxJunctionCandidates = 10;
-    uint32_t const kMaxProjectionCandidates = 5;
+    double constexpr kPathLengthTolerance = 0.30;
+    uint32_t constexpr kMaxJunctionCandidates = 10;
+    uint32_t constexpr kMaxProjectionCandidates = 5;
 
     m_graph.ResetFakes();
 
@@ -302,8 +310,8 @@ public:
     lineCandidates.reserve(points.size());
     LOG(LDEBUG, ("Decoding segment:", segment.m_segmentId, "with", points.size(), "points"));
 
-    CandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates, m_dataSource,
-                                       m_graph);
+    CandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates,
+                                       m_dataSource, m_graph);
     CandidatePathsGetter pathsGetter(pointsGetter, m_graph, m_infoGetter, stat);
 
     if (!pathsGetter.GetLineCandidatesForPoints(points, lineCandidates))
@@ -346,7 +354,99 @@ public:
     ExpandFakes(m_dataSource, m_graph, route);
     ASSERT(none_of(begin(route), end(route), mem_fn(&Graph::Edge::IsFake)), (segment.m_segmentId));
     CopyWithoutOffsets(begin(route), end(route), back_inserter(path.m_path), positiveOffsetM,
-                       negativeOffsetM);
+                       negativeOffsetM, false /* keep ends */);
+
+    if (path.m_path.empty())
+    {
+      ++stat.m_wrongOffsets;
+      LOG(LINFO, ("Path is empty after offsets cutting. segmentId:", segment.m_segmentId));
+      return false;
+    }
+
+    return true;
+  }
+
+private:
+  DataSource const & m_dataSource;
+  Graph m_graph;
+  RoadInfoGetter m_infoGetter;
+};
+
+// The idea behind the third version of matching algorithm is to collect a lot of candidates (paths)
+// which correspond an openlr edges with some score. And on the final stage to decide which
+// candidate is better.
+class SegmentsDecoderV3
+{
+public:
+  SegmentsDecoderV3(DataSource const & dataSource, unique_ptr<CarModelFactory> cmf)
+      : m_dataSource(dataSource), m_graph(dataSource, move(cmf)), m_infoGetter(dataSource)
+  {
+  }
+
+  bool DecodeSegment(LinearSegment const & segment, DecodedPath & path, v2::Stats & stat)
+  {
+    LOG(LINFO, ("DecodeSegment(...) seg id:", segment.m_segmentId, ", point num:", segment.GetLRPs().size()));
+
+    uint32_t constexpr kMaxJunctionCandidates = 10;
+    uint32_t constexpr kMaxProjectionCandidates = 5;
+
+    path.m_segmentId.Set(segment.m_segmentId);
+
+    auto const & points = segment.GetLRPs();
+    CHECK_GREATER(points.size(), 1, ("A segment cannot consist of less than two points"));
+    vector<ScorePathVec> lineCandidates;
+    lineCandidates.reserve(points.size());
+    LOG(LDEBUG, ("Decoding segment:", segment.m_segmentId, "with", points.size(), "points"));
+
+    ScoreCandidatePointsGetter pointsGetter(kMaxJunctionCandidates, kMaxProjectionCandidates,
+                                            m_dataSource, m_graph);
+    ScoreCandidatePathsGetter pathsGetter(pointsGetter, m_graph, m_infoGetter, stat);
+
+    if (!pathsGetter.GetLineCandidatesForPoints(points, lineCandidates))
+      return false;
+
+    vector<Graph::EdgeVector> resultPath;
+    ScorePathsConnector connector(m_graph, m_infoGetter, stat);
+    if (!connector.FindBestPath(points, lineCandidates, resultPath))
+    {
+      LOG(LINFO, ("Connections not found:", segment.m_segmentId));
+      return false;
+    }
+
+    Graph::EdgeVector route;
+    for (auto const & part : resultPath)
+      route.insert(route.end(), part.begin(), part.end());
+
+    double requiredRouteDistanceM = 0.0;
+    // Sum up all distances between points. Last point's m_distanceToNextPoint
+    // should be equal to zero, but let's skip it just in case.
+    CHECK(!points.empty(), ());
+    for (auto it = points.begin(); it != prev(points.end());  ++it)
+      requiredRouteDistanceM += it->m_distanceToNextPoint;
+
+    double actualRouteDistanceM = 0.0;
+    for (auto const & e : route)
+      actualRouteDistanceM += EdgeLength(e);
+
+    auto const scale = actualRouteDistanceM / requiredRouteDistanceM;
+    LOG(LDEBUG, ("actualRouteDistance:", actualRouteDistanceM,
+        "requiredRouteDistance:", requiredRouteDistanceM, "scale:", scale));
+
+    if (segment.m_locationReference.m_positiveOffsetMeters +
+            segment.m_locationReference.m_negativeOffsetMeters >=
+        requiredRouteDistanceM)
+    {
+      ++stat.m_wrongOffsets;
+      LOG(LINFO, ("Wrong offsets for segment:", segment.m_segmentId));
+      return false;
+    }
+
+    auto const positiveOffsetM = segment.m_locationReference.m_positiveOffsetMeters * scale;
+    auto const negativeOffsetM = segment.m_locationReference.m_negativeOffsetMeters * scale;
+
+    CHECK(none_of(route.begin(), route.end(), mem_fn(&Graph::Edge::IsFake)), (segment.m_segmentId));
+    CopyWithoutOffsets(route.begin(), route.end(), back_inserter(path.m_path), positiveOffsetM,
+                       negativeOffsetM, true /* keep ends */);
 
     if (path.m_path.empty())
     {
@@ -419,6 +519,12 @@ void OpenLRDecoder::DecodeV2(vector<LinearSegment> const & segments, uint32_t co
   Decode<SegmentsDecoderV2, v2::Stats>(segments, numThreads, paths);
 }
 
+void OpenLRDecoder::DecodeV3(vector<LinearSegment> const & segments, uint32_t numThreads,
+                             vector<DecodedPath> & paths)
+{
+  Decode<SegmentsDecoderV3, v2::Stats>(segments, numThreads, paths);
+}
+
 template <typename Decoder, typename Stats>
 void OpenLRDecoder::Decode(vector<LinearSegment> const & segments,
                            uint32_t const numThreads, vector<DecodedPath> & paths)