libcelt/pitch.c

   1 /* Copyright (c) 2007-2008 CSIRO
   2    Copyright (c) 2007-2009 Xiph.Org Foundation
   3    Written by Jean-Marc Valin */
   4 /**
   5    @file pitch.c
   6    @brief Pitch analysis
   7  */
   8
   9 /*
  10    Redistribution and use in source and binary forms, with or without
  11    modification, are permitted provided that the following conditions
  12    are met:
  13
  14    - Redistributions of source code must retain the above copyright
  15    notice, this list of conditions and the following disclaimer.
  16
  17    - Redistributions in binary form must reproduce the above copyright
  18    notice, this list of conditions and the following disclaimer in the
  19    documentation and/or other materials provided with the distribution.
  20
  21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  24    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  25    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  32 */
  33
  34
  35 #ifdef HAVE_CONFIG_H
  36 #include "config.h"
  37 #endif
  38
  39 /* Always enable postfilter for Opus */
  40 #if defined(OPUS_BUILD) && !defined(ENABLE_POSTFILTER)
  41 #define ENABLE_POSTFILTER
  42 #endif
  43
  44 #include "pitch.h"
  45 #include "os_support.h"
  46 #include "modes.h"
  47 #include "stack_alloc.h"
  48 #include "mathops.h"
  49
  50 static void find_best_pitch(celt_word32 *xcorr, celt_word32 maxcorr, celt_word16 *y,
  51                             int yshift, int len, int max_pitch, int best_pitch[2])
  52 {
  53    int i, j;
  54    celt_word32 Syy=1;
  55    celt_word16 best_num[2];
  56    celt_word32 best_den[2];
  57 #ifdef FIXED_POINT
  58    int xshift;
  59
  60    xshift = celt_ilog2(maxcorr)-14;
  61 #endif
  62
  63    best_num[0] = -1;
  64    best_num[1] = -1;
  65    best_den[0] = 0;
  66    best_den[1] = 0;
  67    best_pitch[0] = 0;
  68    best_pitch[1] = 1;
  69    for (j=0;j<len;j++)
  70       Syy = MAC16_16(Syy, y[j],y[j]);
  71    for (i=0;i<max_pitch;i++)
  72    {
  73       if (xcorr[i]>0)
  74       {
  75          celt_word16 num;
  76          celt_word32 xcorr16;
  77          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
  78          num = MULT16_16_Q15(xcorr16,xcorr16);
  79          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
  80          {
  81             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
  82             {
  83                best_num[1] = best_num[0];
  84                best_den[1] = best_den[0];
  85                best_pitch[1] = best_pitch[0];
  86                best_num[0] = num;
  87                best_den[0] = Syy;
  88                best_pitch[0] = i;
  89             } else {
  90                best_num[1] = num;
  91                best_den[1] = Syy;
  92                best_pitch[1] = i;
  93             }
  94          }
  95       }
  96       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
  97       Syy = MAX32(1, Syy);
  98    }
  99 }
 100
 101 #include "plc.h"
 102 void pitch_downsample(celt_sig * restrict x[], celt_word16 * restrict x_lp,
 103       int len, int _C)
 104 {
 105    int i;
 106    celt_word32 ac[5];
 107    celt_word16 tmp=Q15ONE;
 108    celt_word16 lpc[4], mem[4]={0,0,0,0};
 109    const int C = CHANNELS(_C);
 110    for (i=1;i<len>>1;i++)
 111       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), SIG_SHIFT+3);
 112    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), SIG_SHIFT+3);
 113    if (C==2)
 114    {
 115       for (i=1;i<len>>1;i++)
 116          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), SIG_SHIFT+3);
 117       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), SIG_SHIFT+3);
 118    }
 119
 120    _celt_autocorr(x_lp, ac, NULL, 0,
 121                   4, len>>1);
 122
 123    /* Noise floor -40 dB */
 124 #ifdef FIXED_POINT
 125    ac[0] += SHR32(ac[0],13);
 126 #else
 127    ac[0] *= 1.0001f;
 128 #endif
 129    /* Lag windowing */
 130    for (i=1;i<=4;i++)
 131    {
 132       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
 133 #ifdef FIXED_POINT
 134       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
 135 #else
 136       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
 137 #endif
 138    }
 139
 140    _celt_lpc(lpc, ac, 4);
 141    for (i=0;i<4;i++)
 142    {
 143       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
 144       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
 145    }
 146    fir(x_lp, lpc, x_lp, len>>1, 4, mem);
 147
 148    mem[0]=0;
 149    lpc[0]=QCONST16(.8f,12);
 150    fir(x_lp, lpc, x_lp, len>>1, 1, mem);
 151
 152 }
 153
 154 void pitch_search(const celt_word16 * restrict x_lp, celt_word16 * restrict y,
 155                   int len, int max_pitch, int *pitch)
 156 {
 157    int i, j;
 158    int lag;
 159    int best_pitch[2]={0};
 160    VARDECL(celt_word16, x_lp4);
 161    VARDECL(celt_word16, y_lp4);
 162    VARDECL(celt_word32, xcorr);
 163    celt_word32 maxcorr=1;
 164    int offset;
 165    int shift=0;
 166
 167    SAVE_STACK;
 168
 169    lag = len+max_pitch;
 170
 171    ALLOC(x_lp4, len>>2, celt_word16);
 172    ALLOC(y_lp4, lag>>2, celt_word16);
 173    ALLOC(xcorr, max_pitch>>1, celt_word32);
 174
 175    /* Downsample by 2 again */
 176    for (j=0;j<len>>2;j++)
 177       x_lp4[j] = x_lp[2*j];
 178    for (j=0;j<lag>>2;j++)
 179       y_lp4[j] = y[2*j];
 180
 181 #ifdef FIXED_POINT
 182    shift = celt_ilog2(MAX16(1, MAX16(celt_maxabs16(x_lp4, len>>2), celt_maxabs16(y_lp4, lag>>2))))-11;
 183    if (shift>0)
 184    {
 185       for (j=0;j<len>>2;j++)
 186          x_lp4[j] = SHR16(x_lp4[j], shift);
 187       for (j=0;j<lag>>2;j++)
 188          y_lp4[j] = SHR16(y_lp4[j], shift);
 189       /* Use double the shift for a MAC */
 190       shift *= 2;
 191    } else {
 192       shift = 0;
 193    }
 194 #endif
 195
 196    /* Coarse search with 4x decimation */
 197
 198    for (i=0;i<max_pitch>>2;i++)
 199    {
 200       celt_word32 sum = 0;
 201       for (j=0;j<len>>2;j++)
 202          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
 203       xcorr[i] = MAX32(-1, sum);
 204       maxcorr = MAX32(maxcorr, sum);
 205    }
 206    find_best_pitch(xcorr, maxcorr, y_lp4, 0, len>>2, max_pitch>>2, best_pitch);
 207
 208    /* Finer search with 2x decimation */
 209    maxcorr=1;
 210    for (i=0;i<max_pitch>>1;i++)
 211    {
 212       celt_word32 sum=0;
 213       xcorr[i] = 0;
 214       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
 215          continue;
 216       for (j=0;j<len>>1;j++)
 217          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
 218       xcorr[i] = MAX32(-1, sum);
 219       maxcorr = MAX32(maxcorr, sum);
 220    }
 221    find_best_pitch(xcorr, maxcorr, y, shift, len>>1, max_pitch>>1, best_pitch);
 222
 223    /* Refine by pseudo-interpolation */
 224    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
 225    {
 226       celt_word32 a, b, c;
 227       a = xcorr[best_pitch[0]-1];
 228       b = xcorr[best_pitch[0]];
 229       c = xcorr[best_pitch[0]+1];
 230       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
 231          offset = 1;
 232       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
 233          offset = -1;
 234       else
 235          offset = 0;
 236    } else {
 237       offset = 0;
 238    }
 239    *pitch = 2*best_pitch[0]-offset;
 240
 241    RESTORE_STACK;
 242 }
 243
 244 #ifdef ENABLE_POSTFILTER
 245 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
 246 celt_word16 remove_doubling(celt_word16 *x, int maxperiod, int minperiod,
 247       int N, int *_T0, int prev_period, celt_word16 prev_gain)
 248 {
 249    int k, i, T, T0;
 250    celt_word16 g, g0;
 251    celt_word16 pg;
 252    celt_word32 xy,xx,yy;
 253    celt_word32 xcorr[3];
 254    celt_word32 best_xy, best_yy;
 255    int offset;
 256    int minperiod0;
 257
 258    minperiod0 = minperiod;
 259    maxperiod /= 2;
 260    minperiod /= 2;
 261    *_T0 /= 2;
 262    prev_period /= 2;
 263    N /= 2;
 264    x += maxperiod;
 265    if (*_T0>=maxperiod)
 266       *_T0=maxperiod-1;
 267
 268    T = T0 = *_T0;
 269    xx=xy=yy=0;
 270    for (i=0;i<N;i++)
 271    {
 272       xy = MAC16_16(xy, x[i], x[i-T0]);
 273       xx = MAC16_16(xx, x[i], x[i]);
 274       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
 275    }
 276    best_xy = xy;
 277    best_yy = yy;
 278 #ifdef FIXED_POINT
 279       {
 280          celt_word32 x2y2;
 281          int sh, t;
 282          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
 283          sh = celt_ilog2(x2y2)>>1;
 284          t = VSHR32(x2y2, 2*(sh-7));
 285          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 286       }
 287 #else
 288       g = g0 = xy/sqrt(1+xx*yy);
 289 #endif
 290    /* Look for any pitch at T/k */
 291    for (k=2;k<=15;k++)
 292    {
 293       int T1, T1b;
 294       celt_word16 g1;
 295       celt_word16 cont=0;
 296       T1 = (2*T0+k)/(2*k);
 297       if (T1 < minperiod)
 298          break;
 299       /* Look for another strong correlation at T1b */
 300       if (k==2)
 301       {
 302          if (T1+T0>maxperiod)
 303             T1b = T0;
 304          else
 305             T1b = T0+T1;
 306       } else
 307       {
 308          T1b = (2*second_check[k]*T0+k)/(2*k);
 309       }
 310       xy=yy=0;
 311       for (i=0;i<N;i++)
 312       {
 313          xy = MAC16_16(xy, x[i], x[i-T1]);
 314          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
 315
 316          xy = MAC16_16(xy, x[i], x[i-T1b]);
 317          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
 318       }
 319 #ifdef FIXED_POINT
 320       {
 321          celt_word32 x2y2;
 322          int sh, t;
 323          x2y2 = 1+MULT32_32_Q31(xx,yy);
 324          sh = celt_ilog2(x2y2)>>1;
 325          t = VSHR32(x2y2, 2*(sh-7));
 326          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 327       }
 328 #else
 329       g1 = xy/sqrt(1+2.f*xx*1.f*yy);
 330 #endif
 331       if (abs(T1-prev_period)<=1)
 332          cont = prev_gain;
 333       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
 334          cont = HALF32(prev_gain);
 335       else
 336          cont = 0;
 337       if (g1 > QCONST16(.3f,15) + MULT16_16_Q15(QCONST16(.4f,15),g0)-cont)
 338       {
 339          best_xy = xy;
 340          best_yy = yy;
 341          T = T1;
 342          g = g1;
 343       }
 344    }
 345    if (best_yy <= best_xy)
 346       pg = Q15ONE;
 347    else
 348       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
 349
 350    for (k=0;k<3;k++)
 351    {
 352       int T1 = T+k-1;
 353       xy = 0;
 354       for (i=0;i<N;i++)
 355          xy = MAC16_16(xy, x[i], x[i-T1]);
 356       xcorr[k] = xy;
 357    }
 358    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
 359       offset = 1;
 360    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
 361       offset = -1;
 362    else
 363       offset = 0;
 364    if (pg > g)
 365       pg = g;
 366    *_T0 = 2*T+offset;
 367
 368    if (*_T0<minperiod0)
 369       *_T0=minperiod0;
 370    return pg;
 371 }
 372
 373 #endif /* ENABLE_POSTFILTER */