Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
undistortion.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 
43 // Own Header
44 #include "undistortion.h"
45 #include <math.h>
46 
57 bool Dhane_distortion(float x_n, float y_n, float* x_nd, float* y_nd, float k) {
58  float R = sqrtf(x_n*x_n + y_n*y_n);
59  float r = tanf( asinf( (1.0f / k) * sinf( atanf( R ) ) ) );
60  float reduction_factor = r/R;
61  (*x_nd) = reduction_factor * x_n;
62  (*y_nd) = reduction_factor * y_n;
63  return true;
64 }
65 
76 bool Dhane_undistortion(float x_nd, float y_nd, float* x_n, float* y_n, float k) {
77  float r = sqrtf( x_nd*x_nd + y_nd*y_nd );
78  float inner_part = sinf( atanf( r ) ) * k;
79  // we will take the asine of the inner part. It can happen that it is outside of [-1, 1], in which case, it would lead to an error.
80  if(fabs(inner_part) > 0.9999) {
81  return false;
82  }
83 
84  float R = tanf( asinf( inner_part ) );
85  float enlargement_factor = R / r;
86  (*x_n) = enlargement_factor * x_nd;
87  (*y_n) = enlargement_factor * y_nd;
88 
89  return true;
90 }
91 
100 void normalized_to_pixels(float x_n_, float y_n_, float* x_p, float* y_p, const float* K) {
101  (*x_p) = x_n_ * K[0] + K[2];
102  (*y_p) = y_n_ * K[4] + K[5];
103 }
104 
113 void pixels_to_normalized(float x_p, float y_p, float* x_n_, float* y_n_, const float* K) {
114  (*x_n_) = (x_p - K[2]) / K[0];
115  (*y_n_) = (y_p - K[5]) / K[4];
116 }
117 
128 bool distorted_pixels_to_normalized_coords(float x_pd, float y_pd, float* x_n, float* y_n, float k, const float* K) {
129  float x_nd, y_nd;
130  pixels_to_normalized(x_pd, y_pd, &x_nd, &y_nd, K);
131  bool success = Dhane_undistortion(x_nd, y_nd, x_n, y_n, k);
132  return success;
133 }
134 
146 bool normalized_coords_to_distorted_pixels(float x_n, float y_n, float *x_pd, float *y_pd, float k, const float* K) {
147  float x_nd, y_nd;
148  bool success = Dhane_distortion(x_n, y_n, &x_nd, &y_nd, k);
149  if(!success) {
150  return false;
151  }
152  else {
153  normalized_to_pixels(x_nd, y_nd, x_pd, y_pd, K);
154  }
155  return success;
156 }
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
void normalized_to_pixels(float x_n_, float y_n_, float *x_p, float *y_p, const float *K)
Transform normalized coordinates to pixel coordinates.
Definition: undistortion.c:100
bool Dhane_undistortion(float x_nd, float y_nd, float *x_n, float *y_n, float k)
Undistort distorted normalized image coordinates with the invertible Dhane method.
Definition: undistortion.c:76
bool normalized_coords_to_distorted_pixels(float x_n, float y_n, float *x_pd, float *y_pd, float k, const float *K)
Transform normalized coordinates to distorted pixel coordinates.
Definition: undistortion.c:146
bool Dhane_distortion(float x_n, float y_n, float *x_nd, float *y_nd, float k)
Distort normalized image coordinates with the invertible Dhane method.
Definition: undistortion.c:57
void pixels_to_normalized(float x_p, float y_p, float *x_n_, float *y_n_, const float *K)
Transform pixel coordinates to normalized coordinates.
Definition: undistortion.c:113
Functions for undistorting camera images.