Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 1 | /* |
| 2 | * Copyrightm (C) 2010 The Android Open Source Project |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | */ |
| 16 | |
| 17 | #include <stdio.h> |
Chung-yih Wang | fb116fb | 2010-10-06 16:46:59 +0800 | [diff] [blame] | 18 | #include <string.h> |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 19 | #include <stdint.h> |
Marco Nelissen | fd1e4ad | 2010-10-06 12:23:02 -0700 | [diff] [blame] | 20 | #include <string.h> |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 21 | #include <math.h> |
| 22 | |
| 23 | #define LOG_TAG "Echo" |
| 24 | #include <utils/Log.h> |
| 25 | |
| 26 | #include "EchoSuppressor.h" |
| 27 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 28 | // It is very difficult to do echo cancellation at this level due to the lack of |
| 29 | // the timing information of the samples being played and recorded. Therefore, |
| 30 | // for the first release only echo suppression is implemented. |
| 31 | |
| 32 | // The algorithm is derived from the "previous works" summarized in |
| 33 | // A new class of doubletalk detectors based on cross-correlation, |
| 34 | // J Benesty, DR Morgan, JH Cho, IEEE Trans. on Speech and Audio Processing. |
| 35 | // The method proposed in that paper is not used because of its high complexity. |
| 36 | |
| 37 | // It is well known that cross-correlation can be computed using convolution, |
| 38 | // but unfortunately not every mobile processor has a (fast enough) FPU. Thus |
| 39 | // we use integer arithmetic as much as possible and do lots of bookkeeping. |
| 40 | // Again, parameters and thresholds are chosen by experiments. |
| 41 | |
| 42 | EchoSuppressor::EchoSuppressor(int sampleCount, int tailLength) |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 43 | { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 44 | tailLength += sampleCount * 4; |
| 45 | |
| 46 | int shift = 0; |
| 47 | while ((sampleCount >> shift) > 1 && (tailLength >> shift) > 256) { |
| 48 | ++shift; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 49 | } |
| 50 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 51 | mShift = shift + 4; |
| 52 | mScale = 1 << shift; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 53 | mSampleCount = sampleCount; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 54 | mWindowSize = sampleCount >> shift; |
| 55 | mTailLength = tailLength >> shift; |
| 56 | mRecordLength = tailLength * 2 / sampleCount; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 57 | mRecordOffset = 0; |
| 58 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 59 | mXs = new uint16_t[mTailLength + mWindowSize]; |
| 60 | memset(mXs, 0, sizeof(*mXs) * (mTailLength + mWindowSize)); |
| 61 | mXSums = new uint32_t[mTailLength]; |
| 62 | memset(mXSums, 0, sizeof(*mXSums) * mTailLength); |
| 63 | mX2Sums = new uint32_t[mTailLength]; |
| 64 | memset(mX2Sums, 0, sizeof(*mX2Sums) * mTailLength); |
| 65 | mXRecords = new uint16_t[mRecordLength * mWindowSize]; |
| 66 | memset(mXRecords, 0, sizeof(*mXRecords) * mRecordLength * mWindowSize); |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 67 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 68 | mYSum = 0; |
| 69 | mY2Sum = 0; |
| 70 | mYRecords = new uint32_t[mRecordLength]; |
| 71 | memset(mYRecords, 0, sizeof(*mYRecords) * mRecordLength); |
| 72 | mY2Records = new uint32_t[mRecordLength]; |
| 73 | memset(mY2Records, 0, sizeof(*mY2Records) * mRecordLength); |
| 74 | |
| 75 | mXYSums = new uint32_t[mTailLength]; |
| 76 | memset(mXYSums, 0, sizeof(*mXYSums) * mTailLength); |
| 77 | mXYRecords = new uint32_t[mRecordLength * mTailLength]; |
| 78 | memset(mXYRecords, 0, sizeof(*mXYRecords) * mRecordLength * mTailLength); |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 79 | |
| 80 | mLastX = 0; |
| 81 | mLastY = 0; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 82 | mWeight = 1.0f / (mRecordLength * mWindowSize); |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 83 | } |
| 84 | |
| 85 | EchoSuppressor::~EchoSuppressor() |
| 86 | { |
| 87 | delete [] mXs; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 88 | delete [] mXSums; |
| 89 | delete [] mX2Sums; |
| 90 | delete [] mXRecords; |
| 91 | delete [] mYRecords; |
| 92 | delete [] mY2Records; |
| 93 | delete [] mXYSums; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 94 | delete [] mXYRecords; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 95 | } |
| 96 | |
| 97 | void EchoSuppressor::run(int16_t *playbacked, int16_t *recorded) |
| 98 | { |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 99 | // Update Xs. |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 100 | for (int i = mTailLength - 1; i >= 0; --i) { |
| 101 | mXs[i + mWindowSize] = mXs[i]; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 102 | } |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 103 | for (int i = mWindowSize - 1, j = 0; i >= 0; --i, j += mScale) { |
| 104 | uint32_t sum = 0; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 105 | for (int k = 0; k < mScale; ++k) { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 106 | int32_t x = playbacked[j + k] << 15; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 107 | mLastX += x; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 108 | sum += ((mLastX >= 0) ? mLastX : -mLastX) >> 15; |
| 109 | mLastX -= (mLastX >> 10) + x; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 110 | } |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 111 | mXs[i] = sum >> mShift; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 112 | } |
| 113 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 114 | // Update XSums, X2Sums, and XRecords. |
| 115 | for (int i = mTailLength - mWindowSize - 1; i >= 0; --i) { |
| 116 | mXSums[i + mWindowSize] = mXSums[i]; |
| 117 | mX2Sums[i + mWindowSize] = mX2Sums[i]; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 118 | } |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 119 | uint16_t *xRecords = &mXRecords[mRecordOffset * mWindowSize]; |
| 120 | for (int i = mWindowSize - 1; i >= 0; --i) { |
| 121 | uint16_t x = mXs[i]; |
| 122 | mXSums[i] = mXSums[i + 1] + x - xRecords[i]; |
| 123 | mX2Sums[i] = mX2Sums[i + 1] + x * x - xRecords[i] * xRecords[i]; |
| 124 | xRecords[i] = x; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 125 | } |
| 126 | |
| 127 | // Compute Ys. |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 128 | uint16_t ys[mWindowSize]; |
| 129 | for (int i = mWindowSize - 1, j = 0; i >= 0; --i, j += mScale) { |
| 130 | uint32_t sum = 0; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 131 | for (int k = 0; k < mScale; ++k) { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 132 | int32_t y = recorded[j + k] << 15; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 133 | mLastY += y; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 134 | sum += ((mLastY >= 0) ? mLastY : -mLastY) >> 15; |
| 135 | mLastY -= (mLastY >> 10) + y; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 136 | } |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 137 | ys[i] = sum >> mShift; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 138 | } |
| 139 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 140 | // Update YSum, Y2Sum, YRecords, and Y2Records. |
| 141 | uint32_t ySum = 0; |
| 142 | uint32_t y2Sum = 0; |
| 143 | for (int i = mWindowSize - 1; i >= 0; --i) { |
| 144 | ySum += ys[i]; |
| 145 | y2Sum += ys[i] * ys[i]; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 146 | } |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 147 | mYSum += ySum - mYRecords[mRecordOffset]; |
| 148 | mY2Sum += y2Sum - mY2Records[mRecordOffset]; |
| 149 | mYRecords[mRecordOffset] = ySum; |
| 150 | mY2Records[mRecordOffset] = y2Sum; |
| 151 | |
| 152 | // Update XYSums and XYRecords. |
| 153 | uint32_t *xyRecords = &mXYRecords[mRecordOffset * mTailLength]; |
| 154 | for (int i = mTailLength - 1; i >= 0; --i) { |
| 155 | uint32_t xySum = 0; |
| 156 | for (int j = mWindowSize - 1; j >= 0; --j) { |
| 157 | xySum += mXs[i + j] * ys[j]; |
| 158 | } |
| 159 | mXYSums[i] += xySum - xyRecords[i]; |
| 160 | xyRecords[i] = xySum; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 161 | } |
| 162 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 163 | // Compute correlations. |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 164 | int latency = 0; |
Chia-chi Yeh | 0c7d306 | 2010-10-27 17:04:46 +0800 | [diff] [blame] | 165 | float corr2 = 0.0f; |
| 166 | float varX = 0.0f; |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 167 | float varY = mY2Sum - mWeight * mYSum * mYSum; |
| 168 | for (int i = mTailLength - 1; i >= 0; --i) { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 169 | float cov = mXYSums[i] - mWeight * mXSums[i] * mYSum; |
Chia-chi Yeh | 0c7d306 | 2010-10-27 17:04:46 +0800 | [diff] [blame] | 170 | if (cov > 0.0f) { |
| 171 | float varXi = mX2Sums[i] - mWeight * mXSums[i] * mXSums[i]; |
| 172 | float corr2i = cov * cov / (varXi * varY + 1); |
| 173 | if (corr2i > corr2) { |
| 174 | varX = varXi; |
| 175 | corr2 = corr2i; |
| 176 | latency = i; |
| 177 | } |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 178 | } |
| 179 | } |
Chia-chi Yeh | 0c7d306 | 2010-10-27 17:04:46 +0800 | [diff] [blame] | 180 | //LOGI("corr^2 %.5f, var %8.0f %8.0f, latency %d", corr2, varX, varY, |
| 181 | // latency * mScale); |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 182 | |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 183 | // Do echo suppression. |
Chia-chi Yeh | 0c7d306 | 2010-10-27 17:04:46 +0800 | [diff] [blame] | 184 | if (corr2 > 0.1f && varX > 10000.0f) { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 185 | int factor = (corr2 > 1.0f) ? 0 : (1.0f - sqrtf(corr2)) * 4096; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 186 | for (int i = 0; i < mSampleCount; ++i) { |
Chia-chi Yeh | 8a68b52 | 2010-10-21 23:39:35 +0800 | [diff] [blame] | 187 | recorded[i] = recorded[i] * factor >> 16; |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 188 | } |
| 189 | } |
Chia-chi Yeh | a8a1009 | 2010-10-05 01:17:13 +0800 | [diff] [blame] | 190 | |
| 191 | // Increase RecordOffset. |
| 192 | ++mRecordOffset; |
| 193 | if (mRecordOffset == mRecordLength) { |
| 194 | mRecordOffset = 0; |
| 195 | } |
| 196 | } |