From d24bfcfbf55efd4c012bd78379a79488ea7010c2 Mon Sep 17 00:00:00 2001 From: Jan Schmidt Date: Mon, 7 Aug 2023 03:43:42 +1000 Subject: [PATCH] d/wmr: Add LED models for constellation tracking --- src/xrt/drivers/CMakeLists.txt | 2 + .../drivers/wmr/constellation/led_models.c | 201 ++++++++++++++++++ .../drivers/wmr/constellation/led_models.h | 67 ++++++ 3 files changed, 270 insertions(+) create mode 100644 src/xrt/drivers/wmr/constellation/led_models.c create mode 100644 src/xrt/drivers/wmr/constellation/led_models.h diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 1f728c1f9..3d52e20f3 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -532,6 +532,8 @@ if(XRT_BUILD_DRIVER_WMR) wmr/wmr_source.h wmr/constellation/blobwatch.c wmr/constellation/blobwatch.h + wmr/constellation/led_models.c + wmr/constellation/led_models.h ) target_link_libraries( drv_wmr diff --git a/src/xrt/drivers/wmr/constellation/led_models.c b/src/xrt/drivers/wmr/constellation/led_models.c new file mode 100644 index 000000000..acae65cc5 --- /dev/null +++ b/src/xrt/drivers/wmr/constellation/led_models.c @@ -0,0 +1,201 @@ +#include +#include + +#include "led_models.h" + +/* Set to 0 to use 3D euclidean distance to sort neighbours, + * 1 to use orthographic projected (undistorted) 2D distance + * with the anchor LED is forward-facing */ +#define PROJECTED_DISTANCE 0 +#define DUMP_FULL_DEBUG 0 + +#if DUMP_FULL_DEBUG +#define DEBUG(s, ...) printf(s, __VA_ARGS__) +#else +#define DEBUG(s, ...) +#endif + +void +constellation_led_model_init(struct constellation_led_model *led_model, uint8_t num_leds) +{ + led_model->leds = calloc(num_leds, sizeof(struct constellation_led)); + led_model->num_leds = num_leds; +} + +void +constellation_led_model_dump(struct constellation_led_model *led_model, const char *desc) +{ + int i; + printf("LED model: %s\n", desc); + for (i = 0; i < led_model->num_leds; i++) { + struct constellation_led *p = led_model->leds + i; + + printf("{ .pos = {%f,%f,%f}, .dir={%f,%f,%f} },\n", p->pos.x, p->pos.y, p->pos.z, p->dir.x, p->dir.y, + p->dir.z); + } +} + +void +constellation_led_model_clear(struct constellation_led_model *led_model) +{ + free(led_model->leds); + led_model->leds = NULL; +} + +struct led_candidate_sort_entry +{ + struct constellation_led *led; + double distance; +}; + +#define POW2(_x) ((_x) * (_x)) + +static double +calc_led_dist(const struct constellation_search_led_candidate *c, + const struct constellation_led *anchor, + struct xrt_vec3 *out_led_pos) +{ + struct xrt_vec3 led_pos; + +#if PROJECTED_DISTANCE + struct xrt_vec3 tmp; + + tmp = m_vec3_add(anchor->pos, c->pose.position); + math_quat_rotate_vec3(&c->pose.orientation, &tmp, &led_pos); + + if (out_led_pos) + *out_led_pos = led_pos; + + if (led_pos.z > 0.0) { + return sqrt(POW2(led_pos.x / led_pos.z) + POW2(led_pos.y / led_pos.z)); + } + return sqrt(POW2(led_pos.x) + POW2(led_pos.y)); +#else + led_pos = m_vec3_add(anchor->pos, c->pose.position); + + if (out_led_pos) + *out_led_pos = led_pos; + + return m_vec3_len(led_pos); +#endif +} + +static int +compare_led_distance(const void *elem1, const void *elem2) +{ + const struct led_candidate_sort_entry *l1 = (const struct led_candidate_sort_entry *)elem1; + const struct led_candidate_sort_entry *l2 = (const struct led_candidate_sort_entry *)elem2; + + if (l1->distance > l2->distance) + return 1; + if (l1->distance < l2->distance) + return -1; + + return 0; +} + +struct constellation_search_led_candidate * +constellation_search_led_candidate_new(struct constellation_led *led, struct constellation_led_model *led_model) +{ + int i; + + struct constellation_search_led_candidate *c = calloc(1, sizeof(struct constellation_search_led_candidate)); + c->led = led; + + /* Calculate the pose that places this LED forward-facing at 0,0,0 + * and then calculate the distance for all visible LEDs when (orthographic) + * projected in that pose */ + const struct xrt_vec3 fwd = (struct xrt_vec3){0.0, 0.0, 1.0}; + + c->pose.position = m_vec3_inverse(led->pos); + math_quat_from_vec_a_to_vec_b(&led->dir, &fwd, &c->pose.orientation); + + struct led_candidate_sort_entry array[256]; + + for (i = 0; i < led_model->num_leds; i++) { + struct constellation_led *cur = led_model->leds + i; + + if (cur == led) + continue; // Don't put the current LED in its own neighbour list + + if (m_vec3_dot(led->dir, cur->dir) <= 0) + continue; // Normals are more than 90 degrees apart - these are mutually exclusive LEDs + + struct led_candidate_sort_entry *entry = &array[c->num_neighbours]; + entry->led = cur; + entry->distance = calc_led_dist(c, cur, NULL); + + c->num_neighbours++; + } + + if (c->num_neighbours > 1) { + qsort(array, c->num_neighbours, sizeof(struct led_candidate_sort_entry), compare_led_distance); + + c->neighbours = calloc(c->num_neighbours, sizeof(struct constellation_led *)); + for (i = 0; i < c->num_neighbours; i++) { + c->neighbours[i] = array[i].led; + } + } + +#if DUMP_FULL_DEBUG + DEBUG("Have %u neighbours for LED %u (%f,%f,%f) dir (%f,%f,%f) ", c->num_neighbours, c->led->id, c->led->pos.x, + c->led->pos.y, c->led->pos.z, c->led->dir.x, c->led->dir.y, c->led->dir.z); + DEBUG(" front-facing rotation (%f,%f,%f,%f):\n", c->pose.orientation.x, c->pose.orientation.y, + c->pose.orientation.z, c->pose.orientation.w); + + for (i = 0; i < c->num_neighbours; i++) { + struct constellation_led *cur = c->neighbours[i]; + struct xrt_vec3 led_pos; + double distance = calc_led_dist(c, cur, &led_pos); + + struct xrt_vec3 tmp; + math_quat_rotate_vec3(&c->pose.orientation, &cur->dir, &tmp); + float theta = m_vec3_dot(tmp, fwd); + + DEBUG( + " LED id %2u @ %10.7f %10.7f %10.7f dir %10.7f %10.7f %10.7f -> %10.7f %10.7f dist %10.7f (dir " + "%f,%f,%f) angle %f\n", + cur->id, cur->pos.x, cur->pos.y, cur->pos.z, cur->dir.x, cur->dir.y, cur->dir.z, led_pos.x, + led_pos.y, distance, tmp.x, tmp.y, tmp.z, acosf(theta) / M_PI * 180.0); + } +#endif + + return c; +} + +void +constellation_search_led_candidate_free(struct constellation_search_led_candidate *candidate) +{ + free(candidate->neighbours); + free(candidate); +} + +struct constellation_search_model * +constellation_search_model_new(struct constellation_led_model *led_model) +{ + struct constellation_search_model *m = calloc(1, sizeof(struct constellation_search_model)); + int i; + + m->led_model = led_model; + + m->points = calloc(led_model->num_leds, sizeof(struct constellation_search_led_candidate *)); + m->num_points = led_model->num_leds; + + for (i = 0; i < led_model->num_leds; i++) { + m->points[i] = constellation_search_led_candidate_new(led_model->leds + i, led_model); + } + + return m; +} + +void +constellation_search_model_free(struct constellation_search_model *model) +{ + int i; + + for (i = 0; i < model->num_points; i++) { + constellation_search_led_candidate_free(model->points[i]); + } + free(model->points); + free(model); +} diff --git a/src/xrt/drivers/wmr/constellation/led_models.h b/src/xrt/drivers/wmr/constellation/led_models.h new file mode 100644 index 000000000..b3c2918a1 --- /dev/null +++ b/src/xrt/drivers/wmr/constellation/led_models.h @@ -0,0 +1,67 @@ +/* + * Blob detection + * Copyright 2014-2015 Philipp Zabel + * Copyright 2019-2023 Jan Schmidt + * SPDX-License-Identifier: LGPL-2.0+ or BSL-1.0 + */ +#pragma once + +#include + +#include "math/m_api.h" +#include "math/m_vec3.h" + +struct constellation_led +{ + // LED + uint32_t id; + // position relative to the device model in micrometers + struct xrt_vec3 pos; + // Normal + struct xrt_vec3 dir; + // LED radius in mm + float radius_mm; +}; + +struct constellation_led_model +{ + struct constellation_led *leds; + uint8_t num_leds; +}; + +void +constellation_led_model_init(struct constellation_led_model *led_model, uint8_t num_leds); +void +constellation_led_model_dump(struct constellation_led_model *led_model, const char *desc); +void +constellation_led_model_clear(struct constellation_led_model *led_model); + +struct constellation_search_led_candidate +{ + struct constellation_led *led; + + /* Transform to rotate the anchor LED to face forward @ 0,0,0 */ + struct xrt_pose pose; + + /* List of possible neighbours for this LED, sorted by distance */ + uint8_t num_neighbours; + struct constellation_led **neighbours; +}; + +struct constellation_search_led_candidate * +constellation_search_led_candidate_new(struct constellation_led *anchor_led, struct constellation_led_model *led_model); +void +constellation_search_led_candidate_free(struct constellation_search_led_candidate *candidate); + +struct constellation_search_model +{ + uint8_t num_points; + struct constellation_search_led_candidate **points; + + struct constellation_led_model *led_model; +}; + +struct constellation_search_model * +constellation_search_model_new(struct constellation_led_model *led_model); +void +constellation_search_model_free(struct constellation_search_model *model);