PnP_AHRS.c File Reference

Functions for solving a perspective-n-point problem, using the AHRS to get the relevant angles. More...

#include "PnP_AHRS.h"
#include "state.h"
#include <stdio.h>

Include dependency graph for PnP_AHRS.c:

Go to the source code of this file.

Functions
struct FloatVect3	get_world_position_from_image_points (int *x_corners, int *y_corners, struct FloatVect3 *world_corners, int n_corners, struct camera_intrinsics_t cam_intrinsics, struct FloatEulers cam_body)
	Get the world position of the camera, given image coordinates and corresponding world coordinates. More...

Detailed Description

Functions for solving a perspective-n-point problem, using the AHRS to get the relevant angles.

Solves a perspective-n-point problem while using the attitude estimates of the AHRS. Using the AHRS makes the solution more robust than a purely maximal likelihood estimate from a standard PnP solver.

The module needs to get information on:

1. The real-world coordinates of the points.
2. The image coordinates of the points.
3. The camera parameters for undistortion purposes.
4. The rotation of the camera to the drone body.

Definition in file PnP_AHRS.c.

Function Documentation

struct FloatVect3 get_world_position_from_image_points	(	int *	x_corners,
		int *	y_corners,
		struct FloatVect3 *	world_corners,
		int	n_corners,
		struct camera_intrinsics_t	cam_intrinsics,
		struct FloatEulers	cam_body
	)

Get the world position of the camera, given image coordinates and corresponding world coordinates.

Parameters

[in]	x_corners	Image x-coordinates of the corners.
[in]	y_corners	Image y-coordinates of the corners.
[in]	world_corners	World coordinates of the corners. The order has to be the same as for the image coordinates.
[in]	n_corners	Number of corners.
[in]	cam_intrinsics	The camera intrinsics necessary for doing undistortion.
[in]	cam_body	The rotation from camera to body.
[out]	A	single struct FloatVect3 representing the camera world position.

Definition at line 54 of file PnP_AHRS.c.

References cam_body, distorted_pixels_to_normalized_coords(), FLOAT_MAT33_DIAG, FLOAT_MAT33_ZERO, float_rmat_of_eulers_321(), FLOAT_VECT3_ZERO, K, MAT33_COPY, MAT33_INV, MAT33_MAT33_DIFF, MAT33_MAT33_SUM, MAT33_TRANS, MAT33_VECT3_MUL, n_corners, FloatEulers::phi, FloatEulers::psi, stateGetNedToBodyEulers_f(), FloatEulers::theta, VECT3_NORM2, VECT3_SDIV, VECT3_SUM, VECT3_VECT3_TRANS_MUL, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by detect_gate_func().

Here is the call graph for this function:

Here is the caller graph for this function: