Issue when Re-writing Matlab code to C++ - c++

Question

I am struggling since a couple of months to re-write Matlab code into C++
The bigger part of the work as been already done (fft, ifft, xcorr and binFreq functions have been already written in c++), now I am stuck to one function. the function is supposed to estimate delay between two signals S1 and S2. The function relies on FFT-based cross-correlation.
I am stuck in this line " e = [e, eIter];" where e is a vector with variable length ( e was declared earlier as : "e =[]";)
original function in 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++ equivalent code I have written so far :
//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);
}
The entire Matlab program can be found here :
https://www.dsprelated.com/showcode/288.php
I will be very grateful for any help

Answer 1

Well, my Matlab training is really far away in time, but assuming that e = [e eIter] means that you append a value to e, then you need use a vector
define your e vector somewhere like this: you already done that in your code. Maybe you copied/pasted some code, I don't know, it looks rather neat.
std::vector<double> e;
However I doubt that will compile since you also have a Complex e declaration in the same scope...
// 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
Then you can access elements of e just like a standard C-array:
e[i]
To perform a loop, I use an old-style C++ iteration. I know there is better but my C++ is a bit rusty since 2006. Comments/edits are welcome to improve this:
// 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)
...