libcm is a C development framework with an emphasis on audio signal processing applications.
選択できるのは25トピックまでです。 トピックは、先頭が英数字で、英数字とダッシュ('-')を使用した35文字以内のものにしてください。

cmProc.c 133KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199320032013202320332043205320632073208320932103211321232133214321532163217321832193220322132223223322432253226322732283229323032313232323332343235323632373238323932403241324232433244324532463247324832493250325132523253325432553256325732583259326032613262326332643265326632673268326932703271327232733274327532763277327832793280328132823283328432853286328732883289329032913292329332943295329632973298329933003301330233033304330533063307330833093310331133123313331433153316331733183319332033213322332333243325332633273328332933303331333233333334333533363337333833393340334133423343334433453346334733483349335033513352335333543355335633573358335933603361336233633364336533663367336833693370337133723373337433753376337733783379338033813382338333843385338633873388338933903391339233933394339533963397339833993400340134023403340434053406340734083409341034113412341334143415341634173418341934203421342234233424342534263427342834293430343134323433343434353436343734383439344034413442344334443445344634473448344934503451345234533454345534563457345834593460346134623463346434653466346734683469347034713472347334743475347634773478347934803481348234833484348534863487348834893490349134923493349434953496349734983499350035013502350335043505350635073508350935103511351235133514351535163517351835193520352135223523352435253526352735283529353035313532353335343535353635373538353935403541354235433544354535463547354835493550355135523553355435553556355735583559356035613562356335643565356635673568356935703571357235733574357535763577357835793580358135823583358435853586358735883589359035913592359335943595359635973598359936003601360236033604360536063607360836093610361136123613361436153616361736183619362036213622362336243625362636273628362936303631363236333634363536363637363836393640364136423643364436453646364736483649365036513652365336543655365636573658365936603661366236633664366536663667366836693670367136723673367436753676367736783679368036813682368336843685368636873688368936903691369236933694369536963697369836993700370137023703370437053706370737083709371037113712371337143715371637173718371937203721372237233724372537263727372837293730373137323733373437353736373737383739374037413742374337443745374637473748374937503751375237533754375537563757375837593760376137623763376437653766376737683769377037713772377337743775377637773778377937803781378237833784378537863787378837893790379137923793379437953796379737983799380038013802380338043805380638073808380938103811381238133814381538163817381838193820382138223823382438253826382738283829383038313832383338343835383638373838383938403841384238433844384538463847384838493850385138523853385438553856385738583859386038613862386338643865386638673868386938703871387238733874387538763877387838793880388138823883388438853886388738883889389038913892389338943895389638973898389939003901390239033904390539063907390839093910391139123913391439153916391739183919392039213922392339243925392639273928392939303931393239333934393539363937393839393940394139423943394439453946394739483949395039513952395339543955395639573958395939603961396239633964396539663967396839693970397139723973397439753976397739783979398039813982398339843985398639873988398939903991399239933994399539963997399839994000400140024003400440054006400740084009401040114012401340144015401640174018401940204021402240234024402540264027402840294030403140324033403440354036403740384039404040414042404340444045404640474048404940504051405240534054405540564057405840594060406140624063406440654066406740684069407040714072407340744075407640774078407940804081408240834084408540864087408840894090409140924093409440954096409740984099410041014102410341044105410641074108410941104111411241134114411541164117411841194120412141224123412441254126412741284129413041314132413341344135413641374138413941404141414241434144414541464147414841494150415141524153415441554156415741584159416041614162416341644165416641674168416941704171417241734174417541764177417841794180418141824183418441854186418741884189419041914192419341944195419641974198419942004201420242034204420542064207420842094210421142124213421442154216421742184219422042214222422342244225422642274228422942304231423242334234423542364237423842394240424142424243424442454246424742484249425042514252425342544255425642574258425942604261426242634264426542664267426842694270427142724273427442754276427742784279428042814282428342844285428642874288428942904291429242934294429542964297429842994300430143024303430443054306430743084309431043114312431343144315431643174318431943204321432243234324432543264327432843294330433143324333433443354336433743384339434043414342434343444345434643474348434943504351435243534354435543564357435843594360436143624363436443654366436743684369437043714372437343744375437643774378437943804381438243834384438543864387438843894390439143924393439443954396439743984399440044014402440344044405440644074408440944104411441244134414441544164417441844194420442144224423442444254426442744284429443044314432443344344435443644374438443944404441444244434444444544464447444844494450445144524453445444554456445744584459446044614462446344644465446644674468446944704471447244734474447544764477447844794480448144824483448444854486448744884489449044914492449344944495449644974498449945004501450245034504450545064507450845094510451145124513451445154516451745184519452045214522452345244525452645274528452945304531453245334534453545364537453845394540454145424543454445454546454745484549455045514552455345544555455645574558455945604561456245634564456545664567456845694570457145724573457445754576457745784579458045814582458345844585458645874588458945904591459245934594459545964597459845994600460146024603460446054606460746084609461046114612461346144615461646174618461946204621462246234624462546264627462846294630463146324633463446354636463746384639464046414642464346444645464646474648464946504651465246534654465546564657465846594660466146624663466446654666466746684669467046714672467346744675467646774678467946804681468246834684468546864687468846894690469146924693469446954696469746984699470047014702470347044705470647074708470947104711471247134714471547164717471847194720472147224723472447254726472747284729473047314732473347344735473647374738473947404741474247434744474547464747474847494750475147524753475447554756475747584759476047614762476347644765476647674768476947704771477247734774477547764777477847794780478147824783478447854786478747884789479047914792479347944795479647974798479948004801480248034804480548064807480848094810481148124813481448154816481748184819482048214822482348244825482648274828482948304831483248334834483548364837483848394840484148424843484448454846484748484849485048514852485348544855485648574858485948604861486248634864486548664867486848694870487148724873487448754876487748784879488048814882488348844885488648874888488948904891489248934894489548964897489848994900490149024903490449054906490749084909491049114912491349144915491649174918491949204921492249234924492549264927492849294930493149324933493449354936493749384939494049414942494349444945494649474948494949504951495249534954495549564957495849594960496149624963496449654966496749684969497049714972497349744975497649774978497949804981498249834984498549864987498849894990499149924993499449954996499749984999500050015002500350045005500650075008500950105011501250135014501550165017501850195020502150225023502450255026502750285029503050315032503350345035503650375038503950405041504250435044504550465047504850495050505150525053505450555056505750585059506050615062506350645065506650675068506950705071507250735074507550765077507850795080508150825083508450855086508750885089509050915092509350945095509650975098509951005101510251035104510551065107510851095110511151125113511451155116511751185119512051215122512351245125512651275128512951305131513251335134513551365137513851395140514151425143514451455146514751485149515051515152515351545155515651575158515951605161516251635164516551665167516851695170517151725173517451755176517751785179518051815182518351845185518651875188518951905191519251935194519551965197519851995200520152025203520452055206520752085209521052115212521352145215521652175218521952205221522252235224522552265227522852295230523152325233523452355236523752385239524052415242524352445245524652475248
  1. #include "cmPrefix.h"
  2. #include "cmGlobal.h"
  3. #include "cmRpt.h"
  4. #include "cmErr.h"
  5. #include "cmCtx.h"
  6. #include "cmMem.h"
  7. #include "cmMallocDebug.h"
  8. #include "cmLinkedHeap.h"
  9. #include "cmSymTbl.h"
  10. #include "cmFloatTypes.h"
  11. #include "cmComplexTypes.h"
  12. #include "cmFileSys.h"
  13. #include "cmProcObj.h"
  14. #include "cmProcTemplate.h"
  15. #include "cmAudioFile.h"
  16. #include "cmMath.h"
  17. #include "cmProc.h"
  18. #include "cmVectOps.h"
  19. #include "cmKeyboard.h"
  20. #include "cmGnuPlot.h"
  21. #include <time.h> // time()
  22. //------------------------------------------------------------------------------------------------------------
  23. void cmFloatPointExceptHandler( int signo, siginfo_t* info, void* context )
  24. {
  25. char* cp = "<Type Unknown>";
  26. switch( info->si_code )
  27. {
  28. case FPE_INTDIV: cp = "integer divide by zero"; break;
  29. case FPE_INTOVF: cp = "integer overflow"; break;
  30. case FPE_FLTDIV: cp = "divide by zero"; break;
  31. case FPE_FLTUND: cp = "underflow"; break;
  32. case FPE_FLTRES: cp = "inexact result"; break;
  33. case FPE_FLTINV: cp = "invalid operation"; break;
  34. case FPE_FLTSUB: cp = "subscript range error"; break;
  35. }
  36. fprintf(stderr,"Floating point exception: Type: %s\n",cp);
  37. exit(1);
  38. }
  39. // set 'orgSaPtr' to NULL to discard the current signal action state
  40. void cmSetupFloatPointExceptHandler( struct sigaction* orgSaPtr )
  41. {
  42. struct sigaction sa;
  43. sa.sa_handler = SIG_DFL;
  44. sa.sa_flags = SA_SIGINFO;
  45. sa.sa_sigaction = cmFloatPointExceptHandler;
  46. sigemptyset(&sa.sa_mask);
  47. // set all FP except flags excetp: FE_INEXACT
  48. #ifdef OS_OSX
  49. // we don't yet support FP exceptions on OSX
  50. // for an example of how to make this work with the linux interface as below
  51. // see: http://www-personal.umich.edu/~williams/archive/computation/fe-handling-example.c
  52. assert(0);
  53. #else
  54. // int flags = FE_DIVBYZERO | FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
  55. // feenableexcept(flags);
  56. assert(0);
  57. #endif
  58. sigaction( SIGFPE, &sa, orgSaPtr );
  59. }
  60. //------------------------------------------------------------------------------------------------------------
  61. cmAudioFileRd* cmAudioFileRdAlloc( cmCtx* c, cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
  62. {
  63. cmAudioFileRd* op = cmObjAlloc( cmAudioFileRd, c, p );
  64. if( fn != NULL )
  65. if( cmAudioFileRdOpen( op, procSmpCnt, fn, chIdx, begFrmIdx, endFrmIdx ) != cmOkRC )
  66. cmAudioFileRdFree(&op);
  67. return op;
  68. }
  69. cmRC_t cmAudioFileRdFree( cmAudioFileRd** pp )
  70. {
  71. cmRC_t rc = cmOkRC;
  72. if( pp != NULL && *pp != NULL )
  73. {
  74. cmAudioFileRd* p = *pp;
  75. if((rc = cmAudioFileRdClose(p)) == cmOkRC )
  76. {
  77. cmMemPtrFree(&p->outV);
  78. cmMemPtrFree(&p->fn);
  79. cmObjFree(pp);
  80. }
  81. }
  82. return rc;
  83. }
  84. cmRC_t cmAudioFileRdOpen( cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
  85. {
  86. cmRC_t rc;
  87. cmRC_t afRC;
  88. if((rc = cmAudioFileRdClose(p)) != cmOkRC )
  89. return rc;
  90. p->h = cmAudioFileNewOpen( fn, &p->info, &afRC, p->obj.err.rpt );
  91. if( afRC != kOkAfRC )
  92. return cmCtxRtCondition( &p->obj, afRC, "Unable to open the audio file:'%s'", fn );
  93. p->chIdx = chIdx;
  94. p->outN = endFrmIdx==cmInvalidIdx ? p->info.frameCnt : procSmpCnt;
  95. p->outV = cmMemResizeZ( cmSample_t, p->outV, p->outN );
  96. p->fn = cmMemResizeZ( cmChar_t, p->fn, strlen(fn)+1 );
  97. strcpy(p->fn,fn);
  98. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"audioFile");
  99. p->lastReadFrmCnt = 0;
  100. p->eofFl = false;
  101. p->begFrmIdx = begFrmIdx;
  102. p->endFrmIdx = endFrmIdx==0 ? p->info.frameCnt : endFrmIdx;
  103. p->curFrmIdx = p->begFrmIdx;
  104. if( p->begFrmIdx > 0 )
  105. rc = cmAudioFileRdSeek(p,p->begFrmIdx);
  106. return rc;
  107. }
  108. cmRC_t cmAudioFileRdClose( cmAudioFileRd* p )
  109. {
  110. cmRC_t rc = cmOkRC;
  111. cmRC_t afRC;
  112. if( p == NULL )
  113. return cmOkRC;
  114. //cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  115. if( cmAudioFileIsOpen(p->h) == false )
  116. return cmOkRC;
  117. if((afRC = cmAudioFileDelete(&p->h)) != cmOkRC )
  118. rc = cmCtxRtCondition( &p->obj, afRC, "An attempt to close the audio file'%s' failed.", p->fn );
  119. return rc;
  120. }
  121. cmRC_t cmAudioFileRdRead( cmAudioFileRd* p )
  122. {
  123. cmRC_t rc = cmOkRC;
  124. cmRC_t afRC;
  125. if(p->eofFl || ((p->eofFl = cmAudioFileIsEOF(p->h)) == true) )
  126. return cmEofRC;
  127. unsigned n = p->endFrmIdx==cmInvalidIdx ? p->outN : cmMin( p->outN, p->endFrmIdx - p->curFrmIdx );
  128. if((afRC = cmAudioFileReadSample( p->h, n, p->chIdx, 1, &p->outV, &p->lastReadFrmCnt )) != kOkAfRC )
  129. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
  130. p->curFrmIdx += p->lastReadFrmCnt;
  131. if( n < p->outN )
  132. {
  133. cmVOS_Zero(p->outV + p->lastReadFrmCnt, p->outN - p->lastReadFrmCnt);
  134. p->eofFl = true;
  135. }
  136. if( p->mfp != NULL )
  137. cmMtxFileSmpExec( p->mfp, p->outV, p->outN );
  138. return rc;
  139. }
  140. cmRC_t cmAudioFileRdSeek( cmAudioFileRd* p, unsigned frmIdx )
  141. {
  142. cmRC_t rc = cmOkRC;
  143. cmRC_t afRC;
  144. if((afRC = cmAudioFileSeek( p->h, frmIdx )) != kOkAfRC )
  145. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);
  146. return rc;
  147. }
  148. cmRC_t cmAudioFileRdMinMaxMean( cmAudioFileRd* p, unsigned chIdx, cmSample_t* minPtr, cmSample_t* maxPtr, cmSample_t* meanPtr )
  149. {
  150. cmRC_t rc = cmOkRC;
  151. cmRC_t afRC;
  152. if(( afRC = cmAudioFileMinMaxMean( p->h, chIdx, minPtr, maxPtr, meanPtr )) != kOkAfRC )
  153. rc = cmCtxRtCondition( &p->obj, afRC, "Audio file min, max, and mean calculation failed on '%s'", p->fn );
  154. return rc;
  155. }
  156. //------------------------------------------------------------------------------------------------------------
  157. cmShiftBuf* cmShiftBufAlloc( cmCtx* c, cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
  158. {
  159. cmShiftBuf* op = cmObjAlloc( cmShiftBuf, c, p );
  160. if( procSmpCnt > 0 && wndSmpCnt > 0 && hopSmpCnt > 0 )
  161. if( cmShiftBufInit(op, procSmpCnt, wndSmpCnt, hopSmpCnt ) != cmOkRC)
  162. cmShiftBufFree(&op);
  163. return op;
  164. }
  165. cmRC_t cmShiftBufFree( cmShiftBuf** pp )
  166. {
  167. cmRC_t rc = cmOkRC;
  168. if( pp != NULL && *pp != NULL )
  169. {
  170. if((rc = cmShiftBufFinal(*pp)) == cmOkRC )
  171. {
  172. cmMemPtrFree(&(*pp)->bufV);
  173. cmObjFree(pp);
  174. }
  175. }
  176. return rc;
  177. }
  178. cmRC_t cmShiftBufInit( cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
  179. {
  180. cmRC_t rc;
  181. if( hopSmpCnt > wndSmpCnt )
  182. return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The window sample count (%i) must be greater than or equal to the hop sample count (%i).", wndSmpCnt, hopSmpCnt );
  183. if((rc = cmShiftBufFinal(p)) != cmOkRC )
  184. return rc;
  185. // The worst case storage requirement is where there are wndSmpCnt-1 samples in outV[] and procSmpCnt new samples arrive.
  186. p->bufSmpCnt = wndSmpCnt + procSmpCnt;
  187. p->bufV = cmMemResizeZ( cmSample_t, p->outV, p->bufSmpCnt );
  188. p->outV = p->bufV;
  189. p->outN = wndSmpCnt;
  190. p->wndSmpCnt = wndSmpCnt;
  191. p->procSmpCnt = procSmpCnt;
  192. p->hopSmpCnt = hopSmpCnt;
  193. p->inPtr = p->outV;
  194. p->fl = false;
  195. return cmOkRC;
  196. }
  197. cmRC_t cmShiftBufFinal( cmShiftBuf* p )
  198. {
  199. return cmOkRC;
  200. }
  201. // This function should be called in a loop until it returns false.
  202. // Note that 'sp' and 'sn' are ignored except p->fl == false.
  203. bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
  204. {
  205. assert( sn <= p->procSmpCnt );
  206. // The active samples are in outV[wndSmpCnt]
  207. // Stored samples are between outV + wndSmpCnt and inPtr.
  208. // if the previous call to this function returned true then the buffer must be
  209. // shifted by hopSmpCnt samples - AND sp[] is ignored.
  210. if( p->fl )
  211. {
  212. // shift the output buffer to the left to remove expired samples
  213. p->outV += p->hopSmpCnt;
  214. // if there are not wndSmpCnt samples left in the buffer
  215. if( p->inPtr - p->outV < p->wndSmpCnt )
  216. {
  217. // then copy the remaining active samples (between outV and inPtr)
  218. // to the base of the physicalbuffer
  219. unsigned n = p->inPtr - p->outV;
  220. memmove( p->bufV, p->outV, n * sizeof(cmSample_t));
  221. p->inPtr = p->bufV + n; // update the input and output positions
  222. p->outV = p->bufV;
  223. }
  224. }
  225. else
  226. {
  227. // if the previous call to this function returned false then sp[sn] should not be ignored
  228. assert( p->inPtr + sn <= p->outV + p->bufSmpCnt );
  229. // copy the incoming samples into the buffer
  230. cmVOS_Copy(p->inPtr,sn,sp);
  231. p->inPtr += sn;
  232. }
  233. // if there are at least wndSmpCnt available samples in outV[]
  234. p->fl = p->inPtr - p->outV >= p->wndSmpCnt;
  235. return p->fl;
  236. }
  237. void cmShiftBufTest( cmCtx* c )
  238. {
  239. unsigned smpCnt = 48;
  240. unsigned procSmpCnt = 5;
  241. unsigned hopSmpCnt = 6;
  242. unsigned wndSmpCnt = 7;
  243. unsigned i;
  244. cmShiftBuf* b = cmShiftBufAlloc(c,NULL,procSmpCnt,wndSmpCnt,hopSmpCnt );
  245. cmSample_t x[ smpCnt ];
  246. cmVOS_Seq(x,smpCnt,1,1);
  247. //cmVOS_Print( rptFuncPtr, 1, smpCnt, x );
  248. for(i=0; i<smpCnt; i+=procSmpCnt)
  249. {
  250. while( cmShiftBufExec( b, x + i, procSmpCnt ) )
  251. {
  252. cmVOS_Print( c->obj.err.rpt, 1, wndSmpCnt, b->outV );
  253. }
  254. }
  255. cmShiftBufFree(&b);
  256. }
  257. /*
  258. bool cmShiftBufExec( cmShiftBuf* p, const cmSample_t* sp, unsigned sn )
  259. {
  260. bool retFl = false;
  261. if( sn > p->procSmpCnt )
  262. {
  263. 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);
  264. return false;
  265. }
  266. assert( sn <= p->procSmpCnt );
  267. cmSample_t* dbp = p->outV;
  268. cmSample_t* dep = p->outV + (p->outN - sn);
  269. cmSample_t* sbp = p->outV + sn;
  270. // shift the last bufCnt-shiftCnt samples over the first shiftCnt samples
  271. while( dbp < dep )
  272. *dbp++ = *sbp++;
  273. // copy in the new samples
  274. dbp = dep;
  275. dep = dbp + sn;
  276. while( dbp < dep )
  277. *dbp++ = *sp++;
  278. // if any space remains at the end of the buffer then zero it
  279. dep = p->outV + p->outN;
  280. while( dbp < dep )
  281. *dbp++ = 0;
  282. if( p->firstPtr > p->outV )
  283. p->firstPtr = cmMax( p->outV, p->firstPtr - p->procSmpCnt);
  284. p->curHopSmpCnt += sn;
  285. if( p->curHopSmpCnt >= p->hopSmpCnt )
  286. {
  287. p->curHopSmpCnt -= p->hopSmpCnt;
  288. retFl = true;
  289. }
  290. if( p->mfp != NULL )
  291. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  292. return retFl;
  293. }
  294. */
  295. //------------------------------------------------------------------------------------------------------------
  296. cmWndFunc* cmWndFuncAlloc( cmCtx* c, cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kaiserSideLobeRejectDb )
  297. {
  298. cmWndFunc* op = cmObjAlloc( cmWndFunc, c, p );
  299. if( wndId != kInvalidWndId )
  300. if( cmWndFuncInit(op,wndId,wndSmpCnt,kaiserSideLobeRejectDb ) != cmOkRC )
  301. cmWndFuncFree(&op);
  302. return op;
  303. }
  304. cmRC_t cmWndFuncFree( cmWndFunc** pp )
  305. {
  306. cmRC_t rc = cmOkRC;
  307. if( pp != NULL && *pp != NULL )
  308. {
  309. cmWndFunc* p = *pp;
  310. if((rc = cmWndFuncFinal(p)) == cmOkRC )
  311. {
  312. cmMemPtrFree(&p->wndV);
  313. cmMemPtrFree(&p->outV);
  314. cmObjFree(pp);
  315. }
  316. }
  317. return rc;
  318. }
  319. cmRC_t cmWndFuncInit( cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kslRejectDb )
  320. {
  321. cmRC_t rc;
  322. if( wndId == (p->wndId | p->flags) && wndSmpCnt == p->outN && kslRejectDb == p->kslRejectDb )
  323. return cmOkRC;
  324. if((rc = cmWndFuncFinal(p)) != cmOkRC )
  325. return rc;
  326. p->wndV = cmMemResize( cmSample_t, p->wndV, wndSmpCnt );
  327. p->outV = cmMemResize( cmSample_t, p->outV, wndSmpCnt );
  328. p->outN = wndSmpCnt;
  329. p->wndId = wndId;
  330. p->kslRejectDb = kslRejectDb;
  331. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"wndFunc");
  332. p->flags = wndId & (~kWndIdMask);
  333. switch( wndId & kWndIdMask )
  334. {
  335. case kHannWndId: cmVOS_Hann( p->wndV, p->outN ); break;
  336. case kHannMatlabWndId: cmVOS_HannMatlab( p->wndV, p->outN ); break;
  337. case kHammingWndId: cmVOS_Hamming( p->wndV, p->outN ); break;
  338. case kTriangleWndId: cmVOS_Triangle( p->wndV, p->outN ); break;
  339. case kUnityWndId: cmVOS_Fill( p->wndV, p->outN, 1.0 ); break;
  340. case kKaiserWndId:
  341. {
  342. double beta;
  343. if( cmIsFlag(wndId,kSlRejIsBetaWndFl) )
  344. beta = kslRejectDb;
  345. else
  346. beta = cmVOS_KaiserBetaFromSidelobeReject(fabs(kslRejectDb));
  347. cmVOS_Kaiser( p->wndV,p->outN, beta);
  348. }
  349. break;
  350. case kInvalidWndId: break;
  351. default:
  352. { assert(0); }
  353. }
  354. cmSample_t den = 0;
  355. cmSample_t num = 1;
  356. if( cmIsFlag(p->flags,kNormBySumWndFl) )
  357. {
  358. den = cmVOS_Sum(p->wndV, p->outN);
  359. num = wndSmpCnt;
  360. }
  361. if( cmIsFlag(p->flags,kNormByLengthWndFl) )
  362. den += wndSmpCnt;
  363. if( den > 0 )
  364. {
  365. cmVOS_MultVS(p->wndV,p->outN,num);
  366. cmVOS_DivVS(p->wndV,p->outN,den);
  367. }
  368. return cmOkRC;
  369. }
  370. cmRC_t cmWndFuncFinal( cmWndFunc* p )
  371. {
  372. //if( p != NULL )
  373. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  374. return cmOkRC;
  375. }
  376. cmRC_t cmWndFuncExec( cmWndFunc* p, const cmSample_t* sp, unsigned sn )
  377. {
  378. if( sn > p->outN )
  379. 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 );
  380. if( p->wndId != kInvalidWndId )
  381. cmVOS_MultVVV( p->outV, sn, sp, p->wndV );
  382. if( p->mfp != NULL )
  383. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  384. return cmOkRC;
  385. }
  386. void cmWndFuncTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  387. {
  388. unsigned wndCnt = 5;
  389. double kaiserSideLobeRejectDb = 30;
  390. cmCtx c;
  391. cmCtxAlloc(&c,rpt,lhH,stH);
  392. cmWndFunc* p = cmWndFuncAlloc(&c,NULL,kHannWndId,wndCnt, 0 );
  393. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  394. cmWndFuncInit(p,kHammingWndId ,wndCnt, 0 );
  395. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  396. cmWndFuncInit(p,kTriangleWndId ,wndCnt, 0 );
  397. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  398. cmWndFuncInit(p,kKaiserWndId ,wndCnt, kaiserSideLobeRejectDb );
  399. cmVOS_Print( rpt, 1, wndCnt, p->wndV );
  400. cmSample_t wV[ wndCnt ];
  401. cmVOS_HannMatlab(wV,wndCnt);
  402. cmVOS_Print( rpt, 1, wndCnt, wV);
  403. cmWndFuncFree(&p);
  404. }
  405. //------------------------------------------------------------------------------------------------------------
  406. cmSpecDelay* cmSpecDelayAlloc( cmCtx* c, cmSpecDelay* ap, unsigned maxDelayCnt, unsigned binCnt )
  407. {
  408. cmSpecDelay* p = cmObjAlloc( cmSpecDelay, c, ap );
  409. if( maxDelayCnt > 0 && binCnt > 0 )
  410. if( cmSpecDelayInit(p,maxDelayCnt,binCnt) != cmOkRC )
  411. cmSpecDelayFree(&p);
  412. return p;
  413. }
  414. cmRC_t cmSpecDelayFree( cmSpecDelay** pp )
  415. {
  416. cmRC_t rc = cmOkRC;
  417. if( pp != NULL && *pp != NULL )
  418. {
  419. cmSpecDelay* p = *pp;
  420. if((rc=cmSpecDelayFinal(p)) == cmOkRC )
  421. {
  422. cmMemPtrFree(&p->bufPtr);
  423. cmObjFree(pp);
  424. }
  425. }
  426. return rc;
  427. }
  428. cmRC_t cmSpecDelayInit( cmSpecDelay* p, unsigned maxDelayCnt, unsigned binCnt )
  429. {
  430. cmRC_t rc;
  431. if((rc = cmSpecDelayFinal(p)) != cmOkRC )
  432. return rc;
  433. p->bufPtr = cmMemResizeZ( cmSample_t, p->bufPtr, binCnt * maxDelayCnt );
  434. p->maxDelayCnt = maxDelayCnt;
  435. p->outN = binCnt;
  436. p->inIdx = 0;
  437. return cmOkRC;
  438. }
  439. cmRC_t cmSpecDelayFinal(cmSpecDelay* p )
  440. { return cmOkRC; }
  441. cmRC_t cmSpecDelayExec( cmSpecDelay* p, const cmSample_t* sp, unsigned sn )
  442. {
  443. cmSample_t* dp = p->bufPtr + (p->inIdx * p->outN);
  444. cmVOS_Copy( dp, cmMin(sn,p->outN), sp);
  445. p->inIdx = (p->inIdx+1) % p->maxDelayCnt;
  446. return cmOkRC;
  447. }
  448. const cmSample_t* cmSpecDelayOutPtr( cmSpecDelay* p, unsigned delayCnt )
  449. {
  450. assert( delayCnt < p->maxDelayCnt );
  451. int i = p->inIdx - delayCnt;
  452. if( i < 0 )
  453. i = p->maxDelayCnt + i;
  454. return p->bufPtr + (i * p->outN);
  455. }
  456. //------------------------------------------------------------------------------------------------------------
  457. cmFilter* cmFilterAlloc( cmCtx* c, cmFilter* ap, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
  458. {
  459. cmRC_t rc;
  460. cmFilter* p = cmObjAlloc(cmFilter,c,ap);
  461. if( (bn > 0 || an > 0) && procSmpCnt > 0 )
  462. if( (rc = cmFilterInit( p, b, bn, a, an, procSmpCnt, d)) != cmOkRC )
  463. cmFilterFree(&p);
  464. return p;
  465. }
  466. 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 )
  467. {
  468. cmRC_t rc;
  469. cmFilter* p = cmObjAlloc(cmFilter,c,ap);
  470. if( srate > 0 && passHz > 0 && procSmpCnt > 0 )
  471. if( (rc = cmFilterInitEllip( p, srate, passHz, stopHz, passDb, stopDb, procSmpCnt, d)) != cmOkRC )
  472. cmFilterFree(&p);
  473. return p;
  474. }
  475. cmRC_t cmFilterFree( cmFilter** pp )
  476. {
  477. cmRC_t rc = cmOkRC;
  478. if( pp != NULL && *pp != NULL )
  479. {
  480. cmFilter* p = *pp;
  481. if((rc = cmFilterFinal(p)) == cmOkRC )
  482. {
  483. cmMemPtrFree(&p->a);
  484. cmMemPtrFree(&p->b);
  485. cmMemPtrFree(&p->d);
  486. cmMemPtrFree(&p->outSmpV);
  487. cmObjFree(pp);
  488. }
  489. }
  490. return rc;
  491. }
  492. cmRC_t cmFilterInit( cmFilter* p, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
  493. {
  494. assert( bn >= 1 );
  495. assert( an >= 1 && a[0] != 0 );
  496. cmRC_t rc;
  497. if((rc = cmFilterFinal(p)) != cmOkRC )
  498. return rc;
  499. int cn = cmMax(an,bn) - 1;
  500. // The output vector may be used as either cmReal_t or cmSample_t.
  501. // Find the larger of the two possible types.
  502. if( sizeof(cmReal_t) > sizeof(cmSample_t) )
  503. {
  504. p->outRealV = cmMemResizeZ( cmReal_t, p->outRealV, procSmpCnt );
  505. p->outSmpV = (cmSample_t*)p->outRealV;
  506. }
  507. else
  508. {
  509. p->outSmpV = cmMemResizeZ( cmSample_t, p->outSmpV, procSmpCnt );
  510. p->outRealV = (cmReal_t*)p->outRealV;
  511. }
  512. p->a = cmMemResizeZ( cmReal_t, p->a, cn );
  513. p->b = cmMemResizeZ( cmReal_t, p->b, cn );
  514. p->d = cmMemResizeZ( cmReal_t, p->d, cn+1 );
  515. //p->outV = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  516. p->outN = procSmpCnt;
  517. p->an = an;
  518. p->bn = bn;
  519. p->cn = cn;
  520. p->di = 0;
  521. p->b0 = b[0] / a[0];
  522. int i;
  523. for(i=0; i<an-1; ++i)
  524. p->a[i] = a[i+1] / a[0];
  525. for(i=0; i<bn-1; ++i)
  526. p->b[i] = b[i+1] / a[0];
  527. if( d != NULL )
  528. cmVOR_Copy(p->d,cn,d);
  529. return cmOkRC;
  530. }
  531. // initialize an elliptic lowpass filter with the given characteristics
  532. // ref: Parks & Burrus, Digital Filter Design, sec. 7.2.7 - 7.2.8
  533. 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 )
  534. {
  535. assert( srate > 0 );
  536. assert( passHz > 0 && stopHz > passHz && srate/2 > stopHz );
  537. cmReal_t Wp, Ws, ep, v0,
  538. k, kc, k1, k1c,
  539. K, Kc, K1, K1c,
  540. sn, cn, dn,
  541. sm, cm, dm,
  542. zr, zi, pr, pi;
  543. unsigned N, L, j;
  544. // prewarp Wp and Ws, calculate k
  545. Wp = 2 * srate * tan(M_PI * passHz / srate);
  546. Ws = 2 * srate * tan(M_PI * stopHz / srate);
  547. k = Wp / Ws;
  548. // calculate ep and k1 from passDb and stopDb
  549. ep = sqrt(pow(10, passDb/10) - 1);
  550. k1 = ep / sqrt(pow(10, stopDb/10) - 1);
  551. // calculate complimentary moduli
  552. kc = sqrt(1-k*k);
  553. k1c = sqrt(1-k1*k1);
  554. // calculate complete elliptic integrals
  555. K = cmEllipK( kc );
  556. Kc = cmEllipK( k );
  557. K1 = cmEllipK( k1c );
  558. K1c = cmEllipK( k1 );
  559. // calculate minimum integer filter order N
  560. N = ceil(K*K1c/Kc/K1);
  561. // recalculate k and kc from chosen N
  562. // Ws is minimized while other specs held constant
  563. k = cmEllipDeg( K1c/K1/N );
  564. kc = sqrt(1-k*k);
  565. K = cmEllipK( kc );
  566. Kc = cmEllipK( k );
  567. Ws = Wp / k;
  568. // initialize temporary coefficient arrays
  569. cmReal_t b[N+1], a[N+1];
  570. a[0] = b[0] = 1;
  571. memset(b+1, 0, N*sizeof(cmReal_t));
  572. memset(a+1, 0, N*sizeof(cmReal_t));
  573. // intermediate value needed for determining poles
  574. v0 = K/K1/N * cmEllipArcSc( 1/ep, k1 );
  575. cmEllipJ( v0, k, &sm, &cm, &dm );
  576. for( L=1-N%2; L<N; L+=2 )
  577. {
  578. // find the next pole and zero on s-plane
  579. cmEllipJ( K*L/N, kc, &sn, &cn, &dn );
  580. zr = 0;
  581. zi = L ? Ws/sn : 1E25;
  582. pr = -Wp*sm*cm*cn*dn/(1-pow(dn*sm,2));
  583. pi = Wp*dm*sn/(1-pow(dn*sm,2));
  584. // convert pole and zero to z-plane using bilinear transform
  585. cmBlt( 1, srate, &zr, &zi );
  586. cmBlt( 1, srate, &pr, &pi );
  587. if( L == 0 )
  588. {
  589. // first order section
  590. b[1] = -zr;
  591. a[1] = -pr;
  592. }
  593. else
  594. {
  595. // replace complex root and its conjugate with 2nd order section
  596. zi = zr*zr + zi*zi;
  597. zr *= -2;
  598. pi = pr*pr + pi*pi;
  599. pr *= -2;
  600. // combine with previous sections to obtain filter coefficients
  601. for( j = L+1; j >= 2; j-- )
  602. {
  603. b[j] = b[j] + zr*b[j-1] + zi*b[j-2];
  604. a[j] = a[j] + pr*a[j-1] + pi*a[j-2];
  605. }
  606. b[1] += zr;
  607. a[1] += pr;
  608. }
  609. }
  610. // scale b coefficients s.t. DC gain is 0 dB
  611. cmReal_t sumB = 0, sumA = 0;
  612. for( j = 0; j < N+1; j++ )
  613. {
  614. sumB += b[j];
  615. sumA += a[j];
  616. }
  617. sumA /= sumB;
  618. for( j = 0; j < N+1; j++ )
  619. b[j] *= sumA;
  620. return cmFilterInit( p, b, N+1, a, N+1, procSmpCnt, d );
  621. }
  622. cmRC_t cmFilterFinal( cmFilter* p )
  623. { return cmOkRC; }
  624. cmRC_t cmFilterExecS( cmFilter* p, const cmSample_t* x, unsigned xn, cmSample_t* yy, unsigned yn )
  625. {
  626. cmSample_t* y;
  627. if( yy == NULL || yn==0 )
  628. {
  629. y = p->outSmpV;
  630. yn = p->outN;
  631. }
  632. else
  633. {
  634. y = yy;
  635. }
  636. cmVOS_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
  637. return cmOkRC;
  638. /*
  639. int i,j;
  640. cmSample_t y0 = 0;
  641. cmSample_t* y;
  642. unsigned n;
  643. if( yy == NULL || yn==0 )
  644. {
  645. n = cmMin(p->outN,xn);
  646. y = p->outSmpV;
  647. yn = p->outN;
  648. }
  649. else
  650. {
  651. n = cmMin(yn,xn);
  652. y = yy;
  653. }
  654. // This is a direct form II algorithm based on the MATLAB implmentation
  655. // http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html#f83-1015962
  656. for(i=0; i<n; ++i)
  657. {
  658. //cmSample_t x0 = x[i];
  659. y[i] = (x[i] * p->b0) + p->d[0];
  660. y0 = y[i];
  661. for(j=0; j<p->cn; ++j)
  662. p->d[j] = (p->b[j] * x[i]) - (p->a[j] * y0) + p->d[j+1];
  663. }
  664. // if fewer input samples than output samples - zero the end of the output buffer
  665. if( yn > xn )
  666. cmVOS_Fill(y+i,yn-i,0);
  667. return cmOkRC;
  668. */
  669. }
  670. cmRC_t cmFilterExecR( cmFilter* p, const cmReal_t* x, unsigned xn, cmReal_t* yy, unsigned yn )
  671. {
  672. cmReal_t* y;
  673. if( yy == NULL || yn==0 )
  674. {
  675. y = p->outRealV;
  676. yn = p->outN;
  677. }
  678. else
  679. {
  680. //n = cmMin(yn,xn);
  681. y = yy;
  682. }
  683. cmVOR_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
  684. return cmOkRC;
  685. }
  686. 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 )
  687. {
  688. int procSmpCnt = cmMin(1024,xn);
  689. cmFilter* p = cmFilterAlloc(c,NULL,b,bn,a,an,procSmpCnt,NULL);
  690. int i,n;
  691. for(i=0; i<xn && i<yn; i+=n)
  692. {
  693. n = cmMin(procSmpCnt,cmMin(yn-i,xn-i));
  694. cmFilterExecS(p,x+i,n,y+i,n);
  695. }
  696. if( i < yn )
  697. cmVOS_Fill(y+i,yn-i,0);
  698. cmFilterFree(&p);
  699. return cmOkRC;
  700. }
  701. 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 )
  702. {
  703. cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  704. cmVOS_FilterFilter( f, cmFilterExecS, bb,bn,aa,an,x,xn,y,yn);
  705. cmFilterFree(&f);
  706. return cmOkRC;
  707. }
  708. 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 )
  709. {
  710. cmFilter* f = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  711. cmVOR_FilterFilter( f, cmFilterExecR, bb,bn,aa,an,x,xn,y,yn);
  712. cmFilterFree(&f);
  713. return cmOkRC;
  714. }
  715. void cmFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  716. {
  717. cmCtx c;
  718. cmCtxAlloc(&c, rpt, lhH, stH );
  719. cmReal_t b[] = { 0.16, 0.32, 0.16 };
  720. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  721. cmReal_t a[] = {1 , -.5949, .2348 };
  722. unsigned an = sizeof(a)/sizeof(cmReal_t);
  723. cmReal_t x[] = { 1,0,0,0,1,0,0,0 };
  724. unsigned xn = sizeof(x)/sizeof(cmReal_t);
  725. cmReal_t d[] = { .5, -.25};
  726. // 0.1600 0.4152 0.3694 0.1223 0.1460 0.3781 0.3507 0.1198
  727. // -0.0111 -0.0281
  728. cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn,d);
  729. cmFilterExecR(p,x,xn,NULL,0);
  730. cmVOR_Print( rpt, 1, xn, p->outRealV );
  731. cmVOR_Print( rpt, 1, p->cn, p->d );
  732. cmFilterFree(&p);
  733. cmObjFreeStatic( cmCtxFree, cmCtx, c );
  734. /*
  735. cmReal_t b[] = { 0.16, 0.32, 0.16 };
  736. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  737. cmReal_t a[] = { 1, -.5949, .2348};
  738. unsigned an = sizeof(a)/sizeof(cmReal_t);
  739. cmSample_t x[] = { 1,0,0,0,0,0,0,0,0,0 };
  740. unsigned xn = sizeof(x)/sizeof(cmSample_t);
  741. cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn);
  742. cmFilterExec(&c,p,x,xn,NULL,0);
  743. cmVOS_Print( vReportFunc, 1, xn, p->outV );
  744. cmVOR_Print( vReportFunc, 1, p->cn, p->d );
  745. cmFilterExec(&c,p,x,xn,NULL,0);
  746. cmVOS_Print( vReportFunc, 1, xn, p->outV );
  747. cmFilterFree(&p);
  748. */
  749. }
  750. void cmFilterFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  751. {
  752. cmCtx c;
  753. cmCtxAlloc(&c, rpt, lhH, stH );
  754. cmReal_t b[] = { 0.36, 0.32, 0.36 };
  755. unsigned bn = sizeof(b)/sizeof(cmReal_t);
  756. cmReal_t a[] = {1 , -.5949, .2348 };
  757. unsigned an = sizeof(a)/sizeof(cmReal_t);
  758. cmReal_t x[] = { 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0 };
  759. unsigned xn = sizeof(x)/sizeof(cmReal_t);
  760. unsigned yn = xn;
  761. cmReal_t y[yn];
  762. memset(y,0,sizeof(y));
  763. cmFilterFilterR(&c, b,bn,a,an,x,xn,y,yn );
  764. cmVOR_Print( rpt, 1, yn, y );
  765. cmObjFreeStatic( cmCtxFree, cmCtx, c );
  766. }
  767. //------------------------------------------------------------------------------------------------------------
  768. cmComplexDetect* cmComplexDetectAlloc(cmCtx* c, cmComplexDetect* p, unsigned binCnt )
  769. {
  770. cmComplexDetect* op = cmObjAlloc( cmComplexDetect, c, p );
  771. cmSpecDelayAlloc(c,&op->phsDelay,0,0);
  772. cmSpecDelayAlloc(c,&op->magDelay,0,0);
  773. if( binCnt > 0 )
  774. if( cmComplexDetectInit(op,binCnt) != cmOkRC && p == NULL )
  775. cmComplexDetectFree(&op);
  776. return op;
  777. }
  778. cmRC_t cmComplexDetectFree( cmComplexDetect** pp )
  779. {
  780. cmRC_t rc;
  781. if( pp != NULL && *pp != NULL )
  782. {
  783. cmComplexDetect* p = *pp;
  784. if((rc = cmComplexDetectFinal(p)) == cmOkRC )
  785. {
  786. cmSpecDelay* sdp;
  787. sdp = &p->phsDelay;
  788. cmSpecDelayFree(&sdp);
  789. sdp = &p->magDelay;
  790. cmSpecDelayFree(&sdp);
  791. cmObjFree(pp);
  792. }
  793. }
  794. return cmOkRC;
  795. }
  796. cmRC_t cmComplexDetectInit( cmComplexDetect* p, unsigned binCnt )
  797. {
  798. cmRC_t rc;
  799. if((rc = cmComplexDetectFinal(p)) != cmOkRC )
  800. return rc;
  801. cmSpecDelayInit(&p->phsDelay,2,binCnt);
  802. cmSpecDelayInit(&p->magDelay,1,binCnt);
  803. p->binCnt = binCnt;
  804. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexDetect");
  805. //p->cdfSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kCDF_FId, 1 );
  806. return cmOkRC;
  807. }
  808. cmRC_t cmComplexDetectFinal( cmComplexDetect* p)
  809. {
  810. //if( p != NULL )
  811. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  812. return cmOkRC;
  813. }
  814. cmRC_t cmComplexDetectExec( cmComplexDetect* p, const cmSample_t* magV, const cmSample_t* phsV, unsigned binCnt )
  815. {
  816. p->out = cmVOS_ComplexDetect( magV, cmSpecDelayOutPtr(&p->magDelay,0), phsV, cmSpecDelayOutPtr(&p->phsDelay,1), cmSpecDelayOutPtr(&p->phsDelay,0), binCnt);
  817. p->out /= 10000000;
  818. cmSpecDelayExec(&p->magDelay,magV,binCnt);
  819. cmSpecDelayExec(&p->phsDelay,phsV,binCnt);
  820. //if( p->mfp != NULL )
  821. // cmMtxFileSmpExec( p->mfp, &p->out, 1 );
  822. return cmOkRC;
  823. }
  824. //------------------------------------------------------------------------------------------------------------
  825. cmSample_t _cmComplexOnsetMedian( const cmSample_t* sp, unsigned sn, void* userPtr )
  826. { return cmVOS_Median(sp,sn); }
  827. cmComplexOnset* cmComplexOnsetAlloc( cmCtx* c, cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
  828. {
  829. cmComplexOnset* op = cmObjAlloc( cmComplexOnset, c, p );
  830. if( procSmpCnt > 0 && srate > 0 && medFiltWndSmpCnt > 0 )
  831. if( cmComplexOnsetInit( op, procSmpCnt, srate, medFiltWndSmpCnt, threshold, frameCnt ) != cmOkRC )
  832. cmComplexOnsetFree(&op);
  833. return op;
  834. }
  835. cmRC_t cmComplexOnsetFree( cmComplexOnset** pp)
  836. {
  837. cmRC_t rc = cmOkRC;
  838. cmComplexOnset* p = *pp;
  839. if( pp==NULL || *pp == NULL )
  840. return cmOkRC;
  841. if((rc = cmComplexOnsetFinal(*pp)) != cmOkRC )
  842. return rc;
  843. cmMemPtrFree(&p->df);
  844. cmMemPtrFree(&p->fdf);
  845. cmObjFree(pp);
  846. return cmOkRC;
  847. }
  848. cmRC_t cmComplexOnsetInit( cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
  849. {
  850. cmRC_t rc;
  851. if(( rc = cmComplexOnsetFinal(p)) != cmOkRC )
  852. return rc;
  853. p->frmCnt = frameCnt;
  854. p->dfi = 0;
  855. p->df = cmMemResizeZ( cmSample_t, p->df, frameCnt );
  856. p->fdf = cmMemResizeZ( cmSample_t, p->fdf, frameCnt );
  857. p->onrate = 0;
  858. p->threshold = threshold;
  859. p->medSmpCnt = medFiltWndSmpCnt;
  860. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"complexOnset");
  861. return cmOkRC;
  862. }
  863. cmRC_t cmComplexOnsetFinal( cmComplexOnset* p)
  864. {
  865. //if( p != NULL )
  866. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  867. return cmOkRC;
  868. }
  869. cmRC_t cmComplexOnsetExec( cmComplexOnset* p, cmSample_t cdf )
  870. {
  871. p->df[p->dfi++] = cdf;
  872. return cmOkRC;
  873. }
  874. cmRC_t cmComplexOnsetCalc( cmComplexOnset* p )
  875. {
  876. // df -= mean(df)
  877. cmVOS_SubVS(p->df,p->frmCnt,cmVOS_Mean(p->df,p->frmCnt));
  878. // low pass forward/backward filter df[] into fdf[]
  879. double d = 2 + sqrt(2);
  880. cmReal_t b[] = {1/d, 2/d, 1/d};
  881. unsigned bn = sizeof(b)/sizeof(b[0]);
  882. cmReal_t a[] = {1, 0, 7-2*d};
  883. unsigned an = sizeof(a)/sizeof(a[0]);
  884. cmFilterFilterS(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
  885. // median filter to low-passed filtered fdf[] into df[]
  886. cmVOS_FnThresh(p->fdf,p->frmCnt,p->medSmpCnt,p->df,1,NULL);
  887. // subtract med filtered signal from the low passed signal.
  888. // fdf[] -= df[];
  889. cmVOS_SubVV(p->fdf,p->frmCnt,p->df);
  890. cmVOS_SubVS(p->fdf,p->frmCnt,p->threshold);
  891. cmSample_t *bp = p->df,
  892. *ep = bp + p->frmCnt - 1,
  893. *dp = p->fdf + 1;
  894. *bp++ = *ep = 0;
  895. for( ; bp<ep; bp++,dp++)
  896. {
  897. *bp = (*dp > *(dp-1) && *dp > *(dp+1) && *dp > 0) ? 1 : 0;
  898. p->onrate += *bp;
  899. }
  900. p->onrate /= p->frmCnt;
  901. /*
  902. if( p->mfp != NULL )
  903. {
  904. bp = p->df;
  905. ep = bp + p->frmCnt;
  906. while( bp < ep )
  907. cmMtxFileSmpExec( p->mfp, bp++, 1 );
  908. }
  909. */
  910. return cmOkRC;
  911. }
  912. //------------------------------------------------------------------------------------------------------------
  913. cmMfcc* cmMfccAlloc( cmCtx* c, cmMfcc* ap, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
  914. {
  915. cmMfcc* p = cmObjAlloc( cmMfcc, c, ap );
  916. if( melBandCnt > 0 && binCnt > 0 && dctCoeffCnt > 0 )
  917. if( cmMfccInit( p, srate, melBandCnt, dctCoeffCnt, binCnt ) != cmOkRC )
  918. cmMfccFree(&p);
  919. return p;
  920. }
  921. cmRC_t cmMfccFree( cmMfcc** pp )
  922. {
  923. cmRC_t rc = cmOkRC;
  924. if( pp==NULL || *pp == NULL )
  925. return cmOkRC;
  926. cmMfcc* p = *pp;
  927. if( (rc = cmMfccFinal(p)) != cmOkRC )
  928. return rc;
  929. cmMemPtrFree(&p->melM);
  930. cmMemPtrFree(&p->dctM);
  931. cmMemPtrFree(&p->outV);
  932. cmObjFree(pp);
  933. return rc;
  934. }
  935. cmRC_t cmMfccInit( cmMfcc* p, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
  936. {
  937. cmRC_t rc;
  938. if((rc = cmMfccFinal(p)) != cmOkRC )
  939. return rc;
  940. p->melM = cmMemResize( cmReal_t, p->melM, melBandCnt * binCnt );
  941. p->dctM = cmMemResize( cmReal_t, p->dctM, dctCoeffCnt * melBandCnt );
  942. p->outV = cmMemResize( cmReal_t, p->outV, dctCoeffCnt );
  943. // each row of the matrix melp contains a mask
  944. cmVOR_MelMask( p->melM, melBandCnt, binCnt, srate, kShiftMelFl );
  945. // each row contains melBandCnt elements
  946. cmVOR_DctMatrix(p->dctM, dctCoeffCnt, melBandCnt );
  947. p->melBandCnt = melBandCnt;
  948. p->dctCoeffCnt = dctCoeffCnt;
  949. p->binCnt = binCnt;
  950. p->outN = dctCoeffCnt;
  951. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"mfcc");
  952. //if( p->obj.ctx->statsProcPtr != NULL )
  953. // p->mfccSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kMFCC_FId, p->outN );
  954. return cmOkRC;
  955. }
  956. cmRC_t cmMfccFinal( cmMfcc* p )
  957. {
  958. //if( p != NULL )
  959. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  960. return cmOkRC;
  961. }
  962. cmRC_t cmMfccExecPower( cmMfcc* p, const cmReal_t* magPowV, unsigned binCnt )
  963. {
  964. assert( binCnt == p->binCnt );
  965. cmReal_t t[ p->melBandCnt ];
  966. // apply the mel filter mask to the power spectrum
  967. cmVOR_MultVMV( t, p->melBandCnt, p->melM, binCnt, magPowV );
  968. // convert mel bands to dB
  969. cmVOR_PowerToDb( t, p->melBandCnt, t );
  970. // decorellate the bands with a DCT
  971. cmVOR_MultVMV( p->outV, p->dctCoeffCnt, p->dctM, p->melBandCnt, t );
  972. /*
  973. cmPlotSelectSubPlot(0,0);
  974. cmPlotClear();
  975. //cmPlotLineS( "power", NULL, magPowV, NULL, 35, NULL, kSolidPlotLineId );
  976. cmPlotLineS( "mel", NULL, t0, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
  977. cmPlotSelectSubPlot(1,0);
  978. cmPlotClear();
  979. //cmPlotLineS( "meldb", NULL, t1, NULL, p->melBandCnt, NULL, kSolidPlotLineId );
  980. cmPlotLineS( "mfcc", NULL, p->outV+1, NULL, p->dctCoeffCnt-1, NULL, kSolidPlotLineId );
  981. */
  982. if( p->mfp != NULL )
  983. cmMtxFileRealExec(p->mfp,p->outV, p->outN);
  984. return cmOkRC;
  985. }
  986. cmRC_t cmMfccExecAmplitude( cmMfcc* p, const cmReal_t* magAmpV, unsigned binCnt )
  987. {
  988. cmReal_t t[ binCnt ];
  989. cmVOR_MultVVV( t,binCnt, magAmpV, magAmpV );
  990. cmMfccExecPower(p,t,binCnt);
  991. if( p->mfp != NULL )
  992. cmMtxFileRealExec(p->mfp,p->outV, p->outN);
  993. return cmOkRC;
  994. }
  995. //------------------------------------------------------------------------------------------------------------
  996. enum { cmSonesEqlConBinCnt = kEqualLoudBandCnt, cmSonesEqlConCnt=13 };
  997. cmSones* cmSonesAlloc( cmCtx* c, cmSones* ap, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
  998. {
  999. cmSones* p = cmObjAlloc( cmSones, c, ap );
  1000. if( srate > 0 && barkBandCnt > 0 && binCnt > 0 )
  1001. if( cmSonesInit(p,srate,barkBandCnt,binCnt,flags) != cmOkRC )
  1002. cmSonesFree(&p);
  1003. return p;
  1004. }
  1005. cmRC_t cmSonesFree( cmSones** pp )
  1006. {
  1007. cmRC_t rc = cmOkRC;
  1008. cmSones* p = *pp;
  1009. if( pp==NULL || *pp==NULL)
  1010. return cmOkRC;
  1011. if((rc = cmSonesFinal(p)) != cmOkRC )
  1012. return rc;
  1013. cmMemPtrFree(&p->ttmV);
  1014. cmMemPtrFree(&p->sfM);
  1015. cmMemPtrFree(&p->barkIdxV);
  1016. cmMemPtrFree(&p->barkCntV);
  1017. cmMemPtrFree(&p->outV);
  1018. cmObjFree(pp);
  1019. return rc;
  1020. }
  1021. cmRC_t cmSonesInit( cmSones* p, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
  1022. {
  1023. p->ttmV = cmMemResize( cmReal_t, p->ttmV, binCnt);
  1024. p->sfM = cmMemResize( cmReal_t, p->sfM, binCnt*barkBandCnt);
  1025. p->barkIdxV = cmMemResize( unsigned, p->barkIdxV, barkBandCnt);
  1026. p->barkCntV = cmMemResize( unsigned, p->barkCntV, barkBandCnt);
  1027. p->outV = cmMemResize( cmReal_t, p->outV, barkBandCnt);
  1028. // calc outer ear filter
  1029. cmVOR_TerhardtThresholdMask( p->ttmV, binCnt, srate, kNoTtmFlags );
  1030. // calc shroeder spreading function
  1031. cmVOR_ShroederSpreadingFunc(p->sfM, barkBandCnt, srate);
  1032. // calc the bin to bark maps
  1033. p->barkBandCnt = cmVOR_BarkMap( p->barkIdxV, p->barkCntV, barkBandCnt, binCnt, srate );
  1034. //unsigned i = 0;
  1035. //for(; i<barkBandCnt; ++i)
  1036. // printf("%i %i %i\n", i+1, barkIdxV[i]+1, barkCntV[i]);
  1037. bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
  1038. p->binCnt = binCnt;
  1039. p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
  1040. p->overallLoudness = 0;
  1041. p->flags = flags;
  1042. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sones");
  1043. //p->sonesSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSones_FId, p->outN );
  1044. //p->loudSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kLoud_FId, 1 );
  1045. return cmOkRC;
  1046. }
  1047. cmRC_t cmSonesFinal( cmSones* p )
  1048. {
  1049. //if( p != NULL )
  1050. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1051. return cmOkRC;
  1052. }
  1053. cmRC_t cmSonesExec( cmSones* p, const cmReal_t* magPowV, unsigned binCnt )
  1054. {
  1055. assert( binCnt == p->binCnt );
  1056. // Equal-loudness and phon map from: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
  1057. // equal-loudness contours
  1058. double eqlcon[cmSonesEqlConCnt][cmSonesEqlConBinCnt] =
  1059. {
  1060. {12,7,4,1,0,0,0,-0.5,-2,-3,-7,-8,-8.5,-8.5,-8.5},
  1061. {20,17,14,12,10,9.5,9,8.5,7.5,6.5,4,3,2.5,2,2.5},
  1062. {29,26,23,21,20,19.5,19.5,19,18,17,15,14,13.5,13,13.5},
  1063. {36,34,32,30,29,28.5,28.5,28.5,28,27.5,26,25,24.5,24,24.5},
  1064. {45,43,41,40,40,40,40,40,40,39.5,38,37,36.5,36,36.5},
  1065. {53,51,50,49,48.5,48.5,49,49,49,49,48,47,46.5,45.5,46},
  1066. {62,60,59,58,58,58.5,59,59,59,59,58,57.5,57,56,56},
  1067. {70,69,68,67.5,67.5,68,68,68,68,68,67,66,65.5,64.5,64.5},
  1068. {79,79,79,79,79,79,79,79,78,77.5,76,75,74.5,73,73},
  1069. {89,89,89,89.5,90,90,90,89.5,89,88.5,87,86,85.5,84,83.5},
  1070. {100,100,100,100,100,99.5,99,99,98.5,98,96,95,94.5,93.5,93},
  1071. {112,112,112,112,111,110.5,109.5,109,108.5,108,106,105,104.5,103,102.5},
  1072. {122,122,121,121,120.5,120,119,118,117,116.5,114.5,113.5,113,111, 110.5}
  1073. };
  1074. // loudness levels (phone scales)
  1075. 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};
  1076. unsigned i,j;
  1077. cmReal_t t0[ binCnt ];
  1078. cmReal_t t1[ p->barkBandCnt ];
  1079. cmReal_t t2[ p->barkBandCnt ];
  1080. unsigned* idxV = p->barkIdxV;
  1081. unsigned* cntV = p->barkCntV;
  1082. cmReal_t* sfM = p->sfM;
  1083. // apply the outer ear filter
  1084. cmVOR_MultVVV( t0, binCnt, magPowV, p->ttmV);
  1085. // apply the bark frequency warping
  1086. for(i=0; i<p->barkBandCnt; ++i)
  1087. {
  1088. if( cntV[i] == 0 )
  1089. t1[i] = 0;
  1090. else
  1091. {
  1092. t1[i] = t0[ idxV[i] ];
  1093. for(j=1; j<cntV[i]; ++j)
  1094. t1[i] += t0[ idxV[i] + j ];
  1095. }
  1096. }
  1097. // apply the spreading filters
  1098. cmVOR_MultVMV( t2, p->barkBandCnt, sfM, p->barkBandCnt, t1 );
  1099. bool elFl = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
  1100. unsigned bandCnt = elFl ? cmMin(p->barkBandCnt,cmSonesEqlConBinCnt) : p->barkBandCnt;
  1101. //p->outN = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
  1102. p->overallLoudness = 0;
  1103. for( i = 0; i <bandCnt; i++ )
  1104. {
  1105. // if using the equal-loudness contours begin with the third bark band
  1106. // and end with the 18th bark band
  1107. unsigned k = elFl ? i+3 : i;
  1108. if( k < p->barkBandCnt )
  1109. {
  1110. double v = t2[k];
  1111. // convert to db
  1112. v = 10*log10( v<1 ? 1 : v );
  1113. if( elFl )
  1114. {
  1115. // db to phons
  1116. // see: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion,
  1117. j = 0;
  1118. // find the equal loudness curve for this frequency and db level
  1119. while( v >= eqlcon[j][i] )
  1120. ++j;
  1121. if( j == cmSonesEqlConCnt )
  1122. {
  1123. cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "Bark band %i is out of range during equal-loudness mapping.",j );
  1124. continue;
  1125. }
  1126. // convert db to phons
  1127. if( j == 0 )
  1128. v = phons[0];
  1129. else
  1130. {
  1131. double t1 = ( v - eqlcon[j-1][i] ) / ( eqlcon[j][i] - eqlcon[j-1][i] );
  1132. v = phons[j-1] + t1 * (phons[j] - phons[j-1]);
  1133. }
  1134. }
  1135. // convert to sones
  1136. // bladon and lindblom, 1981, JASA, modelling the judment of vowel quality differences
  1137. if( v >= 40 )
  1138. p->outV[i] = pow(2,(v-40)/10);
  1139. else
  1140. p->outV[i] = pow(v/40,2.642);
  1141. p->overallLoudness += p->outV[i];
  1142. }
  1143. }
  1144. if( p->mfp != NULL )
  1145. cmMtxFileRealExec( p->mfp, p->outV, p->outN );
  1146. return cmOkRC;
  1147. }
  1148. void cmSonesTest()
  1149. {
  1150. cmKbRecd kb;
  1151. double srate = 44100;
  1152. unsigned bandCnt = 23;
  1153. unsigned binCnt = 513;
  1154. cmSample_t tv[ binCnt ];
  1155. cmSample_t sm[ bandCnt * bandCnt ];
  1156. cmSample_t t[ bandCnt * bandCnt ];
  1157. unsigned binIdxV[ bandCnt ];
  1158. unsigned cntV[ bandCnt ];
  1159. unsigned i;
  1160. cmPlotSetup("Sones",1,1);
  1161. cmVOS_TerhardtThresholdMask(tv,binCnt,srate, kModifiedTtmFl );
  1162. cmVOS_ShroederSpreadingFunc(sm, bandCnt, srate);
  1163. cmVOS_Transpose( t, sm, bandCnt, bandCnt );
  1164. bandCnt = cmVOS_BarkMap(binIdxV,cntV, bandCnt, binCnt, srate );
  1165. for(i=0; i<bandCnt; ++i)
  1166. printf("%i %i %i\n", i, binIdxV[i], cntV[i] );
  1167. for(i=0; i<bandCnt; ++i )
  1168. {
  1169. cmPlotLineS( NULL, NULL, t+(i*bandCnt), NULL, bandCnt, NULL, kSolidPlotLineId );
  1170. }
  1171. //cmPlotLineS( NULL, NULL, tv, NULL, binCnt, NULL, kSolidPlotLineId );
  1172. cmPlotDraw();
  1173. cmKeyPress(&kb);
  1174. }
  1175. //------------------------------------------------------------------------------------------------------------
  1176. cmAudioOffsetScale* cmAudioOffsetScaleAlloc( cmCtx* c, cmAudioOffsetScale* ap, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
  1177. {
  1178. cmAudioOffsetScale* p = cmObjAlloc( cmAudioOffsetScale, c, ap );
  1179. if( procSmpCnt > 0 && srate > 0 )
  1180. if( cmAudioOffsetScaleInit( p, procSmpCnt, srate, offset, rmsWndSecs, dBref, flags ) != cmOkRC )
  1181. cmAudioOffsetScaleFree(&p);
  1182. return p;
  1183. }
  1184. cmRC_t cmAudioOffsetScaleFree( cmAudioOffsetScale** pp )
  1185. {
  1186. cmRC_t rc = cmOkRC;
  1187. cmAudioOffsetScale* p = *pp;
  1188. if( pp == NULL || *pp == NULL )
  1189. return cmOkRC;
  1190. if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
  1191. return rc;
  1192. cmMemPtrFree(&p->cBufPtr);
  1193. cmMemPtrFree(&p->cCntPtr);
  1194. cmMemPtrFree(&p->outV);
  1195. cmObjFree(pp);
  1196. return rc;
  1197. }
  1198. cmRC_t cmAudioOffsetScaleInit( cmAudioOffsetScale* p, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
  1199. {
  1200. assert( procSmpCnt > 0 && srate > 0);
  1201. cmRC_t rc;
  1202. if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
  1203. return rc;
  1204. p->cBufCnt = 0;
  1205. if( cmIsFlag(flags, kRmsAudioScaleFl) )
  1206. {
  1207. if( rmsWndSecs > 0 )
  1208. {
  1209. unsigned rmsSmpCnt = srate * rmsWndSecs;
  1210. p->cBufCnt = (unsigned)ceil( rmsSmpCnt / procSmpCnt );
  1211. if( p->cBufCnt > 0 )
  1212. {
  1213. p->cBufPtr = cmMemResizeZ( cmReal_t, p->cBufPtr, p->cBufCnt );
  1214. p->cCntPtr = cmMemResizeZ( unsigned, p->cCntPtr, p->cBufCnt );
  1215. }
  1216. }
  1217. else
  1218. {
  1219. p->cBufCnt = 0;
  1220. p->cBufPtr = NULL;
  1221. p->cCntPtr = NULL;
  1222. }
  1223. }
  1224. p->cBufIdx = 0;
  1225. p->cBufCurCnt = 0;
  1226. p->cBufSum = 0;
  1227. p->cCntSum = 0;
  1228. p->outV = cmMemResize( cmSample_t, p->outV, procSmpCnt );
  1229. p->outN = procSmpCnt;
  1230. p->offset = offset;
  1231. p->dBref = dBref;
  1232. p->flags = flags;
  1233. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"audioOffsetScale");
  1234. return cmOkRC;
  1235. }
  1236. cmRC_t cmAudioOffsetScaleFinal( cmAudioOffsetScale* p )
  1237. {
  1238. //if( p != NULL)
  1239. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1240. return cmOkRC;
  1241. }
  1242. cmRC_t cmAudioOffsetScaleExec( cmAudioOffsetScale* p, const cmSample_t* sp, unsigned sn )
  1243. {
  1244. double Pref = 20.0 / 1000000; // 20 micro Pascals
  1245. cmSample_t* dbp = p->outV;
  1246. const cmSample_t* dep = dbp + sn;
  1247. double scale = 0;
  1248. // if no scaling was requested then add offset only
  1249. if( cmIsFlag(p->flags, kNoAudioScaleFl) )
  1250. {
  1251. while( dbp < dep )
  1252. *dbp++ = *sp++ + p->offset;
  1253. goto doneLabel;
  1254. }
  1255. // if fixed scaling
  1256. if( cmIsFlag(p->flags, kFixedAudioScaleFl) )
  1257. {
  1258. if( scale == 0 )
  1259. scale = pow(10,p->dBref/20);
  1260. while( dbp < dep )
  1261. *dbp++ = (*sp++ + p->offset) * scale;
  1262. }
  1263. else // if RMS scaling
  1264. {
  1265. double sum = 0;
  1266. double rms = 0;
  1267. while( dbp < dep )
  1268. {
  1269. double v = (*sp++ + p->offset) / Pref;
  1270. sum += v*v;
  1271. *dbp++ = v;
  1272. }
  1273. // if there is no RMS buffer calc RMS on procSmpCnt samles
  1274. if( p->cBufCnt == 0 )
  1275. rms = sqrt( sum / sn );
  1276. else
  1277. {
  1278. p->cBufSum -= p->cBufPtr[ p->cBufIdx ];
  1279. p->cBufSum += sum;
  1280. p->cCntSum -= p->cCntPtr[ p->cBufIdx ];
  1281. p->cCntSum += sn;
  1282. p->cBufIdx = (p->cBufIdx+1) % p->cBufCnt;
  1283. p->cBufCurCnt = cmMin( p->cBufCurCnt+1, p->cBufCnt );
  1284. assert( p->cCntSum > 0 );
  1285. rms = sqrt( p->cBufSum / p->cCntSum );
  1286. }
  1287. double sigSPL = 20*log10(rms);
  1288. scale = pow(10,(p->dBref - sigSPL)/20);
  1289. dbp = p->outV;
  1290. while( dbp < dep )
  1291. *dbp++ *= scale;
  1292. }
  1293. doneLabel:
  1294. dbp = p->outV + sn;
  1295. dep = p->outV + p->outN;
  1296. while( dbp < dep )
  1297. *dbp++ = 0;
  1298. if( p->mfp != NULL )
  1299. cmMtxFileSmpExec(p->mfp,p->outV,p->outN);
  1300. return cmOkRC;
  1301. }
  1302. //------------------------------------------------------------------------------------------------------------
  1303. cmSpecMeas* cmSpecMeasAlloc( cmCtx* c, cmSpecMeas* ap, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
  1304. {
  1305. cmSpecMeas* p = cmObjAlloc( cmSpecMeas, c, ap );
  1306. if( srate > 0 && binCnt > 0 )
  1307. if( cmSpecMeasInit( p, srate, binCnt, wndFrmCnt, flags ) != cmOkRC )
  1308. cmSpecMeasFree(&p);
  1309. return p;
  1310. }
  1311. cmRC_t cmSpecMeasFree( cmSpecMeas** pp )
  1312. {
  1313. cmRC_t rc = cmOkRC;
  1314. cmSpecMeas* p = *pp;
  1315. if( pp == NULL || *pp == NULL )
  1316. return cmOkRC;
  1317. if((rc = cmSpecMeasFinal(p)) != cmOkRC )
  1318. return rc;
  1319. cmMemPtrFree(&p->rmsV);
  1320. cmMemPtrFree(&p->hfcV);
  1321. cmMemPtrFree(&p->scnV);
  1322. cmObjFree(pp);
  1323. return rc;
  1324. }
  1325. cmRC_t cmSpecMeasInit( cmSpecMeas* p, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
  1326. {
  1327. cmRC_t rc;
  1328. if((rc = cmSpecMeasFinal(p)) != cmOkRC )
  1329. return rc;
  1330. if( cmIsFlag(flags, kUseWndSpecMeasFl) )
  1331. {
  1332. p->rmsV = cmMemResizeZ( cmReal_t, p->rmsV, wndFrmCnt );
  1333. p->hfcV = cmMemResizeZ( cmReal_t, p->hfcV, wndFrmCnt );
  1334. p->scnV = cmMemResizeZ( cmReal_t, p->scnV, wndFrmCnt );
  1335. }
  1336. p->rmsSum = 0;
  1337. p->hfcSum = 0;
  1338. p->scnSum = 0;
  1339. p->binCnt = binCnt;
  1340. p->flags = flags;
  1341. p->wndFrmCnt = wndFrmCnt;
  1342. p->frameCnt = 0;
  1343. p->frameIdx = 0;
  1344. p->binHz = srate / ((binCnt-1) * 2);
  1345. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"specMeas");
  1346. //p->rmsSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kRMS_FId, 1);
  1347. //p->hfcSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kHFC_FId, 1);
  1348. //p->scSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSC_FId, 1);
  1349. //p->ssSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSS_FId, 1);
  1350. return cmOkRC;
  1351. }
  1352. cmRC_t cmSpecMeasFinal( cmSpecMeas* p )
  1353. {
  1354. //if( p != NULL )
  1355. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1356. return cmOkRC;
  1357. }
  1358. cmRC_t cmSpecMeasExec( cmSpecMeas* p, const cmReal_t* magPowV, unsigned binCnt )
  1359. {
  1360. assert( binCnt == p->binCnt );
  1361. unsigned i = 0;
  1362. const cmReal_t* sbp = magPowV;
  1363. const cmReal_t* sep = sbp + binCnt;
  1364. cmReal_t rmsSum = 0;
  1365. cmReal_t hfcSum = 0;
  1366. cmReal_t scnSum = 0;
  1367. cmReal_t ssSum = 0;
  1368. for(i=0; sbp < sep; ++i, ++sbp )
  1369. {
  1370. rmsSum += *sbp;
  1371. hfcSum += *sbp * i;
  1372. scnSum += *sbp * i * p->binHz;
  1373. }
  1374. p->frameCnt++;
  1375. if( cmIsFlag(p->flags, kUseWndSpecMeasFl) )
  1376. {
  1377. p->frameCnt = cmMin( p->frameCnt, p->wndFrmCnt );
  1378. cmReal_t* rmsV = p->rmsV + p->frameIdx;
  1379. cmReal_t* hfcV = p->hfcV + p->frameIdx;
  1380. cmReal_t* scnV = p->scnV + p->frameIdx;
  1381. p->rmsSum -= *rmsV;
  1382. p->hfcSum -= *hfcV;
  1383. p->scnSum -= *scnV;
  1384. *rmsV = rmsSum;
  1385. *hfcV = hfcSum;
  1386. *scnV = scnSum;
  1387. p->frameIdx = (p->frameIdx+1) % p->frameCnt;
  1388. }
  1389. p->rmsSum += rmsSum;
  1390. p->hfcSum += hfcSum;
  1391. p->scnSum += scnSum;
  1392. p->rms = sqrt(p->rmsSum / (p->binCnt * p->frameCnt) );
  1393. p->hfc = p->hfcSum / ( p->binCnt * p->frameCnt );
  1394. p->sc = p->scnSum / cmMax( cmReal_EPSILON, p->rmsSum );
  1395. sbp = magPowV;
  1396. for(i=0; sbp < sep; ++i )
  1397. {
  1398. cmReal_t t = (i*p->binHz) - p->sc;
  1399. ssSum += *sbp++ * (t*t);
  1400. }
  1401. p->ss = sqrt(ssSum / cmMax( cmReal_EPSILON, p->rmsSum ));
  1402. if( p->mfp != NULL )
  1403. {
  1404. cmReal_t a[4] = { p->rms, p->hfc, p->sc, p->ss };
  1405. cmMtxFileRealExec( p->mfp, a, 4 );
  1406. }
  1407. return cmOkRC;
  1408. }
  1409. //------------------------------------------------------------------------------------------------------------
  1410. cmSigMeas* cmSigMeasAlloc( cmCtx* c, cmSigMeas* ap, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
  1411. {
  1412. cmSigMeas* p = cmObjAlloc( cmSigMeas, c, ap );
  1413. p->sbp = cmShiftBufAlloc(c,&p->shiftBuf,0,0,0);
  1414. if( srate > 0 && procSmpCnt > 0 && measSmpCnt > 0 )
  1415. if( cmSigMeasInit( p, srate, procSmpCnt, measSmpCnt ) != cmOkRC )
  1416. cmSigMeasFree(&p);
  1417. return p;
  1418. }
  1419. cmRC_t cmSigMeasFree( cmSigMeas** pp )
  1420. {
  1421. cmRC_t rc = cmOkRC;
  1422. cmSigMeas* p = *pp;
  1423. if( pp==NULL || *pp==NULL)
  1424. return cmOkRC;
  1425. if((rc = cmSigMeasFinal(p)) != cmOkRC )
  1426. return rc;
  1427. cmShiftBufFree(&p->sbp);
  1428. cmObjFree(pp);
  1429. return rc;
  1430. }
  1431. cmRC_t cmSigMeasInit( cmSigMeas* p, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
  1432. {
  1433. cmRC_t rc;
  1434. if((rc = cmSigMeasFinal(p)) != cmOkRC )
  1435. return rc;
  1436. if( procSmpCnt != measSmpCnt )
  1437. cmShiftBufInit( p->sbp, procSmpCnt, measSmpCnt, procSmpCnt );
  1438. p->zcrDelay = 0;
  1439. p->srate = srate;
  1440. p->measSmpCnt = measSmpCnt;
  1441. p->procSmpCnt = procSmpCnt;
  1442. //p->zcrSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kZCR_FId, 1 );
  1443. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"sigMeas");
  1444. return cmOkRC;
  1445. }
  1446. cmRC_t cmSigMeasFinal( cmSigMeas* p )
  1447. {
  1448. //if( p != NULL )
  1449. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1450. return cmOkRC;
  1451. }
  1452. cmRC_t cmSigMeasExec( cmSigMeas* p, const cmSample_t* sp, unsigned sn )
  1453. {
  1454. if( p->procSmpCnt != p->measSmpCnt )
  1455. {
  1456. cmShiftBufExec( p->sbp, sp, sn );
  1457. sp = p->sbp->outV;
  1458. sn = p->sbp->wndSmpCnt;
  1459. assert( p->sbp->wndSmpCnt == p->measSmpCnt );
  1460. }
  1461. unsigned zcn = cmVOS_ZeroCrossCount( sp, sn, NULL );
  1462. p->zcr = (cmReal_t)zcn * p->srate / p->measSmpCnt;
  1463. if( p->mfp != NULL )
  1464. cmMtxFileRealExec( p->mfp, &p->zcr, 1 );
  1465. return cmOkRC;
  1466. }
  1467. //------------------------------------------------------------------------------------------------------------
  1468. cmSRC* cmSRCAlloc( cmCtx* c, cmSRC* ap, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
  1469. {
  1470. cmSRC* p = cmObjAlloc( cmSRC, c,ap );
  1471. cmFilterAlloc(c,&p->filt,NULL,0,NULL,0,0,NULL);
  1472. if( srate > 0 && procSmpCnt > 0 )
  1473. if( cmSRCInit( p, srate, procSmpCnt, upFact, dnFact ) != cmOkRC )
  1474. cmSRCFree(&p);
  1475. return p;
  1476. }
  1477. cmRC_t cmSRCFree( cmSRC** pp )
  1478. {
  1479. cmRC_t rc;
  1480. if( pp != NULL && *pp != NULL )
  1481. {
  1482. cmSRC* p = *pp;
  1483. if((rc = cmSRCFinal( p )) == cmOkRC )
  1484. {
  1485. cmFilter* fp = &p->filt;
  1486. cmFilterFree(&fp);
  1487. cmMemPtrFree(&p->outV);
  1488. cmObjFree(pp);
  1489. }
  1490. }
  1491. return cmOkRC;
  1492. }
  1493. cmRC_t cmSRCInit( cmSRC* p, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
  1494. {
  1495. cmRC_t rc;
  1496. if((rc = cmSRCFinal(p)) != cmOkRC )
  1497. return rc;
  1498. double hiRate = upFact * srate;
  1499. double loRate = hiRate / dnFact;
  1500. double minRate= cmMin( loRate, srate );
  1501. double fcHz = minRate/2;
  1502. double dHz = (fcHz * .1); // transition band is 5% of min sample rate
  1503. double passHz = fcHz-dHz;
  1504. double stopHz = fcHz;
  1505. double passDb = 0.001;
  1506. double stopDb = 20;
  1507. cmFilterInitEllip( &p->filt, hiRate, passHz, stopHz, passDb, stopDb, procSmpCnt, NULL );
  1508. //printf("CoeffCnt:%i dHz:%f passHz:%f stopHz:%f passDb:%f stopDb:%f\n", p->fir.coeffCnt, dHz, passHz, stopHz, passDb, stopDb );
  1509. p->outN = (unsigned)ceil(procSmpCnt * upFact / dnFact);
  1510. p->outV = cmMemResize( cmSample_t, p->outV, p->outN );
  1511. p->upi = 0;
  1512. p->dni = 0;
  1513. p->upFact = upFact;
  1514. p->dnFact = dnFact;
  1515. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"src");
  1516. return cmOkRC;
  1517. }
  1518. cmRC_t cmSRCFinal( cmSRC* p )
  1519. {
  1520. //if( p != NULL )
  1521. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1522. return cmOkRC;
  1523. }
  1524. cmRC_t cmSRCExec( cmSRC* p, const cmSample_t* sp, unsigned sn )
  1525. {
  1526. const cmSample_t* sep = sp + sn;
  1527. cmSample_t* op = p->outV;
  1528. const cmSample_t* oep = op + p->outN;
  1529. unsigned iN = sn * p->upFact;
  1530. unsigned i,j;
  1531. // run the filter at the upsampled rate ...
  1532. for(i=0; i<iN; ++i)
  1533. {
  1534. assert( p->upi!=0 || sp<sep );
  1535. cmSample_t x0 = p->upi==0 ? *sp++ : 0,
  1536. y0 = x0 * p->filt.b0 + p->filt.d[0];
  1537. // ... but output at the down sampled rate
  1538. if( p->dni==0 )
  1539. {
  1540. assert( op < oep );
  1541. *op++ = y0;
  1542. }
  1543. // advance the filter delay line
  1544. for(j=0; j<p->filt.cn; ++j)
  1545. p->filt.d[j] = p->filt.b[j]*x0 - p->filt.a[j]*y0 + p->filt.d[j+1];
  1546. // update the input/output clocks
  1547. p->upi = (p->upi + 1) % p->upFact;
  1548. p->dni = (p->dni + 1) % p->dnFact;
  1549. }
  1550. p->outN = op - p->outV;
  1551. if( p->mfp != NULL )
  1552. cmMtxFileSmpExec(p->mfp,p->outV,p->outN );
  1553. return cmOkRC;
  1554. }
  1555. //------------------------------------------------------------------------------------------------------------
  1556. cmConstQ* cmConstQAlloc( cmCtx* c, cmConstQ* ap, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
  1557. {
  1558. cmConstQ* p = cmObjAlloc( cmConstQ, c, ap );
  1559. if( srate >0 )
  1560. if( cmConstQInit(p,srate,minMidiPitch,maxMidiPitch,binsPerOctave,thresh) != cmOkRC )
  1561. cmConstQFree(&p);
  1562. return p;
  1563. }
  1564. cmRC_t cmConstQFree( cmConstQ** pp )
  1565. {
  1566. cmRC_t rc;
  1567. cmConstQ* p = *pp;
  1568. if( pp==NULL || *pp==NULL)
  1569. return cmOkRC;
  1570. if((rc = cmConstQFinal(p)) != cmOkRC )
  1571. return rc;
  1572. cmMemPtrFree(&p->fiV);
  1573. cmMemPtrFree(&p->foV);
  1574. cmMemPtrFree(&p->skM);
  1575. cmMemPtrFree(&p->outV);
  1576. cmMemPtrFree(&p->magV);
  1577. cmMemPtrFree(&p->skBegV);
  1578. cmMemPtrFree(&p->skEndV);
  1579. cmObjFree(pp);
  1580. return cmOkRC;
  1581. }
  1582. cmRC_t cmConstQInit( cmConstQ* p, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
  1583. {
  1584. cmRC_t rc;
  1585. if((rc = cmConstQFinal(p)) != cmOkRC )
  1586. return rc;
  1587. cmReal_t minHz = cmMidiToHz(minMidiPitch);
  1588. cmReal_t maxHz = cmMidiToHz(maxMidiPitch);
  1589. cmReal_t Q = 1.0/(pow(2,(double)1.0/binsPerOctave)-1);
  1590. unsigned K = (unsigned)ceil( binsPerOctave * log2(maxHz/minHz) );
  1591. unsigned fftN = cmNextPowerOfTwo( (unsigned)ceil(Q*srate/minHz) );
  1592. unsigned k = 0;
  1593. p->fiV = cmMemResize(cmComplexR_t, p->fiV, fftN);
  1594. p->foV = cmMemResize(cmComplexR_t, p->foV, fftN);
  1595. cmFftPlanR_t plan = cmFft1dPlanAllocR(fftN, p->fiV, p->foV, FFTW_FORWARD, FFTW_ESTIMATE );
  1596. p->wndSmpCnt = fftN;
  1597. p->constQBinCnt = K;
  1598. p->binsPerOctave= binsPerOctave;
  1599. p->skM = cmMemResizeZ( cmComplexR_t, p->skM, p->wndSmpCnt * p->constQBinCnt);
  1600. p->outV = cmMemResizeZ( cmComplexR_t, p->outV, p->constQBinCnt);
  1601. p->magV = cmMemResizeZ( cmReal_t, p->magV, p->constQBinCnt);
  1602. p->skBegV = cmMemResizeZ( unsigned, p->skBegV, p->constQBinCnt);
  1603. p->skEndV = cmMemResizeZ( unsigned, p->skEndV, p->constQBinCnt);
  1604. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx, "constQ");
  1605. //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);
  1606. double* hamm = NULL;
  1607. // note that the bands are created in reverse order
  1608. for(k=0; k<K; ++k)
  1609. {
  1610. unsigned iN = ceil( Q * srate / (minHz * pow(2,(double)k/binsPerOctave)));
  1611. unsigned start = fftN/2;
  1612. //double hamm[ iN ];
  1613. hamm = cmMemResizeZ(double,hamm,iN);
  1614. memset( p->fiV, 0, fftN * sizeof(cmComplexR_t));
  1615. memset( p->foV, 0, fftN * sizeof(cmComplexR_t));
  1616. cmVOD_Hamming( hamm, iN );
  1617. cmVOD_DivVS( hamm, iN, iN );
  1618. if( cmIsEvenU(iN) )
  1619. start -= iN/2;
  1620. else
  1621. start -= (iN+1)/2;
  1622. //printf("k:%i iN:%i start:%i %i\n",k,iN,start,start+iN-1);
  1623. unsigned i = start;
  1624. for(; i<=start+iN-1; ++i)
  1625. {
  1626. double arg = 2.0*M_PI*Q*(i-start)/iN;
  1627. double mag = hamm[i-start];
  1628. p->fiV[i-1] = (mag * cos(arg)) + (mag * I * sin(arg));
  1629. }
  1630. cmFftExecuteR(plan);
  1631. // since the bands are created in reverse order they are also stored in reverse order
  1632. // (i.e column k-1 is stored first and column 0 is stored last)
  1633. i=0;
  1634. unsigned minIdx = -1;
  1635. unsigned maxIdx = 0;
  1636. for(; i<fftN; ++i)
  1637. {
  1638. bool fl = cabs(p->foV[i]) <= thresh;
  1639. p->skM[ (k*p->wndSmpCnt) + i ] = fl ? 0 : p->foV[i]/fftN;
  1640. if( fl==false && minIdx == -1 )
  1641. minIdx = i;
  1642. if( fl==false && i>maxIdx )
  1643. maxIdx = i;
  1644. }
  1645. p->skBegV[k] = minIdx;
  1646. p->skEndV[k] = maxIdx;
  1647. }
  1648. cmMemPtrFree(&hamm);
  1649. cmFftPlanFreeR(plan);
  1650. return cmOkRC;
  1651. }
  1652. cmRC_t cmConstQFinal( cmConstQ* p )
  1653. {
  1654. //if( p != NULL )
  1655. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1656. return cmOkRC;
  1657. }
  1658. cmRC_t cmConstQExec( cmConstQ* p, const cmComplexR_t* ftV, unsigned srcBinCnt )
  1659. {
  1660. //acVORC_MultVVM( p->outV, p->constQBinCnt,ftV,p->wndSmpCnt, p->skM );
  1661. cmReal_t* mbp = p->magV;
  1662. cmComplexR_t* dbp = p->outV;
  1663. const cmComplexR_t* dep = p->outV + p->constQBinCnt;
  1664. unsigned i = 0;
  1665. for(; dbp < dep; ++dbp,++i,++mbp )
  1666. {
  1667. const cmComplexR_t* sbp = ftV + p->skBegV[i];
  1668. const cmComplexR_t* kp = p->skM + (i*p->wndSmpCnt) + p->skBegV[i];
  1669. const cmComplexR_t* ep = kp + (p->skEndV[i] - p->skBegV[i]) + 1;
  1670. *dbp = 0;
  1671. while( kp < ep )
  1672. *dbp += *sbp++ * *kp++;
  1673. *mbp = cmCabsR(*dbp);
  1674. }
  1675. if( p->mfp != NULL )
  1676. cmMtxFileComplexExec( p->mfp, p->outV, p->constQBinCnt, 1 );
  1677. return cmOkRC;
  1678. }
  1679. void cmConstQTest( cmConstQ* p )
  1680. {
  1681. cmKbRecd kb;
  1682. unsigned i,j;
  1683. cmSample_t* t = cmMemAlloc( cmSample_t, p->wndSmpCnt );
  1684. for(i=0; i<p->constQBinCnt; ++i)
  1685. {
  1686. for(j=0; j<p->wndSmpCnt; ++j)
  1687. t[j] = cabs( p->skM[ (i*p->wndSmpCnt) + j ]);
  1688. //cmPlotClear();
  1689. cmPlotLineS( NULL, NULL, t, NULL, 500, NULL, kSolidPlotLineId );
  1690. }
  1691. cmPlotDraw();
  1692. cmKeyPress(&kb);
  1693. cmMemPtrFree(&t);
  1694. }
  1695. //------------------------------------------------------------------------------------------------------------
  1696. cmHpcp* cmTunedHpcpAlloc( cmCtx* c, cmHpcp* ap, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
  1697. {
  1698. cmHpcp* p = cmObjAlloc( cmHpcp, c, ap );
  1699. if( binsPerOctave > 0 && constQBinCnt >> 0 )
  1700. if( cmTunedHpcpInit( p, binsPerOctave, constQBinCnt, cqMinMidiPitch, frameCnt, medFiltOrder ) != cmOkRC)
  1701. cmTunedHpcpFree(&p);
  1702. return p;
  1703. }
  1704. cmRC_t cmTunedHpcpFree( cmHpcp** pp )
  1705. {
  1706. cmRC_t rc = cmOkRC;
  1707. cmHpcp* p = *pp;
  1708. if(pp==NULL || *pp==NULL)
  1709. return cmOkRC;
  1710. if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
  1711. return rc;
  1712. cmMemPtrFree(&p->hpcpM);
  1713. cmMemPtrFree(&p->fhpcpM);
  1714. cmMemPtrFree(&p->histV);
  1715. cmMemPtrFree(&p->outM);
  1716. cmMemPtrFree(&p->meanV);
  1717. cmMemPtrFree(&p->varV);
  1718. cmObjFree(pp);
  1719. return cmOkRC;
  1720. }
  1721. cmRC_t cmTunedHpcpInit( cmHpcp* p, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
  1722. {
  1723. assert( binsPerOctave % kStPerOctave == 0 );
  1724. assert( cmIsOddU( binsPerOctave / kStPerOctave ) );
  1725. cmRC_t rc;
  1726. if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
  1727. return rc;
  1728. p->histN = binsPerOctave/kStPerOctave;
  1729. p->hpcpM = cmMemResizeZ( cmReal_t, p->hpcpM, frameCnt*binsPerOctave );
  1730. p->fhpcpM = cmMemResizeZ( cmReal_t, p->fhpcpM, binsPerOctave*frameCnt );
  1731. p->histV = cmMemResizeZ( unsigned, p->histV, p->histN );
  1732. p->outM = cmMemResizeZ( cmReal_t, p->outM, kStPerOctave * frameCnt );
  1733. p->meanV = cmMemResizeZ( cmReal_t, p->meanV, kStPerOctave );
  1734. p->varV = cmMemResizeZ( cmReal_t, p->varV, kStPerOctave );
  1735. p->constQBinCnt = constQBinCnt;
  1736. p->binsPerOctave = binsPerOctave;
  1737. p->frameCnt = frameCnt;
  1738. p->frameIdx = 0;
  1739. p->cqMinMidiPitch= cqMinMidiPitch;
  1740. p->medFiltOrder = medFiltOrder;
  1741. //p->mf0p = cmCtxAllocDebugFile(p->obj.ctx,"hpcp");
  1742. //p->mf1p = cmCtxAllocDebugFile(p->obj.ctx,"fhpcp");
  1743. //p->mf2p = cmCtxAllocDebugFile(p->obj.ctx,"chroma");
  1744. return cmOkRC;
  1745. }
  1746. cmRC_t cmTunedHpcpFinal( cmHpcp* p )
  1747. {
  1748. /*
  1749. if( p != NULL )
  1750. {
  1751. cmCtxFreeDebugFile(p->obj.ctx,&p->mf0p);
  1752. cmCtxFreeDebugFile(p->obj.ctx,&p->mf1p);
  1753. cmCtxFreeDebugFile(p->obj.ctx,&p->mf2p);
  1754. }
  1755. */
  1756. return cmOkRC;
  1757. }
  1758. cmRC_t cmTunedHpcpExec( cmHpcp* p, const cmComplexR_t* cqp, unsigned cqn )
  1759. {
  1760. assert( cqn == p->constQBinCnt );
  1761. // if there is no space to store the output then do nothing
  1762. if( p->frameIdx >= p->frameCnt )
  1763. return cmOkRC;
  1764. unsigned octCnt = (unsigned)floor(p->constQBinCnt / p->binsPerOctave);
  1765. unsigned i;
  1766. cmReal_t hpcpV[ p->binsPerOctave + 2 ];
  1767. unsigned idxV[ p->binsPerOctave ];
  1768. unsigned binsPerSt = p->binsPerOctave / kStPerOctave;
  1769. // Notice that the first and last elements of p->hpcp are reserved for
  1770. // use in producing the appeareance of circularity for the peak picking
  1771. // algorithm. The cmtual hpcp[] data begins on the index 1 (not 0) and
  1772. // ends on p->binsPerOctave (not p->binsPerOctave-1).
  1773. // sum the constQBinCnt constant Q bins into binsPerOctave bins to form the HPCP
  1774. for(i=0; i<p->binsPerOctave; ++i)
  1775. {
  1776. cmReal_t sum = 0;
  1777. const cmComplexR_t* sbp = cqp + i;
  1778. const cmComplexR_t* sep = cqp + (octCnt * p->binsPerOctave);
  1779. for(; sbp < sep; sbp += p->binsPerOctave)
  1780. sum += cmCabsR(*sbp);
  1781. hpcpV[i+1] = sum;
  1782. }
  1783. // shift the lowest ST center bin to (binsPerSt+1)/2 such that an equal number of
  1784. // flat and sharp bins are above an below it
  1785. int rotateCnt = ((binsPerSt+1)/2) - 1;
  1786. // shift pitch class C to the lowest semitone boundary
  1787. rotateCnt -= ( 48-(int)p->cqMinMidiPitch) * binsPerSt;
  1788. // perform the shift
  1789. cmVOR_Rotate(hpcpV+1, p->binsPerOctave, rotateCnt);
  1790. // duplicate the first and last bin to produce circularity in the hpcp
  1791. hpcpV[0] = hpcpV[ p->binsPerOctave ];
  1792. hpcpV[ p->binsPerOctave+1 ] = hpcpV[1];
  1793. // locate the indexes of the positive peaks in the hpcp
  1794. unsigned pkN = cmVOR_PeakIndexes( idxV, p->binsPerOctave, hpcpV, p->binsPerOctave, 0 );
  1795. // Convert the peak indexes to values in the range 0 to binsPerSet-1
  1796. // If stPerBin == 3 : 0=flat 1=in tune 2=sharp
  1797. cmVOU_Mod( idxV, pkN, binsPerSt );
  1798. // Form a histogram to keep count of the number of flat,in-tune and sharp peaks
  1799. cmVOU_Hist( p->histV, binsPerSt, idxV, pkN );
  1800. // store the hpcpV[] to the row p->hpcpM[p->frameIdx,:]
  1801. cmVOR_CopyN( p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt, hpcpV+1, 1 );
  1802. // write the hpcp debug file
  1803. if( p->mf0p != NULL )
  1804. cmMtxFileRealExecN( p->mf0p, p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt );
  1805. p->frameIdx++;
  1806. return cmOkRC;
  1807. }
  1808. cmRC_t cmTunedHpcpTuneAndFilter( cmHpcp* p)
  1809. {
  1810. // note: p->frameIdx now holds the cmtual count of frames in p->hpcpA[].
  1811. // p->frameCnt holds the allocated count of frames in p->hpcpA[].
  1812. unsigned i,j;
  1813. // filter each column of hpcpA[] into each row of fhpcpA[]
  1814. for(i=0; i<p->binsPerOctave; ++i)
  1815. {
  1816. cmVOR_MedianFilt( p->hpcpM + (i * p->frameCnt), p->frameIdx, p->medFiltOrder, p->fhpcpM + i, p->binsPerOctave );
  1817. // write the fhpcp[i,:] to the debug file
  1818. if( p->mf1p != NULL )
  1819. cmMtxFileRealExecN( p->mf1p, p->fhpcpM + i, p->frameIdx, p->binsPerOctave );
  1820. }
  1821. unsigned binsPerSt = p->histN;
  1822. assert( (binsPerSt > 0) && (cmIsOddU(binsPerSt)) );
  1823. unsigned maxIdx = cmVOU_MaxIndex(p->histV,binsPerSt,1);
  1824. int tuneShift = -(maxIdx - ((binsPerSt+1)/2));
  1825. cmReal_t gaussWndV[ binsPerSt ];
  1826. // generate a gaussian window
  1827. cmVOR_GaussWin( gaussWndV, binsPerSt, 2.5 );
  1828. // Rotate the window to apply tuning via the weighted sum operation below
  1829. // (the result will be equivalent to rotating p->fhpcpM[] prior to reducing the )
  1830. cmVOR_Rotate(gaussWndV, binsPerSt, tuneShift);
  1831. // zero the meanV[] before summing into it
  1832. cmVOR_Fill(p->meanV,kStPerOctave,0);
  1833. // for each frame
  1834. for(i=0; i<p->frameIdx; ++i)
  1835. {
  1836. // for each semitone
  1837. for(j=0; j<kStPerOctave; ++j)
  1838. {
  1839. // reduce each semitone to a single value by forming a weighted sum of all the assoc'd bins
  1840. cmReal_t sum = cmVOR_MultSumVV( gaussWndV, p->fhpcpM + (i*p->binsPerOctave) + (j*binsPerSt), binsPerSt );
  1841. // store time-series output to the ith column
  1842. p->outM[ (i*kStPerOctave) + j ] = sum;
  1843. // calc the sum of all chroma values in bin j.
  1844. p->meanV[ j ] += sum;
  1845. }
  1846. // write the chroma debug file
  1847. if( p->mf2p != NULL )
  1848. cmMtxFileRealExec( p->mf2p, p->outM + (i*kStPerOctave), kStPerOctave );
  1849. }
  1850. // form the chroma mean from the sum calc'd above
  1851. cmVOR_DivVS( p->meanV, kStPerOctave, p->frameIdx );
  1852. // variance
  1853. for(j=0; j<kStPerOctave; ++j)
  1854. p->varV[j] = cmVOR_VarianceN( p->outM + j, p->frameIdx, kStPerOctave, p->meanV + j );
  1855. return cmOkRC;
  1856. }
  1857. //------------------------------------------------------------------------------------------------------------
  1858. /*
  1859. cmStatsProc* cmStatsProcAlloc( cmCtx* c, cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
  1860. {
  1861. cmStatsProc* op = cmObjAlloc(cmStatsProc,c,p);
  1862. if( wndEleCnt > 0 )
  1863. if( cmStatsProcInit(op, wndEleCnt, flags ) != cmOkRC )
  1864. cmStatsProcFree(&op);
  1865. if( op != NULL )
  1866. cmCtxSetStatsProc(c,op);
  1867. return op;
  1868. }
  1869. cmRC_t cmStatsProcFree( cmStatsProc** pp )
  1870. {
  1871. cmRC_t rc = cmOkRC;
  1872. cmStatsProc* p = *pp;
  1873. if( pp == NULL || *pp == NULL )
  1874. return cmOkRC;
  1875. if((rc = cmStatsProcFinal(p) ) != cmOkRC )
  1876. return rc;
  1877. cmCtxSetStatsProc(p->obj.ctx,NULL); // complement to cmSetStatsProc() in cmStatsProcAlloc()
  1878. cmMemPtrFree(&p->regArrayV);
  1879. cmMemPtrFree(&p->m);
  1880. cmMemPtrFree(&p->sumV);
  1881. cmMemPtrFree(&p->meanV);
  1882. cmMemPtrFree(&p->varV);
  1883. cmObjFree(pp);
  1884. return cmOkRC;
  1885. }
  1886. cmRC_t cmStatsProcInit( cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
  1887. {
  1888. cmRC_t rc;
  1889. if((rc = cmStatsProcFinal(p)) != cmOkRC )
  1890. return rc;
  1891. p->wndEleCnt = wndEleCnt;
  1892. p->dimCnt = 0;
  1893. p->curIdx = 0;
  1894. p->curCnt = 1;
  1895. p->flags = flags;
  1896. p->execCnt = 0;
  1897. p->regArrayN = 0;
  1898. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"statsProc");
  1899. return rc;
  1900. }
  1901. cmRC_t cmStatsProcFinal( cmStatsProc* p )
  1902. {
  1903. if( p != NULL )
  1904. cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  1905. return cmOkRC;
  1906. }
  1907. unsigned cmStatsProcReg( cmStatsProc* p, unsigned featId, unsigned featEleCnt )
  1908. {
  1909. cmStatsProcRecd r;
  1910. r.featId = featId;
  1911. r.idx = p->dimCnt;
  1912. r.cnt = featEleCnt;
  1913. p->dimCnt += featEleCnt;
  1914. //unsigned i;
  1915. // printf("B:\n");
  1916. //for(i=0; i<p->regArrayN; ++i)
  1917. // 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);
  1918. //printf("\nA:\n");
  1919. p->regArrayV = cmMemResizePZ( cmStatsProcRecd, p->regArrayV, p->regArrayN+1 );
  1920. p->regArrayV[ p->regArrayN ] = r;
  1921. p->regArrayN++;
  1922. //for(i=0; i<p->regArrayN; ++i)
  1923. // 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);
  1924. //printf("\n");
  1925. return p->regArrayN-1; // return the index of the new reg recd
  1926. }
  1927. const cmStatsProcRecd* cmStatsProcRecdPtr( cmStatsProc* p, unsigned regId )
  1928. {
  1929. assert( regId < p->regArrayN );
  1930. return p->regArrayV + regId;
  1931. }
  1932. cmRC_t cmStatsProcExecD( cmStatsProc* p, unsigned regId, const double v[], unsigned vCnt )
  1933. {
  1934. cmRC_t rc = cmOkRC;
  1935. // on the first pass allcoate the storage buffer (m) and vectors (sumV,meanV and varV)
  1936. if( p->execCnt == 0 )
  1937. {
  1938. p->sumV = cmMemResizeZ( double, p->sumV, p->dimCnt);
  1939. p->meanV = cmMemResizeZ( double, p->meanV, p->dimCnt );
  1940. p->varV = cmMemResizeZ( double, p->varV, p->dimCnt );
  1941. p->m = cmMemResizeZ( double, p->m, p->dimCnt * p->wndEleCnt );
  1942. }
  1943. // if the storage matrix is full
  1944. if( p->curIdx == p->wndEleCnt )
  1945. return rc;
  1946. // get the pointer to this data source reg recd
  1947. assert( regId < p->regArrayN);
  1948. cmStatsProcRecd* r = p->regArrayV + regId;
  1949. // the dimensionality of the incoming data must be <= the registered dimensionality
  1950. assert( r->cnt <= vCnt );
  1951. unsigned dimIdx = r->idx;
  1952. bool updateFl = cmIsFlag(p->flags,kUpdateOnExecStatProcFl);
  1953. double* sbp = p->sumV + dimIdx; // sum base ptr
  1954. // mbp point to a segment (mbp[vCnt]) in column p->curIdx
  1955. double* mbp = p->m + (p->curIdx * p->dimCnt) + dimIdx; // mem col base ptr
  1956. const double* mep = p->m + p->dimCnt * p->wndEleCnt;
  1957. // decr the current col segment from the sum
  1958. if( updateFl )
  1959. cmVOD_SubVV( sbp, vCnt, mbp );
  1960. assert( p->m <= mbp && mbp < mep && p->m <= mbp+vCnt && mbp+vCnt <= mep );
  1961. // copy in the incoming values to mem col segment
  1962. cmVOD_Copy( mbp, vCnt, v );
  1963. if( updateFl )
  1964. {
  1965. // incr the sum from the incoming value
  1966. cmVOD_AddVV( sbp, vCnt, mbp );
  1967. // use the new sum to compute new mean values
  1968. cmVOD_DivVVS( p->meanV + dimIdx, vCnt, sbp, p->curCnt );
  1969. // update the variance - cmross each row
  1970. unsigned di;
  1971. for(di=dimIdx; di<dimIdx+vCnt; ++di )
  1972. p->varV[di] = cmVOD_VarianceN( p->m + dimIdx, p->curCnt, p->dimCnt, p->meanV + dimIdx );
  1973. }
  1974. ++p->execCnt;
  1975. return cmOkRC;
  1976. }
  1977. cmRC_t cmStatsProcExecF( cmStatsProc* p, unsigned regId, const float v[], unsigned vCnt )
  1978. {
  1979. double dv[ vCnt ];
  1980. cmVOD_CopyF(dv,vCnt,v);
  1981. cmStatsProcExecD(p,regId,dv,vCnt);
  1982. return cmOkRC;
  1983. }
  1984. cmRC_t cmStatsProcCalc(cmStatsProc* p )
  1985. {
  1986. unsigned colCnt = cmMin(p->curCnt,p->wndEleCnt);
  1987. unsigned i = 0;
  1988. cmVOD_Fill(p->sumV,p->dimCnt,0);
  1989. // sum the ith col of p->m[] into p->sumV[i]
  1990. for(; i<colCnt; ++i)
  1991. {
  1992. cmVOD_AddVV( p->sumV, p->dimCnt, p->m + (i * p->dimCnt) );
  1993. if( p->mfp != NULL )
  1994. cmMtxFileDoubleExec( p->mfp, p->sumV, p->dimCnt, 1 );
  1995. }
  1996. // calc the mean of each row
  1997. cmVOD_DivVVS( p->meanV, p->dimCnt, p->sumV, colCnt );
  1998. // calc the variance cmross each row
  1999. for(i=0; i<p->dimCnt; ++i)
  2000. p->varV[i] = cmVOD_VarianceN(p->m + i, colCnt, p->dimCnt, p->meanV + i );
  2001. return cmOkRC;
  2002. }
  2003. cmRC_t cmStatsProcAdvance( cmStatsProc* p )
  2004. {
  2005. ++p->curIdx;
  2006. if( p->curIdx > p->wndEleCnt )
  2007. p->curIdx = 0;
  2008. p->curCnt = cmMin(p->curCnt+1,p->wndEleCnt);
  2009. return cmOkRC;
  2010. }
  2011. void cmStatsProcTest( cmVReportFuncPtr_t vReportFunc )
  2012. {
  2013. enum
  2014. {
  2015. wndEleCnt = 7,
  2016. dDimCnt = 3,
  2017. fDimCnt = 2,
  2018. dimCnt = dDimCnt + fDimCnt,
  2019. kTypeId0 = 0,
  2020. kTypeId1 = 1
  2021. };
  2022. unsigned flags = 0;
  2023. unsigned i;
  2024. double dd[ dDimCnt * wndEleCnt ] =
  2025. {
  2026. 0, 1, 2,
  2027. 3, 4, 5,
  2028. 6, 7, 8,
  2029. 9, 10, 11,
  2030. 12, 13, 14,
  2031. 15, 16, 17,
  2032. 18, 19, 20
  2033. };
  2034. float fd[ 14 ] =
  2035. {
  2036. 0, 1,
  2037. 2, 3,
  2038. 4, 5,
  2039. 6, 7,
  2040. 8, 9,
  2041. 10, 11,
  2042. 12, 13
  2043. };
  2044. cmCtx c;
  2045. cmCtxInit(&c, vReportFunc, vReportFunc, NULL );
  2046. cmStatsProc* p = cmStatsProcAlloc( &c, NULL, wndEleCnt, flags );
  2047. unsigned regId0 = cmStatsProcReg( p, kTypeId0, dDimCnt );
  2048. unsigned regId1 = cmStatsProcReg( p, kTypeId1, fDimCnt );
  2049. for(i=0; i<wndEleCnt; ++i)
  2050. {
  2051. cmStatsProcExecD( p, regId0, dd + (i*dDimCnt), dDimCnt );
  2052. cmStatsProcExecF( p, regId1, fd + (i*fDimCnt), fDimCnt );
  2053. cmStatsProcAdvance(p);
  2054. }
  2055. cmStatsProcCalc( p);
  2056. cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->meanV );
  2057. cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->varV );
  2058. cmStatsProcFree(&p);
  2059. }
  2060. */
  2061. //------------------------------------------------------------------------------------------------------------
  2062. cmBeatHist* cmBeatHistAlloc( cmCtx* c, cmBeatHist* ap, unsigned frmCnt )
  2063. {
  2064. cmBeatHist* p = cmObjAlloc(cmBeatHist,c,ap);
  2065. p->fft = cmFftAllocRR(c,NULL,NULL,0,kToPolarFftFl);
  2066. p->ifft = cmIFftAllocRR(c,NULL,0);
  2067. if( frmCnt > 0 )
  2068. if( cmBeatHistInit(p,frmCnt) != cmOkRC )
  2069. cmBeatHistFree(&p);
  2070. return p;
  2071. }
  2072. cmRC_t cmBeatHistFree( cmBeatHist** pp )
  2073. {
  2074. cmRC_t rc = cmOkRC;
  2075. cmBeatHist* p = *pp;
  2076. if( pp == NULL || *pp == NULL )
  2077. return cmOkRC;
  2078. if((rc = cmBeatHistFinal(p)) != cmOkRC )
  2079. return rc;
  2080. cmMemPtrFree(&p->m);
  2081. cmMemPtrFree(&p->H);
  2082. cmMemPtrFree(&p->df);
  2083. cmMemPtrFree(&p->fdf);
  2084. cmMemPtrFree(&p->histV);
  2085. cmFftFreeRR(&p->fft);
  2086. cmIFftFreeRR(&p->ifft);
  2087. cmObjFree(&p);
  2088. return rc;
  2089. }
  2090. void _cmBeatHistInitH( cmReal_t* H, unsigned hrn, unsigned hcn, unsigned ri, unsigned c0, unsigned c1 )
  2091. {
  2092. unsigned ci;
  2093. for(ci=c0; ci<=c1; ++ci)
  2094. H[ (ci*hrn) + ri ] = 1;
  2095. }
  2096. cmRC_t cmBeatHistInit( cmBeatHist* p, unsigned frmCnt )
  2097. {
  2098. cmRC_t rc;
  2099. unsigned i,j,k;
  2100. enum { kLagFact = 4, kHistBinCnt=15, kHColCnt=128 };
  2101. if((rc = cmBeatHistFinal(p)) != cmOkRC )
  2102. return rc;
  2103. p->frmCnt = frmCnt;
  2104. p->maxLagCnt = (unsigned)floor(p->frmCnt / kLagFact);
  2105. p->histBinCnt= kHistBinCnt;
  2106. p->hColCnt = kHColCnt;
  2107. p->dfi = 0;
  2108. unsigned cfbMemN = p->frmCnt * p->maxLagCnt;
  2109. unsigned hMemN = p->histBinCnt * kHColCnt;
  2110. //cmArrayResizeVZ(p->obj.ctx,&p->cfbMem, cmReal_t, &p->m, cfbMemN, &p->H, hMemN, NULL);
  2111. p->m = cmMemResizeZ( cmReal_t, p->m, cfbMemN );
  2112. p->H = cmMemResizeZ( cmReal_t, p->H, hMemN );
  2113. //p->df = cmArrayResizeZ(c,&p->dfMem, 2*p->frmCnt + kHistBinCnt, cmReal_t);
  2114. //p->fdf = p->df + p->frmCnt;
  2115. //p->histV = p->fdf + p->frmCnt;
  2116. //cmArrayResizeVZ(p->obj.ctx, &p->dfMem, cmReal_t, &p->df, p->frmCnt, &p->fdf, p->frmCnt, &p->histV, kHistBinCnt, NULL );
  2117. p->df = cmMemResizeZ( cmReal_t, p->df, p->frmCnt );
  2118. p->fdf = cmMemResizeZ( cmReal_t, p->fdf, p->frmCnt );
  2119. p->histV = cmMemResizeZ( cmReal_t, p->histV, kHistBinCnt );
  2120. cmFftInitRR( p->fft,NULL,cmNextPowerOfTwo(2*frmCnt),kToPolarFftFl);
  2121. cmIFftInitRR(p->ifft,p->fft->binCnt);
  2122. // initialize H
  2123. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 0, 103, 127 );
  2124. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 1, 86, 102 );
  2125. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 2, 73, 85 );
  2126. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 3, 64, 72 );
  2127. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 4, 57, 63 );
  2128. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 5, 51, 56 );
  2129. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 6, 46, 50 );
  2130. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 7, 43, 45 );
  2131. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 8, 39, 42 );
  2132. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 9, 36, 38 );
  2133. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 10,32, 35 );
  2134. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 11,28, 31 );
  2135. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 12,25, 27 );
  2136. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 13,21, 24 );
  2137. _cmBeatHistInitH( p->H, p->histBinCnt, p->hColCnt, 14,11, 20 );
  2138. // for each column
  2139. for(i=0; i<p->maxLagCnt; ++i)
  2140. {
  2141. // for each lag group
  2142. for(j=0; j<kLagFact; ++j)
  2143. {
  2144. for(k=0; k<=2*j; ++k)
  2145. {
  2146. unsigned idx = (i*p->frmCnt) + (i*j) + i + k;
  2147. if( idx < cfbMemN )
  2148. p->m[ idx ] = 1.0/(2*j+1);
  2149. }
  2150. }
  2151. }
  2152. double g[ p->maxLagCnt ];
  2153. double g_max = 0;
  2154. double b = 43;
  2155. b = b*b;
  2156. for(i=0; i<p->maxLagCnt; ++i)
  2157. {
  2158. double n = i+1;
  2159. g[i] = n/b * exp(-(n*n) / (2*b));
  2160. if( g[i] > g_max )
  2161. g_max = g[i];
  2162. }
  2163. // normalize g[]
  2164. cmVOD_DivVS( g, p->maxLagCnt, g_max );
  2165. // for each column of p->m[]
  2166. for(i=0; i<p->maxLagCnt; ++i)
  2167. {
  2168. double gg = g[i];
  2169. k = i*p->frmCnt;
  2170. for(j=0; j<p->frmCnt; ++j)
  2171. p->m[ k + j ] *= gg;
  2172. }
  2173. //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"beatHist");
  2174. return cmOkRC;
  2175. }
  2176. cmRC_t cmBeatHistFinal( cmBeatHist* p )
  2177. {
  2178. //if( p != NULL )
  2179. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  2180. return cmOkRC;
  2181. }
  2182. cmRC_t cmBeatHistExec( cmBeatHist* p, cmSample_t df )
  2183. {
  2184. if( p->dfi < p->frmCnt )
  2185. p->df[p->dfi++] = df;
  2186. return cmOkRC;
  2187. }
  2188. cmRC_t cmBeatHistCalc( cmBeatHist* p )
  2189. {
  2190. unsigned i;
  2191. // df -= mean(df)
  2192. cmVOR_SubVS(p->df,p->frmCnt,cmVOR_Mean(p->df,p->frmCnt));
  2193. //cmPlotLineR( "dfm", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2194. // take alpha norm of df
  2195. double alpha = 9;
  2196. cmVOR_DivVS(p->df,p->frmCnt, cmVOR_AlphaNorm(p->df,p->frmCnt,alpha));
  2197. //cmPlotLineS( "dfd", NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2198. // low pass forward/backward filter df[] into fdf[]
  2199. cmReal_t b[] = {0.1600, 0.3200, 0.1600};
  2200. unsigned bn = sizeof(b)/sizeof(b[0]);
  2201. cmReal_t a[] = {1.0000, -0.5949, 0.2348};
  2202. unsigned an = sizeof(a)/sizeof(a[0]);
  2203. cmFilterFilterR(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);
  2204. //cmPlotLineS( "fdf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2205. // median filter to low-passed filtered fdf[] into df[]
  2206. cmVOR_FnThresh(p->fdf,p->frmCnt,16,p->df,1,NULL);
  2207. // subtract med filtered signal from the low pa1ssed signal.
  2208. // fdf[] -= df[];
  2209. cmVOR_SubVV(p->fdf,p->frmCnt,p->df);
  2210. // half-wave rectify fdf[] = set all negative values in fdf[] to zero.
  2211. cmVOR_HalfWaveRectify(p->fdf,p->frmCnt,p->fdf);
  2212. //cmPlotLineS( "meddf", NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2213. // take FT of fdf[]
  2214. cmFftExecRR(p->fft,p->fdf,p->frmCnt);
  2215. // square FT magn.
  2216. cmVOR_PowVS(p->fft->magV,p->fft->binCnt,2);
  2217. //cmPlotLineS( "mag", NULL, p->fft->magV, NULL, p->fft->binCnt, NULL, kSolidPlotLineId );
  2218. // take the IFFT of the squared magnitude vector.
  2219. cmVOR_Fill(p->fft->phsV,p->fft->binCnt,0);
  2220. cmIFftExecRectRR(p->ifft,p->fft->magV,p->fft->phsV);
  2221. // Matlab automatically provides this scaling as part of the IFFT.
  2222. cmVOR_DivVS(p->ifft->outV,p->ifft->outN,p->ifft->outN);
  2223. // remove bias for short periods from CMF
  2224. for(i=0; i<p->frmCnt; ++i)
  2225. p->ifft->outV[i] /= (p->frmCnt - i);
  2226. //cmPlotLineS( "cm", NULL, p->ifft->outV, NULL, p->frmCnt, NULL, kSolidPlotLineId );
  2227. // apply comb filter to the CMF and store result in df[maxLagCnt]
  2228. cmVOR_MultVMtV(p->df,p->maxLagCnt,p->m,p->frmCnt,p->ifft->outV);
  2229. //acPlotLineS( "cfb", NULL, p->df, NULL, p->maxLagCnt, NULL, kSolidPlotLineId );
  2230. //acVOR_Print(p->obj.err.rpt,1,p->maxLagCnt,p->df);
  2231. assert( p->maxLagCnt == p->hColCnt );
  2232. cmVOR_MultVMV(p->histV,p->histBinCnt,p->H,p->hColCnt,p->df);
  2233. cmReal_t bins[] = { 25, 17, 13, 9, 7, 6, 5, 3, 4, 3, 4, 4, 3, 4, 10};
  2234. cmVOR_DivVV( p->histV, p->histBinCnt, bins );
  2235. //cmPlotLineS( "cfb", NULL, p->histV, NULL, p->histBinCnt, NULL, kSolidPlotLineId );
  2236. if( p->mfp != NULL )
  2237. cmMtxFileRealExec( p->mfp, p->histV, p->histBinCnt );
  2238. return cmOkRC;
  2239. }
  2240. //------------------------------------------------------------------------------------------------------------
  2241. 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 )
  2242. {
  2243. cmGmm_t* p = cmObjAlloc( cmGmm_t, c, ap );
  2244. if( K > 0 && D > 0 )
  2245. if( cmGmmInit(p,K,D,gM,uM,sMM,uflags) != cmOkRC )
  2246. cmGmmFree(&p);
  2247. return p;
  2248. }
  2249. cmRC_t cmGmmFree( cmGmm_t** pp )
  2250. {
  2251. cmRC_t rc = cmOkRC;
  2252. cmGmm_t* p = *pp;
  2253. if( pp == NULL || *pp == NULL )
  2254. return cmOkRC;
  2255. if((rc = cmGmmFinal(p)) != cmOkRC )
  2256. return rc;
  2257. cmMemPtrFree(&p->gV);
  2258. cmMemPtrFree(&p->uM);
  2259. cmMemPtrFree(&p->sMM);
  2260. cmMemPtrFree(&p->isMM);
  2261. cmMemPtrFree(&p->uMM);
  2262. cmMemPtrFree(&p->logDetV);
  2263. cmMemPtrFree(&p->t);
  2264. cmObjFree(pp);
  2265. return rc;
  2266. }
  2267. cmRC_t _cmGmmUpdateCovar( cmGmm_t* p, const cmReal_t* sMM )
  2268. {
  2269. unsigned i;
  2270. if( sMM == NULL )
  2271. return cmOkRC;
  2272. unsigned De2 = p->D*p->D;
  2273. unsigned KDe2 = p->K*De2;
  2274. cmVOR_Copy(p->sMM, KDe2, sMM);
  2275. cmVOR_Copy(p->isMM,KDe2, sMM);
  2276. cmVOR_Copy(p->uMM, KDe2, sMM);
  2277. //if( cmIsFlag(p->uflags,cmGmmCovarNoProcFl) )
  2278. // return cmOkRC;
  2279. // for each component
  2280. for(i=0; i<p->K; ++i)
  2281. {
  2282. cmReal_t* is = p->isMM + i*De2;
  2283. cmReal_t* u = p->uMM + i*De2;
  2284. cmReal_t* r;
  2285. // if the covariance matrix is diagnal
  2286. if( cmIsFlag(p->uflags,cmGmmDiagFl))
  2287. {
  2288. p->logDetV[i] = cmVOR_LogDetDiagM(is,p->D); // calc the det. of diag. covar. mtx
  2289. r = cmVOR_InvDiagM(is,p->D); // calc the inverse in place
  2290. }
  2291. else
  2292. {
  2293. p->logDetV[i] = cmVOR_LogDetM(is,p->D); // calc the det. of covar mtx
  2294. r = cmVOR_InvM(is,p->D); // calc the inverse in place
  2295. }
  2296. if( fabs(p->logDetV[i]) < 1e-20 )
  2297. {
  2298. cmCtxPrint(p->obj.ctx,"%i\n",i);
  2299. cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
  2300. }
  2301. if( cmVOR_CholZ(u,p->D) == NULL )
  2302. {
  2303. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (Cholesky factorization failed.) was encountered in _cmGmmUpdateCovar().");
  2304. }
  2305. if( p->logDetV[i] == 0 )
  2306. {
  2307. cmGmmPrint(p,true);
  2308. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (det==0) was encountered in _cmGmmUpdateCovar().");
  2309. }
  2310. if( r == NULL )
  2311. {
  2312. //cmCtxPrint(c,"%i\n",i);
  2313. //cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
  2314. return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (inversion failed) was encountered in _cmGmmUpdateCovar().");
  2315. }
  2316. }
  2317. return cmOkRC;
  2318. }
  2319. cmRC_t cmGmmInit( cmGmm_t* p, unsigned K, unsigned D, const cmReal_t* gV, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
  2320. {
  2321. cmRC_t rc;
  2322. if((rc = cmGmmFinal(p)) != cmOkRC )
  2323. return rc;
  2324. // gM[K] uM[DK] sMM[DDK] isMM[DDK]+ uMM[DDK] logDetV[K] t[DD] fact[K]
  2325. /*
  2326. unsigned n = K + (D*K) + (D*D*K) + (D*D*K) + (D*D*K) + K + (D*D) + K;
  2327. p->gV = cmArrayResizeZ(c,&p->memA, n, cmReal_t );
  2328. p->uM = p->gV + K;
  2329. p->sMM = p->uM + (D*K);
  2330. p->isMM = p->sMM + (D*D*K);
  2331. p->uMM = p->isMM + (D*D*K);
  2332. p->logDetV = p->uMM + (D*D*K);
  2333. p->t = p->logDetV + K;
  2334. */
  2335. //cmArrayResizeVZ(c, &p->memA, cmReal_t, &p->gV,K, &p->uM,D*K, &p->sMM,D*D*K,
  2336. //&p->isMM,D*D*K, &p->uMM,D*D*K, &p->logDetV,K, &p->t,D*D, NULL );
  2337. p->gV = cmMemResizeZ( cmReal_t, p->gV, K );
  2338. p->uM = cmMemResizeZ( cmReal_t, p->uM, D*K);
  2339. p->sMM = cmMemResizeZ( cmReal_t, p->sMM, D*D*K);
  2340. p->isMM = cmMemResizeZ( cmReal_t, p->isMM, D*D*K);
  2341. p->uMM = cmMemResizeZ( cmReal_t, p->uMM, D*D*K);
  2342. p->logDetV = cmMemResizeZ( cmReal_t, p->logDetV, K);
  2343. p->t = cmMemResizeZ( cmReal_t, p->t, D*D );
  2344. p->K = K;
  2345. p->D = D;
  2346. p->uflags = uflags;
  2347. if( gV != NULL )
  2348. cmVOR_Copy(p->gV,K,gV);
  2349. if( uM != NULL )
  2350. cmVOR_Copy(p->uM,D*K,uM);
  2351. return _cmGmmUpdateCovar(p,sMM );
  2352. }
  2353. cmRC_t cmGmmFinal( cmGmm_t* p )
  2354. { return cmOkRC; }
  2355. typedef struct
  2356. {
  2357. cmGmm_t* p;
  2358. const cmReal_t* xM;
  2359. unsigned colCnt;
  2360. } _cmGmmRdFuncData_t;
  2361. const cmReal_t* _cmGmmReadFunc( void* userPtr, unsigned colIdx )
  2362. {
  2363. assert(colIdx < ((const _cmGmmRdFuncData_t*)userPtr)->colCnt);
  2364. return ((const _cmGmmRdFuncData_t*)userPtr)->xM + (colIdx * ((const _cmGmmRdFuncData_t*)userPtr)->p->D);
  2365. }
  2366. // xM[D,xN]
  2367. // yV[xN]
  2368. // yM[xN,K]
  2369. cmRC_t cmGmmEval( cmGmm_t* p, const cmReal_t* xM, unsigned xN, cmReal_t* yV, cmReal_t* yM )
  2370. {
  2371. _cmGmmRdFuncData_t r;
  2372. r.colCnt = xN;
  2373. r.p = p;
  2374. r.xM = xM;
  2375. return cmGmmEval2(p,_cmGmmReadFunc,&r,xN,yV,yM);
  2376. }
  2377. cmRC_t cmGmmEval2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, cmReal_t* yV, cmReal_t* yM)
  2378. {
  2379. cmReal_t tV[xN];
  2380. unsigned k;
  2381. //cmVOR_PrintL("cV: ",p->obj.err.rpt, 1, p->K, p->gV);
  2382. //cmVOR_PrintL("uM:\n",p->obj.err.rpt, p->D, p->K, p->uM );
  2383. //
  2384. cmVOR_Fill(yV,xN,0);
  2385. // for each component PDF
  2386. for(k=0; k<p->K; k++)
  2387. {
  2388. const cmReal_t* meanV = p->uM + (k*p->D);
  2389. const cmReal_t* isM = p->isMM + (k*p->D*p->D);
  2390. cmReal_t* pV;
  2391. if( yM == NULL )
  2392. pV = tV;
  2393. else
  2394. pV = yM + (k*xN);
  2395. // evaluate the kth component PDF with xM[1:T]
  2396. //cmVOR_MultVarGaussPDF2( pV, xM, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2397. cmVOR_MultVarGaussPDF3( pV, readFunc, userFuncPtr, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2398. // apply the kth component weight
  2399. cmVOR_MultVS( pV, xN, p->gV[k] );
  2400. // sum the result into the output vector
  2401. cmVOR_AddVV( yV, xN, pV );
  2402. }
  2403. return cmOkRC;
  2404. }
  2405. // Evaluate each component for a single data point
  2406. // xV[D] - observed data point
  2407. // yV[K] - output contains the evaluation for each component
  2408. cmRC_t cmGmmEval3( cmGmm_t* p, const cmReal_t* xV, cmReal_t* yV )
  2409. {
  2410. unsigned k;
  2411. for(k=0; k<p->K; ++k)
  2412. {
  2413. const cmReal_t* meanV = p->uM + (k*p->D);
  2414. const cmReal_t* isM = p->isMM + (k*p->D*p->D);
  2415. // evaluate the kth component PDF with xM[1:T]
  2416. cmVOR_MultVarGaussPDF2( yV + k, xV, meanV, isM, p->logDetV[k], p->D, 1, cmIsFlag(p->uflags,cmGmmDiagFl) );
  2417. // apply the kth component weight
  2418. yV[k] *= p->gV[k];
  2419. }
  2420. return cmOkRC;
  2421. }
  2422. cmReal_t _cmGmmKmeansDistFunc( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
  2423. { return cmVOR_EuclidDistance(v0,v1,vn); }
  2424. cmRC_t cmGmmRandomize2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* funcUserPtr, unsigned xN, const cmReal_t* uM, const bool* roFlV )
  2425. {
  2426. unsigned k;
  2427. unsigned iV[ p->K ];
  2428. if( uM == NULL )
  2429. roFlV = NULL;
  2430. // randomize the mixture coefficients
  2431. cmVOR_Random( p->gV, p->K, 0.0, 1.0 );
  2432. cmVOR_NormalizeProbability(p->gV,p->K);
  2433. // fill iV with random integers between 0 and xN-1
  2434. cmVOU_Random( iV, p->K, xN-1 );
  2435. // for each component
  2436. for(k=0; k<p->K; ++k)
  2437. {
  2438. cmReal_t r[ p->D ];
  2439. // if this component's mean is not read-only
  2440. if( roFlV==NULL || roFlV[k]==false )
  2441. {
  2442. const cmReal_t* xV = NULL;
  2443. if( uM == NULL )
  2444. xV = readFunc( funcUserPtr, iV[k] ); // read a random frame index
  2445. else
  2446. xV = uM + (k*p->D); // select a user supplied mean vector
  2447. assert( xV != NULL );
  2448. // set the random feature vector as this components mean value
  2449. cmVOR_Copy(p->uM+(k*p->D),p->D,xV);
  2450. }
  2451. cmReal_t* sM = p->sMM+(k*p->D*p->D);
  2452. // create a random covariance mtx
  2453. if( cmIsFlag(p->uflags,cmGmmDiagFl) )
  2454. {
  2455. // create a random diag. covar mtx
  2456. cmVOR_Random(r,p->D,0.0,1.0);
  2457. cmVOR_Diag(sM,p->D,r);
  2458. }
  2459. else
  2460. {
  2461. // create a random symetric positive definite matrix
  2462. cmVOR_RandSymPosDef(sM,p->D,p->t);
  2463. }
  2464. }
  2465. unsigned* classIdxV = cmMemAllocZ(unsigned, xN );
  2466. // if some components have read-only mean's
  2467. if( uM != NULL && roFlV != NULL )
  2468. {
  2469. assert( xN >= p->K );
  2470. for(k=0; k<p->K; ++k)
  2471. classIdxV[k] = roFlV[k];
  2472. }
  2473. // use kmeans clustering to move the means closer to their center values
  2474. if( cmIsFlag( p->uflags, cmGmmSkipKmeansFl) == false )
  2475. cmVOR_Kmeans2( classIdxV, p->uM, p->K, readFunc, p->D, xN, funcUserPtr, _cmGmmKmeansDistFunc, NULL, -1, 0 );
  2476. cmMemPtrFree(&classIdxV);
  2477. return _cmGmmUpdateCovar(p,p->sMM);
  2478. }
  2479. cmRC_t cmGmmRandomize( cmGmm_t* p, const cmReal_t* xM, unsigned xN )
  2480. {
  2481. _cmGmmRdFuncData_t r;
  2482. r.colCnt = xN;
  2483. r.p = p;
  2484. r.xM = xM;
  2485. return cmGmmRandomize2(p,_cmGmmReadFunc,&r,xN,NULL,NULL);
  2486. }
  2487. // xM[D,xN]
  2488. cmRC_t cmGmmTrain( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
  2489. {
  2490. unsigned i,k;
  2491. cmRC_t rc;
  2492. if((rc = cmGmmRandomize(p,xM,xN)) != cmOkRC )
  2493. return rc;
  2494. //cmGmmPrint(c,p);
  2495. // wM[xN,K]
  2496. cmReal_t wM[ xN * p->K ]; // wM[N,K] soft assignment mtx
  2497. unsigned wV[ xN ]; // wV[N] hard assignment vector
  2498. unsigned stopCnt = 0;
  2499. unsigned curStopCnt = 0;
  2500. if( iterCntPtr != NULL )
  2501. {
  2502. stopCnt = *iterCntPtr;
  2503. *iterCntPtr = 0;
  2504. }
  2505. else
  2506. {
  2507. // BUG BUG BUG
  2508. // stopCnt is used uninitialized when iterCntPtr == NULL
  2509. assert( 0 );
  2510. }
  2511. while(1)
  2512. {
  2513. //cmVOR_NormalizeProbability(p->gV,p->K);
  2514. cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
  2515. cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2516. cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
  2517. //cmGmmPrint(c,p);
  2518. for(k=0; k<p->K; ++k)
  2519. {
  2520. cmReal_t* wp = wM + (k*xN);
  2521. // calc the prob that each data point in xM[] was generated by the kth gaussian
  2522. // and store as a column vector in wM[:,k]
  2523. 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) );
  2524. // scale the prob by the gain coeff for gaussian k
  2525. cmVOR_MultVS( wp, xN, p->gV[k]);
  2526. }
  2527. //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
  2528. bool doneFl = true;
  2529. for(i=0; i<xN; ++i)
  2530. {
  2531. // form a probability for the ith data point weights
  2532. cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
  2533. // select the cluster to which the ith data point is most likely to belong
  2534. unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
  2535. // if the ith data point changed clusters
  2536. if( mi != wV[i] )
  2537. {
  2538. doneFl = false;
  2539. wV[i] = mi;
  2540. }
  2541. }
  2542. curStopCnt = doneFl ? curStopCnt+1 : 0;
  2543. // if no data points changed owners then the clustering is complete
  2544. if( curStopCnt == stopCnt )
  2545. {
  2546. //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
  2547. break;
  2548. }
  2549. // for each cluster
  2550. for(k=0; k<p->K; ++k)
  2551. {
  2552. cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
  2553. cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
  2554. cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
  2555. // update the kth weight
  2556. p->gV[k] = sw / xN;
  2557. // form a sum of all data points weighted by their soft assignment to cluster k
  2558. cmReal_t sumV[p->D];
  2559. cmVOR_MultVMV( sumV, p->D, xM, xN, wM + k*xN );
  2560. // update the mean[k]
  2561. cmVOR_DivVVS(uV, p->D, sumV, sw );
  2562. // update the covar[k]
  2563. // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
  2564. cmVOR_Fill(sM,p->D*p->D,0);
  2565. for(i=0; i<xN; ++i)
  2566. {
  2567. cmReal_t duV[ p->D ];
  2568. cmVOR_SubVVV( duV, p->D, xM + (i*p->D), uV ); // duV[] = xM[:,i] - uV[];
  2569. cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
  2570. cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
  2571. cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
  2572. }
  2573. cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
  2574. }
  2575. // update the inverted covar mtx and covar det.
  2576. if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
  2577. return rc;
  2578. if( iterCntPtr != NULL )
  2579. *iterCntPtr += 1;
  2580. }
  2581. return cmOkRC;
  2582. }
  2583. // xM[D,xN]
  2584. cmRC_t cmGmmTrain2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, unsigned* iterCntPtr, const cmReal_t* uM, const bool* roFlV, int maxIterCnt )
  2585. {
  2586. unsigned i,j,k;
  2587. cmRC_t rc;
  2588. // if uM[] is not set then ignore roFlV[]
  2589. if( uM == NULL )
  2590. roFlV=NULL;
  2591. if((rc = cmGmmRandomize2(p,readFunc,userFuncPtr,xN,uM,roFlV)) != cmOkRC )
  2592. return rc;
  2593. //cmGmmPrint(c,p);
  2594. // wM[xN,K] soft assignment mtx
  2595. cmReal_t* wM = cmMemAlloc(cmReal_t, xN * p->K );
  2596. // wV[N] hard assignment vector
  2597. unsigned* wV = cmMemAlloc(unsigned, xN );
  2598. unsigned stopCnt = 0;
  2599. unsigned curStopCnt = 0;
  2600. if( iterCntPtr != NULL )
  2601. {
  2602. stopCnt = *iterCntPtr;
  2603. *iterCntPtr = 0;
  2604. }
  2605. else
  2606. {
  2607. // BUG BUG BUG
  2608. // stopCnt is used uninitialized when iterCntPtr == NULL
  2609. assert( 0 );
  2610. }
  2611. while(1)
  2612. {
  2613. //cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
  2614. //cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2615. cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1, p->K, p->gV );
  2616. //cmGmmPrint(c,p);
  2617. for(k=0; k<p->K; ++k)
  2618. {
  2619. cmReal_t* wp = wM + (k*xN);
  2620. // calc the prob that each data point in xM[] was generated by the kth gaussian
  2621. // and store as a column vector in wM[:,k]
  2622. 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) );
  2623. // scale the prob by the gain coeff for gaussian k
  2624. cmVOR_MultVS( wp, xN, p->gV[k]);
  2625. }
  2626. //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);
  2627. bool doneFl = true;
  2628. unsigned changeCnt = 0;
  2629. for(i=0; i<xN; ++i)
  2630. {
  2631. // form a probability for the ith data point weights
  2632. cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
  2633. // select the cluster to which the ith data point is most likely to belong
  2634. unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);
  2635. // if the ith data point changed clusters
  2636. if( mi != wV[i] )
  2637. {
  2638. ++changeCnt;
  2639. doneFl = false;
  2640. wV[i] = mi;
  2641. }
  2642. }
  2643. curStopCnt = doneFl ? curStopCnt+1 : 0;
  2644. printf("%i stopCnt:%i changeCnt:%i\n",*iterCntPtr,curStopCnt,changeCnt);
  2645. // if no data points changed owners then the clustering is complete
  2646. if( curStopCnt == stopCnt )
  2647. {
  2648. //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
  2649. break;
  2650. }
  2651. // if a maxIterCnt was given and the cur iter cnt exceeds the max iter cnt then stop
  2652. if( maxIterCnt>=1 && *iterCntPtr >= maxIterCnt )
  2653. break;
  2654. // form a sum of all data points weighted by their soft assignment to cluster k
  2655. // NOTE: cmGmmTrain() performs this step more efficiently because it use
  2656. // an LAPACK matrix multiply.
  2657. cmReal_t sumM[ p->D * p->K ];
  2658. cmVOR_Zero(sumM,p->D*p->K);
  2659. for(i=0; i<xN; ++i)
  2660. {
  2661. const cmReal_t* xV = readFunc( userFuncPtr, i );
  2662. assert( xV != NULL );
  2663. for(k=0; k<p->K; ++k)
  2664. {
  2665. cmReal_t weight = wM[ i + (k*xN)];
  2666. for(j=0; j<p->D; ++j)
  2667. sumM[ j + (k*p->D) ] += xV[j] * weight;
  2668. }
  2669. }
  2670. // for each cluster that is not marked as read-only
  2671. for(k=0; k<p->K; ++k)
  2672. {
  2673. cmReal_t* uV = p->uM + (k*p->D); // meanV[k]
  2674. cmReal_t* sM = p->sMM + (k*p->D*p->D); // covarM[k]
  2675. cmReal_t sw = cmVOR_Sum(wM + (k*xN), xN );
  2676. // update the kth weight
  2677. p->gV[k] = sw / xN;
  2678. // if this component's mean is not read-only
  2679. if( (roFlV==NULL || roFlV[k]==false) && sw != 0)
  2680. {
  2681. // get vector of all data points weighted by their soft assignment to cluster k
  2682. cmReal_t* sumV = sumM + (k*p->D); // sumV[p->D];
  2683. // update the mean[k]
  2684. cmVOR_DivVVS(uV, p->D, sumV, sw );
  2685. }
  2686. // update the covar[k]
  2687. // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)'));
  2688. cmVOR_Fill(sM,p->D*p->D,0);
  2689. for(i=0; i<xN; ++i)
  2690. {
  2691. cmReal_t duV[ p->D ];
  2692. const cmReal_t* xV = readFunc( userFuncPtr, i );
  2693. assert( xV != NULL );
  2694. cmVOR_SubVVV( duV, p->D, xV, uV ); // duV[] = xM[:,i] - uV[];
  2695. cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 ); // t[,] = duV[] * duV[]'
  2696. cmVOR_MultVS( p->t, p->D*p->D, wM[ k * xN + i ]); // t[,] *= wM[i,k]
  2697. cmVOR_AddVV( sM, p->D*p->D, p->t ); // sM[,] += t[,];
  2698. }
  2699. if( sw != 0 )
  2700. cmVOR_DivVS( sM, p->D*p->D, sw ); // sM[,] ./ sw;
  2701. }
  2702. // update the inverted covar mtx and covar det.
  2703. if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
  2704. goto errLabel;
  2705. if( iterCntPtr != NULL )
  2706. *iterCntPtr += 1;
  2707. }
  2708. cmMemPtrFree(&wM);
  2709. cmMemPtrFree(&wV);
  2710. errLabel:
  2711. return cmOkRC;
  2712. }
  2713. cmRC_t cmGmmTrain3( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
  2714. {
  2715. _cmGmmRdFuncData_t r;
  2716. r.colCnt = xN;
  2717. r.p = p;
  2718. r.xM = xM;
  2719. return cmGmmTrain2(p,_cmGmmReadFunc,&r,xN,iterCntPtr,NULL,NULL,-1);
  2720. }
  2721. cmRC_t cmGmmGenerate( cmGmm_t* p, cmReal_t* yM, unsigned yN )
  2722. {
  2723. unsigned i=0;
  2724. unsigned kV[yN];
  2725. // use weighted random selection to choose the component for each output value
  2726. cmVOR_WeightedRandInt(kV,yN, p->gV, p->K );
  2727. // cmVOU_Print(p->obj.err.rpt,1,yN,kV);
  2728. for(i=0; i<yN; ++i)
  2729. {
  2730. const cmReal_t* uV = p->uM + (kV[i] * p->D);
  2731. unsigned idx = kV[i] * p->D * p->D;
  2732. //cmVOR_PrintL("sM\n",p->obj.err.rpt,p->D,p->D,sM);
  2733. if( cmIsFlag(p->uflags,cmGmmDiagFl) )
  2734. {
  2735. const cmReal_t* sM = p->sMM + idx;
  2736. cmVOR_RandomGaussDiagM( yM + (i*p->D), p->D, 1, uV, sM );
  2737. }
  2738. else
  2739. {
  2740. const cmReal_t* uM = p->uMM + idx;
  2741. cmVOR_RandomGaussNonDiagM2(yM + (i*p->D), p->D, 1, uV, uM );
  2742. }
  2743. }
  2744. return cmOkRC;
  2745. }
  2746. void cmGmmPrint( cmGmm_t* p, bool fl )
  2747. {
  2748. unsigned k;
  2749. //cmCtx* c = p->obj.ctx;
  2750. cmVOR_PrintL("gV: ", p->obj.err.rpt, 1, p->K, p->gV );
  2751. cmVOR_PrintL("mM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
  2752. for(k=0; k<p->K; ++k)
  2753. cmVOR_PrintL("sM:\n", p->obj.err.rpt, p->D, p->D, p->sMM + (k*p->D*p->D));
  2754. if( fl )
  2755. {
  2756. for(k=0; k<p->K; ++k)
  2757. cmVOR_PrintL("isM:\n", p->obj.err.rpt, p->D, p->D, p->isMM + (k*p->D*p->D));
  2758. for(k=0; k<p->K; ++k)
  2759. cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->D, p->uMM + (k*p->D*p->D));
  2760. cmVOR_PrintL("logDetV:\n", p->obj.err.rpt, 1, p->K, p->logDetV);
  2761. }
  2762. }
  2763. void cmGmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  2764. {
  2765. cmCtx* c = cmCtxAlloc(NULL,rpt,lhH,stH);
  2766. unsigned K = 2;
  2767. unsigned D = 2;
  2768. cmReal_t gV[ 2 ] = { .5, .5 };
  2769. cmReal_t uM[ 4 ] = { .3, .3, .8, .8 };
  2770. cmReal_t sMM[ 8 ] = { .1, 0, 0, .1, .1, 0, 0, .1 };
  2771. unsigned flags = cmGmmDiagFl ;
  2772. unsigned M = 100;
  2773. cmReal_t xM[ D*M ];
  2774. cmReal_t yV[ M ];
  2775. unsigned i,j;
  2776. cmPlotSetup("MultDimGauss Test",1,1);
  2777. cmGmm_t* p = cmGmmAlloc(c, NULL, K, D, gV, uM, sMM, flags );
  2778. if(0)
  2779. {
  2780. cmGmmPrint(p,true);
  2781. for(i=0; i<10; i++)
  2782. for(j=0; j<20; j+=2)
  2783. {
  2784. xM[(i*20)+j] = .1 * i;;
  2785. xM[(i*20)+j + 1] = .1 * (j/2);;
  2786. }
  2787. // octave equivalent
  2788. // x0= .1 * ones(1,10);
  2789. // x = [ 0*x0 1*x0 2*x0 3*x0 4*x0 5*x0 6*x0 7*x0 8*x0 9*x0];
  2790. // x = [x; repmat([0:.1:1],1,10)];
  2791. // y = mvnpdf(x',[.3 .3],[.1 0; 0 .1]); plot(y);
  2792. cmGmmEval(p,xM,M,yV,NULL);
  2793. //cmVOR_PrintL( "xM\n", rpt, D, M, xM );
  2794. cmVOR_PrintL( "yV\n", rpt, 10, 10, yV );
  2795. //printf("y:%f\n",yV[0]);
  2796. cmPlotLineD( NULL, NULL, yV, NULL, M, NULL, kSolidPlotLineId );
  2797. }
  2798. if(0)
  2799. {
  2800. cmReal_t yM[ D*M ];
  2801. cmReal_t yMt[ M*D ];
  2802. cmReal_t uMt[ p->K*D];
  2803. unsigned iterCnt = 10;
  2804. //srand( time(NULL) );
  2805. cmGmmGenerate( p, yM, M );
  2806. p->uflags = 0; // turn off diagonal condition
  2807. if( cmGmmTrain3( p, yM, M, &iterCnt ) != cmOkRC )
  2808. return;
  2809. cmCtxPrint(c,"iterCnt:%i\n",iterCnt);
  2810. cmGmmPrint( p, true );
  2811. cmVOR_Transpose(yMt, yM, D, M );
  2812. //cmVOR_PrintL("yMt\n",vReportFunc,M,D,yMt);
  2813. cmPlotLineD(NULL, yMt, yMt+M, NULL, M, "blue", kAsteriskPlotPtId );
  2814. cmVOR_Transpose( uMt, p->uM, D, p->K);
  2815. cmVOR_PrintL("uMt:\n", p->obj.err.rpt, p->K, p->D, uMt );
  2816. cmPlotLineD(NULL, uMt, uMt+p->K, NULL, p->D, "red", kXPlotPtId );
  2817. }
  2818. if(1)
  2819. {
  2820. cmGmmFree(&p);
  2821. cmReal_t cV0[] = { .7, .3 };
  2822. cmReal_t uM0[] = { .2, .1, .1, .2 };
  2823. cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  2824. unsigned flags = 0;
  2825. K = 2;
  2826. D = 2;
  2827. cmGmm_t* p = cmGmmAlloc(c,NULL, K, D, cV0, uM0, sMM0, flags );
  2828. xM[0] = 0.117228;
  2829. xM[1] = 0.110079;
  2830. cmGmmEval(p,xM,1,yV,NULL);
  2831. cmCtxPrint(c,"y: %f\n",yV[0]);
  2832. }
  2833. cmPlotDraw();
  2834. cmGmmFree(&p);
  2835. cmCtxFree(&c);
  2836. }
  2837. //------------------------------------------------------------------------------------------------------------
  2838. cmChmm_t* cmChmmAlloc( cmCtx* c, cmChmm_t* ap, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
  2839. {
  2840. cmChmm_t* p = cmObjAlloc(cmChmm_t,c,ap);
  2841. if( stateN >0 && dimN > 0 )
  2842. if( cmChmmInit(p,stateN,mixN,dimN,iV,aM) != cmOkRC )
  2843. cmChmmFree(&p);
  2844. return p;
  2845. }
  2846. cmRC_t cmChmmFree( cmChmm_t** pp )
  2847. {
  2848. cmRC_t rc = cmOkRC;
  2849. cmChmm_t* p = *pp;
  2850. if( pp == NULL || *pp == NULL )
  2851. return cmOkRC;
  2852. if((rc = cmChmmFinal(p)) != cmOkRC )
  2853. return rc;
  2854. cmMemPtrFree(&p->iV);
  2855. cmMemPtrFree(&p->aM);
  2856. cmMemPtrFree(&p->bV);
  2857. cmMemPtrFree(&p->bM);
  2858. cmObjFree(pp);
  2859. return cmOkRC;
  2860. }
  2861. cmRC_t cmChmmInit( cmChmm_t* p, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
  2862. {
  2863. cmRC_t rc;
  2864. unsigned i;
  2865. if((rc = cmChmmFinal(p)) != cmOkRC )
  2866. return rc;
  2867. // iV[] aM
  2868. /*
  2869. unsigned n = stateN + (stateN*stateN);
  2870. p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t );
  2871. p->aM = p->iV + stateN;
  2872. */
  2873. //cmArrayResizeVZ(c,&p->memA, cmReal_t, &p->iV,stateN, &p->aM, stateN*stateN, NULL );
  2874. p->iV = cmMemResizeZ( cmReal_t, p->iV, stateN );
  2875. p->aM = cmMemResizeZ( cmReal_t, p->aM, stateN * stateN );
  2876. p->bV = cmMemResizeZ( cmGmm_t*, p->bV, stateN );
  2877. p->N = stateN;
  2878. p->K = mixN;
  2879. p->D = dimN;
  2880. if( iV != NULL )
  2881. cmVOR_Copy(p->iV,p->N,iV);
  2882. if( aM != NULL )
  2883. cmVOR_Copy(p->aM,p->N*p->N,aM);
  2884. for(i=0; i<p->N; ++i)
  2885. p->bV[i] = cmGmmAlloc( p->obj.ctx, NULL, p->K, p->D, NULL, NULL, NULL, 0 );
  2886. //p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"chmm");
  2887. return cmOkRC;
  2888. }
  2889. cmRC_t cmChmmFinal( cmChmm_t* p )
  2890. {
  2891. if( p != NULL )
  2892. {
  2893. unsigned i;
  2894. for(i=0; i<p->N; ++i)
  2895. cmGmmFree( &p->bV[i] );
  2896. cmMemPtrFree(&p->bM);
  2897. //if( p->mfp != NULL )
  2898. // cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);
  2899. }
  2900. return cmOkRC;
  2901. }
  2902. cmRC_t cmChmmRandomize( cmChmm_t* p, const cmReal_t* oM, unsigned T )
  2903. {
  2904. cmRC_t rc;
  2905. unsigned i;
  2906. unsigned N = p->N;
  2907. // randomize the initial state probabilities
  2908. cmVOR_Random( p->iV, N, 0.0, 1.0 );
  2909. cmVOR_NormalizeProbability( p->iV, N );
  2910. // randomize the state transition matrix
  2911. cmVOR_Random( p->aM, N*N, 0.0, 1.0 );
  2912. for(i=0; i<N; ++i)
  2913. {
  2914. cmVOR_NormalizeProbabilityN( p->aM + i, N, N ); // rows of aM must sum to 1.0
  2915. if((rc = cmGmmRandomize( p->bV[i], oM, T )) != cmOkRC) // randomize the GMM assoc'd with state i
  2916. return rc;
  2917. }
  2918. cmMemPtrFree(&p->bM); // force bM[] to be recalculated
  2919. return cmOkRC;
  2920. }
  2921. cmReal_t _cmChmmKmeansDist( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
  2922. { return cmVOR_EuclidDistance(v0,v1,vn); }
  2923. cmRC_t cmChmmSegKMeans( cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t threshProb, unsigned maxIterCnt, unsigned iterCnt )
  2924. {
  2925. cmCtx* c = p->obj.ctx;
  2926. cmRC_t rc = cmOkRC;
  2927. unsigned i,j,k,t;
  2928. unsigned N = p->N;
  2929. unsigned K = p->K;
  2930. unsigned D = p->D;
  2931. //unsigned qV[T];
  2932. //cmReal_t alphaM[N*T];
  2933. //unsigned clusterIdxV[T];
  2934. //cmReal_t centroidM[D*N];
  2935. /*
  2936. unsigned sz = 2*ALIGN_B(T,unsigned) +
  2937. ALIGN_B(N*T,cmReal_t) +
  2938. ALIGN_B(D*N,cmReal_t);
  2939. cmArray mem;
  2940. cmArrayAlloc(c, &mem);
  2941. unsigned* qV = (unsigned*) cmArrayResize(c, &mem, sz, char);
  2942. cmReal_t* alphaM = (cmReal_t*) (qV + ALIGN_T(T, unsigned));
  2943. unsigned* clusterIdxV = (unsigned*) (alphaM + ALIGN_T(N*T,cmReal_t));
  2944. cmReal_t* centroidM = (cmReal_t*) (clusterIdxV + ALIGN_T(T, unsigned));
  2945. */
  2946. unsigned* qV = cmMemAlloc( unsigned, T );
  2947. cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T);
  2948. unsigned* clusterIdxV = cmMemAlloc( unsigned, T );
  2949. cmReal_t* centroidM = cmMemAlloc( cmReal_t, D*N);
  2950. cmReal_t logPr = 0;
  2951. bool reportFl = true;
  2952. cmChmmRandomize(p,oM,T);
  2953. // cluster the observations into N groups
  2954. cmVOR_Kmeans( qV, centroidM, N, oM, D, T, NULL, 0, false, _cmChmmKmeansDist, NULL );
  2955. for(i=0; i<maxIterCnt; ++i)
  2956. {
  2957. unsigned jnV[N];
  2958. if( reportFl )
  2959. cmCtxPrint(c,"SegKM: ----------------------------------------------------%i\n",i);
  2960. // get the count of data points in each state
  2961. cmVOU_Fill(jnV,N,0);
  2962. for(t=0; t<T; ++t)
  2963. ++jnV[ qV[t] ];
  2964. // for each state
  2965. for(j=0; j<N; ++j)
  2966. {
  2967. cmGmm_t* g = p->bV[j];
  2968. // cluster all datapoints which were assigned to state j
  2969. cmVOR_Kmeans( clusterIdxV, g->uM, K, oM, D, T, qV, j, false, _cmChmmKmeansDist, NULL );
  2970. // for each cluster
  2971. for(k=0; k<K; ++k)
  2972. {
  2973. unsigned kN = 0;
  2974. // kN is count of data points assigned to cluster k
  2975. for(t=0; t<T; ++t)
  2976. if( clusterIdxV[t] == k )
  2977. ++kN;
  2978. g->gV[k] = (cmReal_t)kN/jnV[j];
  2979. // the covar of the kth component is the sample covar of cluster k
  2980. cmVOR_GaussCovariance(g->sMM + (k*D*D), D, oM, T, g->uM + (k*D), clusterIdxV, k );
  2981. }
  2982. if((rc = _cmGmmUpdateCovar(g, g->sMM )) != cmOkRC )
  2983. goto errLabel;
  2984. }
  2985. if( i== 0 )
  2986. {
  2987. // count transitions from i to j
  2988. for(t=0; t<T-1; ++t)
  2989. p->aM[ (qV[t+1]*N) + qV[t] ] += 1.0;
  2990. for(j=0; j<N; ++j)
  2991. {
  2992. // normalize state transitions by dividing by times in each state
  2993. for(k=0; k<N; k++)
  2994. p->aM[ (k*N) + j ] /= jnV[j];
  2995. cmVOR_NormalizeProbabilityN(p->aM + j, N, N);
  2996. cmGmmEval( p->bV[j], oM, 1, p->iV + j, NULL );
  2997. }
  2998. }
  2999. if((rc = cmChmmTrain(p, oM, T, iterCnt,0,0 )) != cmOkRC )
  3000. goto errLabel;
  3001. // calculate the prob. that the new model generated the data
  3002. cmReal_t logPr0 = cmChmmForward(p,oM,T,alphaM,NULL);
  3003. cmReal_t dLogPr = logPr0 - logPr;
  3004. if( reportFl )
  3005. cmCtxPrint(c,"pr:%f d:%f\n",logPr0,dLogPr);
  3006. if( (dLogPr > 0) && (dLogPr < threshProb) )
  3007. break;
  3008. logPr = logPr0;
  3009. // fill qV[] with the state at each time t
  3010. cmChmmDecode(p,oM,T,qV);
  3011. }
  3012. errLabel:
  3013. cmMemPtrFree(&qV);
  3014. cmMemPtrFree(&alphaM);
  3015. cmMemPtrFree(&clusterIdxV);
  3016. cmMemPtrFree(&centroidM);
  3017. return rc;
  3018. }
  3019. cmRC_t cmChmmSetGmm( cmChmm_t* p, unsigned i, const cmReal_t* wV, const cmReal_t* uM, const cmReal_t* sMM, unsigned flags )
  3020. {
  3021. assert( i < p->N);
  3022. cmMemPtrFree(&p->bM); // force bM[] to be recalculated
  3023. return cmGmmInit(p->bV[i],p->K,p->D,wV,uM,sMM,flags);
  3024. }
  3025. // Return the probability of the observation for each state.
  3026. // oV[D] - multi-dim. observation data point
  3027. // pV[N] - probability of this observation for each state
  3028. void cmChmmObsProb( const cmChmm_t* p, const cmReal_t* oV, cmReal_t* prV )
  3029. {
  3030. unsigned i;
  3031. for(i=0; i<p->N; ++i)
  3032. cmGmmEval( p->bV[i], oV, 1, prV + i, NULL );
  3033. }
  3034. // oM[D,T] - observation matrix
  3035. // alphaM[N,T] - prob of being in each state and observtin oM(:,t)
  3036. // bM[N,T] - (optional) state-observation probability matrix
  3037. // logPrV[T] - (optional) record the log prob of the data given the model at each time step
  3038. // Returns sum(logPrV[T])
  3039. cmReal_t cmChmmForward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* alphaM, cmReal_t* logPrV )
  3040. {
  3041. unsigned t;
  3042. cmReal_t logPr = 0;
  3043. // calc the prob of starting in each state
  3044. if( p->bM == NULL )
  3045. cmChmmObsProb( p, oM, alphaM );
  3046. else
  3047. cmVOR_Copy( alphaM, p->N*T, p->bM );
  3048. cmVOR_MultVV( alphaM, p->N, p->iV );
  3049. cmReal_t s = cmVOR_Sum(alphaM,p->N);
  3050. cmVOR_DivVS(alphaM,p->N,s);
  3051. //cmVOR_PrintL("alpha:\n",p->obj.err.rpt,p->N,1,alphaM);
  3052. for(t=1; t<T; ++t)
  3053. {
  3054. cmReal_t tmp[p->N];
  3055. cmReal_t* alphaV = alphaM + t*p->N;
  3056. // calc the prob of the observation for each state
  3057. if( p->bM == NULL )
  3058. cmChmmObsProb(p,oM + (t*p->D), alphaV );
  3059. // calc. the prob. of transitioning to each state at time t, from each state at t-1
  3060. cmVOR_MultVVM(tmp,p->N, alphaM + ((t-1)*p->N), p->N, p->aM );
  3061. // calc the joint prob of transitioning from each state to each state and observing O(t).
  3062. cmVOR_MultVV(alphaV, p->N, tmp );
  3063. // scale the probabilities to prevent underflow
  3064. s = cmVOR_Sum(alphaV,p->N);
  3065. cmVOR_DivVS(alphaV,p->N,s);
  3066. // track the log prob. of the model having generated the data up to time t.
  3067. cmReal_t pr = log(s);
  3068. if( logPrV != NULL )
  3069. logPrV[t] = pr;
  3070. logPr += pr;
  3071. }
  3072. return logPr;
  3073. }
  3074. // oM[D,T]
  3075. // betaM[N,T]
  3076. void cmChmmBackward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* betaM )
  3077. {
  3078. cmVOR_Fill(betaM,p->N*T,1.0);
  3079. assert(T >= 2 );
  3080. int t = (int)T - 2;
  3081. for(; t>=0; --t)
  3082. {
  3083. cmReal_t tmp[p->N];
  3084. if( p->bM == NULL )
  3085. cmChmmObsProb(p,oM+((t+1)*p->D), tmp );
  3086. else
  3087. cmVOR_Copy(tmp,p->N,p->bM + ((t+1)*p->N));
  3088. cmVOR_MultVV(tmp,p->N,betaM + ((t+1)*p->N));
  3089. cmVOR_MultVMV(betaM+(t*p->N),p->N, p->aM, p->N, tmp );
  3090. cmVOR_NormalizeProbability(betaM+(t*p->N),p->N );
  3091. }
  3092. }
  3093. cmReal_t cmChmmCompare( const cmChmm_t* p0, const cmChmm_t* p1, unsigned T )
  3094. {
  3095. assert(p0->D == p1->D);
  3096. assert(p0->N == p1->N);
  3097. cmReal_t oM[p0->D*T];
  3098. cmReal_t alphaM[p0->N*T];
  3099. cmChmmGenerate(p0,oM,T,NULL);
  3100. cmReal_t logPr00 = cmChmmForward(p0,oM,T,alphaM,NULL);
  3101. cmReal_t logPr01 = cmChmmForward(p1,oM,T,alphaM,NULL);
  3102. cmChmmGenerate(p1,oM,T,NULL);
  3103. cmReal_t logPr10 = cmChmmForward(p0,oM,T,alphaM,NULL);
  3104. cmReal_t logPr11 = cmChmmForward(p1,oM,T,alphaM,NULL);
  3105. cmReal_t d0 = (logPr01-logPr00)/T;
  3106. cmReal_t d1 = (logPr10-logPr11)/T;
  3107. return (d0+d1)/2;
  3108. }
  3109. cmRC_t cmChmmGenerate( const cmChmm_t* p, cmReal_t* oM, unsigned T, unsigned* sV )
  3110. {
  3111. unsigned i,si;
  3112. // use weighted random selection to choose an intitial state
  3113. cmVOR_WeightedRandInt(&si, 1, p->iV, p->N );
  3114. for(i=0; i<T; ++i)
  3115. {
  3116. if( sV != NULL )
  3117. sV[i] = si;
  3118. // generate a random value using the GMM assoc'd with the current state
  3119. cmGmmGenerate( p->bV[si], oM + (i*p->D), 1 );
  3120. // choose the next state using the transition weights from the current state
  3121. cmVOR_WeightedRandInt(&si, 1, p->aM + (si*p->N), p->N );
  3122. }
  3123. return cmOkRC;
  3124. }
  3125. cmRC_t cmChmmDecode( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned* yV )
  3126. {
  3127. int i,j,t;
  3128. unsigned N = p->N;
  3129. //unsigned psiM[N*T];
  3130. //cmReal_t delta[N];
  3131. /*
  3132. unsigned sz = ALIGN_B(N*T,unsigned) + ALIGN_B(N,cmReal_t);
  3133. cmArray mem;
  3134. cmArrayAlloc(c, &mem);
  3135. unsigned* psiM = (unsigned*) cmArrayResize(c, &mem, sz, char);
  3136. cmReal_t* delta = (cmReal_t*) (psiM + ALIGN_T(N*T,unsigned));
  3137. */
  3138. unsigned* psiM = cmMemAlloc( unsigned, N*T );
  3139. cmReal_t* delta= cmMemAlloc( cmReal_t, N );
  3140. // get the prob of starting in each state
  3141. if( p->bM == NULL )
  3142. cmChmmObsProb( p, oM, delta );
  3143. else
  3144. cmVOR_Copy( delta, N, p->bM );
  3145. cmVOR_MultVV( delta, p->N, p->iV );
  3146. cmVOR_NormalizeProbability(delta, p->N);
  3147. for(t=1; t<T; ++t)
  3148. {
  3149. cmReal_t mV[N];
  3150. const cmReal_t* ap = p->aM;
  3151. // calc. the most likely new state given the most likely prev state
  3152. // and the transition matrix
  3153. for(i=0; i<N; ++i)
  3154. {
  3155. const cmReal_t* dp = delta;
  3156. unsigned psiIdx = t*N + i;
  3157. mV[i] = *ap++ * *dp++;
  3158. psiM[ psiIdx ] = 0;
  3159. // find max value of: delta .* A(:,i)
  3160. for(j=1; j<N; ++j )
  3161. {
  3162. cmReal_t v = *ap++ * *dp++;
  3163. if( v > mV[i] )
  3164. {
  3165. mV[i] = v;
  3166. psiM[ psiIdx ] = j;
  3167. }
  3168. }
  3169. }
  3170. // mV[] now holds the prob. of the max likelihood state at time t
  3171. // for each possible state at t-1
  3172. // psiM[:,t] holds the index of the max likelihood state
  3173. // condition the most likely new state on the observations
  3174. if( p->bM == NULL )
  3175. cmChmmObsProb(p,oM + (t*p->D), delta);
  3176. else
  3177. cmVOR_Copy(delta, N, p->bM + (t*p->N) );
  3178. cmVOR_MultVV(delta, N, mV ); // condition it on the max. like current states
  3179. cmVOR_NormalizeProbability( delta, N ); // normalize the prob.
  3180. }
  3181. // unwind psiM[] to form the max. likelihood state sequence
  3182. yV[T-1] = cmVOR_MaxIndex(delta,N,1);
  3183. for(t=T-2; t>=0; --t)
  3184. yV[t] = psiM[ ((t+1)*N) + yV[t+1] ];
  3185. cmMemPtrFree(&psiM);
  3186. cmMemPtrFree(&delta);
  3187. return cmOkRC;
  3188. }
  3189. cmRC_t cmChmmTrain( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned iterCnt, cmReal_t thresh, unsigned flags )
  3190. {
  3191. cmRC_t rc = cmOkRC;
  3192. unsigned i,j,k,t,d;
  3193. unsigned iter;
  3194. unsigned N = p->N;
  3195. unsigned K = p->K;
  3196. unsigned D = p->D;
  3197. unsigned De2 = D * D;
  3198. bool mixFl = !cmIsFlag(flags,kNoTrainMixCoeffChmmFl);
  3199. bool meanFl = !cmIsFlag(flags,kNoTrainMeanChmmFl);
  3200. bool covarFl = !cmIsFlag(flags,kNoTrainCovarChmmFl);
  3201. bool bFl = mixFl | meanFl | covarFl;
  3202. bool calcBFl = true;
  3203. bool progFl = false;
  3204. bool timeProgFl = false;
  3205. cmReal_t progInc = 0.1;
  3206. cmReal_t progFrac = 0;
  3207. cmReal_t logProb = 0;
  3208. //cmReal_t alphaM[N*T]; // alpha[N,T]
  3209. //cmReal_t betaM[N*T]; // betaM[N,T]
  3210. //cmReal_t logPrV[T];
  3211. //cmReal_t EpsM[N*N];
  3212. //cmReal_t BK[N*K*T];
  3213. //cmReal_t gamma_jk[N*K];
  3214. //cmReal_t uM[K*D*N];
  3215. //cmReal_t sMM[K*De2*N];
  3216. /*
  3217. unsigned sz = ALIGN_T(N*T, cmReal_t) +
  3218. ALIGN_T(N*T, cmReal_t) +
  3219. ALIGN_T(T, cmReal_t) +
  3220. ALIGN_T(N*N, cmReal_t) +
  3221. ALIGN_T(N*K*T, cmReal_t) +
  3222. ALIGN_T(N*K, cmReal_t) +
  3223. ALIGN_T(K*D*N, cmReal_t) +
  3224. ALIGN_T(K*De2*N,cmReal_t);
  3225. cmArray mem;
  3226. cmArrayAlloc(c, &mem);
  3227. cmReal_t* alphaM = cmArrayResize(c, &mem, sz, cmReal_t); // alpha[N,T]
  3228. cmReal_t* betaM = alphaM + ALIGN_T(N*T, cmReal_t); // betaM[N,T]
  3229. cmReal_t* logPrV = betaM + ALIGN_T(N*T, cmReal_t);
  3230. cmReal_t* EpsM = logPrV + ALIGN_T(T, cmReal_t);
  3231. cmReal_t* BK = EpsM + ALIGN_T(N*N, cmReal_t);
  3232. cmReal_t* gamma_jk = BK + ALIGN_T(N*K*T,cmReal_t);
  3233. cmReal_t* uM = gamma_jk + ALIGN_T(N*K, cmReal_t);
  3234. cmReal_t* sMM = uM + ALIGN_T(K*D*N,cmReal_t);
  3235. */
  3236. cmReal_t* alphaM = cmMemAlloc( cmReal_t, N*T );
  3237. cmReal_t* betaM = cmMemAlloc( cmReal_t, N*T );
  3238. cmReal_t* logPrV = cmMemAlloc( cmReal_t, T );
  3239. cmReal_t* EpsM = cmMemAlloc( cmReal_t, N*N );
  3240. cmReal_t* BK = cmMemAlloc( cmReal_t, N*K*T );
  3241. cmReal_t* gamma_jk = cmMemAlloc( cmReal_t, N*K );
  3242. cmReal_t* uM = cmMemAlloc( cmReal_t, K*D*N );
  3243. cmReal_t* sMM = cmMemAlloc( cmReal_t, K*De2*N );
  3244. if( thresh <=0 )
  3245. thresh = 0.0001;
  3246. //cmArrayResizeZ(c,&p->memC,N*T,cmReal_t);
  3247. p->bM = cmMemResizeZ( cmReal_t, p->bM, N*T);
  3248. for(iter=0; iter<iterCnt; ++iter)
  3249. {
  3250. // zero the mean and covar summation arrays
  3251. cmVOR_Fill(uM, K*D *N,0);
  3252. cmVOR_Fill(sMM, K*De2*N,0);
  3253. cmVOR_Fill(EpsM,N*N, 0);
  3254. cmVOR_Fill(gamma_jk,N*K,0);
  3255. //
  3256. // B[i,t] The prob that state i generated oM(:,t)
  3257. // BK[i,k,t] The prob that state i component k generated oM(:,t)
  3258. // Note: B[i,t] = sum(BK(i,k,:))
  3259. //
  3260. if( calcBFl || bFl )
  3261. {
  3262. calcBFl = false;
  3263. for(t=0; t<T; ++t)
  3264. {
  3265. // prob. that state i generated objservation O[t]
  3266. for(i=0; i<N; ++i )
  3267. cmGmmEval( p->bV[i], oM + (t*D), 1, p->bM + (t*N) + i, BK + (t*N*K) + (i*K) );
  3268. }
  3269. }
  3270. // alpha[N,T] is prob. of transitioning forward to each state given the observed data
  3271. cmReal_t logProb0 = cmChmmForward( p, oM, T, alphaM, logPrV );
  3272. // check for convergence
  3273. cmReal_t dLogProb = labs(logProb0-logProb) / ((labs(logProb0)+labs(logProb)+cmReal_EPSILON)/2);
  3274. if( dLogProb < thresh )
  3275. break;
  3276. logProb = logProb0;
  3277. // betaM[N,T] is prob of transitioning backward from each state given the observed data
  3278. cmChmmBackward(p, oM, T, betaM );
  3279. if(progFl)
  3280. cmCtxPrint(p->obj.ctx,"%i (%f) ",iter+1, dLogProb );
  3281. if(timeProgFl)
  3282. progFrac = progInc;
  3283. // for each time step
  3284. for(t=0; t<T-1; ++t)
  3285. {
  3286. // oV[D] is the observation at step t
  3287. const cmReal_t* oV = oM + (t*D);
  3288. //
  3289. // Update EpsM[N,N] (6.37)
  3290. // (prob. of being in state i at time t and transitioning
  3291. // to state j at time t+1)
  3292. //
  3293. cmReal_t E[N*N];
  3294. // for each possible state transition
  3295. for(i=0; i<N; ++i)
  3296. for(j=0; j<N; ++j)
  3297. {
  3298. E[ i + (j*N) ]
  3299. = exp(log(alphaM[ (t*N) + i ])
  3300. + log(p->aM[ i + (j*N) ])
  3301. + log(p->bM[ ((t+1)*N) + j ])
  3302. + log(betaM[ ((t+1)*N) + j ]));
  3303. }
  3304. cmVOR_NormalizeProbability( E, N*N );
  3305. cmVOR_AddVV( EpsM, N*N, E );
  3306. // If t==0 then update the initial state prob's
  3307. if( t == 0 )
  3308. {
  3309. for(i=0; i<N; ++i)
  3310. p->iV[i] = cmVOR_SumN(EpsM+i, N, N);
  3311. assert( cmVOR_IsNormal(p->iV,N) );
  3312. }
  3313. if( bFl )
  3314. {
  3315. //
  3316. // Calculate gamma_jk[]
  3317. //
  3318. cmReal_t gtjk[N*K]; // gamma_jk[N,K] at time t
  3319. cmReal_t abV[N]; //
  3320. // (alphaM[j,t] * betaM[j:t]) / (sum(alphaM[:,t] * betaM[:,t]))
  3321. cmVOR_MultVVV(abV,N,alphaM + t*N, betaM+t*N);
  3322. cmReal_t abSum = cmVOR_Sum(abV,N);
  3323. if( abSum<=0 )
  3324. assert(abSum>0);
  3325. cmVOR_DivVS(abV,N,abSum);
  3326. for(j=0; j<N; ++j)
  3327. {
  3328. cmReal_t bkSum = cmVOR_Sum(BK + (t*N*K) + (j*K), K );
  3329. for(k=0; k<K; ++k)
  3330. gtjk[ (k*N)+j ] = abV[j] * (BK[ (t*N*K) + (j*K) + k ] / bkSum);
  3331. }
  3332. // sum gtjk[N,K] into gamma_jk (integrate gamma over time)
  3333. cmVOR_AddVV( gamma_jk, N*K, gtjk );
  3334. // update the mean and covar numerators
  3335. for(j=0; j<N; ++j)
  3336. {
  3337. cmReal_t* uV = uM + (j*D*K);
  3338. cmReal_t* sV = sMM + (j*De2*K);
  3339. for(k=0; k<K; ++k,uV+=D,sV+=De2)
  3340. {
  3341. cmReal_t c = gtjk[ (k*N)+j ];
  3342. if( covarFl )
  3343. {
  3344. cmReal_t dV[D];
  3345. cmReal_t dM[D*D];
  3346. // covar numerator b[j].sM[k]
  3347. cmVOR_SubVVV(dV, D, oV, p->bV[j]->uM + (k*D));
  3348. cmVOR_MultMMM( dM, D, D, dV, dV, 1 );
  3349. cmVOR_MultVS( dM, De2, c );
  3350. cmVOR_AddVV( sV, De2, dM );
  3351. }
  3352. if( meanFl )
  3353. {
  3354. // mean numerator b[j].uM[k]
  3355. for(d=0; d<D; ++d)
  3356. uV[d] += c * oV[ d ];
  3357. }
  3358. }
  3359. }
  3360. }
  3361. if( timeProgFl && (t >= floor(T*progFrac)) )
  3362. {
  3363. cmCtxPrint(p->obj.ctx,"%i ", (unsigned)round(progFrac*100) );
  3364. progFrac+=progInc;
  3365. }
  3366. } // end time loop
  3367. for(i=0; i<N; ++i)
  3368. {
  3369. // update the state transition matrix
  3370. cmReal_t den = cmVOR_SumN(EpsM + i, N, N );
  3371. assert(den != 0 );
  3372. for(j=0; j<N; ++j)
  3373. p->aM[ i + (j*N) ] = EpsM[ i + (j*N) ] / den;
  3374. if( bFl )
  3375. {
  3376. // update the mean, covariance and mix coefficient
  3377. cmGmm_t* g = p->bV[i];
  3378. const cmReal_t* uV = uM + (i*D*K);
  3379. const cmReal_t* sMV = sMM + (i*De2*K);
  3380. for(k=0; k<K; ++k,uV+=D,sMV+=De2)
  3381. {
  3382. cmReal_t gjk = gamma_jk[ (k*N) + i ];
  3383. if( meanFl )
  3384. cmVOR_DivVVS(g->uM + (k*D), D, uV, gjk );
  3385. if( covarFl )
  3386. cmVOR_DivVVS(g->sMM + (k*De2), De2, sMV, gjk );
  3387. if( mixFl )
  3388. g->gV[k] = gjk / cmVOR_SumN( gamma_jk + i, K, N );
  3389. }
  3390. if((rc = _cmGmmUpdateCovar(g,g->sMM)) != cmOkRC )
  3391. goto errLabel;
  3392. }
  3393. }
  3394. assert( cmVOR_IsNormalZ(p->aM,N*N) );
  3395. if( timeProgFl )
  3396. cmCtxPrint(p->obj.ctx,"\n");
  3397. } // end iter loop
  3398. if( progFl)
  3399. cmCtxPrint(p->obj.ctx,"\n");
  3400. if( p->mfp != NULL )
  3401. {
  3402. // first line is iV[N]
  3403. cmMtxFileRealExec(p->mfp,p->iV,p->N);
  3404. // next N lines are aM[N,N]
  3405. for(i=0; i<p->N; ++i)
  3406. cmMtxFileRealExecN(p->mfp,p->aM + i,p->N,p->N);
  3407. // next T lines are bM[T,N]
  3408. if( p->bM != NULL )
  3409. for(i=0; i<T; ++i)
  3410. cmMtxFileRealExec(p->mfp, p->bM + (i*p->N),p->N);
  3411. }
  3412. errLabel:
  3413. cmMemPtrFree(&alphaM);
  3414. cmMemPtrFree(&betaM);
  3415. cmMemPtrFree(&logPrV);
  3416. cmMemPtrFree(&EpsM);
  3417. cmMemPtrFree(&BK);
  3418. cmMemPtrFree(&gamma_jk);
  3419. cmMemPtrFree(&uM);
  3420. cmMemPtrFree(&sMM);
  3421. return rc;
  3422. }
  3423. void cmChmmPrint( cmChmm_t* p )
  3424. {
  3425. unsigned i;
  3426. cmCtxPrint(p->obj.ctx,"======================================== \n");
  3427. cmVOR_PrintL("iV: ", p->obj.err.rpt, 1, p->N, p->iV);
  3428. cmVOR_PrintL("aM:\n", p->obj.err.rpt, p->N, p->N, p->aM);
  3429. for(i=0; i<p->N; ++i)
  3430. {
  3431. cmCtxPrint(p->obj.ctx,"bV[%i] ----------------- %i \n",i,i);
  3432. cmGmmPrint(p->bV[i],false);
  3433. }
  3434. }
  3435. void cmChmmTestForward( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3436. {
  3437. cmReal_t oM[] = {
  3438. 0.117228, 0.110079,
  3439. 0.154646, 0.210436,
  3440. 0.947468, 0.558136,
  3441. 0.202023, 0.138123,
  3442. 0.929933, 0.456102,
  3443. 0.897566, 0.685078,
  3444. 0.945177, 0.663145,
  3445. 0.272399, 0.055107,
  3446. 0.863386, 0.621546,
  3447. 0.217545, 0.274709,
  3448. 0.838777, 0.650038,
  3449. 0.134966, 0.159472,
  3450. 0.053990, 0.264051,
  3451. 0.884269, 0.550019,
  3452. 0.764787, 0.554484,
  3453. 0.114771, 0.077518,
  3454. 0.835121, 0.606137,
  3455. 0.070733, 0.120015,
  3456. 0.819814, 0.588482,
  3457. 0.105511, 0.197699,
  3458. 0.824778, 0.533047,
  3459. 0.945223, 0.511411,
  3460. 0.126971, 0.050083,
  3461. 0.869497, 0.567737,
  3462. 0.144866, 0.197363,
  3463. 0.985726, 0.590402,
  3464. 0.181094, 0.192827,
  3465. 0.162179, 0.155297,
  3466. 1.034691, 0.513413,
  3467. 0.220708, 0.036158,
  3468. 0.750061, 0.671224,
  3469. 0.246971, 0.093246,
  3470. 0.997567, 0.680491,
  3471. 0.916887, 0.530981,
  3472. 0.022328, 0.121969,
  3473. 0.794031, 0.618081,
  3474. 0.845066, 0.625512,
  3475. 0.174731, 0.094773,
  3476. 0.968665, 0.652435,
  3477. 0.932484, 0.388081,
  3478. 0.202732, 0.148710,
  3479. 0.911307, 0.637139,
  3480. 0.211127, 0.201362,
  3481. 0.138152, 0.057290,
  3482. 0.819132, 0.579888,
  3483. 0.135625, 0.176140,
  3484. 0.146017, 0.157853,
  3485. 0.950319, 0.624150,
  3486. 0.285064, 0.038825,
  3487. 0.716844, 0.575189,
  3488. 0.907433, 0.504946,
  3489. 0.219772, 0.129993,
  3490. 0.076507, 0.193079,
  3491. 0.808906, 0.548409,
  3492. 0.880892, 0.523950,
  3493. 0.758099, 0.636729,
  3494. 1.014017, 0.557120,
  3495. 0.277888, 0.181492,
  3496. 0.877588, 0.508634,
  3497. 0.251266, 0.225890,
  3498. 0.990904, 0.482949,
  3499. 0.999899, 0.534579,
  3500. 0.904179, 0.707349,
  3501. 0.952879, 0.617955,
  3502. 0.172068, 0.151984,
  3503. 1.026262, 0.662600,
  3504. 0.812003, 0.430856,
  3505. 0.173393, 0.017885,
  3506. 0.099370, 0.146661,
  3507. 0.785785, 0.564333,
  3508. 0.698222, 0.449299,
  3509. 0.276539, 0.225314,
  3510. 0.799271, 0.618159,
  3511. 0.098813, 0.090839,
  3512. 0.883666, 0.554150,
  3513. 0.274934, 0.185403,
  3514. 0.200419, 0.109972,
  3515. 0.925076, 0.608610,
  3516. 0.864486, 0.348689,
  3517. 0.176733, 0.136235,
  3518. 0.967278, 0.656875,
  3519. 0.986994, 0.659877,
  3520. 1.015618, 0.596549,
  3521. 0.689903, 0.528107,
  3522. 0.978238, 0.630989,
  3523. 0.269847, 0.144358,
  3524. 0.092303, 0.139894,
  3525. 0.168185, 0.095327,
  3526. 0.897767, 0.584203,
  3527. 0.068316, 0.018452,
  3528. 0.953395, 0.530545,
  3529. 0.266405, 0.173987,
  3530. 0.233845, 0.205276,
  3531. 0.900060, 0.477108,
  3532. 0.052909, 0.053077,
  3533. 0.885850, 0.496546,
  3534. 0.268494, 0.104785,
  3535. 1.041405, 0.655079,
  3536. 1.055915, 0.697988,
  3537. 0.181569, 0.146840
  3538. };
  3539. unsigned i;
  3540. cmReal_t iV[] = { .5 , .5};
  3541. cmReal_t A[] = { .3, .6, .7, .4 };
  3542. cmReal_t cV0[] = { .7, .3 };
  3543. cmReal_t uM0[] = { .2, .1, .1, .2 };
  3544. cmReal_t sMM0[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
  3545. cmReal_t cV1[] = { .2, .8 };
  3546. cmReal_t uM1[] = { .8, .9, .9, .5 };
  3547. cmReal_t sMM1[]= { .01, 0, 0, .01, .01, 0, 0, .01 };
  3548. unsigned T = 100;
  3549. unsigned N = 2;
  3550. unsigned K = 2;
  3551. unsigned D = 2;
  3552. cmReal_t alphaM[N*T];
  3553. cmReal_t betaM[N*T];
  3554. cmReal_t logPrV[T];
  3555. unsigned qV[T];
  3556. unsigned sV[T];
  3557. cmReal_t oMt[T*D];
  3558. // scale covariance
  3559. cmVOR_MultVS(sMM0,D*D*K,1);
  3560. cmVOR_MultVS(sMM1,D*D*K,1);
  3561. cmCtx c;
  3562. cmCtxAlloc(&c,rpt,lhH,stH);
  3563. cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
  3564. cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  3565. cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
  3566. cmChmmPrint(p);
  3567. cmChmmGenerate(p, oM, T, sV );
  3568. cmChmmForward( p, oM, T, alphaM,logPrV );
  3569. //cmVOR_PrintL("logPrV:\n",rpt,1,T,logPrV);
  3570. cmCtxPrint(&c,"log prob:%f\n", cmVOR_Sum(logPrV,T));
  3571. cmChmmBackward( p, oM, T, betaM );
  3572. //cmVOR_PrintL("beta:\n",rpt,N,T,betaM);
  3573. cmChmmDecode(p,oM,T,qV);
  3574. cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  3575. cmVOU_PrintL("qV:\n",rpt,1,T,qV);
  3576. unsigned d=0;
  3577. for(i=0; i<T; ++i)
  3578. d += sV[i] != qV[i];
  3579. cmCtxPrint(&c,"Diff:%i\n",d);
  3580. cmPlotSetup("Chmm Forward Test",1,1);
  3581. cmVOR_Transpose(oMt,oM,D,T);
  3582. cmPlotLineD(NULL, oMt, oMt+T, NULL, T, "blue", kXPlotPtId );
  3583. cmPlotDraw();
  3584. cmChmmFree(&p);
  3585. }
  3586. void cmChmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3587. {
  3588. time_t t = time(NULL); //0x4b9e82aa; //time(NULL);
  3589. srand( t );
  3590. printf("TIME: 0x%x\n",(unsigned)t);
  3591. //cmChmmTestForward(vReportFunc);
  3592. //return;
  3593. unsigned i;
  3594. cmReal_t iV[] = { 1.0/3.0, 1.0/3.0, 1.0/3.0 };
  3595. cmReal_t A[] = { .1, .4, .7, .4, .2, .2 };
  3596. cmReal_t cV0[] = { .7, .3 };
  3597. cmReal_t uM0[] = { .2, .1, .1, .2 };
  3598. cmReal_t sMM0[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3599. cmReal_t cV1[] = { .2, .8 };
  3600. cmReal_t uM1[] = { .8, .9, .9, .8 };
  3601. cmReal_t sMM1[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3602. cmReal_t cV2[] = { .5, .5 };
  3603. cmReal_t uM2[] = { .5, .5, .5, .5 };
  3604. cmReal_t sMM2[] = { .01, 0, 0, .01, .01, 0, 0, .01 };
  3605. cmReal_t kmThreshProb = 0.001;
  3606. unsigned kmMaxIterCnt = 10;
  3607. unsigned iterCnt = 20;
  3608. unsigned N = sizeof(iV) / sizeof(iV[0]);
  3609. unsigned K = sizeof(cV0) / sizeof(cV0[0]);
  3610. unsigned D = sizeof(uM0) / sizeof(uM0[0]) / K;
  3611. unsigned T = 100;
  3612. cmReal_t alphaM[N*T];
  3613. cmReal_t oM[D*T];
  3614. unsigned sV[T];
  3615. unsigned qV[T];
  3616. cmCtx c;
  3617. cmCtxAlloc(&c,rpt,lhH,stH);
  3618. cmCtxPrint(&c,"N:%i K:%i D:%i\n",N,K,D);
  3619. cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);
  3620. cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  3621. cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
  3622. cmChmmSetGmm(p,2,cV2,uM2,sMM2,0);
  3623. // generate data using the parameters above
  3624. cmChmmGenerate(p,oM,T,sV);
  3625. cmVOU_PrintL("sV: ",rpt,1,T,sV);
  3626. cmChmmRandomize(p,oM,T);
  3627. if(cmChmmSegKMeans(p,oM,T,kmThreshProb,kmMaxIterCnt,iterCnt) != cmOkRC )
  3628. goto errLabel;
  3629. if( cmChmmTrain(p,oM,T,iterCnt,0,0) != cmOkRC )
  3630. goto errLabel;
  3631. //cmChmmPrint(p);
  3632. cmChmmDecode(p,oM,T,qV);
  3633. cmReal_t pr = cmChmmForward(p,oM,T,alphaM,NULL);
  3634. cmCtxPrint(&c,"pr:%f\n",pr);
  3635. cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  3636. cmVOU_PrintL("qV:\n",rpt,1,T,qV);
  3637. unsigned d=0;
  3638. for(i=0; i<T; ++i)
  3639. d += sV[i] != qV[i];
  3640. cmCtxPrint(&c,"Diff:%i\n",d);
  3641. errLabel:
  3642. cmChmmFree(&p);
  3643. }
  3644. //------------------------------------------------------------------------------------------------------------
  3645. cmChord* cmChordAlloc( cmCtx* c, cmChord* ap, const cmReal_t* chromaM, unsigned T )
  3646. {
  3647. unsigned i,j;
  3648. unsigned S = 6;
  3649. unsigned N = 24;
  3650. unsigned D = 12;
  3651. cmChord* p = cmObjAlloc(cmChord,c,ap);
  3652. p->h = cmChmmAlloc( p->obj.ctx, NULL, 0, 0, 0, NULL, NULL );
  3653. if( chromaM != NULL && T > 0 )
  3654. if( cmChordInit(p,chromaM,T) != cmOkRC )
  3655. cmChordFree(&p);
  3656. p->N = N;
  3657. p->D = D;
  3658. p->S = kTonalSpaceDimCnt;
  3659. /*
  3660. // iv[N] aM[N*N] uM[D*N] sMM[D*D*N] phiM[D*S] tsxxxV[S]
  3661. unsigned n = ALIGN_T(N, cmReal_t) +
  3662. ALIGN_T(N*N, cmReal_t) +
  3663. ALIGN_T(D*N, cmReal_t) +
  3664. ALIGN_T(D*D*N,cmReal_t) +
  3665. ALIGN_T(D*S, cmReal_t) +
  3666. 2*ALIGN_T(S, cmReal_t);
  3667. p->iV = cmArrayResizeZ(c, &p->memA, n, cmReal_t);
  3668. p->aM = p->iV + ALIGN_T(N, cmReal_t);
  3669. p->uM = p->aM + ALIGN_T(N*N, cmReal_t);
  3670. p->sMM = p->uM + ALIGN_T(D*N, cmReal_t);
  3671. p->phiM = p->sMM + ALIGN_T(D*D*N,cmReal_t);
  3672. p->tsMeanV = p->phiM + ALIGN_T(D*S, cmReal_t);
  3673. p->tsVarV = p->tsMeanV + ALIGN_T(S, cmReal_t);
  3674. */
  3675. p->iV = cmMemAllocZ( cmReal_t, N );
  3676. p->aM = cmMemAllocZ( cmReal_t, N*N);
  3677. p->uM = cmMemAllocZ( cmReal_t, D*N);
  3678. p->sMM = cmMemAllocZ( cmReal_t, D*D*N );
  3679. p->phiM = cmMemAllocZ( cmReal_t, D*S);
  3680. p->tsMeanV = cmMemAllocZ( cmReal_t, S );
  3681. p->tsVarV = cmMemAllocZ( cmReal_t, S );
  3682. // initialize iV[N] (HMM initial state probabilities)
  3683. cmVOR_Fill(p->iV,N,1.0/N);
  3684. // initialize aM[N,N] (HMM transition matrix)
  3685. cmReal_t epsilon = 0.01;
  3686. 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 };
  3687. for(i=0; i<N; ++i)
  3688. {
  3689. cmVOR_Copy( p->aM+(i*N), N, CMaj2any );
  3690. cmVOR_Rotate( CMaj2any, N, 1 );
  3691. }
  3692. cmVOR_AddVS(p->aM, N*N, epsilon);
  3693. cmVOR_DivVS(p->aM, N*N, ( (N/2)*(N/2) ) + (N*epsilon) );
  3694. //cmVOR_PrintL("A:\n",p->obj.err.rpt,N,N,A);
  3695. // initialize sMM[D*D,N] (HMM covariance matrices)
  3696. 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 };
  3697. 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 };
  3698. cmReal_t Maj[D*D];
  3699. cmReal_t Min[D*D];
  3700. cmVOR_DiagZ(Maj,D,diagMV);
  3701. Maj[ (4*D) + 0 ] = 0.6; Maj[ (0*D) + 4 ] = 0.6;
  3702. Maj[ (7*D) + 0 ] = 0.8; Maj[ (0*D) + 7 ] = 0.8;
  3703. Maj[ (7*D) + 4 ] = 0.8; Maj[ (4*D) + 7 ] = 0.8;
  3704. cmVOR_DiagZ(Min,D,diagmV);
  3705. Min[ (3*D) + 0 ] = 0.6; Min[ (0*D) + 3 ] = 0.6;
  3706. Min[ (7*D) + 0 ] = 0.8; Min[ (0*D) + 7 ] = 0.8;
  3707. Min[ (7*D) + 3 ] = 0.8; Min[ (3*D) + 7 ] = 0.8;
  3708. cmReal_t* sM = p->sMM;
  3709. for(i=0; i<N/2; ++i,sM+=D*D)
  3710. cmVOR_RotateM( sM, D, D, Maj, i, i );
  3711. for(i=0; i<N/2; ++i,sM+=D*D)
  3712. cmVOR_RotateM( sM, D, D, Min, i, i );
  3713. /*
  3714. cmVOR_PrintL("Maj:\n",p->obj.err.rpt,D,D,Maj);
  3715. cmVOR_PrintL("Min:\n",p->obj.err.rpt,D,D,Min);
  3716. for(i=0; i<N; ++i)
  3717. {
  3718. cmCtxPrint(c,"%i----\n",i);
  3719. cmVOR_PrintL("sM:\n",p->obj.err.rpt,D,D,sMM + (i*D*D));
  3720. }
  3721. */
  3722. // initialize uM[D,N] (HMM GMM mean vectors)
  3723. cmVOR_Fill(p->uM,D*N,0);
  3724. for(i=0; i<D; ++i)
  3725. {
  3726. unsigned dom = (i+7) % D;
  3727. unsigned medM = (i+4) % D;
  3728. unsigned medm = (i+3) % D;
  3729. p->uM[ (i * D) + i ] = 1;
  3730. p->uM[ (i * D) + medM ] = 1;
  3731. p->uM[ (i * D) + dom ] = 1;
  3732. p->uM[ ((i+D) * D) + i ] = 1;
  3733. p->uM[ ((i+D) * D) + medm ] = 1;
  3734. p->uM[ ((i+D) * D) + dom ] = 1;
  3735. }
  3736. cmVOR_AddVS(p->uM,D*N,0.01);
  3737. for(i=0; i<N; ++i)
  3738. cmVOR_NormalizeProbability( p->uM + (i*D), D);
  3739. // initialize phiM[D,S]
  3740. cmReal_t phi[D*S];
  3741. for(i=0,j=0; i<D; ++i,++j)
  3742. phi[j] = sin( M_PI*7.0*i/6.0 );
  3743. for(i=0; i<D; ++i,++j)
  3744. phi[j] = cos( M_PI*7.0*i/6.0 );
  3745. for(i=0; i<D; ++i,++j)
  3746. phi[j] = sin( M_PI*3.0*i/2.0 );
  3747. for(i=0; i<D; ++i,++j)
  3748. phi[j] = cos( M_PI*3.0*i/2.0 );
  3749. for(i=0; i<D; ++i,++j)
  3750. phi[j] = 0.5 * sin( M_PI*2.0*i/3.0 );
  3751. for(i=0; i<D; ++i,++j)
  3752. phi[j] = 0.5 * cos( M_PI*2.0*i/3.0 );
  3753. cmVOR_Transpose(p->phiM,phi,D,S);
  3754. return p;
  3755. }
  3756. cmRC_t cmChordFree( cmChord** pp )
  3757. {
  3758. cmRC_t rc = cmOkRC;
  3759. cmChord* p = *pp;
  3760. if( pp == NULL || *pp == NULL )
  3761. return cmOkRC;
  3762. if((rc = cmChordFinal(p)) != cmOkRC )
  3763. return rc;
  3764. cmChmmFree( &p->h );
  3765. cmMemPtrFree(&p->iV);
  3766. cmMemPtrFree(&p->aM);
  3767. cmMemPtrFree(&p->uM);
  3768. cmMemPtrFree(&p->sMM);
  3769. cmMemPtrFree(&p->phiM);
  3770. cmMemPtrFree(&p->tsMeanV);
  3771. cmMemPtrFree(&p->tsVarV);
  3772. cmMemPtrFree(&p->chromaM);
  3773. cmMemPtrFree(&p->tsM);
  3774. cmMemPtrFree(&p->cdtsV);
  3775. cmObjFree(pp);
  3776. return rc;
  3777. }
  3778. cmRC_t cmChordInit( cmChord* p, const cmReal_t* chromaM, unsigned T )
  3779. {
  3780. cmRC_t rc = cmOkRC;
  3781. unsigned i;
  3782. unsigned N = p->N; // count of states
  3783. unsigned K = 1; // count of components per mixture
  3784. unsigned D = p->D; // dimensionality of the observation vector
  3785. unsigned S = p->S; //
  3786. cmReal_t alpha = 6.63261; // alpha norm coeff
  3787. if((rc = cmChordFinal(p)) != cmOkRC )
  3788. return rc;
  3789. // Create the hidden markov model
  3790. cmChmmInit( p->h, N, K, D, p->iV, p->aM);
  3791. // load the GMM's for each markov state
  3792. cmReal_t mixCoeff = 1.0;
  3793. bool diagFl = false;
  3794. for(i=0; i<N; ++i)
  3795. if((rc = cmChmmSetGmm(p->h, i, &mixCoeff, p->uM + (i*D), p->sMM+(i*D*D), diagFl )) != cmOkRC )
  3796. return rc;
  3797. // Allocate memory
  3798. // chromaM[D,T] tsM[S,T] cdtsV[T]
  3799. /*
  3800. unsigned n = ALIGN_T(D*T,cmReal_t) + ALIGN_T(S*T,cmReal_t) + ALIGN_T(T,cmReal_t);
  3801. p->chromaM = cmArrayResizeZ(c, &p->memB, n, cmReal_t);
  3802. p->tsM = p->chromaM + ALIGN_T(D*T,cmReal_t);
  3803. p->cdtsV = p->tsM + ALIGN_T(S*T,cmReal_t);
  3804. p->T = T;
  3805. */
  3806. p->chromaM = cmMemResizeZ( cmReal_t, p->chromaM, p->D*T );
  3807. p->tsM = cmMemResizeZ( cmReal_t, p->tsM, p->S*T );
  3808. p->cdtsV = cmMemResizeZ( cmReal_t, p->cdtsV, p->D*T );
  3809. p->T = T;
  3810. // Allocate local memory
  3811. // qV[], triadIntV[] triadSeqV[] tsNormsV[]
  3812. /*
  3813. n = 2*ALIGN_B(T,unsigned) + ALIGN_B(T,int) + ALIGN_B(T,cmReal_t);
  3814. cmArray mem;
  3815. cmArrayAlloc(c, &mem);
  3816. unsigned* qV = (unsigned*) cmArrayResize(c, &mem, n, char);
  3817. unsigned* triadSeqV = (unsigned*) (qV + ALIGN_T(T,unsigned));
  3818. int* triadIntV = (int*) (triadSeqV + ALIGN_T(T,unsigned));
  3819. cmReal_t* tsNormsV = (cmReal_t*) (triadIntV + ALIGN_T(T,int));
  3820. */
  3821. //unsigned qV[T];
  3822. //unsigned triadSeqV[T];
  3823. //int triadIntV[T];
  3824. //cmReal_t tsNormsV[T];
  3825. unsigned* qV = cmMemAlloc( unsigned, T );
  3826. unsigned* triadSeqV = cmMemAlloc( unsigned, T );
  3827. int* triadIntV = cmMemAlloc( int, T );
  3828. cmReal_t* tsNormsV = cmMemAlloc( cmReal_t, T );
  3829. // Take the alpha norm of chroma and store the result in p->chromaM[]
  3830. for(i=0; i<T; ++i)
  3831. p->chromaM[i] = cmVOR_AlphaNorm( chromaM + (i*D), D, alpha);
  3832. cmVOR_DivVVS(p->chromaM,D*T,chromaM, cmVOR_AlphaNorm(p->chromaM,T,alpha));
  3833. // Train the HMM iniital state prob. p->h->iV[] and transition matrix p->h->aM[]
  3834. unsigned flags = kNoTrainMixCoeffChmmFl | kNoTrainMeanChmmFl | kNoTrainCovarChmmFl;
  3835. unsigned iterCnt = 40;
  3836. if( chromaM != NULL && T > 0 )
  3837. if((rc = cmChmmTrain(p->h, p->chromaM, p->T, iterCnt, 0, flags )) != cmOkRC )
  3838. goto errLabel;
  3839. // Find the most likely chords using a Viterbi decoding of the chroma.
  3840. cmChmmDecode(p->h,p->chromaM,T,qV);
  3841. // Reorder the chord sequence cmcording to circle of fifths
  3842. 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 };
  3843. for(i=0; i<T; ++i)
  3844. qV[i] = map[ qV[i] ];
  3845. //cmVOU_PrintL("qV:\n",p->obj.err.rpt,1,T,qV);
  3846. // Smooth the chord sequence with a median filter.
  3847. cmVOU_MedianFilt(qV,T,3,triadSeqV,1);
  3848. //cmVOU_PrintL("triadSeqV:\n",p->obj.err.rpt,1,T,triadSeqV);
  3849. // Calculate the chord change distance on the circle of fifths.
  3850. int d = 0;
  3851. for(i=0; i<T; ++i)
  3852. {
  3853. int v = abs(d);
  3854. assert(v<N);
  3855. v = (v<=11) ? v : -(12-(v-12));
  3856. if( i > 0 )
  3857. triadIntV[i-1] = (d < 0 ? -1 : 1) * v;
  3858. if( i + 1 < T)
  3859. d = triadSeqV[i+1] - triadSeqV[i];
  3860. }
  3861. // Project chroma into a 6D tonal space.
  3862. cmVOR_MultMMM( p->tsM,S,T,p->phiM,p->chromaM,D);
  3863. // Find the norm of p->tsM[6,T]
  3864. cmVOR_Fill(tsNormsV,T,0);
  3865. for(i=0; i<T; ++i)
  3866. tsNormsV[i] = cmVOR_MultSumVV( p->tsM + (i*S), p->tsM + (i*S), S );
  3867. cmVOR_PowVS(tsNormsV,T,0.5);
  3868. // Take the cosine distance.
  3869. p->cdtsV[0] = 1;
  3870. for(i=1; i<T; ++i)
  3871. p->cdtsV[i] = cmVOR_MultSumVV( p->tsM + ((i-1)*S), p->tsM + (i*S), S );
  3872. for(i=0; i<T-1; ++i)
  3873. p->cdtsV[i+1] /= tsNormsV[i] * tsNormsV[i+1];
  3874. //cmVOR_PrintL("tsNormsV:\n",p->obj.err.rpt,1,T,tsNormsV);
  3875. //cmVOR_PrintL("CDTS:\n", p->obj.err.rpt,1,T,p->cdtsV);
  3876. p->triadSeqMode = cmVOU_Mode(triadSeqV,T);
  3877. p->triadSeqVar = cmVOU_Variance(triadSeqV,T,NULL);
  3878. p->triadIntMean = cmVOI_Mean(triadIntV,T);
  3879. p->triadIntVar = cmVOI_Variance(triadIntV,T,&p->triadIntMean);
  3880. cmVOR_MeanM( p->tsMeanV, p->tsM, S, T, 1 );
  3881. cmVOR_VarianceM( p->tsVarV, p->tsM, S, T, p->tsMeanV, 1);
  3882. p->cdtsMean = cmVOR_Mean( p->cdtsV, T );
  3883. p->cdtsVar = cmVOR_Variance( p->cdtsV, T, &p->cdtsMean );
  3884. /*
  3885. cmReal_t tsMt[T*S];
  3886. cmKbRecd kb;
  3887. cmPlotInitialize(NULL);
  3888. cmPlotSetup("Chords",1,1);
  3889. cmCtxPrint(c,"%s\n","press any key");
  3890. //cmPlotLineD( NULL, NULL, p->cdtsV, NULL, T, NULL, kSolidPlotLineId );
  3891. cmVOR_Transpose(tsMt,p->tsM,S,T);
  3892. cmPlotLineMD(NULL, tsMt, NULL, T, S, kSolidPlotLineId);
  3893. cmPlotDraw();
  3894. cmKeyPress(&kb);
  3895. cmPlotFinalize();
  3896. */
  3897. errLabel:
  3898. cmMemPtrFree(&qV);
  3899. cmMemPtrFree(&triadSeqV);
  3900. cmMemPtrFree(&triadIntV);
  3901. cmMemPtrFree(&tsNormsV);
  3902. return rc;
  3903. }
  3904. cmRC_t cmChordFinal( cmChord* p )
  3905. { return cmOkRC; }
  3906. void cmChordTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
  3907. {
  3908. cmCtx c;
  3909. cmCtxAlloc(&c,rpt,lhH,stH);
  3910. cmChord* p = cmChordAlloc(&c,NULL,NULL,0);
  3911. cmChordFree(&p);
  3912. }