correspondence_search.c File Reference

Ab-initio blob<->LED correspondence search. More...

#include "math/m_api.h"
#include "math/m_vec3.h"
#include "os/os_time.h"
#include "constellation/pose_metrics.h"
#include "lambdatwist/lambdatwist_p3p.h"
#include "correspondence_search.h"
#include "led_search_model.h"
#include "image_point_sort.h"
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <assert.h>
#include <math.h>

Include dependency graph for correspondence_search.c:

Macros
#define	CS_TRACE(cs, ...)   U_LOG_IFL_T((*((cs)->ct_log_level)), __VA_ARGS__)
#define	CS_DEBUG(cs, ...)   U_LOG_IFL_D((*((cs)->ct_log_level)), __VA_ARGS__)
#define	CS_INFO(cs, ...)   U_LOG_IFL_I((*((cs)->ct_log_level)), __VA_ARGS__)
#define	CS_WARN(cs, ...)   U_LOG_IFL_W((*((cs)->ct_log_level)), __VA_ARGS__)
#define	CS_ERROR(cs, ...)   U_LOG_IFL_E((*((cs)->ct_log_level)), __VA_ARGS__)
#define	DUMP_SCENE   0
#define	DUMP_BLOBS   0
#define	DUMP_FULL_DEBUG   0
#define	DUMP_FULL_LOG   0
#define	DUMP_TIMING   0
#define	CHECK_ALL_PROJECTIONS   0
#define	CS_DUMP_BLOBS(cs, ...)
#define	CS_FULL_DEBUG(cs, ...)
#define	CS_FULL_LOG(cs, ...)
#define	CS_TIMING(cs, ...)
#define	MAX_LED_SEARCH_DEPTH   8

Functions
static void	undistort_blob_points (struct t_blob *blobs, int num_blobs, struct xrt_vec2 *out_points, struct camera_model *calib)
static void	dump_pose (const struct correspondence_search *cs, const struct t_constellation_search_model *model, const struct xrt_pose *pose, const struct cs_model_info *mi)
static bool	correspondence_search_project_pose (struct correspondence_search *cs, const struct t_constellation_search_model *model, const struct xrt_pose *pose, struct cs_model_info *mi, int depth)
static void	quat_from_rotation_matrix (struct xrt_quat *me, double R[9])
static void	check_led_against_model_subset (struct correspondence_search *cs, struct cs_model_info *mi, struct cs_image_point **blobs, struct t_constellation_tracker_led *model_leds[4], int depth)
static void	select_k_blobs_from_n (struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_tracker_led **model_leds, struct cs_image_point **result_list, struct cs_image_point **output_list, struct cs_image_point **candidate_list, int k, int n, int depth)
	Select k entries from the n provided in candidate_list into output_list, then call check_led_match() with the result_list.
static void	check_leds_against_anchor (struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_tracker_led **model_leds, struct cs_image_point *anchor)
static void	check_led_match (struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_tracker_led **model_leds, int depth)
static void	select_k_leds_from_n (struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_tracker_led **result_list, struct t_constellation_tracker_led **output_list, struct t_constellation_tracker_led **candidate_list, int k, int n, int depth)
static void	generate_led_match_candidates (struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_search_led_candidate *c)
static bool	search_pose_for_model (struct correspondence_search *cs, struct cs_model_info *mi)
struct correspondence_search *	correspondence_search_new (const enum u_logging_level *ct_log_level, struct camera_model *camera_calib)
void	correspondence_search_free (struct correspondence_search *cs)
void	correspondence_search_set_blobs (struct correspondence_search *cs, struct t_blob *blobs, int num_blobs)
bool	correspondence_search_find_one_pose (struct correspondence_search *cs, const struct t_constellation_search_model *model, enum correspondence_search_flags search_flags, struct xrt_pose *inout_pose, const struct xrt_vec3 *pos_error_thresh, const struct xrt_vec3 *rot_error_thresh, const struct xrt_vec3 *gravity_vector, float gravity_tolerance_rad, struct pose_metrics *out_score)
	Does a brute-force search for a device's pose.

Detailed Description

Ab-initio blob<->LED correspondence search.

Author

Jan Schmidt jan@c.nosp@m.entr.nosp@m.icula.nosp@m.r.co.nosp@m.m

Beyley Cardellio ep1cm.nosp@m.1n10.nosp@m.n123@.nosp@m.gmai.nosp@m.l.com

Function Documentation

correspondence_search_find_one_pose()

bool correspondence_search_find_one_pose	(	struct correspondence_search *	cs,
		const struct t_constellation_search_model *	model,
		enum correspondence_search_flags	search_flags,
		struct xrt_pose *	inout_pose,
		const struct xrt_vec3 *	pos_error_thresh,
		const struct xrt_vec3 *	rot_error_thresh,
		const struct xrt_vec3 *	gravity_vector,
		float	gravity_tolerance_rad,
		struct pose_metrics *	out_score
	)

Does a brute-force search for a device's pose.

Note

The pose and score parameters are only written on success.

References CS_FLAG_DEEP_SEARCH, CS_FLAG_HAVE_POSE_PRIOR, CS_FLAG_MATCH_GRAVITY, CS_FLAG_SHALLOW_SEARCH, xrt_quat::math_quat_decompose_swing_twist(), os_monotonic_get_ns(), and POSE_MATCH_GOOD.

select_k_blobs_from_n()

static void select_k_blobs_from_n ( struct correspondence_search *  cs, struct cs_model_info *  mi, struct t_constellation_tracker_led **  model_leds, struct cs_image_point **  result_list, struct cs_image_point **  output_list, struct cs_image_point **  candidate_list, int  k, int  n, int  depth  )

static

Select k entries from the n provided in candidate_list into output_list, then call check_led_match() with the result_list.

References CS_FLAG_STOP_FOR_STRONG_MATCH, POSE_MATCH_STRONG, and select_k_blobs_from_n().

Referenced by select_k_blobs_from_n().