Commit source

signalProcessingMexCall/src/deleteTransmissionSignal.cpp

@@ -0,0 +1,129 @@
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <cstring>
+#include <algorithm>
+#include <cmath>
+
+#include "mathMethods.hpp"
+
+using namespace std; 
+
+#define PI 3.141592653589793
+
+/*
+ * calculates energy sum form energy move and energy pot
+ *
+ */
+void energyEstimation(double* partData, int numElements, double* energySum) {
+
+	//calculate energy sum
+	energySum[0] = partData[0];
+	diff(partData, numElements, energySum);
+
+	numElements--; // get last element index
+
+	for (int i = 0; i < numElements; i++) {
+		energySum[i] += energySum[i + 1];
+		energySum[i] = (energySum[i] * energySum[i]) * 0.5;
+		energySum[i] += (partData[i] * partData[i]);
+	}
+
+	energySum[numElements] = (energySum[numElements] * energySum[numElements]) * 0.5;
+	energySum[numElements] += partData[numElements] * partData[numElements];
+}
+
+/*
+ * detection and deletion of transmission signal in given window (defined by start, endPos)
+ *
+ */
+void deleteTransmissionSignal(double* aScan, int numSamples, int windowLength, int startPos, int endPos) {
+    
+    // variable definitions
+
+	// counter
+	int i, j;
+    
+    int index, index2;
+    int l_partData;
+    
+	l_partData = endPos - startPos; // num Data
+	l_partData++;
+
+	double partData[l_partData], energySum[l_partData];
+    
+    // cutting relevant part from aScan and weightning over range
+	j = startPos;	
+	for (i = 0; i < l_partData; i++) {
+		partData[i] = aScan[j] * (0.75 - 0.25 * cos((2 * PI * i / (l_partData - 1))));
+		j++;
+	}
+
+	// calculate energy
+	energyEstimation(partData, l_partData, energySum);
+
+	// calculate mean of energy along dimension
+	double mean_energySum[l_partData + windowLength] = { 0 };
+	for (i = 0; i < windowLength; i++) {
+		for (j = 0; j < l_partData; j++) {
+			mean_energySum[i + j] += energySum[j];
+		}
+	}
+
+	// windowing of mean energy
+	int num = (l_partData + windowLength - round((double)(windowLength - 0.001) / 2.0)) - windowLength / 2 + 1;
+	double mean_energySum2[num];
+	for (i = 0; i < num; i++) {
+		mean_energySum2[i] = mean_energySum[windowLength / 2 + i - 1];
+
+	}
+
+	// calculate g
+	double g[num];
+	g[0] = 0;
+	diff(mean_energySum2, num, g);
+	addFollower(g, num);
+
+	// calculate f
+	double f[num];
+	f[0] = g[0];
+	diff(g, num, f);
+	addFollower(f, num);
+
+	for (i = 0; i < num; i++) {
+		if (f[i] < 0 || g[i] < 0) {
+			f[i] = 0;
+		}
+	}
+
+    int peak = distance(g, max_element(g, g + num));
+	index = distance(f, max_element(f, f + peak));
+	double medianMeanEnergySum = findMedian(mean_energySum2, num);
+	
+    // SEEK for MAX == begin
+	if (mean_energySum2[index] > medianMeanEnergySum) {
+		while (mean_energySum2[index] >= medianMeanEnergySum && index > 0) {
+			--index;
+		}
+	}
+
+	// SEEK for NEXT MAX == end
+	index2 = peak;
+	while (mean_energySum2[index2] >= medianMeanEnergySum && index2 < num) {
+		index2++;
+	}
+	if (index2 >= num) {
+		index2 = num - 2 ;// take second last element
+	}
+
+    // absolute start/end psoitions
+	endPos = index2 + startPos;
+	startPos += index;
+
+	// remove transmission signal    
+	memset(&aScan[startPos], 0, sizeof(double)*(endPos - startPos + 1));
+
+	return;
+
+}
+

signalProcessingMexCall/src/deleteTransmissionSignal.hpp

@@ -0,0 +1,2 @@
+extern void energyEstimation(double* partData, int numElements, double* energySum);
+extern void deleteTransmissionSignal(double* aScan, int numSamples, int windowLength, int startPos, int endPos);

signalProcessingMexCall/src/mathMethods.cpp

@@ -0,0 +1,34 @@
+#include <algorithm>
+#include <cstring>
+
+using namespace std; 
+
+double findMedian(double a[], int numElements) 
+{ 
+    double aSort [numElements];
+    memcpy(aSort, a,sizeof(aSort)); // muss noch gecheckt werden
+    // First we sort the array 
+    sort(aSort, aSort+numElements); 
+  
+    // check for even case 
+    if (numElements & 1 )  //& 1  
+        return aSort[numElements/2]; 
+
+    return (aSort[(numElements-1)/2] + aSort[numElements/2])/2.0;            
+
+} 
+
+void diff(double* a, int numElements, double* aResult) 
+{ 
+    for (int i = 1; i < numElements; i++){
+        aResult[i] = a[i] - a[i-1];
+    }
+} 
+
+void addFollower(double* a, int numElements)
+{
+    for (int i = 0; i < numElements-1; i++){
+        a[i] += a[i+1];
+    }
+}
+

signalProcessingMexCall/src/mathMethods.hpp

@@ -0,0 +1,3 @@
+extern double findMedian(double a[], int numElements);
+extern void diff(double* a, int numElements, double* aResult);
+extern void addFollower(double* a, int numElements);

signalProcessingMexCall/src/performSignalProcessing.cpp

@@ -0,0 +1,93 @@
+#include "performSignalProcessing.h"
+#include "stdio.h"
+#include <complex.h>
+#include "fftw3.h"
+#include <iomanip>
+#include <cmath>
+#include <stdlib.h>
+#include "performSignalProcessingC.hpp"
+
+#include <iostream>
+
+
+
+void printInputInfo(){
+		printf( "  ================================================================================================= \n");
+    	printf( "   Inputparameter         \n");
+        
+		printf( "   1-prhs[0]  AScan data                    [nSamplesxnAscans]     double\n");
+		printf( "   2-prhs[1]  matched filter                [nSamplesxnChannels]   complex double\n");
+		printf( "   3-prhs[2]  channel                       [nSamplesxnAscans]     int32\n");
+		printf( "   4-prhs[3]  offset                        [nSamplesx1]           complex double\n");
+	    printf( "   5-prhs[4]  sincPeak                      [nSamplesx1]           complex double\n");
+        printf( "   6-prhs[5]  start position                [1xnAscans]            int32\n");
+	    printf( "   7-prhs[6]  end position                  [1xnAscans]            int32\n");
+	    printf( "   8-prhs[7]  combined adittional processing informations          struct, with numeric scalars\n");
+	    printf( "              fields: useCorrelation, matchedFilterCeAScan, useOptPulse, limitNumPulsesTo, removeTransmissionSignal, windowLength, numThreads");
+
+
+		printf( "  ================================================================================================= \n");
+}
+
+/*
+ * mex function, handles data inputs from matlab, calls actual processing function
+ *
+ * Example MATLAB call:
+ *
+ *
+ * % Input vars
+ *
+ * blockedAScans = repmat(gauspuls(-100e-05:1e-6:100e-05,50e3,1)',1,2);
+ * matchedFilter = fft(blockedAScans(:,1));
+ * blockedAScans(:,2) = blockedAScans + rand(size(blockedAScans,1),1);
+ * blockedChannels = [1,1];
+ * exponent = zeros(size(blockedAScans,1),1);
+ * optPulse = ;
+ * blockedstartPosition = [975, 975] ;
+ * blockedEndPosition = [1030, 1030] ;;
+ *
+ * blockedAScansProcessed = performSignalProcessingMex(
+ *                                      double(blockedAScans), 
+ *                                      double(matchedFilter),
+ *                                      int32(blockedChannels), 
+ *                                      double(exponent),
+ *                                      double(optPulse),
+ *                                      int32(blockedstartPosition),
+ *                                      int32(blockedEndPosition), 
+ *                                      signalProcessingParams); 
+ **/
+double* performSignalProcessing(    
+    double* aScanArray,         // data block of AScan data    
+	int numSamples,             // number of AScan samples
+    int numScans,
+    double* matchedFilter_r,    // matchedFilter in fourier space (real)
+    double* matchedFilter_i,    // matchedFilter in fourier space (imag)
+    int* channelList,           // lookup table with channels for matched filter
+  
+
+    double* offset_r,			// electronic offset in fourier space (real)
+    double* offset_i,			// electronic offset in fourier space (imag)
+
+    double* sincPeak_r,			// sincPeak in fourier space (real)
+    double* sincPeak_i,			// sincPeak in fourier space (imag)
+    
+	int* startPos,			    // startPosition for transmission signal removal
+    int* endPos,		        // endPosition for transmission signal removal
+    Params paramStruct          // additional configuration infos
+    )
+{  
+    /* output */    
+    double* aScanArrayProcessed = (double*)malloc(sizeof(double)*numSamples*numScans);
+        
+	/* execute signal processing */
+    try{
+        performSignalProcessingC(aScanArray, numScans, numSamples, matchedFilter_r, matchedFilter_i, channelList, offset_r, offset_i, sincPeak_r, sincPeak_i, startPos, endPos, &paramStruct, aScanArrayProcessed);
+        return aScanArrayProcessed;
+    }catch(const char* msg){  
+        std::cerr<<"performSignalProcessingMex:signalProcessing, error msg:"<<msg<<std::endl;
+        free(aScanArrayProcessed);
+        return nullptr;
+        // exit MEX file  
+    }     
+            
+}

signalProcessingMexCall/src/performSignalProcessing.h

@@ -0,0 +1,20 @@
+#include "structDef.hpp"
+extern double* performSignalProcessing(    
+    double* aScanArray,         // data block of AScan data    
+	int numSamples,             // number of AScan samples
+    int numScans,
+    double* matchedFilter_r,    // matchedFilter in fourier space (real)
+    double* matchedFilter_i,    // matchedFilter in fourier space (imag)
+    int* channelList,           // lookup table with channels for matched filter
+  
+
+    double* offset_r,			// electronic offset in fourier space (real)
+    double* offset_i,			// electronic offset in fourier space (imag)
+
+    double* sincPeak_r,			// sincPeak in fourier space (real)
+    double* sincPeak_i,			// sincPeak in fourier space (imag)
+    
+	int* startPos,			    // startPosition for transmission signal removal
+    int* endPos,		        // endPosition for transmission signal removal
+    Params paramStruct          // additional configuration infos
+    );

signalProcessingMexCall/src/performSignalProcessingC.cpp

@@ -0,0 +1,222 @@
+
+#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;
+}

signalProcessingMexCall/src/performSignalProcessingC.hpp

@@ -0,0 +1 @@
+extern 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);

signalProcessingMexCall/src/structDef.hpp

@@ -0,0 +1,10 @@
+
+struct Params{
+    int useCorrelation;
+    int matchedFilterCeAScan;
+    int useOptPulse;
+    int limitNumPulsesTo;
+    int removeTransmissionSignal;
+    int windowLength;
+    int numThreads;
+};