diff --git a/map/extrapolation/extrapolator.cpp b/map/extrapolation/extrapolator.cpp index b70f412cbb..c8370a9fcb 100644 --- a/map/extrapolation/extrapolator.cpp +++ b/map/extrapolation/extrapolator.cpp @@ -5,6 +5,7 @@ #include "platform/platform.hpp" #include "base/logging.hpp" +#include "base/math.hpp" #include #include @@ -54,8 +55,8 @@ location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, LinearExtrapolator e(timeBetweenPointsMs, timeAfterPoint2Ms); result.m_timestamp += static_cast(timeAfterPoint2Ms) / 1000.0; - result.m_longitude = e.Extrapolate(gpsInfo1.m_longitude, gpsInfo2.m_longitude); - result.m_latitude = e.Extrapolate(gpsInfo1.m_latitude, gpsInfo2.m_latitude); + result.m_longitude = my::clamp(e.Extrapolate(gpsInfo1.m_longitude, gpsInfo2.m_longitude), -180.0, 180.0); + result.m_latitude = my::clamp(e.Extrapolate(gpsInfo1.m_latitude, gpsInfo2.m_latitude), -90.0, 90.0); result.m_horizontalAccuracy = e.Extrapolate(gpsInfo1.m_horizontalAccuracy, gpsInfo2.m_horizontalAccuracy); result.m_altitude = e.Extrapolate(gpsInfo1.m_altitude, gpsInfo2.m_altitude); @@ -84,7 +85,33 @@ bool AreCoordsGoodForExtrapolation(location::GpsInfo const & beforeLastGpsInfo, double const distM = ms::DistanceOnEarth(beforeLastGpsInfo.m_latitude, beforeLastGpsInfo.m_longitude, lastGpsInfo.m_latitude, lastGpsInfo.m_longitude); + double const timeS = lastGpsInfo.m_timestamp - beforeLastGpsInfo.m_timestamp; + if (timeS <= 0.0) + return false; + + // |maxDistAfterExtrapolationM| is maximum possible distance from |lastGpsInfo| to + // the furthest extrapolated point. + double const maxDistAfterExtrapolationM = + distM * (Extrapolator::kMaxExtrapolationTimeMs / 1000.0) / timeS; + // |maxDistForAllExtrapolationsM| is maximum possible distance from |lastGpsInfo| to + // all extrapolated points in any cases. + double const maxDistForAllExtrapolationsM = kMaxExtrapolationSpeedMPS / kMaxExtrapolationTimeSeconds; + double const distLastToMeridian180 = ms::DistanceOnEarth( + lastGpsInfo.m_latitude, lastGpsInfo.m_longitude, lastGpsInfo.m_latitude, 180.0 /* lon2Deg */); + // Switching off extrapolation if |lastGpsInfo| are so close to meridian 180 that extrapolated points + // may cross meridian 180 or if |beforeLastGpsInfo| and |lastGpsInfo| are located on + // different sides of meridian 180. + if (distLastToMeridian180 < maxDistAfterExtrapolationM || + (distLastToMeridian180 < maxDistForAllExtrapolationsM && + lastGpsInfo.m_longitude * beforeLastGpsInfo.m_longitude < 0.0) || + ms::DistanceOnEarth(lastGpsInfo.m_latitude, lastGpsInfo.m_longitude, 90.0 /* lat2Deg */, + lastGpsInfo.m_longitude) < maxDistAfterExtrapolationM || + ms::DistanceOnEarth(lastGpsInfo.m_latitude, lastGpsInfo.m_longitude, -90.0 /* lat2Deg */, + lastGpsInfo.m_longitude) < maxDistAfterExtrapolationM) + { + return false; + } // Note. |timeS| may be less than zero. (beforeLastGpsInfo.m_timestampS >= lastGpsInfo.m_timestampS) // It may happen in rare cases because GpsInfo::m_timestampS is not monotonic generally. @@ -164,16 +191,17 @@ void Extrapolator::ExtrapolatedLocationUpdate(uint64_t extrapolatedUpdateCounter void Extrapolator::RunTaskOnBackgroundThread(bool delayed) { + uint64_t extrapolatedUpdateCounter = 0; { lock_guard guard(m_mutex); ++m_extrapolatedUpdateCounter; + extrapolatedUpdateCounter = m_extrapolatedUpdateCounter; } - auto const extrapolatedUpdateCounter = m_extrapolatedUpdateCounter; if (delayed) { - auto constexpr kSExtrapolationPeriod = std::chrono::milliseconds(kExtrapolationPeriodMs); - GetPlatform().RunDelayedTask(Platform::Thread::Background, kSExtrapolationPeriod, + auto constexpr kExtrapolationPeriod = std::chrono::milliseconds(kExtrapolationPeriodMs); + GetPlatform().RunDelayedTask(Platform::Thread::Background, kExtrapolationPeriod, [this, extrapolatedUpdateCounter] { ExtrapolatedLocationUpdate(extrapolatedUpdateCounter); }); diff --git a/map/extrapolation/extrapolator.hpp b/map/extrapolation/extrapolator.hpp index 70cd6556af..fd638f312b 100644 --- a/map/extrapolation/extrapolator.hpp +++ b/map/extrapolation/extrapolator.hpp @@ -29,8 +29,8 @@ public: // extrapolated after last location gotten from GPS. static uint64_t constexpr kMaxExtrapolationTimeMs = 1000; // |kExtrapolationPeriodMs| is time in milliseconds showing how often location will be - // extrapolated. So if the last location was gotten from GPS at time X the next location - // will be emulated by Extrapolator at X + kExtrapolationPeriodMs. + // extrapolated. So if the last location was obtained from GPS at time X the next location + // will be emulated by Extrapolator at time X + kExtrapolationPeriodMs. // Then X + 2 * kExtrapolationPeriodMs and so on till // X + n * kExtrapolationPeriodMs <= kMaxExtrapolationTimeMs. static uint64_t constexpr kExtrapolationPeriodMs = 200; diff --git a/map/extrapolation_benchmark/extrapolation_benchmark.cpp b/map/extrapolation_benchmark/extrapolation_benchmark.cpp index 0c83aaebb4..b05c180ef2 100644 --- a/map/extrapolation_benchmark/extrapolation_benchmark.cpp +++ b/map/extrapolation_benchmark/extrapolation_benchmark.cpp @@ -12,6 +12,7 @@ #include "base/logging.hpp" #include "base/string_utils.hpp" +#include #include #include #include @@ -26,13 +27,8 @@ // This tool is written to estimate quality of location extrapolation. To launch the benchmark // you need tracks in csv file with the format described below. To generate the csv file -// you need to go through following steps: -// * take logs from "trafin" project production in gz files -// * extract them (gzip -d) -// * run track_analyzer tool with unmatched_tracks command to generate csv -// (track_analyzer -cmd unmatched_tracks -in ./trafin_log.20180517-0000) -// * run this tool with csv_path equal to path to the csv -// (extrapolation_benchmark -csv_path=trafin_log.20180517-0000.track.csv) +// you need to run track_analyzer tool with unmatched_tracks command. +// For example: extrapolation_benchmark -csv_path=trafin_log.20180517-0000.track.csv DEFINE_string(csv_path, "", "Path to csv file with user in following format: mwm id (string), aloha id (string), " @@ -61,27 +57,27 @@ struct GpsPoint double m_lon; }; -class Expectation +class MovingAverage { public: void Add(double value) { - m_averageValue = (m_averageValue * m_counter + value) / (m_counter + 1); ++m_counter; + m_movingAverage += (value - m_movingAverage) / m_counter; } - double Get() const { return m_averageValue; } + double Get() const { return m_movingAverage; } size_t GetCounter() const { return m_counter; } private: - double m_averageValue = 0.0; + double m_movingAverage = 0.0; size_t m_counter = 0; }; -class ExpectationVec +class MovingAverageVec { public: - explicit ExpectationVec(size_t size) { m_mes.resize(size); } + explicit MovingAverageVec(size_t size) { m_mes.resize(size); } void Add(vector const & values) { @@ -90,16 +86,16 @@ public: m_mes[i].Add(values[i]); } - vector const & Get() const { return m_mes; } + vector const & Get() const { return m_mes; } private: - vector m_mes; + vector m_mes; }; using Track = vector; using Tracks = vector; -bool GetString(stringstream & lineStream, string & result) +bool GetString(istringstream & lineStream, string & result) { if (!lineStream.good()) return false; @@ -107,7 +103,7 @@ bool GetString(stringstream & lineStream, string & result) return true; } -bool GetDouble(stringstream & lineStream, double & result) +bool GetDouble(istringstream & lineStream, double & result) { string strResult; if (!GetString(lineStream, strResult)) @@ -115,7 +111,7 @@ bool GetDouble(stringstream & lineStream, double & result) return strings::to_double(strResult, result); } -bool GetUint64(stringstream & lineStream, uint64_t & result) +bool GetUint64(istringstream & lineStream, uint64_t & result) { string strResult; if (!GetString(lineStream, strResult)) @@ -123,7 +119,7 @@ bool GetUint64(stringstream & lineStream, uint64_t & result) return strings::to_uint64(strResult, result); } -bool GetGpsPoint(stringstream & lineStream, uint64_t & timestampS, double & lat, double & lon) +bool GetGpsPoint(istringstream & lineStream, uint64_t & timestampS, double & lat, double & lon) { if (!GetUint64(lineStream, timestampS)) return false; @@ -146,11 +142,10 @@ bool Parse(string const & pathToCsv, Tracks & tracks) if (!csvStream.is_open()) return false; - while (!csvStream.eof()) + string line; + while (getline(csvStream, line)) { - string line; - getline(csvStream, line); - stringstream lineStream(line); + istringstream lineStream(line); string dummy; GetString(lineStream, dummy); // mwm id GetString(lineStream, dummy); // aloha id @@ -186,8 +181,8 @@ void GpsPointToGpsInfo(GpsPoint const gpsPoint, GpsInfo & gpsInfo) int main(int argc, char * argv[]) { google::SetUsageMessage( - "Location extrapolation benchmark. Calculates expected value and variance for " - "all extrapolation deviations from tracks passed in csv with with csv_path."); + "Location extrapolation benchmark. Cumulative moving average, variance and standard " + "deviation for all extrapolation deviations from tracks passed in csv with with csv_path."); google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_csv_path.empty()) @@ -229,11 +224,11 @@ int main(int argc, char * argv[]) // For all points of each track in |tracks| some extrapolations will be calculated. // The number of extrapolations depends on |Extrapolator::kExtrapolationPeriodMs| // and |Extrapolator::kMaxExtrapolationTimeMs| and equal for all points. - // Then expected value and variance each extrapolation will be printed. + // Then cumulative moving average and variance each extrapolation will be printed. auto const extrapolationNumber = static_cast(Extrapolator::kMaxExtrapolationTimeMs / Extrapolator::kExtrapolationPeriodMs); - ExpectationVec mes(extrapolationNumber); - ExpectationVec squareMes(extrapolationNumber); + MovingAverageVec mes(extrapolationNumber); + MovingAverageVec squareMes(extrapolationNumber); // Number of extrapolations which projections are calculated successfully for. size_t projectionCounter = 0; for (auto const & t : tracks) @@ -260,8 +255,8 @@ int main(int argc, char * argv[]) break; vector onePointDeviations; - vector onePointDeviationsInSqare; - bool cannotFindClosestProjection = false; + vector onePointDeviationsSquared; + bool canFindClosestProjection = true; for (size_t timeMs = Extrapolator::kExtrapolationPeriodMs; timeMs <= Extrapolator::kMaxExtrapolationTimeMs; timeMs += Extrapolator::kExtrapolationPeriodMs) @@ -294,20 +289,20 @@ int main(int argc, char * argv[]) // This situation is possible if |posRect| param of GetClosestProjectionInInterval() // method is too small and there is no segment in |followedPoly| // which is covered by this rect. It's a rare situation. - cannotFindClosestProjection = true; + canFindClosestProjection = false; break; } double const distFromPoly = MercatorBounds::DistanceOnEarth(iter.m_pt, extrapolatedMerc); onePointDeviations.push_back(distFromPoly); - onePointDeviationsInSqare.push_back(distFromPoly * distFromPoly); + onePointDeviationsSquared.push_back(distFromPoly * distFromPoly); } - if (!cannotFindClosestProjection) + if (canFindClosestProjection) { CHECK_EQUAL(onePointDeviations.size(), extrapolationNumber, ()); mes.Add(onePointDeviations); - squareMes.Add(onePointDeviationsInSqare); + squareMes.Add(onePointDeviationsSquared); } } } @@ -317,13 +312,13 @@ int main(int argc, char * argv[]) " ", mes.Get()[0].GetCounter() * extrapolationNumber, "extrapolations is calculated.\n", " Projection is calculated for", projectionCounter, "extrapolations.")); - LOG(LINFO, ("Expected value for each extrapolation:")); + LOG(LINFO, ("Cumulative moving average, variance and standard deviation for each extrapolation:")); for (size_t i = 0; i < extrapolationNumber; ++i) { - double const variance = squareMes.Get()[i].Get() - mes.Get()[i].Get() * mes.Get()[i].Get(); + double const variance = squareMes.Get()[i].Get() - pow(mes.Get()[i].Get(), 2.0); LOG(LINFO, ("Extrapolation", i + 1, ",", Extrapolator::kExtrapolationPeriodMs * (i + 1), - "seconds after point two. Expected value =", mes.Get()[i].Get(), - "meters.", "Variance =", variance, ". Standard deviation =", sqrt(variance))); + "seconds after point two. Cumulative moving average =", mes.Get()[i].Get(), + "meters.", "Variance =", max(0.0, variance), ". Standard deviation =", sqrt(variance))); } return 0; diff --git a/map/map_tests/extrapolator_tests.cpp b/map/map_tests/extrapolator_tests.cpp index 1868cec962..fefbbc726d 100644 --- a/map/map_tests/extrapolator_tests.cpp +++ b/map/map_tests/extrapolator_tests.cpp @@ -24,28 +24,41 @@ void TestGpsInfo(GpsInfo const & tested, GpsInfo const & expected) TEST(my::AlmostEqualAbs(tested.m_speed, expected.m_speed, kEpsilon), ()); } +GpsInfo GetGpsInfo(double timestampS, double lat, double lon) +{ + return GpsInfo{EAppleNative, + timestampS, + lat, + lon, + 10.0 /* m_horizontalAccuracy */, + 1.0 /* m_altitude */, + 10.0 /* m_verticalAccuracy */, + 0.0 /* m_bearing */, + 10.0 /* m_speed */}; +} + UNIT_TEST(LinearExtrapolation) { - GpsInfo const point1 = {EAppleNative, - 0.0 /* m_timestamp in seconds */, - 1.0 /* m_latitude */, - 1.0 /* m_longitude */, - 10.0 /* m_horizontalAccuracy */, - 1.0 /* m_altitude */, - 10.0 /* m_verticalAccuracy */, - 0.0 /* m_bearing */, - 10.0 /* m_speed */}; - GpsInfo const point2 = {EAppleNative, - 1.0 /* m_timestamp in seconds */, - 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 */}; + GpsInfo const loc1 = {EAppleNative, + 0.0 /* m_timestamp in seconds */, + 1.0 /* m_latitude */, + 1.0 /* m_longitude */, + 10.0 /* m_horizontalAccuracy */, + 1.0 /* m_altitude */, + 10.0 /* m_verticalAccuracy */, + 0.0 /* m_bearing */, + 10.0 /* m_speed */}; + GpsInfo const loc2 = {EAppleNative, + 1.0 /* m_timestamp in seconds */, + 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 */}; // 0 ms after |point2|. - TestGpsInfo(LinearExtrapolation(point1, point2, 0 /* timeAfterPoint2Ms */), point2); + TestGpsInfo(LinearExtrapolation(loc1, loc2, 0 /* timeAfterPoint2Ms */), loc2); // 100 ms after |point2|. { @@ -58,7 +71,7 @@ UNIT_TEST(LinearExtrapolation) 10.0 /* m_verticalAccuracy */, 1.0 /* m_bearing */, 12.2 /* m_speed */}; - TestGpsInfo(LinearExtrapolation(point1, point2, 100 /* timeAfterPoint2Ms */), expected); + TestGpsInfo(LinearExtrapolation(loc1, loc2, 100 /* timeAfterPoint2Ms */), expected); } // 200 ms after |point2|. @@ -72,7 +85,7 @@ UNIT_TEST(LinearExtrapolation) 10.0 /* m_verticalAccuracy */, 1.0 /* m_bearing */, 12.4 /* m_speed */}; - TestGpsInfo(LinearExtrapolation(point1, point2, 200 /* timeAfterPoint2Ms */), expected); + TestGpsInfo(LinearExtrapolation(loc1, loc2, 200 /* timeAfterPoint2Ms */), expected); } // 1000 ms after |point2|. @@ -86,7 +99,81 @@ UNIT_TEST(LinearExtrapolation) 10.0 /* m_verticalAccuracy */, 1.0 /* m_bearing */, 14.0 /* m_speed */}; - TestGpsInfo(LinearExtrapolation(point1, point2, 1000 /* timeAfterPoint2Ms */), expected); + TestGpsInfo(LinearExtrapolation(loc1, loc2, 1000 /* timeAfterPoint2Ms */), expected); + } +} + +UNIT_TEST(AreCoordsGoodForExtrapolation) +{ + { + GpsInfo loc1; + GpsInfo loc2; + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Locations are not valid.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 10.0 /* lat */, 179.999999 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 10.0 /* lat */, -179.999999 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Crossing meridian 180.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 10.0 /* lat */, 179.999997 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 10.0 /* lat */, 179.999999 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Near meridian 180.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 10.0 /* lat */, 179.999997 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 10.0 /* lat */, 179.999998 /* lon */); + TEST(AreCoordsGoodForExtrapolation(loc1, loc2), ("Near meridian 180 but ok.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 10.0 /* lat */, -179.999997 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 10.0 /* lat */, -179.999999 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Near meridian -180.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 89.9997 /* lat */, -10.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 89.9999 /* lat */, -10.0 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Close to North Pole.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 89.9997 /* lat */, -10.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 89.9998 /* lat */, -10.0 /* lon */); + TEST(AreCoordsGoodForExtrapolation(loc1, loc2), ("Close to North Pole but ok.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, -89.9997 /* lat */, -10.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, -89.9999 /* lat */, -10.0 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Close to South Pole.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, -89.9997 /* lat */, -10.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, -89.9998 /* lat */, -10.0 /* lon */); + TEST(AreCoordsGoodForExtrapolation(loc1, loc2), ("Close to South Pole but ok.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 10.0 /* lat */, -179.999997 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 10.0 /* lat */, -179.999998 /* lon */); + TEST(AreCoordsGoodForExtrapolation(loc1, loc2), ("Near meridian -180 but ok.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 1.0 /* lat */, 10.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 2.0 /* lat */, 10.0 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Locations are too far.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 1.0 /* lat */, 1.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(1.0 /* timestamp */, 1.0 /* lat */, 1.00001 /* lon */); + TEST(AreCoordsGoodForExtrapolation(loc1, loc2), ("Locations are close enough.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 1.0 /* lat */, 1.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(0.0 /* timestamp */, 1.0 /* lat */, 1.00001 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Time is the same.")); + } + { + GpsInfo const loc1 = GetGpsInfo(0.0 /* timestamp */, 1.0 /* lat */, 1.0 /* lon */); + GpsInfo const loc2 = GetGpsInfo(3.0 /* timestamp */, 1.0 /* lat */, 1.00001 /* lon */); + TEST(!AreCoordsGoodForExtrapolation(loc1, loc2), ("Too rare locations.")); } } } // namespace diff --git a/track_analyzing/track_analyzer/track_analyzer.cpp b/track_analyzing/track_analyzer/track_analyzer.cpp index 10033a177e..b3b12ffdc9 100644 --- a/track_analyzing/track_analyzer/track_analyzer.cpp +++ b/track_analyzing/track_analyzer/track_analyzer.cpp @@ -25,8 +25,10 @@ namespace DEFINE_string_ext(cmd, "", "command:\n" - "match - based on raw logs gathers points to tracks and matches them to features\n" - "unmatched_tracks - based on raw logs gathers points to tracks " + "match - based on raw logs gathers points to tracks and matches them to " + "features. To use the tool raw logs should be taken from \"trafin\" project " + "production in gz files and extracted.\n" + "unmatched_tracks - based on raw logs gathers points to tracks\n" "and save tracks to csv. Track points save as lat, log, timestamp in seconds\n" "tracks - prints track statistics\n" "track - prints info about single track\n"