void TempoTapDegara::createViterbiTransitionMatrix() {
// Prepare a transition matrix for Viterbi algorithm: it is a _hopSizeODF x
// _hopSizeODF matrix, where each column i consists of a gaussian centered
// at i, with stddev=8 by default (i.e., when _hopSizeODF=128), and leave
// columns before 28th and after 108th zeroed, as well as the lines before
// 28th and after 108th. Paper: informal tests revealed that stddev parameter
// can vary by a factor of 2 without altering the overall performance of beat
// tracker.
// Generalize values to any ODF sample rate.
_transitionsViterbi.resize(_hopSizeODF);
for (int i=0; i<_hopSizeODF; ++i) {
_transitionsViterbi[i].resize(_hopSizeODF);
}
Real scale = _sampleRateODF / (44100./512);
// each sequent column contains a gaussian shifted by 1 line
vector<Real> gaussian;
gaussianPDF(gaussian, 8*scale, 1.);
int minIndex = floor(28 * scale) - 1; // because 0-th index is 1st column/line
int maxIndex = ceil(108 * scale) - 1;
int gaussianMean = gaussian.size() / 2;
for (int i=minIndex; i<=maxIndex; ++i) {
// gaussian with mean=i, std=8*scale;
for (int j=i-gaussianMean; j<=i+gaussianMean; ++j) {
if (j>=minIndex && j <= maxIndex) {
_transitionsViterbi[i][j] = gaussian[j - (i-gaussianMean)];
}
}
}
}
void TempoTapDegara::computeBeatsDegara(vector <Real>& detections,
const vector<Real>& beatPeriods,
const vector<Real>& beatEndPositions,
vector<Real>& ticks) {
// Implementation of Degara's beat tracking using a probabilitic framework
// (Hidden Markov Model). Tempo estimations throughout the track are assumed
// to be computed from the algorithm by M. Davies.
// avoid zeros to avoid log(0) error in future
for(size_t i=0; i<detections.size(); ++i) {
if (detections[i]==0) {
detections[i] = numeric_limits<Real>::epsilon();
}
}
// Minimum tempo (i.e., maximum period) to be considered
Real periodMax = beatPeriods[argmax(beatPeriods)];
// The number of states of the HMM is determined bt the largest time between
// beats allowed (periodMax + 3 standard deviations). Compute a list of
// inter-beat time intervals corresponding to each state (ignore zero period):
vector<Real> ibi;
Real ibiMax = periodMax + 3 *_sigma_ibi;
ibi.reserve(ceil(ibiMax / _resolutionODF));
for (Real t=_resolutionODF; t<=ibiMax; t+=_resolutionODF) {
ibi.push_back(t);
}
_numberStates = (int) ibi.size();
// Compute transition matrix from the inter-beat-interval distribution
// according to the tempo estimates. Transition matrix is unique for each beat
// period.
map<Real, vector<vector<Real> > > transitionMatrix;
vector<Real> gaussian;
vector<Real> ibiPDF(_numberStates);
gaussianPDF(gaussian, _sigma_ibi, _resolutionODF, 0.01 / _resample);
// Scale down to avoid computational errors,
// * _resolutionODF, as in matlab code, works worse
for (size_t i=0; i<beatPeriods.size(); ++i) {
// no need to recompute if we have seen this beat period before
if (transitionMatrix.count(beatPeriods[i])==0) {
// Shift gaussian vector to be centered at beatPeriods[i] secs which is
// equivalent to round(beatPeriods[i] / _resolutionODF) samples.
int shift = (int) gaussian.size()/2 - round(beatPeriods[i]/_resolutionODF - 1);
for (int j=0; j<_numberStates; ++j) {
int j_new = j + shift;
ibiPDF[j] = j_new < 0 || j_new >= (int) gaussian.size() ? 0 : gaussian[j_new];
}
computeHMMTransitionMatrix(ibiPDF, transitionMatrix[beatPeriods[i]]);
}
}
// Compute observation likelihoods for each HMM state
vector<vector<Real> > biy; // _numberStates x _numberFramesODF
biy.reserve(_numberStates);
// treat ODF as probability, normalize to 0.99 to avoid numerical problems
_numberFrames = detections.size();
vector<Real> beatProbability(_numberFrames);
vector<Real> noBeatProbability(_numberFrames);
for (size_t i=0; i<_numberFrames; ++i) {
beatProbability[i] = 0.99 * detections[i];
noBeatProbability[i] = 1. - beatProbability[i];
// NB: work in log space to avoid numerical issues
beatProbability[i] = (1-_alpha) * log(beatProbability[i]);
noBeatProbability[i] = (1-_alpha) * log(noBeatProbability[i]);
}
biy.push_back(beatProbability);
biy.insert(biy.end(), _numberStates-1, noBeatProbability);
// Decoding
vector<int> stateSequence;
decodeBeats(transitionMatrix, beatPeriods, beatEndPositions, biy, stateSequence);
for (size_t i=0; i<stateSequence.size(); ++i) {
if (stateSequence[i] == 0) { // beat detected
ticks.push_back(i * _resolutionODF);
}
}
}
// sample a new pose from a given command and previous pose
// see Table 5.3 - Probabilistic Robotics
void SampleVelocityModel::samplePose2D(Pose2D *p) {
// auxiliar variables
double v, w, y, v2, w2, dt, vw, *pose;
// get the current pose
pose = p->v;
// iterate over the Velocity Commands
for (int j = 0; j < commands.size() - 1; j++) {
// just to be easy to write and reduce repetitive multiplication
v2 = std::fabs(commands[j].linear);
w2 = std::fabs(commands[j].angular);
// the time betwen the current and the next command
// the last command in this command vector is a hack
// just to obtain the time from the laser and set the last command as
// the first one in the next MCL call (next LaserScan)
dt = (commands[j+1].stamp - commands[j].stamp).toSec();
if (real_world) {
// get the linear velocity
v = commands[j].linear + gaussianPDF(a1*v2 + a2*w2);
// get the angular velocity
w = commands[j].angular + gaussianPDF(a3*v2 + a4*w2);
} else {
// get the linear velocity
v = commands[j].linear*1.2 + gaussianPDF(a1*v2 + a2*w2);
// get the angular velocity
w = commands[j].angular*1.5 + gaussianPDF(a3*v2 + a4*w2);
}
// get the final angle extra noisy angular velocity
y = gaussianPDF(a5*v2 + a6*w2);
// updates the pose based on this current command
// verify if the angular is zero or very close to zero
if (0.0 != commands[j].angular) {
// here we can use the given algorithm directly
vw = v/w;
// get the x distance
pose[0] += - vw*sin(pose[2]) + vw*sin(pose[2] + w*dt);
// get the y distance
pose[1] += (vw*cos(pose[2]) - vw*cos(pose[2] + w*dt));
// get the new angle
pose[2] += w*dt + y*dt;
} else if (0.0 != commands[j].linear) {
// get the x distance
pose[0] += v*cos(pose[2])*dt;
// get the y distance
pose[1] += v*sin(pose[2])*dt;
// get the new angle, just adding some noise
pose[2] += y*dt;
} else {
// just return
return;
}
// angular velocity
// maintain the orientation between -PI/2 and PI/2
pose[2] = mrpt::math::wrapToPi(pose[2]);
}
}
