Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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

◆ get_world_position_from_image_points()

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_cornersImage x-coordinates of the corners.
[in]y_cornersImage y-coordinates of the corners.
[in]world_cornersWorld coordinates of the corners. The order has to be the same as for the image coordinates.
[in]n_cornersNumber of corners.
[in]cam_intrinsicsThe camera intrinsics necessary for doing undistortion.
[in]cam_bodyThe rotation from camera to body.
[out]Asingle struct FloatVect3 representing the camera world position.

Definition at line 54 of file PnP_AHRS.c.

References attitude, 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, stateGetNedToBodyEulers_f(), FloatEulers::theta, VECT3_NORM2, VECT3_SDIV, VECT3_SUM, VECT3_VECT3_TRANS_MUL, world_corners, 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: