static long Pitch_getMeanAbsoluteSlope (Pitch me,
double *out_hertz, double *out_mel, double *out_semitones, double *out_erb, double *out_withoutOctaveJumps)
{
long firstVoicedFrame = 0, lastVoicedFrame = 0, nVoiced = 0, i;
double *frequencies = NUMvector <double> (1, my nx);
for (i = 1; i <= my nx; i ++) {
double frequency = my frame [i]. candidate [1]. frequency;
frequencies [i] = frequency > 0.0 && frequency < my ceiling ? frequency : 0.0;
if (frequencies [i]) nVoiced ++;
}
for (i = 1; i <= my nx; i ++) /* Look for first voiced frame. */
if (frequencies [i] != 0.0) { firstVoicedFrame = i; break; }
for (i = my nx; i >= 1; i --) /* Look for last voiced frame. */
if (frequencies [i] != 0.0) { lastVoicedFrame = i; break; }
if (nVoiced > 1) {
int ilast = firstVoicedFrame;
double span = (lastVoicedFrame - firstVoicedFrame) * my dx, flast = frequencies [ilast];
double slopeHz = 0, slopeMel = 0, slopeSemitones = 0, slopeErb = 0, slopeRobust = 0;
for (i = firstVoicedFrame + 1; i <= lastVoicedFrame; i ++) if (frequencies [i] != 0.0) {
double localStepSemitones = fabs (SEMITONES (frequencies [i]) - SEMITONES (flast));
slopeHz += fabs (frequencies [i] - flast);
slopeMel += fabs (MEL (frequencies [i]) - MEL (flast));
slopeSemitones += localStepSemitones;
slopeErb += fabs (ERB (frequencies [i]) - ERB (flast));
while (localStepSemitones >= 12.0) localStepSemitones -= 12.0; /* Kill octave jumps. */
if (localStepSemitones > 6.0) localStepSemitones = 12.0 - localStepSemitones;
slopeRobust += localStepSemitones;
ilast = i;
flast = frequencies [i];
}
if (out_hertz) *out_hertz = slopeHz / span;
if (out_mel) *out_mel = slopeMel / span;
if (out_semitones) *out_semitones = slopeSemitones / span;
if (out_erb) *out_erb = slopeErb / span;
if (out_withoutOctaveJumps) *out_withoutOctaveJumps = slopeRobust / span;
} else {
if (out_hertz) *out_hertz = NUMundefined;
if (out_mel) *out_mel = NUMundefined;
if (out_semitones) *out_semitones = NUMundefined;
if (out_erb) *out_erb = NUMundefined;
if (out_withoutOctaveJumps) *out_withoutOctaveJumps = NUMundefined;
}
NUMvector_free (frequencies, 1);
return nVoiced;
}
void DebugStiffnessKernel::get_per_layer_dynamical_matrices(double qx,
double qy,
double_complex **D,
double *xcshift,
double *ycshift)
{
for (int i = 0; i < 2*nu_+1; i++) {
for (int k = 0; k < ndof_; k++) {
for (int l = 0; l < ndof_; l++) {
MEL(ndof_, D[i], k, l) =
(i*1000.+k*10.+l*1.) + (i*10000.+k*10.+l*1.)*I;
}
}
}
}
/**
* MEX function entry point.
*/
void
mexFunction(
int nlhs,
mxArray *plhs[],
int nrhs,
const mxArray *prhs[]
) {
double *q, *qd, *qdd;
double *tau;
unsigned int m,n;
int j, njoints, p, nq;
double *fext = NULL;
double *grav = NULL;
Robot robot;
mxArray *link0;
mxArray *mx_robot;
mxArray *mx_links;
static int first_time = 0;
/*
fprintf(stderr, "Fast RNE: (c) Peter Corke 2002-2011\n");
*/
if ( !mxIsClass(ROBOT_IN, "SerialLink") )
mexErrMsgTxt("frne: first argument is not a robot structure\n");
mx_robot = (mxArray *)ROBOT_IN;
njoints = mstruct_getint(mx_robot, 0, "n");
/***********************************************************************
* Handle the different calling formats.
* Setup pointers to q, qd and qdd inputs
***********************************************************************/
switch (nrhs) {
case 2:
/*
* TAU = RNE(ROBOT, [Q QD QDD])
*/
if (NUMCOLS(A1_IN) != 3 * njoints)
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
break;
case 3:
/*
* TAU = RNE(ROBOT, [Q QD QDD], GRAV)
*/
if (NUMCOLS(A1_IN) != (3 * njoints))
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
break;
case 4:
/*
* TAU = RNE(ROBOT, Q, QD, QDD)
* TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
*/
if (NUMCOLS(A1_IN) == (3 * njoints)) {
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
if (NUMELS(A3_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A3_IN);
} else {
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
}
break;
case 5: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
break;
}
case 6: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
if (NUMELS(A5_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A5_IN);
break;
}
default:
mexErrMsgTxt("frne: wrong number of arguments.");
}
/*
* fill out the robot structure
*/
robot.njoints = njoints;
if (grav)
robot.gravity = (Vect *)grav;
else
robot.gravity = (Vect *)mxGetPr( mxGetProperty(mx_robot, (mwIndex)0, "gravity") );
robot.dhtype = mstruct_getint(mx_robot, 0, "mdh");
/* build link structure */
robot.links = (Link *)mxCalloc((mwSize) njoints, (mwSize) sizeof(Link));
/***********************************************************************
* Now we have to get pointers to data spread all across a cell-array
* of Matlab structures.
*
* Matlab structure elements can be found by name (slow) or by number (fast).
* We assume that since the link structures are all created by the same
* constructor, the index number for each element will be the same for all
* links. However we make no assumption about the numbers themselves.
***********************************************************************/
/* get pointer to the first link structure */
link0 = mxGetProperty(mx_robot, (mwIndex) 0, "links");
if (link0 == NULL)
mexErrMsgTxt("couldnt find element link in robot object");
/*
* Elements of the link structure are:
*
* alpha:
* A:
* theta:
* D:
* offset:
* sigma:
* mdh:
* m:
* r:
* I:
* Jm:
* G:
* B:
* Tc:
*/
if (first_time == 0) {
mexPrintf("Fast RNE: (c) Peter Corke 2002-2012\n");
first_time = 1;
}
/*
* copy data from the Link objects into the local links structure
* to save function calls later
*/
for (j=0; j<njoints; j++) {
Link *l = &robot.links[j];
mxArray *links = mxGetProperty(mx_robot, (mwIndex) 0, "links"); // links array
l->alpha = mxGetScalar( mxGetProperty(links, (mwIndex) j, "alpha") );
l->A = mxGetScalar( mxGetProperty(links, (mwIndex) j, "a") );
l->theta = mxGetScalar( mxGetProperty(links, (mwIndex) j, "theta") );
l->D = mxGetScalar( mxGetProperty(links, (mwIndex) j, "d") );
l->sigma = mxGetScalar( mxGetProperty(links, (mwIndex) j, "sigma") );
l->offset = mxGetScalar( mxGetProperty(links, (mwIndex) j, "offset") );
l->m = mxGetScalar( mxGetProperty(links, (mwIndex) j, "m") );
l->rbar = (Vect *)mxGetPr( mxGetProperty(links, (mwIndex) j, "r") );
l->I = mxGetPr( mxGetProperty(links, (mwIndex) j, "I") );
l->Jm = mxGetScalar( mxGetProperty(links, (mwIndex) j, "Jm") );
l->G = mxGetScalar( mxGetProperty(links, (mwIndex) j, "G") );
l->B = mxGetScalar( mxGetProperty(links, (mwIndex) j, "B") );
l->Tc = mxGetPr( mxGetProperty(links, (mwIndex) j, "Tc") );
}
/* Create a matrix for the return argument */
TAU_OUT = mxCreateDoubleMatrix((mwSize) nq, (mwSize) njoints, mxREAL);
tau = mxGetPr(TAU_OUT);
#define MEL(x,R,C) (x[(R)+(C)*nq])
/* for each point in the input trajectory */
for (p=0; p<nq; p++) {
/*
* update all position dependent variables
*/
for (j = 0; j < njoints; j++) {
Link *l = &robot.links[j];
switch (l->sigma) {
case REVOLUTE:
rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype);
break;
case PRISMATIC:
rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype);
break;
}
#ifdef DEBUG
rot_print("R", &l->R);
vect_print("p*", &l->r);
#endif
}
newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq);
}
mxFree(robot.links);
}
void structPitch :: v_info () {
long nVoiced;
autoNUMvector <double> frequencies (Sampled_getSortedValues (this, Pitch_LEVEL_FREQUENCY, kPitch_unit_HERTZ, & nVoiced), 1);
structDaata :: v_info ();
MelderInfo_writeLine (U"Time domain:");
MelderInfo_writeLine (U" Start time: ", xmin, U" seconds");
MelderInfo_writeLine (U" End time: ", xmax, U" seconds");
MelderInfo_writeLine (U" Total duration: ", xmax - xmin, U" seconds");
MelderInfo_writeLine (U"Time sampling:");
MelderInfo_writeLine (U" Number of frames: ", nx, U" (", nVoiced, U" voiced)");
MelderInfo_writeLine (U" Time step: ", dx, U" seconds");
MelderInfo_writeLine (U" First frame centred at: ", x1, U" seconds");
MelderInfo_writeLine (U"Ceiling at: ", ceiling, U" Hz");
if (nVoiced >= 1) { // quantiles
double quantile10, quantile16, quantile50, quantile84, quantile90;
quantile10 = NUMquantile (nVoiced, frequencies.peek(), 0.10);
quantile16 = NUMquantile (nVoiced, frequencies.peek(), 0.16);
quantile50 = NUMquantile (nVoiced, frequencies.peek(), 0.50); // median
quantile84 = NUMquantile (nVoiced, frequencies.peek(), 0.84);
quantile90 = NUMquantile (nVoiced, frequencies.peek(), 0.90);
MelderInfo_writeLine (U"\nEstimated quantiles:");
MelderInfo_write (U" 10% = ", Melder_single (quantile10), U" Hz = ", Melder_single (MEL (quantile10)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (quantile10)), U" semitones above 100 Hz = ", Melder_single (ERB (quantile10)), U" ERB");
MelderInfo_write (U" 16% = ", Melder_single (quantile16), U" Hz = ", Melder_single (MEL (quantile16)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (quantile16)), U" semitones above 100 Hz = ", Melder_single (ERB (quantile16)), U" ERB");
MelderInfo_write (U" 50% = ", Melder_single (quantile50), U" Hz = ", Melder_single (MEL (quantile50)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (quantile50)), U" semitones above 100 Hz = ", Melder_single (ERB (quantile50)), U" ERB");
MelderInfo_write (U" 84% = ", Melder_single (quantile84), U" Hz = ", Melder_single (MEL (quantile84)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (quantile84)), U" semitones above 100 Hz = ", Melder_single (ERB (quantile84)), U" ERB");
MelderInfo_write (U" 90% = ", Melder_single (quantile90), U" Hz = ", Melder_single (MEL (quantile90)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (quantile90)), U" semitones above 100 Hz = ", Melder_single (ERB (quantile90)), U" ERB");
if (nVoiced > 1) {
double corr = sqrt (nVoiced / (nVoiced - 1.0));
MelderInfo_writeLine (U"\nEstimated spreading:");
MelderInfo_write (U" 84%-median = ", Melder_half ((quantile84 - quantile50) * corr), U" Hz = ", Melder_half ((MEL (quantile84) - MEL (quantile50)) * corr), U" Mel = ");
MelderInfo_writeLine (Melder_half ((SEMITONES (quantile84) - SEMITONES (quantile50)) * corr), U" semitones = ", Melder_half ((ERB (quantile84) - ERB (quantile50)) * corr), U" ERB");
MelderInfo_write (U" median-16% = ", Melder_half ((quantile50 - quantile16) * corr), U" Hz = ", Melder_half ((MEL (quantile50) - MEL (quantile16)) * corr), U" Mel = ");
MelderInfo_writeLine (Melder_half ((SEMITONES (quantile50) - SEMITONES (quantile16)) * corr), U" semitones = ", Melder_half ((ERB (quantile50) - ERB (quantile16)) * corr), U" ERB");
MelderInfo_write (U" 90%-10% = ", Melder_half ((quantile90 - quantile10) * corr), U" Hz = ", Melder_half ((MEL (quantile90) - MEL (quantile10)) * corr), U" Mel = ");
MelderInfo_writeLine (Melder_half ((SEMITONES (quantile90) - SEMITONES (quantile10)) * corr), U" semitones = ", Melder_half ((ERB (quantile90) - ERB (quantile10)) * corr), U" ERB");
}
}
if (nVoiced >= 1) { // extrema, range, mean and standard deviation
double minimum = Pitch_getMinimum (this, xmin, xmax, kPitch_unit_HERTZ, false);
double maximum = Pitch_getMaximum (this, xmin, xmax, kPitch_unit_HERTZ, false);
double meanHertz, meanMel, meanSemitones, meanErb;
MelderInfo_write (U"\nMinimum ", Melder_single (minimum), U" Hz = ", Melder_single (MEL (minimum)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (minimum)), U" semitones above 100 Hz = ", Melder_single (ERB (minimum)), U" ERB");
MelderInfo_write (U"Maximum ", Melder_single (maximum), U" Hz = ", Melder_single (MEL (maximum)), U" Mel = ");
MelderInfo_writeLine (Melder_single (SEMITONES (maximum)), U" semitones above 100 Hz = ", Melder_single (ERB (maximum)), U" ERB");
MelderInfo_write (U"Range ", Melder_half (maximum - minimum), U" Hz = ", Melder_single (MEL (maximum) - MEL (minimum)), U" Mel = ");
MelderInfo_writeLine (Melder_half (SEMITONES (maximum) - SEMITONES (minimum)), U" semitones = ", Melder_half (ERB (maximum) - ERB (minimum)), U" ERB");
meanHertz = Pitch_getMean (this, 0, 0, kPitch_unit_HERTZ);
meanMel = Pitch_getMean (this, 0, 0, kPitch_unit_MEL);
meanSemitones = Pitch_getMean (this, 0, 0, kPitch_unit_SEMITONES_100);
meanErb = Pitch_getMean (this, 0, 0, kPitch_unit_ERB);
MelderInfo_write (U"Average: ", Melder_single (meanHertz), U" Hz = ", Melder_single (meanMel), U" Mel = ");
MelderInfo_writeLine (Melder_single (meanSemitones), U" semitones above 100 Hz = ", Melder_single (meanErb), U" ERB");
if (nVoiced >= 2) {
double stdevHertz = Pitch_getStandardDeviation (this, 0, 0, kPitch_unit_HERTZ);
double stdevMel = Pitch_getStandardDeviation (this, 0, 0, kPitch_unit_MEL);
double stdevSemitones = Pitch_getStandardDeviation (this, 0, 0, kPitch_unit_SEMITONES_100);
double stdevErb = Pitch_getStandardDeviation (this, 0, 0, kPitch_unit_ERB);
MelderInfo_write (U"Standard deviation: ", Melder_half (stdevHertz), U" Hz = ", Melder_half (stdevMel), U" Mel = ");
MelderInfo_writeLine (Melder_half (stdevSemitones), U" semitones = ", Melder_half (stdevErb), U" ERB");
}
}
if (nVoiced > 1) { // variability: mean absolute slope
double slopeHertz, slopeMel, slopeSemitones, slopeErb, slopeWithoutOctaveJumps;
Pitch_getMeanAbsoluteSlope (this, & slopeHertz, & slopeMel, & slopeSemitones, & slopeErb, & slopeWithoutOctaveJumps);
MelderInfo_write (U"\nMean absolute slope: ", Melder_half (slopeHertz), U" Hz/s = ", Melder_half (slopeMel), U" Mel/s = ");
MelderInfo_writeLine (Melder_half (slopeSemitones), U" semitones/s = ", Melder_half (slopeErb), U" ERB/s");
MelderInfo_writeLine (U"Mean absolute slope without octave jumps: ", Melder_half (slopeWithoutOctaveJumps), U" semitones/s");
}
}
int main(int narg, char *arg[])
{
int iarg, nspec, npts, i;
double *specx, *specy, *specd;
double *ptx, *pty, *ptd;
double_complex mat[MAX_DYN_DIM_SQ];
char *specsym, *stifffn, *gffn;
StiffnessKernel *kernel;
Domain domain;
Error error;
Memory memory(&error);
FILE *f;
/* --- */
domain.xprd = 1.0;
domain.yprd = 1.0;
domain.zprd = 1.0;
if (narg < 4) {
printf("Syntax: eval_stiffness <filename with special points> "
"<stiffness filename> <greens functions filename> "
"<number of band points> <kernel> <kernel parameters>\n");
exit(999);
}
/*
nspec = atoi(arg[1]);
specsym = (char *) malloc(nspec*sizeof(char));
specx = (double *) malloc(nspec*sizeof(double));
specy = (double *) malloc(nspec*sizeof(double));
specd = (double *) malloc(nspec*sizeof(double));
*/
iarg = 1;
f = fopen(arg[iarg], "r");
if (!f) {
printf("Error opening %s.\n", arg[iarg]);
exit(999);
}
iarg++;
fscanf(f, "%i", &nspec);
specsym = (char *) malloc(nspec*SYM_LEN*sizeof(char));
specx = (double *) malloc(nspec*sizeof(double));
specy = (double *) malloc(nspec*sizeof(double));
specd = (double *) malloc(nspec*sizeof(double));
for (i = 0; i < nspec; i++) {
fscanf(f, "%s%lf%lf", &specsym[i*SYM_LEN], &specx[i], &specy[i]);
}
fclose(f);
stifffn = arg[iarg];
iarg++;
gffn = arg[iarg];
iarg++;
npts = atoi(arg[iarg]);
iarg++;
if (npts <= nspec) {
printf("Number of band points must be larger that number of special "
"points.\n");
exit(999);
}
iarg++;
kernel = stiffness_kernel_factory(arg[iarg-1], narg, &iarg, arg,
&domain, &memory, &error);
if (!kernel) {
printf("Could not find stiffness kernel %s.\n", arg[iarg-1]);
exit(999);
}
ptx = (double *) malloc(npts*sizeof(double));
pty = (double *) malloc(npts*sizeof(double));
ptd = (double *) malloc(npts*sizeof(double));
band_lines(nspec, specx, specy, specd, npts, ptx, pty, ptd);
/*
* stiffness.out
*/
f = fopen(stifffn, "w");
if (!f) {
printf("Error opening %s.\n", stifffn);
exit(999);
}
fprintf(f, "# dist qx qy");
int dim = kernel->get_dimension();
i = 4;
for (int k = 0; k < dim; k++)
for (int l = 0; l < dim-k; l++) {
fprintf(f, " %i:mat%i%i", i, l+1, l+k+1);
i += 2;
}
fprintf(f, "\n");
for (i = 0; i < npts; i++) {
kernel->get_stiffness_matrix(2*M_PI*ptx[i], 2*M_PI*pty[i], mat);
fprintf(f, "%e %e %e", ptd[i], ptx[i], pty[i]);
for (int k = 0; k < dim; k++)
for (int l = 0; l < dim-k; l++)
fprintf(f, " %e %e",
creal(MEL(dim, mat, l, l+k)), cimag(MEL(dim, mat, l, l+k)));
fprintf(f, "\n");
}
fclose(f);
/*
* greens_function.out
*/
f = fopen(gffn, "w");
if (!f) {
printf("Error opening %s.\n", gffn);
exit(999);
}
fprintf(f, "# dist qx qy");
dim = kernel->get_dimension();
i = 4;
for (int k = 0; k < dim; k++)
for (int l = 0; l < dim-k; l++) {
fprintf(f, " %i:mat%i%i", i, l+1, l+k+1);
i += 2;
}
fprintf(f, "\n");
for (i = 0; i < npts; i++) {
kernel->get_stiffness_matrix(2*M_PI*ptx[i], 2*M_PI*pty[i], mat);
GaussJordan(dim, mat, &error);
fprintf(f, "%e %e %e", ptd[i], ptx[i], pty[i]);
for (int k = 0; k < dim; k++)
for (int l = 0; l < dim-k; l++)
fprintf(f, " %e %e",
creal(MEL(dim, mat, l, l+k)), cimag(MEL(dim, mat, l, l+k)));
fprintf(f, "\n");
}
fclose(f);
f = fopen("specpoints.out", "w");
fprintf(f, "# symbol dist qx qy\n");
for (i = 0; i < nspec; i++) {
// fprintf(f, "%c %e %e %e\n", specsym[i], specd[i], specx[i], specy[i]);
fprintf(f, "%s %e %e %e\n", &specsym[i*SYM_LEN], specd[i], specx[i],
specy[i]);
}
fclose(f);
free(specsym);
free(specx);
free(specy);
free(specd);
free(ptx);
free(pty);
free(ptd);
}