Repo created
This commit is contained in:
parent
4af19165ec
commit
68073add76
12458 changed files with 12350765 additions and 2 deletions
208
libs/map/extrapolation/extrapolator.cpp
Normal file
208
libs/map/extrapolation/extrapolator.cpp
Normal file
|
|
@ -0,0 +1,208 @@
|
|||
#include "map/extrapolation/extrapolator.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
|
||||
#include "platform/platform.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
#include "base/math.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
|
||||
namespace
|
||||
{
|
||||
// If the difference of values between two instances of GpsInfo is greater
|
||||
// than the appropriate constant below the extrapolator will be switched off for
|
||||
// these two instances of GpsInfo.
|
||||
double constexpr kMaxExtrapolationSpeedMPS = 75.0;
|
||||
double constexpr kMaxExtrapolationDistMeters = 100.0;
|
||||
double constexpr kMaxExtrapolationTimeSeconds = 2.1;
|
||||
|
||||
class LinearExtrapolator
|
||||
{
|
||||
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) const { return x2 + ((x2 - x1) / m_timeBetweenMs) * m_timeAfterMs; }
|
||||
|
||||
private:
|
||||
uint64_t m_timeBetweenMs;
|
||||
uint64_t m_timeAfterMs;
|
||||
};
|
||||
} // namespace
|
||||
|
||||
namespace extrapolation
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, location::GpsInfo const & gpsInfo2,
|
||||
uint64_t timeAfterPoint2Ms)
|
||||
{
|
||||
if (gpsInfo2.m_timestamp <= gpsInfo1.m_timestamp)
|
||||
{
|
||||
ASSERT(false, ("Incorrect gps data"));
|
||||
return gpsInfo2;
|
||||
}
|
||||
|
||||
auto const timeBetweenPointsMs = static_cast<uint64_t>((gpsInfo2.m_timestamp - gpsInfo1.m_timestamp) * 1000);
|
||||
if (timeBetweenPointsMs == 0)
|
||||
return gpsInfo2;
|
||||
|
||||
location::GpsInfo result = gpsInfo2;
|
||||
LinearExtrapolator const e(timeBetweenPointsMs, timeAfterPoint2Ms);
|
||||
|
||||
result.m_timestamp += static_cast<double>(timeAfterPoint2Ms) / 1000.0;
|
||||
result.m_longitude = math::Clamp(e.Extrapolate(gpsInfo1.m_longitude, gpsInfo2.m_longitude), -180.0, 180.0);
|
||||
result.m_latitude = math::Clamp(e.Extrapolate(gpsInfo1.m_latitude, gpsInfo2.m_latitude), -90.0, 90.0);
|
||||
result.m_altitude = e.Extrapolate(gpsInfo1.m_altitude, gpsInfo2.m_altitude);
|
||||
|
||||
// @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. It's important to implement it
|
||||
// because current implementation leads to changing course by steps. It doesn't
|
||||
// look nice when the road changes its direction.
|
||||
|
||||
if (gpsInfo1.HasSpeed() && gpsInfo2.HasSpeed())
|
||||
result.m_speed = e.Extrapolate(gpsInfo1.m_speed, gpsInfo2.m_speed);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool AreCoordsGoodForExtrapolation(location::GpsInfo const & info1, location::GpsInfo const & info2)
|
||||
{
|
||||
if (!info1.IsValid() || !info2.IsValid())
|
||||
return false;
|
||||
|
||||
double const distM = ms::DistanceOnEarth(info1.m_latitude, info1.m_longitude, info2.m_latitude, info2.m_longitude);
|
||||
|
||||
double const timeS = info2.m_timestamp - info1.m_timestamp;
|
||||
if (timeS <= 0.0)
|
||||
return false;
|
||||
|
||||
// |maxDistAfterExtrapolationM| is maximum possible distance from |info2| to
|
||||
// the furthest extrapolated point.
|
||||
double const maxDistAfterExtrapolationM = distM * (Extrapolator::kMaxExtrapolationTimeMs / 1000.0) / timeS;
|
||||
// |maxDistForAllExtrapolationsM| is maximum possible distance from |info2| to
|
||||
// all extrapolated points in any cases.
|
||||
double const maxDistForAllExtrapolationsM = kMaxExtrapolationSpeedMPS * kMaxExtrapolationTimeSeconds;
|
||||
double const distLastGpsInfoToMeridian180 =
|
||||
ms::DistanceOnEarth(info2.m_latitude, info2.m_longitude, info2.m_latitude, 180.0 /* lon2Deg */);
|
||||
// Switching off extrapolation if |info2| are so close to meridian 180 that extrapolated
|
||||
// points may cross meridian 180 or if |info1| and |info2| are located on
|
||||
// different sides of meridian 180.
|
||||
if (distLastGpsInfoToMeridian180 < maxDistAfterExtrapolationM ||
|
||||
(distLastGpsInfoToMeridian180 < maxDistForAllExtrapolationsM && info2.m_longitude * info1.m_longitude < 0.0) ||
|
||||
ms::DistanceOnEarth(info2.m_latitude, info2.m_longitude, 90.0 /* lat2Deg */, info2.m_longitude) <
|
||||
maxDistAfterExtrapolationM ||
|
||||
ms::DistanceOnEarth(info2.m_latitude, info2.m_longitude, -90.0 /* lat2Deg */, info2.m_longitude) <
|
||||
maxDistAfterExtrapolationM)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Note. |timeS| may be less than zero. (info1.m_timestampS >=
|
||||
// info2.m_timestampS) It may happen in rare cases because GpsInfo::m_timestampS is not
|
||||
// monotonic generally. Please see comment in declaration of class GpsInfo for details.
|
||||
|
||||
// @TODO(bykoianko) Switching off extrapolation based on acceleration should be implemented.
|
||||
// Switching off extrapolation based on speed, distance and time.
|
||||
return distM / timeS <= kMaxExtrapolationSpeedMPS && distM <= kMaxExtrapolationDistMeters &&
|
||||
timeS <= kMaxExtrapolationTimeSeconds;
|
||||
}
|
||||
|
||||
// Extrapolator ------------------------------------------------------------------------------------
|
||||
Extrapolator::Extrapolator(ExtrapolatedLocationUpdateFn const & update)
|
||||
: m_isEnabled(false)
|
||||
, m_extrapolatedLocationUpdate(update)
|
||||
{
|
||||
RunTaskOnBackgroundThread(false /* delayed */);
|
||||
}
|
||||
|
||||
void Extrapolator::OnLocationUpdate(location::GpsInfo const & gpsInfo)
|
||||
{
|
||||
{
|
||||
lock_guard<mutex> guard(m_mutex);
|
||||
m_beforeLastGpsInfo = m_lastGpsInfo;
|
||||
m_lastGpsInfo = gpsInfo;
|
||||
m_consecutiveRuns = 0;
|
||||
// Canceling all background tasks which are put to the queue before the task run in this method.
|
||||
++m_locationUpdateCounter;
|
||||
m_locationUpdateMinValid = m_locationUpdateCounter;
|
||||
}
|
||||
RunTaskOnBackgroundThread(false /* delayed */);
|
||||
}
|
||||
|
||||
void Extrapolator::Enable(bool enabled)
|
||||
{
|
||||
lock_guard<mutex> guard(m_mutex);
|
||||
m_isEnabled = enabled;
|
||||
}
|
||||
|
||||
void Extrapolator::ExtrapolatedLocationUpdate(uint64_t locationUpdateCounter)
|
||||
{
|
||||
location::GpsInfo gpsInfo;
|
||||
{
|
||||
lock_guard<mutex> guard(m_mutex);
|
||||
// Canceling all calls of the method which were activated before |m_locationUpdateMinValid|.
|
||||
if (locationUpdateCounter < m_locationUpdateMinValid)
|
||||
return;
|
||||
|
||||
uint64_t const extrapolationTimeMs = kExtrapolationPeriodMs * m_consecutiveRuns;
|
||||
if (extrapolationTimeMs < kMaxExtrapolationTimeMs && m_lastGpsInfo.IsValid())
|
||||
{
|
||||
if (DoesExtrapolationWork())
|
||||
gpsInfo = LinearExtrapolation(m_beforeLastGpsInfo, m_lastGpsInfo, extrapolationTimeMs);
|
||||
else
|
||||
gpsInfo = m_lastGpsInfo;
|
||||
}
|
||||
}
|
||||
|
||||
if (gpsInfo.IsValid())
|
||||
GetPlatform().RunTask(Platform::Thread::Gui, [this, gpsInfo]() { m_extrapolatedLocationUpdate(gpsInfo); });
|
||||
|
||||
{
|
||||
lock_guard<mutex> guard(m_mutex);
|
||||
if (m_consecutiveRuns != kExtrapolationCounterUndefined)
|
||||
++m_consecutiveRuns;
|
||||
}
|
||||
|
||||
// Calling ExtrapolatedLocationUpdate() in |kExtrapolationPeriodMs| milliseconds.
|
||||
RunTaskOnBackgroundThread(true /* delayed */);
|
||||
}
|
||||
|
||||
void Extrapolator::RunTaskOnBackgroundThread(bool delayed)
|
||||
{
|
||||
uint64_t locationUpdateCounter = 0;
|
||||
{
|
||||
lock_guard<mutex> guard(m_mutex);
|
||||
locationUpdateCounter = m_locationUpdateCounter;
|
||||
}
|
||||
|
||||
if (delayed)
|
||||
{
|
||||
auto constexpr period = std::chrono::milliseconds(kExtrapolationPeriodMs);
|
||||
GetPlatform().RunDelayedTask(Platform::Thread::Background, period,
|
||||
[this, locationUpdateCounter] { ExtrapolatedLocationUpdate(locationUpdateCounter); });
|
||||
}
|
||||
else
|
||||
{
|
||||
GetPlatform().RunTask(Platform::Thread::Background,
|
||||
[this, locationUpdateCounter] { ExtrapolatedLocationUpdate(locationUpdateCounter); });
|
||||
}
|
||||
}
|
||||
|
||||
bool Extrapolator::DoesExtrapolationWork() const
|
||||
{
|
||||
if (!m_isEnabled || m_consecutiveRuns == kExtrapolationCounterUndefined)
|
||||
return false;
|
||||
|
||||
return AreCoordsGoodForExtrapolation(m_beforeLastGpsInfo, m_lastGpsInfo);
|
||||
}
|
||||
} // namespace extrapolation
|
||||
79
libs/map/extrapolation/extrapolator.hpp
Normal file
79
libs/map/extrapolation/extrapolator.hpp
Normal file
|
|
@ -0,0 +1,79 @@
|
|||
#pragma once
|
||||
|
||||
#include "platform/location.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
|
||||
namespace extrapolation
|
||||
{
|
||||
/// \brief Returns extrapolated position after |point2| in |timeAfterPoint2Ms|.
|
||||
/// \note |timeAfterPoint2Ms| should be reasonably small (several seconds maximum).
|
||||
location::GpsInfo LinearExtrapolation(location::GpsInfo const & gpsInfo1, location::GpsInfo const & gpsInfo2,
|
||||
uint64_t timeAfterPoint2Ms);
|
||||
|
||||
/// \returns true if linear extrapolation based on |info1| and |info2| should be done.
|
||||
/// \param info1 location information about point gotten before last one.
|
||||
/// \param info2 the latest location information.
|
||||
bool AreCoordsGoodForExtrapolation(location::GpsInfo const & info1, location::GpsInfo const & info2);
|
||||
|
||||
/// \brief This class implements linear extrapolation based on methods LinearExtrapolation()
|
||||
/// and AreCoordsGoodForExtrapolation(). The idea implemented in this class is
|
||||
/// - OnLocationUpdate() should be called from gui thread when new data from gps is available.
|
||||
/// - When OnLocationUpdate() was called twice so that AreCoordsGoodForExtrapolation()
|
||||
/// returns true, extrapolation for this two location will be launched.
|
||||
/// - That means all obsolete extrapolation calls in background thread will be canceled by
|
||||
/// incrementing |m_locationUpdateMinValid|.
|
||||
/// - Several new extrapolations based on two previous locations will be generated.
|
||||
class Extrapolator
|
||||
{
|
||||
static uint64_t constexpr kExtrapolationCounterUndefined = std::numeric_limits<uint64_t>::max();
|
||||
|
||||
public:
|
||||
using ExtrapolatedLocationUpdateFn = std::function<void(location::GpsInfo const &)>;
|
||||
|
||||
// |kMaxExtrapolationTimeMs| is time in milliseconds showing how long location will be
|
||||
// extrapolated after last location gotten from GPS.
|
||||
static uint64_t constexpr kMaxExtrapolationTimeMs = 2000;
|
||||
// |kExtrapolationPeriodMs| is time in milliseconds showing how often location will be
|
||||
// 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;
|
||||
|
||||
/// \param update is a function which is called with params according to extrapolated position.
|
||||
/// |update| will be called on gui thread.
|
||||
explicit Extrapolator(ExtrapolatedLocationUpdateFn const & update);
|
||||
void OnLocationUpdate(location::GpsInfo const & gpsInfo);
|
||||
// @TODO(bykoianko) Gyroscope information should be taken into account as well for calculation
|
||||
// extrapolated position.
|
||||
|
||||
void Enable(bool enabled);
|
||||
|
||||
private:
|
||||
/// \returns true if there's enough information for extrapolation and extrapolation is enabled.
|
||||
/// \note This method should be called only when |m_mutex| is locked.
|
||||
bool DoesExtrapolationWork() const;
|
||||
void ExtrapolatedLocationUpdate(uint64_t locationUpdateCounter);
|
||||
void RunTaskOnBackgroundThread(bool delayed);
|
||||
|
||||
bool m_isEnabled;
|
||||
|
||||
std::mutex m_mutex;
|
||||
ExtrapolatedLocationUpdateFn m_extrapolatedLocationUpdate;
|
||||
location::GpsInfo m_lastGpsInfo;
|
||||
location::GpsInfo m_beforeLastGpsInfo;
|
||||
uint64_t m_consecutiveRuns = kExtrapolationCounterUndefined;
|
||||
// Number of calls Extrapolator::OnLocationUpdate() method. This way |m_locationUpdateCounter|
|
||||
// reflects generation of extrapolations. That mean the next gps location is
|
||||
// the next generation.
|
||||
uint64_t m_locationUpdateCounter = 0;
|
||||
// If |m_locationUpdateCounter| < |m_locationUpdateMinValid| when
|
||||
// ExtrapolatedLocationUpdate() is called (on background thread)
|
||||
// ExtrapolatedLocationUpdate() cancels its execution.
|
||||
uint64_t m_locationUpdateMinValid = 0;
|
||||
};
|
||||
} // namespace extrapolation
|
||||
Loading…
Add table
Add a link
Reference in a new issue