#include <math.h>
#include <stdio.h>
#include "common/debug_print.h"
#include "apriltag_pose.h"
#include "common/homography.h"

Include dependency graph for apriltag_pose.c:

Functions
matd_t *	calculate_F (matd_t *v)

void	estimate_pose_for_tag_homography (apriltag_detection_info_t info, apriltag_pose_t solution)

double	estimate_tag_pose (apriltag_detection_info_t info, apriltag_pose_t pose)

void	estimate_tag_pose_orthogonal_iteration (apriltag_detection_info_t info, double err1, apriltag_pose_t solution1, double err2, apriltag_pose_t *solution2, int nIters)

matd_t *	fix_pose_ambiguities (matd_t v, matd_t p, matd_t t, matd_t R, int n_points)

double	matd_to_double (matd_t *a)

double	orthogonal_iteration (matd_t v, matd_t p, matd_t t, matd_t R, int n_points, int n_steps)

double	polyval (double *p, int degree, double x)

void	solve_poly_approx (double p, int degree, double roots, int *n_roots)

Function Documentation

◆ calculate_F()

matd_t* calculate_F ( matd_t * v )

Calculate projection operator from image points.

Definition at line 12 of file apriltag_pose.c.

◆ estimate_pose_for_tag_homography()

void estimate_pose_for_tag_homography	(	apriltag_detection_info_t *	info,
		apriltag_pose_t *	solution
	)

Estimate pose of the tag using the homography method.

Definition at line 455 of file apriltag_pose.c.

◆ estimate_tag_pose()

double estimate_tag_pose	(	apriltag_detection_info_t *	info,
		apriltag_pose_t *	pose
	)

Estimate tag pose.

Definition at line 528 of file apriltag_pose.c.

◆ estimate_tag_pose_orthogonal_iteration()

void estimate_tag_pose_orthogonal_iteration	(	apriltag_detection_info_t *	info,
		double *	err1,
		apriltag_pose_t *	solution1,
		double *	err2,
		apriltag_pose_t *	solution2,
		int	nIters
	)

Estimate tag pose using orthogonal iteration.

Definition at line 490 of file apriltag_pose.c.

◆ fix_pose_ambiguities()

matd_t* fix_pose_ambiguities	(	matd_t **	v,
		matd_t **	p,
		matd_t *	t,
		matd_t *	R,
		int	n_points
	)

Given a local minima of the pose error tries to find the other minima.

Definition at line 259 of file apriltag_pose.c.

◆ matd_to_double()

double matd_to_double ( matd_t * a )

Returns the value of the supplied scalar matrix 'a' and destroys the matrix.

Definition at line 23 of file apriltag_pose.c.

◆ orthogonal_iteration()

double orthogonal_iteration	(	matd_t **	v,
		matd_t **	p,
		matd_t **	t,
		matd_t **	R,
		int	n_points,
		int	n_steps
	)

Parameters

v	Image points on the image plane.
p	Object points in object space. t Optimal translation.
R	In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation.
n_points	Number of points.
n_steps	Number of iterations.

Returns: Object-space error after iteration.

Implementation of Orthogonal Iteration from Lu, 2000.

Definition at line 43 of file apriltag_pose.c.

◆ polyval()

double polyval	(	double *	p,
		int	degree,
		double	x
	)

Evaluates polynomial p at x.

Definition at line 143 of file apriltag_pose.c.

◆ solve_poly_approx()

void solve_poly_approx	(	double *	p,
		int	degree,
		double *	roots,
		int *	n_roots
	)

Numerically solve small degree polynomials. This is a customized method. It ignores roots larger than 1000 and only gives small roots approximately.