celt/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 COPYRIGHT OWNER
  25    OR 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 #ifdef HAVE_CONFIG_H
  35 #include "config.h"
  36 #endif
  37
  38 #include "pitch.h"
  39 #include "os_support.h"
  40 #include "modes.h"
  41 #include "stack_alloc.h"
  42 #include "mathops.h"
  43 #include "celt_lpc.h"
  44
  45 static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
  46                             int max_pitch, int *best_pitch
  47 #ifdef FIXED_POINT
  48                             , int yshift, opus_val32 maxcorr
  49 #endif
  50                             )
  51 {
  52    int i, j;
  53    opus_val32 Syy=1;
  54    opus_val16 best_num[2];
  55    opus_val32 best_den[2];
  56 #ifdef FIXED_POINT
  57    int xshift;
  58
  59    xshift = celt_ilog2(maxcorr)-14;
  60 #endif
  61
  62    best_num[0] = -1;
  63    best_num[1] = -1;
  64    best_den[0] = 0;
  65    best_den[1] = 0;
  66    best_pitch[0] = 0;
  67    best_pitch[1] = 1;
  68    for (j=0;j<len;j++)
  69       Syy = ADD32(Syy, SHR32(MULT16_16(y[j],y[j]), yshift));
  70    for (i=0;i<max_pitch;i++)
  71    {
  72       if (xcorr[i]>0)
  73       {
  74          opus_val16 num;
  75          opus_val32 xcorr16;
  76          xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
  77 #ifndef FIXED_POINT
  78          /* Considering the range of xcorr16, this should avoid both underflows
  79             and overflows (inf) when squaring xcorr16 */
  80          xcorr16 *= 1e-12;
  81 #endif
  82          num = MULT16_16_Q15(xcorr16,xcorr16);
  83          if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
  84          {
  85             if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
  86             {
  87                best_num[1] = best_num[0];
  88                best_den[1] = best_den[0];
  89                best_pitch[1] = best_pitch[0];
  90                best_num[0] = num;
  91                best_den[0] = Syy;
  92                best_pitch[0] = i;
  93             } else {
  94                best_num[1] = num;
  95                best_den[1] = Syy;
  96                best_pitch[1] = i;
  97             }
  98          }
  99       }
 100       Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
 101       Syy = MAX32(1, Syy);
 102    }
 103 }
 104
 105 void pitch_downsample(celt_sig * restrict x[], opus_val16 * restrict x_lp,
 106       int len, int C)
 107 {
 108    int i;
 109    opus_val32 ac[5];
 110    opus_val16 tmp=Q15ONE;
 111    opus_val16 lpc[4], mem[4]={0,0,0,0};
 112 #ifdef FIXED_POINT
 113    int shift;
 114    opus_val32 maxabs = celt_maxabs32(x[0], len);
 115    if (C==2)
 116    {
 117       opus_val32 maxabs_1 = celt_maxabs32(x[1], len);
 118       maxabs = MAX32(maxabs, maxabs_1);
 119    }
 120    if (maxabs<1)
 121       maxabs=1;
 122    shift = celt_ilog2(maxabs)-10;
 123    if (shift<0)
 124       shift=0;
 125    if (C==2)
 126       shift++;
 127 #endif
 128    for (i=1;i<len>>1;i++)
 129       x_lp[i] = SHR32(HALF32(HALF32(x[0][(2*i-1)]+x[0][(2*i+1)])+x[0][2*i]), shift);
 130    x_lp[0] = SHR32(HALF32(HALF32(x[0][1])+x[0][0]), shift);
 131    if (C==2)
 132    {
 133       for (i=1;i<len>>1;i++)
 134          x_lp[i] += SHR32(HALF32(HALF32(x[1][(2*i-1)]+x[1][(2*i+1)])+x[1][2*i]), shift);
 135       x_lp[0] += SHR32(HALF32(HALF32(x[1][1])+x[1][0]), shift);
 136    }
 137
 138    _celt_autocorr(x_lp, ac, NULL, 0,
 139                   4, len>>1);
 140
 141    /* Noise floor -40 dB */
 142 #ifdef FIXED_POINT
 143    ac[0] += SHR32(ac[0],13);
 144 #else
 145    ac[0] *= 1.0001f;
 146 #endif
 147    /* Lag windowing */
 148    for (i=1;i<=4;i++)
 149    {
 150       /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/
 151 #ifdef FIXED_POINT
 152       ac[i] -= MULT16_32_Q15(2*i*i, ac[i]);
 153 #else
 154       ac[i] -= ac[i]*(.008f*i)*(.008f*i);
 155 #endif
 156    }
 157
 158    _celt_lpc(lpc, ac, 4);
 159    for (i=0;i<4;i++)
 160    {
 161       tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp);
 162       lpc[i] = MULT16_16_Q15(lpc[i], tmp);
 163    }
 164    celt_fir(x_lp, lpc, x_lp, len>>1, 4, mem);
 165
 166    mem[0]=0;
 167    lpc[0]=QCONST16(.8f,12);
 168    celt_fir(x_lp, lpc, x_lp, len>>1, 1, mem);
 169
 170 }
 171
 172 void pitch_search(const opus_val16 * restrict x_lp, opus_val16 * restrict y,
 173                   int len, int max_pitch, int *pitch)
 174 {
 175    int i, j;
 176    int lag;
 177    int best_pitch[2]={0,0};
 178    VARDECL(opus_val16, x_lp4);
 179    VARDECL(opus_val16, y_lp4);
 180    VARDECL(opus_val32, xcorr);
 181 #ifdef FIXED_POINT
 182    opus_val32 maxcorr=1;
 183    opus_val16 xmax, ymax;
 184    int shift=0;
 185 #endif
 186    int offset;
 187
 188    SAVE_STACK;
 189
 190    celt_assert(len>0);
 191    celt_assert(max_pitch>0);
 192    lag = len+max_pitch;
 193
 194    ALLOC(x_lp4, len>>2, opus_val16);
 195    ALLOC(y_lp4, lag>>2, opus_val16);
 196    ALLOC(xcorr, max_pitch>>1, opus_val32);
 197
 198    /* Downsample by 2 again */
 199    for (j=0;j<len>>2;j++)
 200       x_lp4[j] = x_lp[2*j];
 201    for (j=0;j<lag>>2;j++)
 202       y_lp4[j] = y[2*j];
 203
 204 #ifdef FIXED_POINT
 205    xmax = celt_maxabs16(x_lp4, len>>2);
 206    ymax = celt_maxabs16(y_lp4, lag>>2);
 207    shift = celt_ilog2(MAX16(1, MAX16(xmax, ymax)))-11;
 208    if (shift>0)
 209    {
 210       for (j=0;j<len>>2;j++)
 211          x_lp4[j] = SHR16(x_lp4[j], shift);
 212       for (j=0;j<lag>>2;j++)
 213          y_lp4[j] = SHR16(y_lp4[j], shift);
 214       /* Use double the shift for a MAC */
 215       shift *= 2;
 216    } else {
 217       shift = 0;
 218    }
 219 #endif
 220
 221    /* Coarse search with 4x decimation */
 222
 223    for (i=0;i<max_pitch>>2;i++)
 224    {
 225       opus_val32 sum = 0;
 226       for (j=0;j<len>>2;j++)
 227          sum = MAC16_16(sum, x_lp4[j],y_lp4[i+j]);
 228       xcorr[i] = MAX32(-1, sum);
 229 #ifdef FIXED_POINT
 230       maxcorr = MAX32(maxcorr, sum);
 231 #endif
 232    }
 233    find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch
 234 #ifdef FIXED_POINT
 235                    , 0, maxcorr
 236 #endif
 237                    );
 238
 239    /* Finer search with 2x decimation */
 240 #ifdef FIXED_POINT
 241    maxcorr=1;
 242 #endif
 243    for (i=0;i<max_pitch>>1;i++)
 244    {
 245       opus_val32 sum=0;
 246       xcorr[i] = 0;
 247       if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2)
 248          continue;
 249       for (j=0;j<len>>1;j++)
 250          sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift);
 251       xcorr[i] = MAX32(-1, sum);
 252 #ifdef FIXED_POINT
 253       maxcorr = MAX32(maxcorr, sum);
 254 #endif
 255    }
 256    find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch
 257 #ifdef FIXED_POINT
 258                    , shift+1, maxcorr
 259 #endif
 260                    );
 261
 262    /* Refine by pseudo-interpolation */
 263    if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1)
 264    {
 265       opus_val32 a, b, c;
 266       a = xcorr[best_pitch[0]-1];
 267       b = xcorr[best_pitch[0]];
 268       c = xcorr[best_pitch[0]+1];
 269       if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a))
 270          offset = 1;
 271       else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c))
 272          offset = -1;
 273       else
 274          offset = 0;
 275    } else {
 276       offset = 0;
 277    }
 278    *pitch = 2*best_pitch[0]-offset;
 279
 280    RESTORE_STACK;
 281 }
 282
 283 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2};
 284 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod,
 285       int N, int *T0_, int prev_period, opus_val16 prev_gain)
 286 {
 287    int k, i, T, T0;
 288    opus_val16 g, g0;
 289    opus_val16 pg;
 290    opus_val32 xy,xx,yy;
 291    opus_val32 xcorr[3];
 292    opus_val32 best_xy, best_yy;
 293    int offset;
 294    int minperiod0;
 295
 296    minperiod0 = minperiod;
 297    maxperiod /= 2;
 298    minperiod /= 2;
 299    *T0_ /= 2;
 300    prev_period /= 2;
 301    N /= 2;
 302    x += maxperiod;
 303    if (*T0_>=maxperiod)
 304       *T0_=maxperiod-1;
 305
 306    T = T0 = *T0_;
 307    xx=xy=yy=0;
 308    for (i=0;i<N;i++)
 309    {
 310       xy = MAC16_16(xy, x[i], x[i-T0]);
 311       xx = MAC16_16(xx, x[i], x[i]);
 312       yy = MAC16_16(yy, x[i-T0],x[i-T0]);
 313    }
 314    best_xy = xy;
 315    best_yy = yy;
 316 #ifdef FIXED_POINT
 317       {
 318          opus_val32 x2y2;
 319          int sh, t;
 320          x2y2 = 1+HALF32(MULT32_32_Q31(xx,yy));
 321          sh = celt_ilog2(x2y2)>>1;
 322          t = VSHR32(x2y2, 2*(sh-7));
 323          g = g0 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 324       }
 325 #else
 326       g = g0 = xy/celt_sqrt(1+xx*yy);
 327 #endif
 328    /* Look for any pitch at T/k */
 329    for (k=2;k<=15;k++)
 330    {
 331       int T1, T1b;
 332       opus_val16 g1;
 333       opus_val16 cont=0;
 334       opus_val16 thresh;
 335       T1 = (2*T0+k)/(2*k);
 336       if (T1 < minperiod)
 337          break;
 338       /* Look for another strong correlation at T1b */
 339       if (k==2)
 340       {
 341          if (T1+T0>maxperiod)
 342             T1b = T0;
 343          else
 344             T1b = T0+T1;
 345       } else
 346       {
 347          T1b = (2*second_check[k]*T0+k)/(2*k);
 348       }
 349       xy=yy=0;
 350       for (i=0;i<N;i++)
 351       {
 352          xy = MAC16_16(xy, x[i], x[i-T1]);
 353          yy = MAC16_16(yy, x[i-T1], x[i-T1]);
 354
 355          xy = MAC16_16(xy, x[i], x[i-T1b]);
 356          yy = MAC16_16(yy, x[i-T1b], x[i-T1b]);
 357       }
 358 #ifdef FIXED_POINT
 359       {
 360          opus_val32 x2y2;
 361          int sh, t;
 362          x2y2 = 1+MULT32_32_Q31(xx,yy);
 363          sh = celt_ilog2(x2y2)>>1;
 364          t = VSHR32(x2y2, 2*(sh-7));
 365          g1 = VSHR32(MULT16_32_Q15(celt_rsqrt_norm(t), xy),sh+1);
 366       }
 367 #else
 368       g1 = xy/celt_sqrt(1+2.f*xx*1.f*yy);
 369 #endif
 370       if (abs(T1-prev_period)<=1)
 371          cont = prev_gain;
 372       else if (abs(T1-prev_period)<=2 && 5*k*k < T0)
 373          cont = HALF32(prev_gain);
 374       else
 375          cont = 0;
 376       thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7,15),g0)-cont);
 377       /* Bias against very high pitch (very short period) to avoid false-positives
 378          due to short-term correlation */
 379       if (T1<3*minperiod)
 380          thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85,15),g0)-cont);
 381       else if (T1<2*minperiod)
 382          thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9,15),g0)-cont);
 383       if (g1 > thresh)
 384       {
 385          best_xy = xy;
 386          best_yy = yy;
 387          T = T1;
 388          g = g1;
 389       }
 390    }
 391    best_xy = MAX32(0, best_xy);
 392    if (best_yy <= best_xy)
 393       pg = Q15ONE;
 394    else
 395       pg = SHR32(frac_div32(best_xy,best_yy+1),16);
 396
 397    for (k=0;k<3;k++)
 398    {
 399       int T1 = T+k-1;
 400       xy = 0;
 401       for (i=0;i<N;i++)
 402          xy = MAC16_16(xy, x[i], x[i-T1]);
 403       xcorr[k] = xy;
 404    }
 405    if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0]))
 406       offset = 1;
 407    else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2]))
 408       offset = -1;
 409    else
 410       offset = 0;
 411    if (pg > g)
 412       pg = g;
 413    *T0_ = 2*T+offset;
 414
 415    if (*T0_<minperiod0)
 416       *T0_=minperiod0;
 417    return pg;
 418 }