src/third_party/webrtc/modules/audio_coding/codecs/isac/fix/source/pitch_estimator.c

   1 /*
   2  *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
   3  *
   4  *  Use of this source code is governed by a BSD-style license
   5  *  that can be found in the LICENSE file in the root of the source
   6  *  tree. An additional intellectual property rights grant can be found
   7  *  in the file PATENTS.  All contributing project authors may
   8  *  be found in the AUTHORS file in the root of the source tree.
   9  */
  10
  11 #include "webrtc/modules/audio_coding/codecs/isac/fix/source/pitch_estimator.h"
  12
  13 #ifdef WEBRTC_ARCH_ARM_NEON
  14 #include <arm_neon.h>
  15 #endif
  16
  17 #include "webrtc/common_audio/signal_processing/include/signal_processing_library.h"
  18 #include "webrtc/system_wrappers/interface/compile_assert_c.h"
  19
  20 /* log2[0.2, 0.5, 0.98] in Q8 */
  21 static const int16_t kLogLagWinQ8[3] = {
  22   -594, -256, -7
  23 };
  24
  25 /* [1 -0.75 0.25] in Q12 */
  26 static const int16_t kACoefQ12[3] = {
  27   4096, -3072, 1024
  28 };
  29
  30 int32_t WebRtcIsacfix_Log2Q8(uint32_t x) {
  31   int32_t zeros, lg2;
  32   int16_t frac;
  33
  34   zeros=WebRtcSpl_NormU32(x);
  35   frac = (int16_t)(((x << zeros) & 0x7FFFFFFF) >> 23);
  36   /* log2(magn(i)) */
  37
  38   lg2= (WEBRTC_SPL_LSHIFT_W32((31-zeros), 8)+frac);
  39   return lg2;
  40
  41 }
  42
  43 static __inline int16_t Exp2Q10(int16_t x) { // Both in and out in Q10
  44
  45   int16_t tmp16_1, tmp16_2;
  46
  47   tmp16_2=(int16_t)(0x0400|(x&0x03FF));
  48   tmp16_1 = -(x >> 10);
  49   if(tmp16_1>0)
  50     return tmp16_2 >> tmp16_1;
  51   else
  52     return tmp16_2 << -tmp16_1;
  53
  54 }
  55
  56
  57
  58 /* 1D parabolic interpolation . All input and output values are in Q8 */
  59 static __inline void Intrp1DQ8(int32_t *x, int32_t *fx, int32_t *y, int32_t *fy) {
  60
  61   int16_t sign1=1, sign2=1;
  62   int32_t r32, q32, t32, nom32, den32;
  63   int16_t t16, tmp16, tmp16_1;
  64
  65   if ((fx[0]>0) && (fx[2]>0)) {
  66     r32=fx[1]-fx[2];
  67     q32=fx[0]-fx[1];
  68     nom32=q32+r32;
  69     den32 = (q32 - r32) * 2;
  70     if (nom32<0)
  71       sign1=-1;
  72     if (den32<0)
  73       sign2=-1;
  74
  75     /* t = (q32+r32)/(2*(q32-r32)) = (fx[0]-fx[1] + fx[1]-fx[2])/(2 * fx[0]-fx[1] - (fx[1]-fx[2]))*/
  76     /* (Signs are removed because WebRtcSpl_DivResultInQ31 can't handle negative numbers) */
  77     /* t in Q31, without signs */
  78     t32 = WebRtcSpl_DivResultInQ31(nom32 * sign1, den32 * sign2);
  79
  80     t16 = (int16_t)(t32 >> 23);  /* Q8 */
  81     t16=t16*sign1*sign2;        /* t in Q8 with signs */
  82
  83     *y = x[0]+t16;          /* Q8 */
  84     // *y = x[1]+t16;          /* Q8 */
  85
  86     /* The following code calculates fy in three steps */
  87     /* fy = 0.5 * t * (t-1) * fx[0] + (1-t*t) * fx[1] + 0.5 * t * (t+1) * fx[2]; */
  88
  89     /* Part I: 0.5 * t * (t-1) * fx[0] */
  90     tmp16_1=(int16_t)WEBRTC_SPL_MUL_16_16(t16,t16); /* Q8*Q8=Q16 */
  91     tmp16_1 >>= 2;  /* Q16>>2 = Q14 */
  92     t16 = (int16_t)WEBRTC_SPL_MUL_16_16(t16, 64);           /* Q8<<6 = Q14  */
  93     tmp16 = tmp16_1-t16;
  94     *fy = WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[0]); /* (Q14 * Q8 >>15)/2 = Q8 */
  95
  96     /* Part II: (1-t*t) * fx[1] */
  97     tmp16 = 16384-tmp16_1;        /* 1 in Q14 - Q14 */
  98     *fy += WEBRTC_SPL_MUL_16_32_RSFT14(tmp16, fx[1]);/* Q14 * Q8 >> 14 = Q8 */
  99
 100     /* Part III: 0.5 * t * (t+1) * fx[2] */
 101     tmp16 = tmp16_1+t16;
 102     *fy += WEBRTC_SPL_MUL_16_32_RSFT15(tmp16, fx[2]);/* (Q14 * Q8 >>15)/2 = Q8 */
 103   } else {
 104     *y = x[0];
 105     *fy= fx[1];
 106   }
 107 }
 108
 109
 110 static void FindFour32(int32_t *in, int16_t length, int16_t *bestind)
 111 {
 112   int32_t best[4]= {-100, -100, -100, -100};
 113   int16_t k;
 114
 115   for (k=0; k<length; k++) {
 116     if (in[k] > best[3]) {
 117       if (in[k] > best[2]) {
 118         if (in[k] > best[1]) {
 119           if (in[k] > best[0]) { // The Best
 120             best[3] = best[2];
 121             bestind[3] = bestind[2];
 122             best[2] = best[1];
 123             bestind[2] = bestind[1];
 124             best[1] = best[0];
 125             bestind[1] = bestind[0];
 126             best[0] = in[k];
 127             bestind[0] = k;
 128           } else { // 2nd best
 129             best[3] = best[2];
 130             bestind[3] = bestind[2];
 131             best[2] = best[1];
 132             bestind[2] = bestind[1];
 133             best[1] = in[k];
 134             bestind[1] = k;
 135           }
 136         } else { // 3rd best
 137           best[3] = best[2];
 138           bestind[3] = bestind[2];
 139           best[2] = in[k];
 140           bestind[2] = k;
 141         }
 142       } else {  // 4th best
 143         best[3] = in[k];
 144         bestind[3] = k;
 145       }
 146     }
 147   }
 148 }
 149
 150
 151
 152
 153
 154 extern void WebRtcIsacfix_PCorr2Q32(const int16_t *in, int32_t *logcorQ8);
 155
 156
 157
 158 void WebRtcIsacfix_InitialPitch(const int16_t *in, /* Q0 */
 159                                 PitchAnalysisStruct *State,
 160                                 int16_t *lagsQ7                   /* Q7 */
 161                                 )
 162 {
 163   int16_t buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2];
 164   int32_t *crrvecQ8_1,*crrvecQ8_2;
 165   int32_t cv1q[PITCH_LAG_SPAN2+2],cv2q[PITCH_LAG_SPAN2+2], peakvq[PITCH_LAG_SPAN2+2];
 166   int k;
 167   int16_t peaks_indq;
 168   int16_t peakiq[PITCH_LAG_SPAN2];
 169   int32_t corr;
 170   int32_t corr32, corr_max32, corr_max_o32;
 171   int16_t npkq;
 172   int16_t best4q[4]={0,0,0,0};
 173   int32_t xq[3],yq[1],fyq[1];
 174   int32_t *fxq;
 175   int32_t best_lag1q, best_lag2q;
 176   int32_t tmp32a,tmp32b,lag32,ratq;
 177   int16_t start;
 178   int16_t oldgQ12, tmp16a, tmp16b, gain_bias16,tmp16c, tmp16d, bias16;
 179   int32_t tmp32c,tmp32d, tmp32e;
 180   int16_t old_lagQ;
 181   int32_t old_lagQ8;
 182   int32_t lagsQ8[4];
 183
 184   old_lagQ = State->PFstr_wght.oldlagQ7; // Q7
 185   old_lagQ8= WEBRTC_SPL_LSHIFT_W32((int32_t)old_lagQ,1); //Q8
 186
 187   oldgQ12= State->PFstr_wght.oldgainQ12;
 188
 189   crrvecQ8_1=&cv1q[1];
 190   crrvecQ8_2=&cv2q[1];
 191
 192
 193   /* copy old values from state buffer */
 194   memcpy(buf_dec16, State->dec_buffer16, WEBRTC_SPL_MUL_16_16(sizeof(int16_t), (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2)));
 195
 196   /* decimation; put result after the old values */
 197   WebRtcIsacfix_DecimateAllpass32(in, State->decimator_state32, PITCH_FRAME_LEN,
 198                                   &buf_dec16[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]);
 199
 200   /* low-pass filtering */
 201   start= PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2;
 202   WebRtcSpl_FilterARFastQ12(&buf_dec16[start],&buf_dec16[start],(int16_t*)kACoefQ12,3, PITCH_FRAME_LEN/2);
 203
 204   /* copy end part back into state buffer */
 205   for (k = 0; k < (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2); k++)
 206     State->dec_buffer16[k] = buf_dec16[k+PITCH_FRAME_LEN/2];
 207
 208
 209   /* compute correlation for first and second half of the frame */
 210   WebRtcIsacfix_PCorr2Q32(buf_dec16, crrvecQ8_1);
 211   WebRtcIsacfix_PCorr2Q32(buf_dec16 + PITCH_CORR_STEP2, crrvecQ8_2);
 212
 213
 214   /* bias towards pitch lag of previous frame */
 215   tmp32a = WebRtcIsacfix_Log2Q8((uint32_t) old_lagQ8) - 2304;
 216       // log2(0.5*oldlag) in Q8
 217   tmp32b = WEBRTC_SPL_MUL_16_16_RSFT(oldgQ12,oldgQ12, 10); //Q12 & * 4.0;
 218   gain_bias16 = (int16_t) tmp32b;  //Q12
 219   if (gain_bias16 > 3276) gain_bias16 = 3276; // 0.8 in Q12
 220
 221
 222   for (k = 0; k < PITCH_LAG_SPAN2; k++)
 223   {
 224     if (crrvecQ8_1[k]>0) {
 225       tmp32b = WebRtcIsacfix_Log2Q8((uint32_t) (k + (PITCH_MIN_LAG/2-2)));
 226       tmp16a = (int16_t) (tmp32b - tmp32a); // Q8 & fabs(ratio)<4
 227       tmp32c = WEBRTC_SPL_MUL_16_16_RSFT(tmp16a,tmp16a, 6); //Q10
 228       tmp16b = (int16_t) tmp32c; // Q10 & <8
 229       tmp32d = WEBRTC_SPL_MUL_16_16_RSFT(tmp16b, 177 , 8); // mult with ln2 in Q8
 230       tmp16c = (int16_t) tmp32d; // Q10 & <4
 231       tmp16d = Exp2Q10((int16_t) -tmp16c); //Q10
 232       tmp32c = WEBRTC_SPL_MUL_16_16_RSFT(gain_bias16,tmp16d,13); // Q10  & * 0.5
 233       bias16 = (int16_t) (1024 + tmp32c); // Q10
 234       tmp32b = WebRtcIsacfix_Log2Q8((uint32_t)bias16) - 2560;
 235           // Q10 in -> Q8 out with 10*2^8 offset
 236       crrvecQ8_1[k] += tmp32b ; // -10*2^8 offset
 237     }
 238   }
 239
 240   /* taper correlation functions */
 241   for (k = 0; k < 3; k++) {
 242     crrvecQ8_1[k] += kLogLagWinQ8[k];
 243     crrvecQ8_2[k] += kLogLagWinQ8[k];
 244
 245     crrvecQ8_1[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k];
 246     crrvecQ8_2[PITCH_LAG_SPAN2-1-k] += kLogLagWinQ8[k];
 247   }
 248
 249
 250   /* Make zeropadded corr vectors */
 251   cv1q[0]=0;
 252   cv2q[0]=0;
 253   cv1q[PITCH_LAG_SPAN2+1]=0;
 254   cv2q[PITCH_LAG_SPAN2+1]=0;
 255   corr_max32 = 0;
 256
 257   for (k = 1; k <= PITCH_LAG_SPAN2; k++)
 258   {
 259
 260
 261     corr32=crrvecQ8_1[k-1];
 262     if (corr32 > corr_max32)
 263       corr_max32 = corr32;
 264
 265     corr32=crrvecQ8_2[k-1];
 266     corr32 += -4; // Compensate for later (log2(0.99))
 267
 268     if (corr32 > corr_max32)
 269       corr_max32 = corr32;
 270
 271   }
 272
 273   /* threshold value to qualify as a peak */
 274   // corr_max32 += -726; // log(0.14)/log(2.0) in Q8
 275   corr_max32 += -1000; // log(0.14)/log(2.0) in Q8
 276   corr_max_o32 = corr_max32;
 277
 278
 279   /* find peaks in corr1 */
 280   peaks_indq = 0;
 281   for (k = 1; k <= PITCH_LAG_SPAN2; k++)
 282   {
 283     corr32=cv1q[k];
 284     if (corr32>corr_max32) { // Disregard small peaks
 285       if ((corr32>=cv1q[k-1]) && (corr32>cv1q[k+1])) { // Peak?
 286         peakvq[peaks_indq] = corr32;
 287         peakiq[peaks_indq++] = k;
 288       }
 289     }
 290   }
 291
 292
 293   /* find highest interpolated peak */
 294   corr_max32=0;
 295   best_lag1q =0;
 296   if (peaks_indq > 0) {
 297     FindFour32(peakvq, (int16_t) peaks_indq, best4q);
 298     npkq = WEBRTC_SPL_MIN(peaks_indq, 4);
 299
 300     for (k=0;k<npkq;k++) {
 301
 302       lag32 =  peakiq[best4q[k]];
 303       fxq = &cv1q[peakiq[best4q[k]]-1];
 304       xq[0]= lag32;
 305       xq[0] = WEBRTC_SPL_LSHIFT_W32(xq[0], 8);
 306       Intrp1DQ8(xq, fxq, yq, fyq);
 307
 308       tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8
 309       /* Bias towards short lags */
 310       /* log(pow(0.8, log(2.0 * *y )))/log(2.0) */
 311       tmp32b= WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32a, -42, 8);
 312       tmp32c= tmp32b + 256;
 313       *fyq += tmp32c;
 314       if (*fyq > corr_max32) {
 315         corr_max32 = *fyq;
 316         best_lag1q = *yq;
 317       }
 318     }
 319     tmp32a = best_lag1q - OFFSET_Q8;
 320     tmp32b = WEBRTC_SPL_LSHIFT_W32(tmp32a, 1);
 321     lagsQ8[0] = tmp32b + PITCH_MIN_LAG_Q8;
 322     lagsQ8[1] = lagsQ8[0];
 323   } else {
 324     lagsQ8[0] = old_lagQ8;
 325     lagsQ8[1] = lagsQ8[0];
 326   }
 327
 328   /* Bias towards constant pitch */
 329   tmp32a = lagsQ8[0] - PITCH_MIN_LAG_Q8;
 330   ratq = (tmp32a >> 1) + OFFSET_Q8;
 331
 332   for (k = 1; k <= PITCH_LAG_SPAN2; k++)
 333   {
 334     tmp32a = WEBRTC_SPL_LSHIFT_W32(k, 7); // 0.5*k Q8
 335     tmp32b = (int32_t) (WEBRTC_SPL_LSHIFT_W32(tmp32a, 1)) - ratq; // Q8
 336     tmp32c = WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32b, (int16_t) tmp32b, 8); // Q8
 337
 338     tmp32b = tmp32c + (ratq >> 1);
 339         // (k-r)^2 + 0.5 * r  Q8
 340     tmp32c = WebRtcIsacfix_Log2Q8((uint32_t)tmp32a) - 2048;
 341         // offset 8*2^8 , log2(0.5*k) Q8
 342     tmp32d = WebRtcIsacfix_Log2Q8((uint32_t)tmp32b) - 2048;
 343         // offset 8*2^8 , log2(0.5*k) Q8
 344     tmp32e =  tmp32c - tmp32d;
 345
 346     cv2q[k] += tmp32e >> 1;
 347
 348   }
 349
 350   /* find peaks in corr2 */
 351   corr_max32 = corr_max_o32;
 352   peaks_indq = 0;
 353
 354   for (k = 1; k <= PITCH_LAG_SPAN2; k++)
 355   {
 356     corr=cv2q[k];
 357     if (corr>corr_max32) { // Disregard small peaks
 358       if ((corr>=cv2q[k-1]) && (corr>cv2q[k+1])) { // Peak?
 359         peakvq[peaks_indq] = corr;
 360         peakiq[peaks_indq++] = k;
 361       }
 362     }
 363   }
 364
 365
 366
 367   /* find highest interpolated peak */
 368   corr_max32 = 0;
 369   best_lag2q =0;
 370   if (peaks_indq > 0) {
 371
 372     FindFour32(peakvq, (int16_t) peaks_indq, best4q);
 373     npkq = WEBRTC_SPL_MIN(peaks_indq, 4);
 374     for (k=0;k<npkq;k++) {
 375
 376       lag32 =  peakiq[best4q[k]];
 377       fxq = &cv2q[peakiq[best4q[k]]-1];
 378
 379       xq[0]= lag32;
 380       xq[0] = WEBRTC_SPL_LSHIFT_W32(xq[0], 8);
 381       Intrp1DQ8(xq, fxq, yq, fyq);
 382
 383       /* Bias towards short lags */
 384       /* log(pow(0.8, log(2.0f * *y )))/log(2.0f) */
 385       tmp32a= WebRtcIsacfix_Log2Q8((uint32_t) *yq) - 2048; // offset 8*2^8
 386       tmp32b= WEBRTC_SPL_MUL_16_16_RSFT((int16_t) tmp32a, -82, 8);
 387       tmp32c= tmp32b + 256;
 388       *fyq += tmp32c;
 389       if (*fyq > corr_max32) {
 390         corr_max32 = *fyq;
 391         best_lag2q = *yq;
 392       }
 393     }
 394
 395     tmp32a = best_lag2q - OFFSET_Q8;
 396     tmp32b = WEBRTC_SPL_LSHIFT_W32(tmp32a, 1);
 397     lagsQ8[2] = tmp32b + PITCH_MIN_LAG_Q8;
 398     lagsQ8[3] = lagsQ8[2];
 399   } else {
 400     lagsQ8[2] = lagsQ8[0];
 401     lagsQ8[3] = lagsQ8[0];
 402   }
 403
 404   lagsQ7[0] = (int16_t)(lagsQ8[0] >> 1);
 405   lagsQ7[1] = (int16_t)(lagsQ8[1] >> 1);
 406   lagsQ7[2] = (int16_t)(lagsQ8[2] >> 1);
 407   lagsQ7[3] = (int16_t)(lagsQ8[3] >> 1);
 408 }
 409
 410
 411
 412 void WebRtcIsacfix_PitchAnalysis(const int16_t *inn,               /* PITCH_FRAME_LEN samples */
 413                                  int16_t *outQ0,                  /* PITCH_FRAME_LEN+QLOOKAHEAD samples */
 414                                  PitchAnalysisStruct *State,
 415                                  int16_t *PitchLags_Q7,
 416                                  int16_t *PitchGains_Q12)
 417 {
 418   int16_t inbufQ0[PITCH_FRAME_LEN + QLOOKAHEAD];
 419   int16_t k;
 420
 421   /* inital pitch estimate */
 422   WebRtcIsacfix_InitialPitch(inn, State,  PitchLags_Q7);
 423
 424
 425   /* Calculate gain */
 426   WebRtcIsacfix_PitchFilterGains(inn, &(State->PFstr_wght), PitchLags_Q7, PitchGains_Q12);
 427
 428   /* concatenate previous input's end and current input */
 429   for (k = 0; k < QLOOKAHEAD; k++) {
 430     inbufQ0[k] = State->inbuf[k];
 431   }
 432   for (k = 0; k < PITCH_FRAME_LEN; k++) {
 433     inbufQ0[k+QLOOKAHEAD] = (int16_t) inn[k];
 434   }
 435
 436   /* lookahead pitch filtering for masking analysis */
 437   WebRtcIsacfix_PitchFilter(inbufQ0, outQ0, &(State->PFstr), PitchLags_Q7,PitchGains_Q12, 2);
 438
 439
 440   /* store last part of input */
 441   for (k = 0; k < QLOOKAHEAD; k++) {
 442     State->inbuf[k] = inbufQ0[k + PITCH_FRAME_LEN];
 443   }
 444 }