452 lines
15 KiB
C++
452 lines
15 KiB
C++
#include "calculation.hpp"
|
||
|
||
Calculation::Calculation() {}
|
||
|
||
Calculation::~Calculation() {}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的复数快速傅里叶变换 */
|
||
/************************************************************************/
|
||
void Calculation::FFT(int n, fftw_complex *in, fftw_complex *out) {
|
||
if (in == NULL || out == NULL) return;
|
||
fftw_plan p;
|
||
p = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
|
||
fftw_execute(p);
|
||
fftw_destroy_plan(p);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的实数快速傅里叶变换 */
|
||
/************************************************************************/
|
||
void Calculation::FFT_R(int n, std::vector<float> &vecData, fftw_complex *out) {
|
||
double in[n];
|
||
for (int i = 0; i < n; i++) {
|
||
in[i] = vecData[i];
|
||
}
|
||
|
||
for (int i = 0; i < n; i++) {
|
||
out[i][0] = (double)vecData[i];
|
||
out[i][1] = 0;
|
||
}
|
||
// create a DFT plan and execute it
|
||
fftw_plan plan = fftw_plan_dft_r2c_1d(n, in, out, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
// destroy the plan to prevent a memory leak
|
||
fftw_destroy_plan(plan);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
/************************************************************************/
|
||
/* 一维数据的快速傅里叶逆变换 */
|
||
/************************************************************************/
|
||
void Calculation::iFFT(int n, fftw_complex *in, fftw_complex *out) {
|
||
if (in == NULL || out == NULL) return;
|
||
fftw_plan p;
|
||
p = fftw_plan_dft_1d(n, in, out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||
fftw_execute(p);
|
||
fftw_destroy_plan(p);
|
||
fftw_cleanup();
|
||
}
|
||
|
||
void Calculation::_iFFT(std::vector<float> &vecrealData, std::vector<float> &vecimageData, std::vector<float> &veciFFTData) {
|
||
fftw_complex *inFFt, *outFFt;
|
||
int N = vecrealData.size();
|
||
inFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
|
||
for (int j = 0; j < N; j++) {
|
||
inFFt[j][0] = (double)vecrealData[j];
|
||
inFFt[j][1] = (double)vecimageData[j];
|
||
}
|
||
iFFT(N, inFFt, outFFt);
|
||
for (int i = 0; i < N; i++) {
|
||
outFFt[i][0] *= 1. / N;
|
||
outFFt[i][1] *= 1. / N;
|
||
veciFFTData.push_back(outFFt[i][0]);
|
||
}
|
||
fftw_free(inFFt);
|
||
fftw_free(outFFt);
|
||
}
|
||
|
||
void Calculation::_FFT(std::vector<float> &vecData, std::vector<float> &vecFFTrealData, std::vector<float> &vecFFTimageData) {
|
||
fftw_complex *inHilFFt, *outHilFFt;
|
||
inHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outHilFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (size_t j = 0; j < vecData.size(); j++) {
|
||
inHilFFt[j][0] = (double)vecData[j];
|
||
inHilFFt[j][1] = 0;
|
||
}
|
||
|
||
FFT(vecData.size(), inHilFFt, outHilFFt);
|
||
// fftShift(outHilFFt, vecData.size());
|
||
for (size_t i = 0; i < vecData.size(); i++) {
|
||
vecFFTrealData.push_back(outHilFFt[i][0]);
|
||
vecFFTimageData.push_back(outHilFFt[i][1]);
|
||
}
|
||
fftw_free(inHilFFt);
|
||
fftw_free(outHilFFt);
|
||
}
|
||
|
||
float Calculation::mean(std::vector<float> &vecData) {
|
||
float meanTemp = 0;
|
||
for (size_t i = 0; i < vecData.size(); i++) {
|
||
meanTemp += vecData[i];
|
||
}
|
||
return meanTemp / vecData.size();
|
||
}
|
||
|
||
float Calculation::getSample_variance(std::vector<float> a) {
|
||
float ss = 0;
|
||
float s = 0;
|
||
float mx = mean(a);
|
||
int len = a.size();
|
||
for (int i = 0; i < len; i++) {
|
||
s = a[i] - mx;
|
||
ss += pow(s, 2);
|
||
}
|
||
return ss / (len - 1);
|
||
}
|
||
|
||
void Calculation::Hanning(std::vector<float> &vecData, std::vector<float> &vecHanningData) {
|
||
int N = vecData.size();
|
||
|
||
float *w = NULL;
|
||
w = (float *)calloc(N, sizeof(float));
|
||
int half, i, idx;
|
||
if (N % 2 == 0) {
|
||
half = N / 2;
|
||
for (i = 0; i < half; i++) // CALC_HANNING Calculates Hanning window samples.
|
||
w[i] = 0.5 * (1 - cos(2 * M_PI * (i + 1) / (N + 1)));
|
||
|
||
idx = half - 1;
|
||
for (i = half; i < N; i++) {
|
||
w[i] = w[idx];
|
||
idx--;
|
||
}
|
||
} else {
|
||
half = (N + 1) / 2;
|
||
for (i = 0; i < half; i++) // CALC_HANNING Calculates Hanning window samples.
|
||
w[i] = 0.5 * (1 - cos(2 * M_PI * (i + 1) / (N + 1)));
|
||
|
||
idx = half - 2;
|
||
for (i = half; i < N; i++) {
|
||
w[i] = w[idx];
|
||
idx--;
|
||
}
|
||
}
|
||
for (int j = 0; j < N; j++) {
|
||
vecHanningData.push_back(w[j]);
|
||
}
|
||
free(w);
|
||
}
|
||
void Calculation::hilbert(std::vector<float> &vecData, std::vector<float> &vecHilbertData, int N) {
|
||
double in[N];
|
||
for (int i = 0; i < N; i++) {
|
||
in[i] = vecData[i];
|
||
}
|
||
fftw_complex *out;
|
||
out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * N);
|
||
|
||
for (int i = 0; i < N; ++i) {
|
||
out[i][0] = (double)vecData[i];
|
||
out[i][1] = 0;
|
||
}
|
||
// create a DFT plan and execute it
|
||
fftw_plan plan = fftw_plan_dft_r2c_1d(N, in, out, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
// destroy the plan to prevent a memory leak
|
||
fftw_destroy_plan(plan);
|
||
int hN = N >> 1; // half of the length (N /2)
|
||
int numRem = hN; // the number of remaining elements
|
||
// multiply the appropriate values by 2
|
||
// (those that should be multiplied by 1 are left intact because they wouldn't change)
|
||
for (int i = 1; i < hN; ++i) // 1,2,...,N/2 - 1 的项乘以2
|
||
{
|
||
out[i][0] *= 2;
|
||
out[i][1] *= 2;
|
||
}
|
||
// if the length is even, the number of remaining elements decreases by 1
|
||
if (N % 2 == 0) numRem--;
|
||
// if it's odd and greater than 1, the middle value must be multiplied by 2
|
||
else if (N > 1) // 奇数非空
|
||
{
|
||
out[hN][0] *= 2;
|
||
out[hN][1] *= 2;
|
||
}
|
||
// set the remaining values to 0
|
||
// (multiplying by 0 gives 0, so we don't care about the multiplicands)
|
||
memset(&out[hN + 1][0], 0, numRem * sizeof(fftw_complex));
|
||
// create an IDFT plan and execute it
|
||
plan = fftw_plan_dft_1d(N, out, out, FFTW_BACKWARD, FFTW_ESTIMATE);
|
||
fftw_execute(plan);
|
||
// do some cleaning
|
||
fftw_destroy_plan(plan);
|
||
fftw_cleanup();
|
||
// scale the IDFT output
|
||
for (int i = 0; i < N; ++i) {
|
||
out[i][0] /= N;
|
||
out[i][1] /= N;
|
||
}
|
||
|
||
for (int n = 0; n < N; n++) //输出
|
||
{
|
||
// xr[n]=cos(n*pi/6);//原始信号
|
||
// y_r[n] = s_i[n];
|
||
complex complex_after;
|
||
complex_after.real = out[n][1];
|
||
complex_after.imag = out[n][0];
|
||
float amp = sqrt(complex_after.real * complex_after.real + complex_after.imag * complex_after.imag);
|
||
vecHilbertData.push_back(amp);
|
||
// printf("%d %f\n",n,vecHilbertData[n]);
|
||
}
|
||
fftw_free(out);
|
||
}
|
||
|
||
void Calculation::fftShift(fftw_complex *in, int l) {
|
||
double temp;
|
||
double temp2;
|
||
|
||
for (int j = 0; j < l / 2; j++) {
|
||
temp = in[j + l / 2][0];
|
||
temp2 = in[j + l / 2][1];
|
||
in[j + l / 2][0] = in[j][0];
|
||
in[j + l / 2][1] = in[j][1];
|
||
in[j][0] = temp;
|
||
in[j][1] = temp2;
|
||
}
|
||
}
|
||
|
||
void Calculation::FFTSpec(std::vector<float> &vecData, std::vector<float> &vecFFTSpecData) {
|
||
fftw_complex *inFFt, *outFFt;
|
||
inFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
outFFt = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * vecData.size());
|
||
|
||
for (size_t j = 0; j < vecData.size(); j++) {
|
||
inFFt[j][0] = (double)vecData[j];
|
||
inFFt[j][1] = 0;
|
||
}
|
||
FFT(vecData.size(), inFFt, outFFt);
|
||
for (size_t j = 0; j < vecData.size() / 2; j++) {
|
||
vecFFTSpecData.push_back(sqrt(outFFt[j][0] * outFFt[j][0] + outFFt[j][1] * outFFt[j][1]) * 2 / vecData.size());
|
||
}
|
||
}
|
||
|
||
void Calculation::envSpec(std::vector<float> &vecData, std::vector<float> &vecEnvSpecData, int StartFrequency, int EndFrequency) {
|
||
std::vector<float> vecFFTrealData, vecFFTimageData;
|
||
std::vector<float> vecRealData, vecImageData;
|
||
std::vector<float> veciFFtData;
|
||
std::vector<float> veciFFtData2;
|
||
std::vector<float> vecHilbertData;
|
||
_FFT(vecData, vecFFTrealData, vecFFTimageData);
|
||
for (size_t i = 0; i < vecFFTrealData.size(); i++) {
|
||
if (i > (size_t)StartFrequency && i < (size_t)EndFrequency) {
|
||
vecRealData.push_back(vecFFTrealData.at(i));
|
||
vecImageData.push_back(vecFFTimageData.at(i));
|
||
} else {
|
||
vecRealData.push_back(0);
|
||
vecImageData.push_back(0);
|
||
}
|
||
}
|
||
_iFFT(vecRealData, vecImageData, veciFFtData);
|
||
for (size_t j = 0; j < veciFFtData.size(); j++) {
|
||
veciFFtData2.push_back(veciFFtData[j] * 2);
|
||
}
|
||
hilbert(veciFFtData2, vecHilbertData, veciFFtData2.size());
|
||
FFTSpec(vecHilbertData, vecEnvSpecData);
|
||
}
|
||
|
||
void Calculation::GenerateSin(std::vector<float> &vecData) {
|
||
double frequency = 800.0; // Frequency of the sine wave in Hz
|
||
double sampling_rate = 12800.0; // Sampling rate in Hz (8 kHz)
|
||
size_t num_samples = 12800; // Total number of samples (1 second of data)
|
||
double dt = 1.0 / sampling_rate; // Time step in seconds
|
||
|
||
// Generate the sine wave
|
||
for (size_t i = 0; i < num_samples; ++i) {
|
||
vecData.push_back(std::sin(2 * M_PI * frequency * i * dt));
|
||
}
|
||
}
|
||
|
||
void Calculation::Integration(std::vector<float> &vecData, std::vector<float> &retData, double &resolution) {
|
||
std::vector<float> realshiftfft;
|
||
std::vector<float> imageshiftfft;
|
||
std::vector<float> realvalue, imagevalue, ifftData;
|
||
_FFT(vecData, realshiftfft, imageshiftfft);
|
||
|
||
//在频域上进行5-1000 Hz的带通滤波
|
||
for (size_t i = 0; i < vecData.size() / 2 + 1; ++i) {
|
||
double frequency = i * resolution; // 计算当前频率分量
|
||
if (frequency < 5 || frequency > 1000) {
|
||
// 将5 Hz 到 1000 Hz 之外的频率成分设置为0
|
||
realshiftfft[i] = 0.0;
|
||
realshiftfft[i] = 0.0;
|
||
imageshiftfft[i] = 0.0;
|
||
imageshiftfft[i] = 0.0;
|
||
}
|
||
}
|
||
|
||
for (size_t k = 1; k < realshiftfft.size() + 1; k++) {
|
||
double frequency = k * resolution; // 计算当前频率分量
|
||
realvalue.push_back((realshiftfft.at(k - 1) / (frequency * 2 * M_PI)) * 1000 * 2); //单位转换mm/s,*1000 *2 精度损失
|
||
imagevalue.push_back((imageshiftfft.at(k - 1) / (frequency * 2 * M_PI)) * 1000 * 2); //单位转换mm/s,*1000
|
||
}
|
||
|
||
_iFFT(realvalue, imagevalue, retData);
|
||
}
|
||
|
||
|
||
std::vector<float> Calculation::fftInterpolate(const std::vector<float>& input, size_t outputSize) {
|
||
const size_t inputSize = input.size();
|
||
if (inputSize == 0 || outputSize == 0) {
|
||
return {};
|
||
}
|
||
|
||
// FFTW 的实数输入建议用 double
|
||
std::vector<double> in(inputSize);
|
||
for (size_t i = 0; i < inputSize; ++i) {
|
||
in[i] = static_cast<double>(input[i]);
|
||
}
|
||
|
||
const size_t inFreqSize = inputSize / 2 + 1;
|
||
const size_t outFreqSize = outputSize / 2 + 1;
|
||
|
||
fftw_complex* freqIn = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * inFreqSize);
|
||
fftw_complex* freqOut = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * outFreqSize);
|
||
|
||
if (!freqIn || !freqOut) {
|
||
if (freqIn) fftw_free(freqIn);
|
||
if (freqOut) fftw_free(freqOut);
|
||
throw std::runtime_error("FFTW malloc failed");
|
||
}
|
||
|
||
// 清零输出频谱
|
||
for (size_t i = 0; i < outFreqSize; ++i) {
|
||
freqOut[i][0] = 0.0;
|
||
freqOut[i][1] = 0.0;
|
||
}
|
||
|
||
fftw_plan forwardPlan = fftw_plan_dft_r2c_1d(
|
||
static_cast<int>(inputSize),
|
||
in.data(),
|
||
freqIn,
|
||
FFTW_ESTIMATE
|
||
);
|
||
|
||
fftw_execute(forwardPlan);
|
||
|
||
// 只复制半谱里能容纳的低频部分
|
||
// 对于重采样，保留共同可表示的频率范围
|
||
size_t copyBins = std::min(inFreqSize, outFreqSize);
|
||
|
||
// Nyquist 点要小心，但先按通用方式复制可用部分
|
||
for (size_t k = 0; k < copyBins; ++k) {
|
||
freqOut[k][0] = freqIn[k][0];
|
||
freqOut[k][1] = freqIn[k][1];
|
||
}
|
||
|
||
// 如果是偶数长度，Nyquist 点必须是纯实数
|
||
if (outputSize % 2 == 0) {
|
||
freqOut[outFreqSize - 1][1] = 0.0;
|
||
}
|
||
|
||
std::vector<double> out(outputSize, 0.0);
|
||
|
||
fftw_plan inversePlan = fftw_plan_dft_c2r_1d(
|
||
static_cast<int>(outputSize),
|
||
freqOut,
|
||
out.data(),
|
||
FFTW_ESTIMATE
|
||
);
|
||
|
||
fftw_execute(inversePlan);
|
||
|
||
// FFTW 的 c2r 没有自动归一化
|
||
// 这里除以 outputSize 是 IFFT 归一化
|
||
// 再乘 outputSize/inputSize 用于尽量保持幅值一致
|
||
const double scale = 1.0 / static_cast<double>(inputSize);
|
||
for (size_t i = 0; i < outputSize; ++i) {
|
||
out[i] *= scale;
|
||
}
|
||
|
||
std::vector<float> result(outputSize);
|
||
for (size_t i = 0; i < outputSize; ++i) {
|
||
result[i] = static_cast<float>(out[i]);
|
||
}
|
||
|
||
fftw_destroy_plan(forwardPlan);
|
||
fftw_destroy_plan(inversePlan);
|
||
fftw_free(freqIn);
|
||
fftw_free(freqOut);
|
||
|
||
return result;
|
||
}
|
||
|
||
bool Calculation::freqDomainDecimateFFTW(const std::vector<float>& input,
|
||
std::vector<float>& output,
|
||
int factor)
|
||
{
|
||
int N = input.size();
|
||
|
||
if(N % factor != 0)
|
||
{
|
||
std::cout<<"Length not divisible by factor\n";
|
||
return false;
|
||
}
|
||
|
||
int N_out = N / factor;
|
||
|
||
// ---------- 分配 ----------
|
||
double* in_time = (double*)fftw_malloc(sizeof(double)*N);
|
||
fftw_complex* spectrum =
|
||
(fftw_complex*)fftw_malloc(sizeof(fftw_complex)*(N/2+1));
|
||
|
||
for(int i=0;i<N;i++)
|
||
in_time[i] = input[i];
|
||
|
||
// ---------- FFT ----------
|
||
fftw_plan plan_fwd =
|
||
fftw_plan_dft_r2c_1d(N,in_time,spectrum,FFTW_ESTIMATE);
|
||
|
||
fftw_execute(plan_fwd);
|
||
|
||
// ---------- 构建新频谱 ----------
|
||
fftw_complex* new_spec =
|
||
(fftw_complex*)fftw_malloc(sizeof(fftw_complex)*(N_out/2+1));
|
||
|
||
memset(new_spec,0,sizeof(fftw_complex)*(N_out/2+1));
|
||
|
||
int half = N_out/2;
|
||
|
||
for(int i=0;i<=half;i++)
|
||
{
|
||
new_spec[i][0] = spectrum[i][0];
|
||
new_spec[i][1] = spectrum[i][1];
|
||
}
|
||
|
||
// ---------- IFFT ----------
|
||
double* out_time =
|
||
(double*)fftw_malloc(sizeof(double)*N_out);
|
||
|
||
fftw_plan plan_inv =
|
||
fftw_plan_dft_c2r_1d(N_out,new_spec,out_time,FFTW_ESTIMATE);
|
||
|
||
fftw_execute(plan_inv);
|
||
|
||
output.resize(N_out);
|
||
|
||
for(int i=0;i<N_out;i++)
|
||
output[i] = out_time[i] / N_out;
|
||
|
||
// ---------- 清理 ----------
|
||
fftw_destroy_plan(plan_fwd);
|
||
fftw_destroy_plan(plan_inv);
|
||
|
||
fftw_free(in_time);
|
||
fftw_free(out_time);
|
||
fftw_free(spectrum);
|
||
fftw_free(new_spec);
|
||
|
||
return true;
|
||
} |