correlation.h

gehe zur Dokumentation dieser Datei

#ifndef CORRELATION_H
#define CORRELATION_H

#include "../fftwext.h"
#include "recognition.h"
#include "imagesource.h"
#include "../micro.h"
#include <iostream>

// TMP
extern "C"
{
    #include <opencv/highgui.h>
}

using namespace std;

namespace seemicro
{

struct CorrelationModule : public RecognitionModule
{
  Micro& micro;

  float correlationStrength;

  ImageSource& src;

  Micro *kparams;

  int arm_auch;

  IplImage *kernel_img;

  double *ffin, *kernelin, *ffback;
  fftw_plan plan, plan_kernel, planB;
  fftw_complex *ffout, *kernelout;

  CorrelationModule(Micro& m, ImageSource& As, Micro *Ak) :
   micro(m), src(As), kparams(Ak)
  {
    int w = src.img->width;
    int h = src.img->height;

    ffin     = (double*) fftw_malloc(w*h*sizeof(double));
    ffback   = (double*) fftw_malloc(w*h*sizeof(double));
    kernelin = (double*) fftw_malloc(w*h*sizeof(double));
    ffout = (fftw_complex*) fftw_malloc(w*(h/2+1)*sizeof(fftw_complex));
    kernelout = (fftw_complex*) fftw_malloc(w*(h/2+1)*sizeof(fftw_complex));

    kernel_img = cvCreateImage(cvSize(w,h), IPL_DEPTH_8U, 1);
    char wisdomfile[] = "wisdom.fftw3";
    FILE *f = fopen(wisdomfile, "r");
    if(f && fftw_import_wisdom_from_file(f)==1)
    {
      if(DEBUGLEVEL>=2) cerr << "Loaded wisdom from 'wisdom.fftw3'.\n";
      fclose(f);
    }
    else if(DEBUGLEVEL>=1) cerr << "Could not load wisdom from 'wisdom.fftw3'.\n";

    plan  = fftw_plan_dft_r2c_2d(w, h, (double*)ffin, (fftw_complex*)ffout, 0);
    planB = fftw_plan_dft_c2r_2d(w, h, (fftw_complex*)ffout, (double*)ffback, 0);
    f = fopen(wisdomfile, "w");
    if(f)
    {
      fftw_export_wisdom_to_file(f);
      fclose(f);
    }

    kparams->mitte.p.x = w/2;
    kparams->mitte.p.y = h/2;
    kparams->radius = w/4;
    kparams->alpha = 0;
    //kparams.beta = M_PI;
    micro.mitteValid = false;

    arm_auch = false;
    paramChanged = true;
  }

  void makeKernel(void)
  {
    if(!paramChanged) return;
    int w = src.img->width;
    int h = src.img->height;
    kparams->mitte.p.x = w/2;
    kparams->mitte.p.y = h/2;
#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "entered makeKernel():" << *kparams << endl;
#endif
    kparams->draw(kernel_img, 0, 0.0, 1, arm_auch);
    // TMP
    //cvSaveImage("vorlage.png", kernel_img);

    circular_shift(kernel_img, kernel_img, w/2, h/2);

    // TMP
    //cvSaveImage("vorlage-nach-cirular-shift.png", kernel_img);

    // mirror kernel image, because what we want is a
    // correlation, not just a convolution
    //cvFlip(kernel_img, NULL, -1);//obsolete by complex multiplication with conjugate complex

    // TMP
    //cvSaveImage("vorlage-nach-flip.png", kernel_img);

    IplImage2fftw_real(kernel_img, (double*)kernelin, w, h);
    plan_kernel = fftw_plan_dft_r2c_2d(w, h,
        (double*)kernelin, (fftw_complex*)kernelout, 0);
    fftw_execute(plan_kernel);
    fftw_destroy_plan(plan_kernel);
    micro.mitteValid = false;
    paramChanged = false;

#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "<-makeKernel()\n";
#endif
  }

  void makeKernel(Micro& p)
  {
    *kparams = p;
    paramChanged = true;
    makeKernel();
  }

  void processKeyFrame(void)
  {
#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "->correlate()\n";
#endif
    if(!src.outputChanged && !paramChanged) return;

    makeKernel();

    int w = src.img->width;
    int h = src.img->height;
      IplImage2fftw_real(src.img, (double*)ffin);
    micro.mitteValid = false;

    fftw_execute(plan);

    // do the actual convolution
    //fftw_complex_multiply((fftw_complex*)ffout, (fftw_complex*)kernelout,
//      w, h/2+1);
    fftw_complex_multiply_conjugate((fftw_complex*)ffout, (fftw_complex*)kernelout,
        w, h/2+1);

    fftw_execute(planB);

    // langsamere, alte variante, wenn mehrere maxima gesucht:
    //  maxlist_count = find_maxima(maxima_list, width, height, ffback,
    //      above_threshold);
    //  params.mitte.p.x = maxima_list[0].x;
    //  params.mitte.p.y = maxima_list[0].y;
    //  correlationStrength = maxima_list[0].v;

    // schnellere Variante, findet nur das globale Maximum;
    // die Suche könnte auf Bereich etwa 10% vom Rand nach innen
    // eingeschränkt werden unter der Annahme, daß das Rad
    // vollständig im Bild sein muß, um getrackt zu werden
    maximum max;
    findMaxPoint(w, h, ffback, 1.0, &max);
    if(max.x>=0 && max.y>=0)
    {
      micro.mitte.p.x = max.x;
      micro.mitte.p.y = max.y;
      micro.mitteValid = true;
      correlationStrength = max.v;
    }
    else
    {
      micro.mitteValid = false;
      if(DEBUGLEVEL>=2) cerr << "Correlation: no global max?\n";
    }

    micro.radius = kparams->radius;
    outputChanged = true;
#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << "<-correlate()\n";
#endif
  }

  void processFrame(myrect *ROI)
  {
  }

  ~CorrelationModule()
  {
    fftw_destroy_plan(plan);
    fftw_destroy_plan(planB);
    cvReleaseImage(&kernel_img);
    fftw_free(ffin); ffin = NULL;
    fftw_free(ffback); ffback = NULL;
    fftw_free(kernelin); kernelin = NULL;
    fftw_free(ffout); ffout = NULL;
    fftw_free(kernelout); kernelout = NULL;
  }

}; // CorrelationModule

} // seemicro

#endif

---