diff options
Diffstat (limited to 'lib/rbcodec/codecs/libopus/silk/float/pitch_analysis_core_FLP.c')
-rw-r--r-- | lib/rbcodec/codecs/libopus/silk/float/pitch_analysis_core_FLP.c | 630 |
1 files changed, 630 insertions, 0 deletions
diff --git a/lib/rbcodec/codecs/libopus/silk/float/pitch_analysis_core_FLP.c b/lib/rbcodec/codecs/libopus/silk/float/pitch_analysis_core_FLP.c new file mode 100644 index 0000000000..f351bc3718 --- /dev/null +++ b/lib/rbcodec/codecs/libopus/silk/float/pitch_analysis_core_FLP.c | |||
@@ -0,0 +1,630 @@ | |||
1 | /*********************************************************************** | ||
2 | Copyright (c) 2006-2011, Skype Limited. All rights reserved. | ||
3 | Redistribution and use in source and binary forms, with or without | ||
4 | modification, are permitted provided that the following conditions | ||
5 | are met: | ||
6 | - Redistributions of source code must retain the above copyright notice, | ||
7 | this list of conditions and the following disclaimer. | ||
8 | - Redistributions in binary form must reproduce the above copyright | ||
9 | notice, this list of conditions and the following disclaimer in the | ||
10 | documentation and/or other materials provided with the distribution. | ||
11 | - Neither the name of Internet Society, IETF or IETF Trust, nor the | ||
12 | names of specific contributors, may be used to endorse or promote | ||
13 | products derived from this software without specific prior written | ||
14 | permission. | ||
15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | ||
16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | ||
17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | ||
18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | ||
19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | ||
20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | ||
21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | ||
22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | ||
23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | ||
24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
25 | POSSIBILITY OF SUCH DAMAGE. | ||
26 | ***********************************************************************/ | ||
27 | |||
28 | #ifdef HAVE_CONFIG_H | ||
29 | #include "config.h" | ||
30 | #endif | ||
31 | |||
32 | /***************************************************************************** | ||
33 | * Pitch analyser function | ||
34 | ******************************************************************************/ | ||
35 | #include "SigProc_FLP.h" | ||
36 | #include "SigProc_FIX.h" | ||
37 | #include "pitch_est_defines.h" | ||
38 | #include "pitch.h" | ||
39 | |||
40 | #define SCRATCH_SIZE 22 | ||
41 | |||
42 | /************************************************************/ | ||
43 | /* Internally used functions */ | ||
44 | /************************************************************/ | ||
45 | static void silk_P_Ana_calc_corr_st3( | ||
46 | silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ | ||
47 | const silk_float frame[], /* I vector to correlate */ | ||
48 | opus_int start_lag, /* I start lag */ | ||
49 | opus_int sf_length, /* I sub frame length */ | ||
50 | opus_int nb_subfr, /* I number of subframes */ | ||
51 | opus_int complexity, /* I Complexity setting */ | ||
52 | int arch /* I Run-time architecture */ | ||
53 | ); | ||
54 | |||
55 | static void silk_P_Ana_calc_energy_st3( | ||
56 | silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ | ||
57 | const silk_float frame[], /* I vector to correlate */ | ||
58 | opus_int start_lag, /* I start lag */ | ||
59 | opus_int sf_length, /* I sub frame length */ | ||
60 | opus_int nb_subfr, /* I number of subframes */ | ||
61 | opus_int complexity /* I Complexity setting */ | ||
62 | ); | ||
63 | |||
64 | /************************************************************/ | ||
65 | /* CORE PITCH ANALYSIS FUNCTION */ | ||
66 | /************************************************************/ | ||
67 | opus_int silk_pitch_analysis_core_FLP( /* O Voicing estimate: 0 voiced, 1 unvoiced */ | ||
68 | const silk_float *frame, /* I Signal of length PE_FRAME_LENGTH_MS*Fs_kHz */ | ||
69 | opus_int *pitch_out, /* O Pitch lag values [nb_subfr] */ | ||
70 | opus_int16 *lagIndex, /* O Lag Index */ | ||
71 | opus_int8 *contourIndex, /* O Pitch contour Index */ | ||
72 | silk_float *LTPCorr, /* I/O Normalized correlation; input: value from previous frame */ | ||
73 | opus_int prevLag, /* I Last lag of previous frame; set to zero is unvoiced */ | ||
74 | const silk_float search_thres1, /* I First stage threshold for lag candidates 0 - 1 */ | ||
75 | const silk_float search_thres2, /* I Final threshold for lag candidates 0 - 1 */ | ||
76 | const opus_int Fs_kHz, /* I sample frequency (kHz) */ | ||
77 | const opus_int complexity, /* I Complexity setting, 0-2, where 2 is highest */ | ||
78 | const opus_int nb_subfr, /* I Number of 5 ms subframes */ | ||
79 | int arch /* I Run-time architecture */ | ||
80 | ) | ||
81 | { | ||
82 | opus_int i, k, d, j; | ||
83 | silk_float frame_8kHz[ PE_MAX_FRAME_LENGTH_MS * 8 ]; | ||
84 | silk_float frame_4kHz[ PE_MAX_FRAME_LENGTH_MS * 4 ]; | ||
85 | opus_int16 frame_8_FIX[ PE_MAX_FRAME_LENGTH_MS * 8 ]; | ||
86 | opus_int16 frame_4_FIX[ PE_MAX_FRAME_LENGTH_MS * 4 ]; | ||
87 | opus_int32 filt_state[ 6 ]; | ||
88 | silk_float threshold, contour_bias; | ||
89 | silk_float C[ PE_MAX_NB_SUBFR][ (PE_MAX_LAG >> 1) + 5 ]; | ||
90 | opus_val32 xcorr[ PE_MAX_LAG_MS * 4 - PE_MIN_LAG_MS * 4 + 1 ]; | ||
91 | silk_float CC[ PE_NB_CBKS_STAGE2_EXT ]; | ||
92 | const silk_float *target_ptr, *basis_ptr; | ||
93 | double cross_corr, normalizer, energy, energy_tmp; | ||
94 | opus_int d_srch[ PE_D_SRCH_LENGTH ]; | ||
95 | opus_int16 d_comp[ (PE_MAX_LAG >> 1) + 5 ]; | ||
96 | opus_int length_d_srch, length_d_comp; | ||
97 | silk_float Cmax, CCmax, CCmax_b, CCmax_new_b, CCmax_new; | ||
98 | opus_int CBimax, CBimax_new, lag, start_lag, end_lag, lag_new; | ||
99 | opus_int cbk_size; | ||
100 | silk_float lag_log2, prevLag_log2, delta_lag_log2_sqr; | ||
101 | silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ]; | ||
102 | silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ]; | ||
103 | opus_int lag_counter; | ||
104 | opus_int frame_length, frame_length_8kHz, frame_length_4kHz; | ||
105 | opus_int sf_length, sf_length_8kHz, sf_length_4kHz; | ||
106 | opus_int min_lag, min_lag_8kHz, min_lag_4kHz; | ||
107 | opus_int max_lag, max_lag_8kHz, max_lag_4kHz; | ||
108 | opus_int nb_cbk_search; | ||
109 | const opus_int8 *Lag_CB_ptr; | ||
110 | |||
111 | /* Check for valid sampling frequency */ | ||
112 | celt_assert( Fs_kHz == 8 || Fs_kHz == 12 || Fs_kHz == 16 ); | ||
113 | |||
114 | /* Check for valid complexity setting */ | ||
115 | celt_assert( complexity >= SILK_PE_MIN_COMPLEX ); | ||
116 | celt_assert( complexity <= SILK_PE_MAX_COMPLEX ); | ||
117 | |||
118 | silk_assert( search_thres1 >= 0.0f && search_thres1 <= 1.0f ); | ||
119 | silk_assert( search_thres2 >= 0.0f && search_thres2 <= 1.0f ); | ||
120 | |||
121 | /* Set up frame lengths max / min lag for the sampling frequency */ | ||
122 | frame_length = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * Fs_kHz; | ||
123 | frame_length_4kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 4; | ||
124 | frame_length_8kHz = ( PE_LTP_MEM_LENGTH_MS + nb_subfr * PE_SUBFR_LENGTH_MS ) * 8; | ||
125 | sf_length = PE_SUBFR_LENGTH_MS * Fs_kHz; | ||
126 | sf_length_4kHz = PE_SUBFR_LENGTH_MS * 4; | ||
127 | sf_length_8kHz = PE_SUBFR_LENGTH_MS * 8; | ||
128 | min_lag = PE_MIN_LAG_MS * Fs_kHz; | ||
129 | min_lag_4kHz = PE_MIN_LAG_MS * 4; | ||
130 | min_lag_8kHz = PE_MIN_LAG_MS * 8; | ||
131 | max_lag = PE_MAX_LAG_MS * Fs_kHz - 1; | ||
132 | max_lag_4kHz = PE_MAX_LAG_MS * 4; | ||
133 | max_lag_8kHz = PE_MAX_LAG_MS * 8 - 1; | ||
134 | |||
135 | /* Resample from input sampled at Fs_kHz to 8 kHz */ | ||
136 | if( Fs_kHz == 16 ) { | ||
137 | /* Resample to 16 -> 8 khz */ | ||
138 | opus_int16 frame_16_FIX[ 16 * PE_MAX_FRAME_LENGTH_MS ]; | ||
139 | silk_float2short_array( frame_16_FIX, frame, frame_length ); | ||
140 | silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) ); | ||
141 | silk_resampler_down2( filt_state, frame_8_FIX, frame_16_FIX, frame_length ); | ||
142 | silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz ); | ||
143 | } else if( Fs_kHz == 12 ) { | ||
144 | /* Resample to 12 -> 8 khz */ | ||
145 | opus_int16 frame_12_FIX[ 12 * PE_MAX_FRAME_LENGTH_MS ]; | ||
146 | silk_float2short_array( frame_12_FIX, frame, frame_length ); | ||
147 | silk_memset( filt_state, 0, 6 * sizeof( opus_int32 ) ); | ||
148 | silk_resampler_down2_3( filt_state, frame_8_FIX, frame_12_FIX, frame_length ); | ||
149 | silk_short2float_array( frame_8kHz, frame_8_FIX, frame_length_8kHz ); | ||
150 | } else { | ||
151 | celt_assert( Fs_kHz == 8 ); | ||
152 | silk_float2short_array( frame_8_FIX, frame, frame_length_8kHz ); | ||
153 | } | ||
154 | |||
155 | /* Decimate again to 4 kHz */ | ||
156 | silk_memset( filt_state, 0, 2 * sizeof( opus_int32 ) ); | ||
157 | silk_resampler_down2( filt_state, frame_4_FIX, frame_8_FIX, frame_length_8kHz ); | ||
158 | silk_short2float_array( frame_4kHz, frame_4_FIX, frame_length_4kHz ); | ||
159 | |||
160 | /* Low-pass filter */ | ||
161 | for( i = frame_length_4kHz - 1; i > 0; i-- ) { | ||
162 | frame_4kHz[ i ] = silk_ADD_SAT16( frame_4kHz[ i ], frame_4kHz[ i - 1 ] ); | ||
163 | } | ||
164 | |||
165 | /****************************************************************************** | ||
166 | * FIRST STAGE, operating in 4 khz | ||
167 | ******************************************************************************/ | ||
168 | silk_memset(C, 0, sizeof(silk_float) * nb_subfr * ((PE_MAX_LAG >> 1) + 5)); | ||
169 | target_ptr = &frame_4kHz[ silk_LSHIFT( sf_length_4kHz, 2 ) ]; | ||
170 | for( k = 0; k < nb_subfr >> 1; k++ ) { | ||
171 | /* Check that we are within range of the array */ | ||
172 | celt_assert( target_ptr >= frame_4kHz ); | ||
173 | celt_assert( target_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); | ||
174 | |||
175 | basis_ptr = target_ptr - min_lag_4kHz; | ||
176 | |||
177 | /* Check that we are within range of the array */ | ||
178 | celt_assert( basis_ptr >= frame_4kHz ); | ||
179 | celt_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); | ||
180 | |||
181 | celt_pitch_xcorr( target_ptr, target_ptr-max_lag_4kHz, xcorr, sf_length_8kHz, max_lag_4kHz - min_lag_4kHz + 1, arch ); | ||
182 | |||
183 | /* Calculate first vector products before loop */ | ||
184 | cross_corr = xcorr[ max_lag_4kHz - min_lag_4kHz ]; | ||
185 | normalizer = silk_energy_FLP( target_ptr, sf_length_8kHz ) + | ||
186 | silk_energy_FLP( basis_ptr, sf_length_8kHz ) + | ||
187 | sf_length_8kHz * 4000.0f; | ||
188 | |||
189 | C[ 0 ][ min_lag_4kHz ] += (silk_float)( 2 * cross_corr / normalizer ); | ||
190 | |||
191 | /* From now on normalizer is computed recursively */ | ||
192 | for( d = min_lag_4kHz + 1; d <= max_lag_4kHz; d++ ) { | ||
193 | basis_ptr--; | ||
194 | |||
195 | /* Check that we are within range of the array */ | ||
196 | silk_assert( basis_ptr >= frame_4kHz ); | ||
197 | silk_assert( basis_ptr + sf_length_8kHz <= frame_4kHz + frame_length_4kHz ); | ||
198 | |||
199 | cross_corr = xcorr[ max_lag_4kHz - d ]; | ||
200 | |||
201 | /* Add contribution of new sample and remove contribution from oldest sample */ | ||
202 | normalizer += | ||
203 | basis_ptr[ 0 ] * (double)basis_ptr[ 0 ] - | ||
204 | basis_ptr[ sf_length_8kHz ] * (double)basis_ptr[ sf_length_8kHz ]; | ||
205 | C[ 0 ][ d ] += (silk_float)( 2 * cross_corr / normalizer ); | ||
206 | } | ||
207 | /* Update target pointer */ | ||
208 | target_ptr += sf_length_8kHz; | ||
209 | } | ||
210 | |||
211 | /* Apply short-lag bias */ | ||
212 | for( i = max_lag_4kHz; i >= min_lag_4kHz; i-- ) { | ||
213 | C[ 0 ][ i ] -= C[ 0 ][ i ] * i / 4096.0f; | ||
214 | } | ||
215 | |||
216 | /* Sort */ | ||
217 | length_d_srch = 4 + 2 * complexity; | ||
218 | celt_assert( 3 * length_d_srch <= PE_D_SRCH_LENGTH ); | ||
219 | silk_insertion_sort_decreasing_FLP( &C[ 0 ][ min_lag_4kHz ], d_srch, max_lag_4kHz - min_lag_4kHz + 1, length_d_srch ); | ||
220 | |||
221 | /* Escape if correlation is very low already here */ | ||
222 | Cmax = C[ 0 ][ min_lag_4kHz ]; | ||
223 | if( Cmax < 0.2f ) { | ||
224 | silk_memset( pitch_out, 0, nb_subfr * sizeof( opus_int ) ); | ||
225 | *LTPCorr = 0.0f; | ||
226 | *lagIndex = 0; | ||
227 | *contourIndex = 0; | ||
228 | return 1; | ||
229 | } | ||
230 | |||
231 | threshold = search_thres1 * Cmax; | ||
232 | for( i = 0; i < length_d_srch; i++ ) { | ||
233 | /* Convert to 8 kHz indices for the sorted correlation that exceeds the threshold */ | ||
234 | if( C[ 0 ][ min_lag_4kHz + i ] > threshold ) { | ||
235 | d_srch[ i ] = silk_LSHIFT( d_srch[ i ] + min_lag_4kHz, 1 ); | ||
236 | } else { | ||
237 | length_d_srch = i; | ||
238 | break; | ||
239 | } | ||
240 | } | ||
241 | celt_assert( length_d_srch > 0 ); | ||
242 | |||
243 | for( i = min_lag_8kHz - 5; i < max_lag_8kHz + 5; i++ ) { | ||
244 | d_comp[ i ] = 0; | ||
245 | } | ||
246 | for( i = 0; i < length_d_srch; i++ ) { | ||
247 | d_comp[ d_srch[ i ] ] = 1; | ||
248 | } | ||
249 | |||
250 | /* Convolution */ | ||
251 | for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) { | ||
252 | d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ]; | ||
253 | } | ||
254 | |||
255 | length_d_srch = 0; | ||
256 | for( i = min_lag_8kHz; i < max_lag_8kHz + 1; i++ ) { | ||
257 | if( d_comp[ i + 1 ] > 0 ) { | ||
258 | d_srch[ length_d_srch ] = i; | ||
259 | length_d_srch++; | ||
260 | } | ||
261 | } | ||
262 | |||
263 | /* Convolution */ | ||
264 | for( i = max_lag_8kHz + 3; i >= min_lag_8kHz; i-- ) { | ||
265 | d_comp[ i ] += d_comp[ i - 1 ] + d_comp[ i - 2 ] + d_comp[ i - 3 ]; | ||
266 | } | ||
267 | |||
268 | length_d_comp = 0; | ||
269 | for( i = min_lag_8kHz; i < max_lag_8kHz + 4; i++ ) { | ||
270 | if( d_comp[ i ] > 0 ) { | ||
271 | d_comp[ length_d_comp ] = (opus_int16)( i - 2 ); | ||
272 | length_d_comp++; | ||
273 | } | ||
274 | } | ||
275 | |||
276 | /********************************************************************************** | ||
277 | ** SECOND STAGE, operating at 8 kHz, on lag sections with high correlation | ||
278 | *************************************************************************************/ | ||
279 | /********************************************************************************* | ||
280 | * Find energy of each subframe projected onto its history, for a range of delays | ||
281 | *********************************************************************************/ | ||
282 | silk_memset( C, 0, PE_MAX_NB_SUBFR*((PE_MAX_LAG >> 1) + 5) * sizeof(silk_float)); | ||
283 | |||
284 | if( Fs_kHz == 8 ) { | ||
285 | target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * 8 ]; | ||
286 | } else { | ||
287 | target_ptr = &frame_8kHz[ PE_LTP_MEM_LENGTH_MS * 8 ]; | ||
288 | } | ||
289 | for( k = 0; k < nb_subfr; k++ ) { | ||
290 | energy_tmp = silk_energy_FLP( target_ptr, sf_length_8kHz ) + 1.0; | ||
291 | for( j = 0; j < length_d_comp; j++ ) { | ||
292 | d = d_comp[ j ]; | ||
293 | basis_ptr = target_ptr - d; | ||
294 | cross_corr = silk_inner_product_FLP( basis_ptr, target_ptr, sf_length_8kHz ); | ||
295 | if( cross_corr > 0.0f ) { | ||
296 | energy = silk_energy_FLP( basis_ptr, sf_length_8kHz ); | ||
297 | C[ k ][ d ] = (silk_float)( 2 * cross_corr / ( energy + energy_tmp ) ); | ||
298 | } else { | ||
299 | C[ k ][ d ] = 0.0f; | ||
300 | } | ||
301 | } | ||
302 | target_ptr += sf_length_8kHz; | ||
303 | } | ||
304 | |||
305 | /* search over lag range and lags codebook */ | ||
306 | /* scale factor for lag codebook, as a function of center lag */ | ||
307 | |||
308 | CCmax = 0.0f; /* This value doesn't matter */ | ||
309 | CCmax_b = -1000.0f; | ||
310 | |||
311 | CBimax = 0; /* To avoid returning undefined lag values */ | ||
312 | lag = -1; /* To check if lag with strong enough correlation has been found */ | ||
313 | |||
314 | if( prevLag > 0 ) { | ||
315 | if( Fs_kHz == 12 ) { | ||
316 | prevLag = silk_LSHIFT( prevLag, 1 ) / 3; | ||
317 | } else if( Fs_kHz == 16 ) { | ||
318 | prevLag = silk_RSHIFT( prevLag, 1 ); | ||
319 | } | ||
320 | prevLag_log2 = silk_log2( (silk_float)prevLag ); | ||
321 | } else { | ||
322 | prevLag_log2 = 0; | ||
323 | } | ||
324 | |||
325 | /* Set up stage 2 codebook based on number of subframes */ | ||
326 | if( nb_subfr == PE_MAX_NB_SUBFR ) { | ||
327 | cbk_size = PE_NB_CBKS_STAGE2_EXT; | ||
328 | Lag_CB_ptr = &silk_CB_lags_stage2[ 0 ][ 0 ]; | ||
329 | if( Fs_kHz == 8 && complexity > SILK_PE_MIN_COMPLEX ) { | ||
330 | /* If input is 8 khz use a larger codebook here because it is last stage */ | ||
331 | nb_cbk_search = PE_NB_CBKS_STAGE2_EXT; | ||
332 | } else { | ||
333 | nb_cbk_search = PE_NB_CBKS_STAGE2; | ||
334 | } | ||
335 | } else { | ||
336 | cbk_size = PE_NB_CBKS_STAGE2_10MS; | ||
337 | Lag_CB_ptr = &silk_CB_lags_stage2_10_ms[ 0 ][ 0 ]; | ||
338 | nb_cbk_search = PE_NB_CBKS_STAGE2_10MS; | ||
339 | } | ||
340 | |||
341 | for( k = 0; k < length_d_srch; k++ ) { | ||
342 | d = d_srch[ k ]; | ||
343 | for( j = 0; j < nb_cbk_search; j++ ) { | ||
344 | CC[j] = 0.0f; | ||
345 | for( i = 0; i < nb_subfr; i++ ) { | ||
346 | /* Try all codebooks */ | ||
347 | CC[ j ] += C[ i ][ d + matrix_ptr( Lag_CB_ptr, i, j, cbk_size )]; | ||
348 | } | ||
349 | } | ||
350 | /* Find best codebook */ | ||
351 | CCmax_new = -1000.0f; | ||
352 | CBimax_new = 0; | ||
353 | for( i = 0; i < nb_cbk_search; i++ ) { | ||
354 | if( CC[ i ] > CCmax_new ) { | ||
355 | CCmax_new = CC[ i ]; | ||
356 | CBimax_new = i; | ||
357 | } | ||
358 | } | ||
359 | |||
360 | /* Bias towards shorter lags */ | ||
361 | lag_log2 = silk_log2( (silk_float)d ); | ||
362 | CCmax_new_b = CCmax_new - PE_SHORTLAG_BIAS * nb_subfr * lag_log2; | ||
363 | |||
364 | /* Bias towards previous lag */ | ||
365 | if( prevLag > 0 ) { | ||
366 | delta_lag_log2_sqr = lag_log2 - prevLag_log2; | ||
367 | delta_lag_log2_sqr *= delta_lag_log2_sqr; | ||
368 | CCmax_new_b -= PE_PREVLAG_BIAS * nb_subfr * (*LTPCorr) * delta_lag_log2_sqr / ( delta_lag_log2_sqr + 0.5f ); | ||
369 | } | ||
370 | |||
371 | if( CCmax_new_b > CCmax_b && /* Find maximum biased correlation */ | ||
372 | CCmax_new > nb_subfr * search_thres2 /* Correlation needs to be high enough to be voiced */ | ||
373 | ) { | ||
374 | CCmax_b = CCmax_new_b; | ||
375 | CCmax = CCmax_new; | ||
376 | lag = d; | ||
377 | CBimax = CBimax_new; | ||
378 | } | ||
379 | } | ||
380 | |||
381 | if( lag == -1 ) { | ||
382 | /* No suitable candidate found */ | ||
383 | silk_memset( pitch_out, 0, PE_MAX_NB_SUBFR * sizeof(opus_int) ); | ||
384 | *LTPCorr = 0.0f; | ||
385 | *lagIndex = 0; | ||
386 | *contourIndex = 0; | ||
387 | return 1; | ||
388 | } | ||
389 | |||
390 | /* Output normalized correlation */ | ||
391 | *LTPCorr = (silk_float)( CCmax / nb_subfr ); | ||
392 | silk_assert( *LTPCorr >= 0.0f ); | ||
393 | |||
394 | if( Fs_kHz > 8 ) { | ||
395 | /* Search in original signal */ | ||
396 | |||
397 | /* Compensate for decimation */ | ||
398 | silk_assert( lag == silk_SAT16( lag ) ); | ||
399 | if( Fs_kHz == 12 ) { | ||
400 | lag = silk_RSHIFT_ROUND( silk_SMULBB( lag, 3 ), 1 ); | ||
401 | } else { /* Fs_kHz == 16 */ | ||
402 | lag = silk_LSHIFT( lag, 1 ); | ||
403 | } | ||
404 | |||
405 | lag = silk_LIMIT_int( lag, min_lag, max_lag ); | ||
406 | start_lag = silk_max_int( lag - 2, min_lag ); | ||
407 | end_lag = silk_min_int( lag + 2, max_lag ); | ||
408 | lag_new = lag; /* to avoid undefined lag */ | ||
409 | CBimax = 0; /* to avoid undefined lag */ | ||
410 | |||
411 | CCmax = -1000.0f; | ||
412 | |||
413 | /* Calculate the correlations and energies needed in stage 3 */ | ||
414 | silk_P_Ana_calc_corr_st3( cross_corr_st3, frame, start_lag, sf_length, nb_subfr, complexity, arch ); | ||
415 | silk_P_Ana_calc_energy_st3( energies_st3, frame, start_lag, sf_length, nb_subfr, complexity ); | ||
416 | |||
417 | lag_counter = 0; | ||
418 | silk_assert( lag == silk_SAT16( lag ) ); | ||
419 | contour_bias = PE_FLATCONTOUR_BIAS / lag; | ||
420 | |||
421 | /* Set up cbk parameters according to complexity setting and frame length */ | ||
422 | if( nb_subfr == PE_MAX_NB_SUBFR ) { | ||
423 | nb_cbk_search = (opus_int)silk_nb_cbk_searchs_stage3[ complexity ]; | ||
424 | cbk_size = PE_NB_CBKS_STAGE3_MAX; | ||
425 | Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; | ||
426 | } else { | ||
427 | nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; | ||
428 | cbk_size = PE_NB_CBKS_STAGE3_10MS; | ||
429 | Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; | ||
430 | } | ||
431 | |||
432 | target_ptr = &frame[ PE_LTP_MEM_LENGTH_MS * Fs_kHz ]; | ||
433 | energy_tmp = silk_energy_FLP( target_ptr, nb_subfr * sf_length ) + 1.0; | ||
434 | for( d = start_lag; d <= end_lag; d++ ) { | ||
435 | for( j = 0; j < nb_cbk_search; j++ ) { | ||
436 | cross_corr = 0.0; | ||
437 | energy = energy_tmp; | ||
438 | for( k = 0; k < nb_subfr; k++ ) { | ||
439 | cross_corr += cross_corr_st3[ k ][ j ][ lag_counter ]; | ||
440 | energy += energies_st3[ k ][ j ][ lag_counter ]; | ||
441 | } | ||
442 | if( cross_corr > 0.0 ) { | ||
443 | CCmax_new = (silk_float)( 2 * cross_corr / energy ); | ||
444 | /* Reduce depending on flatness of contour */ | ||
445 | CCmax_new *= 1.0f - contour_bias * j; | ||
446 | } else { | ||
447 | CCmax_new = 0.0f; | ||
448 | } | ||
449 | |||
450 | if( CCmax_new > CCmax && ( d + (opus_int)silk_CB_lags_stage3[ 0 ][ j ] ) <= max_lag ) { | ||
451 | CCmax = CCmax_new; | ||
452 | lag_new = d; | ||
453 | CBimax = j; | ||
454 | } | ||
455 | } | ||
456 | lag_counter++; | ||
457 | } | ||
458 | |||
459 | for( k = 0; k < nb_subfr; k++ ) { | ||
460 | pitch_out[ k ] = lag_new + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size ); | ||
461 | pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag, PE_MAX_LAG_MS * Fs_kHz ); | ||
462 | } | ||
463 | *lagIndex = (opus_int16)( lag_new - min_lag ); | ||
464 | *contourIndex = (opus_int8)CBimax; | ||
465 | } else { /* Fs_kHz == 8 */ | ||
466 | /* Save Lags */ | ||
467 | for( k = 0; k < nb_subfr; k++ ) { | ||
468 | pitch_out[ k ] = lag + matrix_ptr( Lag_CB_ptr, k, CBimax, cbk_size ); | ||
469 | pitch_out[ k ] = silk_LIMIT( pitch_out[ k ], min_lag_8kHz, PE_MAX_LAG_MS * 8 ); | ||
470 | } | ||
471 | *lagIndex = (opus_int16)( lag - min_lag_8kHz ); | ||
472 | *contourIndex = (opus_int8)CBimax; | ||
473 | } | ||
474 | celt_assert( *lagIndex >= 0 ); | ||
475 | /* return as voiced */ | ||
476 | return 0; | ||
477 | } | ||
478 | |||
479 | /*********************************************************************** | ||
480 | * Calculates the correlations used in stage 3 search. In order to cover | ||
481 | * the whole lag codebook for all the searched offset lags (lag +- 2), | ||
482 | * the following correlations are needed in each sub frame: | ||
483 | * | ||
484 | * sf1: lag range [-8,...,7] total 16 correlations | ||
485 | * sf2: lag range [-4,...,4] total 9 correlations | ||
486 | * sf3: lag range [-3,....4] total 8 correltions | ||
487 | * sf4: lag range [-6,....8] total 15 correlations | ||
488 | * | ||
489 | * In total 48 correlations. The direct implementation computed in worst | ||
490 | * case 4*12*5 = 240 correlations, but more likely around 120. | ||
491 | ***********************************************************************/ | ||
492 | static void silk_P_Ana_calc_corr_st3( | ||
493 | silk_float cross_corr_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ | ||
494 | const silk_float frame[], /* I vector to correlate */ | ||
495 | opus_int start_lag, /* I start lag */ | ||
496 | opus_int sf_length, /* I sub frame length */ | ||
497 | opus_int nb_subfr, /* I number of subframes */ | ||
498 | opus_int complexity, /* I Complexity setting */ | ||
499 | int arch /* I Run-time architecture */ | ||
500 | ) | ||
501 | { | ||
502 | const silk_float *target_ptr; | ||
503 | opus_int i, j, k, lag_counter, lag_low, lag_high; | ||
504 | opus_int nb_cbk_search, delta, idx, cbk_size; | ||
505 | silk_float scratch_mem[ SCRATCH_SIZE ]; | ||
506 | opus_val32 xcorr[ SCRATCH_SIZE ]; | ||
507 | const opus_int8 *Lag_range_ptr, *Lag_CB_ptr; | ||
508 | |||
509 | celt_assert( complexity >= SILK_PE_MIN_COMPLEX ); | ||
510 | celt_assert( complexity <= SILK_PE_MAX_COMPLEX ); | ||
511 | |||
512 | if( nb_subfr == PE_MAX_NB_SUBFR ) { | ||
513 | Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ]; | ||
514 | Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; | ||
515 | nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ]; | ||
516 | cbk_size = PE_NB_CBKS_STAGE3_MAX; | ||
517 | } else { | ||
518 | celt_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1); | ||
519 | Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ]; | ||
520 | Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; | ||
521 | nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; | ||
522 | cbk_size = PE_NB_CBKS_STAGE3_10MS; | ||
523 | } | ||
524 | |||
525 | target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; /* Pointer to middle of frame */ | ||
526 | for( k = 0; k < nb_subfr; k++ ) { | ||
527 | lag_counter = 0; | ||
528 | |||
529 | /* Calculate the correlations for each subframe */ | ||
530 | lag_low = matrix_ptr( Lag_range_ptr, k, 0, 2 ); | ||
531 | lag_high = matrix_ptr( Lag_range_ptr, k, 1, 2 ); | ||
532 | silk_assert(lag_high-lag_low+1 <= SCRATCH_SIZE); | ||
533 | celt_pitch_xcorr( target_ptr, target_ptr - start_lag - lag_high, xcorr, sf_length, lag_high - lag_low + 1, arch ); | ||
534 | for( j = lag_low; j <= lag_high; j++ ) { | ||
535 | silk_assert( lag_counter < SCRATCH_SIZE ); | ||
536 | scratch_mem[ lag_counter ] = xcorr[ lag_high - j ]; | ||
537 | lag_counter++; | ||
538 | } | ||
539 | |||
540 | delta = matrix_ptr( Lag_range_ptr, k, 0, 2 ); | ||
541 | for( i = 0; i < nb_cbk_search; i++ ) { | ||
542 | /* Fill out the 3 dim array that stores the correlations for */ | ||
543 | /* each code_book vector for each start lag */ | ||
544 | idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta; | ||
545 | for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) { | ||
546 | silk_assert( idx + j < SCRATCH_SIZE ); | ||
547 | silk_assert( idx + j < lag_counter ); | ||
548 | cross_corr_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ]; | ||
549 | } | ||
550 | } | ||
551 | target_ptr += sf_length; | ||
552 | } | ||
553 | } | ||
554 | |||
555 | /********************************************************************/ | ||
556 | /* Calculate the energies for first two subframes. The energies are */ | ||
557 | /* calculated recursively. */ | ||
558 | /********************************************************************/ | ||
559 | static void silk_P_Ana_calc_energy_st3( | ||
560 | silk_float energies_st3[ PE_MAX_NB_SUBFR ][ PE_NB_CBKS_STAGE3_MAX ][ PE_NB_STAGE3_LAGS ], /* O 3 DIM correlation array */ | ||
561 | const silk_float frame[], /* I vector to correlate */ | ||
562 | opus_int start_lag, /* I start lag */ | ||
563 | opus_int sf_length, /* I sub frame length */ | ||
564 | opus_int nb_subfr, /* I number of subframes */ | ||
565 | opus_int complexity /* I Complexity setting */ | ||
566 | ) | ||
567 | { | ||
568 | const silk_float *target_ptr, *basis_ptr; | ||
569 | double energy; | ||
570 | opus_int k, i, j, lag_counter; | ||
571 | opus_int nb_cbk_search, delta, idx, cbk_size, lag_diff; | ||
572 | silk_float scratch_mem[ SCRATCH_SIZE ]; | ||
573 | const opus_int8 *Lag_range_ptr, *Lag_CB_ptr; | ||
574 | |||
575 | celt_assert( complexity >= SILK_PE_MIN_COMPLEX ); | ||
576 | celt_assert( complexity <= SILK_PE_MAX_COMPLEX ); | ||
577 | |||
578 | if( nb_subfr == PE_MAX_NB_SUBFR ) { | ||
579 | Lag_range_ptr = &silk_Lag_range_stage3[ complexity ][ 0 ][ 0 ]; | ||
580 | Lag_CB_ptr = &silk_CB_lags_stage3[ 0 ][ 0 ]; | ||
581 | nb_cbk_search = silk_nb_cbk_searchs_stage3[ complexity ]; | ||
582 | cbk_size = PE_NB_CBKS_STAGE3_MAX; | ||
583 | } else { | ||
584 | celt_assert( nb_subfr == PE_MAX_NB_SUBFR >> 1); | ||
585 | Lag_range_ptr = &silk_Lag_range_stage3_10_ms[ 0 ][ 0 ]; | ||
586 | Lag_CB_ptr = &silk_CB_lags_stage3_10_ms[ 0 ][ 0 ]; | ||
587 | nb_cbk_search = PE_NB_CBKS_STAGE3_10MS; | ||
588 | cbk_size = PE_NB_CBKS_STAGE3_10MS; | ||
589 | } | ||
590 | |||
591 | target_ptr = &frame[ silk_LSHIFT( sf_length, 2 ) ]; | ||
592 | for( k = 0; k < nb_subfr; k++ ) { | ||
593 | lag_counter = 0; | ||
594 | |||
595 | /* Calculate the energy for first lag */ | ||
596 | basis_ptr = target_ptr - ( start_lag + matrix_ptr( Lag_range_ptr, k, 0, 2 ) ); | ||
597 | energy = silk_energy_FLP( basis_ptr, sf_length ) + 1e-3; | ||
598 | silk_assert( energy >= 0.0 ); | ||
599 | scratch_mem[lag_counter] = (silk_float)energy; | ||
600 | lag_counter++; | ||
601 | |||
602 | lag_diff = ( matrix_ptr( Lag_range_ptr, k, 1, 2 ) - matrix_ptr( Lag_range_ptr, k, 0, 2 ) + 1 ); | ||
603 | for( i = 1; i < lag_diff; i++ ) { | ||
604 | /* remove part outside new window */ | ||
605 | energy -= basis_ptr[sf_length - i] * (double)basis_ptr[sf_length - i]; | ||
606 | silk_assert( energy >= 0.0 ); | ||
607 | |||
608 | /* add part that comes into window */ | ||
609 | energy += basis_ptr[ -i ] * (double)basis_ptr[ -i ]; | ||
610 | silk_assert( energy >= 0.0 ); | ||
611 | silk_assert( lag_counter < SCRATCH_SIZE ); | ||
612 | scratch_mem[lag_counter] = (silk_float)energy; | ||
613 | lag_counter++; | ||
614 | } | ||
615 | |||
616 | delta = matrix_ptr( Lag_range_ptr, k, 0, 2 ); | ||
617 | for( i = 0; i < nb_cbk_search; i++ ) { | ||
618 | /* Fill out the 3 dim array that stores the correlations for */ | ||
619 | /* each code_book vector for each start lag */ | ||
620 | idx = matrix_ptr( Lag_CB_ptr, k, i, cbk_size ) - delta; | ||
621 | for( j = 0; j < PE_NB_STAGE3_LAGS; j++ ) { | ||
622 | silk_assert( idx + j < SCRATCH_SIZE ); | ||
623 | silk_assert( idx + j < lag_counter ); | ||
624 | energies_st3[ k ][ i ][ j ] = scratch_mem[ idx + j ]; | ||
625 | silk_assert( energies_st3[ k ][ i ][ j ] >= 0.0f ); | ||
626 | } | ||
627 | } | ||
628 | target_ptr += sf_length; | ||
629 | } | ||
630 | } | ||