pitch.c (15133B)
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-12f; 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 static void celt_fir5(opus_val16 *x, 106 const opus_val16 *num, 107 int N) 108 { 109 int i; 110 opus_val16 num0, num1, num2, num3, num4; 111 opus_val32 mem0, mem1, mem2, mem3, mem4; 112 num0=num[0]; 113 num1=num[1]; 114 num2=num[2]; 115 num3=num[3]; 116 num4=num[4]; 117 mem0=0; 118 mem1=0; 119 mem2=0; 120 mem3=0; 121 mem4=0; 122 for (i=0;i<N;i++) 123 { 124 opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT); 125 sum = MAC16_16(sum,num0,mem0); 126 sum = MAC16_16(sum,num1,mem1); 127 sum = MAC16_16(sum,num2,mem2); 128 sum = MAC16_16(sum,num3,mem3); 129 sum = MAC16_16(sum,num4,mem4); 130 mem4 = mem3; 131 mem3 = mem2; 132 mem2 = mem1; 133 mem1 = mem0; 134 mem0 = x[i]; 135 x[i] = ROUND16(sum, SIG_SHIFT); 136 } 137 } 138 139 140 void pitch_downsample(celt_sig * OPUS_RESTRICT x[], opus_val16 * OPUS_RESTRICT x_lp, 141 int len, int C, int factor, int arch) 142 { 143 int i; 144 opus_val32 ac[5]; 145 opus_val16 tmp=Q15ONE; 146 opus_val16 lpc[4]; 147 opus_val16 lpc2[5]; 148 opus_val16 c1 = QCONST16(.8f,15); 149 int offset; 150 #ifdef FIXED_POINT 151 int shift; 152 opus_val32 maxabs; 153 #endif 154 offset = factor/2; 155 #ifdef FIXED_POINT 156 maxabs = celt_maxabs32(x[0], len*factor); 157 if (C==2) 158 { 159 opus_val32 maxabs_1 = celt_maxabs32(x[1], len*factor); 160 maxabs = MAX32(maxabs, maxabs_1); 161 } 162 if (maxabs<1) 163 maxabs=1; 164 shift = celt_ilog2(maxabs)-10; 165 if (shift<0) 166 shift=0; 167 if (C==2) 168 shift++; 169 for (i=1;i<len;i++) 170 x_lp[i] = SHR32(x[0][(factor*i-offset)], shift+2) + SHR32(x[0][(factor*i+offset)], shift+2) + SHR32(x[0][factor*i], shift+1); 171 x_lp[0] = SHR32(x[0][offset], shift+2) + SHR32(x[0][0], shift+1); 172 if (C==2) 173 { 174 for (i=1;i<len;i++) 175 x_lp[i] += SHR32(x[1][(factor*i-offset)], shift+2) + SHR32(x[1][(factor*i+offset)], shift+2) + SHR32(x[1][factor*i], shift+1); 176 x_lp[0] += SHR32(x[1][offset], shift+2) + SHR32(x[1][0], shift+1); 177 } 178 #else 179 for (i=1;i<len;i++) 180 x_lp[i] = .25f*x[0][(factor*i-offset)] + .25f*x[0][(factor*i+offset)] + .5f*x[0][factor*i]; 181 x_lp[0] = .25f*x[0][offset] + .5f*x[0][0]; 182 if (C==2) 183 { 184 for (i=1;i<len;i++) 185 x_lp[i] += .25f*x[1][(factor*i-offset)] + .25f*x[1][(factor*i+offset)] + .5f*x[1][factor*i]; 186 x_lp[0] += .25f*x[1][offset] + .5f*x[1][0]; 187 } 188 #endif 189 _celt_autocorr(x_lp, ac, NULL, 0, 190 4, len, arch); 191 192 /* Noise floor -40 dB */ 193 #ifdef FIXED_POINT 194 ac[0] += SHR32(ac[0],13); 195 #else 196 ac[0] *= 1.0001f; 197 #endif 198 /* Lag windowing */ 199 for (i=1;i<=4;i++) 200 { 201 /*ac[i] *= exp(-.5*(2*M_PI*.002*i)*(2*M_PI*.002*i));*/ 202 #ifdef FIXED_POINT 203 ac[i] -= MULT16_32_Q15(2*i*i, ac[i]); 204 #else 205 ac[i] -= ac[i]*(.008f*i)*(.008f*i); 206 #endif 207 } 208 209 _celt_lpc(lpc, ac, 4); 210 for (i=0;i<4;i++) 211 { 212 tmp = MULT16_16_Q15(QCONST16(.9f,15), tmp); 213 lpc[i] = MULT16_16_Q15(lpc[i], tmp); 214 } 215 /* Add a zero */ 216 lpc2[0] = lpc[0] + QCONST16(.8f,SIG_SHIFT); 217 lpc2[1] = lpc[1] + MULT16_16_Q15(c1,lpc[0]); 218 lpc2[2] = lpc[2] + MULT16_16_Q15(c1,lpc[1]); 219 lpc2[3] = lpc[3] + MULT16_16_Q15(c1,lpc[2]); 220 lpc2[4] = MULT16_16_Q15(c1,lpc[3]); 221 celt_fir5(x_lp, lpc2, len); 222 } 223 224 /* Pure C implementation. */ 225 #ifdef FIXED_POINT 226 opus_val32 227 #else 228 void 229 #endif 230 celt_pitch_xcorr_c(const opus_val16 *_x, const opus_val16 *_y, 231 opus_val32 *xcorr, int len, int max_pitch, int arch) 232 { 233 234 #if 0 /* This is a simple version of the pitch correlation that should work 235 well on DSPs like Blackfin and TI C5x/C6x */ 236 int i, j; 237 #ifdef FIXED_POINT 238 opus_val32 maxcorr=1; 239 #endif 240 #if !defined(OVERRIDE_PITCH_XCORR) 241 (void)arch; 242 #endif 243 for (i=0;i<max_pitch;i++) 244 { 245 opus_val32 sum = 0; 246 for (j=0;j<len;j++) 247 sum = MAC16_16(sum, _x[j], _y[i+j]); 248 xcorr[i] = sum; 249 #ifdef FIXED_POINT 250 maxcorr = MAX32(maxcorr, sum); 251 #endif 252 } 253 #ifdef FIXED_POINT 254 return maxcorr; 255 #endif 256 257 #else /* Unrolled version of the pitch correlation -- runs faster on x86 and ARM */ 258 int i; 259 /*The EDSP version requires that max_pitch is at least 1, and that _x is 260 32-bit aligned. 261 Since it's hard to put asserts in assembly, put them here.*/ 262 #ifdef FIXED_POINT 263 opus_val32 maxcorr=1; 264 #endif 265 celt_assert(max_pitch>0); 266 celt_sig_assert(((size_t)_x&3)==0); 267 for (i=0;i<max_pitch-3;i+=4) 268 { 269 opus_val32 sum[4]={0,0,0,0}; 270 #if defined(OPUS_CHECK_ASM) && defined(FIXED_POINT) 271 { 272 opus_val32 sum_c[4]={0,0,0,0}; 273 xcorr_kernel_c(_x, _y+i, sum_c, len); 274 #endif 275 xcorr_kernel(_x, _y+i, sum, len, arch); 276 #if defined(OPUS_CHECK_ASM) && defined(FIXED_POINT) 277 celt_assert(memcmp(sum, sum_c, sizeof(sum)) == 0); 278 } 279 #endif 280 xcorr[i]=sum[0]; 281 xcorr[i+1]=sum[1]; 282 xcorr[i+2]=sum[2]; 283 xcorr[i+3]=sum[3]; 284 #ifdef FIXED_POINT 285 sum[0] = MAX32(sum[0], sum[1]); 286 sum[2] = MAX32(sum[2], sum[3]); 287 sum[0] = MAX32(sum[0], sum[2]); 288 maxcorr = MAX32(maxcorr, sum[0]); 289 #endif 290 } 291 /* In case max_pitch isn't a multiple of 4, do non-unrolled version. */ 292 for (;i<max_pitch;i++) 293 { 294 opus_val32 sum; 295 sum = celt_inner_prod(_x, _y+i, len, arch); 296 xcorr[i] = sum; 297 #ifdef FIXED_POINT 298 maxcorr = MAX32(maxcorr, sum); 299 #endif 300 } 301 #ifdef FIXED_POINT 302 return maxcorr; 303 #endif 304 #endif 305 } 306 307 void pitch_search(const opus_val16 * OPUS_RESTRICT x_lp, opus_val16 * OPUS_RESTRICT y, 308 int len, int max_pitch, int *pitch, int arch) 309 { 310 int i, j; 311 int lag; 312 int best_pitch[2]={0,0}; 313 VARDECL(opus_val16, x_lp4); 314 VARDECL(opus_val16, y_lp4); 315 VARDECL(opus_val32, xcorr); 316 #ifdef FIXED_POINT 317 opus_val32 maxcorr; 318 opus_val32 xmax, ymax; 319 int shift=0; 320 #endif 321 int offset; 322 323 SAVE_STACK; 324 325 celt_assert(len>0); 326 celt_assert(max_pitch>0); 327 lag = len+max_pitch; 328 329 ALLOC(x_lp4, len>>2, opus_val16); 330 ALLOC(y_lp4, lag>>2, opus_val16); 331 ALLOC(xcorr, max_pitch>>1, opus_val32); 332 333 /* Downsample by 2 again */ 334 for (j=0;j<len>>2;j++) 335 x_lp4[j] = x_lp[2*j]; 336 for (j=0;j<lag>>2;j++) 337 y_lp4[j] = y[2*j]; 338 339 #ifdef FIXED_POINT 340 xmax = celt_maxabs16(x_lp4, len>>2); 341 ymax = celt_maxabs16(y_lp4, lag>>2); 342 shift = celt_ilog2(MAX32(1, MAX32(xmax, ymax))) - 14 + celt_ilog2(len)/2; 343 if (shift>0) 344 { 345 for (j=0;j<len>>2;j++) 346 x_lp4[j] = SHR16(x_lp4[j], shift); 347 for (j=0;j<lag>>2;j++) 348 y_lp4[j] = SHR16(y_lp4[j], shift); 349 /* Use double the shift for a MAC */ 350 shift *= 2; 351 } else { 352 shift = 0; 353 } 354 #endif 355 356 /* Coarse search with 4x decimation */ 357 358 #ifdef FIXED_POINT 359 maxcorr = 360 #endif 361 celt_pitch_xcorr(x_lp4, y_lp4, xcorr, len>>2, max_pitch>>2, arch); 362 363 find_best_pitch(xcorr, y_lp4, len>>2, max_pitch>>2, best_pitch 364 #ifdef FIXED_POINT 365 , 0, maxcorr 366 #endif 367 ); 368 369 /* Finer search with 2x decimation */ 370 #ifdef FIXED_POINT 371 maxcorr=1; 372 #endif 373 for (i=0;i<max_pitch>>1;i++) 374 { 375 opus_val32 sum; 376 xcorr[i] = 0; 377 if (abs(i-2*best_pitch[0])>2 && abs(i-2*best_pitch[1])>2) 378 continue; 379 #ifdef FIXED_POINT 380 sum = 0; 381 for (j=0;j<len>>1;j++) 382 sum += SHR32(MULT16_16(x_lp[j],y[i+j]), shift); 383 #else 384 sum = celt_inner_prod(x_lp, y+i, len>>1, arch); 385 #endif 386 xcorr[i] = MAX32(-1, sum); 387 #ifdef FIXED_POINT 388 maxcorr = MAX32(maxcorr, sum); 389 #endif 390 } 391 find_best_pitch(xcorr, y, len>>1, max_pitch>>1, best_pitch 392 #ifdef FIXED_POINT 393 , shift+1, maxcorr 394 #endif 395 ); 396 397 /* Refine by pseudo-interpolation */ 398 if (best_pitch[0]>0 && best_pitch[0]<(max_pitch>>1)-1) 399 { 400 opus_val32 a, b, c; 401 a = xcorr[best_pitch[0]-1]; 402 b = xcorr[best_pitch[0]]; 403 c = xcorr[best_pitch[0]+1]; 404 if ((c-a) > MULT16_32_Q15(QCONST16(.7f,15),b-a)) 405 offset = 1; 406 else if ((a-c) > MULT16_32_Q15(QCONST16(.7f,15),b-c)) 407 offset = -1; 408 else 409 offset = 0; 410 } else { 411 offset = 0; 412 } 413 *pitch = 2*best_pitch[0]-offset; 414 415 RESTORE_STACK; 416 } 417 418 #ifdef FIXED_POINT 419 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy) 420 { 421 opus_val32 x2y2; 422 int sx, sy, shift; 423 opus_val32 g; 424 opus_val16 den; 425 if (xy == 0 || xx == 0 || yy == 0) 426 return 0; 427 sx = celt_ilog2(xx)-14; 428 sy = celt_ilog2(yy)-14; 429 shift = sx + sy; 430 x2y2 = SHR32(MULT16_16(VSHR32(xx, sx), VSHR32(yy, sy)), 14); 431 if (shift & 1) { 432 if (x2y2 < 32768) 433 { 434 x2y2 <<= 1; 435 shift--; 436 } else { 437 x2y2 >>= 1; 438 shift++; 439 } 440 } 441 den = celt_rsqrt_norm(x2y2); 442 g = MULT16_32_Q15(den, xy); 443 g = VSHR32(g, (shift>>1)-1); 444 return EXTRACT16(MAX32(-Q15ONE, MIN32(g, Q15ONE))); 445 } 446 #else 447 static opus_val16 compute_pitch_gain(opus_val32 xy, opus_val32 xx, opus_val32 yy) 448 { 449 return xy/celt_sqrt(1+xx*yy); 450 } 451 #endif 452 453 static const int second_check[16] = {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2}; 454 opus_val16 remove_doubling(opus_val16 *x, int maxperiod, int minperiod, 455 int N, int *T0_, int prev_period, opus_val16 prev_gain, int arch) 456 { 457 int k, i, T, T0; 458 opus_val16 g, g0; 459 opus_val16 pg; 460 opus_val32 xy,xx,yy,xy2; 461 opus_val32 xcorr[3]; 462 opus_val32 best_xy, best_yy; 463 int offset; 464 int minperiod0; 465 VARDECL(opus_val32, yy_lookup); 466 SAVE_STACK; 467 468 minperiod0 = minperiod; 469 maxperiod /= 2; 470 minperiod /= 2; 471 *T0_ /= 2; 472 prev_period /= 2; 473 N /= 2; 474 x += maxperiod; 475 if (*T0_>=maxperiod) 476 *T0_=maxperiod-1; 477 478 T = T0 = *T0_; 479 ALLOC(yy_lookup, maxperiod+1, opus_val32); 480 dual_inner_prod(x, x, x-T0, N, &xx, &xy, arch); 481 yy_lookup[0] = xx; 482 yy=xx; 483 for (i=1;i<=maxperiod;i++) 484 { 485 yy = yy+MULT16_16(x[-i],x[-i])-MULT16_16(x[N-i],x[N-i]); 486 yy_lookup[i] = MAX32(0, yy); 487 } 488 yy = yy_lookup[T0]; 489 best_xy = xy; 490 best_yy = yy; 491 g = g0 = compute_pitch_gain(xy, xx, yy); 492 /* Look for any pitch at T/k */ 493 for (k=2;k<=15;k++) 494 { 495 int T1, T1b; 496 opus_val16 g1; 497 opus_val16 cont=0; 498 opus_val16 thresh; 499 T1 = celt_udiv(2*T0+k, 2*k); 500 if (T1 < minperiod) 501 break; 502 /* Look for another strong correlation at T1b */ 503 if (k==2) 504 { 505 if (T1+T0>maxperiod) 506 T1b = T0; 507 else 508 T1b = T0+T1; 509 } else 510 { 511 T1b = celt_udiv(2*second_check[k]*T0+k, 2*k); 512 } 513 dual_inner_prod(x, &x[-T1], &x[-T1b], N, &xy, &xy2, arch); 514 xy = HALF32(xy + xy2); 515 yy = HALF32(yy_lookup[T1] + yy_lookup[T1b]); 516 g1 = compute_pitch_gain(xy, xx, yy); 517 if (abs(T1-prev_period)<=1) 518 cont = prev_gain; 519 else if (abs(T1-prev_period)<=2 && 5*k*k < T0) 520 cont = HALF16(prev_gain); 521 else 522 cont = 0; 523 thresh = MAX16(QCONST16(.3f,15), MULT16_16_Q15(QCONST16(.7f,15),g0)-cont); 524 /* Bias against very high pitch (very short period) to avoid false-positives 525 due to short-term correlation */ 526 if (T1<3*minperiod) 527 thresh = MAX16(QCONST16(.4f,15), MULT16_16_Q15(QCONST16(.85f,15),g0)-cont); 528 else if (T1<2*minperiod) 529 thresh = MAX16(QCONST16(.5f,15), MULT16_16_Q15(QCONST16(.9f,15),g0)-cont); 530 if (g1 > thresh) 531 { 532 best_xy = xy; 533 best_yy = yy; 534 T = T1; 535 g = g1; 536 } 537 } 538 best_xy = MAX32(0, best_xy); 539 if (best_yy <= best_xy) 540 pg = Q15ONE; 541 else 542 pg = SHR32(frac_div32(best_xy,best_yy+1),16); 543 544 for (k=0;k<3;k++) 545 xcorr[k] = celt_inner_prod(x, x-(T+k-1), N, arch); 546 if ((xcorr[2]-xcorr[0]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[0])) 547 offset = 1; 548 else if ((xcorr[0]-xcorr[2]) > MULT16_32_Q15(QCONST16(.7f,15),xcorr[1]-xcorr[2])) 549 offset = -1; 550 else 551 offset = 0; 552 if (pg > g) 553 pg = g; 554 *T0_ = 2*T+offset; 555 556 if (*T0_<minperiod0) 557 *T0_=minperiod0; 558 RESTORE_STACK; 559 return pg; 560 }