Fix the gain calculation in IntelligibilityEnhancer

webrtc/modules/audio_processing/intelligibility/intelligibility_enhancer.cc

 /*
  *  Copyright (c) 2014 The WebRTC project authors. All Rights Reserved.
  *
  *  Use of this source code is governed by a BSD-style license
  *  that can be found in the LICENSE file in the root of the source
  *  tree. An additional intellectual property rights grant can be found
  *  in the file PATENTS.  All contributing project authors may
  *  be found in the AUTHORS file in the root of the source tree.
  */
 
@@ skipping 12 matching lines @@
 namespace webrtc {
 
 namespace {
 
 const size_t kErbResolution = 2;
 const int kWindowSizeMs = 16;
 const int kChunkSizeMs = 10;  // Size provided by APM.
 const float kClipFreqKhz = 0.2f;
 const float kKbdAlpha = 1.5f;
 const float kLambdaBot = -1.0f;      // Extreme values in bisection
-const float kLambdaTop = -10e-18f;  // search for lamda.
+const float kLambdaTop = -1e-5f;      // search for lamda.
 const float kVoiceProbabilityThreshold = 0.02f;
 // Number of chunks after voice activity which is still considered speech.
 const size_t kSpeechOffsetDelay = 80;
 const float kDecayRate = 0.98f;              // Power estimation decay rate.
 const float kMaxRelativeGainChange = 0.04f;  // Maximum relative change in gain.
 const float kRho = 0.0004f;  // Default production and interpretation SNR.
 
 // Returns dot product of vectors |a| and |b| with size |length|.
 float DotProduct(const float* a, const float* b, size_t length) {
   float ret = 0.f;
@@ skipping 113 matching lines @@
                 filtered_noise_pow_.get());
   SolveForGainsGivenLambda(kLambdaTop, start_freq_, gains_eq_.get());
   const float power_target =
       std::accumulate(&clear_power[0], &clear_power[0] + freqs_, 0.f);
   const float power_top =
       DotProduct(gains_eq_.get(), filtered_clear_pow_.get(), bank_size_);
   SolveForGainsGivenLambda(kLambdaBot, start_freq_, gains_eq_.get());
   const float power_bot =
       DotProduct(gains_eq_.get(), filtered_clear_pow_.get(), bank_size_);
   if (power_target >= power_bot && power_target <= power_top) {
-    SolveForLambda(power_target, power_bot, power_top);
+    SolveForLambda(power_target);
     UpdateErbGains();
   }  // Else experiencing power underflow, so do nothing.
   gain_applier_.Apply(in_block, out_block);
 }
 
-void IntelligibilityEnhancer::SolveForLambda(float power_target,
-                                             float power_bot,
-                                             float power_top) {
+void IntelligibilityEnhancer::SolveForLambda(float power_target) {
   const float kConvergeThresh = 0.001f;  // TODO(ekmeyerson): Find best values
   const int kMaxIters = 100;             // for these, based on experiments.
 
   const float reciprocal_power_target =
       1.f / (power_target + std::numeric_limits<float>::epsilon());
   float lambda_bot = kLambdaBot;
   float lambda_top = kLambdaTop;
   float power_ratio = 2.f;  // Ratio of achieved power to target power.
   int iters = 0;
   while (std::fabs(power_ratio - 1.f) > kConvergeThresh && iters <= kMaxIters) {
-    const float lambda = lambda_bot + (lambda_top - lambda_bot) / 2.f;
+    const float lambda = (lambda_bot + lambda_top) / 2.f;
     SolveForGainsGivenLambda(lambda, start_freq_, gains_eq_.get());
     const float power =
         DotProduct(gains_eq_.get(), filtered_clear_pow_.get(), bank_size_);
     if (power < power_target) {
       lambda_bot = lambda;
     } else {
       lambda_top = lambda;
     }
     power_ratio = std::fabs(power * reciprocal_power_target);
     ++iters;
@@ skipping 82 matching lines @@
     for (size_t j = 0; j < bank_size_; ++j) {
       filter_bank[j][i] /= sum;
     }
   }
   return filter_bank;
 }
 
 void IntelligibilityEnhancer::SolveForGainsGivenLambda(float lambda,
                                                        size_t start_freq,
                                                        float* sols) {
-  bool quadratic = (kRho < 1.f);
+  const float kMinPower = 1e-5f;
+
   const float* pow_x0 = filtered_clear_pow_.get();
   const float* pow_n0 = filtered_noise_pow_.get();
 
   for (size_t n = 0; n < start_freq; ++n) {
     sols[n] = 1.f;
   }
 
   // Analytic solution for optimal gains. See paper for derivation.
-  for (size_t n = start_freq - 1; n < bank_size_; ++n) {
-    float alpha0, beta0, gamma0;
-    gamma0 = 0.5f * kRho * pow_x0[n] * pow_n0[n] +
-             lambda * pow_x0[n] * pow_n0[n] * pow_n0[n];
-    beta0 = lambda * pow_x0[n] * (2 - kRho) * pow_x0[n] * pow_n0[n];
-    if (quadratic) {
-      alpha0 = lambda * pow_x0[n] * (1 - kRho) * pow_x0[n] * pow_x0[n];
-      sols[n] =
-          (-beta0 - sqrtf(beta0 * beta0 - 4 * alpha0 * gamma0)) /
-          (2 * alpha0 + std::numeric_limits<float>::epsilon());
+  for (size_t n = start_freq; n < bank_size_; ++n) {
+    if (pow_x0[n] < kMinPower || pow_n0[n] < kMinPower) {
+      sols[n] = 1.f;
     } else {
-      sols[n] = -gamma0 / beta0;
+      const float gamma0 = 0.5f * kRho * pow_x0[n] * pow_n0[n] +
+                           lambda * pow_x0[n] * pow_n0[n] * pow_n0[n];
+      const float beta0 =
+          lambda * pow_x0[n] * (2.f - kRho) * pow_x0[n] * pow_n0[n];
+      const float alpha0 =
+          lambda * pow_x0[n] * (1.f - kRho) * pow_x0[n] * pow_x0[n];
+      RTC_DCHECK_LT(alpha0, 0.f);
+      // The quadratic equation should always have real roots, but to guard
+      // against numerical errors we limit it to a minimum of zero.
+      sols[n] = std::max(
+          0.f, (-beta0 - std::sqrt(std::max(
+                             0.f, beta0 * beta0 - 4.f * alpha0 * gamma0))) /
+                   (2.f * alpha0));
     }
-    sols[n] = fmax(0, sols[n]);
   }
 }
 
 bool IntelligibilityEnhancer::IsSpeech(const float* audio) {
   FloatToS16(audio, chunk_length_, &audio_s16_[0]);
   vad_.ProcessChunk(&audio_s16_[0], chunk_length_, sample_rate_hz_);
   if (vad_.last_voice_probability() > kVoiceProbabilityThreshold) {
     chunks_since_voice_ = 0;
   } else if (chunks_since_voice_ < kSpeechOffsetDelay) {
     ++chunks_since_voice_;
   }
   return chunks_since_voice_ < kSpeechOffsetDelay;
 }
 
 }  // namespace webrtc