d/wmr: Pass model directly for correspondence search

Remove the old list of models from a prior iteration of the
correspondence_search code. Add a device ID to the LED constellation
structures.

src/xrt/drivers/wmr/constellation/correspondence_search.c

@@ -21,6 +21,7 @@
 #include <math.h>
 
 #include "math/m_vec3.h"
+#include "os/os_time.h"
 
 #include "lambdatwist/lambdatwist_p3p.h"
 #include "correspondence_search.h"
@@ -115,18 +116,12 @@ undistort_blob_points(struct blob *blobs, int num_blobs, struct xrt_vec2 *out_po
 void
 correspondence_search_set_blobs(struct correspondence_search *cs, struct blob *blobs, int num_blobs)
 {
-	int i, m;
+	int i;
 	struct xrt_vec2 undistorted_points[MAX_BLOBS_PER_FRAME];
 	struct cs_image_point *blob_list[MAX_BLOBS_PER_FRAME];
 
 	assert(num_blobs <= MAX_BLOBS_PER_FRAME);
 
-	/* Updating the blobs means we don't know if we have good poses any more */
-	for (m = 0; m < cs->num_models; m++) {
-		struct cs_model_info *mi = &cs->models[m];
-		mi->match_flags = 0;
-	}
-
 	if (cs->points != NULL)
 		free(cs->points);
 
@@ -172,19 +167,6 @@ correspondence_search_set_blobs(struct correspondence_search *cs, struct blob *b
 	}
 }
 
-bool
-correspondence_search_set_model(struct correspondence_search *cs, int id, struct constellation_search_model *model)
-{
-	if (cs->num_models == CS_MAX_MODELS)
-		return false;
-
-	cs->models[cs->num_models].id = id;
-	cs->models[cs->num_models].model = model;
-	cs->num_models++;
-
-	return true;
-}
-
 void
 correspondence_search_free(struct correspondence_search *cs)
 {
@@ -217,7 +199,7 @@ compare_blobs_distance(const void *elem1, const void *elem2, void *arg)
 #if DUMP_SCENE
 static void
 dump_pose(struct correspondence_search *cs,
-          struct led_search_model *model,
+          struct constellation_search_model *model,
           struct xrt_pose *pose,
           struct cs_model_info *mi)
 {
@@ -229,23 +211,24 @@ dump_pose(struct correspondence_search *cs,
 		struct xrt_vec3 pos, dir;
 
 		/* Project HMD LED into the image (no distortion) */
-		oquatf_get_rotated(&pose->orient, &leds->points[i].pos, &pos);
-		ovec3f_add(&pos, &pose->pos, &pos);
+		math_quat_rotate_vec3(&pose->orientation, &leds->leds[i].pos, &pos);
+		math_vec3_accum(&pose->position, &pos);
+
 		if (pos.z < 0)
 			continue; // Can't be behind the camera
 
-		ovec3f_multiply_scalar(&pos, 1.0 / pos.z, &pos);
+		math_vec3_scalar_mul(1.0 / pos.z, &pos);
 
 		double x = pos.x;
 		double y = pos.y;
 
-		ovec3f_normalize_me(&pos);
+		math_vec3_normalize(&pos);
 
-		oquatf_get_rotated(&pose->orient, &leds->points[i].dir, &dir);
-		ovec3f_normalize_me(&dir);
+		math_quat_rotate_vec3(&pose->orientation, &leds->leds[i].dir, &dir);
+		math_vec3_normalize(&dir);
 
 		double facing_dot;
-		facing_dot = m_vec3_dot(&pos, &dir);
+		facing_dot = m_vec3_dot(pos, dir);
 
 		if (facing_dot < 0) {
 			printf("  (%f,%f),\n", x, y);
@@ -334,9 +317,10 @@ correspondence_search_project_pose(struct correspondence_search *cs,
 				    mi->best_pose_led_depth, mi->best_pose_blob_depth,
 				    (mi->best_pose_found_time - mi->search_start_time) * 1000.0, mi->led_index,
 				    mi->blob_index);
-				DEBUG_TIMING("# pose orient %f %f %f %f pos %f %f %f\n", mi->best_pose.orient.x,
-				             mi->best_pose.orient.y, mi->best_pose.orient.z, mi->best_pose.orient.w,
-				             mi->best_pose.pos.x, mi->best_pose.pos.y, mi->best_pose.pos.z);
+				DEBUG_TIMING("# pose orient %f %f %f %f pos %f %f %f\n", mi->best_pose.orientation.x,
+				             mi->best_pose.orientation.y, mi->best_pose.orientation.z,
+				             mi->best_pose.orientation.w, mi->best_pose.position.x,
+				             mi->best_pose.position.y, mi->best_pose.position.z);
 			}
 
 #if DUMP_FULL_DEBUG
@@ -345,9 +329,9 @@ correspondence_search_project_pose(struct correspondence_search *cs,
 			DEBUG(
 			    "model %d new best pose candidate orient %f %f %f %f pos %f %f %f has %u visible LEDs, "
 			    "error %f (%f / LED) after %u trials and %u pose checks\n",
-			    mi->id, pose->orient.x, pose->orient.y, pose->orient.z, pose->orient.w, pose->pos.x,
-			    pose->pos.y, pose->pos.z, score.visible_leds, score.reprojection_error, error_per_led,
-			    cs->num_trials, cs->num_pose_checks);
+			    mi->id, pose->orientation.x, pose->orientation.y, pose->orientation.z, pose->orientation.w,
+			    pose->position.x, pose->position.y, pose->position.z, score.visible_leds,
+			    score.reprojection_error, error_per_led, cs->num_trials, cs->num_pose_checks);
 
 			DEBUG("model %d matched %u blobs of %u\n", mi->id, score.matched_blobs, score.visible_leds);
 #endif
@@ -361,8 +345,9 @@ correspondence_search_project_pose(struct correspondence_search *cs,
 			    score.visible_leds > 0 ? score.reprojection_error / score.visible_leds : -1;
 			DEBUG(
 			    "pose candidate orient %f %f %f %f pos %f %f %f has %u visible LEDs, error %f (%f / LED)\n",
-			    pose->orient.x, pose->orient.y, pose->orient.z, pose->orient.w, pose->pos.x, pose->pos.y,
-			    pose->pos.z, score.visible_leds, score.reprojection_error, error_per_led);
+			    pose->orientation.x, pose->orientation.y, pose->orientation.z, pose->orientation.w,
+			    pose->position.x, pose->position.y, pose->position.z, score.visible_leds,
+			    score.reprojection_error, error_per_led);
 			DEBUG("matched %u blobs of %u\n", score.matched_blobs, score.visible_leds);
 #endif
 		}
@@ -813,31 +798,9 @@ search_pose_for_model(struct correspondence_search *cs, struct cs_model_info *mi
 	return false;
 }
 
-bool
-correspondence_search_have_pose(struct correspondence_search *cs,
-                                int model_id,
-                                struct xrt_pose *pose,
-                                struct pose_metrics *score)
-{
-	int i;
-
-	for (i = 0; i < cs->num_models; i++) {
-		struct cs_model_info *mi = &cs->models[i];
-		if (mi->id != model_id)
-			continue;
-
-		if (mi->match_flags & POSE_MATCH_GOOD) {
-			*pose = mi->best_pose;
-			*score = mi->best_score;
-			return true;
-		}
-	}
-	return false;
-}
-
 bool
 correspondence_search_find_one_pose(struct correspondence_search *cs,
-                                    int model_id,
+                                    struct constellation_search_model *model,
                                     enum correspondence_search_flags search_flags,
                                     struct xrt_pose *pose,
                                     struct xrt_vec3 *pos_error_thresh,
@@ -846,8 +809,6 @@ correspondence_search_find_one_pose(struct correspondence_search *cs,
                                     float gravity_tolerance_rad,
                                     struct pose_metrics *score)
 {
-	int i;
-
 	assert(pose != NULL);
 	assert(score != NULL);
 
@@ -855,61 +816,53 @@ correspondence_search_find_one_pose(struct correspondence_search *cs,
 	if ((search_flags & (CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH)) == 0)
 		search_flags |= CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH;
 
-	for (i = 0; i < cs->num_models; i++) {
-		struct cs_model_info *mi = &cs->models[i];
-		if (mi->id != model_id)
-			continue;
+	struct cs_model_info mi;
 
-		mi->search_flags = search_flags;
-		mi->match_flags = 0;
+	mi.id = model->id;
+	mi.model = model;
+	mi.search_flags = search_flags;
+	mi.match_flags = 0;
 
-		if (search_flags & CS_FLAG_HAVE_POSE_PRIOR) {
-			assert(pos_error_thresh != NULL);
-			assert(rot_error_thresh != NULL);
+	if (search_flags & CS_FLAG_HAVE_POSE_PRIOR) {
+		assert(pos_error_thresh != NULL);
+		assert(rot_error_thresh != NULL);
 
-			mi->pose_prior = *pose;
-			mi->pos_error_thresh = pos_error_thresh;
-			mi->rot_error_thresh = rot_error_thresh;
-		}
-
-		if (search_flags & CS_FLAG_MATCH_GRAVITY) {
-			struct xrt_quat pose_gravity_twist;
-
-			/* We need a pose prior to extract the gravity swing to match */
-			assert((search_flags & CS_FLAG_HAVE_POSE_PRIOR) != 0);
-			assert(gravity_vector != NULL);
-
-			mi->gravity_vector = *gravity_vector;
-			mi->gravity_tolerance_rad = gravity_tolerance_rad;
-
-			math_quat_decompose_swing_twist(&pose->orientation, gravity_vector, &mi->gravity_swing,
-			                                &pose_gravity_twist);
-		}
-
-		if (search_pose_for_model(cs, mi) && (mi->match_flags & POSE_MATCH_GOOD)) {
-			*pose = mi->best_pose;
-			*score = mi->best_score;
-
-			DEBUG_TIMING("# Best %s match for model %d was %d points out of %d with error %f pixels^2\n",
-			             (search_flags & CS_FLAG_MATCH_GRAVITY) ? "aligned" : "unaligned", model_id,
-			             mi->best_score.matched_blobs, mi->best_score.visible_leds,
-			             mi->best_score.reprojection_error);
-			DEBUG_TIMING("# Found at LED depth %d blob depth %d after %f ms of %f ms\n",
-			             mi->best_pose_led_depth, mi->best_pose_blob_depth,
-			             (mi->best_pose_found_time - mi->search_start_time) * 1000.0,
-			             (os_monotonic_get_ns() - mi->search_start_time) * 1000.0);
-			DEBUG_TIMING("# pose orient %f %f %f %f pos %f %f %f\n", mi->best_pose.orient.x,
-			             mi->best_pose.orient.y, mi->best_pose.orient.z, mi->best_pose.orient.w,
-			             mi->best_pose.pos.x, mi->best_pose.pos.y, mi->best_pose.pos.z);
-			return true;
-		}
-
-		*pose = mi->best_pose;
-		*score = mi->best_score;
-		return false;
+		mi.pose_prior = *pose;
+		mi.pos_error_thresh = pos_error_thresh;
+		mi.rot_error_thresh = rot_error_thresh;
 	}
 
-	DEBUG("No model found for id %d!\n", model_id);
-	score->match_flags = 0;
+	if (search_flags & CS_FLAG_MATCH_GRAVITY) {
+		struct xrt_quat pose_gravity_twist;
+
+		/* We need a pose prior to extract the gravity swing to match */
+		assert((search_flags & CS_FLAG_HAVE_POSE_PRIOR) != 0);
+		assert(gravity_vector != NULL);
+
+		mi.gravity_vector = *gravity_vector;
+		mi.gravity_tolerance_rad = gravity_tolerance_rad;
+
+		math_quat_decompose_swing_twist(&pose->orientation, gravity_vector, &mi.gravity_swing,
+		                                &pose_gravity_twist);
+	}
+
+	if (search_pose_for_model(cs, &mi) && (mi.match_flags & POSE_MATCH_GOOD)) {
+		*pose = mi.best_pose;
+		*score = mi.best_score;
+
+		DEBUG_TIMING("# Best %s match for model %d was %d points out of %d with error %f pixels^2\n",
+		             (search_flags & CS_FLAG_MATCH_GRAVITY) ? "aligned" : "unaligned", mi.id,
+		             mi.best_score.matched_blobs, mi.best_score.visible_leds, mi.best_score.reprojection_error);
+		DEBUG_TIMING("# Found at LED depth %d blob depth %d after %f ms of %f ms\n", mi.best_pose_led_depth,
+		             mi.best_pose_blob_depth, (mi.best_pose_found_time - mi.search_start_time) * 1000.0,
+		             (os_monotonic_get_ns() - mi.search_start_time) * 1000.0);
+		DEBUG_TIMING("# pose orient %f %f %f %f pos %f %f %f\n", mi.best_pose.orientation.x,
+		             mi.best_pose.orientation.y, mi.best_pose.orientation.z, mi.best_pose.orientation.w,
+		             mi.best_pose.position.x, mi.best_pose.position.y, mi.best_pose.position.z);
+		return true;
+	}
+
+	*pose = mi.best_pose;
+	*score = mi.best_score;
 	return false;
 }

src/xrt/drivers/wmr/constellation/correspondence_search.h

@@ -17,8 +17,6 @@
 #include "led_models.h"
 #include "pose_metrics.h"
 
-#define CS_MAX_MODELS 3
-
 #define MAX_BLOB_SEARCH_DEPTH 5
 
 enum correspondence_search_flags
@@ -88,9 +86,6 @@ struct correspondence_search
 	struct cs_image_point *points;
 	struct blob *blobs; /* Original blobs structs [num_points] */
 
-	int num_models;
-	struct cs_model_info models[CS_MAX_MODELS];
-
 	unsigned int num_trials;
 	unsigned int num_pose_checks;
 
@@ -104,16 +99,13 @@ struct correspondence_search *
 correspondence_search_new(struct camera_model *camera_calib);
 void
 correspondence_search_set_blobs(struct correspondence_search *cs, struct blob *blobs, int num_blobs);
-bool
-correspondence_search_set_model(struct correspondence_search *cs,
-                                int model_id,
-                                struct constellation_search_model *model);
+
 void
 correspondence_search_free(struct correspondence_search *cs);
 
 bool
 correspondence_search_find_one_pose(struct correspondence_search *cs,
-                                    int model_id,
+                                    struct constellation_search_model *model,
                                     enum correspondence_search_flags search_flags,
                                     struct xrt_pose *pose,
                                     struct xrt_vec3 *pos_error_thresh,

src/xrt/drivers/wmr/constellation/led_models.c

@@ -16,8 +16,9 @@
 #endif
 
 void
-constellation_led_model_init(struct constellation_led_model *led_model, uint8_t num_leds)
+constellation_led_model_init(uint32_t device_id, struct constellation_led_model *led_model, uint8_t num_leds)
 {
+	led_model->id = device_id;
 	led_model->leds = calloc(num_leds, sizeof(struct constellation_led));
 	led_model->num_leds = num_leds;
 }
@@ -176,6 +177,7 @@ 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->id = led_model->id;
 	m->led_model = led_model;
 
 	m->points = calloc(led_model->num_leds, sizeof(struct constellation_search_led_candidate *));

src/xrt/drivers/wmr/constellation/led_models.h

@@ -29,12 +29,15 @@ struct constellation_led
 
 struct constellation_led_model
 {
+	// Device ID
+	uint32_t id;
+
 	struct constellation_led *leds;
 	uint8_t num_leds;
 };
 
 void
-constellation_led_model_init(struct constellation_led_model *led_model, uint8_t num_leds);
+constellation_led_model_init(uint32_t device_id, 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
@@ -59,6 +62,8 @@ constellation_search_led_candidate_free(struct constellation_search_led_candidat
 
 struct constellation_search_model
 {
+	uint32_t id; /* Device ID */
+
 	uint8_t num_points;
 	struct constellation_search_led_candidate **points;
 

src/xrt/drivers/wmr/wmr_controller_base.c

@@ -794,7 +794,7 @@ wmr_controller_base_get_led_model(struct wmr_controller_base *wcb, struct conste
 		return false;
 	}
 
-	constellation_led_model_init(led_model, wcb->config.led_count);
+	constellation_led_model_init((int)wcb->base.device_type, led_model, wcb->config.led_count);
 	for (int i = 0; i < wcb->config.led_count; i++) {
 		struct constellation_led *led = led_model->leds + i;