dbscan-gps/dbscan-gps.cpp at master · GoodRon/dbscan-gps · GitHub

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
//
// Created by roman on 25.05.16.
//

#include <set>

#include "dbscan-gps.hxx"
#include "filtersFunctions.h"
#include "nmea.h"

using namespace std;
using namespace nmea;

namespace DbscanGps {

set<size_t> findNeighbours(size_t ipoint, const GpsPoints& points, const SelectionRules& rules) {
    if (ipoint >= points.size()) {
        return set<size_t>();
    }

    set<size_t> neighbours;
    const GpsPoint& point = points[ipoint];

    for (size_t ineighbour = 0; ineighbour < points.size(); ++ineighbour) {
        if (ineighbour == ipoint) {
            continue;
        }
        const GpsPoint& neighbour = points[ineighbour];

        if (rules.epsMeters > 0.0) {
            double distance = calculateDistance(convertDegreesFromNmeaToNormal(point.latitude),
                                                convertDegreesFromNmeaToNormal(point.longitude),
                                                convertDegreesFromNmeaToNormal(neighbour.latitude),
                                                convertDegreesFromNmeaToNormal(neighbour.longitude));

            if (distance > rules.epsMeters) {
                continue;
            }
        }

        if (rules.epsAngle > 0.0) {
            double delta = calculateAngleDelta(point.direction, neighbour.direction);
            if (delta > rules.epsAngle) {
                continue;
            }
        }

        if (rules.epsTimestamp > 0) {
            if (labs(point.timestamp - neighbour.timestamp) > rules.epsTimestamp) {
                continue;
            }
        }

        if (rules.epsSpeed > 0.0) {
            if (fabs(point.speed - neighbour.speed) > rules.epsSpeed) {
                continue;
            }
        }

        if (rules.epsHdop > 0.0) {
            if (fabs(point.hdop - neighbour.hdop) > rules.epsHdop) {
                continue;
            }
        }

        if (rules.epsVdop > 0.0) {
            if (fabs(point.vdop - neighbour.vdop) > rules.epsVdop) {
                continue;
            }
        }

        neighbours.insert(ineighbour);
    }
    return neighbours;
}

GpsClusters scan(const GpsPoints& points, const SelectionRules& rules) {
    set<size_t> visited;
    GpsClusters clusters;
    GpsCluster noise = {};

    for (size_t ipoint = 0; ipoint < points.size(); ++ipoint) {
        if (visited.find(ipoint) != visited.end()) {
            continue;
        }
        visited.insert(ipoint);

        auto neighbours = findNeighbours(ipoint, points, rules);
        if (neighbours.size() < rules.minPts) {
            noise.points.push_back(points[ipoint]);
        }

        GpsCluster cluster = {};
        cluster.points.push_back(points[ipoint]);

        while (!neighbours.empty()) {
            set<size_t> moreNeighbours;
            for (auto& ineighbour: neighbours) {
                if (visited.find(ineighbour) != visited.end()) {
                    continue;
                }
                visited.insert(ineighbour);

                auto nextNeighbours = findNeighbours(ineighbour, points, rules);
                if (nextNeighbours.size() >= rules.minPts) {
                    moreNeighbours.insert(nextNeighbours.begin(),
                                          nextNeighbours.end());
                }
                cluster.points.push_back(points[ineighbour]);
            }
            neighbours = move(moreNeighbours);
        }
        clusters.push_back(move(cluster));
    }
//    clusters.push_back(noise);
    return clusters;
}

}