From fa20976bcbedc72cd37100fa40c67a9d299b1b0e Mon Sep 17 00:00:00 2001 From: tsteven4 <13596209+tsteven4@users.noreply.github.com> Date: Sun, 21 Nov 2021 14:04:55 -0700 Subject: [PATCH] Introduce resampling filter and nvectors. (#763) * Introduce resampling filter and nvectors. * use ellipsoid model with nvectors. the code had been forcing a spherical earth model to match the existing grtcirc.cc calculations. --- CMakeLists.txt | 6 +- GPSBabel.pro | 6 +- filter_vecs.h | 7 + reference/filter0.txt | 1 + reference/filter1.txt | 4 + reference/help.txt | 4 + reference/track/sim.csv | 49 ++ reference/track/simdec.csv | 25 + reference/track/simint.csv | 96 ++++ resample.cc | 320 +++++++++++++ resample.h | 85 ++++ src/core/nvector.cc | 450 ++++++++++++++++++ src/core/nvector.h | 97 ++++ src/core/vector3d.cc | 149 ++++++ src/core/vector3d.h | 66 +++ testo.d/resample.test | 5 + xmldoc/filters/options/resample-average.xml | 3 + xmldoc/filters/options/resample-decimate.xml | 3 + .../filters/options/resample-interpolate.xml | 4 + xmldoc/filters/resample.xml | 28 ++ 20 files changed, 1406 insertions(+), 2 deletions(-) create mode 100644 reference/track/sim.csv create mode 100644 reference/track/simdec.csv create mode 100644 reference/track/simint.csv create mode 100644 resample.cc create mode 100644 resample.h create mode 100644 src/core/nvector.cc create mode 100644 src/core/nvector.h create mode 100644 src/core/vector3d.cc create mode 100644 src/core/vector3d.h create mode 100644 testo.d/resample.test create mode 100644 xmldoc/filters/options/resample-average.xml create mode 100644 xmldoc/filters/options/resample-decimate.xml create mode 100644 xmldoc/filters/options/resample-interpolate.xml create mode 100644 xmldoc/filters/resample.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c1dfbf75..4e12d0013 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ set(FILTERS position.cc radius.cc duplicate.cc arcdist.cc polygon.cc smplrout.cc reverse_route.cc sort.cc stackfilter.cc trackfilter.cc discard.cc nukedata.cc interpolate.cc transform.cc height.cc swapdata.cc bend.cc - validate.cc + validate.cc resample.cc ) set(SHAPE @@ -96,8 +96,10 @@ set(SUPPORT formspec.cc xmltag.cc cet.cc cet_util.cc fatal.cc rgbcolors.cc inifile.cc garmin_fs.cc units.cc gbser.cc gbfile.cc parse.cc session.cc main.cc globals.cc + src/core/nvector.cc src/core/textstream.cc src/core/usasciicodec.cc + src/core/vector3d.cc src/core/xmlstreamwriter.cc ) if(${QT_VERSION_MAJOR} EQUAL "6") @@ -187,8 +189,10 @@ set(HEADERS src/core/datetime.h src/core/file.h src/core/logging.h + src/core/nvector.h src/core/textstream.h src/core/usasciicodec.h + src/core/vector3d.h src/core/xmlstreamwriter.h src/core/xmltag.h ) diff --git a/GPSBabel.pro b/GPSBabel.pro index 4cfbb7332..00bc8d03c 100644 --- a/GPSBabel.pro +++ b/GPSBabel.pro @@ -89,7 +89,7 @@ DEPRECATED_SHAPE=pdbfile.cc FILTERS=position.cc radius.cc duplicate.cc arcdist.cc polygon.cc smplrout.cc \ reverse_route.cc sort.cc stackfilter.cc trackfilter.cc discard.cc \ nukedata.cc interpolate.cc transform.cc height.cc swapdata.cc bend.cc \ - validate.cc + validate.cc resample.cc FILTER_HEADERS = $$FILTERS FILTER_HEADERS ~= s/\\.cc/.h/g @@ -107,8 +107,10 @@ SUPPORT = route.cc waypt.cc filter_vecs.cc util.cc vecs.cc mkshort.cc \ formspec.cc xmltag.cc cet.cc cet_util.cc fatal.cc rgbcolors.cc \ inifile.cc garmin_fs.cc units.cc gbser.cc \ gbfile.cc parse.cc session.cc main.cc globals.cc \ + src/core/nvector.cc \ src/core/textstream.cc \ src/core/usasciicodec.cc \ + src/core/vector3d.cc \ src/core/xmlstreamwriter.cc versionAtLeast(QT_VERSION, 6.0): SUPPORT += src/core/codecdevice.cc @@ -184,8 +186,10 @@ HEADERS = \ src/core/datetime.h \ src/core/file.h \ src/core/logging.h \ + src/core/nvector.h \ src/core/textstream.h \ src/core/usasciicodec.h \ + src/core/vector3d.h \ src/core/xmlstreamwriter.h \ src/core/xmltag.h diff --git a/filter_vecs.h b/filter_vecs.h index b52bd19c8..5906d7781 100644 --- a/filter_vecs.h +++ b/filter_vecs.h @@ -36,6 +36,7 @@ #include "polygon.h" // for PolygonFilter #include "position.h" // for PositionFilter #include "radius.h" // for RadiusFilter +#include "resample.h" // for ResampleFilter #include "reverse_route.h" // for ReverseRouteFilter #include "smplrout.h" // for SimplifyRouteFilter #include "sort.h" // for SortFilter @@ -97,6 +98,7 @@ static bool validate_filter_vec(const fl_vecs_t& vec); PolygonFilter polygon; PositionFilter position; RadiusFilter radius; + ResampleFilter resample; ReverseRouteFilter reverse_route; SimplifyRouteFilter routesimple; SortFilter sort; @@ -153,6 +155,11 @@ static bool validate_filter_vec(const fl_vecs_t& vec); "radius", "Include Only Points Within Radius", }, + { + &resample, + "resample", + "Resample Track", + }, { &routesimple, "simplify", diff --git a/reference/filter0.txt b/reference/filter0.txt index c81fce189..46591eb84 100644 --- a/reference/filter0.txt +++ b/reference/filter0.txt @@ -10,6 +10,7 @@ nuketypes Remove all waypoints, tracks, or routes duplicate Remove Duplicates position Remove Points Within Distance discard Remove unreliable points with high hdop or vdop +resample Resample Track reverse Reverse stops within routes stack Save and restore waypoint lists simplify Simplify routes diff --git a/reference/filter1.txt b/reference/filter1.txt index 262824b13..2b48ea54e 100644 --- a/reference/filter1.txt +++ b/reference/filter1.txt @@ -83,6 +83,10 @@ option discard matchname Suppress points where name matches given name string option discard matchdesc Suppress points where description matches given name string https://www.gpsbabel.org/WEB_DOC_DIR/filter_discard.html#fmt_discard_o_matchdesc option discard matchcmt Suppress points where comment matches given name string https://www.gpsbabel.org/WEB_DOC_DIR/filter_discard.html#fmt_discard_o_matchcmt option discard matchicon Suppress points where type matches given name string https://www.gpsbabel.org/WEB_DOC_DIR/filter_discard.html#fmt_discard_o_matchicon +resample Resample Track https://www.gpsbabel.org/WEB_DOC_DIR/filter_resample.html +option resample decimate Decimate, decrease sample rate by a factor of n integer 2 https://www.gpsbabel.org/WEB_DOC_DIR/filter_resample.html#fmt_resample_o_decimate +option resample interpolate Interpolate, increase sample rate by a factor of n integer 2 https://www.gpsbabel.org/WEB_DOC_DIR/filter_resample.html#fmt_resample_o_interpolate +option resample average Running average of n points integer 2 https://www.gpsbabel.org/WEB_DOC_DIR/filter_resample.html#fmt_resample_o_average reverse Reverse stops within routes https://www.gpsbabel.org/WEB_DOC_DIR/filter_reverse.html stack Save and restore waypoint lists https://www.gpsbabel.org/WEB_DOC_DIR/filter_stack.html option stack push Push waypoint list onto stack boolean https://www.gpsbabel.org/WEB_DOC_DIR/filter_stack.html#fmt_stack_o_push diff --git a/reference/help.txt b/reference/help.txt index 2037340e2..79acd79b8 100644 --- a/reference/help.txt +++ b/reference/help.txt @@ -742,6 +742,10 @@ Supported data filters: nosort Inhibit sort by distance to center maxcount Output no more than this number of points asroute Put resulting waypoints in route of this name + resample Resample Track + decimate Decimate, decrease sample rate by a factor of n + interpolate Interpolate, increase sample rate by a factor of n + average Running average of n points simplify Simplify routes count Maximum number of points in route error Maximum error diff --git a/reference/track/sim.csv b/reference/track/sim.csv new file mode 100644 index 000000000..8a00c4a00 --- /dev/null +++ b/reference/track/sim.csv @@ -0,0 +1,49 @@ +utc_d,utc_t,lat,lon +2021/11/10,11:00:00 AM,66.000103228632,-179.97904 +2021/11/10,11:00:01 AM,66.0001884601077,-179.97903 +2021/11/10,11:00:02 AM,66.0000230237331,-179.97947 +2021/11/10,11:00:03 AM,66.000045323685,-179.98008 +2021/11/10,11:00:04 AM,66.0001767900345,-179.98048 +2021/11/10,11:00:05 AM,66.0001092928285,-179.98067 +2021/11/10,11:00:06 AM,66.0001668298279,-179.98197 +2021/11/10,11:00:07 AM,66.0000748518391,-179.98320 +2021/11/10,11:00:08 AM,66.0001341555094,-179.98429 +2021/11/10,11:00:09 AM,66.0001213660596,-179.98519 +2021/11/10,11:00:10 AM,66.0001967593428,-179.98558 +2021/11/10,11:00:11 AM,66.0001580625671,-179.98671 +2021/11/10,11:00:12 AM,66.0001378472268,-179.98805 +2021/11/10,11:00:13 AM,66.0001709596168,-179.98906 +2021/11/10,11:00:14 AM,66.0000502908297,-179.98970 +2021/11/10,11:00:15 AM,66.000066383696,-179.99130 +2021/11/10,11:00:16 AM,66.0000462879889,-179.99183 +2021/11/10,11:00:17 AM,66.0001312399228,-179.99340 +2021/11/10,11:00:18 AM,66.0000883525882,-179.99384 +2021/11/10,11:00:19 AM,66.0001647357197,-179.99504 +2021/11/10,11:00:20 AM,66.0001959985441,-179.99596 +2021/11/10,11:00:21 AM,66.0001581122524,-179.99737 +2021/11/10,11:00:22 AM,66.0001028337378,-179.99798 +2021/11/10,11:00:23 AM,66.000105956278,-179.99903 +2021/11/10,11:00:24 AM,66.0000574941283,-179.99956 +2021/11/10,11:00:25 AM,66.0000416369344,179.99915 +2021/11/10,11:00:26 AM,66.0001504925119,179.99831 +2021/11/10,11:00:27 AM,66.0001927654035,179.99675 +2021/11/10,11:00:28 AM,66.0001282065146,179.99621 +2021/11/10,11:00:29 AM,66.0000911889429,179.99493 +2021/11/10,11:00:30 AM,66.0001909788552,179.99362 +2021/11/10,11:00:31 AM,66.0000568057737,179.99293 +2021/11/10,11:00:32 AM,66.0000978383165,179.99211 +2021/11/10,11:00:33 AM,66.0000403599515,179.99138 +2021/11/10,11:00:34 AM,66.0001144849062,179.99045 +2021/11/10,11:00:35 AM,66.0000769781374,179.98854 +2021/11/10,11:00:36 AM,66.0000133158984,179.98783 +2021/11/10,11:00:37 AM,66.0001758676388,179.98699 +2021/11/10,11:00:38 AM,66.0000207460354,179.98606 +2021/11/10,11:00:39 AM,66.0000931231469,179.98538 +2021/11/10,11:00:40 AM,66.0001991309548,179.98429 +2021/11/10,11:00:41 AM,66.0001841979519,179.98311 +2021/11/10,11:00:42 AM,66.0001217081583,179.98251 +2021/11/10,11:00:43 AM,66.0001017396611,179.98182 +2021/11/10,11:00:44 AM,66.0001469324212,179.98183 +2021/11/10,11:00:45 AM,66.0000706021879,179.98175 +2021/11/10,11:00:46 AM,66.0000732617344,179.98178 +2021/11/10,11:00:47 AM,66.0001673249007,179.98162 diff --git a/reference/track/simdec.csv b/reference/track/simdec.csv new file mode 100644 index 000000000..15b47f75e --- /dev/null +++ b/reference/track/simdec.csv @@ -0,0 +1,25 @@ +No,Latitude,Longitude,Date,Time +1,66.000106,-179.979157,2021/11/10,11:00:00 +2,66.000098,-179.979623,2021/11/10,11:00:02 +3,66.000113,-179.980580,2021/11/10,11:00:04 +4,66.000126,-179.982144,2021/11/10,11:00:06 +5,66.000132,-179.984051,2021/11/10,11:00:08 +6,66.000151,-179.985935,2021/11/10,11:00:10 +7,66.000139,-179.987910,2021/11/10,11:00:12 +8,66.000098,-179.989984,2021/11/10,11:00:14 +9,66.000087,-179.992037,2021/11/10,11:00:16 +10,66.000122,-179.994058,2021/11/10,11:00:18 +11,66.000148,-179.996071,2021/11/10,11:00:20 +12,66.000120,-179.998003,2021/11/10,11:00:22 +13,66.000096,-179.999865,2021/11/10,11:00:24 +14,66.000117,179.998134,2021/11/10,11:00:26 +15,66.000134,179.995989,2021/11/10,11:00:28 +16,66.000118,179.993927,2021/11/10,11:00:30 +17,66.000091,179.992061,2021/11/10,11:00:32 +18,66.000079,179.990085,2021/11/10,11:00:34 +19,66.000076,179.987981,2021/11/10,11:00:36 +20,66.000099,179.986078,2021/11/10,11:00:38 +21,66.000137,179.984286,2021/11/10,11:00:40 +22,66.000138,179.982762,2021/11/10,11:00:42 +23,66.000115,179.981959,2021/11/10,11:00:44 +24,66.000110,179.981758,2021/11/10,11:00:46 diff --git a/reference/track/simint.csv b/reference/track/simint.csv new file mode 100644 index 000000000..0b41f78d5 --- /dev/null +++ b/reference/track/simint.csv @@ -0,0 +1,96 @@ +No,Latitude,Longitude,Date,Time +1,66.000103,-179.979040,2021/11/10,11:00:00 +2,66.000146,-179.979035,2021/11/10,11:00:00.500 +3,66.000188,-179.979030,2021/11/10,11:00:01 +4,66.000106,-179.979250,2021/11/10,11:00:01.500 +5,66.000023,-179.979470,2021/11/10,11:00:02 +6,66.000034,-179.979775,2021/11/10,11:00:02.500 +7,66.000045,-179.980080,2021/11/10,11:00:03 +8,66.000111,-179.980280,2021/11/10,11:00:03.500 +9,66.000177,-179.980480,2021/11/10,11:00:04 +10,66.000143,-179.980575,2021/11/10,11:00:04.500 +11,66.000109,-179.980670,2021/11/10,11:00:05 +12,66.000138,-179.981320,2021/11/10,11:00:05.500 +13,66.000167,-179.981970,2021/11/10,11:00:06 +14,66.000121,-179.982585,2021/11/10,11:00:06.500 +15,66.000075,-179.983200,2021/11/10,11:00:07 +16,66.000105,-179.983745,2021/11/10,11:00:07.500 +17,66.000134,-179.984290,2021/11/10,11:00:08 +18,66.000128,-179.984740,2021/11/10,11:00:08.500 +19,66.000121,-179.985190,2021/11/10,11:00:09 +20,66.000159,-179.985385,2021/11/10,11:00:09.500 +21,66.000197,-179.985580,2021/11/10,11:00:10 +22,66.000177,-179.986145,2021/11/10,11:00:10.500 +23,66.000158,-179.986710,2021/11/10,11:00:11 +24,66.000148,-179.987380,2021/11/10,11:00:11.500 +25,66.000138,-179.988050,2021/11/10,11:00:12 +26,66.000154,-179.988555,2021/11/10,11:00:12.500 +27,66.000171,-179.989060,2021/11/10,11:00:13 +28,66.000111,-179.989380,2021/11/10,11:00:13.500 +29,66.000050,-179.989700,2021/11/10,11:00:14 +30,66.000058,-179.990500,2021/11/10,11:00:14.500 +31,66.000066,-179.991300,2021/11/10,11:00:15 +32,66.000056,-179.991565,2021/11/10,11:00:15.500 +33,66.000046,-179.991830,2021/11/10,11:00:16 +34,66.000089,-179.992615,2021/11/10,11:00:16.500 +35,66.000131,-179.993400,2021/11/10,11:00:17 +36,66.000110,-179.993620,2021/11/10,11:00:17.500 +37,66.000088,-179.993840,2021/11/10,11:00:18 +38,66.000127,-179.994440,2021/11/10,11:00:18.500 +39,66.000165,-179.995040,2021/11/10,11:00:19 +40,66.000180,-179.995500,2021/11/10,11:00:19.500 +41,66.000196,-179.995960,2021/11/10,11:00:20 +42,66.000177,-179.996665,2021/11/10,11:00:20.500 +43,66.000158,-179.997370,2021/11/10,11:00:21 +44,66.000130,-179.997675,2021/11/10,11:00:21.500 +45,66.000103,-179.997980,2021/11/10,11:00:22 +46,66.000104,-179.998505,2021/11/10,11:00:22.500 +47,66.000106,-179.999030,2021/11/10,11:00:23 +48,66.000082,-179.999295,2021/11/10,11:00:23.500 +49,66.000057,-179.999560,2021/11/10,11:00:24 +50,66.000050,179.999795,2021/11/10,11:00:24.500 +51,66.000042,179.999150,2021/11/10,11:00:25 +52,66.000096,179.998730,2021/11/10,11:00:25.500 +53,66.000150,179.998310,2021/11/10,11:00:26 +54,66.000172,179.997530,2021/11/10,11:00:26.500 +55,66.000193,179.996750,2021/11/10,11:00:27 +56,66.000160,179.996480,2021/11/10,11:00:27.500 +57,66.000128,179.996210,2021/11/10,11:00:28 +58,66.000110,179.995570,2021/11/10,11:00:28.500 +59,66.000091,179.994930,2021/11/10,11:00:29 +60,66.000141,179.994275,2021/11/10,11:00:29.500 +61,66.000191,179.993620,2021/11/10,11:00:30 +62,66.000124,179.993275,2021/11/10,11:00:30.500 +63,66.000057,179.992930,2021/11/10,11:00:31 +64,66.000077,179.992520,2021/11/10,11:00:31.500 +65,66.000098,179.992110,2021/11/10,11:00:32 +66,66.000069,179.991745,2021/11/10,11:00:32.500 +67,66.000040,179.991380,2021/11/10,11:00:33 +68,66.000077,179.990915,2021/11/10,11:00:33.500 +69,66.000114,179.990450,2021/11/10,11:00:34 +70,66.000096,179.989495,2021/11/10,11:00:34.500 +71,66.000077,179.988540,2021/11/10,11:00:35 +72,66.000045,179.988185,2021/11/10,11:00:35.500 +73,66.000013,179.987830,2021/11/10,11:00:36 +74,66.000095,179.987410,2021/11/10,11:00:36.500 +75,66.000176,179.986990,2021/11/10,11:00:37 +76,66.000098,179.986525,2021/11/10,11:00:37.500 +77,66.000021,179.986060,2021/11/10,11:00:38 +78,66.000057,179.985720,2021/11/10,11:00:38.500 +79,66.000093,179.985380,2021/11/10,11:00:39 +80,66.000146,179.984835,2021/11/10,11:00:39.500 +81,66.000199,179.984290,2021/11/10,11:00:40 +82,66.000192,179.983700,2021/11/10,11:00:40.500 +83,66.000184,179.983110,2021/11/10,11:00:41 +84,66.000153,179.982810,2021/11/10,11:00:41.500 +85,66.000122,179.982510,2021/11/10,11:00:42 +86,66.000112,179.982165,2021/11/10,11:00:42.500 +87,66.000102,179.981820,2021/11/10,11:00:43 +88,66.000124,179.981825,2021/11/10,11:00:43.500 +89,66.000147,179.981830,2021/11/10,11:00:44 +90,66.000109,179.981790,2021/11/10,11:00:44.500 +91,66.000071,179.981750,2021/11/10,11:00:45 +92,66.000072,179.981765,2021/11/10,11:00:45.500 +93,66.000073,179.981780,2021/11/10,11:00:46 +94,66.000120,179.981700,2021/11/10,11:00:46.500 +95,66.000167,179.981620,2021/11/10,11:00:47 diff --git a/resample.cc b/resample.cc new file mode 100644 index 000000000..2b72ea0f5 --- /dev/null +++ b/resample.cc @@ -0,0 +1,320 @@ +/* + Track resampling filter + + Copyright (C) 2021 Robert Lipe, robertlipe+source@gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +#include "resample.h" + +#include // for round +#include // for optional + +#include // for QDebug +#include // for QList<>::const_iterator +#include // for QString +#include // for qSetRealNumberPrecision +#include // for qDebug, qAsConst, qint64 + +#include "defs.h" // for Waypoint, route_head, fatal, WaypointList, track_add_wpt, track_disp_all, RouteList, track_add_head, track_del_wpt, track_swap, UrlList, gb_color, global_options, global_opts +#include "formspec.h" // for FormatSpecificDataList +#include "src/core/datetime.h" // for DateTime +#include "src/core/logging.h" // for FatalMsg +#include "src/core/nvector.h" // for NVector +#include "src/core/vector3d.h" // for operator<<, Vector3D + + +#if FILTERS_ENABLED +#define MYNAME "resample" + + +void ResampleFilter::average_waypoint(Waypoint* wpt, bool zero_stuffed) +{ + // We filter in the n-vector coordinate system. + // This removes difficulties at the discontinuity at longitude = +/- 180 degrees, + // as well as at the singularities at the poles. + // Our filter is from Gade, 5.3.6. Horizontal geographical mean, equation 17. + gpsbabel::NVector current_position; + if (wpt->extra_data) { // zero stuffing? + current_position = gpsbabel::Vector3D(0.0, 0.0, 0.0); + wpt->extra_data = nullptr; + } else { + current_position = gpsbabel::NVector(wpt->latitude, wpt->longitude); + } + int current_altitude_valid_count = wpt->altitude != unknown_alt? 1 : 0; + double current_altitude = wpt->altitude != unknown_alt? wpt->altitude : 0.0; + auto current = std::tuple(current_position, current_altitude_valid_count, current_altitude); + + if (history.isEmpty()) { + if (zero_stuffed) { + gpsbabel::NVector zero_position = gpsbabel::Vector3D(0.0, 0.0, 0.0); + int zero_altitude_valid_count = 1; + double zero_altitude = 0.0; + auto zero = std::tuple(zero_position, zero_altitude_valid_count, zero_altitude); + int nonzeros = 0; + history.resize(average_count); + for (int i = 0; i < average_count; ++i) { + if (i % interpolate_count == interpolate_count - 1) { + history[average_count - 1 - i] = current; + ++nonzeros; + } else { + history[average_count - 1 - i] = zero; + } + } + accumulated_position = current_position * nonzeros; + accumulated_altitude_valid_count = current_altitude_valid_count * average_count; + accumulated_altitude = current_altitude * nonzeros; + filter_gain = static_cast(interpolate_count) / static_cast(average_count); + } else { + history.fill(current, average_count); + accumulated_position = current_position * average_count; + accumulated_altitude_valid_count = current_altitude_valid_count * average_count; + accumulated_altitude = current_altitude * average_count; + filter_gain = 1.0 / average_count; + } + counter = 0; + if (global_opts.debug_level >= 5) { + for (auto& entry : history) { + auto [pos, avc, alt] = entry; + qDebug() << "initial conditions" << pos << avc << alt; + } + qDebug() << "initial accumulator" << accumulated_position << accumulated_altitude_valid_count << accumulated_altitude; + } + } + + auto [oldest_position, oldest_altitude_valid_count, oldest_altitude] = history.at(counter); + + // subtract off the oldest values + accumulated_position -= oldest_position; + accumulated_altitude_valid_count -= oldest_altitude_valid_count; + accumulated_altitude -= oldest_altitude; + + history[counter] = current; + + // add in the newest values + accumulated_position += current_position; + accumulated_altitude_valid_count += current_altitude_valid_count; + accumulated_altitude += current_altitude; + + if (global_opts.debug_level >= 5) { + qDebug() << "position" << qSetRealNumberPrecision(12) << current_position << accumulated_position << accumulated_position.norm(); + qDebug() << "altitude" << qSetRealNumberPrecision(12) << accumulated_altitude_valid_count << current_altitude << accumulated_altitude; + } + + gpsbabel::NVector normalized_position = accumulated_position / accumulated_position.norm(); + wpt->latitude = normalized_position.latitude(); + wpt->longitude = normalized_position.longitude(); + if (accumulated_altitude_valid_count == average_count) { + wpt->altitude = accumulated_altitude * filter_gain; + } else { + wpt->altitude = unknown_alt; + } + + counter = (counter + 1) % average_count; +} + +void ResampleFilter::process() +{ + if (interpolateopt) { + RouteList backuptrack; + track_swap(backuptrack); + + if (backuptrack.empty()) { + fatal(FatalMsg() << MYNAME ": Found no tracks to operate on."); + } + + for (const auto* rte_old : qAsConst(backuptrack)) { + // FIXME: Allocating a new route_head and copying the members one at a + // time is not maintainable. When new members are added it is likely + // they will not be copied here! + // We want a deep copy of everything but with an empty WaypointList. + auto* rte_new = new route_head; + rte_new->rte_name = rte_old->rte_name; + rte_new->rte_desc = rte_old->rte_desc; + rte_new->rte_urls = rte_old->rte_urls; + rte_new->rte_num = rte_old->rte_num; + rte_new->fs = rte_old->fs.FsChainCopy(); + rte_new->line_color = rte_old->line_color; + rte_new->line_width = rte_old->line_width; + rte_new->session = rte_old->session; + track_add_head(rte_new); + + bool first = true; + const Waypoint* prevwpt; + for (const auto* wpt : rte_old->waypoint_list) { + if (first) { + first = false; + } else { + std::optional timespan; + if (prevwpt->creation_time.isValid() && wpt->creation_time.isValid()) { + timespan = wpt->creation_time.toMSecsSinceEpoch() - + prevwpt->creation_time.toMSecsSinceEpoch(); + } + + { + auto* newwpt = new Waypoint(*const_cast(prevwpt)); + newwpt->extra_data = nullptr; + track_add_wpt(rte_new, newwpt); + } + // Insert the required points + for (int n = 0; n < interpolate_count - 1; ++n) { + double frac = static_cast(n + 1) / + static_cast(interpolate_count); + // We create the inserted point from the Waypoint at the + // beginning of the span. We clear some fields but use a + // copy of the rest or the interpolated value. + auto* wpt_new = new Waypoint(*prevwpt); + wpt_new->wpt_flags.new_trkseg = 0; + wpt_new->shortname = QString(); + wpt_new->description = QString(); + if (timespan.has_value()) { + wpt_new->SetCreationTime(0, prevwpt->creation_time.toMSecsSinceEpoch() + + round(frac * *timespan)); + } else { + wpt_new->creation_time = gpsbabel::DateTime(); + } + // zero stuff + wpt_new->latitude = 0.0; + wpt_new->longitude = 0.0; + wpt_new->altitude = 0.0; + wpt_new->extra_data = &wpt_zero_stuffed; + track_add_wpt(rte_new, wpt_new); + } + + if (wpt == rte_old->waypoint_list.back()) { + auto* newwpt = new Waypoint(*const_cast(wpt)); + newwpt->extra_data = nullptr; + track_add_wpt(rte_new, newwpt); + } + } + + prevwpt = wpt; + } + } + backuptrack.flush(); + } + + if (averageopt) { + auto route_hdr = [this](const route_head* rte)->void { + // Filter in the forward direction + history.clear(); + for (auto it = rte->waypoint_list.cbegin(); it != rte->waypoint_list.cend(); ++it) + { + average_waypoint(*it, interpolateopt); + } + + // Filter in the reverse direction + if (global_opts.debug_level >= 5) + { + qDebug() << "Backward pass"; + } + history.clear(); + for (auto it = rte->waypoint_list.crbegin(); it != rte->waypoint_list.crend(); ++it) + { + average_waypoint(*it, false); + } + }; + + track_disp_all(route_hdr, nullptr, nullptr); + } + + if (decimateopt) { + // This is ~20x faster than deleting the points in the existing route one at a time. + RouteList backuptrack; + track_swap(backuptrack); + + if (backuptrack.empty()) { + fatal(FatalMsg() << MYNAME ": Found no tracks to operate on."); + } + + for (const auto* rte_old : qAsConst(backuptrack)) { + // FIXME: Allocating a new route_head and copying the members one at a + // time is not maintainable. When new members are added it is likely + // they will not be copied here! + // We want a deep copy of everything but with an empty WaypointList. + auto* rte_new = new route_head; + rte_new->rte_name = rte_old->rte_name; + rte_new->rte_desc = rte_old->rte_desc; + rte_new->rte_urls = rte_old->rte_urls; + rte_new->rte_num = rte_old->rte_num; + rte_new->fs = rte_old->fs.FsChainCopy(); + rte_new->line_color = rte_old->line_color; + rte_new->line_width = rte_old->line_width; + rte_new->session = rte_old->session; + track_add_head(rte_new); + + bool newseg = false; + int index = 0; + for (const auto* wpt : rte_old->waypoint_list) { + if (index % decimate_count == 0) { + auto* newwpt = new Waypoint(*const_cast(wpt)); + if (newseg) { + newwpt->wpt_flags.new_trkseg = 1; + } + track_add_wpt(rte_new, newwpt); + newseg = false; + } else { + // carry any new track segment marker forward. + if (wpt->wpt_flags.new_trkseg) { + newseg = true; + } + } + ++index; + } + } + backuptrack.flush(); + } +} + +void ResampleFilter::init() +{ + + if (averageopt) { + bool ok; + average_count = QString(averageopt).toInt(&ok); + if (!ok || average_count < 2) { + fatal(FatalMsg() << MYNAME ": the average count must be greater than one."); + } + } + + if (decimateopt) { + bool ok; + decimate_count = QString(decimateopt).toInt(&ok); + if (!ok || decimate_count < 2) { + fatal(FatalMsg() << MYNAME ": the decimate count must be greater than one."); + } + } + + if (interpolateopt) { + bool ok; + interpolate_count = QString(interpolateopt).toInt(&ok); + if (!ok || interpolate_count < 2) { + fatal(FatalMsg() << MYNAME ": the interpolate count must be greater than one."); + } + if (!averageopt || average_count < interpolate_count) { + fatal(FatalMsg() << MYNAME ": the average option must be used with interpolation, and the average count must be greater than or equal to the interpolation count."); + } + } +} + +void ResampleFilter::deinit() +{ + history.clear(); + history.squeeze(); +} + +#endif // FILTERS_ENABLED diff --git a/resample.h b/resample.h new file mode 100644 index 000000000..51cb0b832 --- /dev/null +++ b/resample.h @@ -0,0 +1,85 @@ +/* + Track resampling filter + + Copyright (C) 2021 Robert Lipe, robertlipe+source@gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +#ifndef RESAMPLE_H_INCLUDED_ +#define RESAMPLE_H_INCLUDED_ + +#include // for tuple + +#include // for QVector + +#include "defs.h" // for arglist_t, ARGTYPE_INT, Waypoint, route_head +#include "filter.h" // for Filter +#include "src/core/nvector.h" // for NVector + + +#if FILTERS_ENABLED + +class ResampleFilter:public Filter +{ +public: + QVector* get_args() override + { + return &args; + } + void init() override; + void deinit() override; + void process() override; + +private: + + void average_waypoint(Waypoint* wpt, bool zero_stuffed); + + QVector> history; + gpsbabel::NVector accumulated_position; + int accumulated_altitude_valid_count{0}; + double accumulated_altitude{0.0}; + double filter_gain{0.0}; + int wpt_zero_stuffed{}; + + int counter{0}; + int average_count{0}; + int decimate_count{0}; + int interpolate_count{0}; + + char* decimateopt{nullptr}; + char* interpolateopt{nullptr}; + char* averageopt{nullptr}; + + QVector args = { + { + "decimate", &decimateopt, "Decimate, decrease sample rate by a factor of n", nullptr, + ARGTYPE_INT, "2", nullptr, nullptr + }, + { + "interpolate", &interpolateopt, "Interpolate, increase sample rate by a factor of n", nullptr, + ARGTYPE_INT, "2", nullptr, nullptr + }, + { + "average", &averageopt, "Running average of n points", nullptr, + ARGTYPE_INT, "2", nullptr, nullptr + } + }; + +}; + +#endif // FILTERS_ENABLED +#endif // RESAMPLE_H_INCLUDED_ diff --git a/src/core/nvector.cc b/src/core/nvector.cc new file mode 100644 index 000000000..a35efc691 --- /dev/null +++ b/src/core/nvector.cc @@ -0,0 +1,450 @@ +/* + Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +// https://en.wikipedia.org/wiki/N-vector +// http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf + +#include +#include +#include +#include +#include + +#include "nvector.h" +#include "vector3d.h" + +namespace gpsbabel +{ + +LatLon::LatLon(double latitude, double longitude) +{ + lat = latitude; + lon = longitude; +} + +NVector::NVector(double latitude_degrees, double longitude_degrees) +{ + // This implements equation 3. + + // The coordinate system choice matches + // A_Nonsingular_Horizontal_Position_Representation.pdf + // The E frame: + // from the earths center + // x points to North pole + // y points towards latitude 0, longitude +90 (east) + // z points to latitude 0, longitude +180, the antimeridian + double latitude_radians = latitude_degrees * kRadiansPerDegree; + double longitude_radians = longitude_degrees * kRadiansPerDegree; + x_ = sin(latitude_radians); + y_ = sin(longitude_radians)*cos(latitude_radians); + z_ = -cos(longitude_radians)*cos(latitude_radians); +} + +NVector::NVector(const Vector3D& v) +{ + x_ = v.getx(); + y_ = v.gety(); + z_ = v.getz(); +} + +std::pair PVector::toNVectorAndHeight() const +{ + // This implements equation 23. + constexpr double a = WGS84_SEMI_MAJOR_AXIS_METERS; + constexpr double a2 = a * a; + constexpr double e2 = WGS84_ECCENTRICITY_SQUARED; + constexpr double e4 = e2 * e2; + + double px2 = x_ * x_; + double py2 = y_ * y_; + double pz2 = z_ * z_; + + double q = ((1.0-e2)/(a2)) * px2; + double p = (py2 + pz2) / (a2); + double r = (p+q-e4) / 6.0; + double s = (e4*p*q) / (4.0*r*r*r); + double t = cbrt(1.0 + s + sqrt(s*(2.0+s))); + double u = r * (1.0 + t + (1.0/t)); + double v = sqrt(u*u + e4*q); + double w = e2 * ((u+v-q)/(2.0*v)); + double k = sqrt(u+v+w*w) - w; + double d = k*sqrt(py2 + pz2) / (k+e2); + double sf = 1.0 / (sqrt(d*d + px2)); + double sf2 = k / (k+e2); + + double height = ((k+e2-1.0)/k) * sqrt(d*d+px2); + double nx = sf * x_; + double ny = sf * sf2 * y_; + double nz = sf * sf2 * z_; + + return {Vector3D(nx, ny, nz), height}; +} + +double NVector::latitude() const +{ + // This implements equation 6. + double latitude_radians = atan2(x_, sqrt(y_*y_ + z_*z_)); + double latitude_degrees = latitude_radians * kDegreesPerRadian; + return latitude_degrees; +} + +double NVector::longitude() const +{ + // This implements equation 5. + double longitude_radians = atan2(y_, -z_); + double longitude_degrees = longitude_radians * kDegreesPerRadian; + return longitude_degrees; +} + +// great circle distance in radians +double NVector::distance_radians(const NVector& n_EA_E, const NVector& n_EB_E) +{ + // This implements equation 16 using arctan for numerical accuracy. + double result = atan2(crossProduct(n_EA_E, n_EB_E).norm(), + dotProduct(n_EA_E, n_EB_E)); + return result; +} + +// great circle distance in meters +double NVector::distance(const NVector& n_EA_E, const NVector& n_EB_E) +{ + double result = distance_radians(n_EA_E, n_EB_E) * MEAN_EARTH_RADIUS_METERS; + return result; +} + +double NVector::distance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees) +{ + NVector n_EA_E(latitude_a_degrees, longitude_a_degrees); + NVector n_EB_E(latitude_b_degrees, longitude_b_degrees); + double result = distance(n_EA_E, n_EB_E); + return result; +} + +double NVector::azimuth_radians(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b) +{ + PVector p_EA_E(n_EA_E, height_a); + PVector p_EB_E(n_EB_E, height_b); + Vector3D p_AB_E = p_EB_E - p_EA_E; + + // equation 9 (normalized) + Vector3D kaeast = crossProduct(Vector3D(1.0, 0.0, 0.0), n_EA_E).normalize(); + // equation 10 (normalized, n_EA_E and kaeast are perpendicular unit vectors) + Vector3D kanorth = crossProduct(n_EA_E, kaeast); + + // equation 11 + // Ren = [kanorth(normalized) kaeast(normalized) -n_EA_E]; + // and a rotation from the E frame to the N frame (North, East, Down). + // P_AB_N = R_EN' * p_AB_E + double P_AB_N_x = kanorth*p_AB_E; + double P_AB_N_y = kaeast*p_AB_E; + // double P_AB_N_z = (-n_EA_E)*p_AB_E; + + double azimuth = atan2(P_AB_N_y, P_AB_N_x); + return azimuth; +} + +double NVector::azimuth(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b) +{ + double azimuth = azimuth_radians(n_EA_E, n_EB_E, height_a, height_b); + double azimuth_degrees = azimuth * kDegreesPerRadian; + return azimuth_degrees; +} + +// returns values in the range [0.0,360) +double NVector::azimuth_true_degrees(const NVector& n_EA_E, const NVector& n_EB_E, double height_a, double height_b) +{ + double result = azimuth(n_EA_E, n_EB_E, height_a, height_b); + if (result < 0.0) { + result += 360.0; + } + if (result >= 360.0) { + result = 0.0; + } + return result; +} + +double NVector::azimuth(double latitude_a_degrees, double longitude_a_degrees, + double latitude_b_degrees, double longitude_b_degrees, + double height_a, double height_b) +{ + NVector n_EA_E(latitude_a_degrees, longitude_a_degrees); + NVector n_EB_E(latitude_b_degrees, longitude_b_degrees); + return azimuth(n_EA_E, n_EB_E, height_a, height_b); +} + +// returns values in the range [0.0,360) +double NVector::azimuth_true_degrees(double latitude_a_degrees, double longitude_a_degrees, + double latitude_b_degrees, double longitude_b_degrees, + double height_a, double height_b) +{ + double result = azimuth(latitude_a_degrees, longitude_a_degrees, + latitude_b_degrees, longitude_b_degrees, + height_a, height_b); + if (result < 0.0) { + result += 360.0; + } + if (result >= 360.0) { + result = 0.0; + } + return result; +} + +#if 0 +// This interpolation is nonlinear! +NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction) +{ + Vector3D n_ER_E = (n_EA_E + (n_EB_E - n_EA_E)*fraction).normalize(); + return n_ER_E; +} +#elif 0 +NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction) +{ + // see example 8. + double sab_rad = distance_radians(n_EA_E, n_EB_E); + double sar_rad = fraction*sab_rad; + double az_rad = azimuth_radians(n_EA_E, n_EB_E); + + // equation 9 (normalized) + Vector3D kaeast = crossProduct(Vector3D(1.0, 0.0, 0.0), n_EA_E).normalize(); + // equation 10 (normalized, n_EA_E and kaeast are perpendicular unit vectors) + Vector3D kanorth = crossProduct(n_EA_E, kaeast); + + Vector3D d_EA_E = kanorth*cos(az_rad) + kaeast*sin(az_rad); + Vector3D n_ER_E = n_EA_E*cos(sar_rad) + d_EA_E*sin(sar_rad); + return n_ER_E; +} +#else +NVector NVector::linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction) +{ + double dp_a_b = dotProduct(n_EA_E, n_EB_E); + if (dp_a_b >= (1.0-8.0*DBL_EPSILON)) { + // The points are so close we will have trouble constructing a basis. + // Since they are so close the non-linearities in direct n vector + // interpolation are negligible. + Vector3D result = (n_EA_E + (n_EB_E-n_EA_E)*fraction).normalize(); + return result; + } + if (dp_a_b <= (-1.0+8.0*DBL_EPSILON)) { + // The points are so close to be exactly opposite each other that + // there isn't a unique great circle between them. + // Unless fraction is 1.0 or 0.0 there isn't a unique answer. + Vector3D result(nan(""), nan(""), nan("")); + return result; + } + // Form an orthonormal basis with one component perpendicular to the great + // circle containing A and B, and one component being A. + // Call this the W frame. + // The columns of the rotation matrix from E to W are w1, w2 and w3. + Vector3D w1 = n_EA_E; + Vector3D w2 = crossProduct(n_EB_E, n_EA_E).normalize(); + Vector3D w3 = crossProduct(n_EA_E,w2); + // Rotate A and B to the W frame. + // Vector3D n_EA_W = Vector3D(w1*n_EA_E, w2*n_EA_E, w3*n_EA_E); + Vector3D n_EB_W = Vector3D(w1*n_EB_E, w2*n_EB_E, w3*n_EB_E); + // By construction n_EA_W.y is (1, 0, 0), + // n_EB_W.y is zero, + // and both n_EA_W and n_EB_W are unit vectors. + // The information is all in the angle between them, + // which is all in n_EB_W.z and n_EB_W.x. + double theta = atan2(n_EB_W.getz(), n_EB_W.getx()); + // We define the interpolated point as the point on the great circle + // between A and B whose great circle distance from A is the given fraction + // of the great circle distance between A and B. For a spheroid the + // distance is proportional to the angle between the vectors. + // The interpolated point is thus: + Vector3D n_EX_W = Vector3D(cos(fraction*theta), 0.0, sin(fraction*theta)); + // Translate the interpolated point back to the E frame. + // We need to invert the matrix composed of the column vectors w1, w2, w3. + // Since this matrix is orthogonal it's inverse equals it's transpose. + Vector3D wt1 = Vector3D(w1.getx(), w2.getx(), w3.getx()); + Vector3D wt2 = Vector3D(w1.gety(), w2.gety(), w3.gety()); + Vector3D wt3 = Vector3D(w1.getz(), w2.getz(), w3.getz()); + Vector3D n_EX_E = Vector3D(wt1*n_EX_W, wt2*n_EX_W, wt3*n_EX_W); + return n_EX_E; +} +#endif + +LatLon NVector::linepart(double latitude_a_degrees, double longitude_a_degrees, + double latitude_b_degrees, double longitude_b_degrees, + double fraction) +{ + NVector n_EA_E(latitude_a_degrees, longitude_a_degrees); + NVector n_EB_E(latitude_b_degrees, longitude_b_degrees); + NVector n_ER_E = linepart(n_EA_E, n_EB_E, fraction); + LatLon result(n_ER_E.latitude(), n_ER_E.longitude()); + return result; +} + +// Find the point of closest approach to Y on the great circle arc AB. +// The great circle arc AB is the shortest of the two possibilities. +#if 0 +// This implementation works and passes regression. +// However, it seems harder to understand than the next implementation. +NVector NVector::crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E) +{ + // Compute the normal to the great circle defined by A and B: + Vector3D a_cross_b = crossProduct(n_EA_E, n_EB_E); + // Because a_cross_b is normal to the plane defined by n_EA_E and n_EB_E, + // any great circle defined by the point defined by a_cross_b and + // any other point, e.g. Y, + // will cross the great circle including the points A and B perpendicularly. + // Thus, if we find the intersection of the two great circles we will + // have found the projection of Y onto the great circle defined + // by A and B. + Vector3D x = a_cross_b; + // Compute the normal to the great circle defined by Y and X: + Vector3D x_cross_y = crossProduct(x, n_EY_E); + // A candidate projection from X onto the great circle defined by A and B is + // c_candidate, the other possibility is -c_candidate. + Vector3D c_candidate = crossProduct(a_cross_b, x_cross_y).normalize(); + // pick the candidate closest to Y. + Vector3D c = dotProduct(c_candidate, n_EY_E) >= 0.0 ? c_candidate : -c_candidate; + // find the midpoint between A and B. + Vector3D m = (n_EA_E + n_EB_E).normalize(); + // Now we have a point C on the great circle defined by A and B, + // and the midpoint M of this arc. + // If C is on the arc defined by A and B then the point of closest + // approach to Y is C. But if C is not on the arc, then the point + // of closest approach is either A or B, whichever is close to C. + // Note that A, B, C, M all are all on the great circle defined by A and B, so + // the dot product of any of these two unit vectors monotonically decreases + // the farther apart they are on the great circle. + Vector3D result; + if (dotProduct(n_EA_E, m) < dotProduct(c, m)) { + result = c; + } else if (dotProduct(n_EA_E, c) < dotProduct(n_EB_E, c)) { + result = n_EB_E; + } else { + result = n_EA_E; + } + return result; +} +#else +NVector NVector::crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E) +{ + // Form an orthonormal basis with one component perpendicular to the great + // circle containing A and B, and one component being A. + // Call this the W frame. + // The columns of the rotation matrix from E to W are w1, w2 and w3. + Vector3D w1 = n_EA_E; + Vector3D w2 = crossProduct(n_EB_E, n_EA_E).normalize(); + Vector3D w3 = crossProduct(n_EA_E,w2); + // Rotate Y to the W frame. + Vector3D n_EY_W = Vector3D(w1*n_EY_E, w2*n_EY_E, w3*n_EY_E); + // By construction n_EA_W.y is (1, 0, 0), + // n_EB_W.y is zero, + // and both n_EA_W and n_EB_W are unit vectors. + // The projection of Y onto the great circle defined by A and B + // is just the n_EY_W with the y component set to zero. + Vector3D n_EC_W = Vector3D(n_EY_W.getx(), 0.0, n_EY_W.getz()).normalize(); + // Translate the projected point back to the E frame. + // We need to invert the matrix composed of the column vectors w1, w2, w3. + // Since this matrix is orthogonal it's inverse equals it's transpose. + Vector3D wt1 = Vector3D(w1.getx(), w2.getx(), w3.getx()); + Vector3D wt2 = Vector3D(w1.gety(), w2.gety(), w3.gety()); + Vector3D wt3 = Vector3D(w1.getz(), w2.getz(), w3.getz()); + Vector3D n_EC_E = Vector3D(wt1*n_EC_W, wt2*n_EC_W, wt3*n_EC_W); + // find the midpoint between A and B. + Vector3D n_EM_E = (n_EA_E + n_EB_E).normalize(); + // Now we have a point C on the great circle defined by A and B, + // and the midpoint M of this arc. + // If C is on the arc defined by A and B then the point of closest + // approach to Y is C. But if C is not on the arc, then the point + // of closest approach is either A or B, whichever is close to C. + // Note that A, B, C, M all are all on the great circle defined by A and B, so + // the dot product of any of these two unit vectors monotonically decreases + // the farther apart they are on the great circle. + Vector3D result; + if (dotProduct(n_EA_E, n_EM_E) < dotProduct(n_EC_E, n_EM_E)) { + result = n_EC_E; + } else if (dotProduct(n_EA_E, n_EC_E) < dotProduct(n_EB_E, n_EC_E)) { + result = n_EB_E; + } else { + result = n_EA_E; + } + return result; +} +#endif + +LatLon NVector::crossTrackProjection(double latitude_a_degrees, double longitude_a_degrees, + double latitude_b_degrees, double longitude_b_degrees, + double latitude_x_degrees, double longitude_x_degrees) +{ + NVector n_EA_E(latitude_a_degrees, longitude_a_degrees); + NVector n_EB_E(latitude_b_degrees, longitude_b_degrees); + NVector n_EX_E(latitude_x_degrees, longitude_x_degrees); + NVector n_EC_E = crossTrackProjection(n_EA_E, n_EB_E, n_EX_E); + LatLon result(NVector(n_EC_E).latitude(), NVector(n_EC_E).longitude()); + return result; +} + +// Find the minimum distance to point X when traveling from A to B. +#if 0 +// This doesn't work if we are more than 90 degrees away +double NVector::crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E) +{ + Vector3D c_E = crossProduct(n_EA_E, n_EB_E).normalize(); + double result = fabs((atan2(crossProduct(c_E, n_EX_E).norm(), + dotProduct(c_E, n_EX_E)) - M_PI/2.0)) * MEAN_EARTH_RADIUS_METERS; + return result; +} +#else +double NVector::crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E) +{ + NVector n_EP_E = crossTrackProjection(n_EA_E, n_EB_E, n_EX_E); + double result = distance(n_EP_E, n_EX_E); + return result; +} +#endif + +double NVector::crossTrackDistance(double latitude_a_degrees, double longitude_a_degrees, + double latitude_b_degrees, double longitude_b_degrees, + double latitude_x_degrees, double longitude_x_degrees) +{ + NVector n_EA_E(latitude_a_degrees, longitude_a_degrees); + NVector n_EB_E(latitude_b_degrees, longitude_b_degrees); + NVector n_EX_E(latitude_x_degrees, longitude_x_degrees); + double result = crossTrackDistance(n_EA_E, n_EB_E, n_EX_E); + return result; +} + +PVector::PVector(const NVector& n_EA_E, double h=0.0) +{ + // This implements equation 22. + + // a semi-major axis, b semi-minor axis, f flattening + // a/b = 1/(1-f) + // aspect ratio = b/a + constexpr double b = WGS84_SEMI_MINOR_AXIS_METERS; + constexpr double asq_over_bsq = 1.0 / (WGS84_ASPECT_RATIO * WGS84_ASPECT_RATIO); + + double denom = sqrt(n_EA_E.getx()*n_EA_E.getx() + n_EA_E.gety()*n_EA_E.gety()*asq_over_bsq + n_EA_E.getz()*n_EA_E.getz()*asq_over_bsq); + x_ = (b/denom)*n_EA_E.getx() + h*n_EA_E.getx(); + y_ = (b/denom)*asq_over_bsq*n_EA_E.gety() + h*n_EA_E.gety(); + z_ = (b/denom)*asq_over_bsq*n_EA_E.getz() + h*n_EA_E.getz(); +} + +PVector::PVector(const Vector3D& v) +{ + x_ = v.getx(); + y_ = v.gety(); + z_ = v.getz(); +} + +} // namespace gpsbabel diff --git a/src/core/nvector.h b/src/core/nvector.h new file mode 100644 index 000000000..4cd7b820f --- /dev/null +++ b/src/core/nvector.h @@ -0,0 +1,97 @@ +/* + Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +#ifndef NVECTOR_H +#define NVECTOR_H + +#include "defs.h" +#include "vector3d.h" + +namespace gpsbabel +{ + +//#define COMPARE_BEARING_TO_GRTCIRC +#ifdef COMPARE_BEARING_TO_GRTCIRC +constexpr double MEAN_EARTH_RADIUS_METERS = 6378137.0; +#else +constexpr double MEAN_EARTH_RADIUS_METERS = 6371000.0; +#endif +constexpr double WGS84_SEMI_MAJOR_AXIS_METERS = 6378137.0; // a +#ifdef COMPARE_BEARING_TO_GRTCIRC +constexpr double WGS84_FLATTENING = 0.0; +#else +constexpr double WGS84_FLATTENING = 1.0/298.257223563; // (a-b)/a +#endif +constexpr double WGS84_ASPECT_RATIO = 1.0 - WGS84_FLATTENING; // b/a +constexpr double WGS84_SEMI_MINOR_AXIS_METERS = WGS84_SEMI_MAJOR_AXIS_METERS * WGS84_ASPECT_RATIO; // b +constexpr double WGS84_ECCENTRICITY_SQUARED = 1.0 - (WGS84_ASPECT_RATIO * WGS84_ASPECT_RATIO); + +constexpr double kRadiansPerDegree = M_PI/180.0; +constexpr double kDegreesPerRadian = 1.0/kRadiansPerDegree; + +class PVector; + +class LatLon +{ +public: + LatLon(double latitude, double longitude); + + double lat; + double lon; +}; + +class NVector : public Vector3D +{ + +public: + NVector() = default; + NVector(double latitude_degrees, double longitude_degrees); + NVector(const Vector3D& v); + + [[nodiscard]] double latitude() const; + [[nodiscard]] double longitude() const; + + static double distance_radians(const NVector& n_EA_E, const NVector& n_EB_E); + static double distance(const NVector& n_EA_E, const NVector& n_EB_E); + static double distance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees); + static double azimuth_radians(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0); + static double azimuth(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0); + static double azimuth(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double height_a = 0.0, double height_b = 0.0); + static double azimuth_true_degrees(const NVector& n_EA_E, const NVector& n_EB_E, double height_a = 0.0, double height_b = 0.0); + static double azimuth_true_degrees(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double height_a = 0.0, double height_b = 0.0); + static NVector linepart(const NVector& n_EA_E, const NVector& n_EB_E, double fraction); + static LatLon linepart(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double fraction); + static NVector crossTrackProjection(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EY_E); + static LatLon crossTrackProjection(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double latitude_x_degrees, double longitude_x_degrees); + static double crossTrackDistance(const NVector& n_EA_E, const NVector& n_EB_E, const NVector& n_EX_E); + static double crossTrackDistance(double latitude_a_degrees, double longitude_a_degrees, double latitude_b_degrees, double longitude_b_degrees, double latitude_x_degrees, double longitude_x_degrees); +}; + +class PVector : public Vector3D +{ +public: + PVector() = default; + PVector(const NVector& n_EA_E, double h); + PVector(const Vector3D& v); + + [[nodiscard]] std::pair toNVectorAndHeight() const; +}; + +} // namespace gpsbabel +#endif // NVECTOR_H diff --git a/src/core/vector3d.cc b/src/core/vector3d.cc new file mode 100644 index 000000000..e5925a5e7 --- /dev/null +++ b/src/core/vector3d.cc @@ -0,0 +1,149 @@ +/* + Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +// https://en.wikipedia.org/wiki/N-vector +// http://www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf + +#include +#include +#include "vector3d.h" + +namespace gpsbabel +{ + +double Vector3D::norm() const +{ + double norm = sqrt(x_*x_ + y_*y_ + z_*z_); + return norm; +} + +Vector3D& Vector3D::normalize() +{ + *this = *this/this->norm(); + return *this; +} + +Vector3D& Vector3D::operator+=(const Vector3D& rhs) +{ + x_ += rhs.x_; + y_ += rhs.y_; + z_ += rhs.z_; + return *this; +} + +Vector3D& Vector3D::operator-=(const Vector3D& rhs) +{ + x_ -= rhs.x_; + y_ -= rhs.y_; + z_ -= rhs.z_; + return *this; +} + +Vector3D& Vector3D::operator*=(double rhs) +{ + x_ *= rhs; + y_ *= rhs; + z_ *= rhs; + return *this; +} + +Vector3D& Vector3D::operator/=(double rhs) +{ + x_ /= rhs; + y_ /= rhs; + z_ /= rhs; + return *this; +} + +Vector3D Vector3D::operator+ (const Vector3D& rhs) const +{ + Vector3D result = *this; + result += rhs; + return result; +} + +Vector3D Vector3D::operator- (const Vector3D& rhs) const +{ + Vector3D result = *this; + result -= rhs; + return result; +} + +Vector3D Vector3D::operator* (double rhs) const +{ + Vector3D result = *this; + result *= rhs; + return result; +} + +Vector3D Vector3D::operator/ (double rhs) const +{ + Vector3D result = *this; + result /= rhs; + return result; +} + +Vector3D Vector3D::operator- () const +{ + Vector3D result(-x_,-y_,-z_); + return result; +} + +double Vector3D::operator* (const Vector3D& b) const +{ + double result = x_*b.x_ + y_*b.y_ + z_*b.z_; + return result; +} + +double Vector3D::dotProduct(const Vector3D& a, const Vector3D& b) +{ + double dotproduct = a.x_*b.x_ + a.y_*b.y_ + a.z_*b.z_; + return dotproduct; +} + +Vector3D Vector3D::crossProduct(const Vector3D& b, const Vector3D& c) +{ + // a = b x c + Vector3D a; + a.x_ = b.y_*c.z_ - b.z_*c.y_; + a.y_ = b.z_*c.x_ - b.x_*c.z_; + a.z_ = b.x_*c.y_ - b.y_*c.x_; + return a; +} + +Vector3D operator* (double lhs, const Vector3D& rhs) +{ + Vector3D result = rhs*lhs; + return result; +} + +std::ostream& operator<< (std::ostream& os, const Vector3D& v) +{ + os << '(' << v.getx() << ", " << v.gety() << ", " << v.getz() << ')'; + return os; +} + +QDebug operator<< (QDebug debug, const Vector3D& v) +{ + QDebugStateSaver saver(debug); + debug.nospace() << '(' << v.getx() << ", " << v.gety() << ", " << v.getz() << ')'; + return debug; +} + +} // namespace gpsbabel diff --git a/src/core/vector3d.h b/src/core/vector3d.h new file mode 100644 index 000000000..ad11521ab --- /dev/null +++ b/src/core/vector3d.h @@ -0,0 +1,66 @@ +/* + Copyright (C) 2018, 2021 Robert Lipe, gpsbabel.org + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + */ + +#ifndef VECTOR3D_H +#define VECTOR3D_H + +#include + +#include + +namespace gpsbabel +{ + +class Vector3D +{ +public: + Vector3D() = default; + Vector3D(double xi, double yi, double zi) : x_(xi), y_(yi), z_(zi) {} + + [[nodiscard]] double norm() const; + [[nodiscard]] double getx() const {return x_;} + [[nodiscard]] double gety() const {return y_;} + [[nodiscard]] double getz() const {return z_;} + Vector3D& normalize(); + + Vector3D& operator+=(const Vector3D& rhs); + Vector3D& operator-=(const Vector3D& rhs); + Vector3D& operator*=(double rhs); + Vector3D& operator/=(double rhs); + Vector3D operator+ (const Vector3D& rhs) const; + Vector3D operator- (const Vector3D& rhs) const; + Vector3D operator* (double rhs) const; + Vector3D operator/ (double rhs) const; + Vector3D operator- () const; + double operator* (const Vector3D& b) const; + + static Vector3D crossProduct(const Vector3D& b, const Vector3D& c); + static double dotProduct(const Vector3D& a, const Vector3D& b); + +protected: + double x_{}; + double y_{}; + double z_{}; +}; +Vector3D operator* (double lhs, const Vector3D& rhs); +std::ostream& operator<< (std::ostream& os, const Vector3D& v); +QDebug operator<<(QDebug debug, const Vector3D& v); + +} // namespace gpsbabel +#endif // VECTOR3D_H diff --git a/testo.d/resample.test b/testo.d/resample.test new file mode 100644 index 000000000..0f2926669 --- /dev/null +++ b/testo.d/resample.test @@ -0,0 +1,5 @@ + +gpsbabel -t -i unicsv -f ${REFERENCE}/track/sim.csv -x resample,interpolate=2,average=2 -o unicsv,utc=0 -F ${TMPDIR}/simint.csv +compare ${REFERENCE}/track/simint.csv ${TMPDIR}/simint.csv +gpsbabel -t -i unicsv -f ${REFERENCE}/track/sim.csv -x resample,average=4,decimate=2 -o unicsv,utc=0 -F ${TMPDIR}/simdec.csv +compare ${REFERENCE}/track/simdec.csv ${TMPDIR}/simdec.csv diff --git a/xmldoc/filters/options/resample-average.xml b/xmldoc/filters/options/resample-average.xml new file mode 100644 index 000000000..52dea2716 --- /dev/null +++ b/xmldoc/filters/options/resample-average.xml @@ -0,0 +1,3 @@ + +This options is used to control the amount of filtering. The value of this option is the length of the running average filter that is used to smooth the data. The running average filter is applied once in the forward direction and once in the backwards direction. If using this option the minimum value is two. + diff --git a/xmldoc/filters/options/resample-decimate.xml b/xmldoc/filters/options/resample-decimate.xml new file mode 100644 index 000000000..5e561d41d --- /dev/null +++ b/xmldoc/filters/options/resample-decimate.xml @@ -0,0 +1,3 @@ + +This options is used to decrease the sample rate. The value of this option is an integer factor to decrease the sample rate by. If using this option the minimum value is two. Normally decimation would also use averaging, but it is not required. This option may be useful in the case of very long tracks that were sampled at an inappropriately high rate. + diff --git a/xmldoc/filters/options/resample-interpolate.xml b/xmldoc/filters/options/resample-interpolate.xml new file mode 100644 index 000000000..cfa4c4942 --- /dev/null +++ b/xmldoc/filters/options/resample-interpolate.xml @@ -0,0 +1,4 @@ + +This options is used to increase the sample rate. The value of this option is an integer factor to increase the sample rate by. If using this option the minimum value is two. +If using this option the average option must be used with average value greater than or equal to the interpolate value. It is recommended to use an average value that is an integer multiple of the interpolate value. + diff --git a/xmldoc/filters/resample.xml b/xmldoc/filters/resample.xml new file mode 100644 index 000000000..bc2368211 --- /dev/null +++ b/xmldoc/filters/resample.xml @@ -0,0 +1,28 @@ + +The resampling filter can be used to change the sample rate of a track. It is intended to be used with track points that have been sampled at a constant rate. It can be used to change the sample rate by a rational factor. It can also be used to smooth a track with or without changing the sample rate. The filter works across the antimeridian. + + + +Interpolation with the resampling filter + +This examples doubles the sample rate. The data is filtered after interpolation regardless of the order of the options. + +gpsbabel -t -i unicsv -f data.csv -x resample,interpolate=2,average=2 -o unicsv,utc=0 -F fast.csv + + + +Decimation with the resampling filter + +This examples reduces the sample rate by a factor of 4. The data if filtered before decimation regardless of the order of the options. + +gpsbabel -t -i unicsv -f data.csv -x resample,average=4,decimate=2 -o unicsv,utc=0 -F slow.csv + + + +Averaging with the resampling filter + +This examples averages the adjacent points. A running average filter of length two samples is applied in the forward and reverse directions. + +gpsbabel -t -i unicsv -f data.csv -x resample,average=2 -o unicsv,utc=0 -F smooth.csv + +