Repo created

tools/track_analyzing/track_matcher.cpp

@@ -0,0 +1,235 @@
+#include "track_analyzing/track_matcher.hpp"
+
+#include "track_analyzing/exceptions.hpp"
+
+#include "routing/index_graph_loader.hpp"
+
+#include "routing_common/car_model.hpp"
+
+#include "indexer/scales.hpp"
+
+#include "geometry/parametrized_segment.hpp"
+
+#include "base/stl_helpers.hpp"
+
+namespace track_analyzing
+{
+using namespace routing;
+using namespace std;
+using namespace track_analyzing;
+
+namespace
+{
+// Matching range in meters.
+double constexpr kMatchingRange = 20.0;
+
+// Mercator distance from segment to point in meters.
+double DistanceToSegment(m2::PointD const & segmentBegin, m2::PointD const & segmentEnd, m2::PointD const & point)
+{
+  m2::ParametrizedSegment<m2::PointD> const segment(segmentBegin, segmentEnd);
+  m2::PointD const projectionPoint = segment.ClosestPointTo(point);
+  return mercator::DistanceOnEarth(point, projectionPoint);
+}
+
+double DistanceToSegment(Segment const & segment, m2::PointD const & point, IndexGraph & indexGraph)
+{
+  auto const & road = indexGraph.GetRoadGeometry(segment.GetFeatureId());
+  return DistanceToSegment(mercator::FromLatLon(road.GetPoint(segment.GetPointId(false))),
+                           mercator::FromLatLon(road.GetPoint(segment.GetPointId(true))), point);
+}
+
+bool EdgesContain(IndexGraph::SegmentEdgeListT const & edges, Segment const & segment)
+{
+  for (auto const & edge : edges)
+    if (edge.GetTarget() == segment)
+      return true;
+
+  return false;
+}
+}  // namespace
+
+// TrackMatcher ------------------------------------------------------------------------------------
+TrackMatcher::TrackMatcher(storage::Storage const & storage, NumMwmId mwmId, platform::CountryFile const & countryFile)
+  : m_mwmId(mwmId)
+  , m_vehicleModel(CarModelFactory({}).GetVehicleModelForCountry(countryFile.GetName()))
+{
+  auto localCountryFile = storage.GetLatestLocalFile(countryFile);
+  CHECK(localCountryFile, ("Can't find latest country file for", countryFile.GetName()));
+  auto registerResult = m_dataSource.Register(*localCountryFile);
+  CHECK_EQUAL(registerResult.second, MwmSet::RegResult::Success, ("Can't register mwm", countryFile.GetName()));
+
+  MwmSet::MwmHandle handle = m_dataSource.GetMwmHandleByCountryFile(countryFile);
+
+  m_graph = make_unique<IndexGraph>(
+      make_shared<Geometry>(GeometryLoader::Create(handle, m_vehicleModel, false /* loadAltitudes */)),
+      EdgeEstimator::Create(VehicleType::Car, *m_vehicleModel, nullptr /* trafficStash */, nullptr /* dataSource */,
+                            nullptr /* numMvmIds */));
+
+  DeserializeIndexGraph(*handle.GetValue(), VehicleType::Car, *m_graph);
+}
+
+void TrackMatcher::MatchTrack(vector<DataPoint> const & track, vector<MatchedTrack> & matchedTracks)
+{
+  m_pointsCount += track.size();
+
+  vector<Step> steps;
+  steps.reserve(track.size());
+  for (auto const & routePoint : track)
+    steps.emplace_back(routePoint);
+
+  for (size_t trackBegin = 0; trackBegin < steps.size();)
+  {
+    for (; trackBegin < steps.size(); ++trackBegin)
+    {
+      steps[trackBegin].FillCandidatesWithNearbySegments(m_dataSource, *m_graph, *m_vehicleModel, m_mwmId);
+      if (steps[trackBegin].HasCandidates())
+        break;
+
+      ++m_nonMatchedPointsCount;
+    }
+
+    if (trackBegin >= steps.size())
+      break;
+
+    size_t trackEnd = trackBegin;
+    for (; trackEnd < steps.size() - 1; ++trackEnd)
+    {
+      Step & nextStep = steps[trackEnd + 1];
+      Step const & prevStep = steps[trackEnd];
+      nextStep.FillCandidates(prevStep, *m_graph);
+      if (!nextStep.HasCandidates())
+        break;
+    }
+
+    steps[trackEnd].ChooseNearestSegment();
+
+    for (size_t i = trackEnd; i > trackBegin; --i)
+      steps[i - 1].ChooseSegment(steps[i], *m_graph);
+
+    ++m_tracksCount;
+
+    matchedTracks.push_back({});
+    MatchedTrack & matchedTrack = matchedTracks.back();
+    for (size_t i = trackBegin; i <= trackEnd; ++i)
+    {
+      Step const & step = steps[i];
+      matchedTrack.emplace_back(step.GetDataPoint(), step.GetSegment());
+    }
+
+    trackBegin = trackEnd + 1;
+  }
+}
+
+// TrackMatcher::Step ------------------------------------------------------------------------------
+TrackMatcher::Step::Step(DataPoint const & dataPoint)
+  : m_dataPoint(dataPoint)
+  , m_point(mercator::FromLatLon(dataPoint.m_latLon))
+{}
+
+void TrackMatcher::Step::FillCandidatesWithNearbySegments(DataSource const & dataSource, IndexGraph const & graph,
+                                                          VehicleModelInterface const & vehicleModel, NumMwmId mwmId)
+{
+  dataSource.ForEachInRect([&](FeatureType & ft)
+  {
+    if (!ft.GetID().IsValid())
+      return;
+
+    if (ft.GetID().m_mwmId.GetInfo()->GetType() != MwmInfo::COUNTRY)
+      return;
+
+    feature::TypesHolder const types(ft);
+    if (!vehicleModel.IsRoad(types))
+      return;
+
+    ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
+
+    for (size_t segIdx = 0; segIdx + 1 < ft.GetPointsCount(); ++segIdx)
+    {
+      double const distance = DistanceToSegment(ft.GetPoint(segIdx), ft.GetPoint(segIdx + 1), m_point);
+      if (distance < kMatchingRange)
+      {
+        AddCandidate(Segment(mwmId, ft.GetID().m_index, static_cast<uint32_t>(segIdx), true /* forward */), distance,
+                     graph);
+
+        if (!vehicleModel.IsOneWay(types))
+        {
+          AddCandidate(Segment(mwmId, ft.GetID().m_index, static_cast<uint32_t>(segIdx), false /* forward */), distance,
+                       graph);
+        }
+      }
+    }
+  }, mercator::RectByCenterXYAndSizeInMeters(m_point, kMatchingRange), scales::GetUpperScale());
+}
+
+void TrackMatcher::Step::FillCandidates(Step const & previousStep, IndexGraph & graph)
+{
+  IndexGraph::SegmentEdgeListT edges;
+
+  for (Candidate const & candidate : previousStep.m_candidates)
+  {
+    Segment const & segment = candidate.GetSegment();
+    m_candidates.emplace_back(segment, DistanceToSegment(segment, m_point, graph));
+
+    edges.clear();
+    graph.GetEdgeList(segment, true /* isOutgoing */, true /* useRoutingOptions */, edges);
+
+    for (SegmentEdge const & edge : edges)
+    {
+      Segment const & target = edge.GetTarget();
+      if (!segment.IsInverse(target))
+        m_candidates.emplace_back(target, DistanceToSegment(target, m_point, graph));
+    }
+  }
+
+  base::SortUnique(m_candidates);
+
+  m_candidates.erase(remove_if(m_candidates.begin(), m_candidates.end(),
+                               [&](Candidate const & candidate) { return candidate.GetDistance() > kMatchingRange; }),
+                     m_candidates.end());
+}
+
+void TrackMatcher::Step::ChooseSegment(Step const & nextStep, IndexGraph & indexGraph)
+{
+  CHECK(!m_candidates.empty(), ());
+
+  double minDistance = numeric_limits<double>::max();
+
+  IndexGraph::SegmentEdgeListT edges;
+  indexGraph.GetEdgeList(nextStep.m_segment, false /* isOutgoing */, true /* useRoutingOptions */, edges);
+  edges.emplace_back(nextStep.m_segment, GetAStarWeightZero<RouteWeight>());
+
+  for (Candidate const & candidate : m_candidates)
+  {
+    if (candidate.GetDistance() < minDistance && EdgesContain(edges, candidate.GetSegment()))
+    {
+      minDistance = candidate.GetDistance();
+      m_segment = candidate.GetSegment();
+    }
+  }
+
+  if (minDistance == numeric_limits<double>::max())
+    MYTHROW(MessageException, ("Can't find previous step for", nextStep.m_segment));
+}
+
+void TrackMatcher::Step::ChooseNearestSegment()
+{
+  CHECK(!m_candidates.empty(), ());
+
+  double minDistance = numeric_limits<double>::max();
+
+  for (Candidate const & candidate : m_candidates)
+  {
+    if (candidate.GetDistance() < minDistance)
+    {
+      minDistance = candidate.GetDistance();
+      m_segment = candidate.GetSegment();
+    }
+  }
+}
+
+void TrackMatcher::Step::AddCandidate(Segment const & segment, double distance, IndexGraph const & graph)
+{
+  if (graph.GetAccessType(segment) == RoadAccess::Type::Yes)
+    m_candidates.emplace_back(segment, distance);
+}
+}  // namespace track_analyzing