From e213be56fc4ec5bf43b482690ac297ae978f3871 Mon Sep 17 00:00:00 2001 From: Vladimir Byko-Ianko Date: Thu, 25 Jul 2019 14:52:23 +0300 Subject: [PATCH] [routing] Adding PositionAccumulator class to keep several last gps positions. --- routing/CMakeLists.txt | 2 + routing/position_accumulator.cpp | 88 +++++++++++++++++++ routing/position_accumulator.hpp | 46 ++++++++++ .../routing/routing.xcodeproj/project.pbxproj | 8 ++ 4 files changed, 144 insertions(+) create mode 100644 routing/position_accumulator.cpp create mode 100644 routing/position_accumulator.hpp diff --git a/routing/CMakeLists.txt b/routing/CMakeLists.txt index 6e3672f51e..f85b2e6373 100644 --- a/routing/CMakeLists.txt +++ b/routing/CMakeLists.txt @@ -83,6 +83,8 @@ set( online_cross_fetcher.hpp pedestrian_directions.cpp pedestrian_directions.hpp + position_accumulator.cpp + position_accumulator.hpp restriction_loader.cpp restriction_loader.hpp restrictions_serialization.cpp diff --git a/routing/position_accumulator.cpp b/routing/position_accumulator.cpp new file mode 100644 index 0000000000..c880d0e767 --- /dev/null +++ b/routing/position_accumulator.cpp @@ -0,0 +1,88 @@ +#include "routing/position_accumulator.hpp" + +#include "geometry/mercator.hpp" + +#include "base/assert.hpp" + +#include + +double constexpr PositionAccumulator::kMinTrackLengthM; +double constexpr PositionAccumulator::kMinGoodSegmentLengthM; +double constexpr PositionAccumulator::kMaxGoodSegmentLengthM; +double constexpr PositionAccumulator::kMinValidSegmentLengthM; +double constexpr PositionAccumulator::kMaxValidSegmentLengthM; + +void PositionAccumulator::PushNextPoint(m2::PointD const & point) +{ + double const lenM = + m_points.empty() ? 0.0 : MercatorBounds::DistanceOnEarth(point, m_points.back()); + + // Only the last segment is used if it has a good length. + if (lenM >= kMinGoodSegmentLengthM && lenM <= kMaxGoodSegmentLengthM) + { + m_points.erase(m_points.begin(), m_points.end() - 1); + m_trackLengthM = lenM; + m_points.push_back(point); + CHECK_EQUAL(m_points.size(), 2, ()); + return; + } + + // If the last segment is too long it tells nothing about an end user direction. + // And the history is not actual. + if (lenM > kMaxValidSegmentLengthM) + { + Clear(); + return; + } + + // If the last segment is too short it means an end user stays we there's no information + // about it's direction. If m_points.empty() == true it means |point| is the first point. + if (!m_points.empty() && lenM < kMinValidSegmentLengthM) + return; + + // If |m_points| is empty |point| should be added any way. + // If the size of |m_points| is 1 and |lenM| is valid |point| should be added. + if (m_points.size() < 2) + { + CHECK_EQUAL(m_trackLengthM, 0.0, ()); + m_trackLengthM = lenM; + m_points.push_back(point); + return; + } + + // If after adding |point| to |m_points| and removing the farthest point the segment length + // is less than |kMinTrackLengthM| we just adding |point|. + double farthestSegmentLenM = MercatorBounds::DistanceOnEarth(m_points[1], m_points[0]); + if (m_trackLengthM + lenM - farthestSegmentLenM <= kMinTrackLengthM) + { + m_trackLengthM += lenM; + m_points.push_back(point); + return; + } + + // Removing the farthest point if length of the track |m_points[1]|, ..., |m_points.back()|, |point| + // is more than |kMinTrackLengthM|. + while (m_trackLengthM + lenM - farthestSegmentLenM > kMinTrackLengthM && m_points.size() > 2) + { + m_trackLengthM -= farthestSegmentLenM; + m_points.pop_front(); + farthestSegmentLenM = MercatorBounds::DistanceOnEarth(m_points[1], m_points[0]); + } + m_trackLengthM += lenM; + m_points.push_back(point); + // @TODO(bykoianko) Error in |m_trackLengthM| may be accumulated. +} + +void PositionAccumulator::PositionAccumulator::Clear() +{ + m_points.clear(); + m_trackLengthM = 0.0; +} + +m2::PointD PositionAccumulator::PositionAccumulator::GetDirection() const +{ + if (m_points.size() <= 1) + return m2::PointD::Zero(); + + return m_points.back() - m_points.front(); +} diff --git a/routing/position_accumulator.hpp b/routing/position_accumulator.hpp new file mode 100644 index 0000000000..459c6b2c1a --- /dev/null +++ b/routing/position_accumulator.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "geometry/point2d.hpp" + +#include + +/// \brief This class accumulates a track of some number of last gps positions. +/// The track is used to calculate the direction of an end user. +class PositionAccumulator +{ +public: + // Several last gps positions are accumulating in |PositionAccumulator::m_points|. + // The last positions may be removed from |PositionAccumulator::m_points| in several cases. + // 1. If after the removing the last point the length of the track will be more than |kMinTrackLengthM|. + static double constexpr kMinTrackLengthM = 70.0; + // 2. All points from |m_points| except for |m_points.back()| if the distance between + // |m_points.back()| and next point (|point|) is more than + static double constexpr kMinGoodSegmentLengthM = 9.0; + // and more than + static double constexpr kMaxGoodSegmentLengthM = 80.0; + // Than means that only last segment is used to get the direction + // |PositionAccumulator::GetDirection()| if a driver goes quick enough. + + // All segments which are shorter than + static double constexpr kMinValidSegmentLengthM = 1.0; + // and longer than + static double constexpr kMaxValidSegmentLengthM = kMaxGoodSegmentLengthM; + // should be removed. + + void PushNextPoint(m2::PointD const & point); + void Clear(); + + /// \returns direction or {0.0, 0.0} if |m_points| is empty or contains only one item. + m2::PointD GetDirection() const; + + // Getters for testing this class. + std::deque const & GetPointsForTesting() const { return m_points; } + double GetTrackLengthMForTesting() const { return m_trackLengthM; } + +private: + // There's the current position at the end. At the beginning there's the last position of the track. + // The distance between them is more or equal than |kMinTrackLengthM| if there're enough points in + // |m_points|. + std::deque m_points; + double m_trackLengthM = 0.0; +}; diff --git a/xcode/routing/routing.xcodeproj/project.pbxproj b/xcode/routing/routing.xcodeproj/project.pbxproj index fc4d636999..f07ff42a82 100644 --- a/xcode/routing/routing.xcodeproj/project.pbxproj +++ b/xcode/routing/routing.xcodeproj/project.pbxproj @@ -148,6 +148,8 @@ 56CA09E71E30E73B00D05C9A /* restriction_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56CA09E21E30E73B00D05C9A /* restriction_test.cpp */; }; 56CA09E91E30F19800D05C9A /* libtraffic.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56CA09E81E30F19800D05C9A /* libtraffic.a */; }; 56CA630A1F619C1000E6681B /* road_graph_tests.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56CA63061F61206700E6681B /* road_graph_tests.cpp */; }; + 56CBED5522E9CE2600D51AF7 /* position_accumulator.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56CBED5322E9CE2500D51AF7 /* position_accumulator.hpp */; }; + 56CBED5622E9CE2600D51AF7 /* position_accumulator.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56CBED5422E9CE2600D51AF7 /* position_accumulator.cpp */; }; 56CC5A371E3884960016AC46 /* cross_mwm_index_graph.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56CC5A361E3884960016AC46 /* cross_mwm_index_graph.hpp */; }; 56EA2FD51D8FD8590083F01A /* routing_helpers.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56EA2FD41D8FD8590083F01A /* routing_helpers.hpp */; }; 56ED7DBD1F69425700B67156 /* turn_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 567E9F741F5850460064CB96 /* turn_test.cpp */; }; @@ -455,6 +457,8 @@ 56CA09E21E30E73B00D05C9A /* restriction_test.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = restriction_test.cpp; sourceTree = ""; }; 56CA09E81E30F19800D05C9A /* libtraffic.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libtraffic.a; path = "/Users/vladimirbykoyanko/src_github_master/omim/xcode/traffic/../../../omim-build/xcode/Debug/libtraffic.a"; sourceTree = ""; }; 56CA63061F61206700E6681B /* road_graph_tests.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = road_graph_tests.cpp; sourceTree = ""; }; + 56CBED5322E9CE2500D51AF7 /* position_accumulator.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = position_accumulator.hpp; sourceTree = ""; }; + 56CBED5422E9CE2600D51AF7 /* position_accumulator.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = position_accumulator.cpp; sourceTree = ""; }; 56CC5A361E3884960016AC46 /* cross_mwm_index_graph.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = cross_mwm_index_graph.hpp; sourceTree = ""; }; 56EA2FD41D8FD8590083F01A /* routing_helpers.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = routing_helpers.hpp; sourceTree = ""; }; 56EE14DC1FE812FC0036F20C /* libtransit.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libtransit.a; sourceTree = BUILT_PRODUCTS_DIR; }; @@ -845,6 +849,8 @@ 675343FA1A3F640D00A0A8C3 /* routing */ = { isa = PBXGroup; children = ( + 56CBED5422E9CE2600D51AF7 /* position_accumulator.cpp */, + 56CBED5322E9CE2500D51AF7 /* position_accumulator.hpp */, 4443DC3522789793000C8E32 /* leaps_postprocessor.cpp */, 4443DC3622789793000C8E32 /* leaps_postprocessor.hpp */, 44A95C6F225F6A4F00C22F4F /* astar_graph.hpp */, @@ -1060,6 +1066,7 @@ 674F9BCD1B0A580E00704FFA /* features_road_graph.hpp in Headers */, 40576F781F7A788B000B593B /* fake_vertex.hpp in Headers */, 670EE5741B664796001E8064 /* pedestrian_directions.hpp in Headers */, + 56CBED5522E9CE2600D51AF7 /* position_accumulator.hpp in Headers */, 5694CECB1EBA25F7004576D3 /* road_access_serialization.hpp in Headers */, 0C08AA371DF8324D004195DD /* vehicle_mask.hpp in Headers */, 6753441D1A3F644F00A0A8C3 /* router.hpp in Headers */, @@ -1352,6 +1359,7 @@ 674F9BD61B0A580E00704FFA /* turns_generator.cpp in Sources */, 5694CECE1EBA25F7004576D3 /* vehicle_mask.cpp in Sources */, 0C45D7391F2F75500065C3ED /* routing_settings.cpp in Sources */, + 56CBED5622E9CE2600D51AF7 /* position_accumulator.cpp in Sources */, 0C5FEC691DDE193F0017688C /* road_index.cpp in Sources */, 0CF709361F05172200D5067E /* checkpoints.cpp in Sources */, 671F58BD1B874EC80032311E /* followed_polyline.cpp in Sources */,