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