Marsyas
0.6.0-alpha
|
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 }