Engauge Digitizer  2
Correlation.cpp
1 #include "Correlation.h"
2 #include "EngaugeAssert.h"
3 #include "fftw3.h"
4 #include "Logger.h"
5 #include <QDebug>
6 #include <qmath.h>
7 
9  m_N (N),
10  m_signalA ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1))),
11  m_signalB ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1))),
12  m_outShifted ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1))),
13  m_outA ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1))),
14  m_outB ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1))),
15  m_out ((fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (2 * N - 1)))
16 {
17  m_planA = fftw_plan_dft_1d(2 * N - 1, m_signalA, m_outA, FFTW_FORWARD, FFTW_ESTIMATE);
18  m_planB = fftw_plan_dft_1d(2 * N - 1, m_signalB, m_outB, FFTW_FORWARD, FFTW_ESTIMATE);
19  m_planX = fftw_plan_dft_1d(2 * N - 1, m_out, m_outShifted, FFTW_BACKWARD, FFTW_ESTIMATE);
20 }
21 
22 Correlation::~Correlation()
23 {
24  fftw_destroy_plan(m_planA);
25  fftw_destroy_plan(m_planB);
26  fftw_destroy_plan(m_planX);
27 
28  fftw_free(m_signalA);
29  fftw_free(m_signalB);
30  fftw_free(m_outShifted);
31  fftw_free(m_out);
32  fftw_free(m_outA);
33  fftw_free(m_outB);
34 
35  fftw_cleanup();
36 }
37 
39  const double function1 [],
40  const double function2 [],
41  int &binStartMax,
42  double &corrMax,
43  double correlations []) const
44 {
45 // LOG4CPP_DEBUG_S ((*mainCat)) << "Correlation::correlateWithShift";
46 
47  int i;
48 
49  ENGAUGE_ASSERT (N == m_N);
50 
51  // Normalize input functions so that:
52  // 1) mean is zero. This is used to compute an additive normalization constant
53  // 2) max value is 1. This is used to compute a multiplicative normalization constant
54  double sumMean1 = 0, sumMean2 = 0, max1 = 0, max2 = 0;
55  for (i = 0; i < N; i++) {
56 
57  sumMean1 += function1 [i];
58  sumMean2 += function2 [i];
59  max1 = qMax (max1, function1 [i]);
60  max2 = qMax (max2, function2 [i]);
61 
62  }
63 
64  double additiveNormalization1 = sumMean1 / N;
65  double additiveNormalization2 = sumMean2 / N;
66  double multiplicativeNormalization1 = 1.0 / max1;
67  double multiplicativeNormalization2 = 1.0 / max2;
68 
69  // Load length N functions into length 2N+1 arrays, padding with zeros before for the first
70  // array, and with zeros after for the second array
71  for (i = 0; i < N - 1; i++) {
72 
73  m_signalA [i] [0] = 0.0;
74  m_signalA [i] [1] = 0.0;
75  m_signalB [i + N] [0] = 0.0;
76  m_signalB [i + N] [1] = 0.0;
77 
78  }
79  for (i = 0; i < N; i++) {
80 
81  m_signalA [i + N - 1] [0] = (function1 [i] - additiveNormalization1) * multiplicativeNormalization1;
82  m_signalA [i + N - 1] [1] = 0.0;
83  m_signalB [i] [0] = (function2 [i] - additiveNormalization2) * multiplicativeNormalization2;
84  m_signalB [i] [1] = 0.0;
85 
86  }
87 
88  fftw_execute(m_planA);
89  fftw_execute(m_planB);
90 
91  // Correlation in frequency space
92  fftw_complex scale = {1.0/(2.0 * N - 1.0), 0.0};
93  for (i = 0; i < 2 * N - 1; i++) {
94  // Multiple m_outA [i] * conj (m_outB) * scale
95  fftw_complex term1 = {m_outA [i] [0], m_outA [i] [1]};
96  fftw_complex term2 = {m_outB [i] [0], m_outB [i] [1] * -1.0};
97  fftw_complex term3 = {scale [0], scale [1]};
98  fftw_complex terms12 = {term1 [0] * term2 [0] - term1 [1] * term2 [1],
99  term1 [0] * term2 [1] + term1 [1] * term2 [0]};
100  m_out [i] [0] = terms12 [0] * term3 [0] - terms12 [1] * term3 [1];
101  m_out [i] [1] = terms12 [0] * term3 [1] + terms12 [1] * term3 [0];
102  }
103 
104  fftw_execute(m_planX);
105 
106  // Search for highest correlation. We have to account for the shift in the index. Specifically,
107  // 0 to N was mapped to the second half of the array that is 0 to 2 * N - 1
108  corrMax = 0.0;
109  for (int i0AtLeft = 0; i0AtLeft < N; i0AtLeft++) {
110 
111  int i0AtCenter = (i0AtLeft + N) % (2 * N - 1);
112  fftw_complex shifted = {m_outShifted [i0AtCenter] [0], m_outShifted [i0AtCenter] [1]};
113  double corr = qSqrt (shifted [0] * shifted [0] + shifted [1] * shifted [1]);
114 
115  if ((i0AtLeft == 0) || (corr > corrMax)) {
116  binStartMax = i0AtLeft;
117  corrMax = corr;
118  }
119 
120  // Save for, if enabled, external logging
121  correlations [i0AtLeft] = corr;
122  }
123 }
124 
126  const double function1 [],
127  const double function2 [],
128  double &corrMax) const
129 {
130 // LOG4CPP_DEBUG_S ((*mainCat)) << "Correlation::correlateWithoutShift";
131 
132  corrMax = 0.0;
133 
134  for (int i = 0; i < N; i++) {
135  corrMax += function1 [i] * function2 [i];
136  }
137 }
void correlateWithShift(int N, const double function1[], const double function2[], int &binStartMax, double &corrMax, double correlations[]) const
Return the shift in function1 that best aligns that function with function2.
Definition: Correlation.cpp:38
Correlation(int N)
Single constructor. Slow memory allocations are done once and then reused repeatedly.
Definition: Correlation.cpp:8
void correlateWithoutShift(int N, const double function1[], const double function2[], double &corrMax) const
Return the correlation of the two functions, without any shift.