libcm is a C development framework with an emphasis on audio signal processing applications.
Du kannst nicht mehr als 25 Themen auswählen Themen müssen mit entweder einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.

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. }