Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
PnP_AHRS.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Guido de Croon, 2018
3  *
4  * This file is part of Paparazzi.
5  *
6  * Paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * Paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with Paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
36 // Own Header
37 #include "PnP_AHRS.h"
38 #include "state.h"
39 #include <stdio.h>
40 
41 // #define DEBUG_PNP
42 
54 struct FloatVect3 get_world_position_from_image_points(int *x_corners, int *y_corners, struct FloatVect3 *world_corners,
55  int n_corners, struct camera_intrinsics_t cam_intrinsics, struct FloatEulers cam_body)
56 {
57 
58  // legend:
59  // E = Earth
60  // B = body
61  // C = camera
62 
63  struct FloatVect3 pos_drone_E_vec;
64  FLOAT_VECT3_ZERO(pos_drone_E_vec);
65 
66  // Make an identity matrix:
67  static struct FloatRMat I_mat;
68  FLOAT_MAT33_DIAG(I_mat, 1., 1., 1.);
69 
70  static struct FloatRMat Q_mat;
71  FLOAT_MAT33_ZERO(Q_mat);
72 
73  //camera to body rotation
74  struct FloatRMat R_E_B, R_B_E, R_C_B;
76 
77  // Use the AHRS roll and pitch from the filter to get the attitude in a rotation matrix R_E_B:
78  struct FloatEulers attitude;
79  attitude.phi = stateGetNedToBodyEulers_f()->phi;//positive ccw
80  attitude.theta = stateGetNedToBodyEulers_f()->theta;//negative downward
81  // local_psi typically assumed 0.0f (meaning you are straight in front of the object):
82  // If you want to use the estimate of psi, stateGetNedToBodyEulers_f()->psi, then you still need to take the psi of the gate into account.
83  float local_psi = 0.0f; // stateGetNedToBodyEulers_f()->psi; // 0.0f;
84  attitude.psi = local_psi;
86  MAT33_TRANS(R_B_E, R_E_B);
87 
88  // Camera calibration matrix:
89  float K[9] = {cam_intrinsics.focal_x, 0.0f, cam_intrinsics.center_x,
90  0.0f, cam_intrinsics.focal_y, cam_intrinsics.center_y,
91  0.0f, 0.0f, 1.0f
92  };
93 
94  // vectors in world coordinates, that will be "attached" to the world coordinates for the PnP:
95  struct FloatVect3 gate_vectors[4], vec_B, vec_E, p_vec, temp_vec;
96  FLOAT_VECT3_ZERO(p_vec);
97 
98  // Solution from: http://cal.cs.illinois.edu/~johannes/research/LS_line_intersect.pdf
99  // Equivalent MATLAB code:
100  //
101  // for i = 1:4
102  // Q = Q + (eye(3,3)-n(:,i)*n(:,i)');
103  // p = p + (eye(3,3)-n(:,i)*n(:,i)')*a(:,i);
104  //
105  // where n is the normalized vector, and a is the world coordinate
106 
107  struct FloatRMat temp_mat, temp_mat_2;
108  FLOAT_MAT33_ZERO(temp_mat);
109 
110  // Construct the matrix for linear least squares:
111  for (int i = 0; i < n_corners; i++) {
112 
113  // undistort the image coordinate and put it in a world vector:
114  float x_n, y_n;
115  bool success = distorted_pixels_to_normalized_coords((float) x_corners[i], (float) y_corners[i], &x_n, &y_n, cam_intrinsics.Dhane_k, K);
116  if(!success) {
117  printf("Undistortion not possible in PnPAHRS.c... why?\n");
118  return pos_drone_E_vec;
119  }
120 
121  gate_vectors[i].x = 1.0; // positive to the front
122  gate_vectors[i].y = y_n; // positive to the right
123  gate_vectors[i].z = -x_n; // positive down
124 
125  // transform the vector to the gate corner to earth coordinates:
126  MAT33_VECT3_MUL(vec_B, R_C_B, gate_vectors[i]);
127  double vec_norm = sqrt(VECT3_NORM2(vec_B));
128  VECT3_SDIV(vec_B, vec_B, vec_norm);
129  MAT33_VECT3_MUL(vec_E, R_B_E, vec_B);
130 
131 #ifdef DEBUG_PNP
132  printf("Determine world vector for corner %d\n", i);
133  printf("Distorted coordinates: (x,y) = (%d, %d)\n", x_corners[i], y_corners[i]);
134  printf("Normalized coordinates: (x_n, y_n) = (%f, %f)\n", x_n, y_n);
135  printf("Gate vector: (%f,%f,%f)\n", gate_vectors[i].x, gate_vectors[i].y, gate_vectors[i].z);
136  printf("Gate vector to Body: (%f,%f,%f)\n", vec_B.x, vec_B.y, vec_B.z);
137  printf("Gate vector to World: (%f,%f,%f)\n", vec_E.x, vec_E.y, vec_E.z);
138  printf("Euler angles body: (pitch, roll, yaw) = (%f, %f, %f)\n", attitude.theta, attitude.phi, attitude.psi);
139  printf("World corner %d: (%f, %f, %f)\n", i, world_corners[i].x, world_corners[i].y, world_corners[i].z);
140 #endif
141 
142  VECT3_VECT3_TRANS_MUL(temp_mat, vec_E, vec_E); // n(:,i)*n(:,i)'
143  MAT33_MAT33_DIFF(temp_mat, I_mat, temp_mat); // (eye(3,3)-n(:,i)*n(:,i)')
144  MAT33_COPY(temp_mat_2, Q_mat);
145  MAT33_MAT33_SUM(Q_mat, temp_mat_2, temp_mat); // Q + (eye(3,3)-n(:,i)*n(:,i)')
146  MAT33_VECT3_MUL(temp_vec, temp_mat, world_corners[i]); // (eye(3,3)-n(:,i)*n(:,i)')*a(:,i)
147  VECT3_SUM(p_vec, p_vec, temp_vec); // p + (eye(3,3)-n(:,i)*n(:,i)')*a(:,i);
148  }
149 
150  // Solve the linear system:
151  MAT33_INV(temp_mat, Q_mat);
152  MAT33_VECT3_MUL(pos_drone_E_vec, temp_mat, p_vec); // position = inv(Q) * p
153 
154 #if CAMERA_ROTATED_90DEG_RIGHT
155  // transformation of coordinates
156  float temp = pos_drone_E_vec.y;
157  pos_drone_E_vec.y = -pos_drone_E_vec.z;
158  pos_drone_E_vec.z = -temp;
159 #endif
160 
161  //bound y to remove outliers
162  float y_threshold = 4;
163  if (pos_drone_E_vec.y > y_threshold) { pos_drone_E_vec.y = y_threshold; }
164  else if (pos_drone_E_vec.y < -y_threshold) { pos_drone_E_vec.y = -y_threshold; }
165 
166  return pos_drone_E_vec;
167 }
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.
Definition: PnP_AHRS.c:54
Functions for solving a perspective-n-point problem, using the AHRS to get the relevant angles.
#define attitude
Definition: cc2500_compat.h:79
struct FloatVect3 world_corners[4]
Definition: detect_gate.c:106
struct FloatEulers cam_body
Definition: detect_gate.c:112
int n_corners
Definition: detect_gate.c:109
float phi
in radians
float theta
in radians
void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e)
Rotation matrix from 321 Euler angles (float).
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
#define FLOAT_MAT33_ZERO(_m)
euler angles
rotation matrix
#define VECT3_SUM(_c, _a, _b)
Definition: pprz_algebra.h:161
#define VECT3_VECT3_TRANS_MUL(_mat, _v_a, _v_b)
Definition: pprz_algebra.h:521
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
#define MAT33_MAT33_SUM(_mat1, _mat2, _mat3)
Definition: pprz_algebra.h:547
#define MAT33_INV(_minv, _m)
Definition: pprz_algebra.h:489
#define VECT3_NORM2(_v)
Definition: pprz_algebra.h:252
#define MAT33_TRANS(_mat1, _mat2)
Definition: pprz_algebra.h:560
#define MAT33_COPY(_mat1, _mat2)
Definition: pprz_algebra.h:438
#define MAT33_MAT33_DIFF(_mat1, _mat2, _mat3)
Definition: pprz_algebra.h:534
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
API to get/set the generic vehicle states.
static float K[9]
bool distorted_pixels_to_normalized_coords(float x_pd, float y_pd, float *x_n, float *y_n, float k, const float *K)
Transform distorted pixel coordinates to normalized coordinates.
Definition: undistortion.c:128