1b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org/* 2b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. 3b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * 4b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * Use of this source code is governed by a BSD-style license 5b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * that can be found in the LICENSE file in the root of the source 6b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * tree. An additional intellectual property rights grant can be found 7b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * in the file PATENTS. All contributing project authors may 8b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * be found in the AUTHORS file in the root of the source tree. 9b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org */ 10b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 11b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#include "pitch_estimator.h" 12b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 13b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#include <math.h> 14b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#include <memory.h> 1505f7eb65ac0297786c073fb6536b1e488d8bed05kjellander@webrtc.org#include <string.h> 16b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#ifdef WEBRTC_ANDROID 17b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#include <stdlib.h> 18b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org#endif 19b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 20b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.orgstatic const double kInterpolWin[8] = {-0.00067556028640, 0.02184247643159, -0.12203175715679, 0.60086484101160, 21b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 0.60086484101160, -0.12203175715679, 0.02184247643159, -0.00067556028640}; 22b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 23b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org/* interpolation filter */ 24b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org__inline static void IntrepolFilter(double *data_ptr, double *intrp) 25b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org{ 26b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp = kInterpolWin[0] * data_ptr[-3]; 27b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[1] * data_ptr[-2]; 28b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[2] * data_ptr[-1]; 29b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[3] * data_ptr[0]; 30b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[4] * data_ptr[1]; 31b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[5] * data_ptr[2]; 32b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[6] * data_ptr[3]; 33b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *intrp += kInterpolWin[7] * data_ptr[4]; 34b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org} 35b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 36b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 37b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org/* 2D parabolic interpolation */ 38b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org/* probably some 0.5 factors can be eliminated, and the square-roots can be removed from the Cholesky fact. */ 39b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org__inline static void Intrpol2D(double T[3][3], double *x, double *y, double *peak_val) 40b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org{ 41b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double c, b[2], A[2][2]; 42b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double t1, t2, d; 43b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double delta1, delta2; 44b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 45b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 46b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org // double T[3][3] = {{-1.25, -.25,-.25}, {-.25, .75, .75}, {-.25, .75, .75}}; 47b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org // should result in: delta1 = 0.5; delta2 = 0.0; peak_val = 1.0 48b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 49b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org c = T[1][1]; 50b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org b[0] = 0.5 * (T[1][2] + T[2][1] - T[0][1] - T[1][0]); 51b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org b[1] = 0.5 * (T[1][0] + T[2][1] - T[0][1] - T[1][2]); 52b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[0][1] = -0.5 * (T[0][1] + T[2][1] - T[1][0] - T[1][2]); 53b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org t1 = 0.5 * (T[0][0] + T[2][2]) - c; 54b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org t2 = 0.5 * (T[2][0] + T[0][2]) - c; 55b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org d = (T[0][1] + T[1][2] + T[1][0] + T[2][1]) - 4.0 * c - t1 - t2; 56b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[0][0] = -t1 - 0.5 * d; 57b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[1][1] = -t2 - 0.5 * d; 58b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 59b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* deal with singularities or ill-conditioned cases */ 60b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( (A[0][0] < 1e-7) || ((A[0][0] * A[1][1] - A[0][1] * A[0][1]) < 1e-7) ) { 61b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *peak_val = T[1][1]; 62b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org return; 63b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 64b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 65b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* Cholesky decomposition: replace A by upper-triangular factor */ 66b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[0][0] = sqrt(A[0][0]); 67b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[0][1] = A[0][1] / A[0][0]; 68b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org A[1][1] = sqrt(A[1][1] - A[0][1] * A[0][1]); 69b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 70b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute [x; y] = -0.5 * inv(A) * b */ 71b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org t1 = b[0] / A[0][0]; 72b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org t2 = (b[1] - t1 * A[0][1]) / A[1][1]; 73b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org delta2 = t2 / A[1][1]; 74b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org delta1 = 0.5 * (t1 - delta2 * A[0][1]) / A[0][0]; 75b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org delta2 *= 0.5; 76b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 77b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* limit norm */ 78b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org t1 = delta1 * delta1 + delta2 * delta2; 79b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (t1 > 1.0) { 80b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org delta1 /= t1; 81b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org delta2 /= t1; 82b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 83b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 84b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *peak_val = 0.5 * (b[0] * delta1 + b[1] * delta2) + c; 85b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 86b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *x += delta1; 87b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *y += delta2; 88b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org} 89b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 90b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 91b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.orgstatic void PCorr(const double *in, double *outcorr) 92b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org{ 93b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double sum, ysum, prod; 94b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org const double *x, *inptr; 95b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int k, n; 96b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 97b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org //ysum = 1e-6; /* use this with float (i.s.o. double)! */ 98b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ysum = 1e-13; 99b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org sum = 0.0; 100b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org x = in + PITCH_MAX_LAG/2 + 2; 101b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (n = 0; n < PITCH_CORR_LEN2; n++) { 102b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ysum += in[n] * in[n]; 103b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org sum += x[n] * in[n]; 104b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 105b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 106b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org outcorr += PITCH_LAG_SPAN2 - 1; /* index of last element in array */ 107b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *outcorr = sum / sqrt(ysum); 108b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 109b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 1; k < PITCH_LAG_SPAN2; k++) { 110b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ysum -= in[k-1] * in[k-1]; 111b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ysum += in[PITCH_CORR_LEN2 + k - 1] * in[PITCH_CORR_LEN2 + k - 1]; 112b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org sum = 0.0; 113b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org inptr = &in[k]; 114b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org prod = x[0] * inptr[0]; 115b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (n = 1; n < PITCH_CORR_LEN2; n++) { 116b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org sum += prod; 117b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org prod = x[n] * inptr[n]; 118b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 119b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org sum += prod; 120b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org outcorr--; 121b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org *outcorr = sum / sqrt(ysum); 122b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 123b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org} 124b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 125b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 126b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.orgvoid WebRtcIsac_InitializePitch(const double *in, 127b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org const double old_lag, 128b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org const double old_gain, 129b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org PitchAnalysisStruct *State, 130b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double *lags) 131b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org{ 132b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2]; 133b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double ratio, log_lag, gain_bias; 134b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double bias; 135b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double corrvec1[PITCH_LAG_SPAN2]; 136b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double corrvec2[PITCH_LAG_SPAN2]; 137b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int m, k; 138b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org // Allocating 10 extra entries at the begining of the CorrSurf 139b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double corrSurfBuff[10 + (2*PITCH_BW+3)*(PITCH_LAG_SPAN2+4)]; 140b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double* CorrSurf[2*PITCH_BW+3]; 141b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double *CorrSurfPtr1, *CorrSurfPtr2; 142b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double LagWin[3] = {0.2, 0.5, 0.98}; 143b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int ind1, ind2, peaks_ind, peak, max_ind; 144b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int peaks[PITCH_MAX_NUM_PEAKS]; 145b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double adj, gain_tmp; 146b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double corr, corr_max; 147b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double intrp_a, intrp_b, intrp_c, intrp_d; 148b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double peak_vals[PITCH_MAX_NUM_PEAKS]; 149b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double lags1[PITCH_MAX_NUM_PEAKS]; 150b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double lags2[PITCH_MAX_NUM_PEAKS]; 151b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double T[3][3]; 152b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int row; 153b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 154b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for(k = 0; k < 2*PITCH_BW+3; k++) 155b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 156b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurf[k] = &corrSurfBuff[10 + k * (PITCH_LAG_SPAN2+4)]; 157b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 158b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* reset CorrSurf matrix */ 159b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memset(corrSurfBuff, 0, sizeof(double) * (10 + (2*PITCH_BW+3) * (PITCH_LAG_SPAN2+4))); 160b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 161b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org //warnings -DH 162b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = 0; 163b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peak = 0; 164b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 165b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* copy old values from state buffer */ 166b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(buf_dec, State->dec_buffer, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)); 167b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 168b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* decimation; put result after the old values */ 169b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_DecimateAllpass(in, State->decimator_state, PITCH_FRAME_LEN, 170b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org &buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]); 171b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 172b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* low-pass filtering */ 173b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2; k < PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2; k++) 174b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org buf_dec[k] += 0.75 * buf_dec[k-1] - 0.25 * buf_dec[k-2]; 175b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 176b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* copy end part back into state buffer */ 177b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(State->dec_buffer, buf_dec+PITCH_FRAME_LEN/2, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)); 178b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 179b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute correlation for first and second half of the frame */ 180b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org PCorr(buf_dec, corrvec1); 181b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org PCorr(buf_dec + PITCH_CORR_STEP2, corrvec2); 182b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 183b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* bias towards pitch lag of previous frame */ 184b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org log_lag = log(0.5 * old_lag); 185b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gain_bias = 4.0 * old_gain * old_gain; 186b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (gain_bias > 0.8) gain_bias = 0.8; 187b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_LAG_SPAN2; k++) 188b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 189b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ratio = log((double) (k + (PITCH_MIN_LAG/2-2))) - log_lag; 190b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org bias = 1.0 + gain_bias * exp(-5.0 * ratio * ratio); 191b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corrvec1[k] *= bias; 192b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 193b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 194b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* taper correlation functions */ 195b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 3; k++) { 196b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gain_tmp = LagWin[k]; 197b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corrvec1[k] *= gain_tmp; 198b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corrvec2[k] *= gain_tmp; 199b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corrvec1[PITCH_LAG_SPAN2-1-k] *= gain_tmp; 200b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corrvec2[PITCH_LAG_SPAN2-1-k] *= gain_tmp; 201b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 202b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 203b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = 0.0; 204b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* fill middle row of correlation surface */ 205b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind1 = 0; 206b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind2 = 0; 207b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[PITCH_BW][2]; 208b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_LAG_SPAN2; k++) { 209b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = corrvec1[ind1++] + corrvec2[ind2++]; 210b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1[k] = corr; 211b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 212b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 213b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 214b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 215b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 216b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* fill first and last rows of correlation surface */ 217b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind1 = 0; 218b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind2 = PITCH_BW; 219b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[0][2]; 220b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2 = &CorrSurf[2*PITCH_BW][PITCH_BW+2]; 221b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW; k++) { 222b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 223b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org adj = 0.2 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 224b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 225b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1[k] = corr; 226b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 227b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 228b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 229b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 230b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 231b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2[k] = corr; 232b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 233b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 234b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 235b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 236b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 237b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* fill second and next to last rows of correlation surface */ 238b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind1 = 0; 239b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind2 = PITCH_BW-1; 240b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[1][2]; 241b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-1][PITCH_BW+1]; 242b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+1; k++) { 243b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 244b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org adj = 0.9 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 245b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 246b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1[k] = corr; 247b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 248b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 249b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 250b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 251b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 252b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2[k] = corr; 253b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 254b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 255b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 256b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 257b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 258b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* fill remainder of correlation surface */ 259b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = 2; m < PITCH_BW; m++) { 260b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind1 = 0; 261b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ind2 = PITCH_BW - m; /* always larger than ind1 */ 262b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[m][2]; 263b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-m][PITCH_BW+2-m]; 264b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+m; k++) { 265b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); 266b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org adj = ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ 267b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind1] + corrvec2[ind2]); 268b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1[k] = corr; 269b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 270b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 271b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 272b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 273b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); 274b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr2[k] = corr; 275b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 276b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; /* update maximum */ 277b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); 278b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 279b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 280b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 281b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 282b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* threshold value to qualify as a peak */ 283b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max *= 0.6; 284b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 285b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peaks_ind = 0; 286b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* find peaks */ 287b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = 1; m < PITCH_BW+1; m++) { 288b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 289b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[m][2]; 290b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 2; k < PITCH_LAG_SPAN2-PITCH_BW-2+m; k++) { 291b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = CorrSurfPtr1[k]; 292b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 293b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { 294b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { 295b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* found a peak; store index into matrix */ 296b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 297b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 298b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 299b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 300b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 301b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 302b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 303b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = PITCH_BW+1; m < 2*PITCH_BW; m++) { 304b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 305b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[m][2]; 306b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 2+m-PITCH_BW; k < PITCH_LAG_SPAN2-2; k++) { 307b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = CorrSurfPtr1[k]; 308b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 309b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { 310b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { 311b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* found a peak; store index into matrix */ 312b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); 313b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; 314b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 315b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 316b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 317b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 318b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 319b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 320b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (peaks_ind > 0) { 321b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* examine each peak */ 322b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org CorrSurfPtr1 = &CorrSurf[0][0]; 323b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < peaks_ind; k++) { 324b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peak = peaks[k]; 325b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 326b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute four interpolated values around current peak */ 327b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)], &intrp_a); 328b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - 1 ], &intrp_b); 329b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak ], &intrp_c); 330b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)], &intrp_d); 331b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 332b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* determine maximum of the interpolated values */ 333b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = CorrSurfPtr1[peak]; 334b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = intrp_a; 335b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (intrp_b > corr_max) corr_max = intrp_b; 336b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (intrp_c > corr_max) corr_max = intrp_c; 337b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (intrp_d > corr_max) corr_max = intrp_d; 338b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 339b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* determine where the peak sits and fill a 3x3 matrix around it */ 340b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org row = peak / (PITCH_LAG_SPAN2+4); 341b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[k] = (double) ((peak - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); 342b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[k] = (double) (lags1[k] + PITCH_BW - row); 343b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if ( corr > corr_max ) { 344b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 345b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 346b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][1] = corr; 347b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 348b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 349b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][0] = intrp_a; 350b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][1] = intrp_b; 351b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][1] = intrp_c; 352b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][2] = intrp_d; 353b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } else { 354b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (intrp_a == corr_max) { 355b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[k] -= 0.5; 356b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[k] += 0.5; 357b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - 2*(PITCH_LAG_SPAN2+5)], &T[0][0]); 358b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)], &T[2][0]); 359b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][1] = intrp_a; 360b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][2] = intrp_b; 361b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][2] = intrp_c; 362b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][0] = CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)]; 363b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 364b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 365b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][2] = corr; 366b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } else if (intrp_b == corr_max) { 367b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[k] -= 0.5; 368b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[k] -= 0.5; 369b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+6)], &T[0][0]); 370b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][0] = intrp_a; 371b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][1] = intrp_b; 372b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+3)], &T[0][2]); 373b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][2] = intrp_d; 374b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; 375b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][1] = CorrSurfPtr1[peak - 1]; 376b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][1] = corr; 377b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 378b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } else if (intrp_c == corr_max) { 379b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[k] += 0.5; 380b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[k] += 0.5; 381b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][0] = intrp_a; 382b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)], &T[2][0]); 383b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][1] = intrp_c; 384b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][2] = intrp_d; 385b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)], &T[2][2]); 386b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; 387b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][1] = corr; 388b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][1] = CorrSurfPtr1[peak + 1]; 389b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 390b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } else { 391b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[k] += 0.5; 392b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[k] -= 0.5; 393b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][0] = intrp_b; 394b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][0] = intrp_c; 395b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][1] = intrp_d; 396b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak + 2*(PITCH_LAG_SPAN2+4)], &T[0][2]); 397b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org IntrepolFilter(&CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)], &T[2][2]); 398b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][0] = corr; 399b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[0][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; 400b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[2][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; 401b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org T[1][2] = CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)]; 402b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 403b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 404b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 405b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* 2D parabolic interpolation gives more accurate lags and peak value */ 406b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org Intrpol2D(T, &lags1[k], &lags2[k], &peak_vals[k]); 407b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 408b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 409b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* determine the highest peak, after applying a bias towards short lags */ 410b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = 0.0; 411b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < peaks_ind; k++) { 412b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr = peak_vals[k] * pow(PITCH_PEAK_DECAY, log(lags1[k] + lags2[k])); 413b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (corr > corr_max) { 414b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org corr_max = corr; 415b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org peak = k; 416b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 417b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 418b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 419b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[peak] *= 2.0; 420b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[peak] *= 2.0; 421b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 422b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags1[peak] < (double) PITCH_MIN_LAG) lags1[peak] = (double) PITCH_MIN_LAG; 423b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags2[peak] < (double) PITCH_MIN_LAG) lags2[peak] = (double) PITCH_MIN_LAG; 424b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags1[peak] > (double) PITCH_MAX_LAG) lags1[peak] = (double) PITCH_MAX_LAG; 425b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags2[peak] > (double) PITCH_MAX_LAG) lags2[peak] = (double) PITCH_MAX_LAG; 426b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 427b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* store lags of highest peak in output array */ 428b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[0] = lags1[peak]; 429b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[1] = lags1[peak]; 430b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[2] = lags2[peak]; 431b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[3] = lags2[peak]; 432b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 433b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org else 434b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 435b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org row = max_ind / (PITCH_LAG_SPAN2+4); 436b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags1[0] = (double) ((max_ind - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); 437b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags2[0] = (double) (lags1[0] + PITCH_BW - row); 438b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 439b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags1[0] < (double) PITCH_MIN_LAG) lags1[0] = (double) PITCH_MIN_LAG; 440b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags2[0] < (double) PITCH_MIN_LAG) lags2[0] = (double) PITCH_MIN_LAG; 441b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags1[0] > (double) PITCH_MAX_LAG) lags1[0] = (double) PITCH_MAX_LAG; 442b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (lags2[0] > (double) PITCH_MAX_LAG) lags2[0] = (double) PITCH_MAX_LAG; 443b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 444b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* store lags of highest peak in output array */ 445b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[0] = lags1[0]; 446b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[1] = lags1[0]; 447b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[2] = lags2[0]; 448b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org lags[3] = lags2[0]; 449b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 450b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org} 451b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 452b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 453b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 454b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org/* create weighting matrix by orthogonalizing a basis of polynomials of increasing order 455b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * t = (0:4)'; 456b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * A = [t.^0, t.^1, t.^2, t.^3, t.^4]; 457b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * [Q, dummy] = qr(A); 458b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * P.Weight = Q * diag([0, .1, .5, 1, 1]) * Q'; */ 459b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.orgstatic const double kWeight[5][5] = { 460b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 0.29714285714286, -0.30857142857143, -0.05714285714286, 0.05142857142857, 0.01714285714286}, 461b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org {-0.30857142857143, 0.67428571428571, -0.27142857142857, -0.14571428571429, 0.05142857142857}, 462b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org {-0.05714285714286, -0.27142857142857, 0.65714285714286, -0.27142857142857, -0.05714285714286}, 463b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 0.05142857142857, -0.14571428571429, -0.27142857142857, 0.67428571428571, -0.30857142857143}, 464b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org { 0.01714285714286, 0.05142857142857, -0.05714285714286, -0.30857142857143, 0.29714285714286} 465b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org}; 466b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 467b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 468b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.orgvoid WebRtcIsac_PitchAnalysis(const double *in, /* PITCH_FRAME_LEN samples */ 469b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double *out, /* PITCH_FRAME_LEN+QLOOKAHEAD samples */ 470b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org PitchAnalysisStruct *State, 471b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double *lags, 472b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double *gains) 473b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org{ 474b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double HPin[PITCH_FRAME_LEN]; 475b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double Weighted[PITCH_FRAME_LEN]; 476b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double Whitened[PITCH_FRAME_LEN + QLOOKAHEAD]; 477b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double inbuf[PITCH_FRAME_LEN + QLOOKAHEAD]; 478b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double out_G[PITCH_FRAME_LEN + QLOOKAHEAD]; // could be removed by using out instead 479b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double out_dG[4][PITCH_FRAME_LEN + QLOOKAHEAD]; 480b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double old_lag, old_gain; 481b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double nrg_wht, tmp; 482b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double Wnrg, Wfluct, Wgain; 483b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double H[4][4]; 484b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double grad[4]; 485b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org double dG[4]; 486b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org int k, m, n, iter; 487b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 488b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* high pass filtering using second order pole-zero filter */ 489b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_Highpass(in, HPin, State->hp_state, PITCH_FRAME_LEN); 490b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 491b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* copy from state into buffer */ 492b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(Whitened, State->whitened_buf, sizeof(double) * QLOOKAHEAD); 493b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 494b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute weighted and whitened signals */ 495b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_WeightingFilter(HPin, &Weighted[0], &Whitened[QLOOKAHEAD], &(State->Wghtstr)); 496b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 497b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* copy from buffer into state */ 498b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(State->whitened_buf, Whitened+PITCH_FRAME_LEN, sizeof(double) * QLOOKAHEAD); 499b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 500b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org old_lag = State->PFstr_wght.oldlagp[0]; 501b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org old_gain = State->PFstr_wght.oldgainp[0]; 502b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 503b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* inital pitch estimate */ 504b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_InitializePitch(Weighted, old_lag, old_gain, State, lags); 505b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 506b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 507b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* Iterative optimization of lags - to be done */ 508b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 509b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute energy of whitened signal */ 510b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org nrg_wht = 0.0; 511b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < PITCH_FRAME_LEN + QLOOKAHEAD; k++) 512b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org nrg_wht += Whitened[k] * Whitened[k]; 513b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 514b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 515b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* Iterative optimization of gains */ 516b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 517b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* set weights for energy, gain fluctiation, and spectral gain penalty functions */ 518b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org Wnrg = 1.0 / nrg_wht; 519b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org Wgain = 0.005; 520b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org Wfluct = 3.0; 521b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 522b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* set initial gains */ 523b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) 524b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gains[k] = PITCH_MAX_GAIN_06; 525b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 526b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* two iterations should be enough */ 527b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (iter = 0; iter < 2; iter++) { 528b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute Jacobian of pre-filter output towards gains */ 529b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_PitchfilterPre_gains(Whitened, out_G, out_dG, &(State->PFstr_wght), lags, gains); 530b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 531b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* gradient and approximate Hessian (lower triangle) for minimizing the filter's output power */ 532b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) { 533b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp = 0.0; 534b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) 535b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp += out_G[n] * out_dG[k][n]; 536b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org grad[k] = tmp * Wnrg; 537b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 538b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) { 539b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = 0; m <= k; m++) { 540b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp = 0.0; 541b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) 542b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp += out_dG[m][n] * out_dG[k][n]; 543b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[k][m] = tmp * Wnrg; 544b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 545b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 546b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 547b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* add gradient and Hessian (lower triangle) for dampening fast gain changes */ 548b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) { 549b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp = kWeight[k+1][0] * old_gain; 550b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = 0; m < 4; m++) 551b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp += kWeight[k+1][m+1] * gains[m]; 552b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org grad[k] += tmp * Wfluct; 553b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 554b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) { 555b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (m = 0; m <= k; m++) { 556b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[k][m] += kWeight[k+1][m+1] * Wfluct; 557b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 558b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 559b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 560b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* add gradient and Hessian for dampening gain */ 561b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 3; k++) { 562b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp = 1.0 / (1 - gains[k]); 563b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org grad[k] += tmp * tmp * Wgain; 564b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[k][k] += 2.0 * tmp * (tmp * tmp * Wgain); 565b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 566b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org tmp = 1.0 / (1 - gains[3]); 567b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org grad[3] += 1.33 * (tmp * tmp * Wgain); 568b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[3][3] += 2.66 * tmp * (tmp * tmp * Wgain); 569b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 570b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 571b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* compute Cholesky factorization of Hessian 572b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * by overwritting the upper triangle; scale factors on diagonal 573b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org * (for non pc-platforms store the inverse of the diagonals seperately to minimize divisions) */ 574b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[0][1] = H[1][0] / H[0][0]; 575b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[0][2] = H[2][0] / H[0][0]; 576b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[0][3] = H[3][0] / H[0][0]; 577b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[1][1] -= H[0][0] * H[0][1] * H[0][1]; 578b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[1][2] = (H[2][1] - H[0][1] * H[2][0]) / H[1][1]; 579b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[1][3] = (H[3][1] - H[0][1] * H[3][0]) / H[1][1]; 580b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[2][2] -= H[0][0] * H[0][2] * H[0][2] + H[1][1] * H[1][2] * H[1][2]; 581b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[2][3] = (H[3][2] - H[0][2] * H[3][0] - H[1][2] * H[1][1] * H[1][3]) / H[2][2]; 582b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org H[3][3] -= H[0][0] * H[0][3] * H[0][3] + H[1][1] * H[1][3] * H[1][3] + H[2][2] * H[2][3] * H[2][3]; 583b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 584b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* Compute update as delta_gains = -inv(H) * grad */ 585b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* copy and negate */ 586b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) 587b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[k] = -grad[k]; 588b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* back substitution */ 589b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[1] -= dG[0] * H[0][1]; 590b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[2] -= dG[0] * H[0][2] + dG[1] * H[1][2]; 591b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[3] -= dG[0] * H[0][3] + dG[1] * H[1][3] + dG[2] * H[2][3]; 592b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* scale */ 593b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) 594b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[k] /= H[k][k]; 595b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* back substitution */ 596b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[2] -= dG[3] * H[2][3]; 597b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[1] -= dG[3] * H[1][3] + dG[2] * H[1][2]; 598b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org dG[0] -= dG[3] * H[0][3] + dG[2] * H[0][2] + dG[1] * H[0][1]; 599b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 600b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* update gains and check range */ 601b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < 4; k++) { 602b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gains[k] += dG[k]; 603b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org if (gains[k] > PITCH_MAX_GAIN) 604b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gains[k] = PITCH_MAX_GAIN; 605b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org else if (gains[k] < 0.0) 606b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org gains[k] = 0.0; 607b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 608b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org } 609b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 610b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* update state for next frame */ 611b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_PitchfilterPre(Whitened, out, &(State->PFstr_wght), lags, gains); 612b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 613b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* concatenate previous input's end and current input */ 614b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(inbuf, State->inbuf, sizeof(double) * QLOOKAHEAD); 615b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org memcpy(inbuf+QLOOKAHEAD, in, sizeof(double) * PITCH_FRAME_LEN); 616b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 617b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* lookahead pitch filtering for masking analysis */ 618b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org WebRtcIsac_PitchfilterPre_la(inbuf, out, &(State->PFstr), lags, gains); 619b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org 620b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org /* store last part of input */ 621b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org for (k = 0; k < QLOOKAHEAD; k++) 622b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org State->inbuf[k] = inbuf[k + PITCH_FRAME_LEN]; 623b015cbede88899f67a53fbbe581b02ce8e32794andrew@webrtc.org} 624