kirylkaveryn/organicmaps
1
Fork 0
forked from organicmaps/organicmaps-tmp
Code Pull requests Activity

Extrapolation of GPS position.

Browse source
This commit is contained in:
Vladimir Byko-Ianko 2018-04-24 11:30:38 +03:00 • committed by mpimenov
parent ce3d3c668e
commit c827ff4d8f
7 changed files with 268 additions and 31 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
map/CMakeLists.txt
View file

@ -47,6 +47,8 @@ set(
everywhere_search_callback.cpp
everywhere_search_callback.hpp
everywhere_search_params.hpp
extrapolation/extrapolator.cpp
extrapolation/extrapolator.hpp
feature_vec_model.cpp
feature_vec_model.hpp
framework.cpp

166
map/extrapolation/extrapolator.cpp Normal file
View file

@ -0,0 +1,166 @@
#include "map/extrapolation/extrapolator.hpp"
#include "geometry/distance_on_sphere.hpp"
#include "platform/platform.hpp"
#include "base/logging.hpp"
#include <chrono>
#include <memory>
namespace
{
uint64_t constexpr kMaxExtrapolationTimeMs = 1000;
uint64_t constexpr kExtrapolationPeriodMs = 200;
double constexpr kMaxExtrapolationSpeedMPerS = 120.0;
double LinearExtrapolationOfOneParam(double param1, double param2, uint64_t timeBetweenPointsMs,
uint64_t timeAfterPoint2Ms)
{
return (param2 - param1) * timeAfterPoint2Ms / timeBetweenPointsMs;
}
/// \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 & point2,
uint64_t timeAfterPoint2Ms)
{
uint64_t const timeBetweenPointsMs =
static_cast<uint64_t>((point2.m_timestamp - point1.m_timestamp) * 1000);
location::GpsInfo extrapolated = point2;
extrapolated.m_timestamp += timeAfterPoint2Ms;
extrapolated.m_longitude += LinearExtrapolationOfOneParam(
point1.m_longitude, point2.m_longitude, timeBetweenPointsMs, timeAfterPoint2Ms);
extrapolated.m_latitude += LinearExtrapolationOfOneParam(point1.m_latitude, point2.m_latitude,
timeBetweenPointsMs, timeAfterPoint2Ms);
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 (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;
}
} // namespace
namespace position_extrapolator
{
using namespace std;
// Extrapolator::Routine ---------------------------------------------------------------------------
Extrapolator::Routine::Routine(ExtrapolatedLocationUpdate const & update)
: m_extrapolatedLocationUpdate(update)
{
}
void Extrapolator::Routine::Do()
{
while (!IsCancelled())
{
{
GetPlatform().RunTask(Platform::Thread::Gui, [this]() {
lock_guard<mutex> guard(m_mutex);
uint64_t const extrapolationTimeMs = kExtrapolationPeriodMs * m_extrapolationCounter;
if (extrapolationTimeMs >= kMaxExtrapolationTimeMs)
return;
if (DoesExtrapolationWork(extrapolationTimeMs))
{
location::GpsInfo gpsInfo =
LinearExtrapolation(m_beforeLastGpsInfo, m_lastGpsInfo, extrapolationTimeMs);
m_extrapolatedLocationUpdate(gpsInfo);
}
else
{
if (m_lastGpsInfo.m_source != location::EUndefine)
{
location::GpsInfo gpsInfo = m_lastGpsInfo;
m_extrapolatedLocationUpdate(gpsInfo);
}
}
});
lock_guard<mutex> guard(m_mutex);
if (m_extrapolationCounter != m_extrapolationCounterUndefined)
++m_extrapolationCounter;
}
// @TODO(bykoinako) Method m_extrapolatedLocationUpdate() is run on gui thread every
// |kExtrapolationPeriodMs| milliseconds. But after changing GPS position
// (that means after a call of method Routine::SetGpsInfo())
// m_extrapolatedLocationUpdate() should be run immediately on gui thread.
this_thread::sleep_for(std::chrono::milliseconds(kExtrapolationPeriodMs));
}
}
void Extrapolator::Routine::SetGpsInfo(location::GpsInfo const & gpsInfo)
{
lock_guard<mutex> guard(m_mutex);
m_beforeLastGpsInfo = m_lastGpsInfo;
m_lastGpsInfo = gpsInfo;
m_extrapolationCounter = 0;
}
bool Extrapolator::Routine::DoesExtrapolationWork(uint64_t extrapolationTimeMs) const
{
// Note. It's possible that m_beforeLastGpsInfo.m_timestamp >= m_lastGpsInfo.m_timestamp.
// It may happen in rare cases because GpsInfo::m_timestamp is not monotonic generally.
// 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_beforeLastGpsInfo.m_timestamp >= m_lastGpsInfo.m_timestamp)
{
return false;
}
double const distM =
ms::DistanceOnEarth(m_beforeLastGpsInfo.m_latitude, m_beforeLastGpsInfo.m_longitude,
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;
// @TODO(bykoianko) Switching off extrapolation based on acceleration should be implemented.
}
// Extrapolator ------------------------------------------------------------------------------------
Extrapolator::Extrapolator(ExtrapolatedLocationUpdate const & update)
: m_extrapolatedLocationThread()
{
m_extrapolatedLocationThread.Create(make_unique<Routine>(update));
}
void Extrapolator::OnLocationUpdate(location::GpsInfo & info)
{
auto * routine = m_extrapolatedLocationThread.GetRoutineAs<Routine>();
CHECK(routine, ());
routine->SetGpsInfo(info);
}
} // namespace position_extrapolator

55
map/extrapolation/extrapolator.hpp Normal file
View file

@ -0,0 +1,55 @@
#pragma once
#include "platform/location.hpp"
#include "base/thread.hpp"
#include <cstdint>
#include <functional>
#include <limits>
#include <mutex>
#include <thread>
namespace position_extrapolator
{
class Extrapolator
{
public:
using ExtrapolatedLocationUpdate = std::function<void(location::GpsInfo &)>;
static uint64_t constexpr m_extrapolationCounterUndefined = std::numeric_limits<uint64_t>::max();
/// \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);
// @TODO(bykoianko) Gyroscope information should be taken into account as well for calculation
// extrapolated position.
void Cancel() { m_extrapolatedLocationThread.Cancel(); }
private:
class Routine : public threads::IRoutine
{
public:
explicit Routine(ExtrapolatedLocationUpdate const & update);
// threads::IRoutine overrides:
void Do() override;
void SetGpsInfo(location::GpsInfo const & gpsInfo);
private:
bool DoesExtrapolationWork(uint64_t extrapolationTimeMs) const;
ExtrapolatedLocationUpdate m_extrapolatedLocationUpdate;
std::mutex m_mutex;
location::GpsInfo m_lastGpsInfo;
location::GpsInfo m_beforeLastGpsInfo;
uint64_t m_extrapolationCounter = m_extrapolationCounterUndefined;
};
threads::Thread m_extrapolatedLocationThread;
};
} // namespace position_extrapolator

29
map/framework.cpp
View file

@ -216,7 +216,7 @@ void Framework::OnLocationUpdate(GpsInfo const & info)
GpsInfo rInfo(info);
#endif
m_routingManager.OnLocationUpdate(rInfo);
m_extrapolator.OnLocationUpdate(rInfo);
}
void Framework::OnCompassUpdate(CompassInfo const & info)
@ -356,18 +356,20 @@ Framework::Framework(FrameworkParams const & params)
, m_storage(platform::migrate::NeedMigrate() ? COUNTRIES_OBSOLETE_FILE : COUNTRIES_FILE)
, m_enabledDiffs(params.m_enableDiffs)
, m_isRenderingEnabled(true)
, m_routingManager(RoutingManager::Callbacks([this]() -> Index & { return m_model.GetIndex(); },
[this]() -> storage::CountryInfoGetter & { return GetCountryInfoGetter(); },
[this](string const & id) -> string {
return m_storage.GetParentIdFor(id);
},
[this](RoutingManager::Callbacks::FeatureCallback const & fn,
vector<FeatureID> const & features)
{
return m_model.ReadFeatures(fn, features);
},
[this]() -> StringsBundle const & { return m_stringsBundle; }),
static_cast<RoutingManager::Delegate &>(*this))
, m_routingManager(
RoutingManager::Callbacks(
[this]() -> Index & { return m_model.GetIndex(); },
[this]() -> storage::CountryInfoGetter & { return GetCountryInfoGetter(); },
[this](string const & id) -> string { return m_storage.GetParentIdFor(id); },
[this](RoutingManager::Callbacks::FeatureCallback const & fn,
vector<FeatureID> const & features) {
return m_model.ReadFeatures(fn, features);
},
[this]() -> StringsBundle const & { return m_stringsBundle; }),
static_cast<RoutingManager::Delegate &>(*this))
, m_extrapolator([this](location::GpsInfo & gpsInfo) {
this->GetRoutingManager().OnLocationUpdate(gpsInfo);
})
, m_trafficManager(bind(&Framework::GetMwmsByRect, this, _1, false /* rough */),
kMaxTrafficCacheSizeBytes, m_routingManager.RoutingSession())
, m_bookingFilterProcessor(m_model.GetIndex(), *m_bookingApi)
@ -495,6 +497,7 @@ Framework::~Framework()
GetBookmarkManager().Teardown();
m_trafficManager.Teardown();
m_extrapolator.Cancel();
DestroyDrapeEngine();
m_model.SetOnMapDeregisteredCallback(nullptr);

2
map/framework.hpp
View file

@ -7,6 +7,7 @@
#include "map/bookmark_manager.hpp"
#include "map/discovery/discovery_manager.hpp"
#include "map/displacement_mode_manager.hpp"
#include "map/extrapolation/extrapolator.hpp"
#include "map/feature_vec_model.hpp"
#include "map/local_ads_manager.hpp"
#include "map/mwm_url.hpp"
@ -207,6 +208,7 @@ protected:
// Note. |m_routingManager| should be declared before |m_trafficManager|
RoutingManager m_routingManager;
position_extrapolator::Extrapolator m_extrapolator;
TrafficManager m_trafficManager;

37
platform/location.hpp
View file

@ -27,6 +27,7 @@ namespace location
enum TLocationSource
{
EUndefine,
EAppleNative,
EWindowsNative,
EAndroidNative,
@ -41,25 +42,25 @@ namespace location
class GpsInfo
{
public:
GpsInfo()
: m_horizontalAccuracy(100.0), // use as a default accuracy
m_altitude(0.0), m_verticalAccuracy(-1.0), m_bearing(-1.0), m_speed(-1.0)
{
}
TLocationSource m_source = EUndefine;
/// \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()
/// method in case of Android. How to calculate such time in case of iOS should be
/// investigated.
/// \note For most cases |m_timestamp| is monotonic.
double m_timestamp = 0.0; //!< seconds from 1st Jan 1970
double m_latitude = 0.0; //!< degrees
double m_longitude = 0.0; //!< degrees
double m_horizontalAccuracy = 100.0; //!< metres
double m_altitude = 0.0; //!< metres
double m_verticalAccuracy = -1.0; //!< metres
double m_bearing = -1.0; //!< positive degrees from the true North
double m_speed = -1.0; //!< metres per second
TLocationSource m_source;
double m_timestamp; //!< seconds from 1st Jan 1970
double m_latitude; //!< degrees
double m_longitude; //!< degrees
double m_horizontalAccuracy; //!< metres
double m_altitude; //!< metres
double m_verticalAccuracy; //!< metres
double m_bearing; //!< positive degrees from the true North
double m_speed; //!< metres per second
//bool HasAltitude() const { return m_verticalAccuracy >= 0.0; }
bool HasBearing() const { return m_bearing >= 0.0; }
bool HasSpeed() const { return m_speed >= 0.0; }
bool IsValid() const { return m_source != EUndefine; }
bool HasBearing() const { return m_bearing >= 0.0; }
bool HasSpeed() const { return m_speed >= 0.0; }
};
/// GpsTrackInfo struct describes a point for GPS tracking

8
xcode/map/map.xcodeproj/project.pbxproj
View file

@ -93,6 +93,8 @@
45F6EE9F1FB1C77600019892 /* search_api.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 45F6EE9C1FB1C77500019892 /* search_api.cpp */; };
47A9D82220A19E9E00E4671B /* libopen_location_code.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 47A9D82120A19E9E00E4671B /* libopen_location_code.a */; };
47A9D82420A19EC300E4671B /* libkml.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 47A9D82320A19EC300E4671B /* libkml.a */; };
56C116602090E5670068BBC0 /* extrapolator.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 56C1165E2090E5670068BBC0 /* extrapolator.cpp */; };
56C116612090E5670068BBC0 /* extrapolator.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 56C1165F2090E5670068BBC0 /* extrapolator.hpp */; };
56EE14D31FE80E900036F20C /* libtransit.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D41FE80E900036F20C /* libtransit.a */; };
56EE14D51FE80EBD0036F20C /* libmwm_diff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D61FE80EBD0036F20C /* libmwm_diff.a */; };
56EE14D71FE80F290036F20C /* libbsdiff.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 56EE14D81FE80F290036F20C /* libbsdiff.a */; };
@ -297,6 +299,8 @@
45F6EE9C1FB1C77500019892 /* search_api.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = search_api.cpp; sourceTree = "<group>"; };
47A9D82120A19E9E00E4671B /* libopen_location_code.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libopen_location_code.a; sourceTree = BUILT_PRODUCTS_DIR; };
47A9D82320A19EC300E4671B /* libkml.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libkml.a; sourceTree = BUILT_PRODUCTS_DIR; };
56C1165E2090E5670068BBC0 /* extrapolator.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = extrapolator.cpp; path = extrapolation/extrapolator.cpp; sourceTree = "<group>"; };
56C1165F2090E5670068BBC0 /* extrapolator.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; name = extrapolator.hpp; path = extrapolation/extrapolator.hpp; sourceTree = "<group>"; };
56EE14D41FE80E900036F20C /* libtransit.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libtransit.a; sourceTree = BUILT_PRODUCTS_DIR; };
56EE14D61FE80EBD0036F20C /* libmwm_diff.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libmwm_diff.a; sourceTree = BUILT_PRODUCTS_DIR; };
56EE14D81FE80F290036F20C /* libbsdiff.a */ = {isa = PBXFileReference; explicitFileType = archive.ar; path = libbsdiff.a; sourceTree = BUILT_PRODUCTS_DIR; };
@ -674,6 +678,8 @@
675345BD1A4054AD00A0A8C3 /* map */ = {
isa = PBXGroup;
children = (
56C1165E2090E5670068BBC0 /* extrapolator.cpp */,
56C1165F2090E5670068BBC0 /* extrapolator.hpp */,
675345CB1A4054E800A0A8C3 /* address_finder.cpp */,
45201E921CE4AC90008A4842 /* api_mark_point.cpp */,
34921F611BFA0A6900737D6E /* api_mark_point.hpp */,
@ -829,6 +835,7 @@
F63421F91DF9BF9100A96868 /* reachable_by_taxi_checker.hpp in Headers */,
BBFC7E3B202D29C000531BE7 /* user_mark_layer.hpp in Headers */,
3D4E99A51FB4A6410025B48C /* booking_filter.hpp in Headers */,
56C116612090E5670068BBC0 /* extrapolator.hpp in Headers */,
675346491A4054E800A0A8C3 /* bookmark_manager.hpp in Headers */,
3DA5714320B5CC80007BDE27 /* booking_filter_processor.hpp in Headers */,
F6B2830A1C1B03320081957A /* gps_track.hpp in Headers */,
@ -1021,6 +1028,7 @@
BB4E5F281FCC664A00A77250 /* transit_reader.cpp in Sources */,
454649F11F2728CE00EF4064 /* local_ads_mark.cpp in Sources */,
F63421F81DF9BF9100A96868 /* reachable_by_taxi_checker.cpp in Sources */,
56C116602090E5670068BBC0 /* extrapolator.cpp in Sources */,
6753466A1A4054E800A0A8C3 /* geourl_process.cpp in Sources */,
348AB57C1D7EE0C6009F8301 /* chart_generator.cpp in Sources */,
342D833A1D5233E8000D8AEA /* displacement_mode_manager.cpp in Sources */,