miniprojet/tests/src/math.hpp

#pragma once

#include <vector>
#include <complex>
#include <opencv2/opencv.hpp>
#include <iterator>

namespace math {

  using complex = std::complex<double>;
  using signal = std::vector<double>;
  using csignal = std::vector<complex>;
  using contour = std::vector<cv::Point>;
  constexpr double pi() {return std::atan(1)*4;}

  csignal cont2sig(const contour& cont) {
    csignal sig;
    auto sig_it = sig.begin();
    auto cont_it = cont.begin();

    for (auto cont_it = cont.begin(); cont_it != cont.end(); ++cont_it) {
      *(sig_it++) = complex((*cont_it).x, (*cont_it).y);
    }
    return sig;
  };

  complex mean(const csignal& sig) {
    complex res = 0;
    for (auto x: sig) {
      res += x;
    }
    return complex(res.real()/sig.size(), res.imag()/sig.size());
  };

  csignal diff(const csignal& input, complex mean) {
    csignal res;
    for (auto x: input) {
      res.push_back(x - mean);
    }
    return res;
  }

  csignal fft_rec(const csignal& input) {
    int size = input.size();

    if (size <= 1) {
      return input;
    } else {
      csignal odd;
      csignal even;
      auto odd_back_it = std::back_inserter(odd);
      auto even_back_it = std::back_inserter(even);
      bool insert_in_even = false;

      for (auto it = input.begin(); it != input.end(); ++it) {
        if (insert_in_even) {
          *(even_back_it++) = *it;
          insert_in_even = false;
        } else {
          *(odd_back_it++) = *it;
          insert_in_even = true;
        }
      }

      csignal odd_fft = fft_rec(odd);
      csignal even_fft = fft_rec(even);
      csignal res(size, complex());

      for (int k=0; k<size/2; ++k) {
        complex t = std::exp(complex(0, -2*pi()*k/size)) * odd_fft[k];
        res[k] = even_fft[k] + t;
        res[size/2+k] = even_fft[k] - t;
      }
      return res;
    }
  }

  csignal fft(const csignal& input, int N=0) {
    int opt_size;
    if (N < input.size()) {
      opt_size = 1 << (int)std::ceil(std::log(input.size())/std::log(2));
    } else {
      opt_size = 1 << (int)std::ceil(std::log(N)/std::log(2));
    }
    csignal sig(input);
    for (int i=0; i<opt_size-input.size(); ++i) {
      sig.push_back(complex(0, 0));
    }
    return fft_rec(sig);
  };

  contour coef2cont(const csignal& tfd, complex mean, int size, int cmax) {
    contour cont;
    auto tf_it = tfd.begin();
    auto cont_it = cont.begin();
    int kmin = tfd.size()/2 - cmax;
    int kmax = tfd.size()/2 + cmax;

    for (int m=0; m<tfd.size(); ++m) {
      complex sum;
      for (int k=kmin; k<kmax; ++k) {
        sum += tfd[k]*std::exp(complex(0, 2*pi()*k*m/tfd.size()));
      }
      complex zm = mean + sum;
      *(cont_it++) = cv::Point(zm.real(), zm.imag());
    }
    return cont;
  };

  contour simplify_contour(const contour& cont, int cmax) {
    contour res;
    csignal z = cont2sig(cont);
    complex zm = mean(z);
    csignal tfd = fft(diff(z, zm));
    return coef2cont(tfd, zm, 0, cmax);
  };

  int max_cont(const std::vector<contour>& contours) {
    int max = 0;
    int id = 0;
    for (int i=0; i<contours.size(); ++i) {
      if (contours[i].size() > max) {
        max = contours[i].size();
        id = i;
      }  
    }
    return id;
  };
}