| /* |
| * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. |
| * |
| * Use of this source code is governed by a BSD-style license |
| * that can be found in the LICENSE file in the root of the source |
| * tree. An additional intellectual property rights grant can be found |
| * in the file PATENTS. All contributing project authors may |
| * be found in the AUTHORS file in the root of the source tree. |
| */ |
| |
| #include "pitch_estimator.h" |
| |
| #include <math.h> |
| #include <memory.h> |
| #ifdef WEBRTC_ANDROID |
| #include <stdlib.h> |
| #endif |
| |
| static const double kInterpolWin[8] = {-0.00067556028640, 0.02184247643159, -0.12203175715679, 0.60086484101160, |
| 0.60086484101160, -0.12203175715679, 0.02184247643159, -0.00067556028640}; |
| |
| /* interpolation filter */ |
| __inline static void IntrepolFilter(double *data_ptr, double *intrp) |
| { |
| *intrp = kInterpolWin[0] * data_ptr[-3]; |
| *intrp += kInterpolWin[1] * data_ptr[-2]; |
| *intrp += kInterpolWin[2] * data_ptr[-1]; |
| *intrp += kInterpolWin[3] * data_ptr[0]; |
| *intrp += kInterpolWin[4] * data_ptr[1]; |
| *intrp += kInterpolWin[5] * data_ptr[2]; |
| *intrp += kInterpolWin[6] * data_ptr[3]; |
| *intrp += kInterpolWin[7] * data_ptr[4]; |
| } |
| |
| |
| /* 2D parabolic interpolation */ |
| /* probably some 0.5 factors can be eliminated, and the square-roots can be removed from the Cholesky fact. */ |
| __inline static void Intrpol2D(double T[3][3], double *x, double *y, double *peak_val) |
| { |
| double c, b[2], A[2][2]; |
| double t1, t2, d; |
| double delta1, delta2; |
| |
| |
| // double T[3][3] = {{-1.25, -.25,-.25}, {-.25, .75, .75}, {-.25, .75, .75}}; |
| // should result in: delta1 = 0.5; delta2 = 0.0; peak_val = 1.0 |
| |
| c = T[1][1]; |
| b[0] = 0.5 * (T[1][2] + T[2][1] - T[0][1] - T[1][0]); |
| b[1] = 0.5 * (T[1][0] + T[2][1] - T[0][1] - T[1][2]); |
| A[0][1] = -0.5 * (T[0][1] + T[2][1] - T[1][0] - T[1][2]); |
| t1 = 0.5 * (T[0][0] + T[2][2]) - c; |
| t2 = 0.5 * (T[2][0] + T[0][2]) - c; |
| d = (T[0][1] + T[1][2] + T[1][0] + T[2][1]) - 4.0 * c - t1 - t2; |
| A[0][0] = -t1 - 0.5 * d; |
| A[1][1] = -t2 - 0.5 * d; |
| |
| /* deal with singularities or ill-conditioned cases */ |
| if ( (A[0][0] < 1e-7) || ((A[0][0] * A[1][1] - A[0][1] * A[0][1]) < 1e-7) ) { |
| *peak_val = T[1][1]; |
| return; |
| } |
| |
| /* Cholesky decomposition: replace A by upper-triangular factor */ |
| A[0][0] = sqrt(A[0][0]); |
| A[0][1] = A[0][1] / A[0][0]; |
| A[1][1] = sqrt(A[1][1] - A[0][1] * A[0][1]); |
| |
| /* compute [x; y] = -0.5 * inv(A) * b */ |
| t1 = b[0] / A[0][0]; |
| t2 = (b[1] - t1 * A[0][1]) / A[1][1]; |
| delta2 = t2 / A[1][1]; |
| delta1 = 0.5 * (t1 - delta2 * A[0][1]) / A[0][0]; |
| delta2 *= 0.5; |
| |
| /* limit norm */ |
| t1 = delta1 * delta1 + delta2 * delta2; |
| if (t1 > 1.0) { |
| delta1 /= t1; |
| delta2 /= t1; |
| } |
| |
| *peak_val = 0.5 * (b[0] * delta1 + b[1] * delta2) + c; |
| |
| *x += delta1; |
| *y += delta2; |
| } |
| |
| |
| static void PCorr(const double *in, double *outcorr) |
| { |
| double sum, ysum, prod; |
| const double *x, *inptr; |
| int k, n; |
| |
| //ysum = 1e-6; /* use this with float (i.s.o. double)! */ |
| ysum = 1e-13; |
| sum = 0.0; |
| x = in + PITCH_MAX_LAG/2 + 2; |
| for (n = 0; n < PITCH_CORR_LEN2; n++) { |
| ysum += in[n] * in[n]; |
| sum += x[n] * in[n]; |
| } |
| |
| outcorr += PITCH_LAG_SPAN2 - 1; /* index of last element in array */ |
| *outcorr = sum / sqrt(ysum); |
| |
| for (k = 1; k < PITCH_LAG_SPAN2; k++) { |
| ysum -= in[k-1] * in[k-1]; |
| ysum += in[PITCH_CORR_LEN2 + k - 1] * in[PITCH_CORR_LEN2 + k - 1]; |
| sum = 0.0; |
| inptr = &in[k]; |
| prod = x[0] * inptr[0]; |
| for (n = 1; n < PITCH_CORR_LEN2; n++) { |
| sum += prod; |
| prod = x[n] * inptr[n]; |
| } |
| sum += prod; |
| outcorr--; |
| *outcorr = sum / sqrt(ysum); |
| } |
| } |
| |
| |
| void WebRtcIsac_InitializePitch(const double *in, |
| const double old_lag, |
| const double old_gain, |
| PitchAnalysisStruct *State, |
| double *lags) |
| { |
| double buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2]; |
| double ratio, log_lag, gain_bias; |
| double bias; |
| double corrvec1[PITCH_LAG_SPAN2]; |
| double corrvec2[PITCH_LAG_SPAN2]; |
| int m, k; |
| // Allocating 10 extra entries at the begining of the CorrSurf |
| double corrSurfBuff[10 + (2*PITCH_BW+3)*(PITCH_LAG_SPAN2+4)]; |
| double* CorrSurf[2*PITCH_BW+3]; |
| double *CorrSurfPtr1, *CorrSurfPtr2; |
| double LagWin[3] = {0.2, 0.5, 0.98}; |
| int ind1, ind2, peaks_ind, peak, max_ind; |
| int peaks[PITCH_MAX_NUM_PEAKS]; |
| double adj, gain_tmp; |
| double corr, corr_max; |
| double intrp_a, intrp_b, intrp_c, intrp_d; |
| double peak_vals[PITCH_MAX_NUM_PEAKS]; |
| double lags1[PITCH_MAX_NUM_PEAKS]; |
| double lags2[PITCH_MAX_NUM_PEAKS]; |
| double T[3][3]; |
| int row; |
| |
| for(k = 0; k < 2*PITCH_BW+3; k++) |
| { |
| CorrSurf[k] = &corrSurfBuff[10 + k * (PITCH_LAG_SPAN2+4)]; |
| } |
| /* reset CorrSurf matrix */ |
| memset(corrSurfBuff, 0, sizeof(double) * (10 + (2*PITCH_BW+3) * (PITCH_LAG_SPAN2+4))); |
| |
| //warnings -DH |
| max_ind = 0; |
| peak = 0; |
| |
| /* copy old values from state buffer */ |
| memcpy(buf_dec, State->dec_buffer, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)); |
| |
| /* decimation; put result after the old values */ |
| WebRtcIsac_DecimateAllpass(in, State->decimator_state, PITCH_FRAME_LEN, |
| &buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]); |
| |
| /* low-pass filtering */ |
| 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++) |
| buf_dec[k] += 0.75 * buf_dec[k-1] - 0.25 * buf_dec[k-2]; |
| |
| /* copy end part back into state buffer */ |
| 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)); |
| |
| /* compute correlation for first and second half of the frame */ |
| PCorr(buf_dec, corrvec1); |
| PCorr(buf_dec + PITCH_CORR_STEP2, corrvec2); |
| |
| /* bias towards pitch lag of previous frame */ |
| log_lag = log(0.5 * old_lag); |
| gain_bias = 4.0 * old_gain * old_gain; |
| if (gain_bias > 0.8) gain_bias = 0.8; |
| for (k = 0; k < PITCH_LAG_SPAN2; k++) |
| { |
| ratio = log((double) (k + (PITCH_MIN_LAG/2-2))) - log_lag; |
| bias = 1.0 + gain_bias * exp(-5.0 * ratio * ratio); |
| corrvec1[k] *= bias; |
| } |
| |
| /* taper correlation functions */ |
| for (k = 0; k < 3; k++) { |
| gain_tmp = LagWin[k]; |
| corrvec1[k] *= gain_tmp; |
| corrvec2[k] *= gain_tmp; |
| corrvec1[PITCH_LAG_SPAN2-1-k] *= gain_tmp; |
| corrvec2[PITCH_LAG_SPAN2-1-k] *= gain_tmp; |
| } |
| |
| corr_max = 0.0; |
| /* fill middle row of correlation surface */ |
| ind1 = 0; |
| ind2 = 0; |
| CorrSurfPtr1 = &CorrSurf[PITCH_BW][2]; |
| for (k = 0; k < PITCH_LAG_SPAN2; k++) { |
| corr = corrvec1[ind1++] + corrvec2[ind2++]; |
| CorrSurfPtr1[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| } |
| } |
| /* fill first and last rows of correlation surface */ |
| ind1 = 0; |
| ind2 = PITCH_BW; |
| CorrSurfPtr1 = &CorrSurf[0][2]; |
| CorrSurfPtr2 = &CorrSurf[2*PITCH_BW][PITCH_BW+2]; |
| for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW; k++) { |
| ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); |
| adj = 0.2 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ |
| corr = adj * (corrvec1[ind1] + corrvec2[ind2]); |
| CorrSurfPtr1[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| } |
| corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); |
| CorrSurfPtr2[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); |
| } |
| } |
| /* fill second and next to last rows of correlation surface */ |
| ind1 = 0; |
| ind2 = PITCH_BW-1; |
| CorrSurfPtr1 = &CorrSurf[1][2]; |
| CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-1][PITCH_BW+1]; |
| for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+1; k++) { |
| ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); |
| adj = 0.9 * ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ |
| corr = adj * (corrvec1[ind1] + corrvec2[ind2]); |
| CorrSurfPtr1[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| } |
| corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); |
| CorrSurfPtr2[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); |
| } |
| } |
| /* fill remainder of correlation surface */ |
| for (m = 2; m < PITCH_BW; m++) { |
| ind1 = 0; |
| ind2 = PITCH_BW - m; /* always larger than ind1 */ |
| CorrSurfPtr1 = &CorrSurf[m][2]; |
| CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-m][PITCH_BW+2-m]; |
| for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+m; k++) { |
| ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12)); |
| adj = ratio * (2.0 - ratio); /* adjustment factor; inverse parabola as a function of ratio */ |
| corr = adj * (corrvec1[ind1] + corrvec2[ind2]); |
| CorrSurfPtr1[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| } |
| corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]); |
| CorrSurfPtr2[k] = corr; |
| if (corr > corr_max) { |
| corr_max = corr; /* update maximum */ |
| max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]); |
| } |
| } |
| } |
| |
| /* threshold value to qualify as a peak */ |
| corr_max *= 0.6; |
| |
| peaks_ind = 0; |
| /* find peaks */ |
| for (m = 1; m < PITCH_BW+1; m++) { |
| if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; |
| CorrSurfPtr1 = &CorrSurf[m][2]; |
| for (k = 2; k < PITCH_LAG_SPAN2-PITCH_BW-2+m; k++) { |
| corr = CorrSurfPtr1[k]; |
| if (corr > corr_max) { |
| if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { |
| if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { |
| /* found a peak; store index into matrix */ |
| peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; |
| } |
| } |
| } |
| } |
| } |
| for (m = PITCH_BW+1; m < 2*PITCH_BW; m++) { |
| if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; |
| CorrSurfPtr1 = &CorrSurf[m][2]; |
| for (k = 2+m-PITCH_BW; k < PITCH_LAG_SPAN2-2; k++) { |
| corr = CorrSurfPtr1[k]; |
| if (corr > corr_max) { |
| if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) { |
| if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) { |
| /* found a peak; store index into matrix */ |
| peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]); |
| if (peaks_ind == PITCH_MAX_NUM_PEAKS) break; |
| } |
| } |
| } |
| } |
| } |
| |
| if (peaks_ind > 0) { |
| /* examine each peak */ |
| CorrSurfPtr1 = &CorrSurf[0][0]; |
| for (k = 0; k < peaks_ind; k++) { |
| peak = peaks[k]; |
| |
| /* compute four interpolated values around current peak */ |
| IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)], &intrp_a); |
| IntrepolFilter(&CorrSurfPtr1[peak - 1 ], &intrp_b); |
| IntrepolFilter(&CorrSurfPtr1[peak ], &intrp_c); |
| IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)], &intrp_d); |
| |
| /* determine maximum of the interpolated values */ |
| corr = CorrSurfPtr1[peak]; |
| corr_max = intrp_a; |
| if (intrp_b > corr_max) corr_max = intrp_b; |
| if (intrp_c > corr_max) corr_max = intrp_c; |
| if (intrp_d > corr_max) corr_max = intrp_d; |
| |
| /* determine where the peak sits and fill a 3x3 matrix around it */ |
| row = peak / (PITCH_LAG_SPAN2+4); |
| lags1[k] = (double) ((peak - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); |
| lags2[k] = (double) (lags1[k] + PITCH_BW - row); |
| if ( corr > corr_max ) { |
| T[0][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; |
| T[2][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; |
| T[1][1] = corr; |
| T[0][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; |
| T[2][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; |
| T[1][0] = intrp_a; |
| T[0][1] = intrp_b; |
| T[2][1] = intrp_c; |
| T[1][2] = intrp_d; |
| } else { |
| if (intrp_a == corr_max) { |
| lags1[k] -= 0.5; |
| lags2[k] += 0.5; |
| IntrepolFilter(&CorrSurfPtr1[peak - 2*(PITCH_LAG_SPAN2+5)], &T[0][0]); |
| IntrepolFilter(&CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)], &T[2][0]); |
| T[1][1] = intrp_a; |
| T[0][2] = intrp_b; |
| T[2][2] = intrp_c; |
| T[1][0] = CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)]; |
| T[0][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; |
| T[2][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; |
| T[1][2] = corr; |
| } else if (intrp_b == corr_max) { |
| lags1[k] -= 0.5; |
| lags2[k] -= 0.5; |
| IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+6)], &T[0][0]); |
| T[2][0] = intrp_a; |
| T[1][1] = intrp_b; |
| IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+3)], &T[0][2]); |
| T[2][2] = intrp_d; |
| T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)]; |
| T[0][1] = CorrSurfPtr1[peak - 1]; |
| T[2][1] = corr; |
| T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; |
| } else if (intrp_c == corr_max) { |
| lags1[k] += 0.5; |
| lags2[k] += 0.5; |
| T[0][0] = intrp_a; |
| IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)], &T[2][0]); |
| T[1][1] = intrp_c; |
| T[0][2] = intrp_d; |
| IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)], &T[2][2]); |
| T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)]; |
| T[0][1] = corr; |
| T[2][1] = CorrSurfPtr1[peak + 1]; |
| T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; |
| } else { |
| lags1[k] += 0.5; |
| lags2[k] -= 0.5; |
| T[0][0] = intrp_b; |
| T[2][0] = intrp_c; |
| T[1][1] = intrp_d; |
| IntrepolFilter(&CorrSurfPtr1[peak + 2*(PITCH_LAG_SPAN2+4)], &T[0][2]); |
| IntrepolFilter(&CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)], &T[2][2]); |
| T[1][0] = corr; |
| T[0][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)]; |
| T[2][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)]; |
| T[1][2] = CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)]; |
| } |
| } |
| |
| /* 2D parabolic interpolation gives more accurate lags and peak value */ |
| Intrpol2D(T, &lags1[k], &lags2[k], &peak_vals[k]); |
| } |
| |
| /* determine the highest peak, after applying a bias towards short lags */ |
| corr_max = 0.0; |
| for (k = 0; k < peaks_ind; k++) { |
| corr = peak_vals[k] * pow(PITCH_PEAK_DECAY, log(lags1[k] + lags2[k])); |
| if (corr > corr_max) { |
| corr_max = corr; |
| peak = k; |
| } |
| } |
| |
| lags1[peak] *= 2.0; |
| lags2[peak] *= 2.0; |
| |
| if (lags1[peak] < (double) PITCH_MIN_LAG) lags1[peak] = (double) PITCH_MIN_LAG; |
| if (lags2[peak] < (double) PITCH_MIN_LAG) lags2[peak] = (double) PITCH_MIN_LAG; |
| if (lags1[peak] > (double) PITCH_MAX_LAG) lags1[peak] = (double) PITCH_MAX_LAG; |
| if (lags2[peak] > (double) PITCH_MAX_LAG) lags2[peak] = (double) PITCH_MAX_LAG; |
| |
| /* store lags of highest peak in output array */ |
| lags[0] = lags1[peak]; |
| lags[1] = lags1[peak]; |
| lags[2] = lags2[peak]; |
| lags[3] = lags2[peak]; |
| } |
| else |
| { |
| row = max_ind / (PITCH_LAG_SPAN2+4); |
| lags1[0] = (double) ((max_ind - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4); |
| lags2[0] = (double) (lags1[0] + PITCH_BW - row); |
| |
| if (lags1[0] < (double) PITCH_MIN_LAG) lags1[0] = (double) PITCH_MIN_LAG; |
| if (lags2[0] < (double) PITCH_MIN_LAG) lags2[0] = (double) PITCH_MIN_LAG; |
| if (lags1[0] > (double) PITCH_MAX_LAG) lags1[0] = (double) PITCH_MAX_LAG; |
| if (lags2[0] > (double) PITCH_MAX_LAG) lags2[0] = (double) PITCH_MAX_LAG; |
| |
| /* store lags of highest peak in output array */ |
| lags[0] = lags1[0]; |
| lags[1] = lags1[0]; |
| lags[2] = lags2[0]; |
| lags[3] = lags2[0]; |
| } |
| } |
| |
| |
| |
| /* create weighting matrix by orthogonalizing a basis of polynomials of increasing order |
| * t = (0:4)'; |
| * A = [t.^0, t.^1, t.^2, t.^3, t.^4]; |
| * [Q, dummy] = qr(A); |
| * P.Weight = Q * diag([0, .1, .5, 1, 1]) * Q'; */ |
| static const double kWeight[5][5] = { |
| { 0.29714285714286, -0.30857142857143, -0.05714285714286, 0.05142857142857, 0.01714285714286}, |
| {-0.30857142857143, 0.67428571428571, -0.27142857142857, -0.14571428571429, 0.05142857142857}, |
| {-0.05714285714286, -0.27142857142857, 0.65714285714286, -0.27142857142857, -0.05714285714286}, |
| { 0.05142857142857, -0.14571428571429, -0.27142857142857, 0.67428571428571, -0.30857142857143}, |
| { 0.01714285714286, 0.05142857142857, -0.05714285714286, -0.30857142857143, 0.29714285714286} |
| }; |
| |
| |
| void WebRtcIsac_PitchAnalysis(const double *in, /* PITCH_FRAME_LEN samples */ |
| double *out, /* PITCH_FRAME_LEN+QLOOKAHEAD samples */ |
| PitchAnalysisStruct *State, |
| double *lags, |
| double *gains) |
| { |
| double HPin[PITCH_FRAME_LEN]; |
| double Weighted[PITCH_FRAME_LEN]; |
| double Whitened[PITCH_FRAME_LEN + QLOOKAHEAD]; |
| double inbuf[PITCH_FRAME_LEN + QLOOKAHEAD]; |
| double out_G[PITCH_FRAME_LEN + QLOOKAHEAD]; // could be removed by using out instead |
| double out_dG[4][PITCH_FRAME_LEN + QLOOKAHEAD]; |
| double old_lag, old_gain; |
| double nrg_wht, tmp; |
| double Wnrg, Wfluct, Wgain; |
| double H[4][4]; |
| double grad[4]; |
| double dG[4]; |
| int k, m, n, iter; |
| |
| /* high pass filtering using second order pole-zero filter */ |
| WebRtcIsac_Highpass(in, HPin, State->hp_state, PITCH_FRAME_LEN); |
| |
| /* copy from state into buffer */ |
| memcpy(Whitened, State->whitened_buf, sizeof(double) * QLOOKAHEAD); |
| |
| /* compute weighted and whitened signals */ |
| WebRtcIsac_WeightingFilter(HPin, &Weighted[0], &Whitened[QLOOKAHEAD], &(State->Wghtstr)); |
| |
| /* copy from buffer into state */ |
| memcpy(State->whitened_buf, Whitened+PITCH_FRAME_LEN, sizeof(double) * QLOOKAHEAD); |
| |
| old_lag = State->PFstr_wght.oldlagp[0]; |
| old_gain = State->PFstr_wght.oldgainp[0]; |
| |
| /* inital pitch estimate */ |
| WebRtcIsac_InitializePitch(Weighted, old_lag, old_gain, State, lags); |
| |
| |
| /* Iterative optimization of lags - to be done */ |
| |
| /* compute energy of whitened signal */ |
| nrg_wht = 0.0; |
| for (k = 0; k < PITCH_FRAME_LEN + QLOOKAHEAD; k++) |
| nrg_wht += Whitened[k] * Whitened[k]; |
| |
| |
| /* Iterative optimization of gains */ |
| |
| /* set weights for energy, gain fluctiation, and spectral gain penalty functions */ |
| Wnrg = 1.0 / nrg_wht; |
| Wgain = 0.005; |
| Wfluct = 3.0; |
| |
| /* set initial gains */ |
| for (k = 0; k < 4; k++) |
| gains[k] = PITCH_MAX_GAIN_06; |
| |
| /* two iterations should be enough */ |
| for (iter = 0; iter < 2; iter++) { |
| /* compute Jacobian of pre-filter output towards gains */ |
| WebRtcIsac_PitchfilterPre_gains(Whitened, out_G, out_dG, &(State->PFstr_wght), lags, gains); |
| |
| /* gradient and approximate Hessian (lower triangle) for minimizing the filter's output power */ |
| for (k = 0; k < 4; k++) { |
| tmp = 0.0; |
| for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) |
| tmp += out_G[n] * out_dG[k][n]; |
| grad[k] = tmp * Wnrg; |
| } |
| for (k = 0; k < 4; k++) { |
| for (m = 0; m <= k; m++) { |
| tmp = 0.0; |
| for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++) |
| tmp += out_dG[m][n] * out_dG[k][n]; |
| H[k][m] = tmp * Wnrg; |
| } |
| } |
| |
| /* add gradient and Hessian (lower triangle) for dampening fast gain changes */ |
| for (k = 0; k < 4; k++) { |
| tmp = kWeight[k+1][0] * old_gain; |
| for (m = 0; m < 4; m++) |
| tmp += kWeight[k+1][m+1] * gains[m]; |
| grad[k] += tmp * Wfluct; |
| } |
| for (k = 0; k < 4; k++) { |
| for (m = 0; m <= k; m++) { |
| H[k][m] += kWeight[k+1][m+1] * Wfluct; |
| } |
| } |
| |
| /* add gradient and Hessian for dampening gain */ |
| for (k = 0; k < 3; k++) { |
| tmp = 1.0 / (1 - gains[k]); |
| grad[k] += tmp * tmp * Wgain; |
| H[k][k] += 2.0 * tmp * (tmp * tmp * Wgain); |
| } |
| tmp = 1.0 / (1 - gains[3]); |
| grad[3] += 1.33 * (tmp * tmp * Wgain); |
| H[3][3] += 2.66 * tmp * (tmp * tmp * Wgain); |
| |
| |
| /* compute Cholesky factorization of Hessian |
| * by overwritting the upper triangle; scale factors on diagonal |
| * (for non pc-platforms store the inverse of the diagonals seperately to minimize divisions) */ |
| H[0][1] = H[1][0] / H[0][0]; |
| H[0][2] = H[2][0] / H[0][0]; |
| H[0][3] = H[3][0] / H[0][0]; |
| H[1][1] -= H[0][0] * H[0][1] * H[0][1]; |
| H[1][2] = (H[2][1] - H[0][1] * H[2][0]) / H[1][1]; |
| H[1][3] = (H[3][1] - H[0][1] * H[3][0]) / H[1][1]; |
| H[2][2] -= H[0][0] * H[0][2] * H[0][2] + H[1][1] * H[1][2] * H[1][2]; |
| H[2][3] = (H[3][2] - H[0][2] * H[3][0] - H[1][2] * H[1][1] * H[1][3]) / H[2][2]; |
| 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]; |
| |
| /* Compute update as delta_gains = -inv(H) * grad */ |
| /* copy and negate */ |
| for (k = 0; k < 4; k++) |
| dG[k] = -grad[k]; |
| /* back substitution */ |
| dG[1] -= dG[0] * H[0][1]; |
| dG[2] -= dG[0] * H[0][2] + dG[1] * H[1][2]; |
| dG[3] -= dG[0] * H[0][3] + dG[1] * H[1][3] + dG[2] * H[2][3]; |
| /* scale */ |
| for (k = 0; k < 4; k++) |
| dG[k] /= H[k][k]; |
| /* back substitution */ |
| dG[2] -= dG[3] * H[2][3]; |
| dG[1] -= dG[3] * H[1][3] + dG[2] * H[1][2]; |
| dG[0] -= dG[3] * H[0][3] + dG[2] * H[0][2] + dG[1] * H[0][1]; |
| |
| /* update gains and check range */ |
| for (k = 0; k < 4; k++) { |
| gains[k] += dG[k]; |
| if (gains[k] > PITCH_MAX_GAIN) |
| gains[k] = PITCH_MAX_GAIN; |
| else if (gains[k] < 0.0) |
| gains[k] = 0.0; |
| } |
| } |
| |
| /* update state for next frame */ |
| WebRtcIsac_PitchfilterPre(Whitened, out, &(State->PFstr_wght), lags, gains); |
| |
| /* concatenate previous input's end and current input */ |
| memcpy(inbuf, State->inbuf, sizeof(double) * QLOOKAHEAD); |
| memcpy(inbuf+QLOOKAHEAD, in, sizeof(double) * PITCH_FRAME_LEN); |
| |
| /* lookahead pitch filtering for masking analysis */ |
| WebRtcIsac_PitchfilterPre_la(inbuf, out, &(State->PFstr), lags, gains); |
| |
| /* store last part of input */ |
| for (k = 0; k < QLOOKAHEAD; k++) |
| State->inbuf[k] = inbuf[k + PITCH_FRAME_LEN]; |
| } |
