diff --git a/map/extrapolation/extrapolator.cpp b/map/extrapolation/extrapolator.cpp index 9a7474cc74..828325ddbb 100644 --- a/map/extrapolation/extrapolator.cpp +++ b/map/extrapolation/extrapolator.cpp @@ -13,60 +13,61 @@ namespace { uint64_t constexpr kMaxExtrapolationTimeMs = 1000; uint64_t constexpr kExtrapolationPeriodMs = 200; -double constexpr kMaxExtrapolationSpeedMPerS = 120.0; +double constexpr kMaxExtrapolationSpeedMPS = 120.0; -double LinearExtrapolationOfOneParam(double param1, double param2, uint64_t timeBetweenPointsMs, - uint64_t timeAfterPoint2Ms) +class LinearExtrapolator { - return (param2 - param1) * timeAfterPoint2Ms / timeBetweenPointsMs; -} +public: + LinearExtrapolator(uint64_t timeBetweenMs, uint64_t timeAfterMs) + : m_timeBetweenMs(timeBetweenMs), m_timeAfterMs(timeAfterMs) + { + CHECK_NOT_EQUAL(m_timeBetweenMs, 0, ()); + } + + double Extrapolate(double x1, double x2) + { + return x2 + ((x2 - x1) / m_timeBetweenMs) * m_timeAfterMs; + } + +private: + uint64_t m_timeBetweenMs; + uint64_t m_timeAfterMs; +}; } // namespace -namespace position_extrapolator +namespace extrapolation { using namespace std; -location::GpsInfo LinearExtrapolation(location::GpsInfo const & point1, location::GpsInfo const & point2, +location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, + location::GpsInfo const & gpsInfo2, uint64_t timeAfterPoint2Ms) { + CHECK_LESS(gpsInfo1.m_timestamp, gpsInfo2.m_timestamp, ()); auto const timeBetweenPointsMs = - static_cast((point2.m_timestamp - point1.m_timestamp) * 1000); + static_cast((gpsInfo2.m_timestamp - gpsInfo1.m_timestamp) * 1000); - location::GpsInfo extrapolated = point2; + location::GpsInfo result = gpsInfo2; + LinearExtrapolator e(timeBetweenPointsMs, timeAfterPoint2Ms); - extrapolated.m_timestamp += timeAfterPoint2Ms; + result.m_timestamp += timeAfterPoint2Ms; + result.m_longitude = e.Extrapolate(gpsInfo1.m_longitude, gpsInfo2.m_longitude); + result.m_latitude = e.Extrapolate(gpsInfo1.m_latitude, gpsInfo2.m_latitude); + result.m_horizontalAccuracy = e.Extrapolate(gpsInfo1.m_horizontalAccuracy, gpsInfo2.m_horizontalAccuracy); + result.m_altitude = e.Extrapolate(gpsInfo1.m_altitude, gpsInfo2.m_altitude); - extrapolated.m_longitude += LinearExtrapolationOfOneParam( - point1.m_longitude, point2.m_longitude, timeBetweenPointsMs, timeAfterPoint2Ms); + if (gpsInfo1.HasVerticalAccuracy() && gpsInfo2.HasVerticalAccuracy()) + result.m_verticalAccuracy = e.Extrapolate(gpsInfo1.m_verticalAccuracy, gpsInfo2.m_verticalAccuracy); - extrapolated.m_latitude += LinearExtrapolationOfOneParam(point1.m_latitude, point2.m_latitude, - timeBetweenPointsMs, timeAfterPoint2Ms); + // @TODO(bykoianko) Now |result.m_bearing == gpsInfo2.m_bearing|. + // In case of |gpsInfo1.HasBearing() && gpsInfo2.HasBearing() == true| + // consider finding an average value between |gpsInfo1.m_bearing| and |gpsInfo2.m_bearing| + // taking into account that they are periodic. - extrapolated.m_horizontalAccuracy += - LinearExtrapolationOfOneParam(point1.m_horizontalAccuracy, point2.m_horizontalAccuracy, - timeBetweenPointsMs, timeAfterPoint2Ms); - extrapolated.m_altitude += LinearExtrapolationOfOneParam(point1.m_altitude, point2.m_altitude, - timeBetweenPointsMs, timeAfterPoint2Ms); + if (gpsInfo1.HasSpeed() && gpsInfo2.HasSpeed()) + result.m_speed = e.Extrapolate(gpsInfo1.m_speed, gpsInfo2.m_speed); - if (point1.m_verticalAccuracy != -1 && point2.m_verticalAccuracy != -1) - { - extrapolated.m_verticalAccuracy += - LinearExtrapolationOfOneParam(point1.m_verticalAccuracy, point2.m_verticalAccuracy, - timeBetweenPointsMs, timeAfterPoint2Ms); - } - - if (point1.m_bearing != -1 && point2.m_bearing != -1) - { - extrapolated.m_bearing += LinearExtrapolationOfOneParam(point1.m_bearing, point2.m_bearing, - timeBetweenPointsMs, timeAfterPoint2Ms); - } - - if (point1.m_speed != -1 && point2.m_speed != -1) - { - extrapolated.m_speed += LinearExtrapolationOfOneParam(point1.m_speed, point2.m_speed, - timeBetweenPointsMs, timeAfterPoint2Ms); - } - return extrapolated; + return result; } // Extrapolator::Routine --------------------------------------------------------------------------- @@ -94,7 +95,7 @@ void Extrapolator::Routine::Do() } else { - if (m_lastGpsInfo.m_source != location::EUndefine) + if (m_lastGpsInfo.m_source != location::EUndefined) { location::GpsInfo gpsInfo = m_lastGpsInfo; m_extrapolatedLocationUpdate(gpsInfo); @@ -129,8 +130,8 @@ bool Extrapolator::Routine::DoesExtrapolationWork(uint64_t extrapolationTimeMs) // Please see comment in declaration of class GpsInfo for details. if (m_extrapolationCounter == m_extrapolationCounterUndefined || - m_lastGpsInfo.m_source == location::EUndefine || - m_beforeLastGpsInfo.m_source == location::EUndefine || + m_lastGpsInfo.m_source == location::EUndefined || + m_beforeLastGpsInfo.m_source == location::EUndefined || m_beforeLastGpsInfo.m_timestamp >= m_lastGpsInfo.m_timestamp) { return false; @@ -141,8 +142,8 @@ bool Extrapolator::Routine::DoesExtrapolationWork(uint64_t extrapolationTimeMs) m_lastGpsInfo.m_latitude, m_lastGpsInfo.m_longitude); double const timeS = m_lastGpsInfo.m_timestamp - m_beforeLastGpsInfo.m_timestamp; - // Switching off extrapolation based on speed. - return distM / timeS < kMaxExtrapolationSpeedMPerS; + // Switching off extrapolation based on speed. + return distM / timeS < kMaxExtrapolationSpeedMPS; // @TODO(bykoianko) Switching off extrapolation based on acceleration should be implemented. } @@ -153,10 +154,10 @@ Extrapolator::Extrapolator(ExtrapolatedLocationUpdate const & update) m_extrapolatedLocationThread.Create(make_unique(update)); } -void Extrapolator::OnLocationUpdate(location::GpsInfo & info) +void Extrapolator::OnLocationUpdate(location::GpsInfo const & info) { auto * routine = m_extrapolatedLocationThread.GetRoutineAs(); CHECK(routine, ()); routine->SetGpsInfo(info); } -} // namespace position_extrapolator +} // namespace extrapolation diff --git a/map/extrapolation/extrapolator.hpp b/map/extrapolation/extrapolator.hpp index 85ce5502ca..0ed1501e84 100644 --- a/map/extrapolation/extrapolator.hpp +++ b/map/extrapolation/extrapolator.hpp @@ -10,12 +10,12 @@ #include #include -namespace position_extrapolator +namespace extrapolation { /// \brief Returns extrapolated position after |point2| in |timeAfterPoint2Ms|. -/// \note This function is assumed that between |point1| and |point2| passed one second. /// \note |timeAfterPoint2Ms| should be relevantly small (several seconds maximum). -location::GpsInfo LinearExtrapolation(location::GpsInfo const & point1, location::GpsInfo const & point2, +location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, + location::GpsInfo const & gpsInfo2, uint64_t timeAfterPoint2Ms); class Extrapolator @@ -28,7 +28,7 @@ public: /// \param update is a function which is called with params according to extrapolated position. /// |update| will be called on gui thread. explicit Extrapolator(ExtrapolatedLocationUpdate const & update); - void OnLocationUpdate(location::GpsInfo & info); + void OnLocationUpdate(location::GpsInfo const & info); // @TODO(bykoianko) Gyroscope information should be taken into account as well for calculation // extrapolated position. @@ -58,4 +58,4 @@ private: threads::Thread m_extrapolatedLocationThread; }; -} // namespace position_extrapolator +} // namespace extrapolation diff --git a/map/framework.hpp b/map/framework.hpp index f867ee64c5..686d5f4608 100644 --- a/map/framework.hpp +++ b/map/framework.hpp @@ -208,7 +208,7 @@ protected: // Note. |m_routingManager| should be declared before |m_trafficManager| RoutingManager m_routingManager; - position_extrapolator::Extrapolator m_extrapolator; + extrapolation::Extrapolator m_extrapolator; TrafficManager m_trafficManager; diff --git a/map/map_tests/extrapolator_tests.cpp b/map/map_tests/extrapolator_tests.cpp index a93a4ca082..1868cec962 100644 --- a/map/map_tests/extrapolator_tests.cpp +++ b/map/map_tests/extrapolator_tests.cpp @@ -9,7 +9,7 @@ namespace { using namespace location; -using namespace position_extrapolator; +using namespace extrapolation; void TestGpsInfo(GpsInfo const & tested, GpsInfo const & expected) { @@ -45,18 +45,7 @@ UNIT_TEST(LinearExtrapolation) 1.0 /* m_bearing */, 12.0 /* m_speed */}; // 0 ms after |point2|. - { - GpsInfo const expected = {EAppleNative, - 1.0 /* m_timestamp */, - 1.01 /* m_latitude */, - 1.01 /* m_longitude */, - 11.0 /* m_horizontalAccuracy */, - 2.0 /* m_altitude */, - 10.0 /* m_verticalAccuracy */, - 1.0 /* m_bearing */, - 12.0 /* m_speed */}; - TestGpsInfo(LinearExtrapolation(point1, point2, 0 /* timeAfterPoint2Ms */), expected); - } + TestGpsInfo(LinearExtrapolation(point1, point2, 0 /* timeAfterPoint2Ms */), point2); // 100 ms after |point2|. { @@ -67,7 +56,7 @@ UNIT_TEST(LinearExtrapolation) 11.1 /* m_horizontalAccuracy */, 2.1 /* m_altitude */, 10.0 /* m_verticalAccuracy */, - 1.1 /* m_bearing */, + 1.0 /* m_bearing */, 12.2 /* m_speed */}; TestGpsInfo(LinearExtrapolation(point1, point2, 100 /* timeAfterPoint2Ms */), expected); } @@ -81,7 +70,7 @@ UNIT_TEST(LinearExtrapolation) 11.2 /* m_horizontalAccuracy */, 2.2 /* m_altitude */, 10.0 /* m_verticalAccuracy */, - 1.2 /* m_bearing */, + 1.0 /* m_bearing */, 12.4 /* m_speed */}; TestGpsInfo(LinearExtrapolation(point1, point2, 200 /* timeAfterPoint2Ms */), expected); } @@ -95,7 +84,7 @@ UNIT_TEST(LinearExtrapolation) 12.0 /* m_horizontalAccuracy */, 3.0 /* m_altitude */, 10.0 /* m_verticalAccuracy */, - 2.0 /* m_bearing */, + 1.0 /* m_bearing */, 14.0 /* m_speed */}; TestGpsInfo(LinearExtrapolation(point1, point2, 1000 /* timeAfterPoint2Ms */), expected); } diff --git a/platform/location.hpp b/platform/location.hpp index d927d7db21..e108f51ccb 100644 --- a/platform/location.hpp +++ b/platform/location.hpp @@ -27,7 +27,7 @@ namespace location enum TLocationSource { - EUndefine, + EUndefined, EAppleNative, EWindowsNative, EAndroidNative, @@ -42,7 +42,7 @@ namespace location class GpsInfo { public: - TLocationSource m_source = EUndefine; + TLocationSource m_source = EUndefined; /// \note |m_timestamp| is calculated based on platform methods which don't /// guarantee that |m_timestamp| is monotonic. |m_monotonicTimeMs| should be added to /// class |GpsInfo|. This time should be calculated based on Location::getElapsedRealtimeNanos() @@ -58,9 +58,10 @@ namespace location double m_bearing = -1.0; //!< positive degrees from the true North double m_speed = -1.0; //!< metres per second - bool IsValid() const { return m_source != EUndefine; } + bool IsValid() const { return m_source != EUndefined; } bool HasBearing() const { return m_bearing >= 0.0; } bool HasSpeed() const { return m_speed >= 0.0; } + bool HasVerticalAccuracy() const { return m_verticalAccuracy >= 0.0; } }; /// GpsTrackInfo struct describes a point for GPS tracking