pose_metrics.c File Reference

Metrics for constellation tracking poses. More...

#include "math/m_api.h"
#include "math/m_vec3.h"
#include "util/u_logging.h"
#include "tracking/t_camera_models.h"
#include "pose_metrics.h"
#include <assert.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>

Include dependency graph for pose_metrics.c:

Macros
#define	POSE_METRICS_SPEW   0
#define	LOG_SPEW(...)

Functions
static void	expand_rect (struct pose_rect *bounds, double x, double y, double w, double h)
static int	find_best_matching_led (struct pose_metrics_visible_led_info *led_points, int num_leds, struct t_blob *blob, double *out_sqerror)
static void	check_pose_prior (struct pose_metrics *score, const struct xrt_pose *pose, const struct xrt_pose *pose_prior, const struct xrt_vec3 *pos_error_thresh, const struct xrt_vec3 *rot_error_thresh)
static bool	project_led_points (struct t_constellation_tracker_led_model *led_model, struct camera_model *calib, const struct xrt_pose *pose, struct xrt_vec3 *out_positions, struct xrt_vec2 *out_points)
static bool	get_visible_leds (const struct xrt_pose *T_cam_obj, struct t_constellation_tracker_led_model *led_model, struct camera_model *calib, struct pose_metrics_visible_led_info *visible_led_points, int *num_visible_leds)
static bool	get_visible_leds_bounds (struct pose_metrics_visible_led_info *visible_led_points, int num_visible_leds, struct pose_rect *bounds)
bool	pose_metrics_match_pose_to_blobs (const struct xrt_pose *pose, struct t_blob *blobs, int num_blobs, struct t_constellation_tracker_led_model *led_model, t_constellation_device_id_t device_id, struct camera_model *calib, struct pose_metrics_blob_match_info *match_info)
bool	pose_metrics_evaluate_pose (struct pose_metrics *score, const struct xrt_pose *pose, struct t_blob *blobs, int num_blobs, struct t_constellation_tracker_led_model *led_model, t_constellation_device_id_t device_id, struct camera_model *calib, struct pose_rect *out_bounds)
bool	pose_metrics_evaluate_pose_with_prior (struct pose_metrics *score, const struct xrt_pose *pose, bool prior_must_match, const struct xrt_pose *pose_prior, const struct xrt_vec3 *pos_error_thresh, const struct xrt_vec3 *rot_error_thresh, struct t_blob *blobs, int num_blobs, struct t_constellation_tracker_led_model *led_model, t_constellation_device_id_t device_id, struct camera_model *calib, struct pose_rect *out_bounds)
bool	pose_metrics_score_is_better_pose (struct pose_metrics *old_score, struct pose_metrics *new_score)
	Compares whether new_score is a better pose than old_score.

Detailed Description

Metrics for constellation tracking poses.

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

get_visible_leds()

static bool get_visible_leds ( const struct xrt_pose *  T_cam_obj, struct t_constellation_tracker_led_model *  led_model, struct camera_model *  calib, struct pose_metrics_visible_led_info *  visible_led_points, int *  num_visible_leds  )

static

Todo:

We should just drop single individual LEDs that are outside screen space, not the whole pose.

References camera_model::bottom_pinhole, camera_model::calib_pinhole, t_constellation_tracker_led_model::compute_led_visibility, t_constellation_tracker_led::id, t_constellation_tracker_led_model::led_count, t_constellation_tracker_led_model::leds, camera_model::left_pinhole, xrt_pose::math_pose_invert(), xrt_vec3::math_vec3_normalize(), MAX, t_constellation_tracker_led::normal, t_constellation_tracker_led::position, t_constellation_tracker_led::radius_m, camera_model::right_pinhole, camera_model::top_pinhole, and t_constellation_tracker_led::visibility_angle.

pose_metrics_score_is_better_pose()

bool pose_metrics_score_is_better_pose	(	struct pose_metrics *	old_score,
		struct pose_metrics *	new_score
	)

Compares whether new_score is a better pose than old_score.

Parameters

old_score	The old score to compare against.
new_score	The new score to compare against the old score.

Returns

true if the new score is a better pose than the old score, false otherwise.

References pose_metrics::orient_error, POSE_HAD_PRIOR, POSE_MATCH_DEGENERATE, POSE_MATCH_GOOD, and POSE_MATCH_STRONG.