Skip to content

Commit

Permalink
use efficient methods to delete trkpts in track filter.
Browse files Browse the repository at this point in the history
  • Loading branch information
@tsteven4
tsteven4 committed Aug 11, 2023
1 parent 9bf15c6 commit a18cfc7
Show file tree
Hide file tree
Showing 2 changed files with 91 additions and 97 deletions.
187 changes: 90 additions & 97 deletions trackfilter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

static constexpr bool TRACKF_DBG = false;

#include <algorithm> // for for_each
#include <cassert> // for assert
#include <cmath> // for nan
#include <cstdio> // for printf
Expand Down Expand Up @@ -195,10 +196,6 @@ void TrackFilter::trackfilter_fill_track_list_cb(const route_head* track) /* ca
fatal(FatalMsg() << "track: name option is an invalid expression.");
}
if (!regex.match(track->rte_name).hasMatch()) {
foreach (Waypoint* wpt, track->waypoint_list) {
track_del_wpt(const_cast<route_head*>(track), wpt);
delete wpt;
}
track_del_head(const_cast<route_head*>(track));
return;
}
Expand Down Expand Up @@ -329,11 +326,20 @@ void TrackFilter::trackfilter_pack()

route_head* master = track_list.first();

if (!master->waypoint_list.empty()) {
std::for_each(std::next(master->waypoint_list.cbegin()), master->waypoint_list.cend(),
[](Waypoint* wpt)->void {wpt->wpt_flags.new_trkseg = 0;});
}

while (track_list.size() > 1) {
route_head* curr = track_list.takeAt(1);

foreach (Waypoint* wpt, curr->waypoint_list) {
track_del_wpt(curr, wpt);
// Steal all the waypoints
WaypointList curr_wpts;
track_swap_wpts(curr, curr_wpts);
// And add them to the master
foreach (Waypoint* wpt, curr_wpts) {
wpt->wpt_flags.new_trkseg = 0;
track_add_wpt(master, wpt);
}
track_del_head(curr);
Expand All @@ -357,15 +363,16 @@ void TrackFilter::trackfilter_merge()
auto it = track_list.begin();
while (it != track_list.end()) { /* put all points into temp buffer */
route_head* track = *it;
foreach (Waypoint* wpt, track->waypoint_list) {
track_del_wpt(track, wpt); /* copies any new_trkseg flag forward, and clears new_trkseg flag. */
// steal all the wpts
WaypointList wpts;
track_swap_wpts(track, wpts);
// add them to the buff or delete them
foreach (Waypoint* wpt, wpts) {
if (wpt->creation_time.isValid()) {
// we will put the merged points in one track segment,
// as it isn't clear how track segments in the original tracks
// should relate to the merged track.
// track_del_wpt cleared new_trkseg flag for wpt.
// track_add_wpt will set new_trkseg for the first point
// added to a track.
wpt->wpt_flags.new_trkseg = 0;
buff.append(wpt);
} else {
delete wpt;
Expand Down Expand Up @@ -416,16 +423,13 @@ void TrackFilter::trackfilter_split()
fatal(MYNAME "-split: Cannot split more than one track, please pack (or merge) before!\n");
} else if (!track_list.isEmpty()) {
route_head* master = track_list.first();
int count = master->rte_waypt_ct();
if (master->rte_waypt_ct() <= 1) {
return;
}

int i, j;
double interval = -1; /* seconds */
double distance = -1; /* meters */

if (count <= 1) {
return;
}

/* check additional options */

opt_interval = (opt_split && (strlen(opt_split) > 0) && (0 != strcmp(opt_split, TRACKFILTER_SPLIT_OPTION)));
Expand Down Expand Up @@ -495,36 +499,41 @@ void TrackFilter::trackfilter_split()
}
}

QList<Waypoint*> buff;
// steal all the waypoints
WaypointList buff;
track_swap_wpts(master, buff);
assert(!buff.empty()); // enforced above

foreach (Waypoint* wpt, master->waypoint_list) {
buff.append(wpt);
}
trackfilter_split_init_rte_name(master, buff.front()->GetCreationTime());

trackfilter_split_init_rte_name(master, buff.at(0)->GetCreationTime());
route_head* curr = master; /* will be reset by first new track */

route_head* curr = nullptr; /* will be set by first new track */
// add the first waypoint to the first track
track_add_wpt(curr, buff.front());
// and add subsequent waypoints to the first track or a new track
for (auto prev_it = buff.cbegin(), it = std::next(buff.cbegin()); it != buff.cend(); ++prev_it, ++it) {
Waypoint* prev_wpt = *prev_it;
Waypoint* wpt = *it;

for (i=0, j=1; j<count; i++, j++) {
bool new_track_flag;

if ((opt_interval == 0) && (opt_distance == 0)) {
// FIXME: This whole function needs to be reconsidered for arbitrary time.
new_track_flag = buff.at(i)->GetCreationTime().toLocalTime().date() !=
buff.at(j)->GetCreationTime().toLocalTime().date();
// FIXME: This whole function needs to be reconsidered for arbitrary time.
new_track_flag = prev_wpt->GetCreationTime().toLocalTime().date() !=
wpt->GetCreationTime().toLocalTime().date();
if constexpr(TRACKF_DBG) {
if (new_track_flag) {
printf(MYNAME ": new day %s\n", qPrintable(buff.at(j)->GetCreationTime().toLocalTime().date().toString(Qt::ISODate)));
printf(MYNAME ": new day %s\n", qPrintable(wpt->GetCreationTime().toLocalTime().date().toString(Qt::ISODate)));
}
}
} else {
new_track_flag = true;

if (distance > 0) {
double rt1 = RAD(buff.at(i)->latitude);
double rn1 = RAD(buff.at(i)->longitude);
double rt2 = RAD(buff.at(j)->latitude);
double rn2 = RAD(buff.at(j)->longitude);
double rt1 = RAD(prev_wpt->latitude);
double rn1 = RAD(prev_wpt->longitude);
double rt2 = RAD(wpt->latitude);
double rn2 = RAD(wpt->longitude);
double curdist = gcdist(rt1, rn1, rt2, rn2);
curdist = radtometers(curdist);
if (curdist <= distance) {
Expand All @@ -535,7 +544,7 @@ void TrackFilter::trackfilter_split()
}

if (interval > 0) {
double tr_interval = 0.001 * buff.at(i)->GetCreationTime().msecsTo(buff.at(j)->GetCreationTime());
double tr_interval = 0.001 * prev_wpt->GetCreationTime().msecsTo(wpt->GetCreationTime());
if (tr_interval <= interval) {
new_track_flag = false;
} else if constexpr(TRACKF_DBG) {
Expand All @@ -549,14 +558,12 @@ void TrackFilter::trackfilter_split()
printf(MYNAME ": splitting new track\n");
}
curr = new route_head;
trackfilter_split_init_rte_name(curr, buff.at(j)->GetCreationTime());
trackfilter_split_init_rte_name(curr, wpt->GetCreationTime());
track_add_head(curr);
track_list.append(curr);
}
if (curr != nullptr) {
track_del_wpt(master, buff.at(j));
track_add_wpt(curr, buff.at(j));
}
wpt->wpt_flags.new_trkseg = 0;
track_add_wpt(curr, wpt);
}
}
}
Expand Down Expand Up @@ -728,10 +735,11 @@ void TrackFilter::trackfilter_range()
}

if (!inside) {
track_del_wpt(track, wpt);
delete wpt;
wpt->wpt_flags.marked_for_deletion = 1;
}
}
// delete marked wpts
track_del_marked_wpts(track);

if (track->rte_waypt_empty()) {
track_del_head(track);
Expand All @@ -756,12 +764,17 @@ void TrackFilter::trackfilter_seg2trk()
QList<route_head*> new_track_list;
for (auto* src : qAsConst(track_list)) {
new_track_list.append(src);
route_head* dest = nullptr;
route_head* dest = src;
route_head* insert_point = src;
int trk_seg_num = 1;
bool first = true;

foreach (Waypoint* wpt, src->waypoint_list) {
// steal all the waypoints from the src
WaypointList src_wpts;
track_swap_wpts(src, src_wpts);

// and add them back to the original or a new route_head.
foreach (Waypoint* wpt, src_wpts) {
if (wpt->wpt_flags.new_trkseg && !first) {

dest = new route_head;
Expand All @@ -778,18 +791,7 @@ void TrackFilter::trackfilter_seg2trk()
new_track_list.append(dest);
}

/* If we found a track separator, transfer from original to
* new track. We have to reset new_trkseg temporarily to
* prevent track_del_wpt() from copying it to the next track
* point.
*/
if (dest) {
unsigned orig_new_trkseg = wpt->wpt_flags.new_trkseg;
wpt->wpt_flags.new_trkseg = 0;
track_del_wpt(src, wpt);
wpt->wpt_flags.new_trkseg = orig_new_trkseg;
track_add_wpt(dest, wpt);
}
track_add_wpt(dest, wpt);
first = false;
}
}
Expand All @@ -809,13 +811,12 @@ void TrackFilter::trackfilter_trk2seg()
while (track_list.size() > 1) {
route_head* curr = track_list.takeAt(1);

// steal all the wpts
WaypointList curr_wpts;
track_swap_wpts(curr, curr_wpts);
// and add them to the master
bool first = true;
foreach (Waypoint* wpt, curr->waypoint_list) {

unsigned orig_new_trkseg = wpt->wpt_flags.new_trkseg;
wpt->wpt_flags.new_trkseg = 0;
track_del_wpt(curr, wpt);
wpt->wpt_flags.new_trkseg = orig_new_trkseg;
foreach (Waypoint* wpt, curr_wpts) {
track_add_wpt(master, wpt);
if (first) {
wpt->wpt_flags.new_trkseg = 1;
Expand Down Expand Up @@ -888,18 +889,11 @@ void TrackFilter::trackfilter_faketime()

bool TrackFilter::trackfilter_points_are_same(const Waypoint* wpta, const Waypoint* wptb)
{
// We use a simpler (non great circle) test for lat/lon here as this
// is used for keeping the 'bookends' of non-moving points.
//
// Latitude spacing is about 27 feet per .00001 degree.
// Longitude spacing varies, but the reality is that anything closer
// than 27 feet does little but clutter the output.
// As this is about the limit of consumer grade GPS, it seems a
// reasonable tradeoff.

return
std::abs(wpta->latitude - wptb->latitude) < .00001 &&
std::abs(wpta->longitude - wptb->longitude) < .00001 &&
radtometers(gcdist(RAD(wpta->latitude),
RAD(wpta->longitude),
RAD(wptb->latitude),
RAD(wptb->longitude))) < kDistanceLimit &&
std::abs(wpta->altitude - wptb->altitude) < 20 &&
wpta->courses_equal(*wptb) &&
wpta->speeds_equal(*wptb) &&
Expand All @@ -911,46 +905,45 @@ bool TrackFilter::trackfilter_points_are_same(const Waypoint* wpta, const Waypoi
void TrackFilter::trackfilter_segment_head(const route_head* rte)
{
double avg_dist = 0;
int index = 0;
Waypoint* prev_wpt = nullptr;
// Consider tossing trackpoints closer than this in radians.
// (Empirically determined; It's a few dozen feet.)
const double ktoo_close = 0.000005;

const auto wptlist = rte->waypoint_list; // track_del_wpt will modify rte->waypoint_list
const auto wptlist = rte->waypoint_list;
for (auto it = wptlist.cbegin(); it != wptlist.cend(); ++it) {
auto* wpt = *it;
if (index > 0) {
double cur_dist = gcdist(RAD(prev_wpt->latitude),
RAD(prev_wpt->longitude),
RAD(wpt->latitude),
RAD(wpt->longitude));
// Denoise points that are on top of each other.
if (avg_dist == 0) {
avg_dist = cur_dist;
}

if (cur_dist < ktoo_close) {
if (wpt != wptlist.back()) {
auto* next_wpt = *std::next(it);
if (it != wptlist.cbegin()) {
double cur_dist = radtometers(gcdist(RAD(prev_wpt->latitude),
RAD(prev_wpt->longitude),
RAD(wpt->latitude),
RAD(wpt->longitude)));

// Denoise points that are on top of each other,
// keeping the first and last of the group.
if (cur_dist < kDistanceLimit) {
if (auto next_it = std::next(it); next_it != wptlist.cend()) {
auto* next_wpt = *next_it;
if (trackfilter_points_are_same(prev_wpt, wpt) &&
trackfilter_points_are_same(wpt, next_wpt)) {
track_del_wpt(const_cast<route_head*>(rte), wpt);
delete wpt;
continue;
wpt->wpt_flags.marked_for_deletion = 1;
continue; // without updating prev_wpt, the first in the group.
}
}
}
if (cur_dist > .001 && cur_dist > 1.2* avg_dist) {
avg_dist = cur_dist = 0;

if (avg_dist == 0) {
avg_dist = cur_dist;
}

if (cur_dist > 6378.14 && cur_dist > 1.2 * avg_dist) {
avg_dist = 0;
wpt->wpt_flags.new_trkseg = 1;
} else {
// Update weighted moving average;
avg_dist = (cur_dist + 4.0 * avg_dist) / 5.0;
}
// Update weighted moving average;
avg_dist = (cur_dist + 4.0 * avg_dist) / 5.0;
}
prev_wpt = wpt;
index++;
}
track_del_marked_wpts(const_cast<route_head*>(rte));
}

/*******************************************************************************
Expand Down
1 change: 1 addition & 0 deletions trackfilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class TrackFilter:public Filter

/* Constants */

static constexpr double kDistanceLimit = 1.11319; // for points to be considered the same, meters.
static constexpr char TRACKFILTER_PACK_OPTION[] = "pack";
static constexpr char TRACKFILTER_SPLIT_OPTION[] = "split";
static constexpr char TRACKFILTER_SDIST_OPTION[] = "sdistance";
Expand Down

0 comments on commit a18cfc7

Please sign in to comment.