undistortion.c

Go to the documentation of this file.

/*
 * Copyright (C) Guido de Croon, 2018
 *
 * This file is part of Paparazzi.
 *
 * Paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * Paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

// Own Header
#include "undistortion.h"
#include <math.h>

bool Dhane_distortion(float x_n, float y_n, float* x_nd, float* y_nd, float k) {
  float R = sqrtf(x_n*x_n + y_n*y_n);
  float r = tanf( asinf( (1.0f / k) * sinf( atanf( R ) ) ) );
  float reduction_factor = r/R;
  (*x_nd) = reduction_factor * x_n;
  (*y_nd) = reduction_factor * y_n;
  return true;
}

bool Dhane_undistortion(float x_nd, float y_nd, float* x_n, float* y_n, float k) {
  float r = sqrtf( x_nd*x_nd + y_nd*y_nd );
  float inner_part = sinf( atanf( r ) ) * k;
  // 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.
  if(fabs(inner_part) > 0.9999) {
    return false;
  }

  float R = tanf( asinf( inner_part ) );
  float enlargement_factor = R / r;
  (*x_n) = enlargement_factor * x_nd;
  (*y_n) = enlargement_factor * y_nd;

  return true;
}

void normalized_to_pixels(float x_n_, float y_n_, float* x_p, float* y_p, const float* K) {
  (*x_p) = x_n_ * K[0] + K[2];
  (*y_p) = y_n_ * K[4] + K[5];
}

void pixels_to_normalized(float x_p, float y_p, float* x_n_, float* y_n_, const float* K) {
  (*x_n_) = (x_p - K[2]) / K[0];
  (*y_n_) = (y_p - K[5]) / K[4];
}

bool distorted_pixels_to_normalized_coords(float x_pd, float y_pd, float* x_n, float* y_n, float k, const float* K) {
  float x_nd, y_nd;
  pixels_to_normalized(x_pd, y_pd, &x_nd, &y_nd, K);
  bool success = Dhane_undistortion(x_nd, y_nd, x_n, y_n, k);
  return success;
}

bool normalized_coords_to_distorted_pixels(float x_n, float y_n, float *x_pd, float *y_pd, float k, const float* K) {
  float x_nd, y_nd;
  bool success = Dhane_distortion(x_n, y_n, &x_nd, &y_nd, k);
  if(!success) {
    return false;
  }
  else {
    normalized_to_pixels(x_nd, y_nd, x_pd, y_pd, K);
  }
  return success;
}