Marsyas
0.6.0-alpha
|
00001 /* 00002 ** Copyright (C) 1998-2010 George Tzanetakis <gtzan@cs.uvic.ca> 00003 ** 00004 ** This program is free software; you can redistribute it and/or modify 00005 ** it under the terms of the GNU General Public License as published by 00006 ** the Free Software Foundation; either version 2 of the License, or 00007 ** (at your option) any later version. 00008 ** 00009 ** This program is distributed in the hope that it will be useful, 00010 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 ** GNU General Public License for more details. 00013 ** 00014 ** You should have received a copy of the GNU General Public License 00015 ** along with this program; if not, write to the Free Software 00016 ** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 00017 */ 00018 00019 #include "CrossCorrelation.h" 00020 00021 00022 using std::ostringstream; 00023 using std::cout; 00024 using std::endl; 00025 using std::abs; 00026 00027 using namespace Marsyas; 00028 00029 CrossCorrelation::CrossCorrelation(mrs_string name):MarSystem("CrossCorrelation",name) 00030 { 00031 myfft_ = NULL; 00032 addControls(); 00033 } 00034 00035 // destructor 00036 CrossCorrelation::~CrossCorrelation() 00037 { 00038 delete myfft_; 00039 } 00040 00041 // copy constructor 00042 CrossCorrelation::CrossCorrelation(const CrossCorrelation& a):MarSystem(a) 00043 { 00044 myfft_ = NULL; 00045 ctrl_mode_ = getctrl("mrs_string/mode"); 00046 } 00047 00048 void 00049 CrossCorrelation::addControls() 00050 { 00051 // cross correlation modes: "general", "phat", "ml" 00052 addctrl("mrs_string/mode","general",ctrl_mode_); 00053 } 00054 00055 MarSystem* 00056 CrossCorrelation::clone() const 00057 { 00058 return new CrossCorrelation(*this); 00059 } 00060 00061 void 00062 CrossCorrelation::myUpdate(MarControlPtr sender) 00063 { 00064 (void) sender; //suppress warning of unused parameter(s) 00065 delete myfft_; //[!] 00066 myfft_ = new fft();//[!] 00067 00068 setctrl("mrs_natural/onSamples", getctrl("mrs_natural/inSamples")); 00069 setctrl("mrs_natural/onObservations", getctrl("mrs_natural/inObservations")->to<mrs_natural>() - 1); 00070 setctrl("mrs_real/osrate", getctrl("mrs_real/israte")); 00071 00072 scratch_.create(getctrl("mrs_natural/onSamples")->to<mrs_natural>()); 00073 scratch1_.create(getctrl("mrs_natural/onSamples")->to<mrs_natural>()); 00074 scratch2_.create(getctrl("mrs_natural/onSamples")->to<mrs_natural>()); 00075 00076 // for sub loop (used to get averaged fft in maximum likelihood mode) 00077 sub_scratch1_.create(getctrl("mrs_natural/onSamples")->to<mrs_natural>()); 00078 sub_scratch2_.create(getctrl("mrs_natural/onSamples")->to<mrs_natural>()); 00079 } 00080 00081 void 00082 CrossCorrelation::myProcess(realvec& in, realvec& out) 00083 { 00084 // this cross correlation will take in N observations and return N-1 observations 00085 mrs_natural o,t; 00086 mrs_real re1,im1,re2,im2,re,im,abs_out; 00087 00088 for (o=0; o < (inObservations_ - 1); o++) 00089 { 00090 mrs_real *channelOut = scratch_.getData(); 00091 mrs_real *channel1 = scratch1_.getData(); 00092 mrs_real *channel2 = scratch2_.getData(); 00093 00094 for (t=0; t < inSamples_; t++) { 00095 scratch_(t) = 0; 00096 scratch1_(t) = in(o,t); 00097 scratch2_(t) = in(o+1,t); 00098 } 00099 00100 mode_ = getctrl("mrs_string/mode")->to<mrs_string>(); 00101 00102 myfft_->rfft(channel1, inSamples_/2, FFT_FORWARD); 00103 myfft_->rfft(channel2, inSamples_/2, FFT_FORWARD); 00104 00105 if (mode_ == "general") 00106 { 00107 00108 //Compress the magnitude spectrum and zero 00109 // the imaginary part. 00110 for (t=1; t < inSamples_/2; t++) 00111 { 00112 re1 = channel1[2*t]; 00113 im1 = channel1[2*t+1]; 00114 00115 re2 = channel2[2*t]; 00116 im2 = channel2[2*t+1]; 00117 00118 //(re1 + j im1)*(re2 - j im2) = (re1*re2 + im1*im2) + j(im1re2 - im2re1) 00119 00120 re = re1*re2 + im1*im2; 00121 im = re2*im1 - re1*im2; 00122 00123 channelOut[2*t] = re; 00124 channelOut[2*t+1] = im; 00125 } 00126 } 00127 else if (mode_ == "phat") 00128 { 00129 00130 //Compress the magnitude spectrum and zero 00131 // the imaginary part. 00132 for (t=1; t < inSamples_/2; t++) 00133 { 00134 re1 = channel1[2*t]; 00135 im1 = channel1[2*t+1]; 00136 00137 re2 = channel2[2*t]; 00138 im2 = channel2[2*t+1]; 00139 00140 //(re1 + j im1)*(re2 - j im2) = (re1*re2 + im1*im2) + j(im1re2 - im2re1) 00141 00142 re = re1*re2 + im1*im2; 00143 im = re2*im1 - re1*im2; 00144 00145 // divide by absolute value (PHAT) 00146 abs_out = sqrt(re*re + im*im); 00147 00148 re = re/abs_out; 00149 im = im/abs_out; 00150 00151 channelOut[2*t] = re; 00152 channelOut[2*t+1] = im; 00153 00154 } 00155 00156 } 00157 else if (mode_ == "ml") 00158 { 00159 // Maximum Likelihood - robust solution for time difference calculation 00160 // 00161 // Reference: 00162 // MAXIMUM LIKELIHOOD TIME DELAY ESTIMATION WITH PHASE DOMAIN ANALYSIS 00163 // IN THE GENERALIZED CROSS CORRELATION FRAMEWORK 00164 00165 mrs_real *sub_channel1 = sub_scratch1_.getData(); 00166 mrs_real *sub_channel2 = sub_scratch2_.getData(); 00167 00168 mrs_real sub_re1, sub_im1, sub_re2, sub_im2; 00169 mrs_natural sub_window_size, sub_hop, sub_count, sub_start, sub_end; 00170 00171 mrs_real re_q1_1,re_q2_2,re_q1_2,im_q1_2; 00172 00173 mrs_natural window_size = inSamples_; 00174 00175 mrs_realvec q1_1(window_size); 00176 mrs_realvec q2_2(window_size); 00177 mrs_realvec q1_2(window_size); 00178 mrs_realvec mu1_2(window_size); 00179 mrs_realvec v1_2(window_size); 00180 00181 00182 //------------------------------------------------------------------------------- 00183 // SUB LOOP - 00184 // Used to calculate expected correlation mean, phase mean and phase variation 00185 // based on subsets of the data 00186 00187 sub_window_size = window_size/4; 00188 sub_hop = window_size/8; 00189 sub_start = 0; 00190 sub_end = sub_start + sub_window_size; 00191 sub_count = 1; 00192 00193 // Set to zero 00194 for (t=0; t < inSamples_; t++) 00195 { 00196 q1_1(t) = 0; 00197 q2_2(t) = 0; 00198 q1_2(t) = 0; 00199 mu1_2(t) = 0; 00200 v1_2(t) = 0; 00201 } 00202 00203 while (sub_end < window_size) 00204 { 00205 for (t=0; t < sub_window_size; t++) 00206 { 00207 sub_scratch1_(t) = 0; 00208 sub_scratch2_(t) = 0; 00209 00210 sub_scratch1_(t) = in(o,t+sub_start); 00211 sub_scratch2_(t) = in(o+1,t+sub_start); 00212 } 00213 00214 // zeropad 00215 for (t=sub_window_size; t < window_size; t++) 00216 { 00217 sub_scratch1_(t) = 0; 00218 sub_scratch2_(t) = 0; 00219 } 00220 00221 myfft_->rfft(sub_channel1, inSamples_/2, FFT_FORWARD); 00222 myfft_->rfft(sub_channel2, inSamples_/2, FFT_FORWARD); 00223 00224 for (t=0; t < inSamples_/2; t++) 00225 { 00226 sub_re1 = sub_channel1[2*t]; 00227 sub_im1 = sub_channel1[2*t+1]; 00228 00229 sub_re2 = sub_channel2[2*t]; 00230 sub_im2 = sub_channel2[2*t+1]; 00231 00232 //(re1 + j im1)*(re2 - j im2) = (re1*re2 + im1*im2) + j(im1re2 - im2re1) 00233 00234 // autocorrelations 00235 q1_1(2*t) = q1_1(2*t) + (sub_re1*sub_re1 + sub_im1*sub_im1); 00236 q1_1(2*t+1) =0; 00237 00238 q2_2(2*t) = q2_2(2*t) + (sub_re2*sub_re2 + sub_im2*sub_im2); 00239 q2_2(2*t+1) =0; 00240 00241 // cross correlation 00242 q1_2(2*t) = q1_2(2*t) + (sub_re1*sub_re2 + sub_im1*sub_im2); 00243 q1_2(2*t+1) = q1_2(2*t+1) + (sub_re2*sub_im1 - sub_re1*sub_im2); 00244 } 00245 00246 sub_start = sub_start + sub_hop; 00247 sub_end = sub_end + sub_hop; 00248 sub_count = sub_count + 1; 00249 } 00250 00251 00252 for (t=0; t < inSamples_/2; t++) 00253 { 00254 // q = q/sub_count; (for averaging) 00255 re_q1_1 = q1_1(2*t)/sub_count; 00256 re_q2_2 = q2_2(2*t)/sub_count; 00257 00258 re_q1_2 = q1_2(2*t)/sub_count; 00259 im_q1_2 = q1_2(2*t+1)/sub_count; 00260 00261 // mu1_2 = abs(q1_2)/sqrt(q2_2*q1_1) 00262 mu1_2(2*t) = sqrt(re_q1_2*re_q1_2 + im_q1_2*im_q1_2)/sqrt(re_q1_1*re_q2_2); 00263 mu1_2(2*t+1) = 0; 00264 00265 v1_2(2*t) = (1-(mu1_2(2*t)*mu1_2(2*t)))/(mu1_2(2*t)*mu1_2(2*t)); 00266 00267 } 00268 00269 //cout << "q1_1: " << q1_1(280) << endl; 00270 //cout << "q2_2: " << q2_2(280) << endl; 00271 //cout << "q1_2: " << q1_2(280) << endl; 00272 //cout << "mu1_2: " << mu1_2(280) << endl; 00273 //cout << "v1_2: " << v1_2(280) << endl; 00274 00275 //------------------------------------------------------------------------------- 00276 // MAIN LOOP 00277 // Multiplied weighting calculated above with generalized cross-correlation 00278 00279 for (t=1; t < inSamples_/2; t++) 00280 { 00281 re1 = channel1[2*t]; 00282 im1 = channel1[2*t+1]; 00283 00284 re2 = channel2[2*t]; 00285 im2 = channel2[2*t+1]; 00286 00287 //(re1 + j im1)*(re2 - j im2) = (re1*re2 + im1*im2) + j(im1re2 - im2re1) 00288 re = re1*re2 + im1*im2; 00289 im = re2*im1 - re1*im2; 00290 00291 // absolute value 00292 abs_out = sqrt(re*re + im*im); 00293 00294 //output = A0_1/(abs(A0_1)*sqrt(v0_1)) 00295 channelOut[2*t] = re/(abs_out*sqrt(v1_2(2*t))); 00296 channelOut[2*t+1] = im/(abs_out*sqrt(v1_2(2*t))); 00297 00298 //channelOut[2*t] = re/abs_out; 00299 //channelOut[2*t+1] = im/abs_out; 00300 00301 } 00302 00303 //cout << "channel1: " << "re " << channel1[280] << "im " << channel1[281] << endl; 00304 //cout << "channel2: " << "re " << channel2[280] << "im " << channel2[281] << endl; 00305 //cout << "channelOut: " << channelOut[280] << endl; 00306 00307 } 00308 00309 else 00310 { 00311 cout << "Invalid Mode" << endl; 00312 } 00313 00314 //Take the inverse Fourier Transform 00315 myfft_->rfft(channelOut, inSamples_/2, FFT_INVERSE); 00316 //cout << channelOut[24] << endl; 00317 00318 // Return Cross Correlation Output (with FFT shift - two loops) 00319 for (t=0; t < inSamples_/2; t++) 00320 out(o,t) = abs(scratch_(t + inSamples_/2)); 00321 //out(o,2*t+1) = 0; 00322 00323 for (t=inSamples_/2; t < inSamples_; t++) 00324 out(o,t) = abs(scratch_(t - inSamples_/2)); 00325 //out(o,2*t+1) = 0; 00326 00327 00328 00329 } 00330 }