cml/libcm
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
1931bfdd6d
libcm/cmProc.c

5243 lines
133 KiB
C
Raw Normal View History

Initial commit
2012-10-30 03:52:39 +00:00
#include "cmPrefix.h"
#include "cmGlobal.h"
#include "cmRpt.h"
#include "cmErr.h"
#include "cmCtx.h"
#include "cmMem.h"
#include "cmMallocDebug.h"
#include "cmLinkedHeap.h"
#include "cmSymTbl.h"
#include "cmFloatTypes.h"
#include "cmComplexTypes.h"
#include "cmFileSys.h"
#include "cmProcObj.h"
#include "cmProcTemplate.h"
#include "cmAudioFile.h"
#include "cmMath.h"
#include "cmProc.h"
#include "cmVectOps.h"
#include "cmKeyboard.h"
#include "cmGnuPlot.h"
#include <time.h> // time()
//------------------------------------------------------------------------------------------------------------
void cmFloatPointExceptHandler( int signo, siginfo_t* info, void* context )
{
char* cp = "<Type Unknown>";
switch( info->si_code )
{
case FPE_INTDIV: cp = "integer divide by zero"; break;
case FPE_INTOVF: cp = "integer overflow"; break;
case FPE_FLTDIV: cp = "divide by zero"; break;
case FPE_FLTUND: cp = "underflow"; break;
case FPE_FLTRES: cp = "inexact result"; break;
case FPE_FLTINV: cp = "invalid operation"; break;
case FPE_FLTSUB: cp = "subscript range error"; break;
}
fprintf(stderr,"Floating point exception: Type: %s\n",cp);
exit(1);
}
// set 'orgSaPtr' to NULL to discard the current signal action state
void cmSetupFloatPointExceptHandler( struct sigaction* orgSaPtr )
{
struct sigaction sa;
sa.sa_handler = SIG_DFL;
sa.sa_flags = SA_SIGINFO;
sa.sa_sigaction = cmFloatPointExceptHandler;
sigemptyset(&sa.sa_mask);
// set all FP except flags excetp: FE_INEXACT
#ifdef OS_OSX
// we don't yet support FP exceptions on OSX
// for an example of how to make this work with the linux interface as below
// see: http://www-personal.umich.edu/~williams/archive/computation/fe-handling-example.c
assert(0);
#else
// int flags = FE_DIVBYZERO | FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
// feenableexcept(flags);
assert(0);
#endif
sigaction( SIGFPE, &sa, orgSaPtr );
}
//------------------------------------------------------------------------------------------------------------
cmAudioFileRd* cmAudioFileRdAlloc( cmCtx* c, cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
{
cmAudioFileRd* op = cmObjAlloc( cmAudioFileRd, c, p );
if( fn != NULL )
if( cmAudioFileRdOpen( op, procSmpCnt, fn, chIdx, begFrmIdx, endFrmIdx ) != cmOkRC )
cmAudioFileRdFree(&op);
return op;
}
cmRC_t cmAudioFileRdFree( cmAudioFileRd** pp )
{
cmRC_t rc = cmOkRC;
if( pp != NULL && *pp != NULL )
{
cmAudioFileRd* p = *pp;
if((rc = cmAudioFileRdClose(p)) == cmOkRC )
{
cmMemPtrFree(&p->outV);
cmMemPtrFree(&p->fn);
cmObjFree(pp);
}
}
return rc;
}
cmRC_t cmAudioFileRdOpen( cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
{
cmRC_t rc;
cmRC_t afRC;
if((rc = cmAudioFileRdClose(p)) != cmOkRC )
return rc;
p->h = cmAudioFileNewOpen( fn, &p->info, &afRC, p->obj.err.rpt );
if( afRC != kOkAfRC )
return cmCtxRtCondition( &p->obj, afRC, "Unable to open the audio file:'%s'", fn );
p->chIdx = chIdx;
cmProc.c : Fix bug where endFrmIdx==cmInvalidIdx is not properly handled in cmAudioFileRdOpen()
2014-04-23 21:43:54 +00:00
p->outN = endFrmIdx==cmInvalidIdx ? p->info.frameCnt : procSmpCnt;
Initial commit
2012-10-30 03:52:39 +00:00
p->outV = cmMemResizeZ( cmSample_t, p->outV, p->outN );
p->fn = cmMemResizeZ( cmChar_t, p->fn, strlen(fn)+1 );
strcpy(p->fn,fn);
//p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"audioFile");
p->lastReadFrmCnt = 0;
p->eofFl = false;
p->begFrmIdx = begFrmIdx;
cmProc.h/c: cmAudioFileRd now recognizes 'endSmpIdx' == 0 as an indication to read 'procSmpCnt' blocks until the end-of-file is reached.
2014-08-10 20:00:17 +00:00
p->endFrmIdx = endFrmIdx==0 ? p->info.frameCnt : endFrmIdx;
Initial commit
2012-10-30 03:52:39 +00:00
p->curFrmIdx = p->begFrmIdx;
if( p->begFrmIdx > 0 )
rc = cmAudioFileRdSeek(p,p->begFrmIdx);
return rc;
}
cmRC_t cmAudioFileRdClose( cmAudioFileRd* p )
{
cmRC_t rc = cmOkRC;
cmRC_t afRC;
if( p == NULL )
return cmOkRC;
//cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
if( cmAudioFileIsOpen(p->h) == false )
return cmOkRC;
if((afRC = cmAudioFileDelete(&p->h)) != cmOkRC )
rc = cmCtxRtCondition( &p->obj, afRC, "An attempt to close the audio file'%s' failed.", p->fn );
return rc;
}
cmRC_t cmAudioFileRdRead( cmAudioFileRd* p )
{
cmRC_t rc = cmOkRC;
cmRC_t afRC;
if(p->eofFl || ((p->eofFl = cmAudioFileIsEOF(p->h)) == true) )
return cmEofRC;
unsigned n = p->endFrmIdx==cmInvalidIdx ? p->outN : cmMin( p->outN, p->endFrmIdx - p->curFrmIdx );
if((afRC = cmAudioFileReadSample( p->h, n, p->chIdx, 1, &p->outV, &p->lastReadFrmCnt )) != kOkAfRC )
rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
p->curFrmIdx += p->lastReadFrmCnt;
if( n < p->outN )
{
cmVOS_Zero(p->outV + p->lastReadFrmCnt, p->outN - p->lastReadFrmCnt);
p->eofFl = true;
}
if( p->mfp != NULL )
cmMtxFileSmpExec( p->mfp, p->outV, p->outN );
return rc;
}
cmRC_t cmAudioFileRdSeek( cmAudioFileRd* p, unsigned frmIdx )
{
cmRC_t rc = cmOkRC;
cmRC_t afRC;
if((afRC = cmAudioFileSeek( p->h, frmIdx )) != kOkAfRC )
rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
return rc;
}
cmRC_t cmAudioFileRdMinMaxMean( cmAudioFileRd* p, unsigned chIdx, cmSample_t* minPtr, cmSample_t* maxPtr, cmSample_t* meanPtr )
{
cmRC_t rc = cmOkRC;
cmRC_t afRC;
if(( afRC = cmAudioFileMinMaxMean( p->h, chIdx, minPtr, maxPtr, meanPtr )) != kOkAfRC )
rc = cmCtxRtCondition( &p->obj, afRC, "Audio file min, max, and mean calculation failed on '%s'", p->fn );
return rc;
}
//------------------------------------------------------------------------------------------------------------
cmShiftBuf* cmShiftBufAlloc( cmCtx* c, cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
{
cmShiftBuf* op = cmObjAlloc( cmShiftBuf, c, p );
if( procSmpCnt > 0 && wndSmpCnt > 0 && hopSmpCnt > 0 )
if( cmShiftBufInit(op, procSmpCnt, wndSmpCnt, hopSmpCnt ) != cmOkRC)
cmShiftBufFree(&op);
return op;
}
cmRC_t cmShiftBufFree( cmShiftBuf** pp )
{
cmRC_t rc = cmOkRC;
if( pp != NULL && *pp != NULL )
{
if((rc = cmShiftBufFinal(*pp)) == cmOkRC )
{
cmMemPtrFree(&(*pp)->bufV);
cmObjFree(pp);
}
}
return rc;
}
cmRC_t cmShiftBufInit( cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
{
cmRC_t rc;
if( hopSmpCnt > wndSmpCnt )
return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The window sample count (%i) must be greater than or equal to the hop sample count (%i).", wndSmpCnt, hopSmpCnt );
if((rc = cmShiftBufFinal(p)) != cmOkRC )
return rc;
// The worst case storage requirement is where there are wndSmpCnt-1 samples in outV[] and procSmpCnt new samples arrive.
p->bufSmpCnt = wndSmpCnt + procSmpCnt;
p->bufV = cmMemResizeZ( cmSample_t, p->outV, p->bufSmpCnt );
p->outV = p->bufV;
p->outN = wndSmpCnt;
p->wndSmpCnt = wndSmpCnt;
p->procSmpCnt = procSmpCnt;
p->hopSmpCnt = hopSmpCnt;
p->inPtr = p->outV;
p->fl = false;
return cmOkRC;
}
cmRC_t cmShiftBufFinal( cmShiftBuf* p )
{
return cmOkRC;
}
// This function should be called in a loop until it returns false.
// Note that 'sp' and 'sn' are ignored except p->fl == false.
bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
{
assert( sn <= p->procSmpCnt );
// The active samples are in outV[wndSmpCnt]
// Stored samples are between outV + wndSmpCnt and inPtr.
// if the previous call to this function returned true then the buffer must be
// shifted by hopSmpCnt samples - AND sp[] is ignored.
if( p->fl )
{
// shift the output buffer to the left to remove expired samples
p->outV += p->hopSmpCnt;
// if there are not wndSmpCnt samples left in the buffer
if( p->inPtr - p->outV < p->wndSmpCnt )
{
// then copy the remaining active samples (between outV and inPtr)
// to the base of the physicalbuffer
unsigned n = p->inPtr - p->outV;
memmove( p->bufV, p->outV, n * sizeof(cmSample_t));
p->inPtr = p->bufV + n; // update the input and output positions
p->outV = p->bufV;
}
}
else
{
// if the previous call to this function returned false then sp[sn] should not be ignored
assert( p->inPtr + sn <= p->outV + p->bufSmpCnt );
// copy the incoming samples into the buffer
cmVOS_Copy(p->inPtr,sn,sp);
p->inPtr += sn;
}
// if there are at least wndSmpCnt available samples in outV[]
p->fl = p->inPtr - p->outV >= p->wndSmpCnt;
return p->fl;
}
void cmShiftBufTest( cmCtx* c )
{
unsigned smpCnt = 48;
unsigned procSmpCnt = 5;
unsigned hopSmpCnt = 6;
unsigned wndSmpCnt = 7;
unsigned i;
cmShiftBuf* b = cmShiftBufAlloc(c,NULL,procSmpCnt,wndSmpCnt,hopSmpCnt );
cmSample_t x[ smpCnt ];
cmVOS_Seq(x,smpCnt,1,1);
//cmVOS_Print( rptFuncPtr, 1, smpCnt, x );
for(i=0; i<smpCnt; i+=procSmpCnt)
{
while( cmShiftBufExec( b, x + i, procSmpCnt ) )
{
cmVOS_Print( c->obj.err.rpt, 1, wndSmpCnt, b->outV );
}
}
cmShiftBufFree(&b);
}
/*
bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
{
bool retFl = false;
if( sn > p->procSmpCnt )
{
cmCtxRtAssertFailed( p->obj.ctx, cmArgAssertRC, "The input sample count (%i) must be less than or equal to the proc sample count (%i).", sn, p->procSmpCnt);
return false;
}
assert( sn <= p->procSmpCnt );
cmSample_t* dbp = p->outV;
cmSample_t* dep = p->outV + (p->outN - sn);
cmSample_t* sbp = p->outV + sn;
// shift the last bufCnt-shiftCnt samples over the first shiftCnt samples
while( dbp < dep )
*dbp++ = *sbp++;
// copy in the new samples
dbp = dep;
dep = dbp + sn;
while( dbp < dep )
*dbp++ = *sp++;
// if any space remains at the end of the buffer then zero it
dep = p->outV + p->outN;
while( dbp < dep )
*dbp++ = 0;
if( p->firstPtr > p->outV )
p->firstPtr = cmMax( p->outV, p->firstPtr - p->procSmpCnt);
p->curHopSmpCnt += sn;
if( p->curHopSmpCnt >= p->hopSmpCnt )
{
p->curHopSmpCnt -= p->hopSmpCnt;
retFl = true;
}
if( p->mfp != NULL )
cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
return retFl;
}
*/
//------------------------------------------------------------------------------------------------------------
cmWndFunc* cmWndFuncAlloc( cmCtx* c, cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kaiserSideLobeRejectDb )
{
cmWndFunc* op = cmObjAlloc( cmWndFunc, c, p );
if( wndId != kInvalidWndId )
if( cmWndFuncInit(op,wndId,wndSmpCnt,kaiserSideLobeRejectDb ) != cmOkRC )
cmWndFuncFree(&op);
return op;
}
cmRC_t cmWndFuncFree( cmWndFunc** pp )
{
cmRC_t rc = cmOkRC;
if( pp != NULL && *pp != NULL )
{
cmWndFunc* p = *pp;
if((rc = cmWndFuncFinal(p)) == cmOkRC )
{
cmMemPtrFree(&p->wndV);
cmMemPtrFree(&p->outV);
cmObjFree(pp);
}
}
return rc;
}
cmRC_t cmWndFuncInit( cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kslRejectDb )
{
cmRC_t rc;
if( wndId == (p->wndId | p->flags) && wndSmpCnt == p->outN && kslRejectDb == p->kslRejectDb )
return cmOkRC;
if((rc = cmWndFuncFinal(p)) != cmOkRC )
return rc;
p->wndV = cmMemResize( cmSample_t, p->wndV, wndSmpCnt );
p->outV = cmMemResize( cmSample_t, p->outV, wndSmpCnt );
p->outN = wndSmpCnt;
p->wndId = wndId;
p->kslRejectDb = kslRejectDb;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"wndFunc");
p->flags = wndId & (~kWndIdMask);
switch( wndId & kWndIdMask )
{
case kHannWndId: cmVOS_Hann( p->wndV, p->outN ); break;
case kHannMatlabWndId: cmVOS_HannMatlab( p->wndV, p->outN ); break;
case kHammingWndId: cmVOS_Hamming( p->wndV, p->outN ); break;
case kTriangleWndId: cmVOS_Triangle( p->wndV, p->outN ); break;
case kUnityWndId: cmVOS_Fill( p->wndV, p->outN, 1.0 ); break;
case kKaiserWndId:
{
double beta = cmVOS_KaiserBetaFromSidelobeReject(fabs(kslRejectDb));
cmVOS_Kaiser( p->wndV,p->outN, beta);
}
break;
case kInvalidWndId: break;
default:
{ assert(0); }
}
cmSample_t den = 0;
cmSample_t num = 1;
if( cmIsFlag(p->flags,kNormBySumWndFl) )
{
den = cmVOS_Sum(p->wndV, p->outN);
num = wndSmpCnt;
}
if( cmIsFlag(p->flags,kNormByLengthWndFl) )
den += wndSmpCnt;
if( den > 0 )
{
cmVOS_MultVS(p->wndV,p->outN,num);
cmVOS_DivVS(p->wndV,p->outN,den);
}
return cmOkRC;
}
cmRC_t cmWndFuncFinal( cmWndFunc* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmWndFuncExec( cmWndFunc* p, const cmSample_t* sp, unsigned sn )
{
if( sn > p->outN )
return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The length of the input vector (%i) is greater thean the length of the window function (%i).", sn, p->outN );
if( p->wndId != kInvalidWndId )
cmVOS_MultVVV( p->outV, sn, sp, p->wndV );
if( p->mfp != NULL )
cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
return cmOkRC;
}
void cmWndFuncTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
unsigned wndCnt = 5;
double kaiserSideLobeRejectDb = 30;
cmCtx c;
cmCtxAlloc(&c,rpt,lhH,stH);
cmWndFunc* p = cmWndFuncAlloc(&c,NULL,kHannWndId,wndCnt, 0 );
cmVOS_Print( rpt, 1, wndCnt, p->wndV );
cmWndFuncInit(p,kHammingWndId ,wndCnt, 0 );
cmVOS_Print( rpt, 1, wndCnt, p->wndV );
cmWndFuncInit(p,kTriangleWndId ,wndCnt, 0 );
cmVOS_Print( rpt, 1, wndCnt, p->wndV );
cmWndFuncInit(p,kKaiserWndId ,wndCnt, kaiserSideLobeRejectDb );
cmVOS_Print( rpt, 1, wndCnt, p->wndV );
cmSample_t wV[ wndCnt ];
cmVOS_HannMatlab(wV,wndCnt);
cmVOS_Print( rpt, 1, wndCnt, wV);
cmWndFuncFree(&p);
}
//------------------------------------------------------------------------------------------------------------
cmSpecDelay* cmSpecDelayAlloc( cmCtx* c, cmSpecDelay* ap, unsigned maxDelayCnt, unsigned binCnt )
{
cmSpecDelay* p = cmObjAlloc( cmSpecDelay, c, ap );
if( maxDelayCnt > 0 && binCnt > 0 )
if( cmSpecDelayInit(p,maxDelayCnt,binCnt) != cmOkRC )
cmSpecDelayFree(&p);
return p;
}
cmRC_t cmSpecDelayFree( cmSpecDelay** pp )
{
cmRC_t rc = cmOkRC;
if( pp != NULL && *pp != NULL )
{
cmSpecDelay* p = *pp;
if((rc=cmSpecDelayFinal(p)) == cmOkRC )
{
cmMemPtrFree(&p->bufPtr);
cmObjFree(pp);
}
}
return rc;
}
cmRC_t cmSpecDelayInit( cmSpecDelay* p, unsigned maxDelayCnt, unsigned binCnt )
{
cmRC_t rc;
if((rc = cmSpecDelayFinal(p)) != cmOkRC )
return rc;
p->bufPtr = cmMemResizeZ( cmSample_t, p->bufPtr, binCnt * maxDelayCnt );
p->maxDelayCnt = maxDelayCnt;
p->outN = binCnt;
p->inIdx = 0;
return cmOkRC;
}
cmRC_t cmSpecDelayFinal(cmSpecDelay* p )
{ return cmOkRC; }
cmRC_t cmSpecDelayExec( cmSpecDelay* p, const cmSample_t* sp, unsigned sn )
{
cmSample_t* dp = p->bufPtr + (p->inIdx * p->outN);
cmVOS_Copy( dp, cmMin(sn,p->outN), sp);
p->inIdx = (p->inIdx+1) % p->maxDelayCnt;
return cmOkRC;
}
const cmSample_t* cmSpecDelayOutPtr( cmSpecDelay* p, unsigned delayCnt )
{
assert( delayCnt < p->maxDelayCnt );
int i = p->inIdx - delayCnt;
if( i < 0 )
i = p->maxDelayCnt + i;
return p->bufPtr + (i * p->outN);
}
//------------------------------------------------------------------------------------------------------------
cmFilter* cmFilterAlloc( cmCtx* c, cmFilter* ap, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
{
cmRC_t rc;
cmFilter* p = cmObjAlloc(cmFilter,c,ap);
if( (bn > 0 || an > 0) && procSmpCnt > 0 )
if( (rc = cmFilterInit( p, b, bn, a, an, procSmpCnt, d)) != cmOkRC )
cmFilterFree(&p);
return p;
}
cmFilter* cmFilterAllocEllip( cmCtx* c, cmFilter* ap, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
{
cmRC_t rc;
cmFilter* p = cmObjAlloc(cmFilter,c,ap);
if( srate > 0 && passHz > 0 && procSmpCnt > 0 )
if( (rc = cmFilterInitEllip( p, srate, passHz, stopHz, passDb, stopDb, procSmpCnt, d)) != cmOkRC )
cmFilterFree(&p);
return p;
}
cmRC_t cmFilterFree( cmFilter** pp )
{
cmRC_t rc = cmOkRC;
if( pp != NULL && *pp != NULL )
{
cmFilter* p = *pp;
if((rc = cmFilterFinal(p)) == cmOkRC )
{
cmMemPtrFree(&p->a);
cmMemPtrFree(&p->b);
cmMemPtrFree(&p->d);
cmMemPtrFree(&p->outSmpV);
cmObjFree(pp);
}
}
return rc;
}
cmRC_t cmFilterInit( cmFilter* p, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
{
assert( bn >= 1 );
assert( an >= 1 && a[0] != 0 );
cmRC_t rc;
if((rc = cmFilterFinal(p)) != cmOkRC )
return rc;
int cn = cmMax(an,bn) - 1;
// The output vector may be used as either cmReal_t or cmSample_t.
// Find the larger of the two possible types.
if( sizeof(cmReal_t) > sizeof(cmSample_t) )
{
p->outRealV = cmMemResizeZ( cmReal_t, p->outRealV, procSmpCnt );
p->outSmpV = (cmSample_t*)p->outRealV;
}
else
{
p->outSmpV = cmMemResizeZ( cmSample_t, p->outSmpV, procSmpCnt );
p->outRealV = (cmReal_t*)p->outRealV;
}
p->a = cmMemResizeZ( cmReal_t, p->a, cn );
p->b = cmMemResizeZ( cmReal_t, p->b, cn );
p->d = cmMemResizeZ( cmReal_t, p->d, cn+1 );
//p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
p->outN = procSmpCnt;
p->an = an;
p->bn = bn;
p->cn = cn;
p->di = 0;
p->b0 = b[0] / a[0];
int i;
for(i=0; i<an-1; ++i)
p->a[i] = a[i+1] / a[0];
for(i=0; i<bn-1; ++i)
p->b[i] = b[i+1] / a[0];
if( d != NULL )
cmVOR_Copy(p->d,cn,d);
return cmOkRC;
}
// initialize an elliptic lowpass filter with the given characteristics
// ref: Parks & Burrus, Digital Filter Design, sec. 7.2.7 - 7.2.8
cmRC_t cmFilterInitEllip( cmFilter* p, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
{
assert( srate > 0 );
assert( passHz > 0 && stopHz > passHz && srate/2 > stopHz );
cmReal_t Wp, Ws, ep, v0,
k, kc, k1, k1c,
K, Kc, K1, K1c,
sn, cn, dn,
sm, cm, dm,
zr, zi, pr, pi;
unsigned N, L, j;
// prewarp Wp and Ws, calculate k
Wp = 2 * srate * tan(M_PI * passHz / srate);
Ws = 2 * srate * tan(M_PI * stopHz / srate);
k = Wp / Ws;
// calculate ep and k1 from passDb and stopDb
ep = sqrt(pow(10, passDb/10) - 1);
k1 = ep / sqrt(pow(10, stopDb/10) - 1);
// calculate complimentary moduli
kc = sqrt(1-k*k);
k1c = sqrt(1-k1*k1);
// calculate complete elliptic integrals
K = cmEllipK( kc );
Kc = cmEllipK( k );
K1 = cmEllipK( k1c );
K1c = cmEllipK( k1 );
// calculate minimum integer filter order N
N = ceil(K*K1c/Kc/K1);
// recalculate k and kc from chosen N
// Ws is minimized while other specs held constant
k = cmEllipDeg( K1c/K1/N );
kc = sqrt(1-k*k);
K = cmEllipK( kc );
Kc = cmEllipK( k );
Ws = Wp / k;
// initialize temporary coefficient arrays
cmReal_t b[N+1], a[N+1];
a[0] = b[0] = 1;
memset(b+1, 0, N*sizeof(cmReal_t));
memset(a+1, 0, N*sizeof(cmReal_t));
// intermediate value needed for determining poles
v0 = K/K1/N * cmEllipArcSc( 1/ep, k1 );
cmEllipJ( v0, k, &sm, &cm, &dm );
for( L=1-N%2; L<N; L+=2 )
{
// find the next pole and zero on s-plane
cmEllipJ( K*L/N, kc, &sn, &cn, &dn );
zr = 0;
zi = L ? Ws/sn : 1E25;
pr = -Wp*sm*cm*cn*dn/(1-pow(dn*sm,2));
pi = Wp*dm*sn/(1-pow(dn*sm,2));
// convert pole and zero to z-plane using bilinear transform
cmBlt( 1, srate, &zr, &zi );
cmBlt( 1, srate, &pr, &pi );
if( L == 0 )
{
// first order section
b[1] = -zr;
a[1] = -pr;
}
else
{
// replace complex root and its conjugate with 2nd order section
zi = zr*zr + zi*zi;
zr *= -2;
pi = pr*pr + pi*pi;
pr *= -2;
// combine with previous sections to obtain filter coefficients
for( j = L+1; j >= 2; j-- )
{
b[j] = b[j] + zr*b[j-1] + zi*b[j-2];
a[j] = a[j] + pr*a[j-1] + pi*a[j-2];
}
b[1] += zr;
a[1] += pr;
}
}
// scale b coefficients s.t. DC gain is 0 dB
cmReal_t sumB = 0, sumA = 0;
for( j = 0; j < N+1; j++ )
{
sumB += b[j];
sumA += a[j];
}
sumA /= sumB;
for( j = 0; j < N+1; j++ )
b[j] *= sumA;
return cmFilterInit( p, b, N+1, a, N+1, procSmpCnt, d );
}
cmRC_t cmFilterFinal( cmFilter* p )
{ return cmOkRC; }
cmRC_t cmFilterExecS( cmFilter* p, const cmSample_t* x, unsigned xn, cmSample_t* yy, unsigned yn )
{
cmSample_t* y;
if( yy == NULL || yn==0 )
{
y = p->outSmpV;
yn = p->outN;
}
else
{
y = yy;
}
cmVOS_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
return cmOkRC;
/*
int i,j;
cmSample_t y0 = 0;
cmSample_t* y;
unsigned n;
if( yy == NULL || yn==0 )
{
n = cmMin(p->outN,xn);
y = p->outSmpV;
yn = p->outN;
}
else
{
n = cmMin(yn,xn);
y = yy;
}
// This is a direct form II algorithm based on the MATLAB implmentation
// http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html#f83-1015962
for(i=0; i<n; ++i)
{
//cmSample_t x0 = x[i];
y[i] = (x[i] * p->b0) + p->d[0];
y0 = y[i];
for(j=0; j<p->cn; ++j)
p->d[j] = (p->b[j] * x[i]) - (p->a[j] * y0) + p->d[j+1];
}
// if fewer input samples than output samples - zero the end of the output buffer
if( yn > xn )
cmVOS_Fill(y+i,yn-i,0);
return cmOkRC;
*/
}
cmRC_t cmFilterExecR( cmFilter* p, const cmReal_t* x, unsigned xn, cmReal_t* yy, unsigned yn )
{
cmReal_t* y;
if( yy == NULL || yn==0 )
{
y = p->outRealV;
yn = p->outN;
}
else
{
//n = cmMin(yn,xn);
y = yy;
}
cmVOR_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
return cmOkRC;
}
cmRC_t cmFilterSignal( cmCtx* c, const cmReal_t b[], unsigned bn, const cmReal_t a[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
{
int procSmpCnt = cmMin(1024,xn);
cmFilter* p = cmFilterAlloc(c,NULL,b,bn,a,an,procSmpCnt,NULL);
int i,n;
for(i=0; i<xn && i<yn; i+=n)
{
n = cmMin(procSmpCnt,cmMin(yn-i,xn-i));
cmFilterExecS(p,x+i,n,y+i,n);
}
if( i < yn )
cmVOS_Fill(y+i,yn-i,0);
cmFilterFree(&p);
return cmOkRC;
}
cmRC_t cmFilterFilterS(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
{
cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
cmVOS_FilterFilter( f, cmFilterExecS, bb,bn,aa,an,x,xn,y,yn);
cmFilterFree(&f);
return cmOkRC;
}
cmRC_t cmFilterFilterR(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmReal_t* x, unsigned xn, cmReal_t* y, unsigned yn )
{
cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
cmVOR_FilterFilter( f, cmFilterExecR, bb,bn,aa,an,x,xn,y,yn);
cmFilterFree(&f);
return cmOkRC;
}
void cmFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
cmCtx c;
cmCtxAlloc(&c, rpt, lhH, stH );
cmReal_t b[] = { 0.16, 0.32, 0.16 };
unsigned bn = sizeof(b)/sizeof(cmReal_t);
cmReal_t a[] = {1 , -.5949, .2348 };
unsigned an = sizeof(a)/sizeof(cmReal_t);
cmReal_t x[] = { 1,0,0,0,1,0,0,0 };
unsigned xn = sizeof(x)/sizeof(cmReal_t);
cmReal_t d[] = { .5, -.25};
// 0.1600 0.4152 0.3694 0.1223 0.1460 0.3781 0.3507 0.1198
// -0.0111 -0.0281
cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn,d);
cmFilterExecR(p,x,xn,NULL,0);
cmVOR_Print( rpt, 1, xn, p->outRealV );
cmVOR_Print( rpt, 1, p->cn, p->d );
cmFilterFree(&p);
cmObjFreeStatic( cmCtxFree, cmCtx, c );
/*
cmReal_t b[] = { 0.16, 0.32, 0.16 };
unsigned bn = sizeof(b)/sizeof(cmReal_t);
cmReal_t a[] = { 1, -.5949, .2348};
unsigned an = sizeof(a)/sizeof(cmReal_t);
cmSample_t x[] = { 1,0,0,0,0,0,0,0,0,0 };
unsigned xn = sizeof(x)/sizeof(cmSample_t);
cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn);
cmFilterExec(&c,p,x,xn,NULL,0);
cmVOS_Print( vReportFunc, 1, xn, p->outV );
cmVOR_Print( vReportFunc, 1, p->cn, p->d );
cmFilterExec(&c,p,x,xn,NULL,0);
cmVOS_Print( vReportFunc, 1, xn, p->outV );
cmFilterFree(&p);
*/
}
void cmFilterFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
cmCtx c;
cmCtxAlloc(&c, rpt, lhH, stH );
cmReal_t b[] = { 0.36, 0.32, 0.36 };
unsigned bn = sizeof(b)/sizeof(cmReal_t);
cmReal_t a[] = {1 , -.5949, .2348 };
unsigned an = sizeof(a)/sizeof(cmReal_t);
cmReal_t x[] = { 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0 };
unsigned xn = sizeof(x)/sizeof(cmReal_t);
unsigned yn = xn;
cmReal_t y[yn];
memset(y,0,sizeof(y));
cmFilterFilterR(&c, b,bn,a,an,x,xn,y,yn );
cmVOR_Print( rpt, 1, yn, y );
cmObjFreeStatic( cmCtxFree, cmCtx, c );
}
//------------------------------------------------------------------------------------------------------------
cmComplexDetect* cmComplexDetectAlloc(cmCtx* c, cmComplexDetect* p, unsigned binCnt )
{
cmComplexDetect* op = cmObjAlloc( cmComplexDetect, c, p );
cmSpecDelayAlloc(c,&op->phsDelay,0,0);
cmSpecDelayAlloc(c,&op->magDelay,0,0);
if( binCnt > 0 )
if( cmComplexDetectInit(op,binCnt) != cmOkRC && p == NULL )
cmComplexDetectFree(&op);
return op;
}
cmRC_t cmComplexDetectFree( cmComplexDetect** pp )
{
cmRC_t rc;
if( pp != NULL && *pp != NULL )
{
cmComplexDetect* p = *pp;
if((rc = cmComplexDetectFinal(p)) == cmOkRC )
{
cmSpecDelay* sdp;
sdp = &p->phsDelay;
cmSpecDelayFree(&sdp);
sdp = &p->magDelay;
cmSpecDelayFree(&sdp);
cmObjFree(pp);
}
}
return cmOkRC;
}
cmRC_t cmComplexDetectInit( cmComplexDetect* p, unsigned binCnt )
{
cmRC_t rc;
if((rc = cmComplexDetectFinal(p)) != cmOkRC )
return rc;
cmSpecDelayInit(&p->phsDelay,2,binCnt);
cmSpecDelayInit(&p->magDelay,1,binCnt);
p->binCnt = binCnt;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexDetect");
//p->cdfSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kCDF_FId, 1 );
return cmOkRC;
}
cmRC_t cmComplexDetectFinal( cmComplexDetect* p)
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmComplexDetectExec( cmComplexDetect* p, const cmSample_t* magV, const cmSample_t* phsV, unsigned binCnt )
{
p->out = cmVOS_ComplexDetect( magV, cmSpecDelayOutPtr(&p->magDelay,0), phsV, cmSpecDelayOutPtr(&p->phsDelay,1), cmSpecDelayOutPtr(&p->phsDelay,0), binCnt);
p->out /= 10000000;
cmSpecDelayExec(&p->magDelay,magV,binCnt);
cmSpecDelayExec(&p->phsDelay,phsV,binCnt);
//if( p->mfp != NULL )
// cmMtxFileSmpExec( p->mfp, &p->out, 1 );
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmSample_t _cmComplexOnsetMedian( const cmSample_t* sp, unsigned sn, void* userPtr )
{ return cmVOS_Median(sp,sn); }
cmComplexOnset* cmComplexOnsetAlloc( cmCtx* c, cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
{
cmComplexOnset* op = cmObjAlloc( cmComplexOnset, c, p );
if( procSmpCnt > 0 && srate > 0 && medFiltWndSmpCnt > 0 )
if( cmComplexOnsetInit( op, procSmpCnt, srate, medFiltWndSmpCnt, threshold, frameCnt ) != cmOkRC )
cmComplexOnsetFree(&op);
return op;
}
cmRC_t cmComplexOnsetFree( cmComplexOnset** pp)
{
cmRC_t rc = cmOkRC;
cmComplexOnset* p = *pp;
if( pp==NULL || *pp == NULL )
return cmOkRC;
if((rc = cmComplexOnsetFinal(*pp)) != cmOkRC )
return rc;
cmMemPtrFree(&p->df);
cmMemPtrFree(&p->fdf);
cmObjFree(pp);
return cmOkRC;
}
cmRC_t cmComplexOnsetInit( cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
{
cmRC_t rc;
if(( rc = cmComplexOnsetFinal(p)) != cmOkRC )
return rc;
p->frmCnt = frameCnt;
p->dfi = 0;
p->df = cmMemResizeZ( cmSample_t, p->df, frameCnt );
p->fdf = cmMemResizeZ( cmSample_t, p->fdf, frameCnt );
p->onrate = 0;
p->threshold = threshold;
p->medSmpCnt = medFiltWndSmpCnt;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexOnset");
return cmOkRC;
}
cmRC_t cmComplexOnsetFinal( cmComplexOnset* p)
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmComplexOnsetExec( cmComplexOnset* p, cmSample_t cdf )
{
p->df[p->dfi++] = cdf;
return cmOkRC;
}
cmRC_t cmComplexOnsetCalc( cmComplexOnset* p )
{
// df -= mean(df)
cmVOS_SubVS(p->df,p->frmCnt,cmVOS_Mean(p->df,p->frmCnt));
// low pass forward/backward filter df[] into fdf[]
double d = 2 + sqrt(2);
cmReal_t b[] = {1/d, 2/d, 1/d};
unsigned bn = sizeof(b)/sizeof(b[0]);
cmReal_t a[] = {1, 0, 7-2*d};
unsigned an = sizeof(a)/sizeof(a[0]);
cmFilterFilterS(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
// median filter to low-passed filtered fdf[] into df[]
cmVOS_FnThresh(p->fdf,p->frmCnt,p->medSmpCnt,p->df,1,NULL);
// subtract med filtered signal from the low passed signal.
// fdf[] -= df[];
cmVOS_SubVV(p->fdf,p->frmCnt,p->df);
cmVOS_SubVS(p->fdf,p->frmCnt,p->threshold);
cmSample_t *bp = p->df,
*ep = bp + p->frmCnt - 1,
*dp = p->fdf + 1;
*bp++ = *ep = 0;
for( ; bp<ep; bp++,dp++)
{
*bp = (*dp > *(dp-1) && *dp > *(dp+1) && *dp > 0) ? 1 : 0;
p->onrate += *bp;
}
p->onrate /= p->frmCnt;
/*
if( p->mfp != NULL )
{
bp = p->df;
ep = bp + p->frmCnt;
while( bp < ep )
cmMtxFileSmpExec( p->mfp, bp++, 1 );
}
*/
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmMfcc* cmMfccAlloc( cmCtx* c, cmMfcc* ap, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
{
cmMfcc* p = cmObjAlloc( cmMfcc, c, ap );
if( melBandCnt > 0 && binCnt > 0 && dctCoeffCnt > 0 )
if( cmMfccInit( p, srate, melBandCnt, dctCoeffCnt, binCnt ) != cmOkRC )
cmMfccFree(&p);
return p;
}
cmRC_t cmMfccFree( cmMfcc** pp )
{
cmRC_t rc = cmOkRC;
if( pp==NULL || *pp == NULL )
return cmOkRC;
cmMfcc* p = *pp;
if( (rc = cmMfccFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->melM);
cmMemPtrFree(&p->dctM);
cmMemPtrFree(&p->outV);
cmObjFree(pp);
return rc;
}
cmRC_t cmMfccInit( cmMfcc* p, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
{
cmRC_t rc;
if((rc = cmMfccFinal(p)) != cmOkRC )
return rc;
p->melM = cmMemResize( cmReal_t, p->melM, melBandCnt * binCnt );
p->dctM = cmMemResize( cmReal_t, p->dctM, dctCoeffCnt * melBandCnt );
p->outV = cmMemResize( cmReal_t, p->outV, dctCoeffCnt );
// each row of the matrix melp contains a mask
cmVOR_MelMask( p->melM, melBandCnt, binCnt, srate, kShiftMelFl );
// each row contains melBandCnt elements
cmVOR_DctMatrix(p->dctM, dctCoeffCnt, melBandCnt );
p->melBandCnt = melBandCnt;
p->dctCoeffCnt = dctCoeffCnt;
p->binCnt = binCnt;
p->outN = dctCoeffCnt;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"mfcc");
//if( p->obj.ctx->statsProcPtr != NULL )
// p->mfccSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kMFCC_FId, p->outN );
return cmOkRC;
}
cmRC_t cmMfccFinal( cmMfcc* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmMfccExecPower( cmMfcc* p, const cmReal_t* magPowV, unsigned binCnt )
{
assert( binCnt == p->binCnt );
cmReal_t t[ p->melBandCnt ];
// apply the mel filter mask to the power spectrum
cmVOR_MultVMV( t, p->melBandCnt, p->melM, binCnt, magPowV );
// convert mel bands to dB
cmVOR_PowerToDb( t, p->melBandCnt, t );
// decorellate the bands with a DCT
cmVOR_MultVMV( p->outV, p->dctCoeffCnt, p->dctM, p->melBandCnt, t );
/*
cmPlotSelectSubPlot(0,0);
cmPlotClear();
//cmPlotLineS( "power", NULL, magPowV, NULL, 35, NULL, kSolidPlotLineId );
cmPlotLineS( "mel", NULL, t0, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
cmPlotSelectSubPlot(1,0);
cmPlotClear();
//cmPlotLineS( "meldb", NULL, t1, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
cmPlotLineS( "mfcc", NULL, p->outV+1, NULL, p->dctCoeffCnt-1, NULL, kSolidPlotLineId );
*/
if( p->mfp != NULL )
cmMtxFileRealExec(p->mfp,p->outV, p->outN);
return cmOkRC;
}
cmRC_t cmMfccExecAmplitude( cmMfcc* p, const cmReal_t* magAmpV, unsigned binCnt )
{
cmReal_t t[ binCnt ];
cmVOR_MultVVV( t,binCnt, magAmpV, magAmpV );
cmMfccExecPower(p,t,binCnt);
if( p->mfp != NULL )
cmMtxFileRealExec(p->mfp,p->outV, p->outN);
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
enum { cmSonesEqlConBinCnt = kEqualLoudBandCnt, cmSonesEqlConCnt=13 };
cmSones* cmSonesAlloc( cmCtx* c, cmSones* ap, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
{
cmSones* p = cmObjAlloc( cmSones, c, ap );
if( srate > 0 && barkBandCnt > 0 && binCnt > 0 )
if( cmSonesInit(p,srate,barkBandCnt,binCnt,flags) != cmOkRC )
cmSonesFree(&p);
return p;
}
cmRC_t cmSonesFree( cmSones** pp )
{
cmRC_t rc = cmOkRC;
cmSones* p = *pp;
if( pp==NULL || *pp==NULL)
return cmOkRC;
if((rc = cmSonesFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->ttmV);
cmMemPtrFree(&p->sfM);
cmMemPtrFree(&p->barkIdxV);
cmMemPtrFree(&p->barkCntV);
cmMemPtrFree(&p->outV);
cmObjFree(pp);
return rc;
}
cmRC_t cmSonesInit( cmSones* p, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
{
p->ttmV = cmMemResize( cmReal_t, p->ttmV, binCnt);
p->sfM = cmMemResize( cmReal_t, p->sfM, binCnt*barkBandCnt);
p->barkIdxV = cmMemResize( unsigned, p->barkIdxV, barkBandCnt);
p->barkCntV = cmMemResize( unsigned, p->barkCntV, barkBandCnt);
p->outV = cmMemResize( cmReal_t, p->outV, barkBandCnt);
// calc outer ear filter
cmVOR_TerhardtThresholdMask( p->ttmV, binCnt, srate, kNoTtmFlags );
// calc shroeder spreading function
cmVOR_ShroederSpreadingFunc(p->sfM, barkBandCnt, srate);
// calc the bin to bark maps
p->barkBandCnt = cmVOR_BarkMap( p->barkIdxV, p->barkCntV, barkBandCnt, binCnt, srate );
//unsigned i = 0;
//for(; i<barkBandCnt; ++i)
// printf("%i %i %i\n", i+1, barkIdxV[i]+1, barkCntV[i]);
bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
p->binCnt = binCnt;
p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
p->overallLoudness = 0;
p->flags = flags;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sones");
//p->sonesSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSones_FId, p->outN );
//p->loudSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kLoud_FId, 1 );
return cmOkRC;
}
cmRC_t cmSonesFinal( cmSones* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmSonesExec( cmSones* p, const cmReal_t* magPowV, unsigned binCnt )
{
assert( binCnt == p->binCnt );
// Equal-loudness and phon map from: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
// equal-loudness contours
double eqlcon[cmSonesEqlConCnt][cmSonesEqlConBinCnt] =
{
{12,7,4,1,0,0,0,-0.5,-2,-3,-7,-8,-8.5,-8.5,-8.5},
{20,17,14,12,10,9.5,9,8.5,7.5,6.5,4,3,2.5,2,2.5},
{29,26,23,21,20,19.5,19.5,19,18,17,15,14,13.5,13,13.5},
{36,34,32,30,29,28.5,28.5,28.5,28,27.5,26,25,24.5,24,24.5},
{45,43,41,40,40,40,40,40,40,39.5,38,37,36.5,36,36.5},
{53,51,50,49,48.5,48.5,49,49,49,49,48,47,46.5,45.5,46},
{62,60,59,58,58,58.5,59,59,59,59,58,57.5,57,56,56},
{70,69,68,67.5,67.5,68,68,68,68,68,67,66,65.5,64.5,64.5},
{79,79,79,79,79,79,79,79,78,77.5,76,75,74.5,73,73},
{89,89,89,89.5,90,90,90,89.5,89,88.5,87,86,85.5,84,83.5},
{100,100,100,100,100,99.5,99,99,98.5,98,96,95,94.5,93.5,93},
{112,112,112,112,111,110.5,109.5,109,108.5,108,106,105,104.5,103,102.5},
{122,122,121,121,120.5,120,119,118,117,116.5,114.5,113.5,113,111, 110.5}
};
// loudness levels (phone scales)
double phons[cmSonesEqlConCnt]= {0.0,10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0,100.0,110.0,120.0};
unsigned i,j;
cmReal_t t0[ binCnt ];
cmReal_t t1[ p->barkBandCnt ];
cmReal_t t2[ p->barkBandCnt ];
unsigned* idxV = p->barkIdxV;
unsigned* cntV = p->barkCntV;
cmReal_t* sfM = p->sfM;
// apply the outer ear filter
cmVOR_MultVVV( t0, binCnt, magPowV, p->ttmV);
// apply the bark frequency warping
for(i=0; i<p->barkBandCnt; ++i)
{
if( cntV[i] == 0 )
t1[i] = 0;
else
{
t1[i] = t0[ idxV[i] ];
for(j=1; j<cntV[i]; ++j)
t1[i] += t0[ idxV[i] + j ];
}
}
// apply the spreading filters
cmVOR_MultVMV( t2, p->barkBandCnt, sfM, p->barkBandCnt, t1 );
bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
unsigned bandCnt = elFl ? cmMin(p->barkBandCnt,cmSonesEqlConBinCnt) : p->barkBandCnt;
//p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
p->overallLoudness = 0;
for( i = 0; i <bandCnt; i++ )
{
// if using the equal-loudness contours begin with the third bark band
// and end with the 18th bark band
unsigned k = elFl ? i+3 : i;
if( k < p->barkBandCnt )
{
double v = t2[k];
// convert to db
v = 10*log10( v<1 ? 1 : v );
if( elFl )
{
// db to phons
// see: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
j = 0;
// find the equal loudness curve for this frequency and db level
while( v >= eqlcon[j][i] )
++j;
if( j == cmSonesEqlConCnt )
{
cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "Bark band %i is out of range during equal-loudness mapping.",j );
continue;
}
// convert db to phons
if( j == 0 )
v = phons[0];
else
{
double t1 = ( v - eqlcon[j-1][i] ) / ( eqlcon[j][i] - eqlcon[j-1][i] );
v = phons[j-1] + t1 * (phons[j] - phons[j-1]);
}
}
// convert to sones
// bladon and lindblom, 1981, JASA, modelling the judment of vowel quality differences
if( v >= 40 )
p->outV[i] = pow(2,(v-40)/10);
else
p->outV[i] = pow(v/40,2.642);
p->overallLoudness += p->outV[i];
}
}
if( p->mfp != NULL )
cmMtxFileRealExec( p->mfp, p->outV, p->outN );
return cmOkRC;
}
void cmSonesTest()
{
cmKbRecd kb;
double srate = 44100;
unsigned bandCnt = 23;
unsigned binCnt = 513;
cmSample_t tv[ binCnt ];
cmSample_t sm[ bandCnt * bandCnt ];
cmSample_t t[ bandCnt * bandCnt ];
unsigned binIdxV[ bandCnt ];
unsigned cntV[ bandCnt ];
unsigned i;
cmPlotSetup("Sones",1,1);
cmVOS_TerhardtThresholdMask(tv,binCnt,srate, kModifiedTtmFl );
cmVOS_ShroederSpreadingFunc(sm, bandCnt, srate);
cmVOS_Transpose( t, sm, bandCnt, bandCnt );
bandCnt = cmVOS_BarkMap(binIdxV,cntV, bandCnt, binCnt, srate );
for(i=0; i<bandCnt; ++i)
printf("%i %i %i\n", i, binIdxV[i], cntV[i] );
for(i=0; i<bandCnt; ++i )
{
cmPlotLineS( NULL, NULL, t+(i*bandCnt), NULL, bandCnt, NULL, kSolidPlotLineId );
}
//cmPlotLineS( NULL, NULL, tv, NULL, binCnt, NULL, kSolidPlotLineId );
cmPlotDraw();
cmKeyPress(&kb);
}
//------------------------------------------------------------------------------------------------------------
cmAudioOffsetScale* cmAudioOffsetScaleAlloc( cmCtx* c, cmAudioOffsetScale* ap, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
{
cmAudioOffsetScale* p = cmObjAlloc( cmAudioOffsetScale, c, ap );
if( procSmpCnt > 0 && srate > 0 )
if( cmAudioOffsetScaleInit( p, procSmpCnt, srate, offset, rmsWndSecs, dBref, flags ) != cmOkRC )
cmAudioOffsetScaleFree(&p);
return p;
}
cmRC_t cmAudioOffsetScaleFree( cmAudioOffsetScale** pp )
{
cmRC_t rc = cmOkRC;
cmAudioOffsetScale* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->cBufPtr);
cmMemPtrFree(&p->cCntPtr);
cmMemPtrFree(&p->outV);
cmObjFree(pp);
return rc;
}
cmRC_t cmAudioOffsetScaleInit( cmAudioOffsetScale* p, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
{
assert( procSmpCnt > 0 && srate > 0);
cmRC_t rc;
if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
return rc;
p->cBufCnt = 0;
if( cmIsFlag(flags, kRmsAudioScaleFl) )
{
if( rmsWndSecs > 0 )
{
unsigned rmsSmpCnt = srate * rmsWndSecs;
p->cBufCnt = (unsigned)ceil( rmsSmpCnt / procSmpCnt );
if( p->cBufCnt > 0 )
{
p->cBufPtr = cmMemResizeZ( cmReal_t, p->cBufPtr, p->cBufCnt );
p->cCntPtr = cmMemResizeZ( unsigned, p->cCntPtr, p->cBufCnt );
}
}
else
{
p->cBufCnt = 0;
p->cBufPtr = NULL;
p->cCntPtr = NULL;
}
}
p->cBufIdx = 0;
p->cBufCurCnt = 0;
p->cBufSum = 0;
p->cCntSum = 0;
p->outV = cmMemResize( cmSample_t, p->outV, procSmpCnt );
p->outN = procSmpCnt;
p->offset = offset;
p->dBref = dBref;
p->flags = flags;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"audioOffsetScale");
return cmOkRC;
}
cmRC_t cmAudioOffsetScaleFinal( cmAudioOffsetScale* p )
{
//if( p != NULL)
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmAudioOffsetScaleExec( cmAudioOffsetScale* p, const cmSample_t* sp, unsigned sn )
{
double Pref = 20.0 / 1000000; // 20 micro Pascals
cmSample_t* dbp = p->outV;
const cmSample_t* dep = dbp + sn;
double scale = 0;
// if no scaling was requested then add offset only
if( cmIsFlag(p->flags, kNoAudioScaleFl) )
{
while( dbp < dep )
*dbp++ = *sp++ + p->offset;
goto doneLabel;
}
// if fixed scaling
if( cmIsFlag(p->flags, kFixedAudioScaleFl) )
{
if( scale == 0 )
scale = pow(10,p->dBref/20);
while( dbp < dep )
*dbp++ = (*sp++ + p->offset) * scale;
}
else // if RMS scaling
{
double sum = 0;
double rms = 0;
while( dbp < dep )
{
double v = (*sp++ + p->offset) / Pref;
sum += v*v;
*dbp++ = v;
}
// if there is no RMS buffer calc RMS on procSmpCnt samles
if( p->cBufCnt == 0 )
rms = sqrt( sum / sn );
else
{
p->cBufSum -= p->cBufPtr[ p->cBufIdx ];
p->cBufSum += sum;
p->cCntSum -= p->cCntPtr[ p->cBufIdx ];
p->cCntSum += sn;
p->cBufIdx = (p->cBufIdx+1) % p->cBufCnt;
p->cBufCurCnt = cmMin( p->cBufCurCnt+1, p->cBufCnt );
assert( p->cCntSum > 0 );
rms = sqrt( p->cBufSum / p->cCntSum );
}
double sigSPL = 20*log10(rms);
scale = pow(10,(p->dBref - sigSPL)/20);
dbp = p->outV;
while( dbp < dep )
*dbp++ *= scale;
}
doneLabel:
dbp = p->outV + sn;
dep = p->outV + p->outN;
while( dbp < dep )
*dbp++ = 0;
if( p->mfp != NULL )
cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmSpecMeas* cmSpecMeasAlloc( cmCtx* c, cmSpecMeas* ap, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
{
cmSpecMeas* p = cmObjAlloc( cmSpecMeas, c, ap );
if( srate > 0 && binCnt > 0 )
if( cmSpecMeasInit( p, srate, binCnt, wndFrmCnt, flags ) != cmOkRC )
cmSpecMeasFree(&p);
return p;
}
cmRC_t cmSpecMeasFree( cmSpecMeas** pp )
{
cmRC_t rc = cmOkRC;
cmSpecMeas* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmSpecMeasFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->rmsV);
cmMemPtrFree(&p->hfcV);
cmMemPtrFree(&p->scnV);
cmObjFree(pp);
return rc;
}
cmRC_t cmSpecMeasInit( cmSpecMeas* p, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
{
cmRC_t rc;
if((rc = cmSpecMeasFinal(p)) != cmOkRC )
return rc;
if( cmIsFlag(flags, kUseWndSpecMeasFl) )
{
p->rmsV = cmMemResizeZ( cmReal_t, p->rmsV, wndFrmCnt );
p->hfcV = cmMemResizeZ( cmReal_t, p->hfcV, wndFrmCnt );
p->scnV = cmMemResizeZ( cmReal_t, p->scnV, wndFrmCnt );
}
p->rmsSum = 0;
p->hfcSum = 0;
p->scnSum = 0;
p->binCnt = binCnt;
p->flags = flags;
p->wndFrmCnt = wndFrmCnt;
p->frameCnt = 0;
p->frameIdx = 0;
p->binHz = srate / ((binCnt-1) * 2);
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"specMeas");
//p->rmsSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kRMS_FId, 1);
//p->hfcSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kHFC_FId, 1);
//p->scSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSC_FId, 1);
//p->ssSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSS_FId, 1);
return cmOkRC;
}
cmRC_t cmSpecMeasFinal( cmSpecMeas* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmSpecMeasExec( cmSpecMeas* p, const cmReal_t* magPowV, unsigned binCnt )
{
assert( binCnt == p->binCnt );
unsigned i = 0;
const cmReal_t* sbp = magPowV;
const cmReal_t* sep = sbp + binCnt;
cmReal_t rmsSum = 0;
cmReal_t hfcSum = 0;
cmReal_t scnSum = 0;
cmReal_t ssSum = 0;
for(i=0; sbp < sep; ++i, ++sbp )
{
rmsSum += *sbp;
hfcSum += *sbp * i;
scnSum += *sbp * i * p->binHz;
}
p->frameCnt++;
if( cmIsFlag(p->flags, kUseWndSpecMeasFl) )
{
p->frameCnt = cmMin( p->frameCnt, p->wndFrmCnt );
cmReal_t* rmsV = p->rmsV + p->frameIdx;
cmReal_t* hfcV = p->hfcV + p->frameIdx;
cmReal_t* scnV = p->scnV + p->frameIdx;
p->rmsSum -= *rmsV;
p->hfcSum -= *hfcV;
p->scnSum -= *scnV;
*rmsV = rmsSum;
*hfcV = hfcSum;
*scnV = scnSum;
p->frameIdx = (p->frameIdx+1) % p->frameCnt;
}
p->rmsSum += rmsSum;
p->hfcSum += hfcSum;
p->scnSum += scnSum;
p->rms = sqrt(p->rmsSum / (p->binCnt * p->frameCnt) );
p->hfc = p->hfcSum / ( p->binCnt * p->frameCnt );
p->sc = p->scnSum / cmMax( cmReal_EPSILON, p->rmsSum );
sbp = magPowV;
for(i=0; sbp < sep; ++i )
{
cmReal_t t = (i*p->binHz) - p->sc;
ssSum += *sbp++ * (t*t);
}
p->ss = sqrt(ssSum / cmMax( cmReal_EPSILON, p->rmsSum ));
if( p->mfp != NULL )
{
cmReal_t a[4] = { p->rms, p->hfc, p->sc, p->ss };
cmMtxFileRealExec( p->mfp, a, 4 );
}
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmSigMeas* cmSigMeasAlloc( cmCtx* c, cmSigMeas* ap, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
{
cmSigMeas* p = cmObjAlloc( cmSigMeas, c, ap );
p->sbp = cmShiftBufAlloc(c,&p->shiftBuf,0,0,0);
if( srate > 0 && procSmpCnt > 0 && measSmpCnt > 0 )
if( cmSigMeasInit( p, srate, procSmpCnt, measSmpCnt ) != cmOkRC )
cmSigMeasFree(&p);
return p;
}
cmRC_t cmSigMeasFree( cmSigMeas** pp )
{
cmRC_t rc = cmOkRC;
cmSigMeas* p = *pp;
if( pp==NULL || *pp==NULL)
return cmOkRC;
if((rc = cmSigMeasFinal(p)) != cmOkRC )
return rc;
cmShiftBufFree(&p->sbp);
cmObjFree(pp);
return rc;
}
cmRC_t cmSigMeasInit( cmSigMeas* p, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
{
cmRC_t rc;
if((rc = cmSigMeasFinal(p)) != cmOkRC )
return rc;
if( procSmpCnt != measSmpCnt )
cmShiftBufInit( p->sbp, procSmpCnt, measSmpCnt, procSmpCnt );
p->zcrDelay = 0;
p->srate = srate;
p->measSmpCnt = measSmpCnt;
p->procSmpCnt = procSmpCnt;
//p->zcrSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kZCR_FId, 1 );
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sigMeas");
return cmOkRC;
}
cmRC_t cmSigMeasFinal( cmSigMeas* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmSigMeasExec( cmSigMeas* p, const cmSample_t* sp, unsigned sn )
{
if( p->procSmpCnt != p->measSmpCnt )
{
cmShiftBufExec( p->sbp, sp, sn );
sp = p->sbp->outV;
sn = p->sbp->wndSmpCnt;
assert( p->sbp->wndSmpCnt == p->measSmpCnt );
}
unsigned zcn = cmVOS_ZeroCrossCount( sp, sn, NULL );
p->zcr = (cmReal_t)zcn * p->srate / p->measSmpCnt;
if( p->mfp != NULL )
cmMtxFileRealExec( p->mfp, &p->zcr, 1 );
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmSRC* cmSRCAlloc( cmCtx* c, cmSRC* ap, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
{
cmSRC* p = cmObjAlloc( cmSRC, c,ap );
cmFilterAlloc(c,&p->filt,NULL,0,NULL,0,0,NULL);
if( srate > 0 && procSmpCnt > 0 )
if( cmSRCInit( p, srate, procSmpCnt, upFact, dnFact ) != cmOkRC )
cmSRCFree(&p);
return p;
}
cmRC_t cmSRCFree( cmSRC** pp )
{
cmRC_t rc;
if( pp != NULL && *pp != NULL )
{
cmSRC* p = *pp;
if((rc = cmSRCFinal( p )) == cmOkRC )
{
cmFilter* fp = &p->filt;
cmFilterFree(&fp);
cmMemPtrFree(&p->outV);
cmObjFree(pp);
}
}
return cmOkRC;
}
cmRC_t cmSRCInit( cmSRC* p, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
{
cmRC_t rc;
if((rc = cmSRCFinal(p)) != cmOkRC )
return rc;
double hiRate = upFact * srate;
double loRate = hiRate / dnFact;
double minRate= cmMin( loRate, srate );
double fcHz = minRate/2;
double dHz = (fcHz * .1); // transition band is 5% of min sample rate
double passHz = fcHz-dHz;
double stopHz = fcHz;
double passDb = 0.001;
double stopDb = 20;
cmFilterInitEllip( &p->filt, hiRate, passHz, stopHz, passDb, stopDb, procSmpCnt, NULL );
//printf("CoeffCnt:%i dHz:%f passHz:%f stopHz:%f passDb:%f stopDb:%f\n", p->fir.coeffCnt, dHz, passHz, stopHz, passDb, stopDb );
p->outN = (unsigned)ceil(procSmpCnt * upFact / dnFact);
p->outV = cmMemResize( cmSample_t, p->outV, p->outN );
p->upi = 0;
p->dni = 0;
p->upFact = upFact;
p->dnFact = dnFact;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"src");
return cmOkRC;
}
cmRC_t cmSRCFinal( cmSRC* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmSRCExec( cmSRC* p, const cmSample_t* sp, unsigned sn )
{
const cmSample_t* sep = sp + sn;
cmSample_t* op = p->outV;
const cmSample_t* oep = op + p->outN;
unsigned iN = sn * p->upFact;
unsigned i,j;
// run the filter at the upsampled rate ...
for(i=0; i<iN; ++i)
{
assert( p->upi!=0 || sp<sep );
cmSample_t x0 = p->upi==0 ? *sp++ : 0,
y0 = x0 * p->filt.b0 + p->filt.d[0];
// ... but output at the down sampled rate
if( p->dni==0 )
{
assert( op < oep );
*op++ = y0;
}
// advance the filter delay line
for(j=0; j<p->filt.cn; ++j)
p->filt.d[j] = p->filt.b[j]*x0 - p->filt.a[j]*y0 + p->filt.d[j+1];
// update the input/output clocks
p->upi = (p->upi + 1) % p->upFact;
p->dni = (p->dni + 1) % p->dnFact;
}
p->outN = op - p->outV;
if( p->mfp != NULL )
cmMtxFileSmpExec(p->mfp,p->outV,p->outN );
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmConstQ* cmConstQAlloc( cmCtx* c, cmConstQ* ap, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
{
cmConstQ* p = cmObjAlloc( cmConstQ, c, ap );
if( srate >0 )
if( cmConstQInit(p,srate,minMidiPitch,maxMidiPitch,binsPerOctave,thresh) != cmOkRC )
cmConstQFree(&p);
return p;
}
cmRC_t cmConstQFree( cmConstQ** pp )
{
cmRC_t rc;
cmConstQ* p = *pp;
if( pp==NULL || *pp==NULL)
return cmOkRC;
if((rc = cmConstQFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->fiV);
cmMemPtrFree(&p->foV);
cmMemPtrFree(&p->skM);
cmMemPtrFree(&p->outV);
cmMemPtrFree(&p->magV);
cmMemPtrFree(&p->skBegV);
cmMemPtrFree(&p->skEndV);
cmObjFree(pp);
return cmOkRC;
}
cmRC_t cmConstQInit( cmConstQ* p, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
{
cmRC_t rc;
if((rc = cmConstQFinal(p)) != cmOkRC )
return rc;
cmReal_t minHz = cmMidiToHz(minMidiPitch);
cmReal_t maxHz = cmMidiToHz(maxMidiPitch);
cmReal_t Q = 1.0/(pow(2,(double)1.0/binsPerOctave)-1);
unsigned K = (unsigned)ceil( binsPerOctave * log2(maxHz/minHz) );
unsigned fftN = cmNextPowerOfTwo( (unsigned)ceil(Q*srate/minHz) );
unsigned k = 0;
p->fiV = cmMemResize(cmComplexR_t, p->fiV, fftN);
p->foV = cmMemResize(cmComplexR_t, p->foV, fftN);
cmFftPlanR_t plan = cmFft1dPlanAllocR(fftN, p->fiV, p->foV, FFTW_FORWARD, FFTW_ESTIMATE );
p->wndSmpCnt = fftN;
p->constQBinCnt = K;
p->binsPerOctave= binsPerOctave;
p->skM = cmMemResizeZ( cmComplexR_t, p->skM, p->wndSmpCnt * p->constQBinCnt);
p->outV = cmMemResizeZ( cmComplexR_t, p->outV, p->constQBinCnt);
p->magV = cmMemResizeZ( cmReal_t, p->magV, p->constQBinCnt);
p->skBegV = cmMemResizeZ( unsigned, p->skBegV, p->constQBinCnt);
p->skEndV = cmMemResizeZ( unsigned, p->skEndV, p->constQBinCnt);
//p->mfp = cmCtxAllocDebugFile( p->obj.ctx, "constQ");
//printf("hz:%f %f bpo:%i sr:%f thresh:%f Q:%f K%i (cols) fftN:%i (rows)\n", minHz,maxHz,binsPerOctave,srate,thresh,Q,K,fftN);
double* hamm = NULL;
// note that the bands are created in reverse order
for(k=0; k<K; ++k)
{
unsigned iN = ceil( Q * srate / (minHz * pow(2,(double)k/binsPerOctave)));
unsigned start = fftN/2;
//double hamm[ iN ];
hamm = cmMemResizeZ(double,hamm,iN);
memset( p->fiV, 0, fftN * sizeof(cmComplexR_t));
memset( p->foV, 0, fftN * sizeof(cmComplexR_t));
cmVOD_Hamming( hamm, iN );
cmVOD_DivVS( hamm, iN, iN );
if( cmIsEvenU(iN) )
start -= iN/2;
else
start -= (iN+1)/2;
//printf("k:%i iN:%i start:%i %i\n",k,iN,start,start+iN-1);
unsigned i = start;
for(; i<=start+iN-1; ++i)
{
double arg = 2.0*M_PI*Q*(i-start)/iN;
double mag = hamm[i-start];
p->fiV[i-1] = (mag * cos(arg)) + (mag * I * sin(arg));
}
cmFftExecuteR(plan);
// since the bands are created in reverse order they are also stored in reverse order
// (i.e column k-1 is stored first and column 0 is stored last)
i=0;
unsigned minIdx = -1;
unsigned maxIdx = 0;
for(; i<fftN; ++i)
{
bool fl = cabs(p->foV[i]) <= thresh;
p->skM[ (k*p->wndSmpCnt) + i ] = fl ? 0 : p->foV[i]/fftN;
if( fl==false && minIdx == -1 )
minIdx = i;
if( fl==false && i>maxIdx )
maxIdx = i;
}
p->skBegV[k] = minIdx;
p->skEndV[k] = maxIdx;
}
cmMemPtrFree(&hamm);
cmFftPlanFreeR(plan);
return cmOkRC;
}
cmRC_t cmConstQFinal( cmConstQ* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmConstQExec( cmConstQ* p, const cmComplexR_t* ftV, unsigned srcBinCnt )
{
//acVORC_MultVVM( p->outV, p->constQBinCnt,ftV,p->wndSmpCnt, p->skM );
cmReal_t* mbp = p->magV;
cmComplexR_t* dbp = p->outV;
const cmComplexR_t* dep = p->outV + p->constQBinCnt;
unsigned i = 0;
for(; dbp < dep; ++dbp,++i,++mbp )
{
const cmComplexR_t* sbp = ftV + p->skBegV[i];
const cmComplexR_t* kp = p->skM + (i*p->wndSmpCnt) + p->skBegV[i];
const cmComplexR_t* ep = kp + (p->skEndV[i] - p->skBegV[i]) + 1;
*dbp = 0;
while( kp < ep )
*dbp += *sbp++ * *kp++;
*mbp = cmCabsR(*dbp);
}
if( p->mfp != NULL )
cmMtxFileComplexExec( p->mfp, p->outV, p->constQBinCnt, 1 );
return cmOkRC;
}
void cmConstQTest( cmConstQ* p )
{
cmKbRecd kb;
unsigned i,j;
cmSample_t* t = cmMemAlloc( cmSample_t, p->wndSmpCnt );
for(i=0; i<p->constQBinCnt; ++i)
{
for(j=0; j<p->wndSmpCnt; ++j)
t[j] = cabs( p->skM[ (i*p->wndSmpCnt) + j ]);
//cmPlotClear();
cmPlotLineS( NULL, NULL, t, NULL, 500, NULL, kSolidPlotLineId );
}
cmPlotDraw();
cmKeyPress(&kb);
cmMemPtrFree(&t);
}
//------------------------------------------------------------------------------------------------------------
cmHpcp* cmTunedHpcpAlloc( cmCtx* c, cmHpcp* ap, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
{
cmHpcp* p = cmObjAlloc( cmHpcp, c, ap );
if( binsPerOctave > 0 && constQBinCnt >> 0 )
if( cmTunedHpcpInit( p, binsPerOctave, constQBinCnt, cqMinMidiPitch, frameCnt, medFiltOrder ) != cmOkRC)
cmTunedHpcpFree(&p);
return p;
}
cmRC_t cmTunedHpcpFree( cmHpcp** pp )
{
cmRC_t rc = cmOkRC;
cmHpcp* p = *pp;
if(pp==NULL || *pp==NULL)
return cmOkRC;
if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->hpcpM);
cmMemPtrFree(&p->fhpcpM);
cmMemPtrFree(&p->histV);
cmMemPtrFree(&p->outM);
cmMemPtrFree(&p->meanV);
cmMemPtrFree(&p->varV);
cmObjFree(pp);
return cmOkRC;
}
cmRC_t cmTunedHpcpInit( cmHpcp* p, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
{
assert( binsPerOctave % kStPerOctave == 0 );
assert( cmIsOddU( binsPerOctave / kStPerOctave ) );
cmRC_t rc;
if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
return rc;
p->histN = binsPerOctave/kStPerOctave;
p->hpcpM = cmMemResizeZ( cmReal_t, p->hpcpM, frameCnt*binsPerOctave );
p->fhpcpM = cmMemResizeZ( cmReal_t, p->fhpcpM, binsPerOctave*frameCnt );
p->histV = cmMemResizeZ( unsigned, p->histV, p->histN );
p->outM = cmMemResizeZ( cmReal_t, p->outM, kStPerOctave * frameCnt );
p->meanV = cmMemResizeZ( cmReal_t, p->meanV, kStPerOctave );
p->varV = cmMemResizeZ( cmReal_t, p->varV, kStPerOctave );
p->constQBinCnt = constQBinCnt;
p->binsPerOctave = binsPerOctave;
p->frameCnt = frameCnt;
p->frameIdx = 0;
p->cqMinMidiPitch= cqMinMidiPitch;
p->medFiltOrder = medFiltOrder;
//p->mf0p = cmCtxAllocDebugFile(p->obj.ctx,"hpcp");
//p->mf1p = cmCtxAllocDebugFile(p->obj.ctx,"fhpcp");
//p->mf2p = cmCtxAllocDebugFile(p->obj.ctx,"chroma");
return cmOkRC;
}
cmRC_t cmTunedHpcpFinal( cmHpcp* p )
{
/*
if( p != NULL )
{
cmCtxFreeDebugFile(p->obj.ctx,&p->mf0p);
cmCtxFreeDebugFile(p->obj.ctx,&p->mf1p);
cmCtxFreeDebugFile(p->obj.ctx,&p->mf2p);
}
*/
return cmOkRC;
}
cmRC_t cmTunedHpcpExec( cmHpcp* p, const cmComplexR_t* cqp, unsigned cqn )
{
assert( cqn == p->constQBinCnt );
// if there is no space to store the output then do nothing
if( p->frameIdx >= p->frameCnt )
return cmOkRC;
unsigned octCnt = (unsigned)floor(p->constQBinCnt / p->binsPerOctave);
unsigned i;
cmReal_t hpcpV[ p->binsPerOctave + 2 ];
unsigned idxV[ p->binsPerOctave ];
unsigned binsPerSt = p->binsPerOctave / kStPerOctave;
// Notice that the first and last elements of p->hpcp are reserved for
// use in producing the appeareance of circularity for the peak picking
// algorithm. The cmtual hpcp[] data begins on the index 1 (not 0) and
// ends on p->binsPerOctave (not p->binsPerOctave-1).
// sum the constQBinCnt constant Q bins into binsPerOctave bins to form the HPCP
for(i=0; i<p->binsPerOctave; ++i)
{
cmReal_t sum = 0;
const cmComplexR_t* sbp = cqp + i;
const cmComplexR_t* sep = cqp + (octCnt * p->binsPerOctave);
for(; sbp < sep; sbp += p->binsPerOctave)
sum += cmCabsR(*sbp);
hpcpV[i+1] = sum;
}
// shift the lowest ST center bin to (binsPerSt+1)/2 such that an equal number of
// flat and sharp bins are above an below it
int rotateCnt = ((binsPerSt+1)/2) - 1;
// shift pitch class C to the lowest semitone boundary
rotateCnt -= ( 48-(int)p->cqMinMidiPitch) * binsPerSt;
// perform the shift
cmVOR_Rotate(hpcpV+1, p->binsPerOctave, rotateCnt);
// duplicate the first and last bin to produce circularity in the hpcp
hpcpV[0] = hpcpV[ p->binsPerOctave ];
hpcpV[ p->binsPerOctave+1 ] = hpcpV[1];
// locate the indexes of the positive peaks in the hpcp
unsigned pkN = cmVOR_PeakIndexes( idxV, p->binsPerOctave, hpcpV, p->binsPerOctave, 0 );
// Convert the peak indexes to values in the range 0 to binsPerSet-1
// If stPerBin == 3 : 0=flat 1=in tune 2=sharp
cmVOU_Mod( idxV, pkN, binsPerSt );
// Form a histogram to keep count of the number of flat,in-tune and sharp peaks
cmVOU_Hist( p->histV, binsPerSt, idxV, pkN );
// store the hpcpV[] to the row p->hpcpM[p->frameIdx,:]
cmVOR_CopyN( p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt, hpcpV+1, 1 );
// write the hpcp debug file
if( p->mf0p != NULL )
cmMtxFileRealExecN( p->mf0p, p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt );
p->frameIdx++;
return cmOkRC;
}
cmRC_t cmTunedHpcpTuneAndFilter( cmHpcp* p)
{
// note: p->frameIdx now holds the cmtual count of frames in p->hpcpA[].
// p->frameCnt holds the allocated count of frames in p->hpcpA[].
unsigned i,j;
// filter each column of hpcpA[] into each row of fhpcpA[]
for(i=0; i<p->binsPerOctave; ++i)
{
cmVOR_MedianFilt( p->hpcpM + (i * p->frameCnt), p->frameIdx, p->medFiltOrder, p->fhpcpM + i, p->binsPerOctave );
// write the fhpcp[i,:] to the debug file
if( p->mf1p != NULL )
cmMtxFileRealExecN( p->mf1p, p->fhpcpM + i, p->frameIdx, p->binsPerOctave );
}
unsigned binsPerSt = p->histN;
assert( (binsPerSt > 0) && (cmIsOddU(binsPerSt)) );
unsigned maxIdx = cmVOU_MaxIndex(p->histV,binsPerSt,1);
int tuneShift = -(maxIdx - ((binsPerSt+1)/2));
cmReal_t gaussWndV[ binsPerSt ];
// generate a gaussian window
cmVOR_GaussWin( gaussWndV, binsPerSt, 2.5 );
// Rotate the window to apply tuning via the weighted sum operation below
// (the result will be equivalent to rotating p->fhpcpM[] prior to reducing the )
cmVOR_Rotate(gaussWndV, binsPerSt, tuneShift);
// zero the meanV[] before summing into it
cmVOR_Fill(p->meanV,kStPerOctave,0);
// for each frame
for(i=0; i<p->frameIdx; ++i)
{
// for each semitone
for(j=0; j<kStPerOctave; ++j)
{
// reduce each semitone to a single value by forming a weighted sum of all the assoc'd bins
cmReal_t sum = cmVOR_MultSumVV( gaussWndV, p->fhpcpM + (i*p->binsPerOctave) + (j*binsPerSt), binsPerSt );
// store time-series output to the ith column
p->outM[ (i*kStPerOctave) + j ] = sum;
// calc the sum of all chroma values in bin j.
p->meanV[ j ] += sum;
}
// write the chroma debug file
if( p->mf2p != NULL )
cmMtxFileRealExec( p->mf2p, p->outM + (i*kStPerOctave), kStPerOctave );
}
// form the chroma mean from the sum calc'd above
cmVOR_DivVS( p->meanV, kStPerOctave, p->frameIdx );
// variance
for(j=0; j<kStPerOctave; ++j)
p->varV[j] = cmVOR_VarianceN( p->outM + j, p->frameIdx, kStPerOctave, p->meanV + j );
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
/*
cmStatsProc* cmStatsProcAlloc( cmCtx* c, cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
{
cmStatsProc* op = cmObjAlloc(cmStatsProc,c,p);
if( wndEleCnt > 0 )
if( cmStatsProcInit(op, wndEleCnt, flags ) != cmOkRC )
cmStatsProcFree(&op);
if( op != NULL )
cmCtxSetStatsProc(c,op);
return op;
}
cmRC_t cmStatsProcFree( cmStatsProc** pp )
{
cmRC_t rc = cmOkRC;
cmStatsProc* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmStatsProcFinal(p) ) != cmOkRC )
return rc;
cmCtxSetStatsProc(p->obj.ctx,NULL); // complement to cmSetStatsProc() in cmStatsProcAlloc()
cmMemPtrFree(&p->regArrayV);
cmMemPtrFree(&p->m);
cmMemPtrFree(&p->sumV);
cmMemPtrFree(&p->meanV);
cmMemPtrFree(&p->varV);
cmObjFree(pp);
return cmOkRC;
}
cmRC_t cmStatsProcInit( cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
{
cmRC_t rc;
if((rc = cmStatsProcFinal(p)) != cmOkRC )
return rc;
p->wndEleCnt = wndEleCnt;
p->dimCnt = 0;
p->curIdx = 0;
p->curCnt = 1;
p->flags = flags;
p->execCnt = 0;
p->regArrayN = 0;
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"statsProc");
return rc;
}
cmRC_t cmStatsProcFinal( cmStatsProc* p )
{
if( p != NULL )
cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
unsigned cmStatsProcReg( cmStatsProc* p, unsigned featId, unsigned featEleCnt )
{
cmStatsProcRecd r;
r.featId = featId;
r.idx = p->dimCnt;
r.cnt = featEleCnt;
p->dimCnt += featEleCnt;
//unsigned i;
// printf("B:\n");
//for(i=0; i<p->regArrayN; ++i)
// printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
//printf("\nA:\n");
p->regArrayV = cmMemResizePZ( cmStatsProcRecd, p->regArrayV, p->regArrayN+1 );
p->regArrayV[ p->regArrayN ] = r;
p->regArrayN++;
//for(i=0; i<p->regArrayN; ++i)
// printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
//printf("\n");
return p->regArrayN-1; // return the index of the new reg recd
}
const cmStatsProcRecd* cmStatsProcRecdPtr( cmStatsProc* p, unsigned regId )
{
assert( regId < p->regArrayN );
return p->regArrayV + regId;
}
cmRC_t cmStatsProcExecD( cmStatsProc* p, unsigned regId, const double v[], unsigned vCnt )
{
cmRC_t rc = cmOkRC;
// on the first pass allcoate the storage buffer (m) and vectors (sumV,meanV and varV)
if( p->execCnt == 0 )
{
p->sumV = cmMemResizeZ( double, p->sumV, p->dimCnt);
p->meanV = cmMemResizeZ( double, p->meanV, p->dimCnt );
p->varV = cmMemResizeZ( double, p->varV, p->dimCnt );
p->m = cmMemResizeZ( double, p->m, p->dimCnt * p->wndEleCnt );
}
// if the storage matrix is full
if( p->curIdx == p->wndEleCnt )
return rc;
// get the pointer to this data source reg recd
assert( regId < p->regArrayN);
cmStatsProcRecd* r = p->regArrayV + regId;
// the dimensionality of the incoming data must be <= the registered dimensionality
assert( r->cnt <= vCnt );
unsigned dimIdx = r->idx;
bool updateFl = cmIsFlag(p->flags,kUpdateOnExecStatProcFl);
double* sbp = p->sumV + dimIdx; // sum base ptr
// mbp point to a segment (mbp[vCnt]) in column p->curIdx
double* mbp = p->m + (p->curIdx * p->dimCnt) + dimIdx; // mem col base ptr
const double* mep = p->m + p->dimCnt * p->wndEleCnt;
// decr the current col segment from the sum
if( updateFl )
cmVOD_SubVV( sbp, vCnt, mbp );
assert( p->m <= mbp && mbp < mep && p->m <= mbp+vCnt && mbp+vCnt <= mep );
// copy in the incoming values to mem col segment
cmVOD_Copy( mbp, vCnt, v );
if( updateFl )
{
// incr the sum from the incoming value
cmVOD_AddVV( sbp, vCnt, mbp );
// use the new sum to compute new mean values
cmVOD_DivVVS( p->meanV + dimIdx, vCnt, sbp, p->curCnt );
// update the variance - cmross each row
unsigned di;
for(di=dimIdx; di<dimIdx+vCnt; ++di )
p->varV[di] = cmVOD_VarianceN( p->m + dimIdx, p->curCnt, p->dimCnt, p->meanV + dimIdx );
}
++p->execCnt;
return cmOkRC;
}
cmRC_t cmStatsProcExecF( cmStatsProc* p, unsigned regId, const float v[], unsigned vCnt )
{
double dv[ vCnt ];
cmVOD_CopyF(dv,vCnt,v);
cmStatsProcExecD(p,regId,dv,vCnt);
return cmOkRC;
}
cmRC_t cmStatsProcCalc(cmStatsProc* p )
{
unsigned colCnt = cmMin(p->curCnt,p->wndEleCnt);
unsigned i = 0;
cmVOD_Fill(p->sumV,p->dimCnt,0);
// sum the ith col of p->m[] into p->sumV[i]
for(; i<colCnt; ++i)
{
cmVOD_AddVV( p->sumV, p->dimCnt, p->m + (i * p->dimCnt) );
if( p->mfp != NULL )
cmMtxFileDoubleExec( p->mfp, p->sumV, p->dimCnt, 1 );
}
// calc the mean of each row
cmVOD_DivVVS( p->meanV, p->dimCnt, p->sumV, colCnt );
// calc the variance cmross each row
for(i=0; i<p->dimCnt; ++i)
p->varV[i] = cmVOD_VarianceN(p->m + i, colCnt, p->dimCnt, p->meanV + i );
return cmOkRC;
}
cmRC_t cmStatsProcAdvance( cmStatsProc* p )
{
++p->curIdx;
if( p->curIdx > p->wndEleCnt )
p->curIdx = 0;
p->curCnt = cmMin(p->curCnt+1,p->wndEleCnt);
return cmOkRC;
}
void cmStatsProcTest( cmVReportFuncPtr_t vReportFunc )
{
enum
{
wndEleCnt = 7,
dDimCnt = 3,
fDimCnt = 2,
dimCnt = dDimCnt + fDimCnt,
kTypeId0 = 0,
kTypeId1 = 1
};
unsigned flags = 0;
unsigned i;
double dd[ dDimCnt * wndEleCnt ] =
{
0, 1, 2,
3, 4, 5,
6, 7, 8,
9, 10, 11,
12, 13, 14,
15, 16, 17,
18, 19, 20
};
float fd[ 14 ] =
{
0, 1,
2, 3,
4, 5,
6, 7,
8, 9,
10, 11,
12, 13
};
cmCtx c;
cmCtxInit(&c, vReportFunc, vReportFunc, NULL );
cmStatsProc* p = cmStatsProcAlloc( &c, NULL, wndEleCnt, flags );
unsigned regId0 = cmStatsProcReg( p, kTypeId0, dDimCnt );
unsigned regId1 = cmStatsProcReg( p, kTypeId1, fDimCnt );
for(i=0; i<wndEleCnt; ++i)
{
cmStatsProcExecD( p, regId0, dd + (i*dDimCnt), dDimCnt );
cmStatsProcExecF( p, regId1, fd + (i*fDimCnt), fDimCnt );
cmStatsProcAdvance(p);
}
cmStatsProcCalc( p);
cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->meanV );
cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->varV );
cmStatsProcFree(&p);
}
*/
//------------------------------------------------------------------------------------------------------------
cmBeatHist* cmBeatHistAlloc( cmCtx* c, cmBeatHist* ap, unsigned frmCnt )
{
cmBeatHist* p = cmObjAlloc(cmBeatHist,c,ap);
p->fft = cmFftAllocRR(c,NULL,NULL,0,kToPolarFftFl);
p->ifft = cmIFftAllocRR(c,NULL,0);
if( frmCnt > 0 )
if( cmBeatHistInit(p,frmCnt) != cmOkRC )
cmBeatHistFree(&p);
return p;
}
cmRC_t cmBeatHistFree( cmBeatHist** pp )
{
cmRC_t rc = cmOkRC;
cmBeatHist* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmBeatHistFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->m);
cmMemPtrFree(&p->H);
cmMemPtrFree(&p->df);
cmMemPtrFree(&p->fdf);
cmMemPtrFree(&p->histV);
cmFftFreeRR(&p->fft);
cmIFftFreeRR(&p->ifft);
cmObjFree(&p);
return rc;
}
void _cmBeatHistInitH( cmReal_t* H, unsigned hrn, unsigned hcn, unsigned ri, unsigned c0, unsigned c1 )
{
unsigned ci;
for(ci=c0; ci<=c1; ++ci)
H[ (ci*hrn) + ri ] = 1;
}
cmRC_t cmBeatHistInit( cmBeatHist* p, unsigned frmCnt )
{
cmRC_t rc;
unsigned i,j,k;
enum { kLagFact = 4, kHistBinCnt=15, kHColCnt=128 };
if((rc = cmBeatHistFinal(p)) != cmOkRC )
return rc;
p->frmCnt = frmCnt;
p->maxLagCnt = (unsigned)floor(p->frmCnt / kLagFact);
p->histBinCnt= kHistBinCnt;
p->hColCnt = kHColCnt;
p->dfi = 0;
unsigned cfbMemN = p->frmCnt * p->maxLagCnt;
unsigned hMemN = p->histBinCnt * kHColCnt;
//cmArrayResizeVZ(p->obj.ctx,&p->cfbMem, cmReal_t, &p->m, cfbMemN, &p->H, hMemN, NULL);
p->m = cmMemResizeZ( cmReal_t, p->m, cfbMemN );
p->H = cmMemResizeZ( cmReal_t, p->H, hMemN );
//p->df = cmArrayResizeZ(c,&p->dfMem, 2*p->frmCnt + kHistBinCnt, cmReal_t);
//p->fdf = p->df + p->frmCnt;
//p->histV = p->fdf + p->frmCnt;
//cmArrayResizeVZ(p->obj.ctx, &p->dfMem, cmReal_t, &p->df, p->frmCnt, &p->fdf, p->frmCnt, &p->histV, kHistBinCnt, NULL );
p->df = cmMemResizeZ( cmReal_t, p->df, p->frmCnt );
p->fdf = cmMemResizeZ( cmReal_t, p->fdf, p->frmCnt );
p->histV = cmMemResizeZ( cmReal_t, p->histV, kHistBinCnt );
cmFftInitRR( p->fft,NULL,cmNextPowerOfTwo(2*frmCnt),kToPolarFftFl);
cmIFftInitRR(p->ifft,p->fft->binCnt);
// initialize H
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 0, 103, 127 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 1, 86, 102 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 2, 73, 85 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 3, 64, 72 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 4, 57, 63 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 5, 51, 56 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 6, 46, 50 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 7, 43, 45 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 8, 39, 42 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 9, 36, 38 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 10,32, 35 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 11,28, 31 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 12,25, 27 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 13,21, 24 );
_cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 14,11, 20 );
// for each column
for(i=0; i<p->maxLagCnt; ++i)
{
// for each lag group
for(j=0; j<kLagFact; ++j)
{
for(k=0; k<=2*j; ++k)
{
unsigned idx = (i*p->frmCnt) + (i*j) + i + k;
if( idx < cfbMemN )
p->m[ idx ] = 1.0/(2*j+1);
}
}
}
double g[ p->maxLagCnt ];
double g_max = 0;
double b = 43;
b = b*b;
for(i=0; i<p->maxLagCnt; ++i)
{
double n = i+1;
g[i] = n/b * exp(-(n*n) / (2*b));
if( g[i] > g_max )
g_max = g[i];
}
// normalize g[]
cmVOD_DivVS( g, p->maxLagCnt, g_max );
// for each column of p->m[]
for(i=0; i<p->maxLagCnt; ++i)
{
double gg = g[i];
k = i*p->frmCnt;
for(j=0; j<p->frmCnt; ++j)
p->m[ k + j ] *= gg;
}
//p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"beatHist");
return cmOkRC;
}
cmRC_t cmBeatHistFinal( cmBeatHist* p )
{
//if( p != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
return cmOkRC;
}
cmRC_t cmBeatHistExec( cmBeatHist* p, cmSample_t df )
{
if( p->dfi < p->frmCnt )
p->df[p->dfi++] = df;
return cmOkRC;
}
cmRC_t cmBeatHistCalc( cmBeatHist* p )
{
unsigned i;
// df -= mean(df)
cmVOR_SubVS(p->df,p->frmCnt,cmVOR_Mean(p->df,p->frmCnt));
//cmPlotLineR( "dfm", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
// take alpha norm of df
double alpha = 9;
cmVOR_DivVS(p->df,p->frmCnt, cmVOR_AlphaNorm(p->df,p->frmCnt,alpha));
//cmPlotLineS( "dfd", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
// low pass forward/backward filter df[] into fdf[]
cmReal_t b[] = {0.1600, 0.3200, 0.1600};
unsigned bn = sizeof(b)/sizeof(b[0]);
cmReal_t a[] = {1.0000, -0.5949, 0.2348};
unsigned an = sizeof(a)/sizeof(a[0]);
cmFilterFilterR(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
//cmPlotLineS( "fdf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
// median filter to low-passed filtered fdf[] into df[]
cmVOR_FnThresh(p->fdf,p->frmCnt,16,p->df,1,NULL);
// subtract med filtered signal from the low pa1ssed signal.
// fdf[] -= df[];
cmVOR_SubVV(p->fdf,p->frmCnt,p->df);
// half-wave rectify fdf[] = set all negative values in fdf[] to zero.
cmVOR_HalfWaveRectify(p->fdf,p->frmCnt,p->fdf);
//cmPlotLineS( "meddf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
// take FT of fdf[]
cmFftExecRR(p->fft,p->fdf,p->frmCnt);
// square FT magn.
cmVOR_PowVS(p->fft->magV,p->fft->binCnt,2);
//cmPlotLineS( "mag", NULL, p->fft->magV, NULL, p->fft->binCnt, NULL, kSolidPlotLineId );
// take the IFFT of the squared magnitude vector.
cmVOR_Fill(p->fft->phsV,p->fft->binCnt,0);
cmIFftExecRectRR(p->ifft,p->fft->magV,p->fft->phsV);
// Matlab automatically provides this scaling as part of the IFFT.
cmVOR_DivVS(p->ifft->outV,p->ifft->outN,p->ifft->outN);
// remove bias for short periods from CMF
for(i=0; i<p->frmCnt; ++i)
p->ifft->outV[i] /= (p->frmCnt - i);
//cmPlotLineS( "cm", NULL, p->ifft->outV, NULL, p->frmCnt, NULL, kSolidPlotLineId );
// apply comb filter to the CMF and store result in df[maxLagCnt]
cmVOR_MultVMtV(p->df,p->maxLagCnt,p->m,p->frmCnt,p->ifft->outV);
//acPlotLineS( "cfb", NULL, p->df, NULL, p->maxLagCnt, NULL, kSolidPlotLineId );
//acVOR_Print(p->obj.err.rpt,1,p->maxLagCnt,p->df);
assert( p->maxLagCnt == p->hColCnt );
cmVOR_MultVMV(p->histV,p->histBinCnt,p->H,p->hColCnt,p->df);
cmReal_t bins[] = { 25, 17, 13, 9, 7, 6, 5, 3, 4, 3, 4, 4, 3, 4, 10};
cmVOR_DivVV( p->histV, p->histBinCnt, bins );
//cmPlotLineS( "cfb", NULL, p->histV, NULL, p->histBinCnt, NULL, kSolidPlotLineId );
if( p->mfp != NULL )
cmMtxFileRealExec( p->mfp, p->histV, p->histBinCnt );
return cmOkRC;
}
//------------------------------------------------------------------------------------------------------------
cmGmm_t* cmGmmAlloc( cmCtx* c, cmGmm_t* ap, unsigned K, unsigned D, const cmReal_t* gM, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
{
cmGmm_t* p = cmObjAlloc( cmGmm_t, c, ap );
if( K > 0 && D > 0 )
if( cmGmmInit(p,K,D,gM,uM,sMM,uflags) != cmOkRC )
cmGmmFree(&p);
return p;
}
cmRC_t cmGmmFree( cmGmm_t** pp )
{
cmRC_t rc = cmOkRC;
cmGmm_t* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmGmmFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->gV);
cmMemPtrFree(&p->uM);
cmMemPtrFree(&p->sMM);
cmMemPtrFree(&p->isMM);
cmMemPtrFree(&p->uMM);
cmMemPtrFree(&p->logDetV);
cmMemPtrFree(&p->t);
cmObjFree(pp);
return rc;
}
cmRC_t _cmGmmUpdateCovar( cmGmm_t* p, const cmReal_t* sMM )
{
unsigned i;
if( sMM == NULL )
return cmOkRC;
unsigned De2 = p->D*p->D;
unsigned KDe2 = p->K*De2;
cmVOR_Copy(p->sMM, KDe2, sMM);
cmVOR_Copy(p->isMM,KDe2, sMM);
cmVOR_Copy(p->uMM, KDe2, sMM);
//if( cmIsFlag(p->uflags,cmGmmCovarNoProcFl) )
// return cmOkRC;
// for each component
for(i=0; i<p->K; ++i)
{
cmReal_t* is = p->isMM + i*De2;
cmReal_t* u = p->uMM + i*De2;
cmReal_t* r;
// if the covariance matrix is diagnal
if( cmIsFlag(p->uflags,cmGmmDiagFl))
{
p->logDetV[i] = cmVOR_LogDetDiagM(is,p->D); // calc the det. of diag. covar. mtx
r = cmVOR_InvDiagM(is,p->D); // calc the inverse in place
}
else
{
p->logDetV[i] = cmVOR_LogDetM(is,p->D); // calc the det. of covar mtx
r = cmVOR_InvM(is,p->D); // calc the inverse in place
}
if( fabs(p->logDetV[i]) < 1e-20 )
{
cmCtxPrint(p->obj.ctx,"%i\n",i);
cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
}
if( cmVOR_CholZ(u,p->D) == NULL )
{
return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (Cholesky factorization failed.) was encountered in _cmGmmUpdateCovar().");
}
if( p->logDetV[i] == 0 )
{
cmGmmPrint(p,true);
return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (det==0) was encountered in _cmGmmUpdateCovar().");
}
if( r == NULL )
{
//cmCtxPrint(c,"%i\n",i);
//cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (inversion failed) was encountered in _cmGmmUpdateCovar().");
}
}
return cmOkRC;
}
cmRC_t cmGmmInit( cmGmm_t* p, unsigned K, unsigned D, const cmReal_t* gV, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
{
cmRC_t rc;
if((rc = cmGmmFinal(p)) != cmOkRC )
return rc;
// gM[K] uM[DK] sMM[DDK] isMM[DDK]+ uMM[DDK] logDetV[K] t[DD] fact[K]
/*
unsigned n = K + (D*K) + (D*D*K) + (D*D*K) + (D*D*K) + K + (D*D) + K;
p->gV = cmArrayResizeZ(c,&p->memA, n, cmReal_t );
p->uM = p->gV + K;
p->sMM = p->uM + (D*K);
p->isMM = p->sMM + (D*D*K);
p->uMM = p->isMM + (D*D*K);
p->logDetV = p->uMM + (D*D*K);
p->t = p->logDetV + K;
*/
//cmArrayResizeVZ(c, &p->memA, cmReal_t, &p->gV,K, &p->uM,D*K, &p->sMM,D*D*K,
//&p->isMM,D*D*K, &p->uMM,D*D*K, &p->logDetV,K, &p->t,D*D, NULL );
p->gV = cmMemResizeZ( cmReal_t, p->gV, K );
p->uM = cmMemResizeZ( cmReal_t, p->uM, D*K);
p->sMM = cmMemResizeZ( cmReal_t, p->sMM, D*D*K);
p->isMM = cmMemResizeZ( cmReal_t, p->isMM, D*D*K);
p->uMM = cmMemResizeZ( cmReal_t, p->uMM, D*D*K);
p->logDetV = cmMemResizeZ( cmReal_t, p->logDetV, K);
p->t = cmMemResizeZ( cmReal_t, p->t, D*D );
p->K = K;
p->D = D;
p->uflags = uflags;
if( gV != NULL )
cmVOR_Copy(p->gV,K,gV);
if( uM != NULL )
cmVOR_Copy(p->uM,D*K,uM);
return _cmGmmUpdateCovar(p,sMM );
}
cmRC_t cmGmmFinal( cmGmm_t* p )
{ return cmOkRC; }
typedef struct
{
cmGmm_t* p;
const cmReal_t* xM;
unsigned colCnt;
} _cmGmmRdFuncData_t;
const cmReal_t* _cmGmmReadFunc( void* userPtr, unsigned colIdx )
{
assert(colIdx < ((const _cmGmmRdFuncData_t*)userPtr)->colCnt);
return ((const _cmGmmRdFuncData_t*)userPtr)->xM + (colIdx * ((const _cmGmmRdFuncData_t*)userPtr)->p->D);
}
// xM[D,xN]
// yV[xN]
// yM[xN,K]
cmRC_t cmGmmEval( cmGmm_t* p, const cmReal_t* xM, unsigned xN, cmReal_t* yV, cmReal_t* yM )
{
_cmGmmRdFuncData_t r;
r.colCnt = xN;
r.p = p;
r.xM = xM;
return cmGmmEval2(p,_cmGmmReadFunc,&r,xN,yV,yM);
}
cmRC_t cmGmmEval2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, cmReal_t* yV, cmReal_t* yM)
{
cmReal_t tV[xN];
unsigned k;
//cmVOR_PrintL("cV: ",p->obj.err.rpt, 1, p->K, p->gV);
//cmVOR_PrintL("uM:\n",p->obj.err.rpt, p->D, p->K, p->uM );
//
cmVOR_Fill(yV,xN,0);
// for each component PDF
for(k=0; k<p->K; k++)
{
const cmReal_t* meanV = p->uM + (k*p->D);
const cmReal_t* isM = p->isMM + (k*p->D*p->D);
cmReal_t* pV;
if( yM == NULL )
pV = tV;
else
pV = yM + (k*xN);
// evaluate the kth component PDF with xM[1:T]
//cmVOR_MultVarGaussPDF2( pV, xM, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
cmVOR_MultVarGaussPDF3( pV, readFunc, userFuncPtr, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
// apply the kth component weight
cmVOR_MultVS( pV, xN, p->gV[k] );
// sum the result into the output vector
cmVOR_AddVV( yV, xN, pV );
}
return cmOkRC;
}
// Evaluate each component for a single data point
// xV[D] - observed data point
// yV[K] - output contains the evaluation for each component
cmRC_t cmGmmEval3( cmGmm_t* p, const cmReal_t* xV, cmReal_t* yV )
{
unsigned k;
for(k=0; k<p->K; ++k)
{
const cmReal_t* meanV = p->uM + (k*p->D);
const cmReal_t* isM = p->isMM + (k*p->D*p->D);
// evaluate the kth component PDF with xM[1:T]
cmVOR_MultVarGaussPDF2( yV + k, xV, meanV, isM, p->logDetV[k], p->D, 1, cmIsFlag(p->uflags,cmGmmDiagFl) );
// apply the kth component weight
yV[k] *= p->gV[k];
}
return cmOkRC;
}
cmReal_t _cmGmmKmeansDistFunc( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
{ return cmVOR_EuclidDistance(v0,v1,vn); }
cmRC_t cmGmmRandomize2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* funcUserPtr, unsigned xN, const cmReal_t* uM, const bool* roFlV )
{
unsigned k;
unsigned iV[ p->K ];
if( uM == NULL )
roFlV = NULL;
// randomize the mixture coefficients
cmVOR_Random( p->gV, p->K, 0.0, 1.0 );
cmVOR_NormalizeProbability(p->gV,p->K);
// fill iV with random integers between 0 and xN-1
cmVOU_Random( iV, p->K, xN-1 );
// for each component
for(k=0; k<p->K; ++k)
{
cmReal_t r[ p->D ];
// if this component's mean is not read-only
if( roFlV==NULL || roFlV[k]==false )
{
const cmReal_t* xV = NULL;
if( uM == NULL )
xV = readFunc( funcUserPtr, iV[k] ); // read a random frame index
else
xV = uM + (k*p->D); // select a user supplied mean vector
assert( xV != NULL );
// set the random feature vector as this components mean value
cmVOR_Copy(p->uM+(k*p->D),p->D,xV);
}
cmReal_t* sM = p->sMM+(k*p->D*p->D);
// create a random covariance mtx
if( cmIsFlag(p->uflags,cmGmmDiagFl) )
{
// create a random diag. covar mtx
cmVOR_Random(r,p->D,0.0,1.0);
cmVOR_Diag(sM,p->D,r);
}
else
{
// create a random symetric positive definite matrix
cmVOR_RandSymPosDef(sM,p->D,p->t);
}
}
unsigned* classIdxV = cmMemAllocZ(unsigned, xN );
// if some components have read-only mean's
if( uM != NULL && roFlV != NULL )
{
assert( xN >= p->K );
for(k=0; k<p->K; ++k)
classIdxV[k] = roFlV[k];
}
// use kmeans clustering to move the means closer to their center values
if( cmIsFlag( p->uflags, cmGmmSkipKmeansFl) == false )
cmVOR_Kmeans2( classIdxV, p->uM, p->K, readFunc, p->D, xN, funcUserPtr, _cmGmmKmeansDistFunc, NULL, -1, 0 );
cmMemPtrFree(&classIdxV);
return _cmGmmUpdateCovar(p,p->sMM);
}
cmRC_t cmGmmRandomize( cmGmm_t* p, const cmReal_t* xM, unsigned xN )
{
_cmGmmRdFuncData_t r;
r.colCnt = xN;
r.p = p;
r.xM = xM;
return cmGmmRandomize2(p,_cmGmmReadFunc,&r,xN,NULL,NULL);
}
// xM[D,xN]
cmRC_t cmGmmTrain( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
{
unsigned i,k;
cmRC_t rc;
if((rc = cmGmmRandomize(p,xM,xN)) != cmOkRC )
return rc;
//cmGmmPrint(c,p);
// wM[xN,K]
cmReal_t wM[ xN * p->K ]; // wM[N,K] soft assignment mtx
unsigned wV[ xN ]; // wV[N] hard assignment vector
unsigned stopCnt = 0;
unsigned curStopCnt = 0;
if( iterCntPtr != NULL )
{
stopCnt = *iterCntPtr;
*iterCntPtr = 0;
}
else
{
// BUG BUG BUG
// stopCnt is used uninitialized when iterCntPtr == NULL
assert( 0 );
}
while(1)
{
//cmVOR_NormalizeProbability(p->gV,p->K);
cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
//cmGmmPrint(c,p);
for(k=0; k<p->K; ++k)
{
cmReal_t* wp = wM + (k*xN);
// calc the prob that each data point in xM[] was generated by the kth gaussian
// and store as a column vector in wM[:,k]
cmVOR_MultVarGaussPDF2( wp, xM, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
// scale the prob by the gain coeff for gaussian k
cmVOR_MultVS( wp, xN, p->gV[k]);
}
//cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
bool doneFl = true;
for(i=0; i<xN; ++i)
{
// form a probability for the ith data point weights
cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
// select the cluster to which the ith data point is most likely to belong
unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
// if the ith data point changed clusters
if( mi != wV[i] )
{
doneFl = false;
wV[i] = mi;
}
}
curStopCnt = doneFl ? curStopCnt+1 : 0;
// if no data points changed owners then the clustering is complete
if( curStopCnt == stopCnt )
{
//cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
break;
}
// for each cluster
for(k=0; k<p->K; ++k)
{
cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
// update the kth weight
p->gV[k] = sw / xN;
// form a sum of all data points weighted by their soft assignment to cluster k
cmReal_t sumV[p->D];
cmVOR_MultVMV( sumV, p->D, xM, xN, wM + k*xN );
// update the mean[k]
cmVOR_DivVVS(uV, p->D, sumV, sw );
// update the covar[k]
// sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
cmVOR_Fill(sM,p->D*p->D,0);
for(i=0; i<xN; ++i)
{
cmReal_t duV[ p->D ];
cmVOR_SubVVV( duV, p->D, xM + (i*p->D), uV ); // duV[] = xM[:,i] - uV[];
cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
}
cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
}
// update the inverted covar mtx and covar det.
if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
return rc;
if( iterCntPtr != NULL )
*iterCntPtr += 1;
}
return cmOkRC;
}
// xM[D,xN]
cmRC_t cmGmmTrain2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, unsigned* iterCntPtr, const cmReal_t* uM, const bool* roFlV, int maxIterCnt )
{
unsigned i,j,k;
cmRC_t rc;
// if uM[] is not set then ignore roFlV[]
if( uM == NULL )
roFlV=NULL;
if((rc = cmGmmRandomize2(p,readFunc,userFuncPtr,xN,uM,roFlV)) != cmOkRC )
return rc;
//cmGmmPrint(c,p);
// wM[xN,K] soft assignment mtx
cmReal_t* wM = cmMemAlloc(cmReal_t, xN * p->K );
// wV[N] hard assignment vector
unsigned* wV = cmMemAlloc(unsigned, xN );
unsigned stopCnt = 0;
unsigned curStopCnt = 0;
if( iterCntPtr != NULL )
{
stopCnt = *iterCntPtr;
*iterCntPtr = 0;
}
else
{
// BUG BUG BUG
// stopCnt is used uninitialized when iterCntPtr == NULL
assert( 0 );
}
while(1)
{
//cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
//cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
//cmGmmPrint(c,p);
for(k=0; k<p->K; ++k)
{
cmReal_t* wp = wM + (k*xN);
// calc the prob that each data point in xM[] was generated by the kth gaussian
// and store as a column vector in wM[:,k]
cmVOR_MultVarGaussPDF3( wp, readFunc, userFuncPtr, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
// scale the prob by the gain coeff for gaussian k
cmVOR_MultVS( wp, xN, p->gV[k]);
}
//cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
bool doneFl = true;
unsigned changeCnt = 0;
for(i=0; i<xN; ++i)
{
// form a probability for the ith data point weights
cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
// select the cluster to which the ith data point is most likely to belong
unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
// if the ith data point changed clusters
if( mi != wV[i] )
{
++changeCnt;
doneFl = false;
wV[i] = mi;
}
}
curStopCnt = doneFl ? curStopCnt+1 : 0;
printf("%i stopCnt:%i changeCnt:%i\n",*iterCntPtr,curStopCnt,changeCnt);
// if no data points changed owners then the clustering is complete
if( curStopCnt == stopCnt )
{
//cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
break;
}
// if a maxIterCnt was given and the cur iter cnt exceeds the max iter cnt then stop
if( maxIterCnt>=1 && *iterCntPtr >= maxIterCnt )
break;
// form a sum of all data points weighted by their soft assignment to cluster k
// NOTE: cmGmmTrain() performs this step more efficiently because it use
// an LAPACK matrix multiply.
cmReal_t sumM[ p->D * p->K ];
cmVOR_Zero(sumM,p->D*p->K);
for(i=0; i<xN; ++i)
{
const cmReal_t* xV = readFunc( userFuncPtr, i );
assert( xV != NULL );
for(k=0; k<p->K; ++k)
{
cmReal_t weight = wM[ i + (k*xN)];
for(j=0; j<p->D; ++j)
sumM[ j + (k*p->D) ] += xV[j] * weight;
}
}
// for each cluster that is not marked as read-only
for(k=0; k<p->K; ++k)
{
cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
// update the kth weight
p->gV[k] = sw / xN;
// if this component's mean is not read-only
if( (roFlV==NULL || roFlV[k]==false) && sw != 0)
{
// get vector of all data points weighted by their soft assignment to cluster k
cmReal_t* sumV = sumM + (k*p->D); // sumV[p->D];
// update the mean[k]
cmVOR_DivVVS(uV, p->D, sumV, sw );
}
// update the covar[k]
// sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
cmVOR_Fill(sM,p->D*p->D,0);
for(i=0; i<xN; ++i)
{
cmReal_t duV[ p->D ];
const cmReal_t* xV = readFunc( userFuncPtr, i );
assert( xV != NULL );
cmVOR_SubVVV( duV, p->D, xV, uV ); // duV[] = xM[:,i] - uV[];
cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
}
if( sw != 0 )
cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
}
// update the inverted covar mtx and covar det.
if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
goto errLabel;
if( iterCntPtr != NULL )
*iterCntPtr += 1;
}
cmMemPtrFree(&wM);
cmMemPtrFree(&wV);
errLabel:
return cmOkRC;
}
cmRC_t cmGmmTrain3( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
{
_cmGmmRdFuncData_t r;
r.colCnt = xN;
r.p = p;
r.xM = xM;
return cmGmmTrain2(p,_cmGmmReadFunc,&r,xN,iterCntPtr,NULL,NULL,-1);
}
cmRC_t cmGmmGenerate( cmGmm_t* p, cmReal_t* yM, unsigned yN )
{
unsigned i=0;
unsigned kV[yN];
// use weighted random selection to choose the component for each output value
cmVOR_WeightedRandInt(kV,yN, p->gV, p->K );
// cmVOU_Print(p->obj.err.rpt,1,yN,kV);
for(i=0; i<yN; ++i)
{
const cmReal_t* uV = p->uM + (kV[i] * p->D);
unsigned idx = kV[i] * p->D * p->D;
//cmVOR_PrintL("sM\n",p->obj.err.rpt,p->D,p->D,sM);
if( cmIsFlag(p->uflags,cmGmmDiagFl) )
{
const cmReal_t* sM = p->sMM + idx;
cmVOR_RandomGaussDiagM( yM + (i*p->D), p->D, 1, uV, sM );
}
else
{
const cmReal_t* uM = p->uMM + idx;
cmVOR_RandomGaussNonDiagM2(yM + (i*p->D), p->D, 1, uV, uM );
}
}
return cmOkRC;
}
void cmGmmPrint( cmGmm_t* p, bool fl )
{
unsigned k;
//cmCtx* c = p->obj.ctx;
cmVOR_PrintL("gV: ", p->obj.err.rpt, 1, p->K, p->gV );
cmVOR_PrintL("mM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
for(k=0; k<p->K; ++k)
cmVOR_PrintL("sM:\n", p->obj.err.rpt, p->D, p->D, p->sMM + (k*p->D*p->D));
if( fl )
{
for(k=0; k<p->K; ++k)
cmVOR_PrintL("isM:\n", p->obj.err.rpt, p->D, p->D, p->isMM + (k*p->D*p->D));
for(k=0; k<p->K; ++k)
cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->D, p->uMM + (k*p->D*p->D));
cmVOR_PrintL("logDetV:\n", p->obj.err.rpt, 1, p->K, p->logDetV);
}
}
void cmGmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
cmCtx* c = cmCtxAlloc(NULL,rpt,lhH,stH);
unsigned K = 2;
unsigned D = 2;
cmReal_t gV[ 2 ] = { .5, .5 };
cmReal_t uM[ 4 ] = { .3, .3, .8, .8 };
cmReal_t sMM[ 8 ] = { .1, 0, 0, .1, .1, 0, 0, .1 };
unsigned flags = cmGmmDiagFl ;
unsigned M = 100;
cmReal_t xM[ D*M ];
cmReal_t yV[ M ];
unsigned i,j;
cmPlotSetup("MultDimGauss Test",1,1);
cmGmm_t* p = cmGmmAlloc(c, NULL, K, D, gV, uM, sMM, flags );
if(0)
{
cmGmmPrint(p,true);
for(i=0; i<10; i++)
for(j=0; j<20; j+=2)
{
xM[(i*20)+j] = .1 * i;;
xM[(i*20)+j + 1] = .1 * (j/2);;
}
// octave equivalent
// x0= .1 * ones(1,10);
// x = [ 0*x0 1*x0 2*x0 3*x0 4*x0 5*x0 6*x0 7*x0 8*x0 9*x0];
// x = [x; repmat([0:.1:1],1,10)];
// y = mvnpdf(x',[.3 .3],[.1 0; 0 .1]); plot(y);
cmGmmEval(p,xM,M,yV,NULL);
//cmVOR_PrintL( "xM\n", rpt, D, M, xM );
cmVOR_PrintL( "yV\n", rpt, 10, 10, yV );
//printf("y:%f\n",yV[0]);
cmPlotLineD( NULL, NULL, yV, NULL, M, NULL, kSolidPlotLineId );
}
if(0)
{
cmReal_t yM[ D*M ];
cmReal_t yMt[ M*D ];
cmReal_t uMt[ p->K*D];
unsigned iterCnt = 10;
//srand( time(NULL) );
cmGmmGenerate( p, yM, M );
p->uflags = 0; // turn off diagonal condition
if( cmGmmTrain3( p, yM, M, &iterCnt ) != cmOkRC )
return;
cmCtxPrint(c,"iterCnt:%i\n",iterCnt);
cmGmmPrint( p, true );
cmVOR_Transpose(yMt, yM, D, M );
//cmVOR_PrintL("yMt\n",vReportFunc,M,D,yMt);
cmPlotLineD(NULL, yMt, yMt+M, NULL, M, "blue", kAsteriskPlotPtId );
cmVOR_Transpose( uMt, p->uM, D, p->K);
cmVOR_PrintL("uMt:\n", p->obj.err.rpt, p->K, p->D, uMt );
cmPlotLineD(NULL, uMt, uMt+p->K, NULL, p->D, "red", kXPlotPtId );
}
if(1)
{
cmGmmFree(&p);
cmReal_t cV0[] = { .7, .3 };
cmReal_t uM0[] = { .2, .1, .1, .2 };
cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
unsigned flags = 0;
K = 2;
D = 2;
cmGmm_t* p = cmGmmAlloc(c,NULL, K, D, cV0, uM0, sMM0, flags );
xM[0] = 0.117228;
xM[1] = 0.110079;
cmGmmEval(p,xM,1,yV,NULL);
cmCtxPrint(c,"y: %f\n",yV[0]);
}
cmPlotDraw();
cmGmmFree(&p);
cmCtxFree(&c);
}
//------------------------------------------------------------------------------------------------------------
cmChmm_t* cmChmmAlloc( cmCtx* c, cmChmm_t* ap, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
{
cmChmm_t* p = cmObjAlloc(cmChmm_t,c,ap);
if( stateN >0 && dimN > 0 )
if( cmChmmInit(p,stateN,mixN,dimN,iV,aM) != cmOkRC )
cmChmmFree(&p);
return p;
}
cmRC_t cmChmmFree( cmChmm_t** pp )
{
cmRC_t rc = cmOkRC;
cmChmm_t* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmChmmFinal(p)) != cmOkRC )
return rc;
cmMemPtrFree(&p->iV);
cmMemPtrFree(&p->aM);
cmMemPtrFree(&p->bV);
cmMemPtrFree(&p->bM);
cmObjFree(pp);
return cmOkRC;
}
cmRC_t cmChmmInit( cmChmm_t* p, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
{
cmRC_t rc;
unsigned i;
if((rc = cmChmmFinal(p)) != cmOkRC )
return rc;
// iV[] aM
/*
unsigned n = stateN + (stateN*stateN);
p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t );
p->aM = p->iV + stateN;
*/
//cmArrayResizeVZ(c,&p->memA, cmReal_t, &p->iV,stateN, &p->aM, stateN*stateN, NULL );
p->iV = cmMemResizeZ( cmReal_t, p->iV, stateN );
p->aM = cmMemResizeZ( cmReal_t, p->aM, stateN * stateN );
p->bV = cmMemResizeZ( cmGmm_t*, p->bV, stateN );
p->N = stateN;
p->K = mixN;
p->D = dimN;
if( iV != NULL )
cmVOR_Copy(p->iV,p->N,iV);
if( aM != NULL )
cmVOR_Copy(p->aM,p->N*p->N,aM);
for(i=0; i<p->N; ++i)
p->bV[i] = cmGmmAlloc( p->obj.ctx, NULL, p->K, p->D, NULL, NULL, NULL, 0 );
//p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"chmm");
return cmOkRC;
}
cmRC_t cmChmmFinal( cmChmm_t* p )
{
if( p != NULL )
{
unsigned i;
for(i=0; i<p->N; ++i)
cmGmmFree( &p->bV[i] );
cmMemPtrFree(&p->bM);
//if( p->mfp != NULL )
// cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
}
return cmOkRC;
}
cmRC_t cmChmmRandomize( cmChmm_t* p, const cmReal_t* oM, unsigned T )
{
cmRC_t rc;
unsigned i;
unsigned N = p->N;
// randomize the initial state probabilities
cmVOR_Random( p->iV, N, 0.0, 1.0 );
cmVOR_NormalizeProbability( p->iV, N );
// randomize the state transition matrix
cmVOR_Random( p->aM, N*N, 0.0, 1.0 );
for(i=0; i<N; ++i)
{
cmVOR_NormalizeProbabilityN( p->aM + i, N, N ); // rows of aM must sum to 1.0
if((rc = cmGmmRandomize( p->bV[i], oM, T )) != cmOkRC) // randomize the GMM assoc'd with state i
return rc;
}
cmMemPtrFree(&p->bM); // force bM[] to be recalculated
return cmOkRC;
}
cmReal_t _cmChmmKmeansDist( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
{ return cmVOR_EuclidDistance(v0,v1,vn); }
cmRC_t cmChmmSegKMeans( cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t threshProb, unsigned maxIterCnt, unsigned iterCnt )
{
cmCtx* c = p->obj.ctx;
cmRC_t rc = cmOkRC;
unsigned i,j,k,t;
unsigned N = p->N;
unsigned K = p->K;
unsigned D = p->D;
//unsigned qV[T];
//cmReal_t alphaM[N*T];
//unsigned clusterIdxV[T];
//cmReal_t centroidM[D*N];
/*
unsigned sz = 2*ALIGN_B(T,unsigned) +
ALIGN_B(N*T,cmReal_t) +
ALIGN_B(D*N,cmReal_t);
cmArray mem;
cmArrayAlloc(c, &mem);
unsigned* qV = (unsigned*) cmArrayResize(c, &mem, sz, char);
cmReal_t* alphaM = (cmReal_t*) (qV + ALIGN_T(T, unsigned));
unsigned* clusterIdxV = (unsigned*) (alphaM + ALIGN_T(N*T,cmReal_t));
cmReal_t* centroidM = (cmReal_t*) (clusterIdxV + ALIGN_T(T, unsigned));
*/
unsigned* qV = cmMemAlloc( unsigned, T );
cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T);
unsigned* clusterIdxV = cmMemAlloc( unsigned, T );
cmReal_t* centroidM = cmMemAlloc( cmReal_t, D*N);
cmReal_t logPr = 0;
bool reportFl = true;
cmChmmRandomize(p,oM,T);
// cluster the observations into N groups
cmVOR_Kmeans( qV, centroidM, N, oM, D, T, NULL, 0, false, _cmChmmKmeansDist, NULL );
for(i=0; i<maxIterCnt; ++i)
{
unsigned jnV[N];
if( reportFl )
cmCtxPrint(c,"SegKM: ----------------------------------------------------%i\n",i);
// get the count of data points in each state
cmVOU_Fill(jnV,N,0);
for(t=0; t<T; ++t)
++jnV[ qV[t] ];
// for each state
for(j=0; j<N; ++j)
{
cmGmm_t* g = p->bV[j];
// cluster all datapoints which were assigned to state j
cmVOR_Kmeans( clusterIdxV, g->uM, K, oM, D, T, qV, j, false, _cmChmmKmeansDist, NULL );
// for each cluster
for(k=0; k<K; ++k)
{
unsigned kN = 0;
// kN is count of data points assigned to cluster k
for(t=0; t<T; ++t)
if( clusterIdxV[t] == k )
++kN;
g->gV[k] = (cmReal_t)kN/jnV[j];
// the covar of the kth component is the sample covar of cluster k
cmVOR_GaussCovariance(g->sMM + (k*D*D), D, oM, T, g->uM + (k*D), clusterIdxV, k );
}
if((rc = _cmGmmUpdateCovar(g, g->sMM )) != cmOkRC )
goto errLabel;
}
if( i== 0 )
{
// count transitions from i to j
for(t=0; t<T-1; ++t)
p->aM[ (qV[t+1]*N) + qV[t] ] += 1.0;
for(j=0; j<N; ++j)
{
// normalize state transitions by dividing by times in each state
for(k=0; k<N; k++)
p->aM[ (k*N) + j ] /= jnV[j];
cmVOR_NormalizeProbabilityN(p->aM + j, N, N);
cmGmmEval( p->bV[j], oM, 1, p->iV + j, NULL );
}
}
if((rc = cmChmmTrain(p, oM, T, iterCnt,0,0 )) != cmOkRC )
goto errLabel;
// calculate the prob. that the new model generated the data
cmReal_t logPr0 = cmChmmForward(p,oM,T,alphaM,NULL);
cmReal_t dLogPr = logPr0 - logPr;
if( reportFl )
cmCtxPrint(c,"pr:%f d:%f\n",logPr0,dLogPr);
if( (dLogPr > 0) && (dLogPr < threshProb) )
break;
logPr = logPr0;
// fill qV[] with the state at each time t
cmChmmDecode(p,oM,T,qV);
}
errLabel:
cmMemPtrFree(&qV);
cmMemPtrFree(&alphaM);
cmMemPtrFree(&clusterIdxV);
cmMemPtrFree(&centroidM);
return rc;
}
cmRC_t cmChmmSetGmm( cmChmm_t* p, unsigned i, const cmReal_t* wV, const cmReal_t* uM, const cmReal_t* sMM, unsigned flags )
{
assert( i < p->N);
cmMemPtrFree(&p->bM); // force bM[] to be recalculated
return cmGmmInit(p->bV[i],p->K,p->D,wV,uM,sMM,flags);
}
// Return the probability of the observation for each state.
// oV[D] - multi-dim. observation data point
// pV[N] - probability of this observation for each state
void cmChmmObsProb( const cmChmm_t* p, const cmReal_t* oV, cmReal_t* prV )
{
unsigned i;
for(i=0; i<p->N; ++i)
cmGmmEval( p->bV[i], oV, 1, prV + i, NULL );
}
// oM[D,T] - observation matrix
// alphaM[N,T] - prob of being in each state and observtin oM(:,t)
// bM[N,T] - (optional) state-observation probability matrix
// logPrV[T] - (optional) record the log prob of the data given the model at each time step
// Returns sum(logPrV[T])
cmReal_t cmChmmForward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* alphaM, cmReal_t* logPrV )
{
unsigned t;
cmReal_t logPr = 0;
// calc the prob of starting in each state
if( p->bM == NULL )
cmChmmObsProb( p, oM, alphaM );
else
cmVOR_Copy( alphaM, p->N*T, p->bM );
cmVOR_MultVV( alphaM, p->N, p->iV );
cmReal_t s = cmVOR_Sum(alphaM,p->N);
cmVOR_DivVS(alphaM,p->N,s);
//cmVOR_PrintL("alpha:\n",p->obj.err.rpt,p->N,1,alphaM);
for(t=1; t<T; ++t)
{
cmReal_t tmp[p->N];
cmReal_t* alphaV = alphaM + t*p->N;
// calc the prob of the observation for each state
if( p->bM == NULL )
cmChmmObsProb(p,oM + (t*p->D), alphaV );
// calc. the prob. of transitioning to each state at time t, from each state at t-1
cmVOR_MultVVM(tmp,p->N, alphaM + ((t-1)*p->N), p->N, p->aM );
// calc the joint prob of transitioning from each state to each state and observing O(t).
cmVOR_MultVV(alphaV, p->N, tmp );
// scale the probabilities to prevent underflow
s = cmVOR_Sum(alphaV,p->N);
cmVOR_DivVS(alphaV,p->N,s);
// track the log prob. of the model having generated the data up to time t.
cmReal_t pr = log(s);
if( logPrV != NULL )
logPrV[t] = pr;
logPr += pr;
}
return logPr;
}
// oM[D,T]
// betaM[N,T]
void cmChmmBackward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* betaM )
{
cmVOR_Fill(betaM,p->N*T,1.0);
assert(T >= 2 );
int t = (int)T - 2;
for(; t>=0; --t)
{
cmReal_t tmp[p->N];
if( p->bM == NULL )
cmChmmObsProb(p,oM+((t+1)*p->D), tmp );
else
cmVOR_Copy(tmp,p->N,p->bM + ((t+1)*p->N));
cmVOR_MultVV(tmp,p->N,betaM + ((t+1)*p->N));
cmVOR_MultVMV(betaM+(t*p->N),p->N, p->aM, p->N, tmp );
cmVOR_NormalizeProbability(betaM+(t*p->N),p->N );
}
}
cmReal_t cmChmmCompare( const cmChmm_t* p0, const cmChmm_t* p1, unsigned T )
{
assert(p0->D == p1->D);
assert(p0->N == p1->N);
cmReal_t oM[p0->D*T];
cmReal_t alphaM[p0->N*T];
cmChmmGenerate(p0,oM,T,NULL);
cmReal_t logPr00 = cmChmmForward(p0,oM,T,alphaM,NULL);
cmReal_t logPr01 = cmChmmForward(p1,oM,T,alphaM,NULL);
cmChmmGenerate(p1,oM,T,NULL);
cmReal_t logPr10 = cmChmmForward(p0,oM,T,alphaM,NULL);
cmReal_t logPr11 = cmChmmForward(p1,oM,T,alphaM,NULL);
cmReal_t d0 = (logPr01-logPr00)/T;
cmReal_t d1 = (logPr10-logPr11)/T;
return (d0+d1)/2;
}
cmRC_t cmChmmGenerate( const cmChmm_t* p, cmReal_t* oM, unsigned T, unsigned* sV )
{
unsigned i,si;
// use weighted random selection to choose an intitial state
cmVOR_WeightedRandInt(&si, 1, p->iV, p->N );
for(i=0; i<T; ++i)
{
if( sV != NULL )
sV[i] = si;
// generate a random value using the GMM assoc'd with the current state
cmGmmGenerate( p->bV[si], oM + (i*p->D), 1 );
// choose the next state using the transition weights from the current state
cmVOR_WeightedRandInt(&si, 1, p->aM + (si*p->N), p->N );
}
return cmOkRC;
}
cmRC_t cmChmmDecode( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned* yV )
{
int i,j,t;
unsigned N = p->N;
//unsigned psiM[N*T];
//cmReal_t delta[N];
/*
unsigned sz = ALIGN_B(N*T,unsigned) + ALIGN_B(N,cmReal_t);
cmArray mem;
cmArrayAlloc(c, &mem);
unsigned* psiM = (unsigned*) cmArrayResize(c, &mem, sz, char);
cmReal_t* delta = (cmReal_t*) (psiM + ALIGN_T(N*T,unsigned));
*/
unsigned* psiM = cmMemAlloc( unsigned, N*T );
cmReal_t* delta= cmMemAlloc( cmReal_t, N );
// get the prob of starting in each state
if( p->bM == NULL )
cmChmmObsProb( p, oM, delta );
else
cmVOR_Copy( delta, N, p->bM );
cmVOR_MultVV( delta, p->N, p->iV );
cmVOR_NormalizeProbability(delta, p->N);
for(t=1; t<T; ++t)
{
cmReal_t mV[N];
const cmReal_t* ap = p->aM;
// calc. the most likely new state given the most likely prev state
// and the transition matrix
for(i=0; i<N; ++i)
{
const cmReal_t* dp = delta;
unsigned psiIdx = t*N + i;
mV[i] = *ap++ * *dp++;
psiM[ psiIdx ] = 0;
// find max value of: delta .* A(:,i)
for(j=1; j<N; ++j )
{
cmReal_t v = *ap++ * *dp++;
if( v > mV[i] )
{
mV[i] = v;
psiM[ psiIdx ] = j;
}
}
}
// mV[] now holds the prob. of the max likelihood state at time t
// for each possible state at t-1
// psiM[:,t] holds the index of the max likelihood state
// condition the most likely new state on the observations
if( p->bM == NULL )
cmChmmObsProb(p,oM + (t*p->D), delta);
else
cmVOR_Copy(delta, N, p->bM + (t*p->N) );
cmVOR_MultVV(delta, N, mV ); // condition it on the max. like current states
cmVOR_NormalizeProbability( delta, N ); // normalize the prob.
}
// unwind psiM[] to form the max. likelihood state sequence
yV[T-1] = cmVOR_MaxIndex(delta,N,1);
for(t=T-2; t>=0; --t)
yV[t] = psiM[ ((t+1)*N) + yV[t+1] ];
cmMemPtrFree(&psiM);
cmMemPtrFree(&delta);
return cmOkRC;
}
cmRC_t cmChmmTrain( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned iterCnt, cmReal_t thresh, unsigned flags )
{
cmRC_t rc = cmOkRC;
unsigned i,j,k,t,d;
unsigned iter;
unsigned N = p->N;
unsigned K = p->K;
unsigned D = p->D;
unsigned De2 = D * D;
bool mixFl = !cmIsFlag(flags,kNoTrainMixCoeffChmmFl);
bool meanFl = !cmIsFlag(flags,kNoTrainMeanChmmFl);
bool covarFl = !cmIsFlag(flags,kNoTrainCovarChmmFl);
bool bFl = mixFl | meanFl | covarFl;
bool calcBFl = true;
bool progFl = false;
bool timeProgFl = false;
cmReal_t progInc = 0.1;
cmReal_t progFrac = 0;
cmReal_t logProb = 0;
//cmReal_t alphaM[N*T]; // alpha[N,T]
//cmReal_t betaM[N*T]; // betaM[N,T]
//cmReal_t logPrV[T];
//cmReal_t EpsM[N*N];
//cmReal_t BK[N*K*T];
//cmReal_t gamma_jk[N*K];
//cmReal_t uM[K*D*N];
//cmReal_t sMM[K*De2*N];
/*
unsigned sz = ALIGN_T(N*T, cmReal_t) +
ALIGN_T(N*T, cmReal_t) +
ALIGN_T(T, cmReal_t) +
ALIGN_T(N*N, cmReal_t) +
ALIGN_T(N*K*T, cmReal_t) +
ALIGN_T(N*K, cmReal_t) +
ALIGN_T(K*D*N, cmReal_t) +
ALIGN_T(K*De2*N,cmReal_t);
cmArray mem;
cmArrayAlloc(c, &mem);
cmReal_t* alphaM = cmArrayResize(c, &mem, sz, cmReal_t); // alpha[N,T]
cmReal_t* betaM = alphaM + ALIGN_T(N*T, cmReal_t); // betaM[N,T]
cmReal_t* logPrV = betaM + ALIGN_T(N*T, cmReal_t);
cmReal_t* EpsM = logPrV + ALIGN_T(T, cmReal_t);
cmReal_t* BK = EpsM + ALIGN_T(N*N, cmReal_t);
cmReal_t* gamma_jk = BK + ALIGN_T(N*K*T,cmReal_t);
cmReal_t* uM = gamma_jk + ALIGN_T(N*K, cmReal_t);
cmReal_t* sMM = uM + ALIGN_T(K*D*N,cmReal_t);
*/
cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T );
cmReal_t* betaM = cmMemAlloc( cmReal_t, N*T );
cmReal_t* logPrV = cmMemAlloc( cmReal_t, T );
cmReal_t* EpsM = cmMemAlloc( cmReal_t, N*N );
cmReal_t* BK = cmMemAlloc( cmReal_t, N*K*T );
cmReal_t* gamma_jk = cmMemAlloc( cmReal_t, N*K );
cmReal_t* uM = cmMemAlloc( cmReal_t, K*D*N );
cmReal_t* sMM = cmMemAlloc( cmReal_t, K*De2*N );
if( thresh <=0 )
thresh = 0.0001;
//cmArrayResizeZ(c,&p->memC,N*T,cmReal_t);
p->bM = cmMemResizeZ( cmReal_t, p->bM, N*T);
for(iter=0; iter<iterCnt; ++iter)
{
// zero the mean and covar summation arrays
cmVOR_Fill(uM, K*D *N,0);
cmVOR_Fill(sMM, K*De2*N,0);
cmVOR_Fill(EpsM,N*N, 0);
cmVOR_Fill(gamma_jk,N*K,0);
//
// B[i,t] The prob that state i generated oM(:,t)
// BK[i,k,t] The prob that state i component k generated oM(:,t)
// Note: B[i,t] = sum(BK(i,k,:))
//
if( calcBFl || bFl )
{
calcBFl = false;
for(t=0; t<T; ++t)
{
// prob. that state i generated objservation O[t]
for(i=0; i<N; ++i )
cmGmmEval( p->bV[i], oM + (t*D), 1, p->bM + (t*N) + i, BK + (t*N*K) + (i*K) );
}
}
// alpha[N,T] is prob. of transitioning forward to each state given the observed data
cmReal_t logProb0 = cmChmmForward( p, oM, T, alphaM, logPrV );
// check for convergence
cmReal_t dLogProb = labs(logProb0-logProb) / ((labs(logProb0)+labs(logProb)+cmReal_EPSILON)/2);
if( dLogProb < thresh )
break;
logProb = logProb0;
// betaM[N,T] is prob of transitioning backward from each state given the observed data
cmChmmBackward(p, oM, T, betaM );
if(progFl)
cmCtxPrint(p->obj.ctx,"%i (%f) ",iter+1, dLogProb );
if(timeProgFl)
progFrac = progInc;
// for each time step
for(t=0; t<T-1; ++t)
{
// oV[D] is the observation at step t
const cmReal_t* oV = oM + (t*D);
//
// Update EpsM[N,N] (6.37)
// (prob. of being in state i at time t and transitioning
// to state j at time t+1)
//
cmReal_t E[N*N];
// for each possible state transition
for(i=0; i<N; ++i)
for(j=0; j<N; ++j)
{
E[ i + (j*N) ]
= exp(log(alphaM[ (t*N) + i ])
+ log(p->aM[ i + (j*N) ])
+ log(p->bM[ ((t+1)*N) + j ])
+ log(betaM[ ((t+1)*N) + j ]));
}
cmVOR_NormalizeProbability( E, N*N );
cmVOR_AddVV( EpsM, N*N, E );
// If t==0 then update the initial state prob's
if( t == 0 )
{
for(i=0; i<N; ++i)
p->iV[i] = cmVOR_SumN(EpsM+i, N, N);
assert( cmVOR_IsNormal(p->iV,N) );
}
if( bFl )
{
//
// Calculate gamma_jk[]
//
cmReal_t gtjk[N*K]; // gamma_jk[N,K] at time t
cmReal_t abV[N]; //
// (alphaM[j,t] * betaM[j:t]) / (sum(alphaM[:,t] * betaM[:,t]))
cmVOR_MultVVV(abV,N,alphaM + t*N, betaM+t*N);
cmReal_t abSum = cmVOR_Sum(abV,N);
if( abSum<=0 )
assert(abSum>0);
cmVOR_DivVS(abV,N,abSum);
for(j=0; j<N; ++j)
{
cmReal_t bkSum = cmVOR_Sum(BK + (t*N*K) + (j*K), K );
for(k=0; k<K; ++k)
gtjk[ (k*N)+j ] = abV[j] * (BK[ (t*N*K) + (j*K) + k ] / bkSum);
}
// sum gtjk[N,K] into gamma_jk (integrate gamma over time)
cmVOR_AddVV( gamma_jk, N*K, gtjk );
// update the mean and covar numerators
for(j=0; j<N; ++j)
{
cmReal_t* uV = uM + (j*D*K);
cmReal_t* sV = sMM + (j*De2*K);
for(k=0; k<K; ++k,uV+=D,sV+=De2)
{
cmReal_t c = gtjk[ (k*N)+j ];
if( covarFl )
{
cmReal_t dV[D];
cmReal_t dM[D*D];
// covar numerator b[j].sM[k]
cmVOR_SubVVV(dV, D, oV, p->bV[j]->uM + (k*D));
cmVOR_MultMMM( dM, D, D, dV, dV, 1 );
cmVOR_MultVS( dM, De2, c );
cmVOR_AddVV( sV, De2, dM );
}
if( meanFl )
{
// mean numerator b[j].uM[k]
for(d=0; d<D; ++d)
uV[d] += c * oV[ d ];
}
}
}
}
if( timeProgFl && (t >= floor(T*progFrac)) )
{
cmCtxPrint(p->obj.ctx,"%i ", (unsigned)round(progFrac*100) );
progFrac+=progInc;
}
} // end time loop
for(i=0; i<N; ++i)
{
// update the state transition matrix
cmReal_t den = cmVOR_SumN(EpsM + i, N, N );
assert(den != 0 );
for(j=0; j<N; ++j)
p->aM[ i + (j*N) ] = EpsM[ i + (j*N) ] / den;
if( bFl )
{
// update the mean, covariance and mix coefficient
cmGmm_t* g = p->bV[i];
const cmReal_t* uV = uM + (i*D*K);
const cmReal_t* sMV = sMM + (i*De2*K);
for(k=0; k<K; ++k,uV+=D,sMV+=De2)
{
cmReal_t gjk = gamma_jk[ (k*N) + i ];
if( meanFl )
cmVOR_DivVVS(g->uM + (k*D), D, uV, gjk );
if( covarFl )
cmVOR_DivVVS(g->sMM + (k*De2), De2, sMV, gjk );
if( mixFl )
g->gV[k] = gjk / cmVOR_SumN( gamma_jk + i, K, N );
}
if((rc = _cmGmmUpdateCovar(g,g->sMM)) != cmOkRC )
goto errLabel;
}
}
assert( cmVOR_IsNormalZ(p->aM,N*N) );
if( timeProgFl )
cmCtxPrint(p->obj.ctx,"\n");
} // end iter loop
if( progFl)
cmCtxPrint(p->obj.ctx,"\n");
if( p->mfp != NULL )
{
// first line is iV[N]
cmMtxFileRealExec(p->mfp,p->iV,p->N);
// next N lines are aM[N,N]
for(i=0; i<p->N; ++i)
cmMtxFileRealExecN(p->mfp,p->aM + i,p->N,p->N);
// next T lines are bM[T,N]
if( p->bM != NULL )
for(i=0; i<T; ++i)
cmMtxFileRealExec(p->mfp, p->bM + (i*p->N),p->N);
}
errLabel:
cmMemPtrFree(&alphaM);
cmMemPtrFree(&betaM);
cmMemPtrFree(&logPrV);
cmMemPtrFree(&EpsM);
cmMemPtrFree(&BK);
cmMemPtrFree(&gamma_jk);
cmMemPtrFree(&uM);
cmMemPtrFree(&sMM);
return rc;
}
void cmChmmPrint( cmChmm_t* p )
{
unsigned i;
cmCtxPrint(p->obj.ctx,"======================================== \n");
cmVOR_PrintL("iV: ", p->obj.err.rpt, 1, p->N, p->iV);
cmVOR_PrintL("aM:\n", p->obj.err.rpt, p->N, p->N, p->aM);
for(i=0; i<p->N; ++i)
{
cmCtxPrint(p->obj.ctx,"bV[%i] ----------------- %i \n",i,i);
cmGmmPrint(p->bV[i],false);
}
}
void cmChmmTestForward( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
cmReal_t oM[] = {
0.117228, 0.110079,
0.154646, 0.210436,
0.947468, 0.558136,
0.202023, 0.138123,
0.929933, 0.456102,
0.897566, 0.685078,
0.945177, 0.663145,
0.272399, 0.055107,
0.863386, 0.621546,
0.217545, 0.274709,
0.838777, 0.650038,
0.134966, 0.159472,
0.053990, 0.264051,
0.884269, 0.550019,
0.764787, 0.554484,
0.114771, 0.077518,
0.835121, 0.606137,
0.070733, 0.120015,
0.819814, 0.588482,
0.105511, 0.197699,
0.824778, 0.533047,
0.945223, 0.511411,
0.126971, 0.050083,
0.869497, 0.567737,
0.144866, 0.197363,
0.985726, 0.590402,
0.181094, 0.192827,
0.162179, 0.155297,
1.034691, 0.513413,
0.220708, 0.036158,
0.750061, 0.671224,
0.246971, 0.093246,
0.997567, 0.680491,
0.916887, 0.530981,
0.022328, 0.121969,
0.794031, 0.618081,
0.845066, 0.625512,
0.174731, 0.094773,
0.968665, 0.652435,
0.932484, 0.388081,
0.202732, 0.148710,
0.911307, 0.637139,
0.211127, 0.201362,
0.138152, 0.057290,
0.819132, 0.579888,
0.135625, 0.176140,
0.146017, 0.157853,
0.950319, 0.624150,
0.285064, 0.038825,
0.716844, 0.575189,
0.907433, 0.504946,
0.219772, 0.129993,
0.076507, 0.193079,
0.808906, 0.548409,
0.880892, 0.523950,
0.758099, 0.636729,
1.014017, 0.557120,
0.277888, 0.181492,
0.877588, 0.508634,
0.251266, 0.225890,
0.990904, 0.482949,
0.999899, 0.534579,
0.904179, 0.707349,
0.952879, 0.617955,
0.172068, 0.151984,
1.026262, 0.662600,
0.812003, 0.430856,
0.173393, 0.017885,
0.099370, 0.146661,
0.785785, 0.564333,
0.698222, 0.449299,
0.276539, 0.225314,
0.799271, 0.618159,
0.098813, 0.090839,
0.883666, 0.554150,
0.274934, 0.185403,
0.200419, 0.109972,
0.925076, 0.608610,
0.864486, 0.348689,
0.176733, 0.136235,
0.967278, 0.656875,
0.986994, 0.659877,
1.015618, 0.596549,
0.689903, 0.528107,
0.978238, 0.630989,
0.269847, 0.144358,
0.092303, 0.139894,
0.168185, 0.095327,
0.897767, 0.584203,
0.068316, 0.018452,
0.953395, 0.530545,
0.266405, 0.173987,
0.233845, 0.205276,
0.900060, 0.477108,
0.052909, 0.053077,
0.885850, 0.496546,
0.268494, 0.104785,
1.041405, 0.655079,
1.055915, 0.697988,
0.181569, 0.146840
};
unsigned i;
cmReal_t iV[] = { .5 , .5};
cmReal_t A[] = { .3, .6, .7, .4 };
cmReal_t cV0[] = { .7, .3 };
cmReal_t uM0[] = { .2, .1, .1, .2 };
cmReal_t sMM0[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
cmReal_t cV1[] = { .2, .8 };
cmReal_t uM1[] = { .8, .9, .9, .5 };
cmReal_t sMM1[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
unsigned T = 100;
unsigned N = 2;
unsigned K = 2;
unsigned D = 2;
cmReal_t alphaM[N*T];
cmReal_t betaM[N*T];
cmReal_t logPrV[T];
unsigned qV[T];
unsigned sV[T];
cmReal_t oMt[T*D];
// scale covariance
cmVOR_MultVS(sMM0,D*D*K,1);
cmVOR_MultVS(sMM1,D*D*K,1);
cmCtx c;
cmCtxAlloc(&c,rpt,lhH,stH);
cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
cmChmmPrint(p);
cmChmmGenerate(p, oM, T, sV );
cmChmmForward( p, oM, T, alphaM,logPrV );
//cmVOR_PrintL("logPrV:\n",rpt,1,T,logPrV);
cmCtxPrint(&c,"log prob:%f\n", cmVOR_Sum(logPrV,T));
cmChmmBackward( p, oM, T, betaM );
//cmVOR_PrintL("beta:\n",rpt,N,T,betaM);
cmChmmDecode(p,oM,T,qV);
cmVOU_PrintL("sV:\n",rpt,1,T,sV);
cmVOU_PrintL("qV:\n",rpt,1,T,qV);
unsigned d=0;
for(i=0; i<T; ++i)
d += sV[i] != qV[i];
cmCtxPrint(&c,"Diff:%i\n",d);
cmPlotSetup("Chmm Forward Test",1,1);
cmVOR_Transpose(oMt,oM,D,T);
cmPlotLineD(NULL, oMt, oMt+T, NULL, T, "blue", kXPlotPtId );
cmPlotDraw();
cmChmmFree(&p);
}
void cmChmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
time_t t = time(NULL); //0x4b9e82aa; //time(NULL);
srand( t );
printf("TIME: 0x%x\n",(unsigned)t);
//cmChmmTestForward(vReportFunc);
//return;
unsigned i;
cmReal_t iV[] = { 1.0/3.0, 1.0/3.0, 1.0/3.0 };
cmReal_t A[] = { .1, .4, .7, .4, .2, .2 };
cmReal_t cV0[] = { .7, .3 };
cmReal_t uM0[] = { .2, .1, .1, .2 };
cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
cmReal_t cV1[] = { .2, .8 };
cmReal_t uM1[] = { .8, .9, .9, .8 };
cmReal_t sMM1[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
cmReal_t cV2[] = { .5, .5 };
cmReal_t uM2[] = { .5, .5, .5, .5 };
cmReal_t sMM2[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
cmReal_t kmThreshProb = 0.001;
unsigned kmMaxIterCnt = 10;
unsigned iterCnt = 20;
unsigned N = sizeof(iV) / sizeof(iV[0]);
unsigned K = sizeof(cV0) / sizeof(cV0[0]);
unsigned D = sizeof(uM0) / sizeof(uM0[0]) / K;
unsigned T = 100;
cmReal_t alphaM[N*T];
cmReal_t oM[D*T];
unsigned sV[T];
unsigned qV[T];
cmCtx c;
cmCtxAlloc(&c,rpt,lhH,stH);
cmCtxPrint(&c,"N:%i K:%i D:%i\n",N,K,D);
cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
cmChmmSetGmm(p,2,cV2,uM2,sMM2,0);
// generate data using the parameters above
cmChmmGenerate(p,oM,T,sV);
cmVOU_PrintL("sV: ",rpt,1,T,sV);
cmChmmRandomize(p,oM,T);
if(cmChmmSegKMeans(p,oM,T,kmThreshProb,kmMaxIterCnt,iterCnt) != cmOkRC )
goto errLabel;
if( cmChmmTrain(p,oM,T,iterCnt,0,0) != cmOkRC )
goto errLabel;
//cmChmmPrint(p);
cmChmmDecode(p,oM,T,qV);
cmReal_t pr = cmChmmForward(p,oM,T,alphaM,NULL);
cmCtxPrint(&c,"pr:%f\n",pr);
cmVOU_PrintL("sV:\n",rpt,1,T,sV);
cmVOU_PrintL("qV:\n",rpt,1,T,qV);
unsigned d=0;
for(i=0; i<T; ++i)
d += sV[i] != qV[i];
cmCtxPrint(&c,"Diff:%i\n",d);
errLabel:
cmChmmFree(&p);
}
//------------------------------------------------------------------------------------------------------------
cmChord* cmChordAlloc( cmCtx* c, cmChord* ap, const cmReal_t* chromaM, unsigned T )
{
unsigned i,j;
unsigned S = 6;
unsigned N = 24;
unsigned D = 12;
cmChord* p = cmObjAlloc(cmChord,c,ap);
p->h = cmChmmAlloc( p->obj.ctx, NULL, 0, 0, 0, NULL, NULL );
if( chromaM != NULL && T > 0 )
if( cmChordInit(p,chromaM,T) != cmOkRC )
cmChordFree(&p);
p->N = N;
p->D = D;
p->S = kTonalSpaceDimCnt;
/*
// iv[N] aM[N*N] uM[D*N] sMM[D*D*N] phiM[D*S] tsxxxV[S]
unsigned n = ALIGN_T(N, cmReal_t) +
ALIGN_T(N*N, cmReal_t) +
ALIGN_T(D*N, cmReal_t) +
ALIGN_T(D*D*N,cmReal_t) +
ALIGN_T(D*S, cmReal_t) +
2*ALIGN_T(S, cmReal_t);
p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t);
p->aM = p->iV + ALIGN_T(N, cmReal_t);
p->uM = p->aM + ALIGN_T(N*N, cmReal_t);
p->sMM = p->uM + ALIGN_T(D*N, cmReal_t);
p->phiM = p->sMM + ALIGN_T(D*D*N,cmReal_t);
p->tsMeanV = p->phiM + ALIGN_T(D*S, cmReal_t);
p->tsVarV = p->tsMeanV + ALIGN_T(S, cmReal_t);
*/
p->iV = cmMemAllocZ( cmReal_t, N );
p->aM = cmMemAllocZ( cmReal_t, N*N);
p->uM = cmMemAllocZ( cmReal_t, D*N);
p->sMM = cmMemAllocZ( cmReal_t, D*D*N );
p->phiM = cmMemAllocZ( cmReal_t, D*S);
p->tsMeanV = cmMemAllocZ( cmReal_t, S );
p->tsVarV = cmMemAllocZ( cmReal_t, S );
// initialize iV[N] (HMM initial state probabilities)
cmVOR_Fill(p->iV,N,1.0/N);
// initialize aM[N,N] (HMM transition matrix)
cmReal_t epsilon = 0.01;
cmReal_t CMaj2any[] = { 12, 2, 8, 6, 4, 10, 0, 10, 4, 6, 8, 2, 5, 5, 9, 1, 11, 3, 7, 7, 3, 11, 1, 9 };
for(i=0; i<N; ++i)
{
cmVOR_Copy( p->aM+(i*N), N, CMaj2any );
cmVOR_Rotate( CMaj2any, N, 1 );
}
cmVOR_AddVS(p->aM, N*N, epsilon);
cmVOR_DivVS(p->aM, N*N, ( (N/2)*(N/2) ) + (N*epsilon) );
//cmVOR_PrintL("A:\n",p->obj.err.rpt,N,N,A);
// initialize sMM[D*D,N] (HMM covariance matrices)
cmReal_t diagMV[] = { 1, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
cmReal_t diagmV[] = { 1, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
cmReal_t Maj[D*D];
cmReal_t Min[D*D];
cmVOR_DiagZ(Maj,D,diagMV);
Maj[ (4*D) + 0 ] = 0.6; Maj[ (0*D) + 4 ] = 0.6;
Maj[ (7*D) + 0 ] = 0.8; Maj[ (0*D) + 7 ] = 0.8;
Maj[ (7*D) + 4 ] = 0.8; Maj[ (4*D) + 7 ] = 0.8;
cmVOR_DiagZ(Min,D,diagmV);
Min[ (3*D) + 0 ] = 0.6; Min[ (0*D) + 3 ] = 0.6;
Min[ (7*D) + 0 ] = 0.8; Min[ (0*D) + 7 ] = 0.8;
Min[ (7*D) + 3 ] = 0.8; Min[ (3*D) + 7 ] = 0.8;
cmReal_t* sM = p->sMM;
for(i=0; i<N/2; ++i,sM+=D*D)
cmVOR_RotateM( sM, D, D, Maj, i, i );
for(i=0; i<N/2; ++i,sM+=D*D)
cmVOR_RotateM( sM, D, D, Min, i, i );
/*
cmVOR_PrintL("Maj:\n",p->obj.err.rpt,D,D,Maj);
cmVOR_PrintL("Min:\n",p->obj.err.rpt,D,D,Min);
for(i=0; i<N; ++i)
{
cmCtxPrint(c,"%i----\n",i);
cmVOR_PrintL("sM:\n",p->obj.err.rpt,D,D,sMM + (i*D*D));
}
*/
// initialize uM[D,N] (HMM GMM mean vectors)
cmVOR_Fill(p->uM,D*N,0);
for(i=0; i<D; ++i)
{
unsigned dom = (i+7) % D;
unsigned medM = (i+4) % D;
unsigned medm = (i+3) % D;
p->uM[ (i * D) + i ] = 1;
p->uM[ (i * D) + medM ] = 1;
p->uM[ (i * D) + dom ] = 1;
p->uM[ ((i+D) * D) + i ] = 1;
p->uM[ ((i+D) * D) + medm ] = 1;
p->uM[ ((i+D) * D) + dom ] = 1;
}
cmVOR_AddVS(p->uM,D*N,0.01);
for(i=0; i<N; ++i)
cmVOR_NormalizeProbability( p->uM + (i*D), D);
// initialize phiM[D,S]
cmReal_t phi[D*S];
for(i=0,j=0; i<D; ++i,++j)
phi[j] = sin( M_PI*7.0*i/6.0 );
for(i=0; i<D; ++i,++j)
phi[j] = cos( M_PI*7.0*i/6.0 );
for(i=0; i<D; ++i,++j)
phi[j] = sin( M_PI*3.0*i/2.0 );
for(i=0; i<D; ++i,++j)
phi[j] = cos( M_PI*3.0*i/2.0 );
for(i=0; i<D; ++i,++j)
phi[j] = 0.5 * sin( M_PI*2.0*i/3.0 );
for(i=0; i<D; ++i,++j)
phi[j] = 0.5 * cos( M_PI*2.0*i/3.0 );
cmVOR_Transpose(p->phiM,phi,D,S);
return p;
}
cmRC_t cmChordFree( cmChord** pp )
{
cmRC_t rc = cmOkRC;
cmChord* p = *pp;
if( pp == NULL || *pp == NULL )
return cmOkRC;
if((rc = cmChordFinal(p)) != cmOkRC )
return rc;
cmChmmFree( &p->h );
cmMemPtrFree(&p->iV);
cmMemPtrFree(&p->aM);
cmMemPtrFree(&p->uM);
cmMemPtrFree(&p->sMM);
cmMemPtrFree(&p->phiM);
cmMemPtrFree(&p->tsMeanV);
cmMemPtrFree(&p->tsVarV);
cmMemPtrFree(&p->chromaM);
cmMemPtrFree(&p->tsM);
cmMemPtrFree(&p->cdtsV);
cmObjFree(pp);
return rc;
}
cmRC_t cmChordInit( cmChord* p, const cmReal_t* chromaM, unsigned T )
{
cmRC_t rc = cmOkRC;
unsigned i;
unsigned N = p->N; // count of states
unsigned K = 1; // count of components per mixture
unsigned D = p->D; // dimensionality of the observation vector
unsigned S = p->S; //
cmReal_t alpha = 6.63261; // alpha norm coeff
if((rc = cmChordFinal(p)) != cmOkRC )
return rc;
// Create the hidden markov model
cmChmmInit( p->h, N, K, D, p->iV, p->aM);
// load the GMM's for each markov state
cmReal_t mixCoeff = 1.0;
bool diagFl = false;
for(i=0; i<N; ++i)
if((rc = cmChmmSetGmm(p->h, i, &mixCoeff, p->uM + (i*D), p->sMM+(i*D*D), diagFl )) != cmOkRC )
return rc;
// Allocate memory
// chromaM[D,T] tsM[S,T] cdtsV[T]
/*
unsigned n = ALIGN_T(D*T,cmReal_t) + ALIGN_T(S*T,cmReal_t) + ALIGN_T(T,cmReal_t);
p->chromaM = cmArrayResizeZ(c, &p->memB, n, cmReal_t);
p->tsM = p->chromaM + ALIGN_T(D*T,cmReal_t);
p->cdtsV = p->tsM + ALIGN_T(S*T,cmReal_t);
p->T = T;
*/
p->chromaM = cmMemResizeZ( cmReal_t, p->chromaM, p->D*T );
p->tsM = cmMemResizeZ( cmReal_t, p->tsM, p->S*T );
p->cdtsV = cmMemResizeZ( cmReal_t, p->cdtsV, p->D*T );
p->T = T;
// Allocate local memory
// qV[], triadIntV[] triadSeqV[] tsNormsV[]
/*
n = 2*ALIGN_B(T,unsigned) + ALIGN_B(T,int) + ALIGN_B(T,cmReal_t);
cmArray mem;
cmArrayAlloc(c, &mem);
unsigned* qV = (unsigned*) cmArrayResize(c, &mem, n, char);
unsigned* triadSeqV = (unsigned*) (qV + ALIGN_T(T,unsigned));
int* triadIntV = (int*) (triadSeqV + ALIGN_T(T,unsigned));
cmReal_t* tsNormsV = (cmReal_t*) (triadIntV + ALIGN_T(T,int));
*/
//unsigned qV[T];
//unsigned triadSeqV[T];
//int triadIntV[T];
//cmReal_t tsNormsV[T];
unsigned* qV = cmMemAlloc( unsigned, T );
unsigned* triadSeqV = cmMemAlloc( unsigned, T );
int* triadIntV = cmMemAlloc( int, T );
cmReal_t* tsNormsV = cmMemAlloc( cmReal_t, T );
// Take the alpha norm of chroma and store the result in p->chromaM[]
for(i=0; i<T; ++i)
p->chromaM[i] = cmVOR_AlphaNorm( chromaM + (i*D), D, alpha);
cmVOR_DivVVS(p->chromaM,D*T,chromaM, cmVOR_AlphaNorm(p->chromaM,T,alpha));
// Train the HMM iniital state prob. p->h->iV[] and transition matrix p->h->aM[]
unsigned flags = kNoTrainMixCoeffChmmFl | kNoTrainMeanChmmFl | kNoTrainCovarChmmFl;
unsigned iterCnt = 40;
if( chromaM != NULL && T > 0 )
if((rc = cmChmmTrain(p->h, p->chromaM, p->T, iterCnt, 0, flags )) != cmOkRC )
goto errLabel;
// Find the most likely chords using a Viterbi decoding of the chroma.
cmChmmDecode(p->h,p->chromaM,T,qV);
// Reorder the chord sequence cmcording to circle of fifths
unsigned map[] = {0, 14, 4, 18, 8, 22, 12, 2, 16, 6, 20, 10, 17, 7, 21, 11, 1, 15, 5, 19, 9, 23, 13, 3 };
for(i=0; i<T; ++i)
qV[i] = map[ qV[i] ];
//cmVOU_PrintL("qV:\n",p->obj.err.rpt,1,T,qV);
// Smooth the chord sequence with a median filter.
cmVOU_MedianFilt(qV,T,3,triadSeqV,1);
//cmVOU_PrintL("triadSeqV:\n",p->obj.err.rpt,1,T,triadSeqV);
// Calculate the chord change distance on the circle of fifths.
int d = 0;
for(i=0; i<T; ++i)
{
int v = abs(d);
assert(v<N);
v = (v<=11) ? v : -(12-(v-12));
if( i > 0 )
triadIntV[i-1] = (d < 0 ? -1 : 1) * v;
if( i + 1 < T)
d = triadSeqV[i+1] - triadSeqV[i];
}
// Project chroma into a 6D tonal space.
cmVOR_MultMMM( p->tsM,S,T,p->phiM,p->chromaM,D);
// Find the norm of p->tsM[6,T]
cmVOR_Fill(tsNormsV,T,0);
for(i=0; i<T; ++i)
tsNormsV[i] = cmVOR_MultSumVV( p->tsM + (i*S), p->tsM + (i*S), S );
cmVOR_PowVS(tsNormsV,T,0.5);
// Take the cosine distance.
p->cdtsV[0] = 1;
for(i=1; i<T; ++i)
p->cdtsV[i] = cmVOR_MultSumVV( p->tsM + ((i-1)*S), p->tsM + (i*S), S );
for(i=0; i<T-1; ++i)
p->cdtsV[i+1] /= tsNormsV[i] * tsNormsV[i+1];
//cmVOR_PrintL("tsNormsV:\n",p->obj.err.rpt,1,T,tsNormsV);
//cmVOR_PrintL("CDTS:\n", p->obj.err.rpt,1,T,p->cdtsV);
p->triadSeqMode = cmVOU_Mode(triadSeqV,T);
p->triadSeqVar = cmVOU_Variance(triadSeqV,T,NULL);
p->triadIntMean = cmVOI_Mean(triadIntV,T);
p->triadIntVar = cmVOI_Variance(triadIntV,T,&p->triadIntMean);
cmVOR_MeanM( p->tsMeanV, p->tsM, S, T, 1 );
cmVOR_VarianceM( p->tsVarV, p->tsM, S, T, p->tsMeanV, 1);
p->cdtsMean = cmVOR_Mean( p->cdtsV, T );
p->cdtsVar = cmVOR_Variance( p->cdtsV, T, &p->cdtsMean );
/*
cmReal_t tsMt[T*S];
cmKbRecd kb;
cmPlotInitialize(NULL);
cmPlotSetup("Chords",1,1);
cmCtxPrint(c,"%s\n","press any key");
//cmPlotLineD( NULL, NULL, p->cdtsV, NULL, T, NULL, kSolidPlotLineId );
cmVOR_Transpose(tsMt,p->tsM,S,T);
cmPlotLineMD(NULL, tsMt, NULL, T, S, kSolidPlotLineId);
cmPlotDraw();
cmKeyPress(&kb);
cmPlotFinalize();
*/
errLabel:
cmMemPtrFree(&qV);
cmMemPtrFree(&triadSeqV);
cmMemPtrFree(&triadIntV);
cmMemPtrFree(&tsNormsV);
return rc;
}
cmRC_t cmChordFinal( cmChord* p )
{ return cmOkRC; }
void cmChordTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
cmCtx c;
cmCtxAlloc(&c,rpt,lhH,stH);
cmChord* p = cmChordAlloc(&c,NULL,NULL,0);
cmChordFree(&p);
}
Reference in New Issue Copy Permalink