annotate data/model/FFTModel.cpp @ 1136:e94719f941ba tony-2.0-integration

Return maximum through getNormalizedMagnitudesAt to avoid having to make more than one call
author Chris Cannam
date Tue, 20 Oct 2015 12:54:06 +0100
parents e994747fb9dd
children 444d133b5ab7
rev   line source
Chris@152 1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*- vi:set ts=8 sts=4 sw=4: */
Chris@152 2
Chris@152 3 /*
Chris@152 4 Sonic Visualiser
Chris@152 5 An audio file viewer and annotation editor.
Chris@152 6 Centre for Digital Music, Queen Mary, University of London.
Chris@152 7 This file copyright 2006 Chris Cannam.
Chris@152 8
Chris@152 9 This program is free software; you can redistribute it and/or
Chris@152 10 modify it under the terms of the GNU General Public License as
Chris@152 11 published by the Free Software Foundation; either version 2 of the
Chris@152 12 License, or (at your option) any later version. See the file
Chris@152 13 COPYING included with this distribution for more information.
Chris@152 14 */
Chris@152 15
Chris@152 16 #include "FFTModel.h"
Chris@152 17 #include "DenseTimeValueModel.h"
Chris@152 18
Chris@183 19 #include "base/Profiler.h"
Chris@275 20 #include "base/Pitch.h"
Chris@183 21
Chris@402 22 #include <algorithm>
Chris@402 23
Chris@152 24 #include <cassert>
Chris@1090 25 #include <deque>
Chris@152 26
Chris@608 27 #ifndef __GNUC__
Chris@608 28 #include <alloca.h>
Chris@608 29 #endif
Chris@608 30
Chris@1090 31 using namespace std;
Chris@1090 32
Chris@152 33 FFTModel::FFTModel(const DenseTimeValueModel *model,
Chris@152 34 int channel,
Chris@152 35 WindowType windowType,
Chris@929 36 int windowSize,
Chris@929 37 int windowIncrement,
Chris@1090 38 int fftSize) :
Chris@1090 39 m_model(model),
Chris@1090 40 m_channel(channel),
Chris@1090 41 m_windowType(windowType),
Chris@1090 42 m_windowSize(windowSize),
Chris@1090 43 m_windowIncrement(windowIncrement),
Chris@1090 44 m_fftSize(fftSize),
Chris@1091 45 m_windower(windowType, windowSize),
Chris@1093 46 m_fft(fftSize),
Chris@1093 47 m_cacheSize(3)
Chris@152 48 {
Chris@1091 49 if (m_windowSize > m_fftSize) {
Chris@1091 50 cerr << "ERROR: FFTModel::FFTModel: window size (" << m_windowSize
Chris@1091 51 << ") must be at least FFT size (" << m_fftSize << ")" << endl;
Chris@1091 52 throw invalid_argument("FFTModel window size must be at least FFT size");
Chris@1091 53 }
Chris@1133 54
Chris@1133 55 connect(model, SIGNAL(modelChanged()), this, SIGNAL(modelChanged()));
Chris@1133 56 connect(model, SIGNAL(modelChangedWithin(sv_frame_t, sv_frame_t)),
Chris@1133 57 this, SIGNAL(modelChangedWithin(sv_frame_t, sv_frame_t)));
Chris@152 58 }
Chris@152 59
Chris@152 60 FFTModel::~FFTModel()
Chris@152 61 {
Chris@152 62 }
Chris@152 63
Chris@360 64 void
Chris@360 65 FFTModel::sourceModelAboutToBeDeleted()
Chris@360 66 {
Chris@1090 67 if (m_model) {
Chris@1090 68 cerr << "FFTModel[" << this << "]::sourceModelAboutToBeDeleted(" << m_model << ")" << endl;
Chris@1090 69 m_model = 0;
Chris@360 70 }
Chris@360 71 }
Chris@360 72
Chris@1091 73 int
Chris@1091 74 FFTModel::getWidth() const
Chris@1091 75 {
Chris@1091 76 if (!m_model) return 0;
Chris@1091 77 return int((m_model->getEndFrame() - m_model->getStartFrame())
Chris@1091 78 / m_windowIncrement) + 1;
Chris@1091 79 }
Chris@1091 80
Chris@1091 81 int
Chris@1091 82 FFTModel::getHeight() const
Chris@1091 83 {
Chris@1091 84 return m_fftSize / 2 + 1;
Chris@1091 85 }
Chris@1091 86
Chris@152 87 QString
Chris@929 88 FFTModel::getBinName(int n) const
Chris@152 89 {
Chris@1040 90 sv_samplerate_t sr = getSampleRate();
Chris@152 91 if (!sr) return "";
Chris@204 92 QString name = tr("%1 Hz").arg((n * sr) / ((getHeight()-1) * 2));
Chris@152 93 return name;
Chris@152 94 }
Chris@152 95
Chris@1091 96 FFTModel::Column
Chris@1091 97 FFTModel::getColumn(int x) const
Chris@1091 98 {
Chris@1091 99 auto cplx = getFFTColumn(x);
Chris@1091 100 Column col;
Chris@1091 101 col.reserve(int(cplx.size()));
Chris@1091 102 for (auto c: cplx) col.push_back(abs(c));
Chris@1091 103 return col;
Chris@1091 104 }
Chris@1091 105
Chris@1091 106 float
Chris@1091 107 FFTModel::getMagnitudeAt(int x, int y) const
Chris@1091 108 {
Chris@1093 109 if (x < 0 || x >= getWidth() || y < 0 || y >= getHeight()) return 0.f;
Chris@1093 110 auto col = getFFTColumn(x);
Chris@1093 111 return abs(col[y]);
Chris@1091 112 }
Chris@1091 113
Chris@1091 114 float
Chris@1091 115 FFTModel::getMaximumMagnitudeAt(int x) const
Chris@1091 116 {
Chris@1091 117 Column col(getColumn(x));
Chris@1092 118 float max = 0.f;
Chris@1092 119 for (int i = 0; i < col.size(); ++i) {
Chris@1092 120 if (col[i] > max) max = col[i];
Chris@1092 121 }
Chris@1092 122 return max;
Chris@1091 123 }
Chris@1091 124
Chris@1091 125 float
Chris@1091 126 FFTModel::getPhaseAt(int x, int y) const
Chris@1091 127 {
Chris@1093 128 if (x < 0 || x >= getWidth() || y < 0 || y >= getHeight()) return 0.f;
Chris@1091 129 return arg(getFFTColumn(x)[y]);
Chris@1091 130 }
Chris@1091 131
Chris@1091 132 void
Chris@1091 133 FFTModel::getValuesAt(int x, int y, float &re, float &im) const
Chris@1091 134 {
Chris@1091 135 auto col = getFFTColumn(x);
Chris@1091 136 re = col[y].real();
Chris@1091 137 im = col[y].imag();
Chris@1091 138 }
Chris@1091 139
Chris@1091 140 bool
Chris@1093 141 FFTModel::isColumnAvailable(int) const
Chris@1091 142 {
Chris@1091 143 //!!!
Chris@1091 144 return true;
Chris@1091 145 }
Chris@1091 146
Chris@1091 147 bool
Chris@1091 148 FFTModel::getMagnitudesAt(int x, float *values, int minbin, int count) const
Chris@1091 149 {
Chris@1091 150 if (count == 0) count = getHeight();
Chris@1091 151 auto col = getFFTColumn(x);
Chris@1091 152 for (int i = 0; i < count; ++i) {
Chris@1091 153 values[i] = abs(col[minbin + i]);
Chris@1091 154 }
Chris@1091 155 return true;
Chris@1091 156 }
Chris@1091 157
Chris@1136 158 float
Chris@1091 159 FFTModel::getNormalizedMagnitudesAt(int x, float *values, int minbin, int count) const
Chris@1091 160 {
Chris@1092 161 if (!getMagnitudesAt(x, values, minbin, count)) return false;
Chris@1092 162 if (count == 0) count = getHeight();
Chris@1092 163 float max = 0.f;
Chris@1092 164 for (int i = 0; i < count; ++i) {
Chris@1092 165 if (values[i] > max) max = values[i];
Chris@1092 166 }
Chris@1092 167 if (max > 0.f) {
Chris@1092 168 for (int i = 0; i < count; ++i) {
Chris@1092 169 values[i] /= max;
Chris@1092 170 }
Chris@1092 171 }
Chris@1136 172 return max;
Chris@1091 173 }
Chris@1091 174
Chris@1091 175 bool
Chris@1091 176 FFTModel::getPhasesAt(int x, float *values, int minbin, int count) const
Chris@1091 177 {
Chris@1091 178 if (count == 0) count = getHeight();
Chris@1091 179 auto col = getFFTColumn(x);
Chris@1091 180 for (int i = 0; i < count; ++i) {
Chris@1091 181 values[i] = arg(col[minbin + i]);
Chris@1091 182 }
Chris@1091 183 return true;
Chris@1091 184 }
Chris@1091 185
Chris@1091 186 bool
Chris@1091 187 FFTModel::getValuesAt(int x, float *reals, float *imags, int minbin, int count) const
Chris@1091 188 {
Chris@1091 189 if (count == 0) count = getHeight();
Chris@1091 190 auto col = getFFTColumn(x);
Chris@1091 191 for (int i = 0; i < count; ++i) {
Chris@1091 192 reals[i] = col[minbin + i].real();
Chris@1091 193 }
Chris@1091 194 for (int i = 0; i < count; ++i) {
Chris@1091 195 imags[i] = col[minbin + i].imag();
Chris@1091 196 }
Chris@1091 197 return true;
Chris@1091 198 }
Chris@1091 199
Chris@1091 200 vector<float>
Chris@1091 201 FFTModel::getSourceSamples(int column) const
Chris@1091 202 {
Chris@1094 203 // m_fftSize may be greater than m_windowSize, but not the reverse
Chris@1094 204
Chris@1094 205 // cerr << "getSourceSamples(" << column << ")" << endl;
Chris@1094 206
Chris@1091 207 auto range = getSourceSampleRange(column);
Chris@1094 208 auto data = getSourceData(range);
Chris@1094 209
Chris@1091 210 int off = (m_fftSize - m_windowSize) / 2;
Chris@1094 211
Chris@1094 212 if (off == 0) {
Chris@1094 213 return data;
Chris@1094 214 } else {
Chris@1094 215 vector<float> pad(off, 0.f);
Chris@1094 216 vector<float> padded;
Chris@1094 217 padded.reserve(m_fftSize);
Chris@1094 218 padded.insert(padded.end(), pad.begin(), pad.end());
Chris@1094 219 padded.insert(padded.end(), data.begin(), data.end());
Chris@1094 220 padded.insert(padded.end(), pad.begin(), pad.end());
Chris@1094 221 return padded;
Chris@1094 222 }
Chris@1094 223 }
Chris@1094 224
Chris@1094 225 vector<float>
Chris@1094 226 FFTModel::getSourceData(pair<sv_frame_t, sv_frame_t> range) const
Chris@1094 227 {
Chris@1094 228 // cerr << "getSourceData(" << range.first << "," << range.second
Chris@1094 229 // << "): saved range is (" << m_savedData.range.first
Chris@1094 230 // << "," << m_savedData.range.second << ")" << endl;
Chris@1094 231
Chris@1100 232 if (m_savedData.range == range) {
Chris@1100 233 return m_savedData.data;
Chris@1100 234 }
Chris@1094 235
Chris@1094 236 if (range.first < m_savedData.range.second &&
Chris@1094 237 range.first >= m_savedData.range.first &&
Chris@1094 238 range.second > m_savedData.range.second) {
Chris@1094 239
Chris@1100 240 sv_frame_t discard = range.first - m_savedData.range.first;
Chris@1100 241
Chris@1100 242 vector<float> acc(m_savedData.data.begin() + discard,
Chris@1100 243 m_savedData.data.end());
Chris@1094 244
Chris@1095 245 vector<float> rest =
Chris@1095 246 getSourceDataUncached({ m_savedData.range.second, range.second });
Chris@1100 247
Chris@1100 248 acc.insert(acc.end(), rest.begin(), rest.end());
Chris@1094 249
Chris@1095 250 m_savedData = { range, acc };
Chris@1095 251 return acc;
Chris@1095 252
Chris@1095 253 } else {
Chris@1095 254
Chris@1095 255 auto data = getSourceDataUncached(range);
Chris@1095 256 m_savedData = { range, data };
Chris@1095 257 return data;
Chris@1094 258 }
Chris@1095 259 }
Chris@1094 260
Chris@1095 261 vector<float>
Chris@1095 262 FFTModel::getSourceDataUncached(pair<sv_frame_t, sv_frame_t> range) const
Chris@1095 263 {
Chris@1091 264 decltype(range.first) pfx = 0;
Chris@1091 265 if (range.first < 0) {
Chris@1091 266 pfx = -range.first;
Chris@1091 267 range = { 0, range.second };
Chris@1091 268 }
Chris@1096 269
Chris@1096 270 auto data = m_model->getData(m_channel,
Chris@1096 271 range.first,
Chris@1096 272 range.second - range.first);
Chris@1096 273
Chris@1096 274 // don't return a partial frame
Chris@1096 275 data.resize(range.second - range.first, 0.f);
Chris@1096 276
Chris@1096 277 if (pfx > 0) {
Chris@1096 278 vector<float> pad(pfx, 0.f);
Chris@1096 279 data.insert(data.begin(), pad.begin(), pad.end());
Chris@1096 280 }
Chris@1096 281
Chris@1091 282 if (m_channel == -1) {
Chris@1091 283 int channels = m_model->getChannelCount();
Chris@1091 284 if (channels > 1) {
Chris@1096 285 int n = int(data.size());
Chris@1096 286 float factor = 1.f / float(channels);
Chris@1100 287 // use mean instead of sum for fft model input
Chris@1096 288 for (int i = 0; i < n; ++i) {
Chris@1096 289 data[i] *= factor;
Chris@1091 290 }
Chris@1091 291 }
Chris@1091 292 }
Chris@1094 293
Chris@1094 294 return data;
Chris@1091 295 }
Chris@1091 296
Chris@1091 297 vector<complex<float>>
Chris@1093 298 FFTModel::getFFTColumn(int n) const
Chris@1091 299 {
Chris@1093 300 for (auto &incache : m_cached) {
Chris@1093 301 if (incache.n == n) {
Chris@1093 302 return incache.col;
Chris@1093 303 }
Chris@1093 304 }
Chris@1093 305
Chris@1093 306 auto samples = getSourceSamples(n);
Chris@1100 307 m_windower.cut(samples.data());
Chris@1093 308 auto col = m_fft.process(samples);
Chris@1093 309
Chris@1093 310 SavedColumn sc { n, col };
Chris@1093 311 if (m_cached.size() >= m_cacheSize) {
Chris@1093 312 m_cached.pop_front();
Chris@1093 313 }
Chris@1093 314 m_cached.push_back(sc);
Chris@1093 315
Chris@1093 316 return col;
Chris@1091 317 }
Chris@1091 318
Chris@275 319 bool
Chris@1045 320 FFTModel::estimateStableFrequency(int x, int y, double &frequency)
Chris@275 321 {
Chris@275 322 if (!isOK()) return false;
Chris@275 323
Chris@1090 324 frequency = double(y * getSampleRate()) / m_fftSize;
Chris@275 325
Chris@275 326 if (x+1 >= getWidth()) return false;
Chris@275 327
Chris@275 328 // At frequency f, a phase shift of 2pi (one cycle) happens in 1/f sec.
Chris@275 329 // At hopsize h and sample rate sr, one hop happens in h/sr sec.
Chris@275 330 // At window size w, for bin b, f is b*sr/w.
Chris@275 331 // thus 2pi phase shift happens in w/(b*sr) sec.
Chris@275 332 // We need to know what phase shift we expect from h/sr sec.
Chris@275 333 // -> 2pi * ((h/sr) / (w/(b*sr)))
Chris@275 334 // = 2pi * ((h * b * sr) / (w * sr))
Chris@275 335 // = 2pi * (h * b) / w.
Chris@275 336
Chris@1038 337 double oldPhase = getPhaseAt(x, y);
Chris@1038 338 double newPhase = getPhaseAt(x+1, y);
Chris@275 339
Chris@929 340 int incr = getResolution();
Chris@275 341
Chris@1090 342 double expectedPhase = oldPhase + (2.0 * M_PI * y * incr) / m_fftSize;
Chris@275 343
Chris@1038 344 double phaseError = princarg(newPhase - expectedPhase);
Chris@275 345
Chris@275 346 // The new frequency estimate based on the phase error resulting
Chris@275 347 // from assuming the "native" frequency of this bin
Chris@275 348
Chris@275 349 frequency =
Chris@1090 350 (getSampleRate() * (expectedPhase + phaseError - oldPhase)) /
Chris@1045 351 (2.0 * M_PI * incr);
Chris@275 352
Chris@275 353 return true;
Chris@275 354 }
Chris@275 355
Chris@275 356 FFTModel::PeakLocationSet
Chris@929 357 FFTModel::getPeaks(PeakPickType type, int x, int ymin, int ymax)
Chris@275 358 {
Chris@551 359 Profiler profiler("FFTModel::getPeaks");
Chris@551 360
Chris@275 361 FFTModel::PeakLocationSet peaks;
Chris@275 362 if (!isOK()) return peaks;
Chris@275 363
Chris@275 364 if (ymax == 0 || ymax > getHeight() - 1) {
Chris@275 365 ymax = getHeight() - 1;
Chris@275 366 }
Chris@275 367
Chris@275 368 if (type == AllPeaks) {
Chris@551 369 int minbin = ymin;
Chris@551 370 if (minbin > 0) minbin = minbin - 1;
Chris@551 371 int maxbin = ymax;
Chris@551 372 if (maxbin < getHeight() - 1) maxbin = maxbin + 1;
Chris@551 373 const int n = maxbin - minbin + 1;
Chris@608 374 #ifdef __GNUC__
Chris@551 375 float values[n];
Chris@608 376 #else
Chris@608 377 float *values = (float *)alloca(n * sizeof(float));
Chris@608 378 #endif
Chris@551 379 getMagnitudesAt(x, values, minbin, maxbin - minbin + 1);
Chris@929 380 for (int bin = ymin; bin <= ymax; ++bin) {
Chris@551 381 if (bin == minbin || bin == maxbin) continue;
Chris@551 382 if (values[bin - minbin] > values[bin - minbin - 1] &&
Chris@551 383 values[bin - minbin] > values[bin - minbin + 1]) {
Chris@275 384 peaks.insert(bin);
Chris@275 385 }
Chris@275 386 }
Chris@275 387 return peaks;
Chris@275 388 }
Chris@275 389
Chris@551 390 Column values = getColumn(x);
Chris@275 391
Chris@500 392 float mean = 0.f;
Chris@551 393 for (int i = 0; i < values.size(); ++i) mean += values[i];
Chris@1038 394 if (values.size() > 0) mean = mean / float(values.size());
Chris@1038 395
Chris@275 396 // For peak picking we use a moving median window, picking the
Chris@275 397 // highest value within each continuous region of values that
Chris@275 398 // exceed the median. For pitch adaptivity, we adjust the window
Chris@275 399 // size to a roughly constant pitch range (about four tones).
Chris@275 400
Chris@1040 401 sv_samplerate_t sampleRate = getSampleRate();
Chris@275 402
Chris@1090 403 deque<float> window;
Chris@1090 404 vector<int> inrange;
Chris@280 405 float dist = 0.5;
Chris@500 406
Chris@929 407 int medianWinSize = getPeakPickWindowSize(type, sampleRate, ymin, dist);
Chris@929 408 int halfWin = medianWinSize/2;
Chris@275 409
Chris@929 410 int binmin;
Chris@275 411 if (ymin > halfWin) binmin = ymin - halfWin;
Chris@275 412 else binmin = 0;
Chris@275 413
Chris@929 414 int binmax;
Chris@275 415 if (ymax + halfWin < values.size()) binmax = ymax + halfWin;
Chris@275 416 else binmax = values.size()-1;
Chris@275 417
Chris@929 418 int prevcentre = 0;
Chris@500 419
Chris@929 420 for (int bin = binmin; bin <= binmax; ++bin) {
Chris@275 421
Chris@275 422 float value = values[bin];
Chris@275 423
Chris@275 424 window.push_back(value);
Chris@275 425
Chris@280 426 // so-called median will actually be the dist*100'th percentile
Chris@280 427 medianWinSize = getPeakPickWindowSize(type, sampleRate, bin, dist);
Chris@275 428 halfWin = medianWinSize/2;
Chris@275 429
Chris@929 430 while ((int)window.size() > medianWinSize) {
Chris@500 431 window.pop_front();
Chris@500 432 }
Chris@500 433
Chris@1038 434 int actualSize = int(window.size());
Chris@275 435
Chris@275 436 if (type == MajorPitchAdaptivePeaks) {
Chris@275 437 if (ymax + halfWin < values.size()) binmax = ymax + halfWin;
Chris@275 438 else binmax = values.size()-1;
Chris@275 439 }
Chris@275 440
Chris@1090 441 deque<float> sorted(window);
Chris@1090 442 sort(sorted.begin(), sorted.end());
Chris@1038 443 float median = sorted[int(float(sorted.size()) * dist)];
Chris@275 444
Chris@929 445 int centrebin = 0;
Chris@500 446 if (bin > actualSize/2) centrebin = bin - actualSize/2;
Chris@500 447
Chris@500 448 while (centrebin > prevcentre || bin == binmin) {
Chris@275 449
Chris@500 450 if (centrebin > prevcentre) ++prevcentre;
Chris@500 451
Chris@500 452 float centre = values[prevcentre];
Chris@500 453
Chris@500 454 if (centre > median) {
Chris@500 455 inrange.push_back(centrebin);
Chris@500 456 }
Chris@500 457
Chris@500 458 if (centre <= median || centrebin+1 == values.size()) {
Chris@500 459 if (!inrange.empty()) {
Chris@929 460 int peakbin = 0;
Chris@500 461 float peakval = 0.f;
Chris@929 462 for (int i = 0; i < (int)inrange.size(); ++i) {
Chris@500 463 if (i == 0 || values[inrange[i]] > peakval) {
Chris@500 464 peakval = values[inrange[i]];
Chris@500 465 peakbin = inrange[i];
Chris@500 466 }
Chris@500 467 }
Chris@500 468 inrange.clear();
Chris@500 469 if (peakbin >= ymin && peakbin <= ymax) {
Chris@500 470 peaks.insert(peakbin);
Chris@275 471 }
Chris@275 472 }
Chris@275 473 }
Chris@500 474
Chris@500 475 if (bin == binmin) break;
Chris@275 476 }
Chris@275 477 }
Chris@275 478
Chris@275 479 return peaks;
Chris@275 480 }
Chris@275 481
Chris@929 482 int
Chris@1040 483 FFTModel::getPeakPickWindowSize(PeakPickType type, sv_samplerate_t sampleRate,
Chris@929 484 int bin, float &percentile) const
Chris@275 485 {
Chris@280 486 percentile = 0.5;
Chris@275 487 if (type == MajorPeaks) return 10;
Chris@275 488 if (bin == 0) return 3;
Chris@280 489
Chris@1091 490 double binfreq = (sampleRate * bin) / m_fftSize;
Chris@1038 491 double hifreq = Pitch::getFrequencyForPitch(73, 0, binfreq);
Chris@280 492
Chris@1091 493 int hibin = int(lrint((hifreq * m_fftSize) / sampleRate));
Chris@275 494 int medianWinSize = hibin - bin;
Chris@275 495 if (medianWinSize < 3) medianWinSize = 3;
Chris@280 496
Chris@1091 497 percentile = 0.5f + float(binfreq / sampleRate);
Chris@280 498
Chris@275 499 return medianWinSize;
Chris@275 500 }
Chris@275 501
Chris@275 502 FFTModel::PeakSet
Chris@929 503 FFTModel::getPeakFrequencies(PeakPickType type, int x,
Chris@929 504 int ymin, int ymax)
Chris@275 505 {
Chris@551 506 Profiler profiler("FFTModel::getPeakFrequencies");
Chris@551 507
Chris@275 508 PeakSet peaks;
Chris@275 509 if (!isOK()) return peaks;
Chris@275 510 PeakLocationSet locations = getPeaks(type, x, ymin, ymax);
Chris@275 511
Chris@1040 512 sv_samplerate_t sampleRate = getSampleRate();
Chris@929 513 int incr = getResolution();
Chris@275 514
Chris@275 515 // This duplicates some of the work of estimateStableFrequency to
Chris@275 516 // allow us to retrieve the phases in two separate vertical
Chris@275 517 // columns, instead of jumping back and forth between columns x and
Chris@275 518 // x+1, which may be significantly slower if re-seeking is needed
Chris@275 519
Chris@1090 520 vector<float> phases;
Chris@275 521 for (PeakLocationSet::iterator i = locations.begin();
Chris@275 522 i != locations.end(); ++i) {
Chris@275 523 phases.push_back(getPhaseAt(x, *i));
Chris@275 524 }
Chris@275 525
Chris@929 526 int phaseIndex = 0;
Chris@275 527 for (PeakLocationSet::iterator i = locations.begin();
Chris@275 528 i != locations.end(); ++i) {
Chris@1038 529 double oldPhase = phases[phaseIndex];
Chris@1038 530 double newPhase = getPhaseAt(x+1, *i);
Chris@1090 531 double expectedPhase = oldPhase + (2.0 * M_PI * *i * incr) / m_fftSize;
Chris@1038 532 double phaseError = princarg(newPhase - expectedPhase);
Chris@1038 533 double frequency =
Chris@275 534 (sampleRate * (expectedPhase + phaseError - oldPhase))
Chris@275 535 / (2 * M_PI * incr);
Chris@1045 536 peaks[*i] = frequency;
Chris@275 537 ++phaseIndex;
Chris@275 538 }
Chris@275 539
Chris@275 540 return peaks;
Chris@275 541 }
Chris@275 542