1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
#include "math.h"
#include "twm.h"
using namespace simpl;
int simpl::best_match(sample freq, std::vector<sample> candidates) {
sample best_diff = 22050.0;
sample diff = 0.0;
int best = 0;
for(int i = 0; i < candidates.size(); i++) {
diff = fabs(freq - candidates[i]);
if(diff < best_diff) {
best_diff = diff;
best = i;
}
}
return best;
}
sample simpl::twm(Peaks peaks, sample f_min, sample f_max, sample f_step) {
sample p = 0.5;
sample q = 1.4;
sample r = 0.5;
sample rho = 0.33;
int N = 30;
std::map<sample, sample> err;
if(peaks.size() == 0) {
return 0.0;
}
sample max_amp = 0.0;
for(int i = 0; i < peaks.size(); i++) {
if(peaks[i]->amplitude > max_amp) {
max_amp = peaks[i]->amplitude;
}
}
if(max_amp == 0) {
return 0.0;
}
// remove all peaks with amplitude of less than 10% of max
// note: this is not in the TWM paper, found that it improved
// accuracy however
for(int i = 0; i < peaks.size(); i++) {
if(peaks[i]->amplitude < (max_amp * 0.1)) {
peaks.erase(peaks.begin() + i);
}
}
// get the max frequency of the remaining peaks
sample max_freq = 0.0;
for(int i = 0; i < peaks.size(); i++) {
if(peaks[i]->frequency > max_freq) {
max_freq = peaks[i]->frequency;
}
}
std::vector<sample> peak_freqs;
for(int i = 0; i < peaks.size(); i++) {
peak_freqs.push_back(peaks[i]->frequency);
}
sample f_current = f_min;
while(f_current < f_max) {
sample err_pm = 0.0;
sample err_mp = 0.0;
std::vector<sample> harmonics;
for(sample f = f_current; f <= f_max; f += f_current) {
harmonics.push_back(f);
if(harmonics.size() >= N) {
break;
}
}
// calculate mismatch between predicted and actual peaks
for(int i = 0; i < harmonics.size(); i++) {
sample h = harmonics[i];
int k = best_match(h, peak_freqs);
sample f = peaks[k]->frequency;
sample a = peaks[k]->amplitude;
err_pm += (fabs(h - f) * pow(h, -p)) +
(((a / max_amp) * (q * fabs(h - f)) * (pow(h, -p) - r)));
}
// calculate the mismatch between actual and predicted peaks
for(int i = 0; i < peaks.size(); i++) {
sample f = peaks[i]->frequency;
sample a = peaks[i]->amplitude;
int k = best_match(f, harmonics);
sample h = harmonics[k];
err_mp += (fabs(f - h) * pow(f, -p)) +
((a / max_amp) * (q * fabs(f - h)) * (pow(f, -p) - r));
}
// calculate the total error for f_current as a fundamental frequency
err[f_current] = (err_pm / harmonics.size()) +
(rho * err_mp / peaks.size());
f_current += f_step;
}
// return the value with the minimum total error
sample best_freq = 0;
sample min_error = 22050;
for(std::map<sample, sample>::iterator i = err.begin(); i != err.end(); i++) {
if(fabs((*i).second) < min_error) {
min_error = fabs((*i).second);
best_freq = (*i).first;
}
}
return best_freq;
}
|