Marsyas  0.6.0-alpha
/usr/src/RPM/BUILD/marsyas-0.6.0/src/marsyas/marsystems/CrossCorrelation.cpp
Go to the documentation of this file.
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 }