Richard Burton | 0055346 | 2021-11-10 16:27:14 +0000 | [diff] [blame^] | 1 | /* |
| 2 | * Copyright (c) 2021 Arm Limited. All rights reserved. |
| 3 | * SPDX-License-Identifier: Apache-2.0 |
| 4 | * |
| 5 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 6 | * you may not use this file except in compliance with the License. |
| 7 | * You may obtain a copy of the License at |
| 8 | * |
| 9 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 10 | * |
| 11 | * Unless required by applicable law or agreed to in writing, software |
| 12 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 14 | * See the License for the specific language governing permissions and |
| 15 | * limitations under the License. |
| 16 | */ |
| 17 | #include "RNNoiseProcess.hpp" |
| 18 | #include <algorithm> |
| 19 | #include <cmath> |
| 20 | #include <cstring> |
| 21 | |
| 22 | namespace arm { |
| 23 | namespace app { |
| 24 | namespace rnn { |
| 25 | |
| 26 | #define VERIFY(x) \ |
| 27 | do { \ |
| 28 | if (!(x)) { \ |
| 29 | printf_err("Assert failed:" #x "\n"); \ |
| 30 | exit(1); \ |
| 31 | } \ |
| 32 | } while(0) |
| 33 | |
| 34 | RNNoiseProcess::RNNoiseProcess() : |
| 35 | m_halfWindow(FRAME_SIZE, 0), |
| 36 | m_dctTable(NB_BANDS * NB_BANDS), |
| 37 | m_analysisMem(FRAME_SIZE, 0), |
| 38 | m_cepstralMem(CEPS_MEM, vec1D32F(NB_BANDS, 0)), |
| 39 | m_memId{0}, |
| 40 | m_synthesisMem(FRAME_SIZE, 0), |
| 41 | m_pitchBuf(PITCH_BUF_SIZE, 0), |
| 42 | m_lastGain{0.0}, |
| 43 | m_lastPeriod{0}, |
| 44 | m_memHpX{}, |
| 45 | m_lastGVec(NB_BANDS, 0) |
| 46 | { |
| 47 | constexpr uint32_t numFFt = 2 * FRAME_SIZE; |
| 48 | static_assert(numFFt != 0, "Num FFT can't be 0"); |
| 49 | |
| 50 | math::MathUtils::FftInitF32(numFFt, this->m_fftInstReal, FftType::real); |
| 51 | math::MathUtils::FftInitF32(numFFt, this->m_fftInstCmplx, FftType::complex); |
| 52 | this->InitTables(); |
| 53 | } |
| 54 | |
| 55 | void RNNoiseProcess::PreprocessFrame(const float* audioData, |
| 56 | const size_t audioLen, |
| 57 | FrameFeatures& features) |
| 58 | { |
| 59 | /* Note audioWindow is modified in place */ |
| 60 | const arrHp aHp {-1.99599, 0.99600 }; |
| 61 | const arrHp bHp {-2.00000, 1.00000 }; |
| 62 | |
| 63 | vec1D32F audioWindow{audioData, audioData + audioLen}; |
| 64 | |
| 65 | this->BiQuad(bHp, aHp, this->m_memHpX, audioWindow); |
| 66 | this->ComputeFrameFeatures(audioWindow, features); |
| 67 | } |
| 68 | |
| 69 | void RNNoiseProcess::PostProcessFrame(vec1D32F& modelOutput, FrameFeatures& features, vec1D32F& outFrame) |
| 70 | { |
| 71 | std::vector<float> g = modelOutput; /* Gain values. */ |
| 72 | std::vector<float> gf(FREQ_SIZE, 0); |
| 73 | |
| 74 | if (!features.m_silence) { |
| 75 | PitchFilter(features, g); |
| 76 | for (size_t i = 0; i < NB_BANDS; i++) { |
| 77 | float alpha = .6f; |
| 78 | g[i] = std::max(g[i], alpha * m_lastGVec[i]); |
| 79 | m_lastGVec[i] = g[i]; |
| 80 | } |
| 81 | InterpBandGain(gf, g); |
| 82 | for (size_t i = 0; i < FREQ_SIZE; i++) { |
| 83 | features.m_fftX[2 * i] *= gf[i]; /* Real. */ |
| 84 | features.m_fftX[2 * i + 1] *= gf[i]; /*imaginary. */ |
| 85 | |
| 86 | } |
| 87 | |
| 88 | } |
| 89 | |
| 90 | FrameSynthesis(outFrame, features.m_fftX); |
| 91 | } |
| 92 | |
| 93 | void RNNoiseProcess::InitTables() |
| 94 | { |
| 95 | constexpr float pi = M_PI; |
| 96 | constexpr float halfPi = M_PI / 2; |
| 97 | constexpr float halfPiOverFrameSz = halfPi/FRAME_SIZE; |
| 98 | |
| 99 | for (uint32_t i = 0; i < FRAME_SIZE; i++) { |
| 100 | const float sinVal = math::MathUtils::SineF32(halfPiOverFrameSz * (i + 0.5f)); |
| 101 | m_halfWindow[i] = math::MathUtils::SineF32(halfPi * sinVal * sinVal); |
| 102 | } |
| 103 | |
| 104 | for (uint32_t i = 0; i < NB_BANDS; i++) { |
| 105 | for (uint32_t j = 0; j < NB_BANDS; j++) { |
| 106 | m_dctTable[i * NB_BANDS + j] = math::MathUtils::CosineF32((i + 0.5f) * j * pi / NB_BANDS); |
| 107 | } |
| 108 | m_dctTable[i * NB_BANDS] *= math::MathUtils::SqrtF32(0.5f); |
| 109 | } |
| 110 | } |
| 111 | |
| 112 | void RNNoiseProcess::BiQuad( |
| 113 | const arrHp& bHp, |
| 114 | const arrHp& aHp, |
| 115 | arrHp& memHpX, |
| 116 | vec1D32F& audioWindow) |
| 117 | { |
| 118 | for (float& audioElement : audioWindow) { |
| 119 | const auto xi = audioElement; |
| 120 | const auto yi = audioElement + memHpX[0]; |
| 121 | memHpX[0] = memHpX[1] + (bHp[0] * xi - aHp[0] * yi); |
| 122 | memHpX[1] = (bHp[1] * xi - aHp[1] * yi); |
| 123 | audioElement = yi; |
| 124 | } |
| 125 | } |
| 126 | |
| 127 | void RNNoiseProcess::ComputeFrameFeatures(vec1D32F& audioWindow, |
| 128 | FrameFeatures& features) |
| 129 | { |
| 130 | this->FrameAnalysis(audioWindow, |
| 131 | features.m_fftX, |
| 132 | features.m_Ex, |
| 133 | this->m_analysisMem); |
| 134 | |
| 135 | float E = 0.0; |
| 136 | |
| 137 | vec1D32F Ly(NB_BANDS, 0); |
| 138 | vec1D32F p(WINDOW_SIZE, 0); |
| 139 | vec1D32F pitchBuf(PITCH_BUF_SIZE >> 1, 0); |
| 140 | |
| 141 | VERIFY(PITCH_BUF_SIZE >= this->m_pitchBuf.size()); |
| 142 | std::copy_n(this->m_pitchBuf.begin() + FRAME_SIZE, |
| 143 | PITCH_BUF_SIZE - FRAME_SIZE, |
| 144 | this->m_pitchBuf.begin()); |
| 145 | |
| 146 | VERIFY(FRAME_SIZE <= audioWindow.size() && PITCH_BUF_SIZE > FRAME_SIZE); |
| 147 | std::copy_n(audioWindow.begin(), |
| 148 | FRAME_SIZE, |
| 149 | this->m_pitchBuf.begin() + PITCH_BUF_SIZE - FRAME_SIZE); |
| 150 | |
| 151 | this->PitchDownsample(pitchBuf, PITCH_BUF_SIZE); |
| 152 | |
| 153 | VERIFY(pitchBuf.size() > PITCH_MAX_PERIOD/2); |
| 154 | vec1D32F xLp(pitchBuf.size() - PITCH_MAX_PERIOD/2, 0); |
| 155 | std::copy_n(pitchBuf.begin() + PITCH_MAX_PERIOD/2, xLp.size(), xLp.begin()); |
| 156 | |
| 157 | int pitchIdx = this->PitchSearch(xLp, pitchBuf, |
| 158 | PITCH_FRAME_SIZE, (PITCH_MAX_PERIOD - (3*PITCH_MIN_PERIOD))); |
| 159 | |
| 160 | pitchIdx = this->RemoveDoubling( |
| 161 | pitchBuf, |
| 162 | PITCH_MAX_PERIOD, |
| 163 | PITCH_MIN_PERIOD, |
| 164 | PITCH_FRAME_SIZE, |
| 165 | PITCH_MAX_PERIOD - pitchIdx); |
| 166 | |
| 167 | size_t stIdx = PITCH_BUF_SIZE - WINDOW_SIZE - pitchIdx; |
| 168 | VERIFY((static_cast<int>(PITCH_BUF_SIZE) - static_cast<int>(WINDOW_SIZE) - pitchIdx) >= 0); |
| 169 | std::copy_n(this->m_pitchBuf.begin() + stIdx, WINDOW_SIZE, p.begin()); |
| 170 | |
| 171 | this->ApplyWindow(p); |
| 172 | this->ForwardTransform(p, features.m_fftP); |
| 173 | this->ComputeBandEnergy(features.m_fftP, features.m_Ep); |
| 174 | this->ComputeBandCorr(features.m_fftX, features.m_fftP, features.m_Exp); |
| 175 | |
| 176 | for (uint32_t i = 0 ; i < NB_BANDS; ++i) { |
| 177 | features.m_Exp[i] /= math::MathUtils::SqrtF32( |
| 178 | 0.001f + features.m_Ex[i] * features.m_Ep[i]); |
| 179 | } |
| 180 | |
| 181 | vec1D32F dctVec(NB_BANDS, 0); |
| 182 | this->DCT(features.m_Exp, dctVec); |
| 183 | |
| 184 | features.m_featuresVec = vec1D32F (NB_FEATURES, 0); |
| 185 | for (uint32_t i = 0; i < NB_DELTA_CEPS; ++i) { |
| 186 | features.m_featuresVec[NB_BANDS + 2*NB_DELTA_CEPS + i] = dctVec[i]; |
| 187 | } |
| 188 | |
| 189 | features.m_featuresVec[NB_BANDS + 2*NB_DELTA_CEPS] -= 1.3; |
| 190 | features.m_featuresVec[NB_BANDS + 2*NB_DELTA_CEPS + 1] -= 0.9; |
| 191 | features.m_featuresVec[NB_BANDS + 3*NB_DELTA_CEPS] = 0.01 * (static_cast<int>(pitchIdx) - 300); |
| 192 | |
| 193 | float logMax = -2.f; |
| 194 | float follow = -2.f; |
| 195 | for (uint32_t i = 0; i < NB_BANDS; ++i) { |
| 196 | Ly[i] = log10f(1e-2f + features.m_Ex[i]); |
| 197 | Ly[i] = std::max<float>(logMax - 7, std::max<float>(follow - 1.5, Ly[i])); |
| 198 | logMax = std::max<float>(logMax, Ly[i]); |
| 199 | follow = std::max<float>(follow - 1.5, Ly[i]); |
| 200 | E += features.m_Ex[i]; |
| 201 | } |
| 202 | |
| 203 | /* If there's no audio avoid messing up the state. */ |
| 204 | features.m_silence = true; |
| 205 | if (E < 0.04) { |
| 206 | return; |
| 207 | } else { |
| 208 | features.m_silence = false; |
| 209 | } |
| 210 | |
| 211 | this->DCT(Ly, features.m_featuresVec); |
| 212 | features.m_featuresVec[0] -= 12.0; |
| 213 | features.m_featuresVec[1] -= 4.0; |
| 214 | |
| 215 | VERIFY(CEPS_MEM > 2); |
| 216 | uint32_t stIdx1 = this->m_memId < 1 ? CEPS_MEM + this->m_memId - 1 : this->m_memId - 1; |
| 217 | uint32_t stIdx2 = this->m_memId < 2 ? CEPS_MEM + this->m_memId - 2 : this->m_memId - 2; |
| 218 | |
| 219 | auto ceps1 = this->m_cepstralMem[stIdx1]; |
| 220 | auto ceps2 = this->m_cepstralMem[stIdx2]; |
| 221 | |
| 222 | /* Ceps 0 */ |
| 223 | for (uint32_t i = 0; i < NB_BANDS; ++i) { |
| 224 | this->m_cepstralMem[this->m_memId][i] = features.m_featuresVec[i]; |
| 225 | } |
| 226 | |
| 227 | for (uint32_t i = 0; i < NB_DELTA_CEPS; ++i) { |
| 228 | features.m_featuresVec[i] = this->m_cepstralMem[this->m_memId][i] + ceps1[i] + ceps2[i]; |
| 229 | features.m_featuresVec[NB_BANDS + i] = this->m_cepstralMem[this->m_memId][i] - ceps2[i]; |
| 230 | features.m_featuresVec[NB_BANDS + NB_DELTA_CEPS + i] = |
| 231 | this->m_cepstralMem[this->m_memId][i] - 2 * ceps1[i] + ceps2[i]; |
| 232 | } |
| 233 | |
| 234 | /* Spectral variability features. */ |
| 235 | this->m_memId += 1; |
| 236 | if (this->m_memId == CEPS_MEM) { |
| 237 | this->m_memId = 0; |
| 238 | } |
| 239 | |
| 240 | float specVariability = 0.f; |
| 241 | |
| 242 | VERIFY(this->m_cepstralMem.size() >= CEPS_MEM); |
| 243 | for (size_t i = 0; i < CEPS_MEM; ++i) { |
| 244 | float minDist = 1e15; |
| 245 | for (size_t j = 0; j < CEPS_MEM; ++j) { |
| 246 | float dist = 0.f; |
| 247 | for (size_t k = 0; k < NB_BANDS; ++k) { |
| 248 | VERIFY(this->m_cepstralMem[i].size() >= NB_BANDS); |
| 249 | auto tmp = this->m_cepstralMem[i][k] - this->m_cepstralMem[j][k]; |
| 250 | dist += tmp * tmp; |
| 251 | } |
| 252 | |
| 253 | if (j != i) { |
| 254 | minDist = std::min<float>(minDist, dist); |
| 255 | } |
| 256 | } |
| 257 | specVariability += minDist; |
| 258 | } |
| 259 | |
| 260 | VERIFY(features.m_featuresVec.size() >= NB_BANDS + 3 * NB_DELTA_CEPS + 1); |
| 261 | features.m_featuresVec[NB_BANDS + 3 * NB_DELTA_CEPS + 1] = specVariability / CEPS_MEM - 2.1; |
| 262 | } |
| 263 | |
| 264 | void RNNoiseProcess::FrameAnalysis( |
| 265 | const vec1D32F& audioWindow, |
| 266 | vec1D32F& fft, |
| 267 | vec1D32F& energy, |
| 268 | vec1D32F& analysisMem) |
| 269 | { |
| 270 | vec1D32F x(WINDOW_SIZE, 0); |
| 271 | |
| 272 | /* Move old audio down and populate end with latest audio window. */ |
| 273 | VERIFY(x.size() >= FRAME_SIZE && analysisMem.size() >= FRAME_SIZE); |
| 274 | VERIFY(audioWindow.size() >= FRAME_SIZE); |
| 275 | |
| 276 | std::copy_n(analysisMem.begin(), FRAME_SIZE, x.begin()); |
| 277 | std::copy_n(audioWindow.begin(), x.size() - FRAME_SIZE, x.begin() + FRAME_SIZE); |
| 278 | std::copy_n(audioWindow.begin(), FRAME_SIZE, analysisMem.begin()); |
| 279 | |
| 280 | this->ApplyWindow(x); |
| 281 | |
| 282 | /* Calculate FFT. */ |
| 283 | ForwardTransform(x, fft); |
| 284 | |
| 285 | /* Compute band energy. */ |
| 286 | ComputeBandEnergy(fft, energy); |
| 287 | } |
| 288 | |
| 289 | void RNNoiseProcess::ApplyWindow(vec1D32F& x) |
| 290 | { |
| 291 | if (WINDOW_SIZE != x.size()) { |
| 292 | printf_err("Invalid size for vector to be windowed\n"); |
| 293 | return; |
| 294 | } |
| 295 | |
| 296 | VERIFY(this->m_halfWindow.size() >= FRAME_SIZE); |
| 297 | |
| 298 | /* Multiply input by sinusoidal function. */ |
| 299 | for (size_t i = 0; i < FRAME_SIZE; i++) { |
| 300 | x[i] *= this->m_halfWindow[i]; |
| 301 | x[WINDOW_SIZE - 1 - i] *= this->m_halfWindow[i]; |
| 302 | } |
| 303 | } |
| 304 | |
| 305 | void RNNoiseProcess::ForwardTransform( |
| 306 | vec1D32F& x, |
| 307 | vec1D32F& fft) |
| 308 | { |
| 309 | /* The input vector can be modified by the fft function. */ |
| 310 | fft.reserve(x.size() + 2); |
| 311 | fft.resize(x.size() + 2, 0); |
| 312 | math::MathUtils::FftF32(x, fft, this->m_fftInstReal); |
| 313 | |
| 314 | /* Normalise. */ |
| 315 | for (auto& f : fft) { |
| 316 | f /= this->m_fftInstReal.m_fftLen; |
| 317 | } |
| 318 | |
| 319 | /* Place the last freq element correctly */ |
| 320 | fft[fft.size()-2] = fft[1]; |
| 321 | fft[1] = 0; |
| 322 | |
| 323 | /* NOTE: We don't truncate out FFT vector as it already contains only the |
| 324 | * first half of the FFT's. The conjugates are not present. */ |
| 325 | } |
| 326 | |
| 327 | void RNNoiseProcess::ComputeBandEnergy(const vec1D32F& fftX, vec1D32F& bandE) |
| 328 | { |
| 329 | bandE = vec1D32F(NB_BANDS, 0); |
| 330 | |
| 331 | VERIFY(this->m_eband5ms.size() >= NB_BANDS); |
| 332 | for (uint32_t i = 0; i < NB_BANDS - 1; i++) { |
| 333 | const auto bandSize = (this->m_eband5ms[i + 1] - this->m_eband5ms[i]) |
| 334 | << FRAME_SIZE_SHIFT; |
| 335 | |
| 336 | for (uint32_t j = 0; j < bandSize; j++) { |
| 337 | const auto frac = static_cast<float>(j) / bandSize; |
| 338 | const auto idx = (this->m_eband5ms[i] << FRAME_SIZE_SHIFT) + j; |
| 339 | |
| 340 | auto tmp = fftX[2 * idx] * fftX[2 * idx]; /* Real part */ |
| 341 | tmp += fftX[2 * idx + 1] * fftX[2 * idx + 1]; /* Imaginary part */ |
| 342 | |
| 343 | bandE[i] += (1 - frac) * tmp; |
| 344 | bandE[i + 1] += frac * tmp; |
| 345 | } |
| 346 | } |
| 347 | bandE[0] *= 2; |
| 348 | bandE[NB_BANDS - 1] *= 2; |
| 349 | } |
| 350 | |
| 351 | void RNNoiseProcess::ComputeBandCorr(const vec1D32F& X, const vec1D32F& P, vec1D32F& bandC) |
| 352 | { |
| 353 | bandC = vec1D32F(NB_BANDS, 0); |
| 354 | VERIFY(this->m_eband5ms.size() >= NB_BANDS); |
| 355 | |
| 356 | for (uint32_t i = 0; i < NB_BANDS - 1; i++) { |
| 357 | const auto bandSize = (this->m_eband5ms[i + 1] - this->m_eband5ms[i]) << FRAME_SIZE_SHIFT; |
| 358 | |
| 359 | for (uint32_t j = 0; j < bandSize; j++) { |
| 360 | const auto frac = static_cast<float>(j) / bandSize; |
| 361 | const auto idx = (this->m_eband5ms[i] << FRAME_SIZE_SHIFT) + j; |
| 362 | |
| 363 | auto tmp = X[2 * idx] * P[2 * idx]; /* Real part */ |
| 364 | tmp += X[2 * idx + 1] * P[2 * idx + 1]; /* Imaginary part */ |
| 365 | |
| 366 | bandC[i] += (1 - frac) * tmp; |
| 367 | bandC[i + 1] += frac * tmp; |
| 368 | } |
| 369 | } |
| 370 | bandC[0] *= 2; |
| 371 | bandC[NB_BANDS - 1] *= 2; |
| 372 | } |
| 373 | |
| 374 | void RNNoiseProcess::DCT(vec1D32F& input, vec1D32F& output) |
| 375 | { |
| 376 | VERIFY(this->m_dctTable.size() >= NB_BANDS * NB_BANDS); |
| 377 | for (uint32_t i = 0; i < NB_BANDS; ++i) { |
| 378 | float sum = 0; |
| 379 | |
| 380 | for (uint32_t j = 0, k = 0; j < NB_BANDS; ++j, k += NB_BANDS) { |
| 381 | sum += input[j] * this->m_dctTable[k + i]; |
| 382 | } |
| 383 | output[i] = sum * math::MathUtils::SqrtF32(2.0/22); |
| 384 | } |
| 385 | } |
| 386 | |
| 387 | void RNNoiseProcess::PitchDownsample(vec1D32F& pitchBuf, size_t pitchBufSz) { |
| 388 | for (size_t i = 1; i < (pitchBufSz >> 1); ++i) { |
| 389 | pitchBuf[i] = 0.5 * ( |
| 390 | 0.5 * (this->m_pitchBuf[2 * i - 1] + this->m_pitchBuf[2 * i + 1]) |
| 391 | + this->m_pitchBuf[2 * i]); |
| 392 | } |
| 393 | |
| 394 | pitchBuf[0] = 0.5*(0.5*(this->m_pitchBuf[1]) + this->m_pitchBuf[0]); |
| 395 | |
| 396 | vec1D32F ac(5, 0); |
| 397 | size_t numLags = 4; |
| 398 | |
| 399 | this->AutoCorr(pitchBuf, ac, numLags, pitchBufSz >> 1); |
| 400 | |
| 401 | /* Noise floor -40db */ |
| 402 | ac[0] *= 1.0001; |
| 403 | |
| 404 | /* Lag windowing. */ |
| 405 | for (size_t i = 1; i < numLags + 1; ++i) { |
| 406 | ac[i] -= ac[i] * (0.008 * i) * (0.008 * i); |
| 407 | } |
| 408 | |
| 409 | vec1D32F lpc(numLags, 0); |
| 410 | this->LPC(ac, numLags, lpc); |
| 411 | |
| 412 | float tmp = 1.0; |
| 413 | for (size_t i = 0; i < numLags; ++i) { |
| 414 | tmp = 0.9f * tmp; |
| 415 | lpc[i] = lpc[i] * tmp; |
| 416 | } |
| 417 | |
| 418 | vec1D32F lpc2(numLags + 1, 0); |
| 419 | float c1 = 0.8; |
| 420 | |
| 421 | /* Add a zero. */ |
| 422 | lpc2[0] = lpc[0] + 0.8; |
| 423 | lpc2[1] = lpc[1] + (c1 * lpc[0]); |
| 424 | lpc2[2] = lpc[2] + (c1 * lpc[1]); |
| 425 | lpc2[3] = lpc[3] + (c1 * lpc[2]); |
| 426 | lpc2[4] = (c1 * lpc[3]); |
| 427 | |
| 428 | this->Fir5(lpc2, pitchBufSz >> 1, pitchBuf); |
| 429 | } |
| 430 | |
| 431 | int RNNoiseProcess::PitchSearch(vec1D32F& xLp, vec1D32F& y, uint32_t len, uint32_t maxPitch) { |
| 432 | uint32_t lag = len + maxPitch; |
| 433 | vec1D32F xLp4(len >> 2, 0); |
| 434 | vec1D32F yLp4(lag >> 2, 0); |
| 435 | vec1D32F xCorr(maxPitch >> 1, 0); |
| 436 | |
| 437 | /* Downsample by 2 again. */ |
| 438 | for (size_t j = 0; j < (len >> 2); ++j) { |
| 439 | xLp4[j] = xLp[2*j]; |
| 440 | } |
| 441 | for (size_t j = 0; j < (lag >> 2); ++j) { |
| 442 | yLp4[j] = y[2*j]; |
| 443 | } |
| 444 | |
| 445 | this->PitchXCorr(xLp4, yLp4, xCorr, len >> 2, maxPitch >> 2); |
| 446 | |
| 447 | /* Coarse search with 4x decimation. */ |
| 448 | arrHp bestPitch = this->FindBestPitch(xCorr, yLp4, len >> 2, maxPitch >> 2); |
| 449 | |
| 450 | /* Finer search with 2x decimation. */ |
| 451 | const int maxIdx = (maxPitch >> 1); |
| 452 | for (int i = 0; i < maxIdx; ++i) { |
| 453 | xCorr[i] = 0; |
| 454 | if (std::abs(i - 2*bestPitch[0]) > 2 and std::abs(i - 2*bestPitch[1]) > 2) { |
| 455 | continue; |
| 456 | } |
| 457 | float sum = 0; |
| 458 | for (size_t j = 0; j < len >> 1; ++j) { |
| 459 | sum += xLp[j] * y[i+j]; |
| 460 | } |
| 461 | |
| 462 | xCorr[i] = std::max(-1.0f, sum); |
| 463 | } |
| 464 | |
| 465 | bestPitch = this->FindBestPitch(xCorr, y, len >> 1, maxPitch >> 1); |
| 466 | |
| 467 | int offset; |
| 468 | /* Refine by pseudo-interpolation. */ |
| 469 | if ( 0 < bestPitch[0] && bestPitch[0] < ((maxPitch >> 1) - 1)) { |
| 470 | float a = xCorr[bestPitch[0] - 1]; |
| 471 | float b = xCorr[bestPitch[0]]; |
| 472 | float c = xCorr[bestPitch[0] + 1]; |
| 473 | |
| 474 | if ( (c-a) > 0.7*(b-a) ) { |
| 475 | offset = 1; |
| 476 | } else if ( (a-c) > 0.7*(b-c) ) { |
| 477 | offset = -1; |
| 478 | } else { |
| 479 | offset = 0; |
| 480 | } |
| 481 | } else { |
| 482 | offset = 0; |
| 483 | } |
| 484 | |
| 485 | return 2*bestPitch[0] - offset; |
| 486 | } |
| 487 | |
| 488 | arrHp RNNoiseProcess::FindBestPitch(vec1D32F& xCorr, vec1D32F& y, uint32_t len, uint32_t maxPitch) |
| 489 | { |
| 490 | float Syy = 1; |
| 491 | arrHp bestNum {-1, -1}; |
| 492 | arrHp bestDen {0, 0}; |
| 493 | arrHp bestPitch {0, 1}; |
| 494 | |
| 495 | for (size_t j = 0; j < len; ++j) { |
| 496 | Syy += (y[j] * y[j]); |
| 497 | } |
| 498 | |
| 499 | for (size_t i = 0; i < maxPitch; ++i ) { |
| 500 | if (xCorr[i] > 0) { |
| 501 | float xCorr16 = xCorr[i] * 1e-12f; /* Avoid problems when squaring. */ |
| 502 | |
| 503 | float num = xCorr16 * xCorr16; |
| 504 | if (num*bestDen[1] > bestNum[1]*Syy) { |
| 505 | if (num*bestDen[0] > bestNum[0]*Syy) { |
| 506 | bestNum[1] = bestNum[0]; |
| 507 | bestDen[1] = bestDen[0]; |
| 508 | bestPitch[1] = bestPitch[0]; |
| 509 | bestNum[0] = num; |
| 510 | bestDen[0] = Syy; |
| 511 | bestPitch[0] = i; |
| 512 | } else { |
| 513 | bestNum[1] = num; |
| 514 | bestDen[1] = Syy; |
| 515 | bestPitch[1] = i; |
| 516 | } |
| 517 | } |
| 518 | } |
| 519 | |
| 520 | Syy += (y[i+len]*y[i+len]) - (y[i]*y[i]); |
| 521 | Syy = std::max(1.0f, Syy); |
| 522 | } |
| 523 | |
| 524 | return bestPitch; |
| 525 | } |
| 526 | |
| 527 | int RNNoiseProcess::RemoveDoubling( |
| 528 | vec1D32F& pitchBuf, |
| 529 | uint32_t maxPeriod, |
| 530 | uint32_t minPeriod, |
| 531 | uint32_t frameSize, |
| 532 | size_t pitchIdx0_) |
| 533 | { |
| 534 | constexpr std::array<size_t, 16> secondCheck {0, 0, 3, 2, 3, 2, 5, 2, 3, 2, 3, 2, 5, 2, 3, 2}; |
| 535 | uint32_t minPeriod0 = minPeriod; |
| 536 | float lastPeriod = static_cast<float>(this->m_lastPeriod)/2; |
| 537 | float lastGain = static_cast<float>(this->m_lastGain); |
| 538 | |
| 539 | maxPeriod /= 2; |
| 540 | minPeriod /= 2; |
| 541 | pitchIdx0_ /= 2; |
| 542 | frameSize /= 2; |
| 543 | uint32_t xStart = maxPeriod; |
| 544 | |
| 545 | if (pitchIdx0_ >= maxPeriod) { |
| 546 | pitchIdx0_ = maxPeriod - 1; |
| 547 | } |
| 548 | |
| 549 | size_t pitchIdx = pitchIdx0_; |
| 550 | size_t pitchIdx0 = pitchIdx0_; |
| 551 | |
| 552 | float xx = 0; |
| 553 | for ( size_t i = xStart; i < xStart+frameSize; ++i) { |
| 554 | xx += (pitchBuf[i] * pitchBuf[i]); |
| 555 | } |
| 556 | |
| 557 | float xy = 0; |
| 558 | for ( size_t i = xStart; i < xStart+frameSize; ++i) { |
| 559 | xy += (pitchBuf[i] * pitchBuf[i-pitchIdx0]); |
| 560 | } |
| 561 | |
| 562 | vec1D32F yyLookup (maxPeriod+1, 0); |
| 563 | yyLookup[0] = xx; |
| 564 | float yy = xx; |
| 565 | |
| 566 | for ( size_t i = 1; i < maxPeriod+1; ++i) { |
| 567 | yy = yy + (pitchBuf[xStart-i] * pitchBuf[xStart-i]) - |
| 568 | (pitchBuf[xStart+frameSize-i] * pitchBuf[xStart+frameSize-i]); |
| 569 | yyLookup[i] = std::max(0.0f, yy); |
| 570 | } |
| 571 | |
| 572 | yy = yyLookup[pitchIdx0]; |
| 573 | float bestXy = xy; |
| 574 | float bestYy = yy; |
| 575 | |
| 576 | float g = this->ComputePitchGain(xy, xx, yy); |
| 577 | float g0 = g; |
| 578 | |
| 579 | /* Look for any pitch at pitchIndex/k. */ |
| 580 | for ( size_t k = 2; k < 16; ++k) { |
| 581 | size_t pitchIdx1 = (2*pitchIdx0+k) / (2*k); |
| 582 | if (pitchIdx1 < minPeriod) { |
| 583 | break; |
| 584 | } |
| 585 | |
| 586 | size_t pitchIdx1b; |
| 587 | /* Look for another strong correlation at T1b. */ |
| 588 | if (k == 2) { |
| 589 | if ((pitchIdx1 + pitchIdx0) > maxPeriod) { |
| 590 | pitchIdx1b = pitchIdx0; |
| 591 | } else { |
| 592 | pitchIdx1b = pitchIdx0 + pitchIdx1; |
| 593 | } |
| 594 | } else { |
| 595 | pitchIdx1b = (2*(secondCheck[k])*pitchIdx0 + k) / (2*k); |
| 596 | } |
| 597 | |
| 598 | xy = 0; |
| 599 | for ( size_t i = xStart; i < xStart+frameSize; ++i) { |
| 600 | xy += (pitchBuf[i] * pitchBuf[i-pitchIdx1]); |
| 601 | } |
| 602 | |
| 603 | float xy2 = 0; |
| 604 | for ( size_t i = xStart; i < xStart+frameSize; ++i) { |
| 605 | xy2 += (pitchBuf[i] * pitchBuf[i-pitchIdx1b]); |
| 606 | } |
| 607 | xy = 0.5f * (xy + xy2); |
| 608 | yy = 0.5f * (yyLookup[pitchIdx1] + yyLookup[pitchIdx1b]); |
| 609 | |
| 610 | float g1 = this->ComputePitchGain(xy, xx, yy); |
| 611 | |
| 612 | float cont; |
| 613 | if (std::abs(pitchIdx1-lastPeriod) <= 1) { |
| 614 | cont = lastGain; |
| 615 | } else if (std::abs(pitchIdx1-lastPeriod) <= 2 and 5*k*k < pitchIdx0) { |
| 616 | cont = 0.5f*lastGain; |
| 617 | } else { |
| 618 | cont = 0.0f; |
| 619 | } |
| 620 | |
| 621 | float thresh = std::max(0.3, 0.7*g0-cont); |
| 622 | |
| 623 | /* Bias against very high pitch (very short period) to avoid false-positives |
| 624 | * due to short-term correlation */ |
| 625 | if (pitchIdx1 < 3*minPeriod) { |
| 626 | thresh = std::max(0.4, 0.85*g0-cont); |
| 627 | } else if (pitchIdx1 < 2*minPeriod) { |
| 628 | thresh = std::max(0.5, 0.9*g0-cont); |
| 629 | } |
| 630 | if (g1 > thresh) { |
| 631 | bestXy = xy; |
| 632 | bestYy = yy; |
| 633 | pitchIdx = pitchIdx1; |
| 634 | g = g1; |
| 635 | } |
| 636 | } |
| 637 | |
| 638 | bestXy = std::max(0.0f, bestXy); |
| 639 | float pg; |
| 640 | if (bestYy <= bestXy) { |
| 641 | pg = 1.0; |
| 642 | } else { |
| 643 | pg = bestXy/(bestYy+1); |
| 644 | } |
| 645 | |
| 646 | std::array<float, 3> xCorr {0}; |
| 647 | for ( size_t k = 0; k < 3; ++k ) { |
| 648 | for ( size_t i = xStart; i < xStart+frameSize; ++i) { |
| 649 | xCorr[k] += (pitchBuf[i] * pitchBuf[i-(pitchIdx+k-1)]); |
| 650 | } |
| 651 | } |
| 652 | |
| 653 | size_t offset; |
| 654 | if ((xCorr[2]-xCorr[0]) > 0.7*(xCorr[1]-xCorr[0])) { |
| 655 | offset = 1; |
| 656 | } else if ((xCorr[0]-xCorr[2]) > 0.7*(xCorr[1]-xCorr[2])) { |
| 657 | offset = -1; |
| 658 | } else { |
| 659 | offset = 0; |
| 660 | } |
| 661 | |
| 662 | if (pg > g) { |
| 663 | pg = g; |
| 664 | } |
| 665 | |
| 666 | pitchIdx0_ = 2*pitchIdx + offset; |
| 667 | |
| 668 | if (pitchIdx0_ < minPeriod0) { |
| 669 | pitchIdx0_ = minPeriod0; |
| 670 | } |
| 671 | |
| 672 | this->m_lastPeriod = pitchIdx0_; |
| 673 | this->m_lastGain = pg; |
| 674 | |
| 675 | return this->m_lastPeriod; |
| 676 | } |
| 677 | |
| 678 | float RNNoiseProcess::ComputePitchGain(float xy, float xx, float yy) |
| 679 | { |
| 680 | return xy / math::MathUtils::SqrtF32(1+xx*yy); |
| 681 | } |
| 682 | |
| 683 | void RNNoiseProcess::AutoCorr( |
| 684 | const vec1D32F& x, |
| 685 | vec1D32F& ac, |
| 686 | size_t lag, |
| 687 | size_t n) |
| 688 | { |
| 689 | if (n < lag) { |
| 690 | printf_err("Invalid parameters for AutoCorr\n"); |
| 691 | return; |
| 692 | } |
| 693 | |
| 694 | auto fastN = n - lag; |
| 695 | |
| 696 | /* Auto-correlation - can be done by PlatformMath functions */ |
| 697 | this->PitchXCorr(x, x, ac, fastN, lag + 1); |
| 698 | |
| 699 | /* Modify auto-correlation by summing with auto-correlation for different lags. */ |
| 700 | for (size_t k = 0; k < lag + 1; k++) { |
| 701 | float d = 0; |
| 702 | for (size_t i = k + fastN; i < n; i++) { |
| 703 | d += x[i] * x[i - k]; |
| 704 | } |
| 705 | ac[k] += d; |
| 706 | } |
| 707 | } |
| 708 | |
| 709 | |
| 710 | void RNNoiseProcess::PitchXCorr( |
| 711 | const vec1D32F& x, |
| 712 | const vec1D32F& y, |
| 713 | vec1D32F& ac, |
| 714 | size_t len, |
| 715 | size_t maxPitch) |
| 716 | { |
| 717 | for (size_t i = 0; i < maxPitch; i++) { |
| 718 | float sum = 0; |
| 719 | for (size_t j = 0; j < len; j++) { |
| 720 | sum += x[j] * y[i + j]; |
| 721 | } |
| 722 | ac[i] = sum; |
| 723 | } |
| 724 | } |
| 725 | |
| 726 | /* Linear predictor coefficients */ |
| 727 | void RNNoiseProcess::LPC( |
| 728 | const vec1D32F& ac, |
| 729 | int32_t p, |
| 730 | vec1D32F& lpc) |
| 731 | { |
| 732 | auto error = ac[0]; |
| 733 | |
| 734 | if (error != 0) { |
| 735 | for (int i = 0; i < p; i++) { |
| 736 | |
| 737 | /* Sum up this iteration's reflection coefficient */ |
| 738 | float rr = 0; |
| 739 | for (int j = 0; j < i; j++) { |
| 740 | rr += lpc[j] * ac[i - j]; |
| 741 | } |
| 742 | |
| 743 | rr += ac[i + 1]; |
| 744 | auto r = -rr / error; |
| 745 | |
| 746 | /* Update LP coefficients and total error */ |
| 747 | lpc[i] = r; |
| 748 | for (int j = 0; j < ((i + 1) >> 1); j++) { |
| 749 | auto tmp1 = lpc[j]; |
| 750 | auto tmp2 = lpc[i - 1 - j]; |
| 751 | lpc[j] = tmp1 + (r * tmp2); |
| 752 | lpc[i - 1 - j] = tmp2 + (r * tmp1); |
| 753 | } |
| 754 | |
| 755 | error = error - (r * r * error); |
| 756 | |
| 757 | /* Bail out once we get 30dB gain */ |
| 758 | if (error < (0.001 * ac[0])) { |
| 759 | break; |
| 760 | } |
| 761 | } |
| 762 | } |
| 763 | } |
| 764 | |
| 765 | void RNNoiseProcess::Fir5( |
| 766 | const vec1D32F &num, |
| 767 | uint32_t N, |
| 768 | vec1D32F &x) |
| 769 | { |
| 770 | auto num0 = num[0]; |
| 771 | auto num1 = num[1]; |
| 772 | auto num2 = num[2]; |
| 773 | auto num3 = num[3]; |
| 774 | auto num4 = num[4]; |
| 775 | auto mem0 = 0; |
| 776 | auto mem1 = 0; |
| 777 | auto mem2 = 0; |
| 778 | auto mem3 = 0; |
| 779 | auto mem4 = 0; |
| 780 | for (uint32_t i = 0; i < N; i++) |
| 781 | { |
| 782 | auto sum_ = x[i] + (num0 * mem0) + (num1 * mem1) + |
| 783 | (num2 * mem2) + (num3 * mem3) + (num4 * mem4); |
| 784 | mem4 = mem3; |
| 785 | mem3 = mem2; |
| 786 | mem2 = mem1; |
| 787 | mem1 = mem0; |
| 788 | mem0 = x[i]; |
| 789 | x[i] = sum_; |
| 790 | } |
| 791 | } |
| 792 | |
| 793 | void RNNoiseProcess::PitchFilter(FrameFeatures &features, vec1D32F &g) { |
| 794 | std::vector<float> r(NB_BANDS, 0); |
| 795 | std::vector<float> rf(FREQ_SIZE, 0); |
| 796 | std::vector<float> newE(NB_BANDS); |
| 797 | |
| 798 | for (size_t i = 0; i < NB_BANDS; i++) { |
| 799 | if (features.m_Exp[i] > g[i]) { |
| 800 | r[i] = 1; |
| 801 | } else { |
| 802 | |
| 803 | |
| 804 | r[i] = std::pow(features.m_Exp[i], 2) * (1 - std::pow(g[i], 2)) / |
| 805 | (.001 + std::pow(g[i], 2) * (1 - std::pow(features.m_Exp[i], 2))); |
| 806 | } |
| 807 | |
| 808 | |
| 809 | r[i] = math::MathUtils::SqrtF32(std::min(1.0f, std::max(0.0f, r[i]))); |
| 810 | r[i] *= math::MathUtils::SqrtF32(features.m_Ex[i] / (1e-8f + features.m_Ep[i])); |
| 811 | } |
| 812 | |
| 813 | InterpBandGain(rf, r); |
| 814 | for (size_t i = 0; i < FREQ_SIZE - 1; i++) { |
| 815 | features.m_fftX[2 * i] += rf[i] * features.m_fftP[2 * i]; /* Real. */ |
| 816 | features.m_fftX[2 * i + 1] += rf[i] * features.m_fftP[2 * i + 1]; /* Imaginary. */ |
| 817 | |
| 818 | } |
| 819 | ComputeBandEnergy(features.m_fftX, newE); |
| 820 | std::vector<float> norm(NB_BANDS); |
| 821 | std::vector<float> normf(FRAME_SIZE, 0); |
| 822 | for (size_t i = 0; i < NB_BANDS; i++) { |
| 823 | norm[i] = math::MathUtils::SqrtF32(features.m_Ex[i] / (1e-8f + newE[i])); |
| 824 | } |
| 825 | |
| 826 | InterpBandGain(normf, norm); |
| 827 | for (size_t i = 0; i < FREQ_SIZE - 1; i++) { |
| 828 | features.m_fftX[2 * i] *= normf[i]; /* Real. */ |
| 829 | features.m_fftX[2 * i + 1] *= normf[i]; /* Imaginary. */ |
| 830 | |
| 831 | } |
| 832 | } |
| 833 | |
| 834 | void RNNoiseProcess::FrameSynthesis(vec1D32F& outFrame, vec1D32F& fftY) { |
| 835 | std::vector<float> x(WINDOW_SIZE, 0); |
| 836 | InverseTransform(x, fftY); |
| 837 | ApplyWindow(x); |
| 838 | for (size_t i = 0; i < FRAME_SIZE; i++) { |
| 839 | outFrame[i] = x[i] + m_synthesisMem[i]; |
| 840 | } |
| 841 | memcpy((m_synthesisMem.data()), &x[FRAME_SIZE], FRAME_SIZE*sizeof(float)); |
| 842 | } |
| 843 | |
| 844 | void RNNoiseProcess::InterpBandGain(vec1D32F& g, vec1D32F& bandE) { |
| 845 | for (size_t i = 0; i < NB_BANDS - 1; i++) { |
| 846 | int bandSize = (m_eband5ms[i + 1] - m_eband5ms[i]) << FRAME_SIZE_SHIFT; |
| 847 | for (int j = 0; j < bandSize; j++) { |
| 848 | float frac = static_cast<float>(j) / bandSize; |
| 849 | g[(m_eband5ms[i] << FRAME_SIZE_SHIFT) + j] = (1 - frac) * bandE[i] + frac * bandE[i + 1]; |
| 850 | } |
| 851 | } |
| 852 | } |
| 853 | |
| 854 | void RNNoiseProcess::InverseTransform(vec1D32F& out, vec1D32F& fftXIn) { |
| 855 | |
| 856 | std::vector<float> x(WINDOW_SIZE * 2); /* This is complex. */ |
| 857 | vec1D32F newFFT; /* This is complex. */ |
| 858 | |
| 859 | size_t i; |
| 860 | for (i = 0; i < FREQ_SIZE * 2; i++) { |
| 861 | x[i] = fftXIn[i]; |
| 862 | } |
| 863 | for (i = FREQ_SIZE; i < WINDOW_SIZE; i++) { |
| 864 | x[2 * i] = x[2 * (WINDOW_SIZE - i)]; /* Real. */ |
| 865 | x[2 * i + 1] = -x[2 * (WINDOW_SIZE - i) + 1]; /* Imaginary. */ |
| 866 | } |
| 867 | |
| 868 | constexpr uint32_t numFFt = 2 * FRAME_SIZE; |
| 869 | static_assert(numFFt != 0); |
| 870 | |
| 871 | vec1D32F fftOut = vec1D32F(x.size(), 0); |
| 872 | math::MathUtils::FftF32(x,fftOut, m_fftInstCmplx); |
| 873 | |
| 874 | /* Normalize. */ |
| 875 | for (auto &f: fftOut) { |
| 876 | f /= numFFt; |
| 877 | } |
| 878 | |
| 879 | out[0] = WINDOW_SIZE * fftOut[0]; /* Real. */ |
| 880 | for (i = 1; i < WINDOW_SIZE; i++) { |
| 881 | out[i] = WINDOW_SIZE * fftOut[(WINDOW_SIZE * 2) - (2 * i)]; /* Real. */ |
| 882 | } |
| 883 | } |
| 884 | |
| 885 | |
| 886 | } /* namespace rnn */ |
| 887 | } /* namespace app */ |
| 888 | } /* namspace arm */ |