peak_finder.cpp

#include "VVT-V2\peak_finder.h"
 
#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
 
void diff(std::vector<float> in, std::vector<float>& out)
{
    out = std::vector<float>(in.size()-1);
 
    for(int i=1; i<in.size(); ++i)
        out[i-1] = in[i] - in[i-1];
}
 
void vectorElementsProduct(std::vector<float> a, std::vector<float> b, std::vector<float>& out)
{
    out = std::vector<float>(a.size());
 
    for(int i=0; i<a.size(); ++i)
        out[i] = a[i] * b[i];
}
 
void findIndicesLessThan(std::vector<float> in, float threshold, std::vector<int>& indices)
{
    for(int i=0; i<in.size(); ++i)
        if(in[i]<threshold)
            indices.push_back(i+1);
}
 
void selectElementsFromIndices(std::vector<float> in, std::vector<int> indices, std::vector<float>& out)
{
    for(int i=0; i<indices.size(); ++i)
        out.push_back(in[indices[i]]);
}
 
void selectElementsFromIndices(std::vector<int> in, std::vector<int> indices, std::vector<int>& out)
{
    for(int i=0; i<indices.size(); ++i)
        out.push_back(in[indices[i]]);
}
 
void signVector(std::vector<float> in, std::vector<int>& out)
{
    out = std::vector<int>(in.size());
 
    for(int i=0; i<in.size(); ++i)
    {
        if(in[i]>0)
            out[i]=1;
        else if(in[i]<0)
            out[i]=-1;
        else
            out[i]=0;
    }
}
 
void scalarProduct(float scalar, std::vector<float> in, std::vector<float>& out)
{
    out = std::vector<float>(in.size());
 
    for(int i=0; i<in.size(); ++i)
        out[i] = scalar * in[i];
}
 
void PeakFinder::findPeaks(std::vector<float> x0, std::vector<int>& peakInds, bool includeEndpoints, float extrema)
{
    int minIdx = distance(x0.begin(), min_element(x0.begin(), x0.end()));
    int maxIdx = distance(x0.begin(), max_element(x0.begin(), x0.end()));
 
    float sel = (x0[maxIdx]-x0[minIdx])/4.0;
    int len0 = x0.size();
 
    scalarProduct(extrema, x0, x0);
 
    std::vector<float> dx;
    diff(x0, dx);
    replace(dx.begin(), dx.end(), 0.0f, -PeakFinder::EPS);
    std::vector<float> dx0(dx.begin(), dx.end()-1);
    std::vector<float> dx0_1(dx.begin()+1, dx.end());
    std::vector<float> dx0_2;
 
    vectorElementsProduct(dx0, dx0_1, dx0_2);
 
    std::vector<int> ind;
    findIndicesLessThan(dx0_2, 0, ind); // Find where the derivative changes sign   
    std::vector<float> x;
    float leftMin;
    int minMagIdx;
    float minMag;
    
    if(includeEndpoints)
    {
        //x = [x0(1);x0(ind);x0(end)];  
        selectElementsFromIndices(x0, ind, x);      
        x.insert(x.begin(), x0[0]);
        x.insert(x.end(), x0[x0.size()-1]);
        //ind = [1;ind;len0];
        ind.insert(ind.begin(), 1);
        ind.insert(ind.end(), len0);
        minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));
        minMag = x[minMagIdx];      
        //std::cout<<"Hola"<<std::endl;
        leftMin = minMag;
    }
    else
    {
        selectElementsFromIndices(x0, ind, x);
        if(x.size()>2)
        {
            minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));      
            minMag = x[minMagIdx];              
            leftMin = x[0]<x0[0]?x[0]:x0[0];
        }
    }
 
    int len = x.size();
 
    if(len>2)
    {
        float tempMag = minMag;
        bool foundPeak = false;
        int ii;
 
        if(includeEndpoints)
        {
            // Deal with first point a little differently since tacked it on
            // Calculate the sign of the derivative since we tacked the first
            //  point on it does not neccessarily alternate like the rest.
            std::vector<float> xSub0(x.begin(), x.begin()+3);//tener cuidado subvector
            std::vector<float> xDiff;//tener cuidado subvector
            diff(xSub0, xDiff);
 
            std::vector<int> signDx;
            signVector(xDiff, signDx);
 
            if (signDx[0] <= 0) // The first point is larger or equal to the second
            {
                if (signDx[0] == signDx[1]) // Want alternating signs
                {
                    x.erase(x.begin()+1);
                    ind.erase(ind.begin()+1);
                    len = len-1;
                }
            }
            else // First point is smaller than the second
            {
                if (signDx[0] == signDx[1]) // Want alternating signs
                {
                    x.erase(x.begin());
                    ind.erase(ind.begin());
                    len = len-1;
                }
            }
        }
 
        //Skip the first point if it is smaller so we always start on maxima
        if ( x[0] >= x[1] )
            ii = 0;
        else
            ii = 1;
 
        //Preallocate max number of maxima
        float maxPeaks = ceil((float)len/2.0);
        std::vector<int> peakLoc(maxPeaks,0);
        std::vector<float> peakMag(maxPeaks,0.0);
        int cInd = 1;
        int tempLoc = 0;        
    
        while(ii < len)
        {
            ii = ii+1;//This is a peak
            //Reset peak finding if we had a peak and the next peak is bigger
            //than the last or the left min was small enough to reset.
            if(foundPeak)
            {
                tempMag = minMag;
                foundPeak = false;
            }
        
            //Found new peak that was lager than temp mag and selectivity larger
            //than the minimum to its left.
        
            if( x[ii-1] > tempMag && x[ii-1] > leftMin + sel )
            {
                tempLoc = ii-1;
                tempMag = x[ii-1];
            }
 
            //Make sure we don't iterate past the length of our vector
            if(ii == len)
                break; //We assign the last point differently out of the loop
 
            ii = ii+1; // Move onto the valley
            
            //Come down at least sel from peak
            if(!foundPeak && tempMag > sel + x[ii-1])
            {               
                foundPeak = true; //We have found a peak
                leftMin = x[ii-1];
                peakLoc[cInd-1] = tempLoc; // Add peak to index
                peakMag[cInd-1] = tempMag;
                cInd = cInd+1;
            }
            else if(x[ii-1] < leftMin) // New left minima
                leftMin = x[ii-1];
            
        }
 
        // Check end point
        if(includeEndpoints)
        {        
            if ( x[x.size()-1] > tempMag && x[x.size()-1] > leftMin + sel )
            {
                peakLoc[cInd-1] = len-1;
                peakMag[cInd-1] = x[x.size()-1];
                cInd = cInd + 1;
            }
            else if( !foundPeak && tempMag > minMag )// Check if we still need to add the last point
            {
                peakLoc[cInd-1] = tempLoc;
                peakMag[cInd-1] = tempMag;
                cInd = cInd + 1;
            }
        }
        else if(!foundPeak)
        {
            float minAux = x0[x0.size()-1]<x[x.size()-1]?x0[x0.size()-1]:x[x.size()-1];
            if ( x[x.size()-1] > tempMag && x[x.size()-1] > leftMin + sel )
            {
                peakLoc[cInd-1] = len-1;
                peakMag[cInd-1] = x[x.size()-1];
                cInd = cInd + 1;
            }
            else if( !tempMag >  minAux + sel)// Check if we still need to add the last point
            {
                peakLoc[cInd-1] = tempLoc;
                peakMag[cInd-1] = tempMag;
                cInd = cInd + 1;
            }
        }
 
        //Create output
        if( cInd > 0 )
        {           
            std::vector<int> peakLocTmp(peakLoc.begin(), peakLoc.begin()+cInd-1);
            selectElementsFromIndices(ind, peakLocTmp, peakInds);           
        }       
 
    }
    //else
    //{
        //input signal length <= 2
    //}
}