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

#include "math/m_api.h"
#include "math/m_vec3.h"
#include "os/os_time.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 <stdio.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 (struct correspondence_search cs, struct t_constellation_search_model model, struct xrt_pose pose, struct cs_model_info mi)

static bool	correspondence_search_project_pose (struct correspondence_search cs, struct t_constellation_search_model model, 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, struct t_constellation_search_model model, enum correspondence_search_flags search_flags, struct xrt_pose pose, struct xrt_vec3 pos_error_thresh, struct xrt_vec3 rot_error_thresh, struct xrt_vec3 gravity_vector, float gravity_tolerance_rad, struct pose_metrics *score)

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

◆ 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().

Macros

Functions

Detailed Description

Function Documentation

◆ select_k_blobs_from_n()