mirror of
https://github.com/lbgos/rift-s-linux.git
synced 2026-10-27 05:01:17 +01:00
d/wmr: Add LED models for constellation tracking
This commit is contained in:
Jan Schmidt committed
1 parent
a96f295789
commit
d24bfcfbf5
3 files changed
+270
No files matched your search
@@ -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
|
||||
|
||||
@@ -0,0 +1,201 @@
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
#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);
|
||||
Reference in new issue
Block a user