Marsyas  0.6.0-alpha
/usr/src/RPM/BUILD/marsyas-0.6.0/src/marsyas/marsystems/CARFAC.cpp
Go to the documentation of this file.
00001 /*
00002 ** Copyright (C) 1998-2011 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 "CARFAC.h"
00020 
00021 using std::cout;
00022 using std::endl;
00023 using std::ostream;
00024 
00025 namespace Marsyas
00026 {
00027 
00028 void CARFAC::CARFAC_AGCStep(const std::vector<std::vector<double> > &avg_detects)
00029 {
00030   int n_AGC_stages = (int) CF.AGC_coeffs.AGC_epsilon.size();
00031   int n_mics = CF.n_mics;
00032   int n_ch = CF.n_ch;
00033 
00034   bool optimize_for_mono = (n_mics == 1) ? true : false;
00035 
00036   for (int stage = 0; stage < n_AGC_stages; stage++) {
00037 
00038     if (!optimize_for_mono) {
00039       if (stage > 0) {
00040         for (int i = 0; i < n_ch; i++) {
00041           agcstep_prev_stage_mean[i] = agcstep_stage_sum[i] / n_mics;
00042         }
00043       }
00044       for (int i = 0; i < n_ch; i++) {
00045         agcstep_stage_sum[i] = 0; // sum accumulating over mics at this stage
00046       }
00047     }
00048     double epsilon = CF.AGC_coeffs.AGC_epsilon[stage];
00049     double polez1 = CF.AGC_coeffs.AGC1_polez[stage];
00050     double polez2 = CF.AGC_coeffs.AGC2_polez[stage];
00051 
00052     for (int mic = 0; mic < n_mics; mic++) {
00053       if (stage == 0) {
00054         for (int i = 0; i < n_ch; i++) {
00055           agcstep_AGC_in[i] = CF.AGC_coeffs.detect_scale * avg_detects[i][mic];
00056         }
00057       } else {
00058         if (optimize_for_mono) {
00059           // Mono optimization ignores AGC_mix_coeff,
00060           // assuming all(agcstep_prev_stage_mean == AGC_memory(:, stage - 1));
00061           // but we also don't even allocate or compute the sum or mean.
00062           for (int i = 0; i < n_ch; i++) {
00063             agcstep_AGC_in[i] = CF.AGC_coeffs.AGC_stage_gain * CF.AGC_state[mic].AGC_memory[stage - 1][i];
00064           }
00065         } else {
00066           for (int i = 0; i < n_ch; i++) {
00067             agcstep_AGC_in[i] = CF.AGC_coeffs.AGC_stage_gain *
00068                                 (CF.AGC_coeffs.AGC_mix_coeff * agcstep_prev_stage_mean[i] +
00069                                  (1 - CF.AGC_coeffs.AGC_mix_coeff) * CF.AGC_state[mic].AGC_memory[stage - 1][i]);
00070           }
00071         }
00072       }
00073 
00074       for (int i = 0; i < n_ch; i++) {
00075         CF.AGC_state[mic].AGC_memory[stage][i] = CF.AGC_state[mic].AGC_memory[stage][i] + epsilon * (agcstep_AGC_in[i] - CF.AGC_state[mic].AGC_memory[stage][i]);
00076       }
00077 
00078       DoubleExponentialSmoothing(CF.AGC_state[mic].AGC_memory[stage], polez1, polez2, n_ch);
00079 
00080       if (stage == 0) {
00081         for (int i = 0; i < n_ch; i++) {
00082           CF.AGC_state[mic].sum_AGC[i] = CF.AGC_state[mic].AGC_memory[stage][i];
00083         }
00084       } else {
00085         for (int i = 0; i < n_ch; i++) {
00086           CF.AGC_state[mic].sum_AGC[i] +=  CF.AGC_state[mic].AGC_memory[stage][i];
00087         }
00088       }
00089       if (!optimize_for_mono) {
00090         for (int i = 0; i < n_ch; i++) {
00091           agcstep_stage_sum[i] = agcstep_stage_sum[i] +  CF.AGC_state[mic].AGC_memory[stage][i];
00092         }
00093       }
00094     }
00095   }
00096 
00097 }
00098 
00099 CARFAC::CARFAC(mrs_string name):MarSystem("CARFAC", name)
00100 {
00101   addControls();
00102 }
00103 
00104 CARFAC::CARFAC(const CARFAC& a) : MarSystem(a)
00105 {
00106   ctrl_printcoeffs_ = getctrl("mrs_bool/printcoeffs");
00107   ctrl_printstate_ = getctrl("mrs_bool/printstate");
00108   ctrl_calculate_binaural_sai_ = getctrl("mrs_bool/calculate_binaural_sai");
00109 
00110   ctrl_sai_output_binaural_sai_ = getctrl("mrs_realvec/sai_output_binaural_sai");
00111   ctrl_sai_output_threshold_ = getctrl("mrs_realvec/sai_output_threshold");
00112   ctrl_sai_output_strobes_ = getctrl("mrs_realvec/sai_output_strobes");
00113 
00114   allocateVectors();
00115 }
00116 
00117 CARFAC::~CARFAC()
00118 {
00119 }
00120 
00121 MarSystem*
00122 CARFAC::clone() const
00123 {
00124   return new CARFAC(*this);
00125 }
00126 
00127 void
00128 CARFAC::addControls()
00129 {
00130   // Add specific controls needed by this MarSystem.
00131   addctrl("mrs_bool/printcoeffs", true, ctrl_printcoeffs_);
00132   setControlState("mrs_bool/printcoeffs", true);
00133 
00134   addctrl("mrs_bool/printstate", true, ctrl_printstate_);
00135   setControlState("mrs_bool/printstate", true);
00136 
00137   // Output the Binaural SAI data to a control
00138   addctrl("mrs_bool/calculate_binaural_sai", false, ctrl_calculate_binaural_sai_);
00139   setControlState("mrs_bool/calculate_binaural_sai", true);
00140 
00141   // Controls for the Binaural SAI
00142   addctrl("mrs_natural/sai_width", 100, ctrl_sai_width_);
00143   setControlState("mrs_natural/sai_width", true);
00144 
00145   addctrl("mrs_real/sai_memory_factor", 0.8, ctrl_sai_memory_factor_);
00146   setControlState("mrs_real/sai_memory_factor", true);
00147 
00148   addctrl("mrs_bool/sai_summary_itd", false, ctrl_sai_summary_itd_);
00149   setControlState("mrs_bool/sai_summary_itd", true);
00150 
00151   addctrl("mrs_real/sai_threshold_alpha", 0.9999, ctrl_sai_threshold_alpha_);
00152   setControlState("mrs_real/sai_threshold_alpha", true);
00153 
00154   addctrl("mrs_real/sai_threshold_jump_factor", 1.5, ctrl_sai_threshold_jump_factor_);
00155   setControlState("mrs_real/sai_threshold_jump_factor", true);
00156 
00157   addctrl("mrs_real/sai_threshold_jump_offset", 0.1, ctrl_sai_threshold_jump_offset_);
00158   setControlState("mrs_real/sai_threshold_jump_offset", true);
00159 
00160   // The output of the SAI module
00161   addctrl("mrs_realvec/sai_output_binaural_sai", realvec(), ctrl_sai_output_binaural_sai_);
00162   addctrl("mrs_realvec/sai_output_threshold", realvec(), ctrl_sai_output_threshold_);
00163   addctrl("mrs_realvec/sai_output_strobes", realvec(), ctrl_sai_output_strobes_);
00164 }
00165 
00166 // Preallocate any vectors that will get reused over and over.
00167 void CARFAC::allocateVectors() {
00168   int n_ch = CF.n_ch;
00169   int n_samp = inSamples_;
00170   int n_mics = CF.n_mics;
00171   int decim = CF.CF_AGC_params.decimation;
00172 
00173   // Create the naps array.
00174   naps.resize(n_samp);
00175   for (int i = 0; i < n_samp; i++) {
00176     naps[i].resize(n_ch);
00177     for (int j = 0; j < n_ch; j++) {
00178       naps[i][j].resize(n_mics);
00179     }
00180   }
00181 
00182   // Create the prev_naps array
00183   prev_naps.resize(n_samp);
00184   for (int i = 0; i < n_samp; i++) {
00185     prev_naps[i].resize(n_ch);
00186     for (int j = 0; j < n_ch; j++) {
00187       prev_naps[i][j].resize(n_mics);
00188     }
00189   }
00190 
00191   // Create the decim_naps array.
00192   int decim_naps_size = n_samp/decim;
00193   decim_naps.resize(decim_naps_size);
00194   for (int i = 0; i < decim_naps_size; i++) {
00195     decim_naps[i].resize(n_ch);
00196     for (int j = 0; j < n_ch; j++) {
00197       decim_naps[i][j].resize(n_mics);
00198     }
00199   }
00200 
00201   // Create the sai array
00202   sai.resize(n_samp);
00203   for (int i = 0; i < n_samp; i++) {
00204     sai[i].resize(n_ch);
00205     for (int j = 0; j < n_ch; j++) {
00206       sai[i][j].resize(n_mics);
00207     }
00208   }
00209 
00210   // For AGCStep
00211   agcstep_prev_stage_mean.resize(n_ch);
00212   agcstep_stage_sum.resize(n_ch);
00213   agcstep_AGC_in.resize(n_ch);
00214   agcstep_AGC_stage.resize(n_ch);
00215 }
00216 
00217 
00218 void
00219 CARFAC::myUpdate(MarControlPtr sender)
00220 {
00221   // Binaural SAI
00222   calculate_binaural_sai_ = getctrl("mrs_bool/calculate_binaural_sai")->to<mrs_bool>();
00223   sai_width_ = getctrl("mrs_natural/sai_width")->to<mrs_natural>();
00224   sai_memory_factor_ = getctrl("mrs_real/sai_memory_factor")->to<mrs_real>();
00225   sai_threshold_alpha_ = getctrl("mrs_real/sai_threshold_alpha")->to<mrs_real>();
00226   sai_threshold_jump_factor_ = getctrl("mrs_real/sai_threshold_jump_factor")->to<mrs_real>();
00227   sai_threshold_jump_offset_ = getctrl("mrs_real/sai_threshold_jump_offset")->to<mrs_real>();
00228 
00229   MarControlAccessor acc_on(ctrl_sai_output_binaural_sai_);
00230   mrs_realvec& sai_output_binaural_sai = acc_on.to<mrs_realvec>();
00231   // sai_output_binaural_sai.stretch(onObservations_,inSamples_);
00232   // TODO(snessnet) - Set this correctly via n_ch
00233   sai_output_binaural_sai.stretch(96,sai_width_*2);
00234 
00235   MarControlAccessor acc_ot(ctrl_sai_output_threshold_);
00236   mrs_realvec& sai_output_threshold = acc_ot.to<mrs_realvec>();
00237   sai_output_threshold.stretch(onObservations_,inSamples_);
00238 
00239   MarControlAccessor acc_os(ctrl_sai_output_strobes_);
00240   mrs_realvec& sai_output_strobes = acc_os.to<mrs_realvec>();
00241   sai_output_strobes.stretch(onObservations_,inSamples_);
00242 
00243   // Initialize the arrays for the Filters and AGC
00244   CF.CARFAC_Init(inObservations_);
00245 
00246   MarSystem::myUpdate(sender);
00247 
00248   // TODO(snessnet) - Don't set n_ch here, set it from a control
00249   int n_ch = 96;
00250   ctrl_onObservations_->setValue(n_ch * 2, NOUPDATE);
00251 
00252   allocateVectors();
00253 
00254 }
00255 
00256 void
00257 CARFAC::myProcess(realvec& in, realvec& out)
00258 {
00259   MarControlAccessor acc_ob(ctrl_sai_output_binaural_sai_);
00260   mrs_realvec& sai_output_binaural_sai_ = acc_ob.to<mrs_realvec>();
00261 
00262   MarControlAccessor acc_ot(ctrl_sai_output_threshold_);
00263   mrs_realvec& sai_output_threshold = acc_ot.to<mrs_realvec>();
00264 
00265   MarControlAccessor acc_os(ctrl_sai_output_strobes_);
00266   mrs_realvec& sai_output_strobes = acc_os.to<mrs_realvec>();
00267 
00268   lastin = in;
00269 
00270   int n_ch = CF.n_ch;
00271   int n_mics = CF.n_mics;
00272 
00273   int decim_k = -1;
00274 
00275   bool make_decim_naps = false;
00276 
00277   int cum_k = -1;
00278   int decim = CF.CF_AGC_params.decimation;
00279 
00280   std::vector<double> detect(n_ch);
00281   std::vector<double> avg_detect(n_ch);
00282   std::vector<int> threshold_histogram(n_ch,0);
00283 
00284   for (mrs_natural k = 0; k < inSamples_; k++) {
00285     cum_k = cum_k + 1;
00286     for (int mic = 0; mic < n_mics; mic++) {
00287       double input_to_filterstep = in(mic,k);
00288       // detect = CARFAC_FilterStep(input_to_filterstep,mic);
00289       CF.filter_state[mic].FilterStep(CF, input_to_filterstep,detect);
00290       for (unsigned int i=0; i < detect.size(); i++) {
00291         naps[k][i][mic] = detect[i];
00292       }
00293     }
00294 
00295     // conditionally update all the AGC stages and channels now
00296     if ((cum_k+1) % decim == 0) { // using cum time in case we're doing segments
00297       // just for the plotting option:
00298       decim_k = decim_k + 1; // index of decimated signal for display
00299       if (make_decim_naps) {
00300         for (int mic = 0; mic < n_mics; mic++) {
00301           for (int i=0; i < n_ch; i++) {
00302             for (int j=0; j < n_ch; j++) {
00303               avg_detect[j] = CF.filter_state[mic].detect_accum[i] / decim;
00304               decim_naps[decim_k][j][mic] = avg_detect[j]; // for cochleagram
00305             }
00306           }
00307         }
00308       }
00309 
00310       std::vector<std::vector<double> > avg_detects(n_ch, std::vector<double>(n_mics));
00311 
00312       for (int mic = 0; mic < n_mics; mic++) {
00313         for (int j=0; j < n_ch; j++) {
00314           avg_detects[j][mic] = CF.filter_state[mic].detect_accum[j] / decim;
00315           CF.filter_state[mic].detect_accum[j] = 0.0;  // zero the detect accumulator
00316         }
00317       }
00318 
00319       CARFAC_AGCStep(avg_detects);
00320       for (int mic = 0; mic < n_mics; mic++) {
00321         for (int i = 0; i < n_ch; i++) {
00322           CF.filter_state[mic].dzB_memory[i] =
00323             (CF.AGC_state[mic].sum_AGC[i] - CF.filter_state[mic].zB_memory[i]) / decim;
00324         }
00325       }
00326     }
00327 
00328     // Detect strobe points
00329     for (int mic = 0; mic < n_mics; mic++) {
00330       int othermic = 1 - mic;
00331       std::vector<bool> above_threshold(n_ch,false);
00332       for (int i = 0; i < n_ch; i++) {
00333         if ((CF.strobe_state[mic].lastdata[i] > CF.strobe_state[mic].thresholds[i]) &&
00334             (CF.strobe_state[mic].lastdata[i] > naps[k][i][mic])) {
00335           above_threshold[i] = true;
00336         } else {
00337           above_threshold[i] = false;
00338         }
00339         if (above_threshold[i]) {
00340           CF.strobe_state[mic].thresholds[i] = naps[k][i][mic] * sai_threshold_jump_factor_ + sai_threshold_jump_offset_;
00341         } else {
00342           CF.strobe_state[mic].thresholds[i] = CF.strobe_state[mic].thresholds[i] * sai_threshold_alpha_;
00343         }
00344         CF.strobe_state[mic].lastdata[i] = naps[k][i][mic];
00345 
00346         // Copy the thresholds to the output control
00347         // TODO(snessnet) - Executive decision to just do first microphone
00348         if (calculate_binaural_sai_ && mic == 0) {
00349           sai_output_threshold(i,k) = CF.strobe_state[0].thresholds[i];
00350           sai_output_strobes(i,k) = above_threshold[i];
00351         }
00352       }
00353 
00354       // If above threshold, copy data from the trigger point onwards
00355       for (int i = 0; i < n_ch; i++) {
00356         if (above_threshold[i] && mic == 0) {
00357           threshold_histogram[i]++;
00358         }
00359 
00360         if (above_threshold[i]) {
00361           CF.strobe_state[mic].trigger_index[i] = k;
00362           CF.strobe_state[mic].sai_index[i] = 0;
00363         }
00364 
00365         if ((CF.strobe_state[mic].sai_index[i] < sai_width_) && (CF.strobe_state[mic].trigger_index[i] < inSamples_)) {
00366           sai[CF.strobe_state[mic].sai_index[i]][i][mic] = naps[CF.strobe_state[mic].trigger_index[i]][i][othermic] + sai[CF.strobe_state[mic].sai_index[i]][i][mic] * sai_memory_factor_;
00367         }
00368         CF.strobe_state[mic].trigger_index[i]++;
00369         CF.strobe_state[mic].sai_index[i]++;
00370       }
00371     }
00372   }
00373 
00374   // Copy the nap data to the output
00375   for (int row = 0; row < n_ch; row++) {
00376     for (int col = 0; col < inSamples_; col++) {
00377       out(row,col) = naps[col][row][0];
00378       out(row+n_ch,col) = naps[col][row][1];
00379     }
00380   }
00381 
00382   // Copy the sai data to the output
00383   if (calculate_binaural_sai_) {
00384     for (int row = 0; row < n_ch; row++) {
00385       for (int col = 0; col < sai_width_; col++) {
00386         sai_output_binaural_sai_(row,sai_width_-col) = sai[col][row][0];
00387         sai_output_binaural_sai_(row,sai_width_+col) = sai[col][row][1];
00388       }
00389     }
00390 
00391     // Save the nap data for the next iteration
00392     for (int i = 0; i < inSamples_; i++) {
00393       for (int j = 0; j < n_ch; j++) {
00394         for (int k = 0; k < n_mics; k++) {
00395           prev_naps[i][j][k] = naps[i][j][k];
00396         }
00397       }
00398     }
00399   }
00400 
00401 }
00402 
00403 
00404 void CARFAC::DoubleExponentialSmoothing(std::vector<double> &data, double polez1, double polez2, int n_ch)
00405 {
00406   double state = 0;
00407   double input;
00408   for (int index = n_ch - 10; index < n_ch; index++) {
00409     input = data[index];
00410     state = state + (1 - polez1) * (input - state);
00411   }
00412 
00413   // smooth backward with polez2, starting with state from above:
00414   for (int index = n_ch - 1; index >= 0; index--) {
00415     input = data[index];
00416     state = state + (1 - polez2) * (input - state);
00417     data[index] = state;
00418   }
00419 
00420   // smooth forward with polez1, starting with state from above:
00421   for (int index = 0; index < n_ch; index++) {
00422     state = state + (1 - polez1) * (data[index] - state);
00423     data[index] = state;
00424   }
00425 
00426 }
00427 
00428 
00429 std::string
00430 CARFAC::toString()
00431 {
00432   std::ostringstream oss;
00433 
00434   CF.printcoeffs = getctrl("mrs_bool/printcoeffs")->to<mrs_bool>();
00435   CF.printstate = getctrl("mrs_bool/printstate")->to<mrs_bool>();
00436 
00437   if (lastin.getSize() > 0) {
00438     cout << "signal";
00439 
00440     oss.precision(5);
00441     oss.flags(std::ios::fixed);
00442 
00443     for (int i = 0; i < 10; i++) {
00444       cout << lastin(0,i) << " ";
00445     }
00446     cout << endl;
00447   }
00448 
00449   oss.precision(4);
00450   oss.flags(std::ios::scientific);
00451   oss << CF << endl;
00452 
00453   return oss.str();
00454 }
00455 }