diff --git a/vibration_convert/core/algorithm/conversion/include/conversion_filter.h b/vibration_convert/core/algorithm/conversion/include/conversion_filter.h index 33db58889809fbe280f11dcdf04e66858b03f083..255aa4cf2cd597ea99b863baaec6db45affb74b9 100644 --- a/vibration_convert/core/algorithm/conversion/include/conversion_filter.h +++ b/vibration_convert/core/algorithm/conversion/include/conversion_filter.h @@ -120,8 +120,8 @@ private: double cutoff_ { 0.0 }; double resonance_ { 0.0 }; double output_ { 0.0 }; - double inputs_[ARRAY_SIZE] { 0.0 }; - double outputs_[ARRAY_SIZE] { 0.0 }; + double sampledData_[ARRAY_SIZE] { 0.0 }; + double filteredData_[ARRAY_SIZE] { 0.0 }; double speed_ { 0.0 }; double pos_ { 0.0 }; double pole_ { 0.0 }; diff --git a/vibration_convert/core/algorithm/conversion/include/conversion_mfcc.h b/vibration_convert/core/algorithm/conversion/include/conversion_mfcc.h index 5a3e18fb9daa3f11d0e7b9dedb9097e9e407f6b7..b7fef1129ca27158c52050d7b7cb538e7b6ee737 100644 --- a/vibration_convert/core/algorithm/conversion/include/conversion_mfcc.h +++ b/vibration_convert/core/algorithm/conversion/include/conversion_mfcc.h @@ -88,8 +88,8 @@ public: int32_t FiltersMel(int32_t nFft, MfccInputPara para, size_t &frmCount, std::vector &melBasis); private: - void HandleDiscreteCosineTransform(std::vector &mfccs); - int32_t MelFilterAndLogSquare(const std::vector &powerSpectrum); + int32_t HandleDiscreteCosineTransform(); + int32_t HandleMelFilterAndLogSquare(const std::vector &powerSpectrum); int32_t CalcMelFilterBank(double sampleRate); int32_t CreateDCTCoeffs(); int32_t SetMelFilters(uint32_t idx, double binFreq, double prevFreq, double thisFreq, double nextFreq); diff --git a/vibration_convert/core/algorithm/conversion/src/conversion_filter.cpp b/vibration_convert/core/algorithm/conversion/src/conversion_filter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f3915b07416e1211d5e8c72f073c3e52a2d2bba0 --- /dev/null +++ b/vibration_convert/core/algorithm/conversion/src/conversion_filter.cpp @@ -0,0 +1,162 @@ +/* + * Copyright (c) 2023 Huawei Device Co., Ltd. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "conversion_filter.h" + +#include "sensor_log.h" + +namespace OHOS { +namespace Sensors { +namespace { +constexpr OHOS::HiviewDFX::HiLogLabel LABEL = { LOG_CORE, SENSOR_LOG_DOMAIN, "ConversionFilter" }; +constexpr double CUT_OFF_MIN { 10.0 }; +constexpr double VALIDE_RESONANCE_MAX { 0.999999 }; +constexpr double AMOUNT_RESONANCE_MIN { 1.0 }; +constexpr double BAND_PASS_COEF { 4.0 }; +} // namespace + +// I particularly like these. cutoff between 0 and 1 +double ConversionFilter::FilterLowPass(double input, double cutoff) +{ + output_ = filteredData_[0] + cutoff * (input - filteredData_[0]); + filteredData_[0] = output_; + return output_; +} + +// as above +double ConversionFilter::FilterHighPass(double input, double cutoff) +{ + output_ = input - (filteredData_[0] + cutoff * (input - filteredData_[0])); + filteredData_[0] = output_; + return output_; +} + +// awesome. cuttof is freq in hz. res is between 1 and whatever. Watch out! +double ConversionFilter::FilterLowResonant(double input, double cutoff, double resonance) +{ + cutoff_ = cutoff; + if (IsLessNotEqual(cutoff_, CUT_OFF_MIN)) { + cutoff_ = CUT_OFF_MIN; + } + if (IsGreatNotEqual(cutoff_, static_cast(sampleRate_))) { + cutoff_ = static_cast(sampleRate_); + } + if (IsLessNotEqual(resonance, AMOUNT_RESONANCE_MIN)) { + resonance = AMOUNT_RESONANCE_MIN; + } + if (sampleRate_ == 0) { + SEN_HILOGE("sampleRate_ should not be 0"); + return 0.0; + } + pole_ = cos(M_PI * 2 * cutoff_ / sampleRate_); + filterCoefficient_ = 2 - 2 * pole_; + if (IsEqual(pole_, 1.0)) { + SEN_HILOGE("pole_ should not be 1.0"); + return 0.0; + } + double r = (sqrt(2.0) * sqrt(-pow((pole_ - 1.0), 3.0)) + resonance * (pole_ - 1.0)) / (resonance * (pole_ - 1.0)); + speed_ = speed_ + (input - pos_) * filterCoefficient_; + pos_ = pos_ + speed_; + speed_ = speed_ * r; + output_ = pos_; + return output_; +} + +// working hires filter +double ConversionFilter::FilterHighResonant(double input, double cutoff, double resonance) +{ + cutoff_ = cutoff; + if (IsLessNotEqual(cutoff_, CUT_OFF_MIN)) { + cutoff_ = CUT_OFF_MIN; + } + if (IsGreatNotEqual(cutoff_, static_cast(sampleRate_))) { + cutoff_ = sampleRate_; + } + if (IsLessNotEqual(resonance, AMOUNT_RESONANCE_MIN)) { + resonance = AMOUNT_RESONANCE_MIN; + } + if (sampleRate_ == 0) { + SEN_HILOGE("sampleRate_ should not be 0"); + return 0.0; + } + pole_ = cos(M_PI * 2 * cutoff_ / sampleRate_); + filterCoefficient_ = 2 - 2 * pole_; + if (IsEqual(pole_, 1.0)) { + SEN_HILOGE("pole_ should not be 1.0"); + return 0.0; + } + double r = (sqrt(2.0) * sqrt(-pow((pole_ - 1.0), F_THREE)) + resonance * (pole_ - 1.0)) / (resonance * (pole_ - 1.0)); + speed_ = speed_ + (input - pos_) * filterCoefficient_; + pos_ = pos_ + speed_; + speed_ = speed_ * r; + output_ = input - pos_; + return output_; +} + +// This works a bit. Needs attention. +double ConversionFilter::FilterBandPass(double input, double cutoff, double resonance) +{ + cutoff_ = cutoff; + if (IsGreatNotEqual(cutoff_, sampleRate_ * F_HALF)) { + cutoff_ = (sampleRate_ * F_HALF); + } + if (IsGreatNotEqual(resonance, VALIDE_RESONANCE_MAX)) { + resonance = VALIDE_RESONANCE_MAX; + } + if (sampleRate_ == 0) { + SEN_HILOGE("sampleRate_ should not be 0"); + return 0.0; + } + pole_ = cos(M_PI * 2 * cutoff_ / sampleRate_); + sampledData_[0] = (1 - resonance) * (sqrt(resonance * (resonance - BAND_PASS_COEF * pow(pole_, 2.0) + 2.0) + 1)); + sampledData_[1] = 2 * pole_ * resonance; + sampledData_[2] = pow(-resonance, 2); + + output_ = sampledData_[0] * input + sampledData_[1] * filteredData_[1] + sampledData_[2] * filteredData_[2]; + filteredData_[2] = filteredData_[1]; + filteredData_[1] = output_; + return output_; +} + +double ConversionFilter::HandleResonant(double input, double cutoff, double resonance, FilterMethod filterMethod) +{ + double output = 0.0; + switch (filterMethod) { + case LOW_RESONANT_FILTER: { + output = FilterLowResonant(input, cutoff, resonance); + break; + } + case HIGH_RESONANT_FILTER: { + output = FilterHighResonant(input, cutoff, resonance); + break; + } + case BAND_PASS_FILTER: { + output = FilterBandPass(input, cutoff, resonance); + break; + } + default: { + SEN_HILOGE("Unsupported filter Method"); + return output; + } + } + return output; +} + +double ConversionFilter::HandleHighPassOrLowPass(double input, double cutoff, bool isHighPass) +{ + return isHighPass ? FilterHighPass(input, cutoff) : FilterLowPass(input, cutoff); +} +} // namespace Sensors +} // namespace OHOS \ No newline at end of file diff --git a/vibration_convert/core/algorithm/conversion/src/conversion_mfcc.cpp b/vibration_convert/core/algorithm/conversion/src/conversion_mfcc.cpp new file mode 100644 index 0000000000000000000000000000000000000000..72691f40bcaa8245b5480bf4225b5bfc92b27e78 --- /dev/null +++ b/vibration_convert/core/algorithm/conversion/src/conversion_mfcc.cpp @@ -0,0 +1,269 @@ +/* + * Copyright (c) 2023 Huawei Device Co., Ltd. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "conversion_mfcc.h" + +#include "audio_utils.h" +#include "sensor_log.h" +#include "sensors_errors.h" +#include "utils.h" + +namespace OHOS { +namespace Sensors { +namespace { +constexpr OHOS::HiviewDFX::HiLogLabel LABEL = { LOG_CORE, SENSOR_LOG_DOMAIN, "ConversionMfcc" }; +constexpr double BANDS_MIN_THRESHOLD { 0.000001 }; +constexpr uint32_t MEL_FILTERS_OR_COEFFS_MAX { 4096 * 4096 }; +} // namespace + +int32_t ConversionMfcc::HandleMelFilterAndLogSquare(const std::vector &powerSpectrum) +{ + size_t powerSpectrumSize = powerSpectrum.size(); + if ((powerSpectrumSize == 0) || (powerSpectrumSize >= numBins_)) { + SEN_HILOGE("Invalid parameter"); + return Sensors::PARAMETER_ERROR; + } + for (uint32_t i = 0; i < numFilters_; ++i) { + melBands_[i] = 0; + for (uint32_t bin = 0; bin < numBins_; ++bin) { + uint32_t idx = i + (bin * numFilters_); + melBands_[i] += (melFilters_[idx] * powerSpectrum[bin]); + } + } + for (uint32_t i = 0; i < numFilters_; ++i) { + // log the square + melBands_[i] = (melBands_[i] > BANDS_MIN_THRESHOLD) ? log(melBands_[i] * melBands_[i]) : 0; + } + return Sensors::SUCCESS; +} + +int32_t ConversionMfcc::Init(uint32_t numBins, uint32_t numCoeffs, const MfccInputPara ¶) +{ + if ((numBins <= 0) || (numCoeffs <= 0) || para.nMels > MEL_FILTERS_OR_COEFFS_MAX || + numCoeffs > MEL_FILTERS_OR_COEFFS_MAX) { + SEN_HILOGE("Invalid parameter, para.nMels:%{public}d, numCoeffs:%{public}d", para.nMels, numCoeffs); + return Sensors::PARAMETER_ERROR; + } + numFilters_ = para.nMels; + numCoeffs_ = numCoeffs; + minFreq_ = para.minFreq; + maxFreq_ = para.maxFreq; + sampleRate_ = para.sampleRate; + numBins_ = numBins; + melBands_.resize(numFilters_, 0.0); + coeffs_.resize(numCoeffs, 0.0); + // create new matrix + dctMatrix_.resize(numCoeffs * numFilters_, 0.0); + // now generate the coefficients for the mag spectrum + melFilters_.resize(numFilters_ * numBins_, 0.0); + if (CalcMelFilterBank(sampleRate_) != Sensors::SUCCESS) { + SEN_HILOGE("CalcMelFilterBank failed"); + return Sensors::ERROR; + } + if (CreateDCTCoeffs() != Sensors::SUCCESS) { + SEN_HILOGE("CreateDCTCoeffs failed"); + return Sensors::ERROR; + } + return Sensors::SUCCESS; +} + +std::vector ConversionMfcc::Mfcc(const std::vector &powerSpectrum) +{ + int32_t ret = HandleMelFilterAndLogSquare(powerSpectrum); + if (ret != Sensors::SUCCESS) { + SEN_HILOGE("HandleMelFilterAndLogSquare failed"); + return {}; + } + if (HandleDiscreteCosineTransform() != Sensors::SUCCESS) { + SEN_HILOGE("HandleDiscreteCosineTransform failed"); + return {}; + } + return coeffs_; +} + +int32_t ConversionMfcc::HandleDiscreteCosineTransform() +{ + if (numCoeffs_ == 0) { + SEN_HILOGE("numCoeffs_ should not be 0"); + return Sensors::ERROR; + } + for (uint32_t i = 0; i < numCoeffs_; i++) { + coeffs_[i] = 0; + for (uint32_t j = 0; j < numFilters_; j++) { + uint32_t idx = i + (j * numCoeffs_); + coeffs_[i] += (dctMatrix_[idx] * melBands_[j]); + } + coeffs_[i] /= numCoeffs_; + } + return Sensors::SUCCESS; +} + +int32_t ConversionMfcc::SetMelFilters(uint32_t idx, double binFreq, double prevFreq, double thisFreq, double nextFreq) +{ + if (nextFreq == 0) { + SEN_HILOGE("Invalid parameter"); + return Sensors::ERROR; + } + if (IsEqual(nextFreq, prevFreq)) { + SEN_HILOGE("The divisor cannot be 0"); + return Sensors::ERROR; + } + melFilters_[idx] = 0; + double height = 2.0 / (nextFreq - prevFreq); + if (IsLessOrEqual(binFreq, thisFreq)) { + if (IsEqual(thisFreq, prevFreq)) { + SEN_HILOGE("The divisor cannot be 0"); + return Sensors::ERROR; + } + melFilters_[idx] = (binFreq - prevFreq) * (height / (thisFreq - prevFreq)); + } else { + if (IsEqual(nextFreq, thisFreq)) { + SEN_HILOGE("The divisor cannot be 0"); + return Sensors::ERROR; + } + melFilters_[idx] = height + ((binFreq - thisFreq) * (-height / (nextFreq - thisFreq))); + } + return Sensors::SUCCESS; +} + +int32_t ConversionMfcc::CalcMelFilterBank(double sampleRate) +{ + if (numBins_ == 0) { + SEN_HILOGE("numBins_ should not be 0"); + return Sensors::PARAMETER_ERROR; + } + double nyquist = sampleRate / 2; + if (IsGreatNotEqual(maxFreq_, nyquist)) { + maxFreq_ = nyquist; + } + double maxMel = OHOS::Sensors::ConvertSlaneyMel(maxFreq_); + double minMel = OHOS::Sensors::ConvertSlaneyMel(minFreq_); + double stepMel = (maxMel - minMel) / (numFilters_ + 1); + std::vector filterHzPos(numFilters_ + 2); + double nextMel = minMel; + for (uint32_t i = 0; i < (numFilters_ + 2); ++i) { + filterHzPos[i] = OHOS::Sensors::ConvertSlaneyHz(nextMel); + nextMel += stepMel; + } + std::vector binFs(numBins_); + double stepHz = sampleRate / (2 * numBins_); + for (uint32_t bin = 0; bin < numBins_; ++bin) { + binFs[bin] = stepHz * bin; + } + for (uint32_t i = 0; i < numFilters_; ++i) { + for (uint32_t j = 0; j < numBins_; ++j) { + uint32_t idx = i + (j * numFilters_); + if (SetMelFilters(idx, binFs[j], filterHzPos[i], filterHzPos[i + 1], + filterHzPos[i + 2]) != Sensors::SUCCESS) { + SEN_HILOGE("SetMelFilters failed"); + return Sensors::ERROR; + } + } + } + return Sensors::SUCCESS; +} + +std::vector ConversionMfcc::GetMelFilterBank() const +{ + std::vector melFilters; + uint32_t num = numFilters_ * numBins_; + for (uint32_t i = 0; i < num; i++) { + melFilters.emplace_back(melFilters_[i]); + } + return melFilters; +} + +int32_t ConversionMfcc::CreateDCTCoeffs() +{ + if (numFilters_ == 0) { + SEN_HILOGE("numFilters_ should not be 0"); + return Sensors::ERROR; + } + double k = M_PI / numFilters_; + double w1 = 1.0 / (sqrt(numFilters_)); + double w2 = sqrt(2.0 / numFilters_); + // generate dct matrix + for (uint32_t i = 0; i < numCoeffs_; i++) { + for (uint32_t j = 0; j < numFilters_; j++) { + uint32_t idx = i + (j * numCoeffs_); + if (i == 0) { + dctMatrix_[idx] = w1 * cos(k * (i + 1) * (j + F_HALF)); + } else { + dctMatrix_[idx] = w2 * cos(k * (i + 1) * (j + F_HALF)); + } + } + } + return Sensors::SUCCESS; +} + +int32_t ConversionMfcc::FiltersMel(int32_t nFft, MfccInputPara para, + size_t &frmCount, std::vector &melBasis) +{ + if (nFft == 0) { + SEN_HILOGE("nFft should not be 0"); + return Sensors::PARAMETER_ERROR; + } + int32_t sr = para.sampleRate; + double fMax = para.maxFreq; + if (IsLessNotEqual(fMax, EPS_MIN)) { + fMax = sr / 2.0; + } + size_t nMels = static_cast(para.nMels); + double fMin = para.minFreq; + frmCount = nFft / 2; + std::vector basis(nMels * frmCount, 0.0); + // generate mel frequencies. + double minMel = OHOS::Sensors::ConvertHtkMel(fMin); + double maxMel = OHOS::Sensors::ConvertHtkMel(fMax); + std::vector filterHzPos(nMels + 2); + double stepMel = (maxMel - minMel) / (nMels + 1); + double stepHz = static_cast(sr) / nFft; + + double nextMel = minMel; + for (size_t i = 0; i < (nMels + 2); i++) { + filterHzPos[i] = OHOS::Sensors::ConvertHtkHz(nextMel); + nextMel += stepMel; + } + std::vector binFs(frmCount); + for (int32_t j = 0; j < frmCount; j++) { + binFs[j] = stepHz * j; + } + int32_t index = 0; + for (size_t i = 2; i < filterHzPos.size(); i++) { + double prevFreq = filterHzPos[i - 2]; + double thisFreq = filterHzPos[i - 1]; + double nextFreq = filterHzPos[i]; + if (IsEqual(thisFreq, prevFreq) || IsEqual(nextFreq, thisFreq) || IsEqual(nextFreq, prevFreq)) { + SEN_HILOGE("The divisor cannot be 0"); + return Sensors::ERROR; + } + for (int32_t j = 0; j < frmCount; j++) { + double binFreq = binFs[j]; + double lower = (binFreq - prevFreq) / (thisFreq - prevFreq); + double upper = (nextFreq - binFreq) / (nextFreq - thisFreq); + double min = IsLessNotEqual(lower, upper) ? lower : upper; + min = IsGreatNotEqual(min, 0.0) ? min : 0.0; + basis[index++] = min * 2.0 / (nextFreq - prevFreq); + } + } + melBasis = OHOS::Sensors::TransposeMatrix(nMels, basis); + if (melBasis.empty()) { + SEN_HILOGE("melBasis is empty"); + return Sensors::ERROR; + } + return Sensors::SUCCESS; +} +} // namespace Sensors +} // namespace OHOS \ No newline at end of file diff --git a/vibration_convert/core/utils/include/utils.h b/vibration_convert/core/utils/include/utils.h index bc9159ef534dcf53b5aeafa5f2ac089b239e37cf..be01a4a991ef1af6db51897201e17676889507ea 100644 --- a/vibration_convert/core/utils/include/utils.h +++ b/vibration_convert/core/utils/include/utils.h @@ -55,6 +55,12 @@ enum WindowType { WND_TYPE_HANNING = 3, }; +enum FilterMethod{ + LOW_RESONANT_FILTER = 1, + HIGH_RESONANT_FILTER = 2, + BAND_PASS_FILTER = 3, +}; + bool IsPowerOfTwo(uint32_t x); uint32_t ObtainNumberOfBits(uint32_t powerOfTwo); uint32_t ReverseBits(uint32_t index, uint32_t numBits); @@ -146,114 +152,34 @@ inline double ConvertSlaneyHz(double mel) return HZ_COEF * (pow(LOG_BASE, mel / MEL_COEF) - 1.0); } -inline bool IsLessOrEqual(double left, double right, const double epsilon) -{ - return (left - right) < epsilon; -} - template -constexpr bool IsLessOrEqual(const T& left, const T& right); - -template<> -inline bool IsLessOrEqual(const float& left, const float& right) +inline bool IsLessOrEqual(const T& left, const T& right) { - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsLessOrEqual(left, right, epsilon); -} - -template<> -inline bool IsLessOrEqual(const double& left, const double& right) -{ - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsLessOrEqual(left, right, epsilon); -} - -inline bool IsGreatOrEqual(double left, double right, const double epsilon) -{ - return (left - right) > epsilon; + return (left - right) < std::numeric_limits::epsilon(); } template -constexpr bool IsGreatOrEqual(const T& left, const T& right); - -template<> -inline bool IsGreatOrEqual(const float& left, const float& right) -{ - constexpr double epsilon = -std::numeric_limits::epsilon(); - return IsGreatOrEqual(left, right, epsilon); -} - -template<> -inline bool IsGreatOrEqual(const double& left, const double& right) -{ - constexpr double epsilon = -std::numeric_limits::epsilon(); - return IsGreatOrEqual(left, right, epsilon); -} - -inline bool IsLessNotEqual(double left, double right, const double epsilon) +inline bool IsGreatOrEqual(const T& left, const T& right) { - return (left - right) < epsilon; + return (left - right) > -std::numeric_limits::epsilon(); } template -constexpr bool IsLessNotEqual(const T& left, const T& right); - -template<> -inline bool IsLessNotEqual(const float& left, const float& right) +inline bool IsLessNotEqual(const T& left, const T& right) { - constexpr double epsilon = -std::numeric_limits::epsilon(); - return IsLessNotEqual(left, right, epsilon); -} - -template<> -inline bool IsLessNotEqual(const double& left, const double& right) -{ - constexpr double epsilon = -std::numeric_limits::epsilon(); - return IsLessNotEqual(left, right, epsilon); -} - -inline bool IsGreatNotEqual(double left, double right, const double epsilon) -{ - return (left - right) > epsilon; + return (left - right) < -std::numeric_limits::epsilon(); } template -constexpr bool IsGreatNotEqual(const T& left, const T& right); - -template<> -inline bool IsGreatNotEqual(const float& left, const float& right) -{ - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsGreatNotEqual(left, right, epsilon); -} - -template<> -inline bool IsGreatNotEqual(const double& left, const double& right) -{ - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsGreatNotEqual(left, right, epsilon); -} - -inline bool IsEqual(double left, double right, const double epsilon) +inline bool IsGreatNotEqual(const T& left, const T& right) { - return (std::abs(left - right) <= epsilon); + return (left - right) > std::numeric_limits::epsilon(); } template -constexpr bool IsEqual(const T& left, const T& right); - -template<> -inline bool IsEqual(const float& left, const float& right) -{ - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsEqual(left, right, epsilon); -} - -template<> -inline bool IsEqual(const double& left, const double& right) +inline bool IsEqual(const T& left, const T& right) { - constexpr double epsilon = std::numeric_limits::epsilon(); - return IsEqual(left, right, epsilon); + return (std::abs(left - right) <= std::numeric_limits::epsilon()); } inline double ConvertHtkMel(double frequencies) @@ -275,4 +201,4 @@ inline double ConvertHtkHz(double mels) } } // namespace Sensors } // namespace OHOS -#endif \ No newline at end of file +#endif // CONVERSION_UTILS_H \ No newline at end of file