From 628be72e1f49749bdf857f476338f1926e6c5e5b Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 10 Oct 2017 17:00:12 +0200 Subject: [PATCH 01/13] addded netlink and iw based wifi scanner for linux --- CMakeLists.txt | 7 +- main.cpp | 15 +- sensors/MACAddress.h | 38 +-- sensors/radio/scan/WiFiChannels.h | 30 ++ sensors/radio/scan/WiFiScan.h | 15 + sensors/radio/scan/WiFiScanLinux.h | 419 ++++++++++++++++++++++++++ sensors/radio/scan/WiFiScanLinux_IW.h | 100 ++++++ 7 files changed, 601 insertions(+), 23 deletions(-) create mode 100644 sensors/radio/scan/WiFiChannels.h create mode 100644 sensors/radio/scan/WiFiScan.h create mode 100644 sensors/radio/scan/WiFiScanLinux.h create mode 100644 sensors/radio/scan/WiFiScanLinux_IW.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f214537..908fa41 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,7 @@ else() # system specific compiler flags ADD_DEFINITIONS( - -std=gnu++11 + -std=gnu++11 -Wall -Werror=return-type @@ -95,11 +95,16 @@ ADD_EXECUTABLE( ${SOURCES} ) +SET(EXTRA_LIBS ${EXTRA_LIBS} nl-genl-3 nl-3) +INCLUDE_DIRECTORIES(/usr/include/libnl3/) +SET(EXTRA_LIBS ${EXTRA_LIBS} iw) + # needed external libraries TARGET_LINK_LIBRARIES( ${PROJECT_NAME} gtest pthread + ${EXTRA_LIBS} ) SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) diff --git a/main.cpp b/main.cpp index 8657c03..492f544 100755 --- a/main.cpp +++ b/main.cpp @@ -10,8 +10,17 @@ class Test : public GridPoint { #include "tests/Tests.h" +#include "sensors/radio/scan/WiFiScanLinux.h" +void wifi() { + const std::string dev = "wlp0s20u2u1"; + WiFiScanLinux scanner(dev); + scanner.scan(); +} + int main(int argc, char** argv) { + wifi(); return 0; + #ifdef WITH_TESTS ::testing::InitGoogleTest(&argc, argv); @@ -25,8 +34,8 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Grid.*"; //::testing::GTEST_FLAG(filter) = "*Dijkstra.*"; - //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; - //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; + //::testing::GTEST_FLAG(filter) = "*LogDistanceCeilingModelBeacon*"; + //::testing::GTEST_FLAG(filter) = "*WiFiOptimizer*"; //::testing::GTEST_FLAG(filter) = "*Offline.readWrite*"; @@ -41,7 +50,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*BVH*"; - //::testing::GTEST_FLAG(filter) = "*Barometer*"; + //::testing::GTEST_FLAG(filter) = "*Barometer*"; //::testing::GTEST_FLAG(filter) = "*GridWalk2RelPressure*"; //::testing::GTEST_FLAG(filter) = "Heading*"; diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h index 8c31237..7443aed 100644 --- a/sensors/MACAddress.h +++ b/sensors/MACAddress.h @@ -47,27 +47,27 @@ public: MACAddress(const std::string& str) { // sanity check - if (str.size() == 17 ){ - mac = 0; // all zeros - fields.h5 = hexWordToInt(str[ 0], str[ 1]); - fields.h4 = hexWordToInt(str[ 3], str[ 4]); - fields.h3 = hexWordToInt(str[ 6], str[ 7]); - fields.h2 = hexWordToInt(str[ 9], str[10]); - fields.h1 = hexWordToInt(str[12], str[13]); - fields.h0 = hexWordToInt(str[15], str[16]); - } - else if (str.size() == 12){ - mac = 0; // all zeros - fields.h5 = hexWordToInt(str[ 0], str[ 1]); - fields.h4 = hexWordToInt(str[ 2], str[ 3]); - fields.h3 = hexWordToInt(str[ 4], str[ 5]); + if (str.size() == 17 ){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 3], str[ 4]); + fields.h3 = hexWordToInt(str[ 6], str[ 7]); + fields.h2 = hexWordToInt(str[ 9], str[10]); + fields.h1 = hexWordToInt(str[12], str[13]); + fields.h0 = hexWordToInt(str[15], str[16]); + } + else if (str.size() == 12){ + mac = 0; // all zeros + fields.h5 = hexWordToInt(str[ 0], str[ 1]); + fields.h4 = hexWordToInt(str[ 2], str[ 3]); + fields.h3 = hexWordToInt(str[ 4], str[ 5]); fields.h2 = hexWordToInt(str[ 6], str[ 7]); fields.h1 = hexWordToInt(str[ 8], str[ 9]); - fields.h0 = hexWordToInt(str[10], str[11]); - } - else{ - throw Exception("invalid hex string length. must be 17 or 12 (without :)"); - } + fields.h0 = hexWordToInt(str[10], str[11]); + } + else{ + throw Exception("invalid hex string length. must be 17 or 12 (without :)"); + } } diff --git a/sensors/radio/scan/WiFiChannels.h b/sensors/radio/scan/WiFiChannels.h new file mode 100644 index 0000000..6585397 --- /dev/null +++ b/sensors/radio/scan/WiFiChannels.h @@ -0,0 +1,30 @@ +#ifndef INDOOR_WIFICHANNELS_H +#define INDOOR_WIFICHANNELS_H + +class WiFiChannels { + +public: + + static int freqToChannel(const int freq) { + switch(freq) { + case 2412: return 1; + case 2417: return 2; + case 2422: return 3; + case 2427: return 4; + case 2432: return 5; + case 2437: return 6; + case 2442: return 7; + case 2447: return 8; + case 2452: return 9; + case 2457: return 10; + case 2462: return 11; + case 2467: return 12; + case 2472: return 13; + case 2484: return 14; + } + throw "error"; + } + +}; + +#endif // INDOOR_WIFICHANNELS_H diff --git a/sensors/radio/scan/WiFiScan.h b/sensors/radio/scan/WiFiScan.h new file mode 100644 index 0000000..5f39105 --- /dev/null +++ b/sensors/radio/scan/WiFiScan.h @@ -0,0 +1,15 @@ +#ifndef INDOOR_WIFI_SCAN_H +#define INDOOR_WIFI_SCAN_H + +#include "../WiFiMeasurements.h" + +class WiFiScan { + +public: + + /** scan for nearby access points */ + virtual WiFiMeasurements scan() = 0; + +}; + +#endif //INDOOR_WIFI_SCAN_H diff --git a/sensors/radio/scan/WiFiScanLinux.h b/sensors/radio/scan/WiFiScanLinux.h new file mode 100644 index 0000000..baab78d --- /dev/null +++ b/sensors/radio/scan/WiFiScanLinux.h @@ -0,0 +1,419 @@ +#ifndef INDOOR_WIFISCANNER_LINUX_H +#define INDOOR_WIFISCANNER_LINUX_H + +/* + * scan_access_points.c: Prints all detected access points with wlan0 using NL80211 (netlink). + * + * Only works on network interfaces whose drivers are compatible with Netlink. Test this by running `iw list`. + * + * Since only privileged users may submit NL80211_CMD_TRIGGER_SCAN, you'll have to run the compiled program as root. + * + * Build with: gcc $(pkg-config --cflags --libs libnl-genl-3.0) scan_access_points.c + * + * Raspbian prerequisites: + * sudo apt-get install libnl-genl-3-dev + * + * Resources: + * http://git.kernel.org/cgit/linux/kernel/git/jberg/iw.git/tree/scan.c + * http://stackoverflow.com/questions/21601521/how-to-use-the-libnl-library-to-trigger-nl80211-commands + * http://stackoverflow.com/questions/23760780/how-to-send-single-channel-scan-request-to-libnl-and-receive-single- + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "WiFiScan.h" + +class WiFiScanLinux : public WiFiScan { + + struct trigger_results { + int done; + int aborted; + }; + + + struct handler_args { // For family_handler() and nl_get_multicast_id(). + const char *group; + int id; + }; + + + static int error_handler(struct sockaddr_nl* nla, struct nlmsgerr* err, void* arg) { + (void) nla; + // Callback for errors. + printf("error_handler() called.\n"); + int* ret = (int*) arg; + *ret = err->error; + return NL_STOP; + } + + + static int finish_handler(struct nl_msg* msg, void* arg) { + (void) msg; + // Callback for NL_CB_FINISH. + int* ret = (int*) arg; + *ret = 0; + return NL_SKIP; + } + + + static int ack_handler(struct nl_msg* msg, void* arg) { + (void) msg; + // Callback for NL_CB_ACK. + int* ret = (int*)arg; + *ret = 0; + return NL_STOP; + } + + + static int no_seq_check(struct nl_msg* msg, void* arg) { + (void) msg; + (void) arg; + // Callback for NL_CB_SEQ_CHECK. + return NL_OK; + } + + + static int family_handler(struct nl_msg* msg, void* arg) { + // Callback for NL_CB_VALID within nl_get_multicast_id(). From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html. + struct handler_args* grp = (struct handler_args*) arg; + struct nlattr *tb[CTRL_ATTR_MAX + 1]; + struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg)); + struct nlattr *mcgrp; + int rem_mcgrp; + + nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL); + + if (!tb[CTRL_ATTR_MCAST_GROUPS]) return NL_SKIP; + + nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) { // This is a loop. + struct nlattr* tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1]; + + nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX, (struct nlattr*) nla_data(mcgrp), nla_len(mcgrp), NULL); + + if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] || !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]) continue; + if (strncmp((const char*) nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]), grp->group, + nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]))) { + continue; + } + + grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]); + break; + } + + return NL_SKIP; + } + + + int nl_get_multicast_id(struct nl_sock *sock, const char *family, const char *group) { + // From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html. + struct nl_msg *msg; + struct nl_cb *cb; + int ret, ctrlid; + struct handler_args grp = { .group = group, .id = -ENOENT, }; + + msg = nlmsg_alloc(); + if (!msg) return -ENOMEM; + + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) { + ret = -ENOMEM; + goto out_fail_cb; + } + + ctrlid = genl_ctrl_resolve(sock, "nlctrl"); + + genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0); + + ret = -ENOBUFS; + NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family); + + ret = nl_send_auto_complete(sock, msg); + if (ret < 0) goto out; + + ret = 1; + + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret); + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, family_handler, &grp); + + while (ret > 0) nl_recvmsgs(sock, cb); + + if (ret == 0) ret = grp.id; + + nla_put_failure: + out: + nl_cb_put(cb); + out_fail_cb: + nlmsg_free(msg); + return ret; + } + + + static void mac_addr_n2a(char *mac_addr, unsigned char *arg) { + // From http://git.kernel.org/cgit/linux/kernel/git/jberg/iw.git/tree/util.c. + int i, l; + + l = 0; + for (i = 0; i < 6; i++) { + if (i == 0) { + sprintf(mac_addr+l, "%02x", arg[i]); + l += 2; + } else { + sprintf(mac_addr+l, ":%02x", arg[i]); + l += 3; + } + } + } + + + static void print_ssid(unsigned char *ie, int ielen) { + uint8_t len; + uint8_t *data; + int i; + + while (ielen >= 2 && ielen >= ie[1]) { + if (ie[0] == 0 && ie[1] >= 0 && ie[1] <= 32) { + len = ie[1]; + data = ie + 2; + for (i = 0; i < len; i++) { + if (isprint(data[i]) && data[i] != ' ' && data[i] != '\\') printf("%c", data[i]); + else if (data[i] == ' ' && (i != 0 && i != len -1)) printf(" "); + else printf("\\x%.2x", data[i]); + } + break; + } + ielen -= ie[1] + 2; + ie += ie[1] + 2; + } + } + + + static int callback_trigger(struct nl_msg *msg, void *arg) { + + // Called by the kernel when the scan is done or has been aborted. + struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg)); + struct trigger_results* results = (struct trigger_results*) arg; + + //printf("Got something.\n"); + //printf("%d\n", arg); + //nl_msg_dump(msg, stdout); + + if (gnlh->cmd == NL80211_CMD_SCAN_ABORTED) { + printf("Got NL80211_CMD_SCAN_ABORTED.\n"); + results->done = 1; + results->aborted = 1; + } else if (gnlh->cmd == NL80211_CMD_NEW_SCAN_RESULTS) { + printf("Got NL80211_CMD_NEW_SCAN_RESULTS.\n"); + results->done = 1; + results->aborted = 0; + } // else probably an uninteresting multicast message. + + return NL_SKIP; + } + + + static int callback_dump(struct nl_msg* msg, void* arg) { + + (void) arg; + + // Called by the kernel with a dump of the successful scan's data. Called for each SSID. + struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg)); + char mac_addr[20]; + struct nlattr *tb[NL80211_ATTR_MAX + 1]; + struct nlattr *bss[NL80211_BSS_MAX + 1]; + + // static struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = { + // [NL80211_BSS_TSF] = { .type = NLA_U64 }, + // [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 }, + // [NL80211_BSS_BSSID] = { }, + // [NL80211_BSS_BEACON_INTERVAL] = { .type = NLA_U16 }, + // [NL80211_BSS_CAPABILITY] = { .type = NLA_U16 }, + // [NL80211_BSS_INFORMATION_ELEMENTS] = { }, + // [NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 }, + // [NL80211_BSS_SIGNAL_UNSPEC] = { .type = NLA_U8 }, + // [NL80211_BSS_STATUS] = { .type = NLA_U32 }, + // [NL80211_BSS_SEEN_MS_AGO] = { .type = NLA_U32 }, + // [NL80211_BSS_BEACON_IES] = { }, + // }; + static struct nla_policy bss_policy[NL80211_BSS_MAX + 1]; + memset(&bss_policy, 0, sizeof(bss_policy)); + bss_policy[NL80211_BSS_TSF].type = NLA_U64; + bss_policy[NL80211_BSS_FREQUENCY].type = NLA_U32; + bss_policy[NL80211_BSS_BSSID];// = { }; + bss_policy[NL80211_BSS_BEACON_INTERVAL].type = NLA_U16; + bss_policy[NL80211_BSS_CAPABILITY].type = NLA_U16; + bss_policy[NL80211_BSS_INFORMATION_ELEMENTS];// = { }; + bss_policy[NL80211_BSS_SIGNAL_MBM].type = NLA_U32; + bss_policy[NL80211_BSS_SIGNAL_UNSPEC].type = NLA_U8; + bss_policy[NL80211_BSS_STATUS].type = NLA_U32; + bss_policy[NL80211_BSS_SEEN_MS_AGO].type = NLA_U32; + bss_policy[NL80211_BSS_BEACON_IES];// = { }; + + + // Parse and error check. + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL); + if (!tb[NL80211_ATTR_BSS]) { + printf("bss info missing!\n"); + return NL_SKIP; + } + if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], bss_policy)) { + printf("failed to parse nested attributes!\n"); + return NL_SKIP; + } + if (!bss[NL80211_BSS_BSSID]) return NL_SKIP; + if (!bss[NL80211_BSS_INFORMATION_ELEMENTS]) return NL_SKIP; + + const uint64_t seen_ago_ms = nla_get_u32(bss[NL80211_BSS_SEEN_MS_AGO]); + const int rssi = (nla_get_s32(bss[NL80211_BSS_SIGNAL_MBM])) / 100.0f; + + // Start printing. + mac_addr_n2a(mac_addr, (unsigned char*) nla_data(bss[NL80211_BSS_BSSID])); + printf("%s, ", mac_addr); + printf("%d MHz, ", nla_get_u32(bss[NL80211_BSS_FREQUENCY])); + print_ssid((unsigned char*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS])); + printf(" %d ms", seen_ago_ms); + printf(" %d dBm", rssi); + printf("\n"); + + return NL_SKIP; + } + + + int do_scan_trigger(struct nl_sock *socket, int if_index, int driver_id) { + // Starts the scan and waits for it to finish. Does not return until the scan is done or has been aborted. + struct trigger_results results = { .done = 0, .aborted = 0 }; + struct nl_msg *msg; + struct nl_cb *cb; + struct nl_msg *ssids_to_scan; + int err; + int ret; + int mcid = nl_get_multicast_id(socket, "nl80211", "scan"); + nl_socket_add_membership(socket, mcid); // Without this, callback_trigger() won't be called. + + // Allocate the messages and callback handler. + msg = nlmsg_alloc(); + if (!msg) { + printf("ERROR: Failed to allocate netlink message for msg.\n"); + return -ENOMEM; + } + ssids_to_scan = nlmsg_alloc(); + if (!ssids_to_scan) { + printf("ERROR: Failed to allocate netlink message for ssids_to_scan.\n"); + nlmsg_free(msg); + return -ENOMEM; + } + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) { + printf("ERROR: Failed to allocate netlink callbacks.\n"); + nlmsg_free(msg); + nlmsg_free(ssids_to_scan); + return -ENOMEM; + } + + // Setup the messages and callback handler. + genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0); // Setup which command to run. + nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. + nla_put(ssids_to_scan, 1, 0, ""); // Scan all SSIDs. + nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan); // Add message attribute, which SSIDs to scan for. + nlmsg_free(ssids_to_scan); // Copied to `msg` above, no longer need this. + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results); // Add the callback. + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); // No sequence checking for multicast messages. + + // Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on + // success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process. + err = 1; + ret = nl_send_auto(socket, msg); // Send the message. + printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret); + printf("Waiting for scan to complete...\n"); + while (err > 0) ret = nl_recvmsgs(socket, cb); // First wait for ack_handler(). This helps with basic errors. + if (err < 0) { + printf("WARNING: err has a value of %d.\n", err); + } + if (ret < 0) { + printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret)); + return ret; + } + while (!results.done) nl_recvmsgs(socket, cb); // Now wait until the scan is done or aborted. + if (results.aborted) { + printf("ERROR: Kernel aborted scan.\n"); + return 1; + } + printf("Scan is done.\n"); + + // Cleanup. + nlmsg_free(msg); + nl_cb_put(cb); + nl_socket_drop_membership(socket, mcid); // No longer need this. + return 0; + } + + + int if_index; + struct nl_sock* socket; + int driver_id; + +public: + + WiFiScanLinux(const std::string& devName) { + + if_index = if_nametoindex("wlp0s20u2u1"); // Use this wireless interface for scanning. + + + // Open socket to kernel. + socket = nl_socket_alloc(); // Allocate new netlink socket in memory. + genl_connect(socket); // Create file descriptor and bind socket. + driver_id = genl_ctrl_resolve(socket, "nl80211"); // Find the nl80211 driver ID. + + } + + /** triger WiFiScan and fetch the result */ + WiFiMeasurements scan() { + + // Issue NL80211_CMD_TRIGGER_SCAN to the kernel and wait for it to finish. + int err = do_scan_trigger(socket, if_index, driver_id); + if (err != 0) { + printf("do_scan_trigger() failed with %d.\n", err); + throw "error"; + } + + // Now get info for all SSIDs detected. + struct nl_msg *msg = nlmsg_alloc(); // Allocate a message. + genlmsg_put(msg, 0, 0, driver_id, 0, NLM_F_DUMP, NL80211_CMD_GET_SCAN, 0); // Setup which command to run. + nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. + nl_socket_modify_cb(socket, NL_CB_VALID, NL_CB_CUSTOM, callback_dump, NULL); // Add the callback. + int ret = nl_send_auto(socket, msg); // Send the message. + printf("NL80211_CMD_GET_SCAN sent %d bytes to the kernel.\n", ret); + ret = nl_recvmsgs_default(socket); // Retrieve the kernel's answer. callback_dump() prints SSIDs to stdout. + nlmsg_free(msg); + if (ret < 0) { + printf("ERROR: nl_recvmsgs_default() returned %d (%s).\n", ret, nl_geterror(-ret)); + throw "error"; + } + + // TODO + WiFiMeasurements mes; + return mes; + + } + + + +}; + + +// gcc -I /usr/include/libnl3 main.c -lnl-genl-3 -lnl-3 + + + +#endif // INDOOR_WIFISCANNER_LINUX_H diff --git a/sensors/radio/scan/WiFiScanLinux_IW.h b/sensors/radio/scan/WiFiScanLinux_IW.h new file mode 100644 index 0000000..138e4f8 --- /dev/null +++ b/sensors/radio/scan/WiFiScanLinux_IW.h @@ -0,0 +1,100 @@ +#ifndef INDOOR_WIFI_SCAN_LINUX_H +#define INDOOR_WIFI_SCAN_LINUX_H + +#include "WiFiChannels.h" +#include "WiFiScan.h" +#include "../WiFiMeasurements.h" + +#include +#include +#include +#include +#include + +class WiFiScanLinux : public WiFiScan { + +private: + + wireless_scan_head head; + wireless_scan *result; + iwrange range; + int sock; + + std::string dev; + +public: + + WiFiScanLinux(const std::string& dev) : dev(dev) { + + /* Open socket to kernel */ + sock = iw_sockets_open(); + std::cout << sock << std::endl; + + } + + WiFiMeasurements scan() override { + + WiFiMeasurements res; + char* dev = (char*) this->dev.c_str(); + + /* Get some metadata to use for scanning */ + if (iw_get_range_info(sock, dev, &range) < 0) { + printf("Error during iw_get_range_info. Aborting.\n"); + exit(2); + } + if (range.we_version_compiled < 14) { + printf("scanning not supported"); + exit(2); + } + +// // params +// struct iwreq request; +// request.u.param.flags = IW_SCAN_DEFAULT; +// request.u.param.value = 0; +// if (iw_set_ext(sock, dev, SIOCSIWSCAN, &request) == -1) { +// printf("iw_set_ext(SIOCSIWSCAN)"); +// exit(EXIT_FAILURE); +// } + + /* Perform the scan */ + if (iw_scan(sock, dev, range.we_version_compiled, &head) < 0) { + printf("Error during iw_scan. Aborting.\n"); + exit(2); + } + + /* Traverse the results */ + result = head.result; + while (NULL != result) { + + // access-point's MAC + const uint8_t* macPtr = (const uint8_t*) result->ap_addr.sa_data; + const uint64_t macLng = ((uint64_t)macPtr[5]<<40)|((uint64_t)macPtr[4]<<32)|((uint64_t)macPtr[3]<<24)|((uint64_t)macPtr[2]<<16)|((uint64_t)macPtr[1]<<8)|((uint64_t)macPtr[0]<<0); + const MACAddress mac(macLng); + + const int8_t rssi = result->stats.qual.level; + const std::string ssid = result->b.essid; + + result->b. + + const int freq = (result->b.freq)/10e5; + const int channel = WiFiChannels::freqToChannel(freq); + //std::cout << ssid << "\t" << "freq: " << freq << "\t" << "rssi: " << (int) (rssi) << " dBm" << std::endl; + //printf("%s - %d\n", result->b.essid, + result = result->next; + + std::cout << mac.asString() << "\t" << ssid << "\t" << channel << "\t" << (int)rssi << " dBm" << std::endl; + + AccessPoint ap(mac, ssid); + + WiFiMeasurement mes(ap, rssi); + res.entries.push_back(mes); + + } + + return res; + + } + +}; + +#endif //INDOOR_WIFI_SCAN_LINUX_H From da477866c1c900d8d402b9f21f60692c902865a7 Mon Sep 17 00:00:00 2001 From: frank Date: Wed, 11 Oct 2017 14:00:24 +0200 Subject: [PATCH 02/13] worked on wifi-scanner for linux new time-grouping for vap grouper adjusted test-cases minor changes/fixes/improvements --- data/Timestamp.h | 24 +- main.cpp | 72 +++- math/stats/Average.h | 4 +- math/stats/Maximum.h | 3 +- math/stats/Minimum.h | 4 +- sensors/MACAddress.h | 12 + sensors/radio/AccessPoint.h | 7 + sensors/radio/VAPGrouper.h | 87 +++-- sensors/radio/WiFiMeasurement.h | 66 ++-- sensors/radio/scan/WiFiRAW.h | 47 +++ sensors/radio/scan/WiFiScanLinux.h | 442 ++++++++++++++++--------- tests/data/TestTimestamp.cpp | 31 ++ tests/sensors/radio/TestVAPGrouper.cpp | 73 +++- 13 files changed, 649 insertions(+), 223 deletions(-) create mode 100644 sensors/radio/scan/WiFiRAW.h diff --git a/data/Timestamp.h b/data/Timestamp.h index 877bfff..90fcf4b 100644 --- a/data/Timestamp.h +++ b/data/Timestamp.h @@ -21,6 +21,7 @@ public: /** empty ctor */ explicit Timestamp() : _ms(0) {;} + /** get timestamp from the given value which represents milliesconds */ static inline Timestamp fromMS(const int64_t ms) {return Timestamp(ms);} @@ -75,11 +76,14 @@ public: Timestamp operator - (const Timestamp& o) const {return Timestamp(_ms - o._ms);} + Timestamp& operator -= (const Timestamp& o) {_ms += o._ms; return *this;} Timestamp operator + (const Timestamp& o) const {return Timestamp(_ms + o._ms);} + Timestamp& operator += (const Timestamp& o) {_ms += o._ms; return *this;} - Timestamp operator * (const float val) const {return Timestamp(_ms * val);} + template Timestamp operator * (const T val) const {return Timestamp(_ms * val);} + template Timestamp operator / (const T val) const {return Timestamp(_ms / val);} // /** cast to float */ // operator float () const {return sec();} @@ -87,4 +91,22 @@ public: }; +namespace std { + template<> class numeric_limits { + public: + static Timestamp min() { + const int64_t minVal = std::numeric_limits::min(); + return Timestamp::fromMS(minVal); + } + static Timestamp lowest() { + const int64_t minVal = std::numeric_limits::min(); + return Timestamp::fromMS(minVal); + } + static Timestamp max() { + const int64_t maxVal = std::numeric_limits::max(); + return Timestamp::fromMS(maxVal); + } + }; +} + #endif // TIMESTAMP_H diff --git a/main.cpp b/main.cpp index 492f544..e7b4eeb 100755 --- a/main.cpp +++ b/main.cpp @@ -11,10 +11,74 @@ class Test : public GridPoint { #include "tests/Tests.h" #include "sensors/radio/scan/WiFiScanLinux.h" +#include "sensors/radio/VAPGrouper.h" + +#include +#include +#include +#include + void wifi() { - const std::string dev = "wlp0s20u2u1"; + + K::Gnuplot gp; + K::GnuplotPlot plot; + std::vector lines; lines.resize(5); + std::vector points; points.resize(5); + + for (int i = 0; i < points.size(); ++i) { + plot.add(&points[i]); + plot.add(&lines[i]); + points[i].setPointSize(1); + points[i].setPointType(7); + lines[i].getStroke().setWidth(2); + } + points[0].getColor().setHexStr("#ff6666"); lines[0].getStroke().getColor().setHexStr("#ff0000"); + points[1].getColor().setHexStr("#cccc66"); lines[1].getStroke().getColor().setHexStr("#cccc00"); + points[2].getColor().setHexStr("#66cccc"); lines[2].getStroke().getColor().setHexStr("#00cccc"); + points[3].getColor().setHexStr("#6666ff"); lines[3].getStroke().getColor().setHexStr("#0000ff"); + points[4].getColor().setHexStr("#cc66cc"); lines[4].getStroke().getColor().setHexStr("#cc00cc"); + + VAPGrouper vap(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::AVERAGE, 1); + + const std::string dev = "wlp0s20u2u4"; WiFiScanLinux scanner(dev); - scanner.scan(); + + Timestamp start = Timestamp::fromUnixTime(); + + for (int i = 0; i < 500; ++i) { + + WiFiMeasurements mes = scanner.scan(); + for (const WiFiMeasurement& m : mes.entries) { + + K::GnuplotPoint2 gp((m.getTimestamp()-start).ms(), m.getRSSI()); + std::string mac = m.getAP().getMAC().asString().substr(0,14); + + if (mac == "5C:CF:7F:C3:F9") {points[0].add(gp);} + if (mac == "18:FE:34:E1:2B") {points[1].add(gp);} + if (mac == "60:01:94:03:16") {points[2].add(gp);} + + } + + WiFiMeasurements mes2 = vap.group(mes); + for (const WiFiMeasurement& m : mes2.entries) { + + K::GnuplotPoint2 gp((m.getTimestamp()-start).ms(), m.getRSSI()); + std::string mac = m.getAP().getMAC().asString().substr(0,14); + + if (mac == "5C:CF:7F:C3:F9") {lines[0].add(gp);} + if (mac == "18:FE:34:E1:2B") {lines[1].add(gp);} + if (mac == "60:01:94:03:16") {lines[2].add(gp);} + + } + + gp.draw(plot); + gp.flush(); + + } + + + + } int main(int argc, char** argv) { @@ -45,7 +109,9 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "Geo_*"; + ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; + //::testing::GTEST_FLAG(filter) = "Timestamp*"; + //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; //::testing::GTEST_FLAG(filter) = "*BVH*"; diff --git a/math/stats/Average.h b/math/stats/Average.h index 5db5149..62d294a 100644 --- a/math/stats/Average.h +++ b/math/stats/Average.h @@ -15,7 +15,7 @@ namespace Stats { public: /** ctor */ - Average() : cnt(0), sum(0) { + Average() : cnt(0), sum() { ; } @@ -33,7 +33,7 @@ namespace Stats { /** get the current value */ Scalar get() const { Assert::isNot0(cnt, "add() values first!"); - return sum / (Scalar)cnt; + return sum / cnt; } }; diff --git a/math/stats/Maximum.h b/math/stats/Maximum.h index f7235e2..533af3e 100644 --- a/math/stats/Maximum.h +++ b/math/stats/Maximum.h @@ -2,6 +2,7 @@ #define STATS_MAXIMUM_H #include "../../Assertions.h" +#include namespace Stats { @@ -9,7 +10,7 @@ namespace Stats { private: - const Scalar START = -99999999; + const Scalar START = std::numeric_limits::lowest(); Scalar curMax; public: diff --git a/math/stats/Minimum.h b/math/stats/Minimum.h index 4567586..b29e426 100644 --- a/math/stats/Minimum.h +++ b/math/stats/Minimum.h @@ -1,13 +1,15 @@ #ifndef STATS_MINIMUM_H #define STATS_MINIMUM_H +#include + namespace Stats { template class Minimum { private: - const Scalar START = +999999999; + const Scalar START = std::numeric_limits::max(); Scalar curMin; public: diff --git a/sensors/MACAddress.h b/sensors/MACAddress.h index 7443aed..35ec1bb 100644 --- a/sensors/MACAddress.h +++ b/sensors/MACAddress.h @@ -71,6 +71,18 @@ public: } + uint8_t getField(const int idx) const { + switch(idx) { + case 0: return fields.h0; + case 1: return fields.h1; + case 2: return fields.h2; + case 3: return fields.h3; + case 4: return fields.h4; + case 5: return fields.h5; + } + throw Exception("field-idx out of bounds"); + } + /** convert to lower-case hex-string ("xx:xx:xx:xx:xx:xx") */ std::string asString() const { diff --git a/sensors/radio/AccessPoint.h b/sensors/radio/AccessPoint.h index 839793c..7211f6d 100644 --- a/sensors/radio/AccessPoint.h +++ b/sensors/radio/AccessPoint.h @@ -58,6 +58,13 @@ public: /** OPTIONAL: get the AP's ssid (if any) */ inline const std::string& getSSID() const {return ssid;} + /** as string for debuging */ + std::string asString() const { + std::string res = "AP(" + mac.asString(); + if (!ssid.empty()) {res += ", '" + ssid + "'";} + res += ")"; + return res; + } }; diff --git a/sensors/radio/VAPGrouper.h b/sensors/radio/VAPGrouper.h index d12cd7a..9a24805 100644 --- a/sensors/radio/VAPGrouper.h +++ b/sensors/radio/VAPGrouper.h @@ -40,6 +40,20 @@ public: }; + /** how to determine the grouped timestamp */ + enum class TimeAggregation { + + /** use the smallest timestamp among all grouped APs */ + MINIMUM, + + /** use the average timestamp among all grouped APs */ + AVERAGE, + + /** use the maximum timestamp among all grouped APs */ + MAXIMUM, + + }; + private: static constexpr const char* name = "VAPGrp"; @@ -50,14 +64,17 @@ private: /** the signal-strength aggregation algorithm to use */ const Aggregation agg; + /** how to aggreage the grouped time */ + const TimeAggregation timeAgg; + /** respect only outputs with at-least X occurences of one physical hardware [can be used to prevent issues] */ int minOccurences; public: /** ctor */ - VAPGrouper(const Mode mode, const Aggregation agg, const int minOccurences = 2) : - mode(mode), agg(agg), minOccurences(minOccurences) { + VAPGrouper(const Mode mode, const Aggregation agg, const TimeAggregation timeAgg = TimeAggregation::AVERAGE, const int minOccurences = 2) : + mode(mode), agg(agg), timeAgg(timeAgg), minOccurences(minOccurences) { ; } @@ -127,6 +144,15 @@ public: private: + struct FieldRSSI { + static float get(const WiFiMeasurement& m) { return m.getRSSI(); } + }; + struct FieldTS { + static Timestamp get(const WiFiMeasurement& m) { return m.getTimestamp(); } + }; + + + /** combine all of the given VAPs into one entry using the configured aggregation method */ WiFiMeasurement groupVAPs(const MACAddress& baseMAC, const std::vector& vaps) const { @@ -134,17 +160,27 @@ private: const AccessPoint baseAP(baseMAC); // the resultign timestamp - const Timestamp baseTS = vaps.front().getTimestamp(); + //Timestamp baseTS = vaps.front().getTimestamp(); // calculate the rssi using the configured aggregate function float rssi = NAN; switch(agg) { - case Aggregation::AVERAGE: rssi = getAVG(vaps); break; - case Aggregation::MEDIAN: rssi = getMedian(vaps); break; - case Aggregation::MAXIMUM: rssi = getMax(vaps); break; - default: throw Exception("unsupported vap-aggregation method"); + case Aggregation::AVERAGE: rssi = getAVG(vaps); break; + case Aggregation::MEDIAN: rssi = getMedian(vaps); break; + case Aggregation::MAXIMUM: rssi = getMax(vaps); break; + default: throw Exception("unsupported rssi-aggregation method"); } + // calculate the time using the configured aggregate function + Timestamp baseTS; + switch(timeAgg) { + case TimeAggregation::MINIMUM: baseTS = getMin(vaps); break; + case TimeAggregation::AVERAGE: baseTS = getAVG(vaps); break; + case TimeAggregation::MAXIMUM: baseTS = getMax(vaps); break; + default: throw Exception("unsupported time-aggregation method"); + } + + // create the result measurement return WiFiMeasurement(baseAP, rssi, baseTS); @@ -153,13 +189,18 @@ private: private: /** get the average signal strength */ - inline float getAVG(const std::vector& vaps) const { + template inline T getAVG(const std::vector& vaps) const { - float rssi = 0; +// T field = T(); +// for (const WiFiMeasurement& vap : vaps) { +// field = field + Field::get(vap); +// } +// return field / vaps.size(); + Stats::Average avg; for (const WiFiMeasurement& vap : vaps) { - rssi += vap.getRSSI(); + avg.add(Field::get(vap)); } - return rssi / vaps.size(); + return avg.get(); } @@ -174,19 +215,25 @@ private: } - /** get the maximum signal strength */ - inline float getMax(const std::vector& vaps) const { + /** get the maximum value */ + template inline T getMax(const std::vector& vaps) const { - Stats::Maximum max; + Stats::Maximum max; for (const WiFiMeasurement& vap : vaps) { - max.add(vap.getRSSI()); + max.add(Field::get(vap)); } return max.get(); -// float max = -9999999; -// for (const WiFiMeasurement& vap : vaps) { -// if (vap.getRSSI() > max) {max = vap.getRSSI();} -// } -// return max; + + } + + /** get the minimum value */ + template inline T getMin(const std::vector& vaps) const { + + Stats::Minimum min; + for (const WiFiMeasurement& vap : vaps) { + min.add(Field::get(vap)); + } + return min.get(); } diff --git a/sensors/radio/WiFiMeasurement.h b/sensors/radio/WiFiMeasurement.h index e8d5d81..a7e39fd 100644 --- a/sensors/radio/WiFiMeasurement.h +++ b/sensors/radio/WiFiMeasurement.h @@ -19,7 +19,7 @@ private: /** the measured signal strength */ float rssi; - /** OPTIONAL. frequence the signal was received */ + /** OPTIONAL. frequence the signal was received */ float freq = NAN; /** OPTIONAL. timestamp the measurement was recorded at */ @@ -32,40 +32,56 @@ public: ; } - /** ctor with freq*/ - WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) { - ; - } + /** ctor with freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq) : ap(ap), rssi(rssi), freq(freq) { + ; + } - /** ctor with timestamp */ - WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), ts(ts) { - ; - } + /** ctor with timestamp */ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const Timestamp ts) : ap(ap), rssi(rssi), freq(NAN), ts(ts) { + ; + } - /** ctor with timestamp and freq*/ - WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { - ; - } + /** ctor with timestamp and freq*/ + WiFiMeasurement(const AccessPoint& ap, const float rssi, const float freq, const Timestamp ts) : ap(ap), rssi(rssi), freq(freq), ts(ts) { + ; + } public: - /** get the AP we got the measurement for */ - const AccessPoint& getAP() const {return ap;} + /** get the AP we got the measurement for */ + const AccessPoint& getAP() const {return ap;} - /** get the measurement's signal strength */ - float getRSSI() const {return rssi;} + /** get the measurement's signal strength */ + float getRSSI() const {return rssi;} - /** OPTIONAL: get the measurement's timestamp (if known!) */ - const Timestamp& getTimestamp() const {return ts;} + /** OPTIONAL: get the measurement's timestamp (if known!) */ + const Timestamp& getTimestamp() const {return ts;} - /** OPTIONAL: get the measurement's frequence (if known!) */ - float getFrequency() const {return freq;} + /** timestamp known? */ + bool hasTimestamp() const {return ts == ts;} - /** set another signal strength */ - void setRssi(float value){rssi = value;} + /** OPTIONAL: get the measurement's frequency (if known!) */ + float getFrequency() const {return freq;} + + /** frequency known? */ + bool hasFrequency() const {return freq == freq;} + + /** set another signal strength */ + void setRssi(float value){rssi = value;} + + /** set the timestamp */ + void setTimestamp(const Timestamp& val){ts = val;} + + /** as string for debug printing */ + std::string asString() const { + std::string res = ap.asString(); + if (hasTimestamp()) {res += " @" + std::to_string(ts.ms());} + if (hasFrequency()) {res += " " + std::to_string((int)freq) + " MHz";} + res += " - " + std::to_string(rssi) + " dBm "; + return res; + } - /** set the timestamp */ - void setTimestamp(const Timestamp& val){ts = val;} }; diff --git a/sensors/radio/scan/WiFiRAW.h b/sensors/radio/scan/WiFiRAW.h new file mode 100644 index 0000000..fd9313d --- /dev/null +++ b/sensors/radio/scan/WiFiRAW.h @@ -0,0 +1,47 @@ +#ifndef INDOOR_WIFIRAW_H +#define INDOOR_WIFIRAW_H + +#include + +/** + * parse raw binary wifi packets as defined within the standard + */ +class WiFiRAW { + +public: + + enum Tags { + TAG_SSID = 0x00 + }; + + struct TaggedParams { + std::string ssid; + }; + + /** parsed tagged params within wifi packets: [tag][len][len-bytes][tag][len][len-bytes]... */ + static TaggedParams parseTaggedParams(const uint8_t* data, const size_t len) { + + TaggedParams res; + + int pos = 0; + while(pos < len) { + + const int tag = data[pos+0]; // the tag-ID + const int len = data[pos+1]; // the lenght of the following data + + switch(tag) { + case TAG_SSID: res.ssid = std::string( (const char*) &(data[pos+2]), len); break; + } + + // position at the start of the next tag + pos += 1 + 1 + len; + + } + + return res; + + } + +}; + +#endif // INDOOR_WIFIRAW_H diff --git a/sensors/radio/scan/WiFiScanLinux.h b/sensors/radio/scan/WiFiScanLinux.h index baab78d..0a2de1d 100644 --- a/sensors/radio/scan/WiFiScanLinux.h +++ b/sensors/radio/scan/WiFiScanLinux.h @@ -28,23 +28,28 @@ #include #include #include +#include +#include "WiFiRAW.h" #include "WiFiScan.h" class WiFiScanLinux : public WiFiScan { + struct TMP { + Timestamp tsStart; + WiFiMeasurements res; + }; + struct trigger_results { int done; int aborted; }; - struct handler_args { // For family_handler() and nl_get_multicast_id(). const char *group; int id; }; - static int error_handler(struct sockaddr_nl* nla, struct nlmsgerr* err, void* arg) { (void) nla; // Callback for errors. @@ -54,7 +59,6 @@ class WiFiScanLinux : public WiFiScan { return NL_STOP; } - static int finish_handler(struct nl_msg* msg, void* arg) { (void) msg; // Callback for NL_CB_FINISH. @@ -63,7 +67,6 @@ class WiFiScanLinux : public WiFiScan { return NL_SKIP; } - static int ack_handler(struct nl_msg* msg, void* arg) { (void) msg; // Callback for NL_CB_ACK. @@ -72,7 +75,6 @@ class WiFiScanLinux : public WiFiScan { return NL_STOP; } - static int no_seq_check(struct nl_msg* msg, void* arg) { (void) msg; (void) arg; @@ -80,7 +82,6 @@ class WiFiScanLinux : public WiFiScan { return NL_OK; } - static int family_handler(struct nl_msg* msg, void* arg) { // Callback for NL_CB_VALID within nl_get_multicast_id(). From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html. struct handler_args* grp = (struct handler_args*) arg; @@ -113,11 +114,16 @@ class WiFiScanLinux : public WiFiScan { int nl_get_multicast_id(struct nl_sock *sock, const char *family, const char *group) { + // From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html. struct nl_msg *msg; struct nl_cb *cb; int ret, ctrlid; - struct handler_args grp = { .group = group, .id = -ENOENT, }; + + //struct handler_args grp = { .group = group, .id = -ENOENT, }; + struct handler_args grp; + grp.group = group; + grp.id = -ENOENT; msg = nlmsg_alloc(); if (!msg) return -ENOMEM; @@ -157,45 +163,6 @@ class WiFiScanLinux : public WiFiScan { } - static void mac_addr_n2a(char *mac_addr, unsigned char *arg) { - // From http://git.kernel.org/cgit/linux/kernel/git/jberg/iw.git/tree/util.c. - int i, l; - - l = 0; - for (i = 0; i < 6; i++) { - if (i == 0) { - sprintf(mac_addr+l, "%02x", arg[i]); - l += 2; - } else { - sprintf(mac_addr+l, ":%02x", arg[i]); - l += 3; - } - } - } - - - static void print_ssid(unsigned char *ie, int ielen) { - uint8_t len; - uint8_t *data; - int i; - - while (ielen >= 2 && ielen >= ie[1]) { - if (ie[0] == 0 && ie[1] >= 0 && ie[1] <= 32) { - len = ie[1]; - data = ie + 2; - for (i = 0; i < len; i++) { - if (isprint(data[i]) && data[i] != ' ' && data[i] != '\\') printf("%c", data[i]); - else if (data[i] == ' ' && (i != 0 && i != len -1)) printf(" "); - else printf("\\x%.2x", data[i]); - } - break; - } - ielen -= ie[1] + 2; - ie += ie[1] + 2; - } - } - - static int callback_trigger(struct nl_msg *msg, void *arg) { // Called by the kernel when the scan is done or has been aborted. @@ -220,42 +187,30 @@ class WiFiScanLinux : public WiFiScan { } - static int callback_dump(struct nl_msg* msg, void* arg) { - (void) arg; + static int addResult(struct nl_msg* msg, void* arg) { + + TMP* tmp = (TMP*) arg; // Called by the kernel with a dump of the successful scan's data. Called for each SSID. struct genlmsghdr* gnlh = (struct genlmsghdr*) nlmsg_data(nlmsg_hdr(msg)); - char mac_addr[20]; + // char mac_addr[20]; struct nlattr *tb[NL80211_ATTR_MAX + 1]; struct nlattr *bss[NL80211_BSS_MAX + 1]; - // static struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = { - // [NL80211_BSS_TSF] = { .type = NLA_U64 }, - // [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 }, - // [NL80211_BSS_BSSID] = { }, - // [NL80211_BSS_BEACON_INTERVAL] = { .type = NLA_U16 }, - // [NL80211_BSS_CAPABILITY] = { .type = NLA_U16 }, - // [NL80211_BSS_INFORMATION_ELEMENTS] = { }, - // [NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 }, - // [NL80211_BSS_SIGNAL_UNSPEC] = { .type = NLA_U8 }, - // [NL80211_BSS_STATUS] = { .type = NLA_U32 }, - // [NL80211_BSS_SEEN_MS_AGO] = { .type = NLA_U32 }, - // [NL80211_BSS_BEACON_IES] = { }, - // }; static struct nla_policy bss_policy[NL80211_BSS_MAX + 1]; memset(&bss_policy, 0, sizeof(bss_policy)); bss_policy[NL80211_BSS_TSF].type = NLA_U64; bss_policy[NL80211_BSS_FREQUENCY].type = NLA_U32; - bss_policy[NL80211_BSS_BSSID];// = { }; +// bss_policy[NL80211_BSS_BSSID] = { }; bss_policy[NL80211_BSS_BEACON_INTERVAL].type = NLA_U16; bss_policy[NL80211_BSS_CAPABILITY].type = NLA_U16; - bss_policy[NL80211_BSS_INFORMATION_ELEMENTS];// = { }; +// bss_policy[NL80211_BSS_INFORMATION_ELEMENTS] = { }; bss_policy[NL80211_BSS_SIGNAL_MBM].type = NLA_U32; bss_policy[NL80211_BSS_SIGNAL_UNSPEC].type = NLA_U8; bss_policy[NL80211_BSS_STATUS].type = NLA_U32; bss_policy[NL80211_BSS_SEEN_MS_AGO].type = NLA_U32; - bss_policy[NL80211_BSS_BEACON_IES];// = { }; +// bss_policy[NL80211_BSS_BEACON_IES] = { }; // Parse and error check. @@ -274,124 +229,212 @@ class WiFiScanLinux : public WiFiScan { const uint64_t seen_ago_ms = nla_get_u32(bss[NL80211_BSS_SEEN_MS_AGO]); const int rssi = (nla_get_s32(bss[NL80211_BSS_SIGNAL_MBM])) / 100.0f; - // Start printing. - mac_addr_n2a(mac_addr, (unsigned char*) nla_data(bss[NL80211_BSS_BSSID])); - printf("%s, ", mac_addr); - printf("%d MHz, ", nla_get_u32(bss[NL80211_BSS_FREQUENCY])); - print_ssid((unsigned char*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS])); - printf(" %d ms", seen_ago_ms); - printf(" %d dBm", rssi); - printf("\n"); +// // Start printing. +// mac_addr_n2a(mac_addr, (unsigned char*) nla_data(bss[NL80211_BSS_BSSID])); +// printf("%s, ", mac_addr); +// printf("%d MHz, ", nla_get_u32(bss[NL80211_BSS_FREQUENCY])); +// //print_ssid((unsigned char*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS])); +// printf(" %d ms", seen_ago_ms); +// printf(" %d dBm", rssi); +// printf("\n"); + + WiFiRAW::TaggedParams params = WiFiRAW::parseTaggedParams( + (const uint8_t*) nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), + nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]) + ); + + const Timestamp ts = Timestamp::fromUnixTime() - Timestamp::fromMS(seen_ago_ms); + const int freq_MHz = nla_get_u32(bss[NL80211_BSS_FREQUENCY]); + const uint8_t* macPtr = (const uint8_t*) nla_data(bss[NL80211_BSS_BSSID]); + const uint64_t macLng = ((uint64_t)macPtr[5]<<40)|((uint64_t)macPtr[4]<<32)|((uint64_t)macPtr[3]<<24)|((uint64_t)macPtr[2]<<16)|((uint64_t)macPtr[1]<<8)|((uint64_t)macPtr[0]<<0); + const MACAddress mac(macLng); + const AccessPoint ap(mac, params.ssid); + const WiFiMeasurement mes(ap, rssi, freq_MHz, ts); + + // by default, linux also lists older scan results + // remove them here! + if (ts > tmp->tsStart) { + //std::cout << seen_ago_ms << std::endl; + tmp->res.entries.push_back(mes); + std::cout << mes.asString() << std::endl; + } + + return NL_SKIP; + } - int do_scan_trigger(struct nl_sock *socket, int if_index, int driver_id) { - // Starts the scan and waits for it to finish. Does not return until the scan is done or has been aborted. - struct trigger_results results = { .done = 0, .aborted = 0 }; - struct nl_msg *msg; - struct nl_cb *cb; - struct nl_msg *ssids_to_scan; - int err; - int ret; - int mcid = nl_get_multicast_id(socket, "nl80211", "scan"); - nl_socket_add_membership(socket, mcid); // Without this, callback_trigger() won't be called. +// int do_scan_trigger(struct nl_sock *socket, int if_index, int driver_id) { - // Allocate the messages and callback handler. - msg = nlmsg_alloc(); - if (!msg) { - printf("ERROR: Failed to allocate netlink message for msg.\n"); - return -ENOMEM; - } - ssids_to_scan = nlmsg_alloc(); - if (!ssids_to_scan) { - printf("ERROR: Failed to allocate netlink message for ssids_to_scan.\n"); - nlmsg_free(msg); - return -ENOMEM; - } - cb = nl_cb_alloc(NL_CB_DEFAULT); - if (!cb) { - printf("ERROR: Failed to allocate netlink callbacks.\n"); - nlmsg_free(msg); - nlmsg_free(ssids_to_scan); - return -ENOMEM; - } +// // Starts the scan and waits for it to finish. Does not return until the scan is done or has been aborted. +// struct trigger_results results; +// results.done = 0; +// results.aborted = 0; - // Setup the messages and callback handler. - genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0); // Setup which command to run. - nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. - nla_put(ssids_to_scan, 1, 0, ""); // Scan all SSIDs. - nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan); // Add message attribute, which SSIDs to scan for. - nlmsg_free(ssids_to_scan); // Copied to `msg` above, no longer need this. - nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results); // Add the callback. - nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); - nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); - nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); - nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); // No sequence checking for multicast messages. +// struct nl_msg *msg; +// struct nl_cb *cb; +// struct nl_msg *ssids_to_scan; +// int err; +// int ret; +// int mcid = nl_get_multicast_id(socket, "nl80211", "scan"); +// nl_socket_add_membership(socket, mcid); // Without this, callback_trigger() won't be called. - // Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on - // success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process. - err = 1; - ret = nl_send_auto(socket, msg); // Send the message. - printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret); - printf("Waiting for scan to complete...\n"); - while (err > 0) ret = nl_recvmsgs(socket, cb); // First wait for ack_handler(). This helps with basic errors. - if (err < 0) { - printf("WARNING: err has a value of %d.\n", err); - } - if (ret < 0) { - printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret)); - return ret; - } - while (!results.done) nl_recvmsgs(socket, cb); // Now wait until the scan is done or aborted. - if (results.aborted) { - printf("ERROR: Kernel aborted scan.\n"); - return 1; - } - printf("Scan is done.\n"); +// // Allocate the messages and callback handler. +// msg = nlmsg_alloc(); +// if (!msg) {throw Exception("Failed to allocate netlink message for msg.");} - // Cleanup. - nlmsg_free(msg); - nl_cb_put(cb); - nl_socket_drop_membership(socket, mcid); // No longer need this. - return 0; - } +// ssids_to_scan = nlmsg_alloc(); +// if (!ssids_to_scan) { +// nlmsg_free(msg); +// throw Exception("Failed to allocate netlink message for ssids_to_scan."); +// } + +// cb = nl_cb_alloc(NL_CB_DEFAULT); +// if (!cb) { +// nlmsg_free(msg); +// nlmsg_free(ssids_to_scan); +// throw Exception("Failed to allocate netlink callbacks."); +// } + +// // Setup the messages and callback handler. +// genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0); // Setup which command to run. +// nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. +// nla_put(ssids_to_scan, 1, 0, ""); // Scan all SSIDs. +// nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan); // Add message attribute, which SSIDs to scan for. +// nlmsg_free(ssids_to_scan); // Copied to `msg` above, no longer need this. +// nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results); // Add the callback. +// nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); +// nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); +// nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); +// nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); // No sequence checking for multicast messages. + +// // Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on +// // success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process. +// err = 1; +// ret = nl_send_auto(socket, msg); // Send the message. +// printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret); +// printf("Waiting for scan to complete...\n"); +// while (err > 0) ret = nl_recvmsgs(socket, cb); // First wait for ack_handler(). This helps with basic errors. +// if (err < 0) { +// printf("WARNING: err has a value of %d.\n", err); +// } +// if (ret < 0) { +// printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret)); +// return ret; +// } +// while (!results.done) nl_recvmsgs(socket, cb); // Now wait until the scan is done or aborted. +// if (results.aborted) { +// printf("ERROR: Kernel aborted scan.\n"); +// return 1; +// } +// printf("Scan is done.\n"); + +// // Cleanup. +// nlmsg_free(msg); +// nl_cb_put(cb); +// nl_socket_drop_membership(socket, mcid); // No longer need this. +// return 0; +// } int if_index; struct nl_sock* socket; int driver_id; -public: - WiFiScanLinux(const std::string& devName) { + struct nl_cb *cb; + int mcid; - if_index = if_nametoindex("wlp0s20u2u1"); // Use this wireless interface for scanning. + int err; + struct trigger_results results; - // Open socket to kernel. - socket = nl_socket_alloc(); // Allocate new netlink socket in memory. - genl_connect(socket); // Create file descriptor and bind socket. - driver_id = genl_ctrl_resolve(socket, "nl80211"); // Find the nl80211 driver ID. + /** configure all needed callback (from netlink to code) once */ + void setupOnce() { + + mcid = nl_get_multicast_id(socket, "nl80211", "scan"); + nl_socket_add_membership(socket, mcid); // Without this, callback_trigger() won't be called. + + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) {throw Exception("Failed to allocate netlink callbacks.");} + + // Setup the messages and callback handler. + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results); // Add the callback. + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); // No sequence checking for multicast messages. } - /** triger WiFiScan and fetch the result */ - WiFiMeasurements scan() { + /** triggers a new scan within the wifi hardware */ + void triggerNewScan() { - // Issue NL80211_CMD_TRIGGER_SCAN to the kernel and wait for it to finish. - int err = do_scan_trigger(socket, if_index, driver_id); - if (err != 0) { - printf("do_scan_trigger() failed with %d.\n", err); - throw "error"; + std::cout << "triggerNewScan()" << std::endl; + + struct nl_msg *ssids_to_scan; + ssids_to_scan = nlmsg_alloc(); + if (!ssids_to_scan) {throw Exception("Failed to allocate netlink message for ssids_to_scan.");} + nla_put(ssids_to_scan, 1, 0, ""); // Scan all SSIDs. + + // construct message + struct nl_msg* msg = nlmsg_alloc(); + if (!msg) {throw Exception("Failed to allocate netlink message for msg.");} + + genlmsg_put(msg, 0, 0, driver_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0); // Setup which command to run. + nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. + nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids_to_scan); // Add message attribute, which SSIDs to scan for. + nlmsg_free(ssids_to_scan); // Copied to `msg` above, no longer need this. + + results.done = 0; + results.aborted = 0; + + // trigger scan by sending the constructed message + const int ret = nl_send_auto(socket, msg); // Send the message. + printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret); + printf("Waiting for scan to complete...\n"); + nlmsg_free(msg); + + } + + /** blocks until the scan-result is available. true if OK, false otherwise */ + bool waitForScanResult() { + + // Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on + // success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process. + err = 0; +// ret = nl_send_auto(socket, msg); // Send the message. +// printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret); +// printf("Waiting for scan to complete...\n"); +// while (err > 0) ret = nl_recvmsgs(socket, cb); // First wait for ack_handler(). This helps with basic errors. +// if (err < 0) { +// printf("WARNING: err has a value of %d.\n", err); +// } + + while(true) { + const int ret = nl_recvmsgs(socket, cb); + printf("-- ret: %d err: %d \n", ret, err); + if (results.done) { + return true; + } + if (ret < 0 || err < 0) { + nl_recvmsgs(socket, cb); // seems to fix issues when device is busy?! + printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret)); + return false; + } } + } + + void scanResult(TMP* res) { + // Now get info for all SSIDs detected. struct nl_msg *msg = nlmsg_alloc(); // Allocate a message. genlmsg_put(msg, 0, 0, driver_id, 0, NLM_F_DUMP, NL80211_CMD_GET_SCAN, 0); // Setup which command to run. nla_put_u32(msg, NL80211_ATTR_IFINDEX, if_index); // Add message attribute, which interface to use. - nl_socket_modify_cb(socket, NL_CB_VALID, NL_CB_CUSTOM, callback_dump, NULL); // Add the callback. + nl_socket_modify_cb(socket, NL_CB_VALID, NL_CB_CUSTOM, addResult, res); // Add the callback and the measurements to fill int ret = nl_send_auto(socket, msg); // Send the message. printf("NL80211_CMD_GET_SCAN sent %d bytes to the kernel.\n", ret); ret = nl_recvmsgs_default(socket); // Retrieve the kernel's answer. callback_dump() prints SSIDs to stdout. @@ -401,9 +444,94 @@ public: throw "error"; } - // TODO - WiFiMeasurements mes; - return mes; + } + + void scanCleanup() { + + // Cleanup. + //nlmsg_free(msg); + nl_cb_put(cb); + nl_socket_drop_membership(socket, mcid); // No longer need this. + //return 0; + + } + + + + + +public: + + WiFiScanLinux(const std::string& devName) { + + // convert interface-name to interface-index + if_index = if_nametoindex(devName.c_str()); + + // Open socket to kernel. + socket = nl_socket_alloc(); // Allocate new netlink socket in memory. + genl_connect(socket); // Create file descriptor and bind socket. + driver_id = genl_ctrl_resolve(socket, "nl80211"); // Find the nl80211 driver ID. + + setupOnce(); + + } + + ~WiFiScanLinux() { + + // cleanup + nl_socket_free(socket); + + } + + + + /** triger WiFiScan and fetch the result */ + WiFiMeasurements scan() { + + + TMP res; + + + // Issue NL80211_CMD_TRIGGER_SCAN to the kernel and wait for it to finish. +// while(true) { + +// // use the current timestamp to suppress older scan results +// // which are cached by linux by default +// res.tsStart = Timestamp::fromUnixTime(); + +// // trigger a scan +// //int err = do_scan_trigger(socket, if_index, driver_id); +// int err = scanTrigger(); +//// if (err == -25) {std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue;} // currently busy. try again +//// if (err != 0) {throw Exception("do_scan_trigger() failed with code: " + std::to_string(err));} +//// break; + +// } + + again:; + + triggerNewScan(); + + std::cout << "scan triggered" << std::endl; + + if (waitForScanResult()) { + + std::cout << "scan done" << std::endl; + scanResult(&res); + + // return constructed result + return res.res; + + + } else { + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + goto again; + + } + + + } diff --git a/tests/data/TestTimestamp.cpp b/tests/data/TestTimestamp.cpp index 4552b4b..d7b723a 100644 --- a/tests/data/TestTimestamp.cpp +++ b/tests/data/TestTimestamp.cpp @@ -52,4 +52,35 @@ TEST(Timestamp, add) { } +TEST(Timestamp, div) { + + Timestamp ts1 = Timestamp::fromMS(1000); + + ASSERT_EQ(100, (ts1/(size_t)10).ms()); + +} + +TEST(Timestamp, minmax) { + + Timestamp tsLow = std::numeric_limits::lowest(); + + + Timestamp tsMin = std::numeric_limits::min(); + Timestamp tsMax = std::numeric_limits::max(); + Timestamp ts0 = Timestamp::fromMS(0); + Timestamp tsNeg = Timestamp::fromMS(-99999999999999L); + Timestamp tsPos = Timestamp::fromMS(+99999999999999L); + + ASSERT_EQ(tsMin, tsLow); + + ASSERT_TRUE(tsMin < tsMax); + ASSERT_TRUE(tsMin < ts0); + ASSERT_TRUE(tsMin < tsNeg); + + ASSERT_TRUE(tsMax > tsMin); + ASSERT_TRUE(tsMax > ts0); + ASSERT_TRUE(tsMax > tsPos); + +} + #endif diff --git a/tests/sensors/radio/TestVAPGrouper.cpp b/tests/sensors/radio/TestVAPGrouper.cpp index 94a251f..77d96f0 100644 --- a/tests/sensors/radio/TestVAPGrouper.cpp +++ b/tests/sensors/radio/TestVAPGrouper.cpp @@ -38,9 +38,9 @@ TEST(WiFiVAPGrouper, baseMAC) { TEST(WiFiVAPGrouper, aggregation) { - VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE); - VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN); - VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM); + VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE, VAPGrouper::TimeAggregation::AVERAGE); + VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::MINIMUM); + VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::MAXIMUM); WiFiMeasurements scan; @@ -59,7 +59,7 @@ TEST(WiFiVAPGrouper, aggregation) { scan.entries.push_back(WiFiMeasurement(vap20, -69, Timestamp::fromMS(22))); scan.entries.push_back(WiFiMeasurement(vap21, -61, Timestamp::fromMS(25))); - scan.entries.push_back(WiFiMeasurement(vap22, -62, Timestamp::fromMS(23))); + scan.entries.push_back(WiFiMeasurement(vap22, -62, Timestamp::fromMS(21))); scan.entries.push_back(WiFiMeasurement(vap23, -60, Timestamp::fromMS(20))); const WiFiMeasurements gAvg = vgAvg.group(scan); @@ -71,26 +71,73 @@ TEST(WiFiVAPGrouper, aggregation) { ASSERT_EQ(2, gMedian.entries.size()); ASSERT_EQ(2, gMax.entries.size()); - // correct average values? + // correct average rssi / average timestamp? ASSERT_EQ(-72, gAvg.entries.back().getRSSI()); ASSERT_EQ(-63, gAvg.entries.front().getRSSI()); - ASSERT_EQ(Timestamp::fromMS(11), gAvg.entries.back().getTimestamp()); - ASSERT_EQ(Timestamp::fromMS(22), gAvg.entries.front().getTimestamp()); + ASSERT_EQ(Timestamp::fromMS(12), gAvg.entries.back().getTimestamp()); // average ts + ASSERT_EQ(Timestamp::fromMS(22), gAvg.entries.front().getTimestamp()); // average ts - // correct median values? + // correct median rssi / min timestamp? ASSERT_EQ(-71, gMedian.entries.back().getRSSI()); ASSERT_EQ(-61.5, gMedian.entries.front().getRSSI()); - ASSERT_EQ(Timestamp::fromMS(11), gMedian.entries.back().getTimestamp()); - ASSERT_EQ(Timestamp::fromMS(22), gMedian.entries.front().getTimestamp()); + ASSERT_EQ(Timestamp::fromMS(11), gMedian.entries.back().getTimestamp()); // min ts + ASSERT_EQ(Timestamp::fromMS(20), gMedian.entries.front().getTimestamp()); // min ts - // correct max values? + // correct max rssi / max timestamp? ASSERT_EQ(-70, gMax.entries.back().getRSSI()); ASSERT_EQ(-60, gMax.entries.front().getRSSI()); - ASSERT_EQ(Timestamp::fromMS(11), gMax.entries.back().getTimestamp()); - ASSERT_EQ(Timestamp::fromMS(22), gMax.entries.front().getTimestamp()); + ASSERT_EQ(Timestamp::fromMS(13), gMax.entries.back().getTimestamp()); // max ts + ASSERT_EQ(Timestamp::fromMS(25), gMax.entries.front().getTimestamp()); // max ts } +TEST(WiFiVAPGrouper, aggregationTS) { + + VAPGrouper vgAvg(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE, VAPGrouper::TimeAggregation::AVERAGE); + VAPGrouper vgMedian(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MEDIAN, VAPGrouper::TimeAggregation::MINIMUM); + VAPGrouper vgMax(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, VAPGrouper::TimeAggregation::MAXIMUM); + + WiFiMeasurements scan; + + const AccessPoint vap0("01:bb:cc:dd:11:a0"); + const AccessPoint vap1("01:bb:cc:dd:11:a1"); + const AccessPoint vap2("01:bb:cc:dd:11:a2"); + const AccessPoint vap3("01:bb:cc:dd:11:a3"); + const AccessPoint vap4("01:bb:cc:dd:11:a4"); + const AccessPoint vap5("01:bb:cc:dd:11:a5"); + const AccessPoint vap6("01:bb:cc:dd:11:a6"); + const AccessPoint vap7("01:bb:cc:dd:11:a7"); + const AccessPoint vap8("01:bb:cc:dd:11:a8"); + + Timestamp base = Timestamp::fromUnixTime(); + + scan.entries.push_back(WiFiMeasurement(vap0, -1, base+Timestamp::fromMS(1))); + scan.entries.push_back(WiFiMeasurement(vap1, -2, base+Timestamp::fromMS(2))); + scan.entries.push_back(WiFiMeasurement(vap2, -3, base+Timestamp::fromMS(3))); + scan.entries.push_back(WiFiMeasurement(vap3, -4, base+Timestamp::fromMS(4))); + scan.entries.push_back(WiFiMeasurement(vap4, -5, base+Timestamp::fromMS(5))); + scan.entries.push_back(WiFiMeasurement(vap5, -6, base+Timestamp::fromMS(6))); + scan.entries.push_back(WiFiMeasurement(vap6, -7, base+Timestamp::fromMS(7))); + scan.entries.push_back(WiFiMeasurement(vap7, -8, base+Timestamp::fromMS(8))); + scan.entries.push_back(WiFiMeasurement(vap8, -9, base+Timestamp::fromMS(9))); + + const WiFiMeasurements gAvg = vgAvg.group(scan); + const WiFiMeasurements gMedian = vgMedian.group(scan); + const WiFiMeasurements gMax = vgMax.group(scan); + + // correct average rssi / average timestamp? + ASSERT_EQ(-5, gAvg.entries.back().getRSSI()); + ASSERT_EQ(base+Timestamp::fromMS(5), gAvg.entries.front().getTimestamp()); + + // correct median rssi / min timestamp? + ASSERT_EQ(-5, gMedian.entries.back().getRSSI()); + ASSERT_EQ(base+Timestamp::fromMS(1), gMedian.entries.back().getTimestamp()); // min ts + + // correct max rssi / max timestamp? + ASSERT_EQ(-1, gMax.entries.back().getRSSI()); + ASSERT_EQ(base+Timestamp::fromMS(9), gMax.entries.back().getTimestamp()); // max ts + +} #endif From 556bbe8829f1239eaebdcc9774e454ccd54fe339 Mon Sep 17 00:00:00 2001 From: frank Date: Thu, 12 Oct 2017 10:55:47 +0200 Subject: [PATCH 03/13] added new math-stats started working on a new grid walker --- grid/walk/v3/GridWalker3.h | 122 +++++++++++++++++++++++++++++++++++++ math/stats/Variance.h | 48 +++++++++++++++ 2 files changed, 170 insertions(+) create mode 100644 grid/walk/v3/GridWalker3.h create mode 100644 math/stats/Variance.h diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h new file mode 100644 index 0000000..e42c91b --- /dev/null +++ b/grid/walk/v3/GridWalker3.h @@ -0,0 +1,122 @@ +#ifndef GRIDWALKER3_H +#define GRIDWALKER3_H + +#include "../../../data/Timestamp.h" +#include "../../Grid.h" +#include "../../../math/DrawList.h" +#include "../../../math/Distributions.h" +#include "../../../math/Stats.h" +#include "../../../geo/Heading.h" +#include "../../../math/stats/Variance.h" + +/** + * modular grid-walker that takes various sub-components to determine + * p(e) and thus randomly pick edges + */ +template class GridWalker3 { + +private: + + /** all modules to evaluate */ +// std::vector*> modules; + + RandomGenerator rnd; + +public: + + + /** one walk along several nodes */ + struct Walk : public std::vector { + + }; + +// struct Walks : std::vector { + +// }; + + + /** perform the walk based on the configured setup */ + const Node* getDestination(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + + const Node* startNode = grid.getNodePtrFor(start); + + Heading desiredHeading = ctrl.heading; + + DrawList walks; + + for (int i = 0; i < 100; ++i) { + const Walk walk = getRandomWalk(grid, startNode, dist_m); + const double prob = eval(walk, desiredHeading); + walks.add(walk, prob); + } + + Walk res = walks.get(); + const Node* dst = res.back(); + return dst; + + } + + double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { + const Node* nStart = w.front(); + const Node* nEnd = w.back(); + if (nStart == nEnd) { + std::cout << "warn! start == end" << std::endl; + return 0; + } + Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); + const float diff = head.getDiffHalfRAD(desiredHeading); + return Distribution::Normal::getProbability(0, 0.3, diff); + } + + double evalHeadingChanges(const Walk& w) const { + Stats::Variance var; + for (int i = 0; i < w.size()-2; ++i) { + const Node* n0 = w[i+0]; + const Node* n1 = w[i+1]; + const Node* n2 = w[i+2]; + Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); + Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); + const float diff = h1.getDiffHalfRAD(h2); + var.add(diff); + } + const float totalVar = var.get(); + return Distribution::Normal::getProbability(0, 0.3, totalVar); + } + + double eval(const Walk& w, const Heading desiredHeading) const { + + return 1.0 + * evalHeadingStartEnd(w, desiredHeading) + // * evalHeadingChanges(w); + ; + + } + + Walk getRandomWalk(Grid& grid, const Node* start, const float dist_m) const { + + Walk walk; + + float dist = 0; + + const Node* cur = start; + while(true) { + + walk.push_back(cur); + if (dist > dist_m) {break;} + + const int numNeighbors = cur->getNumNeighbors(); + //std::cout << "neighbors: " << numNeighbors << std::endl; + int idx = rand() % numNeighbors; + const Node* next = &grid.getNeighbor(*cur, idx); + dist += next->inMeter().getDistance(cur->inMeter()); + cur = next; + + } + + return walk; + + } + +}; + +#endif // GRIDWALKER3_H diff --git a/math/stats/Variance.h b/math/stats/Variance.h new file mode 100644 index 0000000..334d2f0 --- /dev/null +++ b/math/stats/Variance.h @@ -0,0 +1,48 @@ +#ifndef STATS_VARIANCE_H +#define STATS_VARIANCE_H + +#include "../../Assertions.h" +#include "Average.h" + +namespace Stats { + + template class Variance { + + Average avg; + Average avg2; + + public: + + /** ctor */ + Variance() { + ; + } + + /** contains valid data? */ + bool isValid() const { + return avg.isValid(); + } + + /** add a new value */ + void add(const Scalar val) { + avg.add(val); + avg2.add(val*val); + } + + /** get the current variance */ + Scalar get() const { + Assert::isTrue(avg.isValid(), "add() values first!"); + return avg2.get() - (avg.get()*avg.get()); + } + + /** get the current stadard-deviation */ + Scalar getStdDev() const { + return std::sqrt(get); + } + + + }; + +} + +#endif // STATS_VARIANCE_H From 3807c621c739241b7dc3455dce43f3484c972f35 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 17 Oct 2017 13:01:26 +0200 Subject: [PATCH 04/13] added new helper methods worked on gridWalker v3 --- geo/Circle2.h | 19 ++++ geo/Heading.h | 13 ++- grid/GridNode.h | 3 + grid/factory/v2/GridFactory.h | 6 +- grid/walk/v3/GridWalk3Helper.h | 113 +++++++++++++++++++++ grid/walk/v3/GridWalker3.h | 173 ++++++++++++++++++++++++++------- math/Interpolator.h | 5 + nav/dijkstra/Dijkstra.h | 9 +- 8 files changed, 299 insertions(+), 42 deletions(-) create mode 100644 grid/walk/v3/GridWalk3Helper.h diff --git a/geo/Circle2.h b/geo/Circle2.h index 49eccc2..e335fb8 100644 --- a/geo/Circle2.h +++ b/geo/Circle2.h @@ -5,6 +5,7 @@ #include "Point2.h" #include "Ray2.h" +#include "Line2.h" #include "../Assertions.h" @@ -65,6 +66,24 @@ public: } + /** does this circle intersect with the given ray? */ + bool intersects(const Line2 line) const { + + // https://math.stackexchange.com/questions/311921/get-location-of-vector-circle-intersection + Point2 dir = line.p2 - line.p1; + Point2 start = line.p1; + const float a = dir.x*dir.x + dir.y*dir.y; + const float b = 2 * dir.x * (start.x-center.x) + 2 * dir.y * (start.y - center.y); + const float c = (start.x-center.x) * (start.x-center.x) + (start.y - center.y)*(start.y - center.y) - radius*radius; + const float discr = b*b - 4*a*c; + + if (discr < 0) {return false;} + + const float t = (2*c) / (-b + std::sqrt(discr)); + return (t <= 1) && (t >= 0); + + } + /** configure this sphere to contain the given point-set */ void adjustToPointSet(const std::vector& lst) { diff --git a/geo/Heading.h b/geo/Heading.h index ca22b6c..a2952d4 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -77,10 +77,10 @@ public: } - Heading& operator = (const float _rad) { - rad = _rad; - return *this; - } + Heading& operator = (const float _rad) { + rad = _rad; + return *this; + } /** compare two headings */ bool operator == (const Heading other) const {return rad == other.rad;} @@ -95,6 +95,11 @@ public: float getRAD() const {return rad;} + /** convert heading into a direction-vector */ + Point2 asVector() const { + return Point2(std::cos(rad), std::sin(rad)); + } + // /** get a random heading */ // static Heading rnd() { // static std::minstd_rand gen(1234); diff --git a/grid/GridNode.h b/grid/GridNode.h index 6b21e87..306105c 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -64,6 +64,9 @@ public: /** get the node's index within its grid */ int getIdx() const {return _idx;} + /** get the x-th neighbor's index within its grid */ + int getNeighborIdx(const int x) const {return _neighbors[x];} + /** get the number of neighbors for this node */ int getNumNeighbors() const {return _numNeighbors;} diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index d79abc8..4a5809a 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -12,6 +12,7 @@ #include "Elevators.h" #include "../../../geo/Units.h" +#include "../../../geo/Circle2.h" #include "../../GridNodeBBox.h" #include "../../Grid.h" @@ -583,7 +584,10 @@ private: if (lineObstacle.getSegmentIntersection(lineNodes)) {return true;} } else if (dynamic_cast(fo)) { - throw Exception("should not happen"); + const Floorplan::FloorObstacleCircle* circle = (Floorplan::FloorObstacleCircle*) fo; + Circle2 circ(circle->center, circle->radius); + if (circ.intersects(lineNodes)) {return true;} + //throw Exception("should not happen"); } else if (dynamic_cast(fo)) { // DOORS ARE NOT AN OBSTACLE diff --git a/grid/walk/v3/GridWalk3Helper.h b/grid/walk/v3/GridWalk3Helper.h new file mode 100644 index 0000000..5b79c75 --- /dev/null +++ b/grid/walk/v3/GridWalk3Helper.h @@ -0,0 +1,113 @@ +#ifndef GRIDWALK3HELPER_H +#define GRIDWALK3HELPER_H + +#include "../../../nav/dijkstra/Dijkstra.h" +#include + + + +template class GridWalk3Helper { + +public: + + /** one walk along several nodes */ + struct Walk3 : public std::vector { + + }; + + struct Walks3 : std::vector { + + }; + + struct Nodes3 : std::vector { + + }; + + /** get all possible walks from start within a given region */ + static Walks3 getAllPossibleWalks(Grid& grid, const Node* start, const float dist_m) { + + struct Access { + Grid& grid; + Access(Grid& grid) : grid(grid) {;} + int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();} + Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);} + float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());} + } acc(grid); + + const float addDist_m = grid.getGridSize_cm() / 100.0f; + const float maxDist_m = dist_m * 1.1 + addDist_m; + Dijkstra dijkstra; + dijkstra.build(start, nullptr, acc, maxDist_m); + + const std::unordered_map*>& nodes = dijkstra.getNodes(); + + Walks3 walks; + + for (const auto& it : nodes) { + Walk3 walk; + DijkstraNode* node = it.second; + do { + const Node* gridNode = node->element; + walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted + node = node->previous; + } while (node); + walks.push_back(walk); + } + + return walks; + + } + + /** get all reachable nodes that are within a given range */ + static Nodes3 getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { + + Nodes3 res; + std::unordered_map distances; + std::vector toVisit; + + toVisit.push_back(start->getIdx()); + distances[start->getIdx()] = 0.0f; + + while (!toVisit.empty()) { + + int curIdx = toVisit.front(); + toVisit.erase(toVisit.begin()); + + const Node& curNode = grid[curIdx]; + const float curDistance = distances[curIdx]; + res.push_back(&curNode); // remember for output + + if (curDistance <= dist_m) { + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float addDist = neighbor.inMeter().getDistance(curNode.inMeter()); + const float totalDist = curDistance + addDist; + + // this is like in dijkstra. keep the smallest distance to reach a node: + + // not yet reached -> store distance + if (distances.find(neighborIdx) == distances.end()) { + toVisit.push_back(neighborIdx); + distances[neighborIdx] = totalDist; + + // reached earlier but found shorter way + } else { + if (distances[neighborIdx] > totalDist) { + distances[neighborIdx] = totalDist; + } + } + + } + } + + } + + return res; + + } + +}; + +#endif // GRIDWALK3HELPER_H diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h index e42c91b..29d025c 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/grid/walk/v3/GridWalker3.h @@ -8,12 +8,14 @@ #include "../../../math/Stats.h" #include "../../../geo/Heading.h" #include "../../../math/stats/Variance.h" +#include "GridWalk3Helper.h" +#include "../../../geo/BBox2.h" /** * modular grid-walker that takes various sub-components to determine * p(e) and thus randomly pick edges */ -template class GridWalker3 { +template class GridWalker3 { private: @@ -24,38 +26,135 @@ private: public: - - /** one walk along several nodes */ - struct Walk : public std::vector { - + struct WalkParams { + Point3 start; + float distance_m; + Heading heading = Heading(0); }; -// struct Walks : std::vector { -// }; + using Helper = GridWalk3Helper; + using Walk = typename GridWalk3Helper::Walk3; + using Walks = typename GridWalk3Helper::Walks3; + using Nodes = typename GridWalk3Helper::Nodes3; + GridPoint p3ToGp(const Point3 p) const { + const Point3 p100 = p*100; + return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); + } + + Point3 gpToP3(const GridPoint gp) const { + return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f); + } /** perform the walk based on the configured setup */ - const Node* getDestination(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + const Point3 getDestination(Grid& grid, const WalkParams& params) { + //return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m); + return _drawThenCheck(grid, params); - const Node* startNode = grid.getNodePtrFor(start); + } - Heading desiredHeading = ctrl.heading; +// /** perform the walk based on the configured setup */ +// const Point3 getDestination(Grid& grid, const GridPoint start, const params) { - DrawList walks; +// //return _getFromPossibleWalks(grid, start, ctrl, dist_m); +// //return _drawThenCheck(grid, start, ctrl, dist_m); +// throw "error"; +// } + + const bool contains(const Grid& grid, const Node* n, Point2 pt) { + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + const float d = gridSize_m / 2.0f; + const Point2 pMin = n->inMeter().xy() - Point2(d, d); + const Point2 pMax = n->inMeter().xy() + Point2(d, d); + const BBox2 bbox(pMin, pMax); + return bbox.contains(pt); + } + + const Point3 _drawThenCheck(Grid& grid, const WalkParams& params) { + + const GridPoint gpStart = p3ToGp(params.start); + const Node* startNode = grid.getNodePtrFor(gpStart); + + static Distribution::Normal dDist(1, 0.02); + static Distribution::Normal dHead(0, 0.02); + + // include one additional grid-cell (increased distance) + const float secBuffer_m = grid.getGridSize_cm() / 100.0f; + const float range_m = params.distance_m + secBuffer_m; + const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m); + + float realDist_m = params.distance_m; + Heading realHead = params.heading;// + dHead.draw(); + + int cnt = 0; + while(true) { + + const Point2 dir = realHead.asVector(); + const Point2 dst = params.start.xy() + (dir * realDist_m); + + // is dst reachable? + for (const Node* n : nodes) { + //const float distToNode = n->inMeter().xy().getDistance(dst); + //if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) { + if (contains(grid, n, dst)) { + const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); + const GridPoint gp = p3ToGp(p3); + if (grid.hasNodeFor(gp)) { + return p3; + } else { + std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl; + } + } + } + + // before trying again, modify distance and angle + if (1 == 0) { + realDist_m *= dDist.draw(); + realHead += dHead.draw(); + } + + // reached max retries? + if (++cnt > 10) {return params.start;} // did not work out.... - for (int i = 0; i < 100; ++i) { - const Walk walk = getRandomWalk(grid, startNode, dist_m); - const double prob = eval(walk, desiredHeading); - walks.add(walk, prob); } - Walk res = walks.get(); - const Node* dst = res.back(); - return dst; + throw "error"; } +// const Node* _getFromPossibleWalks(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + +// const Node* startNode = grid.getNodePtrFor(start); + +// Heading desiredHeading = ctrl.heading; + +// DrawList weightedWalks; + +// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m); +// for (const Walk& walk : walks) { +// const double prob = eval(walk, desiredHeading, dist_m); +// weightedWalks.add(walk, prob); +// } + + +// Walk res = weightedWalks.get(); +// const Node* dst = res.back(); +// return dst; + +// } + + + + + + double evalDistance(const Walk& w, const float desiredDist) const { + const Node* nStart = w.front(); + const Node* nEnd = w.back(); + const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter()); + return Distribution::Normal::getProbability(desiredDist, 0.1, walkDist); + } + double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { const Node* nStart = w.front(); const Node* nEnd = w.back(); @@ -83,39 +182,43 @@ public: return Distribution::Normal::getProbability(0, 0.3, totalVar); } - double eval(const Walk& w, const Heading desiredHeading) const { + double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const { return 1.0 * evalHeadingStartEnd(w, desiredHeading) + * evalDistance(w, desiredDistance) // * evalHeadingChanges(w); ; } - Walk getRandomWalk(Grid& grid, const Node* start, const float dist_m) const { - Walk walk; - float dist = 0; - const Node* cur = start; - while(true) { +// Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { - walk.push_back(cur); - if (dist > dist_m) {break;} +// Walk walk; - const int numNeighbors = cur->getNumNeighbors(); - //std::cout << "neighbors: " << numNeighbors << std::endl; - int idx = rand() % numNeighbors; - const Node* next = &grid.getNeighbor(*cur, idx); - dist += next->inMeter().getDistance(cur->inMeter()); - cur = next; +// float dist = 0; - } +// const Node* cur = start; +// while(true) { - return walk; +// walk.push_back(cur); +// if (dist > dist_m) {break;} - } +// const int numNeighbors = cur->getNumNeighbors(); +// //std::cout << "neighbors: " << numNeighbors << std::endl; +// int idx = rand() % numNeighbors; +// const Node* next = &grid.getNeighbor(*cur, idx); +// dist += next->inMeter().getDistance(cur->inMeter()); +// cur = next; + +// } + +// return walk; + +// } }; diff --git a/math/Interpolator.h b/math/Interpolator.h index d362949..b1f0b57 100644 --- a/math/Interpolator.h +++ b/math/Interpolator.h @@ -56,6 +56,11 @@ public: } + /** get a list of all raw entries */ + const std::vector& getEntries() const { + return entries; + } + protected: /** special interpolation for the timestamp class */ diff --git a/nav/dijkstra/Dijkstra.h b/nav/dijkstra/Dijkstra.h index 8ca5ccb..f1ffbe3 100644 --- a/nav/dijkstra/Dijkstra.h +++ b/nav/dijkstra/Dijkstra.h @@ -30,11 +30,16 @@ public: } /** get the dijkstra-pendant for the given user-node. null if none matches */ - const inline DijkstraNode* getNode(const T& userNode) const { + inline const DijkstraNode* getNode(const T& userNode) const { auto it = nodes.find(&userNode); return (unlikely(it == nodes.end())) ? (nullptr) : (it->second); } + /** get all constructed dijkstra-nodes and their original pendant */ + inline const std::unordered_map*>& getNodes() const { + return nodes; + } + /** calculate all shortest paths from ANY node to the given destination */ template void build(const T* end, const Access& acc) { build(end, nullptr, acc, NAN); @@ -80,7 +85,7 @@ public: if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;} // when a maximum weight is given, stop when current cum-dist > maxWeight - if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;} + if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached weight limit: " + std::to_string(maxWeight)); break;} // visit (and maybe update) each neighbor of the current element for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) { From 72f083d32adea6068a6e9d16e9007ed92f6f6405 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Tue, 17 Oct 2017 17:10:23 +0200 Subject: [PATCH 05/13] worked on grid-walker started adding code for synthetic sensor simulation based on given paths --- grid/walk/v3/GridWalker3.h | 31 +++++++--- sensors/imu/StepDetection.h | 51 ++++++++++++----- synthetic/SyntheticPath.h | 109 ++++++++++++++++++++++++++++++++++++ synthetic/SyntheticSteps.h | 67 ++++++++++++++++++++++ synthetic/SyntheticTurns.h | 4 ++ synthetic/SyntheticWalker.h | 70 +++++++++++++++++++++++ 6 files changed, 311 insertions(+), 21 deletions(-) create mode 100644 synthetic/SyntheticPath.h create mode 100644 synthetic/SyntheticSteps.h create mode 100644 synthetic/SyntheticTurns.h create mode 100644 synthetic/SyntheticWalker.h diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h index 29d025c..ec5d73b 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/grid/walk/v3/GridWalker3.h @@ -26,12 +26,19 @@ private: public: + /** paremters for the walk */ struct WalkParams { Point3 start; float distance_m; Heading heading = Heading(0); }; + /** result of the random walk */ + struct WalkResult { + Point3 position; + Heading heading = Heading(0); + }; + using Helper = GridWalk3Helper; using Walk = typename GridWalk3Helper::Walk3; @@ -48,7 +55,7 @@ public: } /** perform the walk based on the configured setup */ - const Point3 getDestination(Grid& grid, const WalkParams& params) { + const WalkResult getDestination(Grid& grid, const WalkParams& params) { //return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m); return _drawThenCheck(grid, params); @@ -62,6 +69,7 @@ public: // throw "error"; // } + /** does the given grid-node contain the provided point-in-question? */ const bool contains(const Grid& grid, const Node* n, Point2 pt) { const float gridSize_m = grid.getGridSize_cm() / 100.0f; const float d = gridSize_m / 2.0f; @@ -71,7 +79,7 @@ public: return bbox.contains(pt); } - const Point3 _drawThenCheck(Grid& grid, const WalkParams& params) { + const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { const GridPoint gpStart = p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); @@ -84,13 +92,15 @@ public: const float range_m = params.distance_m + secBuffer_m; const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m); + WalkResult res; + res.heading = params.heading; + res.position = params.start; float realDist_m = params.distance_m; - Heading realHead = params.heading;// + dHead.draw(); int cnt = 0; while(true) { - const Point2 dir = realHead.asVector(); + const Point2 dir = res.heading.asVector(); const Point2 dst = params.start.xy() + (dir * realDist_m); // is dst reachable? @@ -101,7 +111,9 @@ public: const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); const GridPoint gp = p3ToGp(p3); if (grid.hasNodeFor(gp)) { - return p3; + res.position = p3; // new position + res.heading; // keep as-is + return res; } else { std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl; } @@ -111,11 +123,16 @@ public: // before trying again, modify distance and angle if (1 == 0) { realDist_m *= dDist.draw(); - realHead += dHead.draw(); + res.heading += dHead.draw(); } // reached max retries? - if (++cnt > 10) {return params.start;} // did not work out.... + if (++cnt > 10) { + WalkResult res; + res.position = params.start; + res.heading = params.heading; + return res; + } // did not work out.... } diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 53fe215..b7eed18 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -38,11 +38,31 @@ private: const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs) const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing! + + #ifdef WITH_DEBUG_PLOT + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines lineDet; + K::GnuplotPlotElementLines lineX; + K::GnuplotPlotElementLines lineY; + K::GnuplotPlotElementLines lineZ; + Timestamp plotRef; + int plotCnt = 0; + #endif + + public: /** ctor */ StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { - ; + + #ifdef WITH_DEBUG_PLOT + plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); + plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); + plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); + plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000"); + #endif + } /** does the given data indicate a step? */ @@ -73,22 +93,25 @@ public: step = true; } -// static K::Gnuplot gp; -// static K::GnuplotPlot plot; -// static K::GnuplotPlotElementLines lines1; plot.add(&lines1); -// static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff"); -// static Timestamp ref = ts; -// static int i = 0; + #ifdef WITH_DEBUG_PLOT -// //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); -// lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) ); + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); -// if (++i % 100 == 0) { -// gp.draw(plot); -// gp.flush(); -// usleep(1000*25); -// } + //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); + lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) ); + lineY.add( K::GnuplotPoint2(tsPlot.ms(), acc.y) ); + lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) ); + lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) ); + + if (++plotCnt % 25 == 0) { + gp.draw(plot); + gp.flush(); + //usleep(1000*25); + } + + #endif return step; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h new file mode 100644 index 0000000..fd50715 --- /dev/null +++ b/synthetic/SyntheticPath.h @@ -0,0 +1,109 @@ +#ifndef INDOOR_SYNTHETICPATH_H +#define INDOOR_SYNTHETICPATH_H + +#include "../math/Interpolator.h" +#include "../floorplan/v2/Floorplan.h" +#include "../floorplan/v2/FloorplanHelper.h" + +/** allows interpolation along a synthetic path */ +class SyntheticPath : private Interpolator { + + using Base = Interpolator; + using Entry = Base::InterpolatorEntry; + +public: + + /** create path using the given ground-truth points from the map */ + void create(const Floorplan::IndoorMap* map, std::vector ids) { + + // get all ground-truth points from the map + auto gtps = FloorplanHelper::getGroundTruthPoints(map); + float dist = 0; + + // create distance-based entries within the interpolator + for (int i = 0; i < ids.size(); ++i) { + const int id = ids[i]; + Point3 gtp = gtps[id]; + if (i >= 1) { + Point3 gtpPrev = gtps[id-1]; + dist += gtpPrev.getDistance(gtp); + } + Base::add(dist, gtp); + } + + } + + /** get all individual entries from the underlying data-structure */ + const std::vector& getEntries() const { + return Base::getEntries(); + } + + /** smooth harsh angles */ + void smooth(float delta = 1, int numRuns = 1) { + + float t = delta*2; + + for (int j = 0; j < numRuns; ++j) { + + t/=2; + + for (int i = 1; i < (int)entries.size()-1; ++i) { + + // the entry to-be-replaced by two others + const Entry& e = entries[i]; + const float key = e.key; + + const Entry e1(key-t, Base::get(key-t)); + const Entry e2(key+t, Base::get(key+t)); + + entries.erase(entries.begin()+i); --i; + ++i; entries.insert(entries.begin()+i, e1); + ++i; entries.insert(entries.begin()+i, e2); + + } + + } + + } + +// /** smooth harsh angles */ +// void smooth(float delta = 1, int numRuns = 1) { + +// float t = delta/numRuns; + +// for (int i = 1; i < (int)entries.size()-1; ++i) { + +// // the entry to-be-replaced by several others +// const Entry& e = entries[i]; +// const float key = e.key; + +// std::vector newEntries; +// for (int x = -numRuns; x <= +numRuns; x+= 2) { +// const float percent = (float)x / (float)numRuns; +// const float keyO = key + percent*t; +// const Point3 pos1 = Base::get(keyO-t*2); +// const Point3 pos2 = Base::get(keyO+t*2); +// const Point3 posO = (pos1+pos2) / 2; +// Entry e(keyO, posO); +// newEntries.push_back(e); +// } + +// entries.erase(entries.begin()+i); --i; + +// for (const Entry& e : newEntries) { +// ++i; +// entries.insert(entries.begin()+i, e); +// } + +// } + +// } + + /** get the position along the path after the given walking distance */ + Point3 getPosAfterDistance(const float distance) const { + return Base::get(distance); + } + +}; + +#endif // INDOOR_SYNTEHTICPATH_H diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h new file mode 100644 index 0000000..ddeae70 --- /dev/null +++ b/synthetic/SyntheticSteps.h @@ -0,0 +1,67 @@ +#ifndef SYNTHETICSTEPS_H +#define SYNTHETICSTEPS_H + +#include "../sensors/imu/AccelerometerData.h" +#include "SyntheticWalker.h" +#include "../math/distribution/Normal.h" + +/** fakes accelerometer-data based on synthetic walking data */ +class SyntheticSteps : SyntheticWalker::Listener { + +public: + + class Listener { + public: + virtual void onSyntheticStepData(const Timestamp ts, const AccelerometerData acc) = 0; + }; + +private: + + /** the walker to listen to */ + SyntheticWalker* walker; + + /** the pedestrian's step-size (in meter) */ + float stepSize_m = 0.7; + + float lastStepAtDistance = 0; + + Distribution::Normal dX = Distribution::Normal(0, 1); + Distribution::Normal dY = Distribution::Normal(0, 1); + Distribution::Normal dZ = Distribution::Normal(0, 1); + + std::vector listeners; + +public: + + /** ctor with the walker to follow */ + SyntheticSteps(SyntheticWalker* walker) { + walker->addListener(this); + } + + void addListener(Listener* l) { + this->listeners.push_back(l); + } + +protected: + + + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + + const float nextStepAt = (lastStepAtDistance + stepSize_m); + + const float x = dX.draw(); + const float y = dY.draw(); + const float z = dZ.draw(); + AccelerometerData acc(x, y, z); + + if (walkedDistance > nextStepAt) { + lastStepAtDistance = walkedDistance; + } + + for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);} + + } + +}; + +#endif // SYNTHETICSTEPS_H diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h new file mode 100644 index 0000000..8faa777 --- /dev/null +++ b/synthetic/SyntheticTurns.h @@ -0,0 +1,4 @@ +#ifndef SYNTHETICTURNS_H +#define SYNTHETICTURNS_H + +#endif // SYNTHETICTURNS_H diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h new file mode 100644 index 0000000..a641871 --- /dev/null +++ b/synthetic/SyntheticWalker.h @@ -0,0 +1,70 @@ +#ifndef SYNTHETICWALKER_H +#define SYNTHETICWALKER_H + +#include "SyntheticPath.h" + +/** walk along a path using a known walking speed */ +class SyntheticWalker { + +public: + + class Listener { + public: + virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0; + }; + +private: + + /** the path to walk along */ + SyntheticPath path; + + /** walking-speed in meter per sec */ + float walkSpeed_meterPerSec = 1.2; + + /** adjusted while walking */ + float walkedDistance = 0; + + /** adjusted while walking */ + Timestamp walkedTime; + + /** the listener to inform */ + std::vector listeners; + + const char* name = "SynWalker"; + +public: + + /** ctor */ + SyntheticWalker(SyntheticPath path) : path(path) { + ; + } + + /** attach a new listener */ + void addListener(Listener* l) { + this->listeners.push_back(l); + } + + /** increment the walk */ + void tick(const Timestamp timePassed) { + + // update time + this->walkedTime += timePassed; + + // update the walked distance using the walking speed + this->walkedDistance += walkSpeed_meterPerSec * timePassed.sec(); + + // get the current position along the path + const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); + + Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() ); + + // inform listener + for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);} + + } + + + +}; + +#endif // SYNTHETICWALKER_H From 3e31f6da5324768673769b179b8ab996368ccac3 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 18 Oct 2017 16:54:57 +0200 Subject: [PATCH 06/13] worked on synthetic sensors worked on grid-walker minor changes/fixes/improvements --- grid/walk/v3/GridWalker3.h | 61 +++++--- sensors/imu/AccelerometerData.h | 8 ++ sensors/imu/StepDetection.h | 73 +++++++--- sensors/imu/TurnDetection.h | 243 ++++++++++++++++++++++++++++++-- sensors/offline/FilePlayer.h | 12 ++ synthetic/SyntheticSteps.h | 63 ++++++++- synthetic/SyntheticTurns.h | 129 ++++++++++++++++- synthetic/SyntheticWalker.h | 4 +- 8 files changed, 522 insertions(+), 71 deletions(-) diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h index ec5d73b..ec59386 100644 --- a/grid/walk/v3/GridWalker3.h +++ b/grid/walk/v3/GridWalker3.h @@ -37,6 +37,7 @@ public: struct WalkResult { Point3 position; Heading heading = Heading(0); + double probability = 1.0; }; @@ -79,18 +80,26 @@ public: return bbox.contains(pt); } + /** does one of the given grid-node scontains the provided point-in-question? */ + const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { + for (const Node* n : nodes) { + if (contains(grid, n, pt)) {return n;} + } + return nullptr; + } + const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { const GridPoint gpStart = p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); static Distribution::Normal dDist(1, 0.02); - static Distribution::Normal dHead(0, 0.02); + static Distribution::Normal dHead(0, 0.01); // include one additional grid-cell (increased distance) const float secBuffer_m = grid.getGridSize_cm() / 100.0f; const float range_m = params.distance_m + secBuffer_m; - const Nodes nodes = Helper::getAllReachableNodes(grid, startNode, range_m); + const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); WalkResult res; res.heading = params.heading; @@ -104,33 +113,39 @@ public: const Point2 dst = params.start.xy() + (dir * realDist_m); // is dst reachable? - for (const Node* n : nodes) { - //const float distToNode = n->inMeter().xy().getDistance(dst); - //if (distToNode < grid.getGridSize_cm() / 2 / 100.0f) { - if (contains(grid, n, dst)) { - const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); - const GridPoint gp = p3ToGp(p3); - if (grid.hasNodeFor(gp)) { - res.position = p3; // new position - res.heading; // keep as-is - return res; - } else { - std::cout << "failed: " << p3.asString() << ":" << gp.asString() << std::endl; - } + const Node* n = contains(grid, reachableNodes, dst); + if (n) { + + const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); + const GridPoint gp = p3ToGp(p3); + + if (grid.hasNodeFor(gp)) { + res.position = p3; // update position + res.heading; // keep as-is + res.probability; // keep as-is + return res; // done + } else { + std::cout << "WARN dst not found" << std::endl; + //throw "should not happen"; } + } + // reduce probability with every new run + ++cnt; + res.probability /= 2; + // before trying again, modify distance and angle - if (1 == 0) { - realDist_m *= dDist.draw(); - res.heading += dHead.draw(); - } + //if (1 == 1) { + res.heading = params.heading + dHead.draw() * cnt; + realDist_m = params.distance_m * dDist.draw();// * cnt; + //} // reached max retries? - if (++cnt > 10) { - WalkResult res; - res.position = params.start; - res.heading = params.heading; + if (cnt > 8) { + res.position = params.start; // reset + res.heading = params.heading; // reset + res.probability = 1e-50; // unlikely return res; } // did not work out.... diff --git a/sensors/imu/AccelerometerData.h b/sensors/imu/AccelerometerData.h index 4a1566f..55e69d4 100644 --- a/sensors/imu/AccelerometerData.h +++ b/sensors/imu/AccelerometerData.h @@ -42,6 +42,14 @@ struct AccelerometerData { return AccelerometerData(x/val, y/val, z/val); } + AccelerometerData operator * (const float val) const { + return AccelerometerData(x*val, y*val, z*val); + } + + AccelerometerData operator + (const AccelerometerData o) const { + return AccelerometerData(x+o.x, y+o.y, z+o.z); + } + std::string asString() const { std::stringstream ss; ss << "(" << x << "," << y << "," << z << ")"; diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index b7eed18..68edfbb 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -8,12 +8,14 @@ #include #include -#include -#include -#include -#include -#include - +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include +#endif #include "../../Assertions.h" #include "../../math/MovingAverageTS.h" @@ -43,11 +45,12 @@ private: K::Gnuplot gp; K::GnuplotPlot plot; K::GnuplotPlotElementLines lineDet; + K::GnuplotPlotElementPoints pointDet; K::GnuplotPlotElementLines lineX; K::GnuplotPlotElementLines lineY; K::GnuplotPlotElementLines lineZ; Timestamp plotRef; - int plotCnt = 0; + Timestamp lastPlot; #endif @@ -57,10 +60,13 @@ public: StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { #ifdef WITH_DEBUG_PLOT - plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); - plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); - plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); + gp << "set autoscale xfix\n"; + plot.setTitle("Step Detection"); + plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX"); + plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY"); + plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ"); plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000"); + plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7); #endif } @@ -78,19 +84,25 @@ public: bool step = false; - if (blockUntil > ts) {return false;} + if (blockUntil > ts) { - // wait for a rising edge - if (waitForUp && delta > upperThreshold) { - blockUntil = ts + blockTime; // block some time - waitForUp = false; - } + step = false; + + } else { + + // wait for a rising edge + if (waitForUp && delta > upperThreshold) { + blockUntil = ts + blockTime; // block some time + waitForUp = false; + } + + // wait for a falling edge + if (!waitForUp && delta < lowerThreshold) { + blockUntil = ts + blockTime; // block some time + waitForUp = true; + step = true; + } - // wait for a falling edge - if (!waitForUp && delta < lowerThreshold) { - blockUntil = ts + blockTime; // block some time - waitForUp = true; - step = true; } @@ -98,6 +110,7 @@ public: if (plotRef.isZero()) {plotRef = ts;} const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) ); lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) ); @@ -105,10 +118,24 @@ public: lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) ); lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) ); - if (++plotCnt % 25 == 0) { + if (step) { + pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) ); + } + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + + lastPlot = tsPlot; + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineX.removeIf(remove); + lineY.removeIf(remove); + lineZ.removeIf(remove); + lineDet.removeIf(remove); + pointDet.removeIf(remove); + gp.draw(plot); gp.flush(); - //usleep(1000*25); + usleep(100); + } #endif diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 67473ff..2ab0f6d 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.h @@ -5,18 +5,23 @@ #include "AccelerometerData.h" #include "../../data/Timestamp.h" #include "../../math/MovingAverageTS.h" +#include "../../geo/Point3.h" #include #include #include -#include -#include -#include -#include -#include - +#ifdef WITH_DEBUG_PLOT + #include + #include + #include + #include + #include + #include + #include + #include +#endif #include "../../Assertions.h" @@ -24,6 +29,37 @@ class TurnDetection { private: + #ifdef WITH_DEBUG_PLOT + Timestamp plotRef; + Timestamp lastPlot; + + K::Gnuplot gp1; + K::Gnuplot gp2; + + K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3); + + K::GnuplotPlot plotGyroRaw; + K::GnuplotPlotElementLines lineGyroRawX; + K::GnuplotPlotElementLines lineGyroRawY; + K::GnuplotPlotElementLines lineGyroRawZ; + + K::GnuplotPlot plotGyroFix; + K::GnuplotPlotElementLines lineGyroFixX; + K::GnuplotPlotElementLines lineGyroFixY; + K::GnuplotPlotElementLines lineGyroFixZ; + + K::GnuplotPlot plotAcc; + K::GnuplotPlotElementLines lineAccX; + K::GnuplotPlotElementLines lineAccY; + K::GnuplotPlotElementLines lineAccZ; + + K::GnuplotSplot plotPose; + K::GnuplotSplotElementLines linePose; + + float plotCurHead = 0; + + #endif + //std::vector gyroData; Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); @@ -44,7 +80,40 @@ public: /** ctor */ TurnDetection() { - ; + + #ifdef WITH_DEBUG_PLOT + + gp1 << "set autoscale xfix\n"; + gp1 << "set view equal xyz\n"; + + multiplot.add(&plotGyroRaw); + multiplot.add(&plotGyroFix); + multiplot.add(&plotAcc); + + plotGyroRaw.setTitle("Gyroscope (raw)"); + plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX"); + plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY"); + plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ"); + + plotGyroFix.setTitle("Gyroscope (fixed)"); + plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX"); + plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY"); + plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ"); + + plotAcc.setTitle("Accelerometer"); + plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX"); + plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY"); + plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ"); + + plotPose.setTitle("Pose"); + plotPose.getView().setEnabled(false); + plotPose.add(&linePose); + plotPose.getAxisX().setRange(-5,+5); + plotPose.getAxisY().setRange(-5,+5); + plotPose.getAxisZ().setRange(-5,+5); + + #endif + } @@ -90,7 +159,6 @@ public: // otherwise we would use a wrong rotation matrix which yields wrong results! if (!orientation.isKnown) {return 0.0f;} - // get the current gyro-reading as vector Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z; @@ -117,6 +185,77 @@ public: // rotation = z-axis only! const float delta = area(2); + + #ifdef WITH_DEBUG_PLOT + + plotCurHead += delta; + + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); + + // raw gyro + lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) ); + lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) ); + lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) ); + + // adjusted gyro + lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(0)) ); + lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) ); + lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) ); + + // adjusted gyro + lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) ); + lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) ); + lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) ); + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + + lastPlot = tsPlot; + + // plot 3D pose + std::vector pose = { + Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1), + Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1), + }; + linePose.clear(); + for (const Point3 p : pose) { + Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z; + Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1; + K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2)); + linePose.add(gp3); + } + + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineGyroRawX.removeIf(remove); + lineGyroRawY.removeIf(remove); + lineGyroRawZ.removeIf(remove); + lineGyroFixX.removeIf(remove); + lineGyroFixY.removeIf(remove); + lineGyroFixZ.removeIf(remove); + lineAccX.removeIf(remove); + lineAccY.removeIf(remove); + lineAccZ.removeIf(remove); + + const float ax = 0.85 + std::cos(plotCurHead)*0.1; + const float ay = 0.85 + std::sin(plotCurHead)*0.1; + gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; + gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; + + + gp1.draw(multiplot); + gp1.flush(); + + gp2.draw(plotPose); + gp2.flush(); + + //usleep(100); + + } + + #endif + + // done return delta; @@ -128,17 +267,31 @@ public: // add accelerometer data //pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z)); - est.addAcc(acc); + est.addAcc(ts, acc); - // start with the first available timestamp - if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;} + if (1 == 0) { + + // FASTER + + // start with the first available timestamp + if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;} + + // if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding + if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) { + orientation.rotationMatrix = est.get(); + orientation.isKnown = true; + orientation.lastEstimation = ts; + est.reset(); + } + + } else { + + // MORE ACCURATE - // if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding - if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) { orientation.rotationMatrix = est.get(); orientation.isKnown = true; orientation.lastEstimation = ts; - est.reset(); + } } @@ -207,7 +360,7 @@ public: } /** add the given accelerometer reading */ - void addAcc(const AccelerometerData& acc) { + void addAcc(const Timestamp ts, const AccelerometerData& acc) { // did NOT improve the result for every smartphone (only some) //const float deltaMag = std::abs(acc.magnitude() - 9.81); @@ -218,6 +371,11 @@ public: sum += vec; ++cnt; + + } + + AccelerometerData getAvg() const { + return AccelerometerData(sum(0), sum(1), sum(2)) / cnt; } /** get the current rotation matrix estimation */ @@ -245,9 +403,64 @@ public: } + }; + + + struct XYZ2 { + + // average the accelerometer + MovingAverageTS avg = MovingAverageTS(Timestamp::fromMS(1250), AccelerometerData()); + + XYZ2() { + + // start approximately + addAcc(Timestamp(), AccelerometerData(0,0,9.81)); + + } + + /** add the given accelerometer reading */ + void addAcc(const Timestamp ts, const AccelerometerData& acc) { + + // did NOT improve the result for every smartphone (only some) + //const float deltaMag = std::abs(acc.magnitude() - 9.81); + //if (deltaMag > 5.0) {return;} + + avg.add(ts, acc); + + } + + AccelerometerData getAvg() const { + return avg.get(); + } + + /** get the current rotation matrix estimation */ + Eigen::Matrix3f get() const { + + // get the current acceleromter average + AccelerometerData avgAcc = getAvg(); + const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); + + // rotate average-accelerometer into (0,0,1) + Eigen::Vector3f zAxis; zAxis << 0, 0, 1; + const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + + // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) + Eigen::Vector3f aligned = (rotMat * avg).normalized(); + Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + + return rotMat; + + } + + /** reset the current sum etc. */ + void reset() { + ; + } + } est; + // struct RotationMatrixEstimationUsingAccAngle { // Eigen::Vector3f lastAvg; diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h index 7d5c6e2..dcb88ba 100644 --- a/sensors/offline/FilePlayer.h +++ b/sensors/offline/FilePlayer.h @@ -83,6 +83,10 @@ namespace Offline { // get all sensor events from the offline file const std::vector events = reader->getEntries(); + // reference time (system vs. first-event) + Timestamp tsRef1 = Timestamp::fromMS(events.front().ts); + Timestamp tsRef2 = Timestamp::fromRunningTime(); + // process every event for (const Entry& e : events) { @@ -92,6 +96,14 @@ namespace Offline { // timestamp const Timestamp ts = Timestamp::fromMS(e.ts); + // ensure events happen occur fast as they did during recording + if (realtime) { + const Timestamp ts1 = ts-tsRef1; + const Timestamp ts2 = Timestamp::fromRunningTime() - tsRef2; + const Timestamp diff = ts1-ts2; + if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));} + } + // event index const size_t idx = e.idx; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index ddeae70..849907f 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -1,11 +1,16 @@ #ifndef SYNTHETICSTEPS_H #define SYNTHETICSTEPS_H -#include "../sensors/imu/AccelerometerData.h" #include "SyntheticWalker.h" + +#include "../sensors/imu/AccelerometerData.h" + #include "../math/distribution/Normal.h" -/** fakes accelerometer-data based on synthetic walking data */ +/** + * fakes accelerometer-data + * based on synthetic walking data + */ class SyntheticSteps : SyntheticWalker::Listener { public: @@ -25,9 +30,14 @@ private: float lastStepAtDistance = 0; - Distribution::Normal dX = Distribution::Normal(0, 1); - Distribution::Normal dY = Distribution::Normal(0, 1); - Distribution::Normal dZ = Distribution::Normal(0, 1); + Timestamp refStepPattern; + Interpolator stepPattern; + + Distribution::Normal dX = Distribution::Normal(0, 0.2); + Distribution::Normal dY = Distribution::Normal(0, 0.3); + Distribution::Normal dZ = Distribution::Normal(0, 0.4); + + int stepPatternPos = -1; std::vector listeners; @@ -35,9 +45,33 @@ public: /** ctor with the walker to follow */ SyntheticSteps(SyntheticWalker* walker) { + walker->addListener(this); + dX.setSeed(1); + dY.setSeed(3); + dZ.setSeed(5); + + // build the step-pattern (how does a step look-like on the accelerometer) + // TODO: switch to MS?! use interpolator? +// int duration_ms = 350; +// for (int i = 0; i < duration_ms; i += 10) { +// float z = std::sin(i*M_PI*2/duration_ms) * 3.0; +// if (z < 0) {z *= 0.75;} // less pronounced in the negative part +// float y = std::cos(i*M_PI*2/duration_ms) * 0.5; +// const float x = dO.draw(); +// z += dO.draw()*2; +// y += dO.draw(); +// AccelerometerData acc(x,y,z); +// stepPattern.add(Timestamp::fromMS(i), acc); +// } + stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); + stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3)); + stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); + stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); + } + /** attach a listener to this provider */ void addListener(Listener* l) { this->listeners.push_back(l); } @@ -47,15 +81,32 @@ protected: void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + (void) curPos; const float nextStepAt = (lastStepAtDistance + stepSize_m); + // 1st, start with random noise on the accelerometer const float x = dX.draw(); const float y = dY.draw(); const float z = dZ.draw(); - AccelerometerData acc(x, y, z); + const AccelerometerData base(0, 4, 9.7); + const AccelerometerData noise(x, y, z); + AccelerometerData acc = base + noise; + // is it time to inject a "step" into the accelerometer data? if (walkedDistance > nextStepAt) { lastStepAtDistance = walkedDistance; + refStepPattern = walkedTime; + } + + // overlay the noise with a step-pattern (see ctor) + if (refStepPattern.ms() > 0) { + Timestamp curPatPos = walkedTime - refStepPattern; + if (curPatPos >= stepPattern.getMaxKey()) { + refStepPattern = Timestamp::fromMS(0); + } else { + const AccelerometerData step = stepPattern.get(curPatPos); + acc = base + noise*2.5f + step; + } } for (Listener* l : listeners) {l->onSyntheticStepData(walkedTime, acc);} diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index 8faa777..bec5bf7 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -1,4 +1,127 @@ -#ifndef SYNTHETICTURNS_H -#define SYNTHETICTURNS_H +#ifndef INDOOR_SYNTHETICTURNS_H +#define INDOOR_SYNTHETICTURNS_H -#endif // SYNTHETICTURNS_H +#include "SyntheticWalker.h" + +#include "../sensors/imu/AccelerometerData.h" +#include "../sensors/imu/GyroscopeData.h" +#include "../geo/Heading.h" + +#include "../math/distribution/Normal.h" + +/** + * simulates acceleromter and gyroscope data + * based on synthetic walking data + * + * @brief The SyntheticTurns class + */ +class SyntheticTurns : public SyntheticWalker::Listener { + +public: + + class Listener { + public: + virtual void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) = 0; + }; + +private: + + /** the walker to listen to */ + SyntheticWalker* walker; + + Distribution::Normal dAccX = Distribution::Normal(0, 2.5); + Distribution::Normal dAccY = Distribution::Normal(0, 1.5); + Distribution::Normal dAccZ = Distribution::Normal(0, 1); + + Distribution::Normal dGyroX = Distribution::Normal(0, 0.02); + Distribution::Normal dGyroY = Distribution::Normal(0, 0.02); + Distribution::Normal dGyroZ = Distribution::Normal(0, 0.02); + + Distribution::Normal dMaxChange = Distribution::Normal(0.011, 0.003); + Distribution::Normal dChange = Distribution::Normal(1.0, 0.25); + Distribution::Normal dHeadErr = Distribution::Normal(0.15, 0.10); // heading error, slightly biased + + + std::vector listeners; + +public: + + /** ctor with the walker to follow */ + SyntheticTurns(SyntheticWalker* walker) { + walker->addListener(this); + dAccX.setSeed(1); + dAccY.setSeed(3); + dAccZ.setSeed(5); + dGyroX.setSeed(7); + dGyroY.setSeed(9); + dGyroZ.setSeed(11); + } + + /** attach a listener to this provider */ + void addListener(Listener* l) { + this->listeners.push_back(l); + } + +protected: + + Timestamp lastTs; + Heading desiredHead = Heading(0); + Heading curHead = Heading(0); + Point3 lastPos = Point3(NAN, NAN, NAN); + float change; + + inline float clamp(const float val, const float min, const float max) { + if (val < min) {return min;} + if (val > max) {return max;} + return val; + } + + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + + // time sine last onWalk(); + if (lastTs.isZero()) {lastTs = walkedTime; return;} + const Timestamp deltaTs = walkedTime - lastTs; + lastTs = walkedTime; + + if (lastPos.x != lastPos.x) { + lastPos = curPos; + } else { + desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();; + lastPos = curPos; + } + + // difference between current-heading and desired-heading + const float diffRad = Heading::getSignedDiff(curHead, desiredHead); + + // slowly change the current heading to match the desired one + const float maxChange = dMaxChange.draw(); + const float toChange = clamp(diffRad, -maxChange, +maxChange); + //if (change < toChange) {change += toChange*0.01;} + if (change > toChange) {change *= 0.93;} + if (change < toChange) {change += dChange.draw()/10000;} + //if (change > toChange) {change -= dChange.draw();} + + + + curHead += change; + + // convert to gyro's radians-per-second + const float radPerSec = change * 1000 / deltaTs.ms();; + + const float accX = 0.00 + dAccX.draw(); + const float accY = 0.00 + dAccY.draw(); + const float accZ = 9.81 + dAccZ.draw(); + AccelerometerData acc(accX, accY, accZ); + + const float gyroX = dGyroX.draw(); + const float gyroY = dGyroY.draw(); + const float gyroZ = dGyroZ.draw() + radPerSec; + GyroscopeData gyro(gyroX, gyroY, gyroZ); + + for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);} + + } + +}; + +#endif // INDOOR_SYNTHETICTURNS_H diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index a641871..2bb5c1e 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -45,7 +45,7 @@ public: } /** increment the walk */ - void tick(const Timestamp timePassed) { + Point3 tick(const Timestamp timePassed) { // update time this->walkedTime += timePassed; @@ -61,6 +61,8 @@ public: // inform listener for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);} + return curPosOnPath; + } From 0151a4f6e40254f4a28f2a24dbbaff4ebba1c0b7 Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 24 Oct 2017 12:44:16 +0200 Subject: [PATCH 07/13] rebase activity rec --- .../{pressure => activity}/ActivityButterPressure.h | 12 ++++++------ .../ActivityButterPressurePercent.h | 3 ++- 2 files changed, 8 insertions(+), 7 deletions(-) rename sensors/{pressure => activity}/ActivityButterPressure.h (94%) rename sensors/{pressure => activity}/ActivityButterPressurePercent.h (99%) diff --git a/sensors/pressure/ActivityButterPressure.h b/sensors/activity/ActivityButterPressure.h similarity index 94% rename from sensors/pressure/ActivityButterPressure.h rename to sensors/activity/ActivityButterPressure.h index be53031..5c9adbf 100644 --- a/sensors/pressure/ActivityButterPressure.h +++ b/sensors/activity/ActivityButterPressure.h @@ -6,7 +6,9 @@ #include "../../math/FixedFrequencyInterpolator.h" #include "../../math/MovingAVG.h" -#include "BarometerData.h" +#include "../pressure/BarometerData.h" + +#include "Activity.h" /** * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering @@ -16,8 +18,6 @@ class ActivityButterPressure { public: - enum Activity {DOWN, STAY, UP}; - struct History { Timestamp ts; BarometerData data; @@ -112,13 +112,13 @@ public: } if(actValue > threshold){ - currentActivity = DOWN; + currentActivity = Activity::WALKING_DOWN; } else if (actValue < -threshold){ - currentActivity = UP; + currentActivity = Activity::WALKING_UP; } else{ - currentActivity = STAY; + currentActivity = Activity::WALKING; } } } diff --git a/sensors/pressure/ActivityButterPressurePercent.h b/sensors/activity/ActivityButterPressurePercent.h similarity index 99% rename from sensors/pressure/ActivityButterPressurePercent.h rename to sensors/activity/ActivityButterPressurePercent.h index c20583e..80541fc 100644 --- a/sensors/pressure/ActivityButterPressurePercent.h +++ b/sensors/activity/ActivityButterPressurePercent.h @@ -8,8 +8,9 @@ #include #include +#include "../pressure/BarometerData.h" -#include "BarometerData.h" +#include "Activity.h" /** * receives pressure measurements, interpolates them to a ficex frequency, lowpass filtering From 81b6b2c96cf2d8de12ff79e6b6fe8d0bf15b7e51 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Tue, 24 Oct 2017 16:48:56 +0200 Subject: [PATCH 08/13] worked on grid-walker v3 --- geo/Heading.h | 5 + grid/walk/v3/GridWalk3Helper.h | 113 ---------- grid/walk/v3/GridWalker3.h | 257 ---------------------- grid/walk/v3/Helper.h | 133 ++++++++++++ grid/walk/v3/Structs.h | 43 ++++ grid/walk/v3/WalkEvaluator.h | 91 ++++++++ grid/walk/v3/WalkGenerator.h | 20 ++ grid/walk/v3/Walker.h | 376 +++++++++++++++++++++++++++++++++ 8 files changed, 668 insertions(+), 370 deletions(-) delete mode 100644 grid/walk/v3/GridWalk3Helper.h delete mode 100644 grid/walk/v3/GridWalker3.h create mode 100644 grid/walk/v3/Helper.h create mode 100644 grid/walk/v3/Structs.h create mode 100644 grid/walk/v3/WalkEvaluator.h create mode 100644 grid/walk/v3/WalkGenerator.h create mode 100644 grid/walk/v3/Walker.h diff --git a/geo/Heading.h b/geo/Heading.h index a2952d4..8381f10 100644 --- a/geo/Heading.h +++ b/geo/Heading.h @@ -27,6 +27,11 @@ public: Assert::isBetween(rad, 0.0f, (float)_2PI, "radians out of bounds"); } + /** ctor from(x,y) -> to(x,y) */ + Heading(const Point2 from, const Point2 to) : Heading(from.x, from.y, to.x, to.y) { + ; + } + /** POSITIVE angular difference [0:PI] */ float getDiffHalfRAD(const Heading other) const { return Angle::getDiffRAD_2PI_PI(rad, other.rad); diff --git a/grid/walk/v3/GridWalk3Helper.h b/grid/walk/v3/GridWalk3Helper.h deleted file mode 100644 index 5b79c75..0000000 --- a/grid/walk/v3/GridWalk3Helper.h +++ /dev/null @@ -1,113 +0,0 @@ -#ifndef GRIDWALK3HELPER_H -#define GRIDWALK3HELPER_H - -#include "../../../nav/dijkstra/Dijkstra.h" -#include - - - -template class GridWalk3Helper { - -public: - - /** one walk along several nodes */ - struct Walk3 : public std::vector { - - }; - - struct Walks3 : std::vector { - - }; - - struct Nodes3 : std::vector { - - }; - - /** get all possible walks from start within a given region */ - static Walks3 getAllPossibleWalks(Grid& grid, const Node* start, const float dist_m) { - - struct Access { - Grid& grid; - Access(Grid& grid) : grid(grid) {;} - int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();} - Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);} - float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());} - } acc(grid); - - const float addDist_m = grid.getGridSize_cm() / 100.0f; - const float maxDist_m = dist_m * 1.1 + addDist_m; - Dijkstra dijkstra; - dijkstra.build(start, nullptr, acc, maxDist_m); - - const std::unordered_map*>& nodes = dijkstra.getNodes(); - - Walks3 walks; - - for (const auto& it : nodes) { - Walk3 walk; - DijkstraNode* node = it.second; - do { - const Node* gridNode = node->element; - walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted - node = node->previous; - } while (node); - walks.push_back(walk); - } - - return walks; - - } - - /** get all reachable nodes that are within a given range */ - static Nodes3 getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { - - Nodes3 res; - std::unordered_map distances; - std::vector toVisit; - - toVisit.push_back(start->getIdx()); - distances[start->getIdx()] = 0.0f; - - while (!toVisit.empty()) { - - int curIdx = toVisit.front(); - toVisit.erase(toVisit.begin()); - - const Node& curNode = grid[curIdx]; - const float curDistance = distances[curIdx]; - res.push_back(&curNode); // remember for output - - if (curDistance <= dist_m) { - for (int i = 0; i < curNode.getNumNeighbors(); ++i) { - - const int neighborIdx = curNode.getNeighborIdx(i); - const Node& neighbor = grid[neighborIdx]; - const float addDist = neighbor.inMeter().getDistance(curNode.inMeter()); - const float totalDist = curDistance + addDist; - - // this is like in dijkstra. keep the smallest distance to reach a node: - - // not yet reached -> store distance - if (distances.find(neighborIdx) == distances.end()) { - toVisit.push_back(neighborIdx); - distances[neighborIdx] = totalDist; - - // reached earlier but found shorter way - } else { - if (distances[neighborIdx] > totalDist) { - distances[neighborIdx] = totalDist; - } - } - - } - } - - } - - return res; - - } - -}; - -#endif // GRIDWALK3HELPER_H diff --git a/grid/walk/v3/GridWalker3.h b/grid/walk/v3/GridWalker3.h deleted file mode 100644 index ec59386..0000000 --- a/grid/walk/v3/GridWalker3.h +++ /dev/null @@ -1,257 +0,0 @@ -#ifndef GRIDWALKER3_H -#define GRIDWALKER3_H - -#include "../../../data/Timestamp.h" -#include "../../Grid.h" -#include "../../../math/DrawList.h" -#include "../../../math/Distributions.h" -#include "../../../math/Stats.h" -#include "../../../geo/Heading.h" -#include "../../../math/stats/Variance.h" -#include "GridWalk3Helper.h" -#include "../../../geo/BBox2.h" - -/** - * modular grid-walker that takes various sub-components to determine - * p(e) and thus randomly pick edges - */ -template class GridWalker3 { - -private: - - /** all modules to evaluate */ -// std::vector*> modules; - - RandomGenerator rnd; - -public: - - /** paremters for the walk */ - struct WalkParams { - Point3 start; - float distance_m; - Heading heading = Heading(0); - }; - - /** result of the random walk */ - struct WalkResult { - Point3 position; - Heading heading = Heading(0); - double probability = 1.0; - }; - - - using Helper = GridWalk3Helper; - using Walk = typename GridWalk3Helper::Walk3; - using Walks = typename GridWalk3Helper::Walks3; - using Nodes = typename GridWalk3Helper::Nodes3; - - GridPoint p3ToGp(const Point3 p) const { - const Point3 p100 = p*100; - return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); - } - - Point3 gpToP3(const GridPoint gp) const { - return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f); - } - - /** perform the walk based on the configured setup */ - const WalkResult getDestination(Grid& grid, const WalkParams& params) { - //return getDestination(grid, GridPoint(start.x*100, start.y*100, start.z*100), ctrl, dist_m); - return _drawThenCheck(grid, params); - - } - -// /** perform the walk based on the configured setup */ -// const Point3 getDestination(Grid& grid, const GridPoint start, const params) { - -// //return _getFromPossibleWalks(grid, start, ctrl, dist_m); -// //return _drawThenCheck(grid, start, ctrl, dist_m); -// throw "error"; -// } - - /** does the given grid-node contain the provided point-in-question? */ - const bool contains(const Grid& grid, const Node* n, Point2 pt) { - const float gridSize_m = grid.getGridSize_cm() / 100.0f; - const float d = gridSize_m / 2.0f; - const Point2 pMin = n->inMeter().xy() - Point2(d, d); - const Point2 pMax = n->inMeter().xy() + Point2(d, d); - const BBox2 bbox(pMin, pMax); - return bbox.contains(pt); - } - - /** does one of the given grid-node scontains the provided point-in-question? */ - const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { - for (const Node* n : nodes) { - if (contains(grid, n, pt)) {return n;} - } - return nullptr; - } - - const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { - - const GridPoint gpStart = p3ToGp(params.start); - const Node* startNode = grid.getNodePtrFor(gpStart); - - static Distribution::Normal dDist(1, 0.02); - static Distribution::Normal dHead(0, 0.01); - - // include one additional grid-cell (increased distance) - const float secBuffer_m = grid.getGridSize_cm() / 100.0f; - const float range_m = params.distance_m + secBuffer_m; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); - - WalkResult res; - res.heading = params.heading; - res.position = params.start; - float realDist_m = params.distance_m; - - int cnt = 0; - while(true) { - - const Point2 dir = res.heading.asVector(); - const Point2 dst = params.start.xy() + (dir * realDist_m); - - // is dst reachable? - const Node* n = contains(grid, reachableNodes, dst); - if (n) { - - const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); - const GridPoint gp = p3ToGp(p3); - - if (grid.hasNodeFor(gp)) { - res.position = p3; // update position - res.heading; // keep as-is - res.probability; // keep as-is - return res; // done - } else { - std::cout << "WARN dst not found" << std::endl; - //throw "should not happen"; - } - - } - - // reduce probability with every new run - ++cnt; - res.probability /= 2; - - // before trying again, modify distance and angle - //if (1 == 1) { - res.heading = params.heading + dHead.draw() * cnt; - realDist_m = params.distance_m * dDist.draw();// * cnt; - //} - - // reached max retries? - if (cnt > 8) { - res.position = params.start; // reset - res.heading = params.heading; // reset - res.probability = 1e-50; // unlikely - return res; - } // did not work out.... - - } - - throw "error"; - - } - -// const Node* _getFromPossibleWalks(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { - -// const Node* startNode = grid.getNodePtrFor(start); - -// Heading desiredHeading = ctrl.heading; - -// DrawList weightedWalks; - -// const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m); -// for (const Walk& walk : walks) { -// const double prob = eval(walk, desiredHeading, dist_m); -// weightedWalks.add(walk, prob); -// } - - -// Walk res = weightedWalks.get(); -// const Node* dst = res.back(); -// return dst; - -// } - - - - - - double evalDistance(const Walk& w, const float desiredDist) const { - const Node* nStart = w.front(); - const Node* nEnd = w.back(); - const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter()); - return Distribution::Normal::getProbability(desiredDist, 0.1, walkDist); - } - - double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { - const Node* nStart = w.front(); - const Node* nEnd = w.back(); - if (nStart == nEnd) { - std::cout << "warn! start == end" << std::endl; - return 0; - } - Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); - const float diff = head.getDiffHalfRAD(desiredHeading); - return Distribution::Normal::getProbability(0, 0.3, diff); - } - - double evalHeadingChanges(const Walk& w) const { - Stats::Variance var; - for (int i = 0; i < w.size()-2; ++i) { - const Node* n0 = w[i+0]; - const Node* n1 = w[i+1]; - const Node* n2 = w[i+2]; - Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); - Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); - const float diff = h1.getDiffHalfRAD(h2); - var.add(diff); - } - const float totalVar = var.get(); - return Distribution::Normal::getProbability(0, 0.3, totalVar); - } - - double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const { - - return 1.0 - * evalHeadingStartEnd(w, desiredHeading) - * evalDistance(w, desiredDistance) - // * evalHeadingChanges(w); - ; - - } - - - - -// Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { - -// Walk walk; - -// float dist = 0; - -// const Node* cur = start; -// while(true) { - -// walk.push_back(cur); -// if (dist > dist_m) {break;} - -// const int numNeighbors = cur->getNumNeighbors(); -// //std::cout << "neighbors: " << numNeighbors << std::endl; -// int idx = rand() % numNeighbors; -// const Node* next = &grid.getNeighbor(*cur, idx); -// dist += next->inMeter().getDistance(cur->inMeter()); -// cur = next; - -// } - -// return walk; - -// } - -}; - -#endif // GRIDWALKER3_H diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h new file mode 100644 index 0000000..5c2ca68 --- /dev/null +++ b/grid/walk/v3/Helper.h @@ -0,0 +1,133 @@ +#ifndef INDOOR_GW3_HELPER_H +#define INDOOR_GW3_HELPER_H + +#include "../../../nav/dijkstra/Dijkstra.h" +#include "../../Grid.h" + +#include "Structs.h" + +#include + +namespace GW3 { + + template class Helper { + + public: + + static GridPoint p3ToGp(const Point3 p) { + const Point3 p100 = p*100; + return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); + } + + static Point3 gpToP3(const GridPoint gp) { + return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f); + } + + /** does the given grid-node contain the provided point-in-question? */ + static bool contains(const Grid& grid, const Node* n, Point2 pt) { + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + const float d = gridSize_m / 2.0f; + const Point2 pMin = n->inMeter().xy() - Point2(d, d); + const Point2 pMax = n->inMeter().xy() + Point2(d, d); + const BBox2 bbox(pMin, pMax); + return bbox.contains(pt); + } + + /** does one of the given grid-nodes contains the provided point-in-question? */ + static const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { + for (const Node* n : nodes) { + if (contains(grid, n, pt)) {return n;} + } + return nullptr; + } + + /** get all possible walks from start within a given region */ + static Walks getAllPossibleWalks(Grid& grid, const Node* start, const float dist_m) { + + struct Access { + Grid& grid; + Access(Grid& grid) : grid(grid) {;} + int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();} + Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);} + float getWeightBetween(const Node& n1, const Node& n2) const {return n1.inMeter().getDistance(n2.inMeter());} + } acc(grid); + + const float addDist_m = grid.getGridSize_cm() / 100.0f; + const float maxDist_m = dist_m * 1.1 + addDist_m; + Dijkstra dijkstra; + dijkstra.build(start, nullptr, acc, maxDist_m); + + const std::unordered_map*>& nodes = dijkstra.getNodes(); + + Walks walks; + + for (const auto& it : nodes) { + Walk walk; + DijkstraNode* node = it.second; + do { + const Node* gridNode = node->element; + walk.insert(walk.begin(), gridNode); // push_front() as dijkstra is inverted + node = node->previous; + } while (node); + walks.push_back(walk); + } + + return walks; + + } + + /** get all reachable nodes that are within a given range */ + static Nodes getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { + + Nodes res; + std::unordered_map distances; + std::vector toVisit; + + toVisit.push_back(start->getIdx()); + distances[start->getIdx()] = 0.0f; + + while (!toVisit.empty()) { + + int curIdx = toVisit.front(); + toVisit.erase(toVisit.begin()); + + const Node& curNode = grid[curIdx]; + const float curDistance = distances[curIdx]; + res.push_back(&curNode); // remember for output + + if (curDistance <= dist_m) { + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float addDist = neighbor.inMeter().getDistance(curNode.inMeter()); + const float totalDist = curDistance + addDist; + + // this is like in dijkstra. keep the smallest distance to reach a node: + + // not yet reached -> store distance + if (distances.find(neighborIdx) == distances.end()) { + toVisit.push_back(neighborIdx); + distances[neighborIdx] = totalDist; + + // reached earlier but found shorter way + } else { + if (distances[neighborIdx] > totalDist) { + distances[neighborIdx] = totalDist; + } + } + + } + } + + } + + return res; + + } + + }; + +} + +#endif // INDOOR_GW3_HELPER_H diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h new file mode 100644 index 0000000..335f0e3 --- /dev/null +++ b/grid/walk/v3/Structs.h @@ -0,0 +1,43 @@ +#ifndef INDOOR_GW3_STRUCTS_H +#define INDOOR_GW3_STRUCTS_H + +#include "../../../geo/Heading.h" +#include "../../../geo/Point3.h" +#include + +namespace GW3 { + + /** paremters for the walk */ + struct WalkParams { + Point3 start; + float distance_m; + Heading heading = Heading(0); + }; + + /** result of the random walk */ + struct WalkResult { + Point3 position; + Heading heading = Heading(0); + double probability = 1.0; + }; + + /** several nodes */ + template struct Nodes : public std::vector { + + }; + + /** one walk along several nodes */ + template struct Walk : public std::vector { + + }; + + /** several walks */ + template struct Walks : public std::vector> { + + }; + + + +} + +#endif // INDOOR_GW3_STRUCTS_H diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h new file mode 100644 index 0000000..c740e72 --- /dev/null +++ b/grid/walk/v3/WalkEvaluator.h @@ -0,0 +1,91 @@ +#ifndef INDOOR_GW3_WALKEVALUATOR_H +#define INDOOR_GW3_WALKEVALUATOR_H + +#include "Structs.h" +#include "Helper.h" + +#include "../../../math/Distributions.h" +#include "../../../grid/Grid.h" + +namespace GW3 { + + /** interface for all evaluators that return a probability for a given walk */ + template class WalkEvaluator { + + public: + + /** get the probability for the given walk */ + //virtual double getProbability(const Walk& walk) const = 0; + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0; + + }; + + + /** evaluate the grid-node-importance importance(end) */ + template class WalkEvalEndNodeProbability : public WalkEvaluator { + + Grid* grid; + + public: + + WalkEvalEndNodeProbability(Grid* grid) : grid(grid) {;} + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + (void) params; + (void) pStart; + + const GridPoint gp = Helper::p3ToGp(pEnd); + const Node& node = grid->getNodeFor(gp); + const double p = node.getWalkImportance(); + return std::pow(p,10); + + } + + }; + + + + /** evaluate the difference between head(start,end) and the requested heading */ + template class WalkEvalHeadingStartEnd : public WalkEvaluator { + + public: + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + (void) params; + + if (pStart == pEnd) { + std::cout << "warn! start-position == end-positon" << std::endl; + return 0; + } + + const Heading head(pStart.xy(), pEnd.xy()); + const float diff = head.getDiffHalfRAD(params.heading); + //const float diff = Heading::getSignedDiff(params.heading, head); + return Distribution::Normal::getProbability(0, 0.04, diff); + + } + + }; + + /** evaluate the difference between distance(start, end) and the requested distance */ + template class WalkEvalDistance : public WalkEvaluator { + + const double sigma = 0.1f; + + public: + + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + + const float walkedDistance_m = pStart.getDistance(pEnd); + return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); + + } + + }; + +} + +#endif // INDOOR_GW3_WALKEVALUATOR_H diff --git a/grid/walk/v3/WalkGenerator.h b/grid/walk/v3/WalkGenerator.h new file mode 100644 index 0000000..28f9cc1 --- /dev/null +++ b/grid/walk/v3/WalkGenerator.h @@ -0,0 +1,20 @@ +#ifndef INDOOR_GRIDWALKER3GENERATOR_H +#define INDOOR_GRIDWALKER3GENERATOR_H + +#include "Structs.h" + + +namespace GW3 { + + class GridWalker3Generator { + + virtual Walk getPossibleWalks() = 0; + + }; + + + + +} + +#endif // INDOOR_GRIDWALKER3GENERATOR_H diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h new file mode 100644 index 0000000..894b631 --- /dev/null +++ b/grid/walk/v3/Walker.h @@ -0,0 +1,376 @@ +#ifndef INDOOR_GW3_WALKER_H +#define INDOOR_GW3_WALKER_H + +#include "../../../data/Timestamp.h" +#include "../../../math/DrawList.h" +#include "../../../math/Distributions.h" +#include "../../../math/Stats.h" +#include "../../../geo/Heading.h" +#include "../../../math/stats/Variance.h" +#include "../../../geo/BBox2.h" + +#include "../../Grid.h" + +#include "Helper.h" +#include "Structs.h" +#include "WalkEvaluator.h" + +namespace GW3 { + + /** + * modular grid-walker that takes various sub-components to determine + * p(e) and thus randomly pick edges + */ + template class WalkerBase { + + public: + + /** get a new destination for the given start */ + virtual const WalkResult getDestination(Grid& grid, const WalkParams& params) const = 0; + + }; + + template class WalkerDirectDestination : public WalkerBase { + + //RandomGenerator rnd; + + std::vector*> evals; + + public: + + /** make the code a little more readable */ + using Helper = GW3::Helper; + using Walk = typename GW3::Walk; + using Walks = typename GW3::Walks; + using Nodes = typename GW3::Nodes; + + /** add a new evaluator to the walker */ + void addEvaluator(WalkEvaluator* eval) { + this->evals.push_back(eval); + } + + /** perform the walk based on the configured setup */ + const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + + static std::mt19937 rndGen; + + const GridPoint gpStart = Helper::p3ToGp(params.start); + const Node* startNode = grid.getNodePtrFor(gpStart); + + // include one additional grid-cell (increased distance) + const float secBuffer_m = grid.getGridSize_cm() / 100.0f; + const float range_m = params.distance_m + secBuffer_m; + const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); + + WalkResult res; + res.heading = params.heading; + res.position = params.start; + float realDist_m = params.distance_m; + + + const Point2 dir = res.heading.asVector(); + const Point2 dst = params.start.xy() + (dir * realDist_m); + + // is dst reachable? + const Node* n = Helper::contains(grid, reachableNodes, dst); + if (n) { + + const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); + const GridPoint gp = Helper::p3ToGp(p3); + + if (grid.hasNodeFor(gp)) { + res.position = p3; // update position + res.heading; // keep as-is + res.probability; // keep as-is + return res; // done + } else { + std::cout << "WARN dst not found" << std::endl; + //throw "should not happen"; + } + + } + + // not found -> try random pick among all reachable nodes + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); + + const Node* dstNode = reachableNodes[dNode(rndGen)]; + + // random position within destination-node + std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); + const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + + const Point3 start = params.start; + const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (start == end) { + res.probability = 0; + return res; + } + + res.heading = Heading(start.xy(), end.xy()); + res.probability = p; + res.position = end; + return res; + + } + + }; + + + /** + * 1) get all reachable nodes from "start" (within a certain distance) + * 2) randomly pick one of em + * 3) scatter positon within the node's square -> end position + * 4) evaluate the probability of walking from start -> end + */ + template class WalkerWeightedRandom : public WalkerBase { + + std::vector*> evals; + + public: + + /** make the code a little more readable */ + using Helper = GW3::Helper; + using Walk = typename GW3::Walk; + using Walks = typename GW3::Walks; + using Nodes = typename GW3::Nodes; + + /** add a new evaluator to the walker */ + void addEvaluator(WalkEvaluator* eval) { + this->evals.push_back(eval); + } + + /** perform the walk based on the configured setup */ + const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + + static std::minstd_rand rndGen; + + const GridPoint gpStart = Helper::p3ToGp(params.start); + const Node* startNode = grid.getNodePtrFor(gpStart); + + // include one additional grid-cell (increased distance) + const float secBuffer_m = grid.getGridSize_cm() / 100.0f; + const float range_m = params.distance_m + secBuffer_m; + const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); + + // not found -> try random pick among all reachable nodes + const float gridSize_m = grid.getGridSize_cm() / 100.0f; + std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); + + + Point3 best; + double bestP = 0; + + const Point3 start = params.start; + + // try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles") + for (int i = 0; i < 25; ++i) { + + const Node* dstNode = reachableNodes[dNode(rndGen)]; + + // random position within destination-node + std::uniform_real_distribution dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f); + const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + + // destination = node-center + offset (within the node's bbox) + const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + + // sanity check + Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + + if (start == end) {continue;} + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = end;} + + } + + //const Point3 end = lst.get(); + const Point3 end = best; + WalkResult res; + if (start == end) { + res.probability = 0; + } else { + res.heading = Heading(start.xy(), end.xy()); + res.probability = bestP; // 1 + } + res.position = end; + return res; + + } + + }; + + + + + //const WalkResult _drawThenCheck(Grid& grid, const WalkParams& params) { + + + + + // Distribution::Normal dDist(1, 0.02); + // Distribution::Normal dHead(0, 0.01); + +// int cnt = 0; +// while(true) { + +// const Point2 dir = res.heading.asVector(); +// const Point2 dst = params.start.xy() + (dir * realDist_m); + +// // is dst reachable? +// const Node* n = Helper::contains(grid, reachableNodes, dst); +// if (n) { + +// const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); +// const GridPoint gp = Helper::p3ToGp(p3); + +// if (grid.hasNodeFor(gp)) { +// res.position = p3; // update position +// res.heading; // keep as-is +// res.probability; // keep as-is +// return res; // done +// } else { +// std::cout << "WARN dst not found" << std::endl; +// //throw "should not happen"; +// } + +// } + +// // reduce probability with every new run +// ++cnt; +// res.probability /= 2; + +// // before trying again, modify distance and angle +// //if (1 == 1) { +// res.heading = params.heading + dHead.draw() * cnt; +// realDist_m = params.distance_m * dDist.draw();// * cnt; +// //} + +// // reached max retries? +// if (cnt > 8) { +// res.position = params.start; // reset +// res.heading = params.heading; // reset +// res.probability = 1e-50; // unlikely +// return res; +// } // did not work out.... + +// } + + // throw "error"; + + //} + + + // const Node* _getFromPossibleWalks(Grid& grid, const GridPoint start, Control& ctrl, const float dist_m) { + + // const Node* startNode = grid.getNodePtrFor(start); + + // Heading desiredHeading = ctrl.heading; + + // DrawList weightedWalks; + + // const Walks walks = Helper::getAllPossibleWalks(grid, startNode, dist_m); + // for (const Walk& walk : walks) { + // const double prob = eval(walk, desiredHeading, dist_m); + // weightedWalks.add(walk, prob); + // } + + + // Walk res = weightedWalks.get(); + // const Node* dst = res.back(); + // return dst; + + // } + + + + + +// double evalDistance(const Walk& w, const float desiredDist) const { +// const Node* nStart = w.front(); +// const Node* nEnd = w.back(); +// const float walkDist = nStart->inMeter().getDistance(nEnd->inMeter()); +// return Distribution::Normal::getProbability(desiredDist, 0.1, walkDist); +// } + +// double evalHeadingStartEnd(const Walk& w, const Heading desiredHeading) const { +// const Node* nStart = w.front(); +// const Node* nEnd = w.back(); +// if (nStart == nEnd) { +// std::cout << "warn! start == end" << std::endl; +// return 0; +// } +// Heading head(nStart->x_cm, nStart->y_cm, nEnd->x_cm, nEnd->y_cm); +// const float diff = head.getDiffHalfRAD(desiredHeading); +// return Distribution::Normal::getProbability(0, 0.3, diff); +// } + +// double evalHeadingChanges(const Walk& w) const { +// Stats::Variance var; +// for (int i = 0; i < w.size()-2; ++i) { +// const Node* n0 = w[i+0]; +// const Node* n1 = w[i+1]; +// const Node* n2 = w[i+2]; +// Heading h1(n0->x_cm, n0->y_cm, n1->x_cm, n1->y_cm); +// Heading h2(n1->x_cm, n1->y_cm, n2->x_cm, n2->y_cm); +// const float diff = h1.getDiffHalfRAD(h2); +// var.add(diff); +// } +// const float totalVar = var.get(); +// return Distribution::Normal::getProbability(0, 0.3, totalVar); +// } + +// double eval(const Walk& w, const Heading desiredHeading, const float desiredDistance) const { + +// return 1.0 +// * evalHeadingStartEnd(w, desiredHeading) +// * evalDistance(w, desiredDistance) +// // * evalHeadingChanges(w); +// ; + +// } + + + + + // Walk getRandomWalk2(Grid& grid, const Node* start, const float dist_m) const { + + // Walk walk; + + // float dist = 0; + + // const Node* cur = start; + // while(true) { + + // walk.push_back(cur); + // if (dist > dist_m) {break;} + + // const int numNeighbors = cur->getNumNeighbors(); + // //std::cout << "neighbors: " << numNeighbors << std::endl; + // int idx = rand() % numNeighbors; + // const Node* next = &grid.getNeighbor(*cur, idx); + // dist += next->inMeter().getDistance(cur->inMeter()); + // cur = next; + + // } + + // return walk; + + // } + +} + +#endif // INDOOR_GW3_WALKER_H From 5d6d62c2d264e944634a49686483007014622a07 Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 24 Oct 2017 18:42:21 +0200 Subject: [PATCH 09/13] fixed activity enum problems --- grid/walk/v2/modules/WalkModuleActivityControl.h | 10 +++++----- main.cpp | 2 +- sensors/activity/ActivityButterPressure.h | 12 +++++------- sensors/activity/ActivityDetector.h | 4 ++-- 4 files changed, 13 insertions(+), 15 deletions(-) diff --git a/grid/walk/v2/modules/WalkModuleActivityControl.h b/grid/walk/v2/modules/WalkModuleActivityControl.h index 95121ed..075f54b 100644 --- a/grid/walk/v2/modules/WalkModuleActivityControl.h +++ b/grid/walk/v2/modules/WalkModuleActivityControl.h @@ -6,7 +6,7 @@ #include "../../../../geo/Heading.h" #include "../../../../math/Distributions.h" -#include "../../../../sensors/pressure/ActivityButterPressure.h" +#include "../../../../sensors/activity/ActivityButterPressure.h" /** @@ -74,20 +74,20 @@ public: const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm; // TODO: general activity enum and activity-detector based on barometer and accelerometer? - const ActivityButterPressure::Activity activity = ctrl->activity; + const Activity activity = ctrl->activity; // const float kappa = 0.75; switch (activity) { - case ActivityButterPressure::Activity::DOWN: + case Activity::WALKING_DOWN: if (deltaZ_cm < 0) {return 0.60;} if (deltaZ_cm == 0) {return 0.25;} {return 0.15;} - case ActivityButterPressure::Activity::UP: + case Activity::WALKING_UP: if (deltaZ_cm > 0) {return 0.60;} if (deltaZ_cm == 0) {return 0.25;} {return 0.15;} - case ActivityButterPressure::Activity::STAY: + case Activity::WALKING: if (deltaZ_cm == 0) {return 0.60;} {return 0.40;} // case ActivityButterPressure::Activity::DOWN: diff --git a/main.cpp b/main.cpp index e7b4eeb..3c2ccd8 100755 --- a/main.cpp +++ b/main.cpp @@ -109,7 +109,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; + ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/sensors/activity/ActivityButterPressure.h b/sensors/activity/ActivityButterPressure.h index 5c9adbf..0dbc19d 100644 --- a/sensors/activity/ActivityButterPressure.h +++ b/sensors/activity/ActivityButterPressure.h @@ -50,13 +50,13 @@ public: /** ctor */ - ActivityButterPressure() : currentActivity(STAY){ + ActivityButterPressure() : currentActivity(Activity::STANDING){ ; } /** add new sensor readings that were received at the given timestamp */ - Activity add(const Timestamp& ts, const BarometerData& baro) { + void add(const Timestamp& ts, const BarometerData& baro) { //init static bool firstCall = false; @@ -65,7 +65,7 @@ public: firstCall = true; butter2.stepInitialization(0); - return STAY; + currentActivity = Activity::STANDING; } bool newInterpolatedValues = false; @@ -123,14 +123,12 @@ public: } } - return currentActivity; - } /** get the current Activity */ - Activity getCurrentActivity() { + Activity get() const { return currentActivity; - } + } }; #endif // ACTIVITYBUTTERPRESSURE_H diff --git a/sensors/activity/ActivityDetector.h b/sensors/activity/ActivityDetector.h index b85ed4b..7cf90dd 100644 --- a/sensors/activity/ActivityDetector.h +++ b/sensors/activity/ActivityDetector.h @@ -41,8 +41,8 @@ private: MovingStdDevTS stdDev; MovingStdDevTS stdDev2; - MovingAverageTS baroAvgSlow; - MovingAverageTS baroAvgFast; + //MovingAverageTS baroAvgSlow; + //MovingAverageTS baroAvgFast; MovingAverageTS baroAvg; HistoryTS baroHistory; From ea351d6377687a7c961b842b87ed51b694c87070 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 25 Oct 2017 16:58:41 +0200 Subject: [PATCH 10/13] worked on grid-walker --- grid/walk/v3/Helper.h | 204 +++++++++++++++++++++++++++++++---- grid/walk/v3/Structs.h | 4 + grid/walk/v3/WalkEvaluator.h | 10 +- grid/walk/v3/Walker.h | 110 +++++++++++++------ 4 files changed, 273 insertions(+), 55 deletions(-) diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index 5c2ca68..9d51178 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -6,10 +6,146 @@ #include "Structs.h" +#include #include +#include + +//#define SHOW_DEBUG_PLOT + +#ifdef SHOW_DEBUG_PLOT + #include + #include + #include +#endif + namespace GW3 { + + /** get an iterator over all nodes reachable from the given start */ + template class ReachableIteratorSorted { + + const Grid& grid; + const Node& start; + + struct Next { + + uint32_t idx; + float distToStart; + + Next(uint32_t idx, float distToStart) : idx(idx), distToStart(distToStart) {;} + + /** compare by weight. same weight? : compare by pointer */ + bool operator < (const Next& o) const { + return (distToStart != o.distToStart) ? (distToStart < o.distToStart) : (idx < o.idx); + } + + }; + + Node* curNode = nullptr; + std::unordered_set visited; + std::set toVisit; + + public: + + ReachableIteratorSorted(const Grid& grid, const Node& start) : grid(grid), start(start) { + toVisit.insert(Next(start.getIdx(),0)); + } + + bool hasNext() const { + return !toVisit.empty(); + } + + const Node& next() { + + const Next cur = *(toVisit.begin()); // visit from inside out (needed for correct distance) + toVisit.erase(toVisit.begin()); + visited.insert(cur.idx); + + const Node& curNode = grid[cur.idx]; + + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float newDist = cur.distToStart + curNode.getDistanceInMeter(neighbor); + + // not yet reached -> store distance + if (visited.find(neighborIdx) == visited.end()) { + toVisit.insert(Next(neighborIdx, newDist)); + } + + } + + // done + return curNode; + + } + + }; + + /** get an iterator over all nodes reachable from the given start */ + template class ReachableIteratorUnsorted { + + const Grid& grid; + const Node& start; + + Node* curNode = nullptr; + std::unordered_set visited; + std::vector toVisit; + + public: + + ReachableIteratorUnsorted(const Grid& grid, const Node& start) : grid(grid), start(start) { + toVisit.push_back(start.getIdx()); + } + + bool hasNext() const { + return !toVisit.empty(); + } + + const Node& next(const std::function& skip) { + + const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance) + toVisit.erase(toVisit.begin()); + visited.insert(curIdx); + + const Node& curNode = grid[curIdx]; + + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + + // not yet reached -> store distance + if (!skip(neighbor)) { + if (visited.find(neighborIdx) == visited.end()) { + toVisit.push_back(neighborIdx); + } + } + + } + + // done + return curNode; + + } + + }; + + + struct ReachableSettings { + + float dist_m; + bool limitDistance = true; + + Heading heading = Heading(0); + float maxHeadingDiff_rad; + bool limitHeading = false; + + }; + + template class Helper { public: @@ -74,54 +210,80 @@ namespace GW3 { return walks; - } + } /** get all reachable nodes that are within a given range */ - static Nodes getAllReachableNodes(Grid& grid, const Node* start, const float dist_m) { + static Nodes getAllReachableNodes(Grid& grid, const Node* start, const ReachableSettings& set ) { + + //auto tStart = std::chrono::system_clock::now(); Nodes res; std::unordered_map distances; - std::vector toVisit; + std::vector toVisit; // std::queue was only barely faster: 900 vs 880 microseconds toVisit.push_back(start->getIdx()); distances[start->getIdx()] = 0.0f; + #ifdef SHOW_DEBUG_PLOT + static K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementColorPoints pts1; pts1.setPointType(7); pts1.setPointSize(1); + plot.add(&pts1); + #endif + while (!toVisit.empty()) { - int curIdx = toVisit.front(); + const int curIdx = toVisit.front(); // visit from inside out (needed for correct distance) toVisit.erase(toVisit.begin()); const Node& curNode = grid[curIdx]; const float curDistance = distances[curIdx]; - res.push_back(&curNode); // remember for output + res.push_back(&curNode); // remember for output - if (curDistance <= dist_m) { - for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + #ifdef SHOW_DEBUG_PLOT + pts1.add(K::GnuplotPoint2(curNode.x_cm, curNode.y_cm), curDistance); + gp.draw(plot); + gp.flush(); + #endif - const int neighborIdx = curNode.getNeighborIdx(i); - const Node& neighbor = grid[neighborIdx]; - const float addDist = neighbor.inMeter().getDistance(curNode.inMeter()); - const float totalDist = curDistance + addDist; + for (int i = 0; i < curNode.getNumNeighbors(); ++i) { - // this is like in dijkstra. keep the smallest distance to reach a node: + const int neighborIdx = curNode.getNeighborIdx(i); + const Node& neighbor = grid[neighborIdx]; + const float addDist = neighbor.getDistanceInMeter(curNode); + const float totalDist = curDistance + addDist; - // not yet reached -> store distance - if (distances.find(neighborIdx) == distances.end()) { - toVisit.push_back(neighborIdx); - distances[neighborIdx] = totalDist; + // this is like in dijkstra. keep the smallest distance to reach a node: - // reached earlier but found shorter way - } else { - if (distances[neighborIdx] > totalDist) { - distances[neighborIdx] = totalDist; - } + // not yet reached -> store distance + if (distances.find(neighborIdx) == distances.end()) { + distances[neighborIdx] = totalDist; + + if (set.limitDistance) { + if (totalDist > set.dist_m) {continue;} } + if (set.limitHeading) { + const Heading head(start->x_cm, start->y_cm, neighbor.x_cm, neighbor.y_cm); // angle between start and current node + const float diff = head.getDiffHalfRAD(set.heading); // difference between above angle and requested angle + if (diff > set.maxHeadingDiff_rad) {continue;} // more than 90 degree difference? -> ignore + } + toVisit.push_back(neighborIdx); // needs a visit? (still some distance left) + + + // reached earlier but found shorter way + } else if (distances[neighborIdx] > totalDist) { + distances[neighborIdx] = totalDist; } + } } + //auto tEnd = std::chrono::system_clock::now(); + //auto elapsed = std::chrono::duration_cast(tEnd - tStart); + //std::cout << elapsed.count() << std::endl; + return res; } diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h index 335f0e3..8d46389 100644 --- a/grid/walk/v3/Structs.h +++ b/grid/walk/v3/Structs.h @@ -9,9 +9,13 @@ namespace GW3 { /** paremters for the walk */ struct WalkParams { + Point3 start; float distance_m; Heading heading = Heading(0); + + float lookFurther_m = 1.5; + }; /** result of the random walk */ diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index c740e72..8b73dbe 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -50,8 +50,12 @@ namespace GW3 { /** evaluate the difference between head(start,end) and the requested heading */ template class WalkEvalHeadingStartEnd : public WalkEvaluator { + const double sigma; + public: + WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;} + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { (void) params; @@ -64,7 +68,7 @@ namespace GW3 { const Heading head(pStart.xy(), pEnd.xy()); const float diff = head.getDiffHalfRAD(params.heading); //const float diff = Heading::getSignedDiff(params.heading, head); - return Distribution::Normal::getProbability(0, 0.04, diff); + return Distribution::Normal::getProbability(0, sigma, diff); } @@ -73,10 +77,12 @@ namespace GW3 { /** evaluate the difference between distance(start, end) and the requested distance */ template class WalkEvalDistance : public WalkEvaluator { - const double sigma = 0.1f; + const double sigma; public: + WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;} + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { const float walkedDistance_m = pStart.getDistance(pEnd); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 894b631..543059b 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -52,24 +52,30 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + Assert::isNot0(params.distance_m, "walking distance must be > 0"); + static std::mt19937 rndGen; const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); // include one additional grid-cell (increased distance) - const float secBuffer_m = grid.getGridSize_cm() / 100.0f; - const float range_m = params.distance_m + secBuffer_m; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); + const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1); + ReachableSettings set; + set.limitDistance = true; + set.dist_m = params.distance_m + secBuffer_m; + set.limitHeading = false; + set.heading = params.heading; + set.maxHeadingDiff_rad = M_PI/2; + const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); WalkResult res; res.heading = params.heading; res.position = params.start; - float realDist_m = params.distance_m; const Point2 dir = res.heading.asVector(); - const Point2 dst = params.start.xy() + (dir * realDist_m); + const Point2 dst = params.start.xy() + (dir * params.distance_m); // is dst reachable? const Node* n = Helper::contains(grid, reachableNodes, dst); @@ -80,12 +86,14 @@ namespace GW3 { if (grid.hasNodeFor(gp)) { res.position = p3; // update position - res.heading; // keep as-is - res.probability; // keep as-is + //res.heading; // keep as-is + //res.probability; // keep as-is return res; // done + } else { std::cout << "WARN dst not found" << std::endl; //throw "should not happen"; + } } @@ -150,61 +158,99 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + Assert::isNot0(params.distance_m, "walking distance must be > 0"); + static std::minstd_rand rndGen; const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); - // include one additional grid-cell (increased distance) - const float secBuffer_m = grid.getGridSize_cm() / 100.0f; - const float range_m = params.distance_m + secBuffer_m; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, range_m); +// // include one additional grid-cell (increased distance) +// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05); + +// ReachableSettings set; +// set.limitDistance = true; +// set.limitHeading = true; +// set.dist_m = params.distance_m + secBuffer_m; +// set.heading = params.heading; +// set.maxHeadingDiff_rad = M_PI/2; +// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); - // not found -> try random pick among all reachable nodes const float gridSize_m = grid.getGridSize_cm() / 100.0f; - std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); + //std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); Point3 best; double bestP = 0; +// DrawList drawer; const Point3 start = params.start; - // try X random destinations, evaluate them, remember the best one (reduces the number of "stupid particles") - for (int i = 0; i < 25; ++i) { + // try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles") + //for (int i = 0; i < 500; ++i) { - const Node* dstNode = reachableNodes[dNode(rndGen)]; + // const Node* dstNode = reachableNodes[dNode(rndGen)]; - // random position within destination-node - std::uniform_real_distribution dFinal(-gridSize_m*0.485f, +gridSize_m*0.4585f); - const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); - // destination = node-center + offset (within the node's bbox) - const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; - // sanity check - Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); - if (start == end) {continue;} + ReachableIteratorUnsorted ri(grid, *startNode); + const float maxDist = params.distance_m * 1.25 + gridSize_m; + + auto skip = [&] (const Node& n) { + const float dist_m = n.getDistanceInMeter(gpStart); + return dist_m > maxDist; + }; + + //for (const Node* dstNode : reachableNodes) { + while(ri.hasNext()) { + + const Node* dstNode = &ri.next(skip); +// const float dist_m = dstNode->getDistanceInMeter(gpStart); + +// if (dist_m > maxDist) { +// break; +// } + + for (int i = 0; i < 25; ++i) { + + // random position within destination-node + const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + + // destination = node-center + offset (within the node's bbox) + const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + + // sanity check + if (start == end) {continue;} + if (!grid.hasNodeFor(Helper::p3ToGp(end))) { + std::cout << "random destination is not part of the grid" << std::endl; + continue; + } + + //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = end;} + //drawer.add(end, p); - double p = 1; - for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); - p *= p1; } - if (p > bestP) {bestP = p; best = end;} - } - //const Point3 end = lst.get(); + //const Point3 end = drawer.get(); const Point3 end = best; WalkResult res; if (start == end) { res.probability = 0; } else { res.heading = Heading(start.xy(), end.xy()); - res.probability = bestP; // 1 + res.probability = bestP; } res.position = end; return res; From 284c6b11a6ed1d32163b2d6a84b80f9ce5fc6610 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 31 Oct 2017 19:38:08 +0100 Subject: [PATCH 11/13] many changes added new helper class for 3x3 matrices and vec3 added magnetometer data added compass detection refactored pose-estimation (single class) refactored debug plots (move to own class) minor changes --- math/Matrix3.h | 175 +++++++++ math/MovingMedian.h | 4 +- math/MovingMedianTS.h | 74 ++++ sensors/imu/CompassDetection.h | 112 ++++++ sensors/imu/CompassDetectionPlot.h | 123 ++++++ sensors/imu/MagnetometerData.h | 81 ++++ sensors/imu/PoseDetection.h | 480 +++++++++++++++++++++++ sensors/imu/PoseDetectionPlot.h | 146 +++++++ sensors/imu/TurnDetection.h | 588 ++--------------------------- sensors/imu/TurnDetectionPlot.h | 118 ++++++ sensors/offline/FilePlayer.h | 21 +- sensors/offline/FileReader.h | 26 +- sensors/offline/Listener.h | 7 +- sensors/offline/Sensors.h | 1 + 14 files changed, 1377 insertions(+), 579 deletions(-) create mode 100644 math/Matrix3.h create mode 100644 math/MovingMedianTS.h create mode 100644 sensors/imu/CompassDetection.h create mode 100644 sensors/imu/CompassDetectionPlot.h create mode 100644 sensors/imu/MagnetometerData.h create mode 100644 sensors/imu/PoseDetection.h create mode 100644 sensors/imu/PoseDetectionPlot.h create mode 100644 sensors/imu/TurnDetectionPlot.h diff --git a/math/Matrix3.h b/math/Matrix3.h new file mode 100644 index 0000000..2ca0b7f --- /dev/null +++ b/math/Matrix3.h @@ -0,0 +1,175 @@ +#ifndef INDOOR_MATH_MATRIX3_H +#define INDOOR_MATH_MATRIX3_H + + +#include +#include + +class Matrix3 { + +private: + + float data[9]; + +public: + + Matrix3(std::initializer_list lst) { + int idx = 0; + for (float f : lst) { + data[idx] = f; ++idx; + } + } + + static Matrix3 identity() { + return Matrix3( {1,0,0, 0,1,0, 0,0,1} ); + } + +// static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) { +// return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI); +// } + +// static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) { + +// const float g = radX; const float b = radY; const float a = radZ; +// const float a11 = std::cos(a)*std::cos(b); +// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); +// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); +// const float a21 = std::sin(a)*std::cos(b); +// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); +// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); +// const float a31 = -std::sin(b); +// const float a32 = std::cos(b)*std::sin(g); +// const float a33 = std::cos(b)*std::cos(g); +// return Matrix4({ +// a11, a12, a13, 0, +// a21, a22, a23, 0, +// a31, a32, a33, 0, +// 0, 0, 0, 1 +// }); + +// } + + static Matrix3 getTranslation(const float x, const float y) { + return Matrix3({ + 1, 0, x, + 0, 1, y, + 0, 0, 1, + }); + } + + static Matrix3 getScale(const float x, const float y) { + return Matrix3({ + x, 0, 0, + 0, y, 0, + 0, 0, 1, + }); + } + + float operator [] (const int idx) const {return data[idx];} + + bool operator == (const Matrix3& o) const { + for (int i = 0; i < 9; ++i) { + if (data[i] != o.data[i]) {return false;} + } + return true; + } + + Matrix3 operator * (const float v) const { + return Matrix3({ + data[0]*v, data[1]*v, data[2]*v, + data[3]*v, data[4]*v, data[5]*v, + data[6]*v, data[7]*v, data[8]*v, + }); + } + + Matrix3 operator + (const Matrix3& m) const { + return Matrix3({ + data[0]+m.data[0], data[1]+m.data[1], data[2]+m.data[2], + data[3]+m.data[3], data[4]+m.data[4], data[5]+m.data[5], + data[6]+m.data[6], data[7]+m.data[7], data[8]+m.data[8], + }); + } + + Matrix3 operator * (const Matrix3& m) const { + return Matrix3({ + data[0]*m.data[0] + data[1]*m.data[3] + data[2]*m.data[6], data[0]*m.data[1] + data[1]*m.data[4] + data[2]*m.data[7], data[0]*m.data[2] + data[1]*m.data[5] + data[2]*m.data[8], + data[3]*m.data[0] + data[4]*m.data[3] + data[5]*m.data[6], data[3]*m.data[1] + data[4]*m.data[4] + data[5]*m.data[7], data[3]*m.data[2] + data[4]*m.data[5] + data[5]*m.data[8], + data[6]*m.data[0] + data[7]*m.data[3] + data[8]*m.data[6], data[6]*m.data[1] + data[7]*m.data[4] + data[8]*m.data[7], data[6]*m.data[2] + data[7]*m.data[5] + data[8]*m.data[8], + }); + } + +}; + +struct Vector3 { + + float x,y,z; + + Vector3() : x(0), y(0), z(0) {;} + + Vector3(float x, float y, float z) : x(x), y(y), z(z) {;} + + Vector3 operator + (const Vector3 o) const { + return Vector3(x+o.x, y+o.y, z+o.z); + } + Vector3 operator - (const Vector3 o) const { + return Vector3(x-o.x, y-o.y, z-o.z); + } + Vector3 operator * (const Vector3 o) const { + return Vector3(x*o.x, y*o.y, z*o.z); + } + + + Vector3 operator * (const float v) const { + return Vector3(x*v, y*v, z*v); + } + Vector3 operator / (const float v) const { + return Vector3(x/v, y/v, z/v); + } + + bool operator == (const Vector3 o) const { + return (x==o.x) && (y==o.y) && (z==o.z); + } + + float norm() const { + return std::sqrt(x*x + y*y + z*z); + } + + Vector3 normalized() const { + const float n = norm(); + return Vector3(x/n, y/n, z/n); + } + + Vector3 cross(const Vector3 o) const { + return Vector3( + y*o.z - z*o.y, + z*o.x - x*o.z, + x*o.y - y*o.x + ); + } + + float dot(const Vector3 o) const { + return (x*o.x) + (y*o.y) + (z*o.z); + } + +}; + +inline Vector3 operator * (const Matrix3& mat, const Vector3& vec) { + + return Vector3( + (mat[ 0]*vec.x + mat[ 1]*vec.y + mat[ 2]*vec.z), + (mat[ 3]*vec.x + mat[ 4]*vec.y + mat[ 5]*vec.z), + (mat[ 6]*vec.x + mat[ 7]*vec.y + mat[ 8]*vec.z) + ); + +} + +//inline Matrix4 operator * (const Matrix4& m1, const Matrix4& m2) { +// return Matrix4({ +// m1[ 0]*m2[ 0] + m1[ 1]*m2[ 4] + m1[ 2]*m2[ 8] + m1[ 3]*m2[12], m1[ 0]*m2[ 1] + m1[ 1]*m2[ 5] + m1[ 2]*m2[ 9] + m1[ 3]*m2[13], m1[ 0]*m2[ 2] + m1[ 1]*m2[ 6] + m1[ 2]*m2[10] + m1[ 3]*m2[14], m1[ 0]*m2[ 3] + m1[ 1]*m2[ 7] + m1[ 2]*m2[11] + m1[ 3]*m2[15], +// m1[ 4]*m2[ 0] + m1[ 5]*m2[ 4] + m1[ 6]*m2[ 8] + m1[ 7]*m2[12], m1[ 4]*m2[ 1] + m1[ 5]*m2[ 5] + m1[ 6]*m2[ 9] + m1[ 7]*m2[13], m1[ 4]*m2[ 2] + m1[ 5]*m2[ 6] + m1[ 6]*m2[10] + m1[ 7]*m2[14], m1[ 4]*m2[ 3] + m1[ 5]*m2[ 7] + m1[ 6]*m2[11] + m1[ 7]*m2[15], +// m1[ 8]*m2[ 0] + m1[ 9]*m2[ 4] + m1[10]*m2[ 8] + m1[11]*m2[12], m1[ 8]*m2[ 1] + m1[ 9]*m2[ 5] + m1[10]*m2[ 9] + m1[11]*m2[13], m1[ 8]*m2[ 2] + m1[ 9]*m2[ 6] + m1[10]*m2[10] + m1[11]*m2[14], m1[ 8]*m2[ 3] + m1[ 9]*m2[ 7] + m1[10]*m2[11] + m1[11]*m2[15], +// m1[12]*m2[ 0] + m1[13]*m2[ 4] + m1[14]*m2[ 8] + m1[15]*m2[12], m1[12]*m2[ 1] + m1[13]*m2[ 5] + m1[14]*m2[ 9] + m1[15]*m2[13], m1[12]*m2[ 2] + m1[13]*m2[ 6] + m1[14]*m2[10] + m1[15]*m2[14], m1[12]*m2[ 3] + m1[13]*m2[ 7] + m1[14]*m2[11] + m1[15]*m2[15] +// }); +//} + +#endif // INDOOR_MATH_MATRIX3_H diff --git a/math/MovingMedian.h b/math/MovingMedian.h index 9ad4cdd..56c8ee5 100644 --- a/math/MovingMedian.h +++ b/math/MovingMedian.h @@ -41,9 +41,9 @@ public: // get the median if (values.size() % 2 != 0) { - return values[(values.size() + 0) / 2]; + return copy[(copy.size() + 0) / 2]; } else { - return (values[values.size() / 2 - 1] + values[values.size() / 2 + 0]) / 2; + return (copy[copy.size() / 2 - 1] + copy[copy.size() / 2 + 0]) / 2; } } diff --git a/math/MovingMedianTS.h b/math/MovingMedianTS.h new file mode 100644 index 0000000..a3722ca --- /dev/null +++ b/math/MovingMedianTS.h @@ -0,0 +1,74 @@ +#ifndef MOVINGMEDIANTS_H +#define MOVINGMEDIANTS_H + +#include +#include +#include "../data/Timestamp.h" + +template class MovingMedianTS { + +private: + + /** timestamp -> value combination */ + struct Entry { + Timestamp ts; + T value; + Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;} + }; + + /** the regional window to use */ + Timestamp window; + + /** the value history for the window-size */ + std::vector history; + + + + +public: + + /** ctor with the window-size to use */ + MovingMedianTS(const Timestamp window) : window(window) { + + } + + /** add a new value */ + void add(const Timestamp ts, const T data) { + + // append to history + history.push_back(Entry(ts, data)); + + // remove too-old history entries + const Timestamp oldest = ts - window; + while(history.front().ts < oldest) { + + // remove from history + history.erase(history.begin()); + + } + + } + + /** get the current median */ + T get() const { + + const auto comp = [] (const Entry& a, const Entry& b) { + return a.value < b.value; + }; + + // create a sorted copy (slow, but works) + std::vector copy = history; + std::sort(copy.begin(), copy.end(), comp); + + // get the median + if (copy.size() % 2 != 0) { + return copy[(copy.size() + 0) / 2].value; + } else { + return (copy[copy.size() / 2 - 1].value + copy[copy.size() / 2 + 0].value) / 2; + } + + } + +}; + +#endif // MOVINGMEDIANTS_H diff --git a/sensors/imu/CompassDetection.h b/sensors/imu/CompassDetection.h new file mode 100644 index 0000000..0af0398 --- /dev/null +++ b/sensors/imu/CompassDetection.h @@ -0,0 +1,112 @@ +#ifndef INDOOR_IMU_COMPASSDETECTION_H +#define INDOOR_IMU_COMPASSDETECTION_H + +#include "MagnetometerData.h" +#include "PoseDetection.h" + +#include "../../data/Timestamp.h" +#include "../../math/MovingAverageTS.h" +#include "../../math/MovingStdDevTS.h" +#include "../../geo/Point3.h" +#include "../../Assertions.h" + +#include "CompassDetectionPlot.h" + +#include +#include + + +/** + * custom compass implementation, similar to turn-detection + */ +class CompassDetection { + +private: + + #ifdef WITH_DEBUG_PLOT + CompassDetectionPlot plot; + #endif + +// struct { +// Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); +// bool isKnown = false; +// Timestamp lastEstimation; +// } orientation; + + MovingAverageTS avgIn = MovingAverageTS(Timestamp::fromMS(150), MagnetometerData(0,0,0)); + + //MovingStdDevTS stdDev = MovingStdDevTS(Timestamp::fromMS(2000), MagnetometerData(0,0,0)); + MovingStdDevTS stdDev = MovingStdDevTS(Timestamp::fromMS(1500), 0); + + PoseDetection* pose = nullptr; + int numMagReadings = 0; + +public: + + /** ctor */ + CompassDetection(PoseDetection* pose) : pose(pose) { + ; + } + + /** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */ + float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) { + + ++numMagReadings; + + // get the current magnetometer-reading as vector + //Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z; + const Vector3 vec(mag.x, mag.y, mag.z); + + // rotate it into our desired coordinate system, where the smartphone lies flat on the ground + //Eigen::Vector3f _curMag = pose->getMatrix() * vec; + const Vector3 _curMag = pose->getMatrix() * vec; + + //avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2))); + avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z)); + const MagnetometerData curMag = avgIn.get(); + //const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z); + + // calculate standard-deviation + //stdDev.add(ts, curMag); + //const float dev = std::max(stdDev.get().x, stdDev.get().y); + + + // calculate angle + // https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf + const float mx = curMag.x; + const float my = curMag.y; + const float tmp = std::atan2(my, mx); + //const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my)); + + // http://www.magnetic-declination.com/ + // http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html + const float declination = 3.0f / 180.0f * M_PI; // GERMANY! + const float curHeading = - tmp + declination; + float resHeading; + bool stable = true; + + // calculate standard-deviation within a certain timeframe + stdDev.add(ts, curHeading); + + // if the standard-deviation is too high, + // the compass is considered unstable + if (numMagReadings < 250 || stdDev.get() > 0.30) { + resHeading = NAN; + stable = false; + } else { + resHeading = curHeading; + stable = true; + } + + #ifdef WITH_DEBUG_PLOT + plot.add(ts, curHeading, stable, mag, curMag); + #endif + + // done + return resHeading; + + } + +}; + +#endif // INDOOR_IMU_COMPASSDETECTION_H diff --git a/sensors/imu/CompassDetectionPlot.h b/sensors/imu/CompassDetectionPlot.h new file mode 100644 index 0000000..2b4db3b --- /dev/null +++ b/sensors/imu/CompassDetectionPlot.h @@ -0,0 +1,123 @@ +#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H +#define INDOOR_IMU_COMPASSDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + + #include "MagnetometerData.h" + #include "../../data/Timestamp.h" + #include "../../math/Matrix3.h" + + class CompassDetectionPlot { + + Timestamp plotRef; + Timestamp lastPlot; + + K::Gnuplot gp1; + K::Gnuplot gp2; + K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2); + + K::GnuplotPlot plotMagRaw; + K::GnuplotPlotElementLines lineMagRawX; + K::GnuplotPlotElementLines lineMagRawY; + K::GnuplotPlotElementLines lineMagRawZ; + K::GnuplotPlotElementLines lineMagRawMagnitude; + + K::GnuplotPlot plotMagFix; + K::GnuplotPlotElementLines lineMagFixX; + K::GnuplotPlotElementLines lineMagFixY; + K::GnuplotPlotElementLines lineMagFixZ; + + K::GnuplotPlot plotMagScatter; + K::GnuplotPlotElementPoints scatterPoints; + + + + public: + + CompassDetectionPlot() { + + gp1 << "set autoscale xfix\n"; + gp2 << "set size ratio -1\n"; + + multiplot.add(&plotMagRaw); + multiplot.add(&plotMagFix); + + plotMagRaw.setTitle("Magnetometer (raw)"); + plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x"); + plotMagRaw.add(&lineMagRawY); lineMagRawY.getStroke().getColor().setHexStr("#00ff00"); lineMagRawY.setTitle("y"); + plotMagRaw.add(&lineMagRawZ); lineMagRawZ.getStroke().getColor().setHexStr("#0000ff"); lineMagRawZ.setTitle("z"); + plotMagRaw.add(&lineMagRawMagnitude); lineMagRawMagnitude.getStroke().getColor().setHexStr("#000000"); lineMagRawMagnitude.setTitle("magnitude"); + + plotMagFix.setTitle("Magnetometer (re-aligned)"); + plotMagFix.add(&lineMagFixX); lineMagFixX.getStroke().getColor().setHexStr("#ff0000"); lineMagFixX.setTitle("x"); + plotMagFix.add(&lineMagFixY); lineMagFixY.getStroke().getColor().setHexStr("#00ff00"); lineMagFixY.setTitle("y"); + plotMagFix.add(&lineMagFixZ); lineMagFixZ.getStroke().getColor().setHexStr("#0000ff"); lineMagFixZ.setTitle("z"); + + plotMagScatter.add(&scatterPoints); + + + } + + void add(Timestamp ts, float curHeading, bool stable, const MagnetometerData& mag, const MagnetometerData& curMag) { + + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); + + // raw gyro + lineMagRawX.add( K::GnuplotPoint2(tsPlot.ms(), mag.x) ); + lineMagRawY.add( K::GnuplotPoint2(tsPlot.ms(), mag.y) ); + lineMagRawZ.add( K::GnuplotPoint2(tsPlot.ms(), mag.z) ); + lineMagRawMagnitude.add( K::GnuplotPoint2(tsPlot.ms(), mag.magnitude()) ); + + // adjusted mag + lineMagFixX.add( K::GnuplotPoint2(tsPlot.ms(), curMag.x) ); + lineMagFixY.add( K::GnuplotPoint2(tsPlot.ms(), curMag.y) ); + lineMagFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curMag.z) ); + + const float len = 1;//curMag.magnitude();// std::sqrt(curMag.x*curMag.x + curMag.y*curMag.y); + scatterPoints.add(K::GnuplotPoint2(curMag.x/len, curMag.y/len)); + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + + lastPlot = tsPlot; + + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineMagRawX.removeIf(remove); + lineMagRawY.removeIf(remove); + lineMagRawZ.removeIf(remove); + lineMagRawMagnitude.removeIf(remove); + lineMagFixX.removeIf(remove); + lineMagFixY.removeIf(remove); + lineMagFixZ.removeIf(remove); + + + const float s = stable ? 0.1 : 0.03; + const float ax = 0.85 + std::cos(curHeading)*s; + const float ay = 0.85 + std::sin(curHeading)*s; + gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; + gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; + + gp1.draw(multiplot); + gp1.flush(); + + gp2.draw(plotMagScatter); + gp2.flush(); + + } + + } + + }; + +#endif + +#endif diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h new file mode 100644 index 0000000..24136c7 --- /dev/null +++ b/sensors/imu/MagnetometerData.h @@ -0,0 +1,81 @@ +#ifndef INDOOR_IMU_MAGNETOMETERDATA_H +#define INDOOR_IMU_MAGNETOMETERDATA_H + + +#include +#include + +/** + * data received from a magnetometer sensor + */ +struct MagnetometerData { + + float x; + float y; + float z; + + MagnetometerData() : x(0), y(0), z(0) {;} + + /** ctor from RADIANS */ + MagnetometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;} + + std::string asString() const { + std::stringstream ss; + ss << "(" << x << "," << y << "," << z << ")"; + return ss.str(); + } + + bool isValid() const { + return (x == x) && (y == y) && (z == z); + } + + bool operator == (const GyroscopeData& o ) const { + return EQ_OR_NAN(x, o.x) && + EQ_OR_NAN(y, o.y) && + EQ_OR_NAN(z, o.z); + } + + float magnitude() const { + return std::sqrt( x*x + y*y + z*z ); + } + + MagnetometerData& operator += (const MagnetometerData& o) { + this->x += o.x; + this->y += o.y; + this->z += o.z; + return *this; + } + + MagnetometerData& operator -= (const MagnetometerData& o) { + this->x -= o.x; + this->y -= o.y; + this->z -= o.z; + return *this; + } + + MagnetometerData operator * (const MagnetometerData& o) const { + return MagnetometerData(x*o.x, y*o.y, z*o.z); + } + + MagnetometerData operator - (const MagnetometerData& o) const { + return MagnetometerData(x-o.x, y-o.y, z-o.z); + } + + MagnetometerData operator / (const float val) const { + return MagnetometerData(x/val, y/val, z/val); + } + + +private: + + static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );} + +}; + +namespace std { + MagnetometerData sqrt(const MagnetometerData& o) { + return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); + } +} + +#endif // INDOOR_IMU_MAGNETOMETERDATA_H diff --git a/sensors/imu/PoseDetection.h b/sensors/imu/PoseDetection.h new file mode 100644 index 0000000..f485534 --- /dev/null +++ b/sensors/imu/PoseDetection.h @@ -0,0 +1,480 @@ +#ifndef INDOOR_IMU_POSEDETECTION_H +#define INDOOR_IMU_POSEDETECTION_H + +#include "AccelerometerData.h" + +#include "../../data/Timestamp.h" + +#include "../../math/MovingAverageTS.h" +#include "../../math/MovingMedianTS.h" +#include "../../math/Matrix3.h" + +#include "../../geo/Point3.h" + +#include + +#include "PoseDetectionPlot.h" + +/** + * estimate the smartphones world-pose, + * based on the accelerometer's data + */ +class PoseDetection { + +// struct LongTermTriggerAverage { + +// Eigen::Vector3f sum; +// int cnt; + +// XYZ() { +// reset(); +// } + +// /** add the given accelerometer reading */ +// void addAcc(const Timestamp ts, const AccelerometerData& acc) { + +// // did NOT improve the result for every smartphone (only some) +// //const float deltaMag = std::abs(acc.magnitude() - 9.81); +// //if (deltaMag > 5.0) {return;} + +// // adjust sum and count (for average calculation) +// Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z; +// sum += vec; +// ++cnt; + + +// } + +// AccelerometerData getAvg() const { +// return AccelerometerData(sum(0), sum(1), sum(2)) / cnt; +// } + +// /** get the current rotation matrix estimation */ +// Eigen::Matrix3f get() const { + +// // get the current acceleromter average +// const Eigen::Vector3f avg = sum / cnt; + +// // rotate average accelerometer into (0,0,1) +// Eigen::Vector3f zAxis; zAxis << 0, 0, 1; +// const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + +// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) +// Eigen::Vector3f aligned = (rotMat * avg).normalized(); +// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + +// return rotMat; + +// } + +// /** reset the current sum etc. */ +// void reset() { +// cnt = 0; +// sum = Eigen::Vector3f::Zero(); +// } + + +// }; + + + /** live-pose-estimation using moving average of the smartphone's accelerometer */ + struct EstMovingAverage { + + // average the accelerometer + MovingAverageTS avg; + + EstMovingAverage(const Timestamp window) : + avg(MovingAverageTS(window, AccelerometerData())) { + + // start approximately + addAcc(Timestamp(), AccelerometerData(0,0,9.81)); + + } + + /** add the given accelerometer reading */ + void addAcc(const Timestamp ts, const AccelerometerData& acc) { + avg.add(ts, acc); + } + + AccelerometerData getBase() const { + return avg.get(); + } + + /** get the current rotation matrix estimation */ + //Eigen::Matrix3f get() const { + Matrix3 get() const { + + // get the current acceleromter average + const AccelerometerData avgAcc = avg.get(); + //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); + const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z); + + // rotate average-accelerometer into (0,0,1) + //Eigen::Vector3f zAxis; zAxis << 0, 0, 1; + const Vector3 zAxis(0,0,1); + const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis); + //const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE + //const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); + + // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) + //Eigen::Vector3f aligned = (rotMat * avg).normalized(); + const Vector3 aligned = (rotMat * avg).normalized(); + Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + + return rotMat; + + } + + }; + + /** live-pose-estimation using moving median of the smartphone's accelerometer */ + struct EstMovingMedian { + + // median the accelerometer + MovingMedianTS medianX; + MovingMedianTS medianY; + MovingMedianTS medianZ; + + EstMovingMedian(const Timestamp window) : + medianX(window), medianY(window), medianZ(window) { + + // start approximately + addAcc(Timestamp(), AccelerometerData(0,0,9.81)); + + } + + /** add the given accelerometer reading */ + void addAcc(const Timestamp ts, const AccelerometerData& acc) { + medianX.add(ts, acc.x); + medianY.add(ts, acc.y); + medianZ.add(ts, acc.z); + } + + AccelerometerData getBase() const { + return AccelerometerData(medianX.get(), medianY.get(), medianZ.get()); + } + + /** get the current rotation matrix estimation */ + //Eigen::Matrix3f get() const { + Matrix3 get() const { + + const Vector3 base(medianX.get(), medianY.get(), medianZ.get()); + + // rotate average-accelerometer into (0,0,1) + const Vector3 zAxis(0,0,1); + const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis); + + // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) + const Vector3 aligned = (rotMat * base).normalized(); + Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); + + return rotMat; + + } + + }; + + + +private: + + struct { + //Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); + Matrix3 rotationMatrix = Matrix3::identity(); + bool isKnown = false; + Timestamp lastEstimation; + } orientation; + + /** how the pose is estimated */ + //LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250)); + EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(300)); + //EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(400)); + + #ifdef WITH_DEBUG_PLOT + PoseDetectionPlot plot; + #endif + + +public: + + /** ctor */ + PoseDetection() { + ; + } + +// /** get the smartphone's rotation matrix */ +// Eigen::Matrix3f getMatrix() const { +// return orientation.rotationMatrix; +// } + + /** get the smartphone's rotation matrix */ + const Matrix3& getMatrix() const { + return orientation.rotationMatrix; + } + + /** is the pose known and stable? */ + bool isKnown() const { + return orientation.isKnown; + } + + void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) { + + // add accelerometer data + est.addAcc(ts, acc); + + // update (if needed) + orientation.rotationMatrix = est.get(); + orientation.isKnown = true; + orientation.lastEstimation = ts; + + // debug-plot (if configured) + #ifdef WITH_DEBUG_PLOT + plot.add(ts, est.getBase(), orientation.rotationMatrix); + #endif + + } + +public: + +// /** get a matrix that rotates the vector "from" into the vector "to" */ +// static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) { + +// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another + +// const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm(); +// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() ); + +// Eigen::Matrix3f A; A << +// 0, -x(2), x(1), +// x(2), 0, -x(0), +// -x(1), x(0), 0; + +// return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A)); + +// } + + /** get a matrix that rotates the vector "from" into the vector "to" */ + static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) { + + // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another + + const Vector3 v = from.cross(to) / from.cross(to).norm(); + const float angle = std::acos( from.dot(to) / from.norm() / to.norm() ); + + Matrix3 A({ + 0.0f, -v.z, v.y, + v.z, 0.0f, -v.x, + -v.y, v.x, 0.0f + }); + + return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle))); + + } + + + + + +// /** get a rotation matrix for the given x,y,z rotation (in radians) */ +// static Eigen::Matrix3f getRotation(const float x, const float y, const float z) { +// const float g = x; const float b = y; const float a = z; +// const float a11 = std::cos(a)*std::cos(b); +// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); +// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); +// const float a21 = std::sin(a)*std::cos(b); +// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); +// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); +// const float a31 = -std::sin(b); +// const float a32 = std::cos(b)*std::sin(g); +// const float a33 = std::cos(b)*std::cos(g); +// Eigen::Matrix3f m; +// m << +// a11, a12, a13, +// a21, a22, a23, +// a31, a32, a33; +// ; +// return m; +// } + +// /** estimate the smartphones current holding position */ +// void estimateHolding2() { + + +// // z-axis points through the device and is the axis we are interested in +// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics + +// avgAcc = Eigen::Vector3f::Zero(); + +// for (const AccelerometerData& acc : accData) { +// //for (const GyroscopeData& acc : gyroData) { +// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); +// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); +// avgAcc += vec; +// } + +// //avgAcc /= accData.size(); +// avgAcc = avgAcc.normalized(); + +// Eigen::Vector3f rev; rev << 0,0,1; + +// rotMat = getRotationMatrix(avgAcc, rev); + + +// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis"); +// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis"); +//// Eigen::Vector3f re = rotMat * avgAcc; +//// Eigen::Vector3f diff = rev-re; +//// Assert::isTrue(diff.norm() < 0.001, "rotation error"); + +// } + +// struct RotationMatrixEstimationUsingAccAngle { + +// Eigen::Vector3f lastAvg; +// Eigen::Vector3f avg; +// int cnt; + +// RotationMatrixEstimationUsingAccAngle() { +// reset(); +// } + +// void add(const float x, const float y, const float z) { + +// Eigen::Vector3f vec; vec << x,y,z; +// avg += vec; +// ++cnt; + +// } + +// void reset() { +// cnt = 0; +// avg = Eigen::Vector3f::Zero(); +// } + +// Eigen::Matrix3f get() { + +// // http://www.hobbytronics.co.uk/accelerometer-info + +// avg /= cnt; +// lastAvg = avg; + +// //const float mag = avg.norm(); + +// const float baseX = 0; +// const float baseY = 0; +// const float baseZ = 0; // mag? + +// const float x = avg(0) - baseX; +// const float y = avg(1) - baseY; +// const float z = avg(2) - baseZ; + +// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) ); +// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) ); + +// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax? + +// // sanity check +// Eigen::Vector3f zAxis; zAxis << 0, 0, 1; +// Eigen::Vector3f aligned = (rotMat * avg).normalized(); +// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); +// // int i = 0; (void) i; + +// reset(); +// return rotMat; + +// } + +// } est; + +// struct PCA { + +// Eigen::Vector3f avg; +// Eigen::Vector3f lastAvg; +// Eigen::Matrix3f covar; +// int cnt = 0; + +// PCA() { +// reset(); +// } + +// void add(const float x, const float y, const float z) { + +// Eigen::Vector3f vec; vec << x,y,z; +// avg += vec; +// covar += vec*vec.transpose(); +// ++cnt; + +// } + +// Eigen::Matrix3f get() { + +// avg /= cnt; +// covar /= cnt; +// lastAvg = avg; + +// std::cout << avg << std::endl; + +// Eigen::Matrix3f Q = covar;// - avg*avg.transpose(); +// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));} + +// Eigen::SelfAdjointEigenSolver solver(Q); +// solver.eigenvalues(); +// solver.eigenvectors(); + +// const auto eval = solver.eigenvalues(); +// const auto evec = solver.eigenvectors(); +// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!"); + +// Eigen::Matrix3f rotMat; +// rotMat.col(0) = evec.col(0); +// rotMat.col(1) = evec.col(1); +// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue +// rotMat.transposeInPlace(); + +// //Eigen::Vector3f gy; gy << 0, 30, 30; +// Eigen::Vector3f avg1 = rotMat * avg; +// int i = 0; (void) i; + +// reset(); + +// return rotMat; + +// } + +// void reset() { +// cnt = 0; +// avg = Eigen::Vector3f::Zero(); +// covar = Eigen::Matrix3f::Zero(); +// } + + +// } pca1; + + + + +// /** estimate the smartphones current holding position */ +// void estimateHolding() { + +// Eigen::Vector3f avg = Eigen::Vector3f::Zero(); +// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero(); + +// for (const AccelerometerData& acc : accData) { +//// for (const GyroscopeData& acc : gyroData) { +// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); +//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z); +// avg += vec; +// covar += vec * vec.transpose(); +// } + +// avg /= accData.size(); // TODO +// covar /= accData.size(); //TODO + +// avgAcc = avg.normalized(); + + +}; + + + +#endif // INDOOR_IMU_POSEDETECTION_H diff --git a/sensors/imu/PoseDetectionPlot.h b/sensors/imu/PoseDetectionPlot.h new file mode 100644 index 0000000..e065ad5 --- /dev/null +++ b/sensors/imu/PoseDetectionPlot.h @@ -0,0 +1,146 @@ +#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H +#define INDOOR_IMU_POSEDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #include "../../data/Timestamp.h" + #include "../../math/Matrix3.h" + #include "AccelerometerData.h" + + class PoseDetectionPlot { + + Timestamp plotRef; + Timestamp lastPlot; + + K::Gnuplot gp1; + K::Gnuplot gp2; + + K::GnuplotPlot plotAcc; + K::GnuplotPlotElementLines lineAccX; + K::GnuplotPlotElementLines lineAccY; + K::GnuplotPlotElementLines lineAccZ; + + K::GnuplotSplot plotPose; + K::GnuplotSplotElementLines linePose; + //K::GnuplotSplotElementEmpty emptyPose; + + std::vector>> pose; + + public: + + /** ctor */ + PoseDetectionPlot() { + + gp1 << "set autoscale xfix\n"; + gp2 << "set view equal xyz\n"; + + plotAcc.setTitle("Accelerometer"); + plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX"); + plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY"); + plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ"); + + plotPose.setTitle("Pose"); + plotPose.getView().setEnabled(false); + plotPose.add(&linePose); + //plotPose.add(&emptyPose); + + plotPose.getAxisX().setRange(-8,+8); + plotPose.getAxisY().setRange(-8,+8); + plotPose.getAxisZ().setRange(-8,+8); + + const float a = 0.05; const float b = 0.95; + pose = { + {{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden + {{0, 0, 0},{0, 0, 1},{0, 1, 1},{0, 1, 0},{0, 0, 0}}, // links + {{1, 0, 0},{1, 0, 1},{1, 1, 1},{1, 1, 0},{1, 0, 0}}, // rechts + {{0, 1, 0},{1, 1, 0},{1, 1, 1},{0, 1, 1},{0, 1, 0}}, // oben + {{0, 0, 0},{1, 0, 0},{1, 0, 1},{0, 0, 1},{0, 0, 0}}, // unten + {{0, 0, 1},{1, 0, 1},{1, 1, 1},{0, 1, 1},{0, 0, 1}}, // deckel + {{a, 0.15, 1},{b, 0.15, 1},{b, 0.95, 1},{a, 0.95, 1},{a, 0.15, 1}}, // display + }; + + //K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000")); + K::GnuplotStroke stroke = K::GnuplotStroke::NONE(); + K::GnuplotFill fillOut = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999")); + K::GnuplotFill fillSide = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666")); + K::GnuplotFill fillDisp = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#333333")); + + plotPose.getObjects().set(1, new K::GnuplotObjectPolygon(fillOut, stroke)); + plotPose.getObjects().set(2, new K::GnuplotObjectPolygon(fillSide, stroke)); + plotPose.getObjects().set(3, new K::GnuplotObjectPolygon(fillSide, stroke)); + plotPose.getObjects().set(4, new K::GnuplotObjectPolygon(fillSide, stroke)); + plotPose.getObjects().set(5, new K::GnuplotObjectPolygon(fillSide, stroke)); + plotPose.getObjects().set(6, new K::GnuplotObjectPolygon(fillOut, stroke)); + plotPose.getObjects().set(7, new K::GnuplotObjectPolygon(fillDisp, stroke)); + + } + + void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) { + + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); + + // acc + lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) ); + lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) ); + lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) ); + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + lastPlot = tsPlot; + + // update 3D smartphone model + for (size_t i = 0; i < pose.size(); ++i) { + K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear(); + for (const std::vector& pts : pose[i]) { + const Vector3 vec1(pts[0], pts[1], pts[2]); + const Vector3 vec2 = vec1 - Vector3(0.5, 0.5, 0.5); // center cube at 0,0,0 + const Vector3 vec3 = vec2 * Vector3(7, 15, 1); // stretch cube + const Vector3 vec4 = rotation * vec3; + gp->add(K::GnuplotCoordinate3(vec4.x, vec4.y, vec4.z, K::GnuplotCoordinateSystem::FIRST)); + } + } + + // add coordinate system + const Vector3 vx = rotation * Vector3(2,0,0); + const Vector3 vy = rotation * Vector3(0,3,0); + const Vector3 vz = rotation * Vector3(0,0,5); + linePose.clear(); + linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z)); + linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z)); + linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z)); + + // remove old accelerometer entries + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineAccX.removeIf(remove); + lineAccY.removeIf(remove); + lineAccZ.removeIf(remove); + + // raw accelerometer + gp1.draw(plotAcc); + gp1.flush(); + + // 3D pose + gp2.draw(plotPose); + gp2.flush(); + + + } + + } + + }; + +#endif + +#endif // INDOOR_IMU_POSEDETECTIONPLOT_H diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 2ab0f6d..01dbe7c 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.h @@ -5,23 +5,18 @@ #include "AccelerometerData.h" #include "../../data/Timestamp.h" #include "../../math/MovingAverageTS.h" +#include "../../math/Matrix3.h" + #include "../../geo/Point3.h" +#include "PoseDetection.h" #include #include #include -#ifdef WITH_DEBUG_PLOT - #include - #include - #include - #include - #include - #include - #include - #include -#endif +#include "TurnDetectionPlot.h" + #include "../../Assertions.h" @@ -29,98 +24,29 @@ class TurnDetection { private: - #ifdef WITH_DEBUG_PLOT - Timestamp plotRef; - Timestamp lastPlot; - - K::Gnuplot gp1; - K::Gnuplot gp2; - - K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3); - - K::GnuplotPlot plotGyroRaw; - K::GnuplotPlotElementLines lineGyroRawX; - K::GnuplotPlotElementLines lineGyroRawY; - K::GnuplotPlotElementLines lineGyroRawZ; - - K::GnuplotPlot plotGyroFix; - K::GnuplotPlotElementLines lineGyroFixX; - K::GnuplotPlotElementLines lineGyroFixY; - K::GnuplotPlotElementLines lineGyroFixZ; - - K::GnuplotPlot plotAcc; - K::GnuplotPlotElementLines lineAccX; - K::GnuplotPlotElementLines lineAccY; - K::GnuplotPlotElementLines lineAccZ; - - K::GnuplotSplot plotPose; - K::GnuplotSplotElementLines linePose; - - float plotCurHead = 0; - - #endif - + PoseDetection* pose = nullptr; //std::vector gyroData; - Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); + //Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); + Vector3 prevGyro = Vector3(0,0,0); Timestamp lastGyroReading; - - struct { - Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); - bool isKnown = false; - Timestamp lastEstimation; - } orientation; - - - + #ifdef WITH_DEBUG_PLOT + TurnDetectionPlot plot; + #endif public: /** ctor */ - TurnDetection() { - - #ifdef WITH_DEBUG_PLOT - - gp1 << "set autoscale xfix\n"; - gp1 << "set view equal xyz\n"; - - multiplot.add(&plotGyroRaw); - multiplot.add(&plotGyroFix); - multiplot.add(&plotAcc); - - plotGyroRaw.setTitle("Gyroscope (raw)"); - plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX"); - plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY"); - plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ"); - - plotGyroFix.setTitle("Gyroscope (fixed)"); - plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX"); - plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY"); - plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ"); - - plotAcc.setTitle("Accelerometer"); - plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX"); - plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY"); - plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ"); - - plotPose.setTitle("Pose"); - plotPose.getView().setEnabled(false); - plotPose.add(&linePose); - plotPose.getAxisX().setRange(-5,+5); - plotPose.getAxisY().setRange(-5,+5); - plotPose.getAxisZ().setRange(-5,+5); - - #endif - + TurnDetection(PoseDetection* pose) : pose(pose) { + ; } // does not seem to help... // struct DriftEstimator { - // MovingAverageTS avg; // DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) { @@ -139,12 +65,8 @@ public: // } driftEst; - - - float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { - // ignore the first reading completely, just remember its timestamp if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;} @@ -157,17 +79,20 @@ public: // ignore readings until the first orientation-estimation is available // otherwise we would use a wrong rotation matrix which yields wrong results! - if (!orientation.isKnown) {return 0.0f;} + if (!pose->isKnown()) {return 0.0f;} // get the current gyro-reading as vector - Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z; + //Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z; + const Vector3 vec(gyro.x, gyro.y, gyro.z); // rotate it into our desired coordinate system, where the smartphone lies flat on the ground - Eigen::Vector3f curGyro = orientation.rotationMatrix * vec; + //Eigen::Vector3f curGyro = orientation.rotationMatrix * vec; + const Vector3 curGyro = pose->getMatrix() * vec; //driftEst.removeDrift(ts, curGyro); // area - const Eigen::Vector3f area = + //const Eigen::Vector3f area = + const Vector3 area = // Trapezoid rule (should be more accurate but does not always help?!) //(prevGyro * curDiff.sec()) + // squared region @@ -183,485 +108,18 @@ public: prevGyro = curGyro; // rotation = z-axis only! - const float delta = area(2); - + //const float delta = area(2); + const float delta = area.z; #ifdef WITH_DEBUG_PLOT - - plotCurHead += delta; - - if (plotRef.isZero()) {plotRef = ts;} - const Timestamp tsPlot = (ts-plotRef); - const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); - - // raw gyro - lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) ); - lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) ); - lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) ); - - // adjusted gyro - lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(0)) ); - lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) ); - lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) ); - - // adjusted gyro - lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) ); - lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) ); - lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) ); - - if (lastPlot + Timestamp::fromMS(50) < tsPlot) { - - lastPlot = tsPlot; - - // plot 3D pose - std::vector pose = { - Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1), - Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1), - }; - linePose.clear(); - for (const Point3 p : pose) { - Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z; - Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1; - K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2)); - linePose.add(gp3); - } - - auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; - lineGyroRawX.removeIf(remove); - lineGyroRawY.removeIf(remove); - lineGyroRawZ.removeIf(remove); - lineGyroFixX.removeIf(remove); - lineGyroFixY.removeIf(remove); - lineGyroFixZ.removeIf(remove); - lineAccX.removeIf(remove); - lineAccY.removeIf(remove); - lineAccZ.removeIf(remove); - - const float ax = 0.85 + std::cos(plotCurHead)*0.1; - const float ay = 0.85 + std::sin(plotCurHead)*0.1; - gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; - gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; - - - gp1.draw(multiplot); - gp1.flush(); - - gp2.draw(plotPose); - gp2.flush(); - - //usleep(100); - - } - + plot.add(ts, delta, gyro, curGyro); #endif - // done return delta; } - - - void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) { - - // add accelerometer data - //pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z)); - est.addAcc(ts, acc); - - if (1 == 0) { - - // FASTER - - // start with the first available timestamp - if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;} - - // if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding - if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) { - orientation.rotationMatrix = est.get(); - orientation.isKnown = true; - orientation.lastEstimation = ts; - est.reset(); - } - - } else { - - // MORE ACCURATE - - orientation.rotationMatrix = est.get(); - orientation.isKnown = true; - orientation.lastEstimation = ts; - - } - - } - -private: - -// /** estimate the smartphones current holding position */ -// void estimateHolding2() { - - -// // z-axis points through the device and is the axis we are interested in -// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics - -// avgAcc = Eigen::Vector3f::Zero(); - -// for (const AccelerometerData& acc : accData) { -// //for (const GyroscopeData& acc : gyroData) { -// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); -// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); -// avgAcc += vec; -// } - -// //avgAcc /= accData.size(); -// avgAcc = avgAcc.normalized(); - -// Eigen::Vector3f rev; rev << 0,0,1; - -// rotMat = getRotationMatrix(avgAcc, rev); - - -// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis"); -// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis"); -//// Eigen::Vector3f re = rotMat * avgAcc; -//// Eigen::Vector3f diff = rev-re; -//// Assert::isTrue(diff.norm() < 0.001, "rotation error"); - -// } - -public: - - /** get a matrix that rotates the vector "from" into the vector "to" */ - static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) { - - // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another - - const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm(); - const float angle = std::acos( from.dot(to) / from.norm() / to.norm() ); - - Eigen::Matrix3f A; A << - 0, -x(2), x(1), - x(2), 0, -x(0), - -x(1), x(0), 0; - - return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A)); - - } - - - struct XYZ { - - Eigen::Vector3f sum; - int cnt; - - XYZ() { - reset(); - } - - /** add the given accelerometer reading */ - void addAcc(const Timestamp ts, const AccelerometerData& acc) { - - // did NOT improve the result for every smartphone (only some) - //const float deltaMag = std::abs(acc.magnitude() - 9.81); - //if (deltaMag > 5.0) {return;} - - // adjust sum and count (for average calculation) - Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z; - sum += vec; - ++cnt; - - - } - - AccelerometerData getAvg() const { - return AccelerometerData(sum(0), sum(1), sum(2)) / cnt; - } - - /** get the current rotation matrix estimation */ - Eigen::Matrix3f get() const { - - // get the current acceleromter average - const Eigen::Vector3f avg = sum / cnt; - - // rotate average accelerometer into (0,0,1) - Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); - - // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) - Eigen::Vector3f aligned = (rotMat * avg).normalized(); - Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); - - return rotMat; - - } - - /** reset the current sum etc. */ - void reset() { - cnt = 0; - sum = Eigen::Vector3f::Zero(); - } - - - }; - - - struct XYZ2 { - - // average the accelerometer - MovingAverageTS avg = MovingAverageTS(Timestamp::fromMS(1250), AccelerometerData()); - - XYZ2() { - - // start approximately - addAcc(Timestamp(), AccelerometerData(0,0,9.81)); - - } - - /** add the given accelerometer reading */ - void addAcc(const Timestamp ts, const AccelerometerData& acc) { - - // did NOT improve the result for every smartphone (only some) - //const float deltaMag = std::abs(acc.magnitude() - 9.81); - //if (deltaMag > 5.0) {return;} - - avg.add(ts, acc); - - } - - AccelerometerData getAvg() const { - return avg.get(); - } - - /** get the current rotation matrix estimation */ - Eigen::Matrix3f get() const { - - // get the current acceleromter average - AccelerometerData avgAcc = getAvg(); - const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z); - - // rotate average-accelerometer into (0,0,1) - Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis); - - // just a small sanity check. after applying to rotation the acc-average should become (0,0,1) - Eigen::Vector3f aligned = (rotMat * avg).normalized(); - Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); - - return rotMat; - - } - - /** reset the current sum etc. */ - void reset() { - ; - } - - } est; - - - -// struct RotationMatrixEstimationUsingAccAngle { - -// Eigen::Vector3f lastAvg; -// Eigen::Vector3f avg; -// int cnt; - -// RotationMatrixEstimationUsingAccAngle() { -// reset(); -// } - -// void add(const float x, const float y, const float z) { - -// Eigen::Vector3f vec; vec << x,y,z; -// avg += vec; -// ++cnt; - -// } - -// void reset() { -// cnt = 0; -// avg = Eigen::Vector3f::Zero(); -// } - -// Eigen::Matrix3f get() { - -// // http://www.hobbytronics.co.uk/accelerometer-info - -// avg /= cnt; -// lastAvg = avg; - -// //const float mag = avg.norm(); - -// const float baseX = 0; -// const float baseY = 0; -// const float baseZ = 0; // mag? - -// const float x = avg(0) - baseX; -// const float y = avg(1) - baseY; -// const float z = avg(2) - baseZ; - -// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) ); -// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) ); - -// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax? - -// // sanity check -// Eigen::Vector3f zAxis; zAxis << 0, 0, 1; -// Eigen::Vector3f aligned = (rotMat * avg).normalized(); -// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high"); -// // int i = 0; (void) i; - -// reset(); -// return rotMat; - -// } - -// } est; - - - /** get a rotation matrix for the given x,y,z rotation (in radians) */ - static Eigen::Matrix3f getRotation(const float x, const float y, const float z) { - const float g = x; const float b = y; const float a = z; - const float a11 = std::cos(a)*std::cos(b); - const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g); - const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g); - const float a21 = std::sin(a)*std::cos(b); - const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g); - const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g); - const float a31 = -std::sin(b); - const float a32 = std::cos(b)*std::sin(g); - const float a33 = std::cos(b)*std::cos(g); - Eigen::Matrix3f m; - m << - a11, a12, a13, - a21, a22, a23, - a31, a32, a33; - ; - return m; - } - - -// struct PCA { - -// Eigen::Vector3f avg; -// Eigen::Vector3f lastAvg; -// Eigen::Matrix3f covar; -// int cnt = 0; - -// PCA() { -// reset(); -// } - -// void add(const float x, const float y, const float z) { - -// Eigen::Vector3f vec; vec << x,y,z; -// avg += vec; -// covar += vec*vec.transpose(); -// ++cnt; - -// } - -// Eigen::Matrix3f get() { - -// avg /= cnt; -// covar /= cnt; -// lastAvg = avg; - -// std::cout << avg << std::endl; - -// Eigen::Matrix3f Q = covar;// - avg*avg.transpose(); -// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));} - -// Eigen::SelfAdjointEigenSolver solver(Q); -// solver.eigenvalues(); -// solver.eigenvectors(); - -// const auto eval = solver.eigenvalues(); -// const auto evec = solver.eigenvectors(); -// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!"); - -// Eigen::Matrix3f rotMat; -// rotMat.col(0) = evec.col(0); -// rotMat.col(1) = evec.col(1); -// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue -// rotMat.transposeInPlace(); - -// //Eigen::Vector3f gy; gy << 0, 30, 30; -// Eigen::Vector3f avg1 = rotMat * avg; -// int i = 0; (void) i; - -// reset(); - -// return rotMat; - -// } - -// void reset() { -// cnt = 0; -// avg = Eigen::Vector3f::Zero(); -// covar = Eigen::Matrix3f::Zero(); -// } - - -// } pca1; - - - - -// /** estimate the smartphones current holding position */ -// void estimateHolding() { - -// Eigen::Vector3f avg = Eigen::Vector3f::Zero(); -// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero(); - -// for (const AccelerometerData& acc : accData) { -//// for (const GyroscopeData& acc : gyroData) { -// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z); -//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z); -// avg += vec; -// covar += vec * vec.transpose(); -// } - -// avg /= accData.size(); // TODO -// covar /= accData.size(); //TODO - -// avgAcc = avg.normalized(); - - - - - - - - - -//// static K::Gnuplot gp; -//// gp << "set view equal xyz\n"; -//// gp << "set xrange[-1:+1]\n"; -//// gp << "set yrange[-1:+1]\n"; -//// gp << "set zrange[-1:+1]\n"; - -//// K::GnuplotSplot plot; -//// K::GnuplotSplotElementLines lines; plot.add(&lines); - -//// K::GnuplotPoint3 p0(0,0,0); -//// K::GnuplotPoint3 px(evec(0,0), evec(1,0), evec(2,0)); //px = px * eval(0); -//// K::GnuplotPoint3 py(evec(0,1), evec(1,1), evec(2,1)); //py = py * eval(1); -//// K::GnuplotPoint3 pz(evec(0,2), evec(1,2), evec(2,2)); //pz = pz * eval(2); - -//// K::GnuplotPoint3 pa(avg(0), avg(1), avg(2)); - -//// lines.addSegment(p0, px); -//// lines.addSegment(p0, py); -//// lines.addSegment(p0, pz); -//// lines.addSegment(p0, pa); - -//// gp.draw(plot); -//// gp.flush(); - -// } - }; #endif // TURNDETECTION_H diff --git a/sensors/imu/TurnDetectionPlot.h b/sensors/imu/TurnDetectionPlot.h new file mode 100644 index 0000000..a0a9857 --- /dev/null +++ b/sensors/imu/TurnDetectionPlot.h @@ -0,0 +1,118 @@ +#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H +#define INDOOR_IMU_TURNDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + #include + + #include "GyroscopeData.h" + #include "../../data/Timestamp.h" + #include "../../math/Matrix3.h" + + class TurnDetectionPlot { + + Timestamp plotRef; + Timestamp lastPlot; + + K::Gnuplot gp1; + + K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2); + + K::GnuplotPlot plotGyroRaw; + K::GnuplotPlotElementLines lineGyroRawX; + K::GnuplotPlotElementLines lineGyroRawY; + K::GnuplotPlotElementLines lineGyroRawZ; + + K::GnuplotPlot plotGyroFix; + K::GnuplotPlotElementLines lineGyroFixX; + K::GnuplotPlotElementLines lineGyroFixY; + K::GnuplotPlotElementLines lineGyroFixZ; + + K::GnuplotSplot plotPose; + K::GnuplotSplotElementLines linePose; + + float plotCurHead = 0; + + public: + + TurnDetectionPlot() { + + gp1 << "set autoscale xfix\n"; + gp1 << "set view equal xyz\n"; + + multiplot.add(&plotGyroRaw); + multiplot.add(&plotGyroFix); + + plotGyroRaw.setTitle("Gyroscope (raw)"); + plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX"); + plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY"); + plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ"); + + plotGyroFix.setTitle("Gyroscope (fixed)"); + plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX"); + plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY"); + plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ"); + + plotPose.setTitle("Pose"); + plotPose.getView().setEnabled(false); + plotPose.add(&linePose); + plotPose.getAxisX().setRange(-5,+5); + plotPose.getAxisY().setRange(-5,+5); + plotPose.getAxisZ().setRange(-5,+5); + + } + + void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) { + + plotCurHead += delta; + + if (plotRef.isZero()) {plotRef = ts;} + const Timestamp tsPlot = (ts-plotRef); + const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000); + + // raw gyro + lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) ); + lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) ); + lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) ); + + // adjusted gyro + lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.x) ); + lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) ); + lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) ); + + if (lastPlot + Timestamp::fromMS(50) < tsPlot) { + + lastPlot = tsPlot; + + auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();}; + lineGyroRawX.removeIf(remove); + lineGyroRawY.removeIf(remove); + lineGyroRawZ.removeIf(remove); + lineGyroFixX.removeIf(remove); + lineGyroFixY.removeIf(remove); + lineGyroFixZ.removeIf(remove); + + const float ax = 0.85 + std::cos(plotCurHead)*0.1; + const float ay = 0.85 + std::sin(plotCurHead)*0.1; + gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n"; + gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n"; + + gp1.draw(multiplot); + gp1.flush(); + + } + + } + + }; + +#endif + +#endif // INDOOR_IMU_TURNDETECTIONPLOT_H diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h index dcb88ba..b257b42 100644 --- a/sensors/offline/FilePlayer.h +++ b/sensors/offline/FilePlayer.h @@ -109,16 +109,17 @@ namespace Offline { #warning "some sensors todo:" switch(e.type) { - case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; - case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break; - case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break; - case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break; - case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break; - case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; - case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; - case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; - case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; - default: throw Exception("code error. found not-yet-implemented sensor"); + case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; + case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break; + case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break; + case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break; + case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break; + case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break; + case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; + case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; + case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; + case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; + default: throw Exception("code error. found not-yet-implemented sensor"); } } diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 67ca235..6bf055e 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -16,6 +16,7 @@ #include "../../sensors/beacon/BeaconMeasurements.h" #include "../../sensors/gps/GPSData.h" #include "../../sensors/imu/CompassData.h" +#include "../../sensors/imu/MagnetometerData.h" #include "../../geo/Point2.h" #include "../../grid/factory/v2/GridFactory.h" @@ -51,6 +52,7 @@ namespace Offline { std::vector> gravity; std::vector> gps; std::vector> compass; + std::vector> magnetometer; /** all entries in linear order as they appeared while recording */ std::vector entries; @@ -88,6 +90,7 @@ namespace Offline { barometer.clear(); lin_acc.clear(); gravity.clear(); + magnetometer.clear(); } const std::vector& getEntries() const {return entries;} @@ -113,6 +116,8 @@ namespace Offline { const std::vector>& getGravity() const {return gravity;} + const std::vector>& getMagnetometer() const {return magnetometer;} + /** get an interpolateable ground-truth based on the time-clicks during recording */ GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector groundTruthPoints) const { @@ -147,7 +152,7 @@ namespace Offline { void parse(const std::string& file) { std::ifstream inp(file); - if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);} + if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file: " + file);} while(!inp.eof() && !inp.bad()) { @@ -172,6 +177,7 @@ namespace Offline { else if (idx == (int)Sensor::GRAVITY) {parseGravity(ts,data);} else if (idx == (int)Sensor::COMPASS) {parseCompass(ts,data);} else if (idx == (int)Sensor::GPS) {parseGPS(ts,data);} + else if (idx == (int)Sensor::MAGNETOMETER) {parseMagnetometer(ts,data);} // TODO: this is a hack... // the loop is called one additional time after the last entry @@ -343,6 +349,24 @@ namespace Offline { } + void parseMagnetometer(const uint64_t ts, const std::string& data) { + + MagnetometerData mag; + Splitter s(data, sep); + + mag.x = s.has(0) ? (s.getFloat(0)) : (NAN); + mag.y = s.has(1) ? (s.getFloat(1)) : (NAN); + mag.z = s.has(2) ? (s.getFloat(2)) : (NAN); + + TS elem(ts, mag); + this->magnetometer.push_back(elem); + entries.push_back(Entry(Sensor::MAGNETOMETER, ts, this->magnetometer.size()-1)); + + // inform listener + //if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);} + + } + /** parse the given GPS entry */ void parseGPS(const uint64_t ts, const std::string& data) { diff --git a/sensors/offline/Listener.h b/sensors/offline/Listener.h index 83cc28c..abdb2c8 100644 --- a/sensors/offline/Listener.h +++ b/sensors/offline/Listener.h @@ -2,11 +2,15 @@ #define OFFLINE_LISTENER_H #include "../gps/GPSData.h" + +#include "../pressure/BarometerData.h" + #include "../imu/CompassData.h" #include "../imu/GravityData.h" -#include "../pressure/BarometerData.h" #include "../imu/GyroscopeData.h" #include "../imu/AccelerometerData.h" +#include "../imu/MagnetometerData.h" + #include "../radio/WiFiMeasurements.h" namespace Offline { @@ -25,6 +29,7 @@ namespace Offline { virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0; virtual void onGPS(const Timestamp ts, const GPSData data) = 0; virtual void onCompass(const Timestamp ts, const CompassData data) = 0; + virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) = 0; }; diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h index 32168cd..80971b9 100644 --- a/sensors/offline/Sensors.h +++ b/sensors/offline/Sensors.h @@ -8,6 +8,7 @@ namespace Offline { GRAVITY = 1, LIN_ACC = 2, GYRO = 3, + MAGNETOMETER = 4, BARO = 5, COMPASS = 6, // also called "orientatioN" WIFI = 8, From 3f8d21e14658a514bad0624d0883b99a9a0e5a3b Mon Sep 17 00:00:00 2001 From: toni Date: Mon, 6 Nov 2017 18:04:55 +0100 Subject: [PATCH 12/13] fixed two bugs in ActivityButterPressure: 1) when barometer produces false measurements, we needed some kind of upper boundary 2) coding error static.. while initializing this object over multiple test iterations isnt the best idea --- grid/walk/v2/modules/WalkModuleHeadingVonMises.h | 1 + sensors/activity/ActivityButterPressure.h | 12 ++++++++---- sensors/activity/ActivityDetector.h | 2 +- sensors/radio/WiFiProbabilityFree.h | 2 +- 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h index 30be8e1..653f9e6 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -48,6 +48,7 @@ public: if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;} // adjust the state's heading using the control-data + //TODO: if motionaxis detects landscaping, do not do this! state.heading.direction += ctrl->turnSinceLastTransition_rad + var; //set kappa of mises diff --git a/sensors/activity/ActivityButterPressure.h b/sensors/activity/ActivityButterPressure.h index 0dbc19d..d72d46b 100644 --- a/sensors/activity/ActivityButterPressure.h +++ b/sensors/activity/ActivityButterPressure.h @@ -27,6 +27,7 @@ public: private: std::vector output; + std::vector baroInterpHistory; Activity currentActivity; MovingAVG mvAvg = MovingAVG(20); @@ -40,9 +41,12 @@ public: */ const bool additionalLowpassFilter = false; const unsigned long diffSize = 20; //the number values used for finding the activity. + //TODO: some kind of algorithm to calculate the threshold const float threshold = 0.025f; // if diffSize is getting smaller, treshold needs to be adjusted in the same direction! + const float upperBoundary = 0.1; // if this boundary is passed, we should have errors in barometer data Filter::ButterworthLP butter = Filter::ButterworthLP(10,0.05f,2); Filter::ButterworthLP butter2 = Filter::ButterworthLP(10,0.05f,2); + bool firstCall = false; FixedFrequencyInterpolator ffi = FixedFrequencyInterpolator(Timestamp::fromMS(100)); @@ -59,7 +63,6 @@ public: void add(const Timestamp& ts, const BarometerData& baro) { //init - static bool firstCall = false; if(!firstCall){ butter.stepInitialization(baro.hPa); firstCall = true; @@ -72,7 +75,9 @@ public: //interpolate & butter auto callback = [&] (const Timestamp ts, const float val) { + //interp float interpValue = val; + baroInterpHistory.push_back(History(ts, BarometerData(interpValue))); //butter float butterValue = butter.process(interpValue); @@ -92,7 +97,6 @@ public: for(unsigned long i = output.size() - diffSize; i < output.size() - 1; ++i){ float diffVal = output[i+1].data.hPa - output[i].data.hPa; - diff.push_back(diffVal); } @@ -111,10 +115,10 @@ public: actValue = sum; } - if(actValue > threshold){ + if(actValue > threshold && actValue < upperBoundary){ currentActivity = Activity::WALKING_DOWN; } - else if (actValue < -threshold){ + else if (actValue < -threshold && actValue > -upperBoundary){ currentActivity = Activity::WALKING_UP; } else{ diff --git a/sensors/activity/ActivityDetector.h b/sensors/activity/ActivityDetector.h index 7cf90dd..6c066de 100644 --- a/sensors/activity/ActivityDetector.h +++ b/sensors/activity/ActivityDetector.h @@ -140,7 +140,7 @@ private: ++xx; #endif - if (std::abs(delta_hPa) < 0.042) { + if (std::abs(delta_hPa) < 0.042) { current = Activity::WALKING; return; } else if (delta_hPa > 0) { diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index c8f79e0..8f69abd 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/sensors/radio/WiFiProbabilityFree.h @@ -69,7 +69,7 @@ public: Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense"); Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?"); - Assert::isBetween(scanRSSI, -100.0f, -30.0f, "scan-rssi out of range"); + Assert::isBetween(scanRSSI, -100.0f, -20.0f, "scan-rssi out of range"); //Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range"); // sigma grows with measurement age From 08d829297696d2c3f4f59b153e31071bbaaf6a38 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 8 Nov 2017 18:09:56 +0100 Subject: [PATCH 13/13] minor changes --- sensors/imu/PoseDetection.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensors/imu/PoseDetection.h b/sensors/imu/PoseDetection.h index f485534..f7278ff 100644 --- a/sensors/imu/PoseDetection.h +++ b/sensors/imu/PoseDetection.h @@ -187,8 +187,8 @@ private: /** how the pose is estimated */ //LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250)); - EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(300)); - //EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(400)); + EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450)); + //EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300)); #ifdef WITH_DEBUG_PLOT PoseDetectionPlot plot;