223 lines
8.4 KiB
C++
223 lines
8.4 KiB
C++
|
|
#include <complex.h>
|
|
#include "fftw3.h"
|
|
#include <stdio.h>
|
|
#include <iostream>
|
|
#include <math.h>
|
|
#include <iomanip>
|
|
#include <cmath>
|
|
#include <stdlib.h>
|
|
#include <omp.h>
|
|
#include <algorithm>
|
|
#include <cstring>
|
|
#include "structDef.hpp"
|
|
#include "deleteTransmissionSignal.hpp"
|
|
|
|
using namespace std;
|
|
|
|
#define REAL 0
|
|
#define IMAG 1
|
|
#define FFTW_WISDOM_TYPE FFTW_MEASURE//FFTW_PATIENT //FFTW_MEASURE //FFTW_ESTIMATE
|
|
|
|
/*
|
|
* C implementation of preprocessing of AScan for reflection reconstruction
|
|
* TODO: memory alloc
|
|
*/
|
|
void performSignalProcessingC(double* aScanArray, int numScans, int numSamples, double* matchedFilter_r, double* matchedFilter_i, int* channelList, double* offsetFour_r, double* offsetFour_i, double* sincPeakFour_r, double* sincPeakFour_i, int* startPos, int* endPos, struct Params *paramStruct, double* aScanArrayProcessed) {
|
|
|
|
fftw_make_planner_thread_safe();
|
|
fftw_plan plan_fftAScanAScanComplex, plan_ifftAScanComplex, plan_fftHelpAScanComplex, plan_ifftAScanComplexHelp;
|
|
char filenameFftwWisdom[200] = "";
|
|
sprintf(filenameFftwWisdom, "fftw_wisdom_reflectionPreprocessing_%d.wis", FFTW_WISDOM_TYPE);
|
|
int loadedWisdomUsed = 0;
|
|
// loading
|
|
|
|
#pragma omp parallel default(none) num_threads(paramStruct->numThreads) shared(loadedWisdomUsed, filenameFftwWisdom, plan_fftAScanAScanComplex, plan_ifftAScanComplex, plan_fftHelpAScanComplex, plan_ifftAScanComplexHelp, numScans, numSamples, aScanArray, matchedFilter_i, matchedFilter_r, channelList, offsetFour_r, offsetFour_i, paramStruct, sincPeakFour_r, sincPeakFour_i, startPos, endPos, aScanArrayProcessed)
|
|
{
|
|
|
|
double *help, *aScan;
|
|
help = (double *) fftw_malloc(sizeof(double) * numSamples);
|
|
aScan = (double *) fftw_malloc(sizeof(double) * numSamples);
|
|
|
|
fftw_complex *aScanComplex;
|
|
aScanComplex = (fftw_complex *) fftw_malloc(sizeof(fftw_complex)* numSamples);
|
|
|
|
|
|
/* plans */
|
|
#pragma omp critical (createPlan)
|
|
{
|
|
// load wisom if exist
|
|
if ( loadedWisdomUsed == 0){
|
|
loadedWisdomUsed = fftw_import_wisdom_from_filename(filenameFftwWisdom);
|
|
}
|
|
|
|
if(plan_fftAScanAScanComplex==NULL){
|
|
plan_fftAScanAScanComplex = fftw_plan_dft_r2c_1d((int)numSamples, aScan, aScanComplex, FFTW_WISDOM_TYPE);
|
|
if (plan_fftAScanAScanComplex == NULL) { throw "plan creation failed."; }
|
|
}
|
|
|
|
if(plan_ifftAScanComplex==NULL){
|
|
plan_ifftAScanComplex = fftw_plan_dft_1d((int)numSamples, aScanComplex, aScanComplex, FFTW_BACKWARD, FFTW_WISDOM_TYPE);
|
|
if (plan_ifftAScanComplex == NULL) { throw "plan creation failed."; }
|
|
}
|
|
|
|
if (paramStruct->useOptPulse == 1 && plan_fftHelpAScanComplex==NULL) {
|
|
plan_fftHelpAScanComplex = fftw_plan_dft_r2c_1d((int)numSamples, help, aScanComplex, FFTW_WISDOM_TYPE);
|
|
if (plan_fftHelpAScanComplex == NULL) { throw "plan creation failed."; }
|
|
}
|
|
|
|
if(plan_ifftAScanComplexHelp==NULL){
|
|
plan_ifftAScanComplexHelp = fftw_plan_dft_c2r_1d((int)numSamples, aScanComplex, help, FFTW_WISDOM_TYPE);
|
|
if (plan_ifftAScanComplexHelp == NULL) { throw "plan creation failed."; }
|
|
}
|
|
}
|
|
|
|
#pragma omp for
|
|
for (int numScan = 0; numScan < numScans; numScan++) {
|
|
|
|
double helpReal, helpImag;
|
|
int i, iScanStart, iMatchedFilter;
|
|
|
|
iScanStart = numScan * numSamples;
|
|
iMatchedFilter = (int)(channelList[numScan] - 1)*numSamples;
|
|
|
|
memcpy(aScan, &aScanArray[iScanStart], sizeof(double) * numSamples);
|
|
|
|
// Change to foutier domain
|
|
fftw_execute_dft_r2c(plan_fftAScanAScanComplex, aScan, aScanComplex);
|
|
|
|
for (i = 0; i < numSamples / 2 + 1; i++) {
|
|
|
|
if ((paramStruct->useCorrelation == 1) && (paramStruct->matchedFilterCeAScan == 1)) {
|
|
// matchedFiltering
|
|
helpReal = (aScanComplex[i][REAL] * matchedFilter_r[iMatchedFilter + i] + aScanComplex[i][IMAG] * matchedFilter_i[iMatchedFilter + i]) / numSamples;
|
|
helpImag = (aScanComplex[i][IMAG] * matchedFilter_r[iMatchedFilter + i] - aScanComplex[i][REAL] * matchedFilter_i[iMatchedFilter + i]) / numSamples;
|
|
|
|
}
|
|
else {
|
|
helpReal = aScanComplex[i][REAL] / numSamples;
|
|
helpImag = aScanComplex[i][IMAG] / numSamples;
|
|
}
|
|
|
|
//remove offset
|
|
aScanComplex[i][REAL] = helpReal * offsetFour_r[i] - helpImag * offsetFour_i[i];
|
|
aScanComplex[i][IMAG] = helpReal * offsetFour_i[i] + offsetFour_r[i] * helpImag;
|
|
|
|
}
|
|
|
|
if (paramStruct->useOptPulse == 1) {
|
|
// analytic signal and optimal pulse...
|
|
|
|
aScanComplex[0][REAL] = 0.0;
|
|
aScanComplex[0][IMAG] = 0.0;
|
|
|
|
for (i = numSamples / 2; i < numSamples; i++) {
|
|
aScanComplex[i][REAL] = 0.0;
|
|
aScanComplex[i][IMAG] = 0.0;
|
|
}
|
|
|
|
for (i = 1; i < numSamples / 2; i++) {
|
|
aScanComplex[i][REAL] *= 2;
|
|
aScanComplex[i][IMAG] *= 2;
|
|
}
|
|
|
|
// back transform for analytic signal computation
|
|
fftw_execute_dft(plan_ifftAScanComplex, aScanComplex, aScanComplex);
|
|
|
|
// abs, cut to original A-Scan length
|
|
for (i = 0; i < numSamples; i++) {
|
|
aScan[i] = sqrt(aScanComplex[i][REAL] * aScanComplex[i][REAL] + aScanComplex[i][IMAG] * aScanComplex[i][IMAG]);
|
|
}
|
|
|
|
// set aScan = 0, where no max point (gradient change)
|
|
help[0] = 0;
|
|
for (i = 1; i < numSamples; i++) {
|
|
help[i] = aScan[i - 1] - aScan[i];
|
|
if (!(help[i - 1] < 0.0 && help[i]>0.0)) {
|
|
aScan[i - 1] = 0.0;
|
|
}
|
|
}
|
|
aScan[numSamples - 1] = 0.0;
|
|
|
|
// help = AScan with negative values set to 0
|
|
for (i = 0; i < numSamples; i++) {
|
|
if (aScan[i] < 0.0) {
|
|
help[i] = 0.0;
|
|
}
|
|
else {
|
|
help[i] = aScan[i];
|
|
}
|
|
}
|
|
|
|
// reduce number of peaks to largest peaks
|
|
if ((paramStruct->limitNumPulsesTo > 0) && (numSamples > paramStruct->limitNumPulsesTo)) {
|
|
double helpSort[numSamples];
|
|
memcpy(helpSort, help, sizeof(helpSort));
|
|
sort(helpSort, helpSort + numSamples);
|
|
for (i = 0; i < numSamples; i++) {
|
|
if (help[i] < helpSort[numSamples - paramStruct->limitNumPulsesTo]) {
|
|
help[i] = 0.0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// change to foutier domain again
|
|
fftw_execute_dft_r2c(plan_fftHelpAScanComplex, help, aScanComplex);
|
|
|
|
for (i = 0; i < numSamples / 2 + 1; i++) {
|
|
helpReal = aScanComplex[i][REAL];
|
|
helpImag = aScanComplex[i][IMAG];
|
|
aScanComplex[i][REAL] = (helpReal* sincPeakFour_r[i] - helpImag * sincPeakFour_i[i]) / numSamples;
|
|
aScanComplex[i][IMAG] = (helpReal * sincPeakFour_i[i] + sincPeakFour_r[i] * helpImag) / numSamples;
|
|
}
|
|
|
|
// back transform
|
|
fftw_execute_dft_c2r(plan_ifftAScanComplexHelp, aScanComplex, help);
|
|
|
|
for (i = 0; i < numSamples; i++) {
|
|
if (aScan[i] >= 0) {
|
|
aScanArrayProcessed[iScanStart + i] = help[i];
|
|
}
|
|
else {
|
|
aScanArrayProcessed[iScanStart + i] = aScan[i];
|
|
}
|
|
}
|
|
|
|
}
|
|
else {
|
|
// back transform
|
|
fftw_execute_dft_c2r(plan_ifftAScanComplexHelp, aScanComplex, help);
|
|
|
|
memcpy(&aScanArrayProcessed[iScanStart], help, sizeof(help));
|
|
}
|
|
|
|
if (paramStruct->useOptPulse == 1) {
|
|
deleteTransmissionSignal(&aScanArrayProcessed[iScanStart], numSamples, paramStruct->windowLength, --startPos[numScan], --endPos[numScan]);
|
|
}
|
|
|
|
}
|
|
|
|
if (aScanComplex != NULL) fftw_free(aScanComplex);
|
|
if (aScan != NULL) fftw_free(aScan);
|
|
if (help != NULL) fftw_free(help);
|
|
|
|
|
|
}
|
|
|
|
// clean
|
|
if (plan_fftAScanAScanComplex != NULL) fftw_destroy_plan(plan_fftAScanAScanComplex);
|
|
if (plan_ifftAScanComplex != NULL) fftw_destroy_plan(plan_ifftAScanComplex);
|
|
if (paramStruct->useOptPulse == 1 && plan_fftHelpAScanComplex != NULL) fftw_destroy_plan(plan_fftHelpAScanComplex);
|
|
if(plan_ifftAScanComplexHelp!=NULL) fftw_destroy_plan(plan_ifftAScanComplexHelp);
|
|
|
|
|
|
if (loadedWisdomUsed == 0 && fftw_export_wisdom_to_filename(filenameFftwWisdom)){
|
|
printf("Wisdom saved.\n");
|
|
}
|
|
|
|
fftw_forget_wisdom();
|
|
// fftw_cleanup();
|
|
|
|
return;
|
|
}
|