将Matlab代码重写为C ++时的问题

时间:2016-07-25 21:33:05

标签: c++ matlab vector

我几个月后一直在努力将Matlab代码重新编写成C ++

已经完成的大部分工作(fft,ifft,xcorr和binFreq函数已经用c ++编写),现在我坚持使用一个函数。该函数用于估计两个信号S1和S2之间的延迟。该函数依赖于基于FFT的互相关。 我被困在这行“e = [e,eIter];”其中e是具有可变长度的向量(e先前声明为:“e = []”;)

Matlab中的原始函数:

% ****************************************************************
% estimates delay and scaling factor 
% ****************************************************************
function [delay_samples, coeff] = iterDelayEst(s1, s2)

    n = numel(s1);
    halfN = floor(n/2);

    assert(numel(s2) == n, 'signals must have same length');

    % ****************************************************************
    % constants
    % ****************************************************************    
    % exit if uncertainty below threshold
    thr_samples = 1e-7;

    % exit after fixed number of iterations
    nIter = 25;

    % frequency domain representation of signals
    fd1 = fft(s1);
    fd2 = fft(s2);    

    % first round: No delay was applied
    tau = [];  %(1,0) ;
    fd2Tau = fd2; % delayed s2 in freq. domain

    % frequency corresponding to each FFT bin -0.5..0.5
    f = binFreq(n);

    % uncertainty plot data
    e = [];

    % normalization factor
    nf = sqrt((fd1 * fd1') * (fd2 * fd2')) / n; % normalizes to 1
    %nf = sqrt((fd1 .* conj(fd1)) * (fd2 .* conj(fd2))') / n;

    % search window: 
    % known maximum and two surrounding points
    x1 = -1;
    x2 = -1;
    x3 = -1;
    y1 = -1;
    y2 = -1;
    y3 = -1;


    % ****************************************************************
    % iteration loop
    % ****************************************************************
    for count = 1:nIter

        % ****************************************************************
        % crosscorrelation with time-shifted signal
        % ****************************************************************
        xcorr = abs(ifft(fd2Tau .* conj(fd1)))/ nf;

        % ****************************************************************
        % detect peak
        % ****************************************************************
        if isempty(tau)

            % ****************************************************************
            % startup
            % initialize with three adjacent bins around peak
            % ****************************************************************
            ix = find(xcorr == max(xcorr));
            ix = ix(1); % use any, if multiple bitwise identical peaks

            % indices of three bins around peak
            ixLow = mod(ix-1-1, n) + 1; % one below
            ixMid = ix;
            ixHigh = mod(ix-1+1, n) + 1; % one above

            % delay corresponding to the three bins
            tauLow = mod(ixLow -1 + halfN, n) - halfN;
            tauMid = mod(ixMid -1 + halfN, n) - halfN;         
            tauHigh = mod(ixHigh -1 + halfN, n) - halfN; 

            % crosscorrelation value for the three bins
            xcLow =  xcorr(ixLow);
            xcMid =  xcorr(ixMid);
            xcHigh = xcorr(ixHigh);

            x1 = tauLow;
            x2 = tauMid;
            x3 = tauHigh;
            y1 = xcLow;
            y2 = xcMid; 
            y3 = xcHigh;
        else
            % ****************************************************************
            % only main peak at first bin is of interest
            % ****************************************************************
            tauMid = tau;
            xcMid = xcorr(1);

            if xcMid > y2
                % ****************************************************************
                % improve midpoint
                % ****************************************************************
                if tauMid > x2
                    % midpoint becomes lower point
                    x1 = x2;
                    y1 = y2;
                else
                    % midpoint becomes upper point
                    x3 = x2;
                    y3 = y2;
                end
                x2 = tauMid;
                y2 = xcMid;

            elseif tauMid < x2
                % ****************************************************************
                % improve low point
                % ****************************************************************
                assert(tauMid >= x1); % bitwise identical is OK
                assert(tauMid > x1 || xcMid > y1); % expect improvement
                x1 = tauMid;
                y1 = xcMid;
            elseif tauMid > x2 
                % ****************************************************************
                % improve high point
                % ****************************************************************
                assert(tauMid <= x3); % bitwise identical is OK                
                assert((tauMid < x3) || (xcMid > y3)); % expect improvement
                x3 = tauMid;
                y3 = xcMid;
            else
                assert(false, '?? evaluated for existing tau ??');
            end
        end

        % ****************************************************************
        % calculate uncertainty (window width)
        % ****************************************************************
        eIter = abs(x3 - x1);
        e = [e, eIter];
        if eIter < thr_samples
            % disp('threshold reached, exiting');
            break;
        end

        if y1 == y2 || y2 == y3
            % reached limit of numerical accuracy on one side
            usePoly = 0;
        else
            % ****************************************************************
            % fit 2nd order polynomial and find maximum
            % ****************************************************************

            num = (x2^2-x1^2)*y3+(x1^2-x3^2)*y2+(x3^2-x2^2)*y1;

            denom = (2*x2-2*x1)*y3+(2*x1-2*x3)*y2+(2*x3-2*x2)*y1;
            if denom ~= 0
                tau = (num / denom);
                % is the point within [x1, x3]? 
                usePoly = ((tau > x1) && (tau < x3));
            else
                usePoly = 0;
            end            
        end
        if ~usePoly
            % revert to linear interpolation on the side with the
            % less-accurate outer sample 
            % Note: There is no guarantee that the side with the more accurate
            % outer sample is the right one, as the samples aren't 
            % placed on a regular grid!
            % Therefore, iterate to improve the "worse" side, which will
            % eventually become the "better side", and iteration converges.

            tauLow = (x1 + x2) / 2;
            tauHigh = (x2 + x3) / 2;
            if y1 < y3
                o = [tauLow, tauHigh];
            else
                o = [tauHigh, tauLow];                
            end
            % don't choose point that is identical to one that is already known
            tau = o(1);
            if tau == x1 || tau == x2 || tau == x3
                tau = o(2);
                if tau == x1 || tau == x2 || tau == x3
                    break;
                end
            end
        end

        % ****************************************************************
        % advance 2nd signal according to location of maximum
        % phase shift in frequency domain - delay in time domain
        % ****************************************************************
        fd2Tau = fd2 .* exp(2i * pi * f * tau);
    end % for


    % ****************************************************************
    % the delay estimate is the final location of the delay that 
    % maximized crosscorrelation (center of window).
    % ****************************************************************
    delay_samples = x2;

    % ****************************************************************
    % Coefficient: Turn signal 1 into signal 2
    % ****************************************************************
    coeff = fd2Tau * fd1' ./ (fd1 * fd1');


end

到目前为止我写的C ++等效代码:

//iterDelayEst.cpp
#include <cstddef>
#include <utility>
#include <numeric>
#include <vector>

std::vector<std::size_t> max_indices(CArray const& y) {
    struct acc_t {
        std::vector<std::size_t> max_idcs;
        double max_value;
        std::size_t current_idx;

        acc_t&& next() {
            ++current_idx;
            return std::move(*this);
        }
        acc_t&& next_with_current() {
            max_idcs.push_back(current_idx++);
            return std::move(*this);
        }
        acc_t&& next_with(Complex const& c) {
            max_value = c.real();
            max_idcs.clear();
            return next_with_current();
        }
    };

    return std::accumulate(
        std::begin(y), std::end(y), acc_t{},
        [](acc_t acc, Complex const& c) {
            return c.real() < acc.max_value ? acc.next()
                 : c.real() > acc.max_value ? acc.next_with(c)
                 :                            acc.next_with_current();
        }
    ).max_idcs;
}
/****************************************************************************/
void tet(CArray const& y) {
    auto const max_idcs = max_indices(y);
    std::cout << "The max is " << y[max_idcs.front()] << " at indices [";
    auto first = true;
    for (auto const idx : max_idcs) {
        if (!first) {
            std::cout << ", ";
        } else {
            first = false;
        }
        std::cout << idx;
    }
    std::cout << "]\n";
}

/****************xcorr function**************************************************************/
void xcorr( CArray & x){
    int i;
        int n=32; 
    fft(x);
    x *=x.apply(std::conj);
    ifft(x);
    for ( i = 0 ; i < n ; i++ ){
            cout << "x[" << i <<"] =" << x[i] << endl;
        }
}

double iterDelayEst(int n,CArray& x, CArray& y)
{
        /***************************constants************************************************
        /************************************************************************************/
        //exit if uncertainty below threshold
    int halfN=std::floor(n/2);
    double thr_samples = 1e-7;
        Complex eIter;
    Complex e;
    //exit after fixed number of iterations
    double nIter = 25;
    fft(x);
    fft(y);

    //frequency domain representation of signals
    typedef std::vector < std::complex < double > > tau;

    auto f = binFreq(n);

    std::vector<double> e;
    Complex nf3(0.0,0.0);

    int j;

    for ( j = 0 ; j < n ; j++ )

    {
        auto nf1 = ((x * x.apply(std::conj)) * (y * y.apply(std::conj)));
        nf3 += nf1[j];
    }

        auto nf2 =std::sqrt(nf3);
        auto nf =nf2/(double)n;
        cout << "nf3" << nf3 <<endl;
        cout << "nf2" << nf2 <<endl;
        cout << "nf" << nf <<endl;
    Complex x1(-1,0);
    Complex x2(-1,0);
    Complex x3(-1,0);
    Complex y1(-1,0);
    Complex y2(-1,0);
    Complex y3(-1,0);
    int i;
    /***************************************iteration loop**********************************************
         **************************************************************************************************/

    //for(i=0; i<nIter; i++)

                 x = x.apply(std::conj);
         y *= x;
             ifft(y);
             y =std::abs(y);
         y=y/nf;

         for ( i = 0 ; i < n ; i++ ){
                        cout << "y[" << i <<"] =" << y[i] << endl;
                     }
        if (tau.empty())
        {
         tet(y);
         Complex ix=y[0]; //use any, if multiple bitwise identical peaks

         /***********indices of three bins around peak***********/
         Complex ixLow= std::fmod(ix-1-1, n) +1  //one below
         Complex ixMid=ix;
                 Complex ixHigh= std::fmod(ix-1+1, n) +1 //one above

         /**********delay corresponding to the three bins*********/
         Complex tauLow = std::fmod(ixLow -1 + halfN, n) - halfN;
                 Complex tauMid = std::fmod(ixMid -1 + halfN, n) - halfN;
         Complex tauHigh = std::fmod(ixHigh -1 + halfN, n) - halfN;

         /**********crosscorrelation value for the three bins******/
         Complex xcLow =  xcorr(ixLow);
                 Complex xcMid =  xcorr(ixMid);
                 Complex xcHigh = xcorr(ixHigh);

         x1 = tauLow;
                 x2 = tauMid;
                 x3 = tauHigh;
                 y1 = xcLow;
                 y2 = xcMid; 
                 y3 = xcHigh;
        }
        else
            {
         /**********only main peak at first bin is of interest****/
         tauMid =tau;
                 xcMid = xcorr(1);

         if (xcMid > y2){

            /**********improve midpoint***************************/
             if(tauMid > x2){
                //midpoint becomes lower point
            x1 = x2;
                        y1 = y2;
             }
             else{
                 //midpoint becomes upper point
             x3 = x2;
                         y3 = y2;
             }

             x2 = tauMid;
                         y2 = xcMid;
         else if (tauMid < x2){
                 //improve high point
             assert(tauMid >= x1); // bitwise identical is OK
                         assert(tauMid > x1 || xcMid > y1); //expect improvement
                     x1 = tauMid;
                     y1 = xcMid;
             }  
         else if (tauMid > x2){
                      //improve high point
              assert(tauMid <= x3); // bitwise identical is OK                
                      assert((tauMid < x3) || (xcMid > y3)); // expect improvement
                      x3 = tauMid;
                      y3 = xcMid;
             }
         else{
             assert(("?? evaluated for existing tau ??", false));
             }
         }
         /*************Calculate uncertainty (Window Width)********************/
         eIter =abs(x3-x1);

}

整个Matlab程序可以在这里找到:

https://www.dsprelated.com/showcode/288.php

我将非常感谢任何帮助

1 个答案:

答案 0 :(得分:0)

好吧,我的Matlab培训确实很遥远,但假设e = [e eIter]表示你将值附加到e,那么你需要使用vector

在这样的地方定义你的e矢量:你已经在你的代码中完成了这个。也许你复制/粘贴了一些代码,我不知道,它看起来很整洁。

std::vector<double> e;

但是我怀疑会编译,因为你在同一范围内也有Complex e声明......

// then in your loop populate it like this
eIter = abs(x3 - x1);  // (this is your code)
e.push_back(eIter);  // push_back was the method you need

然后你可以像标准的C阵列一样访问e的元素:

e[i]

要执行循环,我使用旧式C ++迭代。我知道有更好的,但自2006年以来我的C ++有点生疏。欢迎评论/编辑改进这一点:

// replace const_iterator by iterator to be able to modify the values
std::vector<double>::const_iterator it;  
for (it = e.begin(); it != e.end(); it++)
{ 
   const double &value = *it; // (remove const to be able to change the value)
   ...