1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
|
#include "map/gps_track_filter.hpp"
#include "geometry/distance_on_sphere.hpp"
#include "geometry/mercator.hpp"
#include "platform/settings.hpp"
#include "base/assert.hpp"
#include "base/macros.hpp"
#include "base/math.hpp"
namespace
{
char const kMinHorizontalAccuracyKey[] = "GpsTrackingMinAccuracy";
// Minimal horizontal accuracy is required to skip 'bad' points.
// Use 250 meters to allow points from a pure GPS + GPS through wifi.
double constexpr kMinHorizontalAccuracyMeters = 250;
// Required for points decimation to reduce number of close points.
double constexpr kClosePointDistanceMeters = 15;
// Max acceptable acceleration to filter gps jumps
double constexpr kMaxAcceptableAcceleration = 2; // m / sec ^ 2
double constexpr kCosine45degrees = 0.70710678118;
m2::PointD GetDirection(location::GpsInfo const & from, location::GpsInfo const & to)
{
m2::PointD const pt0 = mercator::FromLatLon(from.m_latitude, from.m_longitude);
m2::PointD const pt1 = mercator::FromLatLon(to.m_latitude, to.m_longitude);
m2::PointD const d = pt1 - pt0;
return d.IsAlmostZero() ? m2::PointD::Zero() : d.Normalize();
}
double GetDistance(location::GpsInfo const & from, location::GpsInfo const & to)
{
return ms::DistanceOnEarth(from.m_latitude, from.m_longitude, to.m_latitude, to.m_longitude);
}
} // namespace
void GpsTrackNullFilter::Process(std::vector<location::GpsInfo> const & inPoints,
std::vector<location::GpsTrackInfo> & outPoints)
{
outPoints.insert(outPoints.end(), inPoints.begin(), inPoints.end());
}
void GpsTrackFilter::StoreMinHorizontalAccuracy(double value)
{
settings::Set(kMinHorizontalAccuracyKey, value);
}
GpsTrackFilter::GpsTrackFilter()
: m_minAccuracy(kMinHorizontalAccuracyMeters)
, m_countLastInfo(0)
, m_countAcceptedInfo(0)
{
settings::TryGet(kMinHorizontalAccuracyKey, m_minAccuracy);
}
void GpsTrackFilter::Process(std::vector<location::GpsInfo> const & inPoints,
std::vector<location::GpsTrackInfo> & outPoints)
{
outPoints.reserve(inPoints.size());
if (m_minAccuracy == 0)
{
// Debugging trick to turn off filtering
outPoints.insert(outPoints.end(), inPoints.begin(), inPoints.end());
return;
}
for (location::GpsInfo const & currInfo : inPoints)
{
// Do not accept points from the predictor
if (currInfo.m_source == location::EPredictor)
continue;
// Skip any function without speed
if (!currInfo.HasSpeed())
continue;
if (m_countAcceptedInfo < 2 || currInfo.m_timestamp < GetLastAcceptedInfo().m_timestamp)
{
AddLastInfo(currInfo);
AddLastAcceptedInfo(currInfo);
continue;
}
if (IsGoodPoint(currInfo))
{
double const predictionDistance = GetDistance(m_lastInfo[1], m_lastInfo[0]); // meters
double const realDistance = GetDistance(m_lastInfo[0], currInfo); // meters
m2::PointD const predictionDirection = GetDirection(m_lastInfo[1], m_lastInfo[0]);
m2::PointD const realDirection = GetDirection(m_lastInfo[0], currInfo);
// Cosine of angle between prediction direction and real direction is
double const cosine = m2::DotProduct(predictionDirection, realDirection);
// Acceptable angle must be from 0 to 45 or from 0 to -45.
// Acceptable distance must be not great than 2x than predicted, otherwise it is jump.
if (cosine >= kCosine45degrees &&
realDistance <= std::max(kClosePointDistanceMeters, 2. * predictionDistance))
{
outPoints.emplace_back(currInfo);
AddLastAcceptedInfo(currInfo);
}
}
AddLastInfo(currInfo);
}
}
bool GpsTrackFilter::IsGoodPoint(location::GpsInfo const & info) const
{
// Filter by point accuracy
if (info.m_horizontalAccuracy > m_minAccuracy)
return false;
auto const & lastInfo = GetLastInfo();
auto const & lastAcceptedInfo = GetLastAcceptedInfo();
// Distance in meters between last accepted and current point is, meters:
double const distanceFromLastAccepted = GetDistance(lastAcceptedInfo, info);
// Filter point by close distance
if (distanceFromLastAccepted < kClosePointDistanceMeters)
return false;
// Filter point if accuracy areas are intersected
if (distanceFromLastAccepted < lastAcceptedInfo.m_horizontalAccuracy &&
info.m_horizontalAccuracy > 0.5 * lastAcceptedInfo.m_horizontalAccuracy)
return false;
// Distance in meters between last and current point is, meters:
double const distanceFromLast = GetDistance(lastInfo, info);
// Time spend to move from the last point to the current point, sec:
double const timeFromLast = info.m_timestamp - lastInfo.m_timestamp;
if (timeFromLast <= 0.0)
return false;
// Speed to move from the last point to the current point
double const speedFromLast = distanceFromLast / timeFromLast;
// Filter by acceleration: skip point if it jumps too far in short time
double const accelerationFromLast = (speedFromLast - lastInfo.m_speedMpS) / timeFromLast;
if (accelerationFromLast > kMaxAcceptableAcceleration)
return false;
return true;
}
location::GpsInfo const & GpsTrackFilter::GetLastInfo() const
{
ASSERT_GREATER(m_countLastInfo, 0, ());
return m_lastInfo[0];
}
location::GpsInfo const & GpsTrackFilter::GetLastAcceptedInfo() const
{
ASSERT_GREATER(m_countAcceptedInfo, 0, ());
return m_lastAcceptedInfo[0];
}
void GpsTrackFilter::AddLastInfo(location::GpsInfo const & info)
{
m_lastInfo[1] = m_lastInfo[0];
m_lastInfo[0] = info;
m_countLastInfo += 1;
}
void GpsTrackFilter::AddLastAcceptedInfo(location::GpsInfo const & info)
{
m_lastAcceptedInfo[1] = m_lastAcceptedInfo[0];
m_lastAcceptedInfo[0] = info;
m_countAcceptedInfo += 1;
}
|
