-
Notifications
You must be signed in to change notification settings - Fork 0
/
gps_track_aggregator.cpp
82 lines (64 loc) · 2.21 KB
/
gps_track_aggregator.cpp
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
#include "gps_track_aggregator.h"
#include "gps_data.h"
#include "gps_utils.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/functional/hash.hpp>
#include <unordered_set>
#include <utility>
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;
using std::string;
//BOOST_GEOMETRY_REGISTER_POINT_2D(gps::Waypoint, double, cs::spherical_equatorial<bg::degree>, lon, lat)
namespace gps {
using Rtree = bgi::rtree<Waypoint, bgi::rstar<32>>;
using points_citer = Points::const_iterator;
using SourceKey = std::pair<string, string>; // name, source file
using SourcesSet = std::unordered_set<SourceKey, boost::hash<SourceKey>>;
static constexpr double epsilon = 0.150; // 150 meters
struct TrackAggregator::Impl{
explicit Impl(const string& dir)
: cacheDir(dir)
{}
string cacheDir;
SourcesSet sources;
Rtree rtree;
};
TrackAggregator::TrackAggregator(const std::string& cacheDir)
: pimpl(new Impl(cacheDir))
{
}
TrackAggregator::~TrackAggregator() = default;
WhatsNew TrackAggregator::add(Track const& track) {
WhatsNew result;
pimpl->sources.emplace(track.name, track.sourceFile);
if(track.points.size() <= 1 )
return result;
auto& tree = pimpl->rtree;
auto left = track.points.cbegin();
auto right = left;
bool is_new_seg = true;
for(auto endit = track.points.cend(); right != endit; ++right){
gps::Waypoint const* cur = &*right;
auto neighbor = tree.qbegin(bgi::nearest(*cur, 1));
if(neighbor == tree.qend())
continue;
gps::Waypoint const* nearest = &*neighbor;
bool is_new = distance(*cur, *nearest) > epsilon;
if(is_new != is_new_seg){
if(is_new_seg)
result.emplace_back(std::make_pair(left, right));
is_new_seg = is_new;
left = right;
}
}
if(is_new_seg)
result.emplace_back(std::make_pair(left, right));
tree.insert(track.points.cbegin(), track.points.cend());
return result;
}
bool TrackAggregator::contains(const Track& track) const{
return pimpl->sources.find(std::make_pair(track.name, track.sourceFile))
!= pimpl->sources.end();
}
}