diff --git a/CMakeLists.txt b/CMakeLists.txt
index f7942870da7ecf50291e955e4aba9d23b02597e1..9767a413e82b6dff4ba2d4ec64e18f18e94ed81c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -359,3 +359,14 @@ target_link_libraries(demo_TPGMMProduct01
${GLFW_LIB}
${ARMADILLO_LIBRARIES}
)
+
+add_executable(demo_TPGMR01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_TPGMR01.cpp
+ )
+target_link_libraries(demo_TPGMR01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+ )
\ No newline at end of file
diff --git a/data/data_tpgmr_frames.txt b/data/data_tpgmr_frames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cfab7a6acd784b50924a512a9ba75b4448eb38ac
--- /dev/null
+++ b/data/data_tpgmr_frames.txt
@@ -0,0 +1,6 @@
+ARMA_MAT_TXT_FN008
+4 10
+ 1.66483521461487e-01 2.16262057423592e-01 1.66483521461487e-01 6.82893320918083e-02 1.66483521461487e-01 8.84615406394005e-02 1.66483521461487e-01 2.68131881952286e-01 1.66483521461487e-01 1.46153852343559e-01
+ 4.58745867013931e-01 1.20309405028820e-01 4.58745867013931e-01 8.72561037540436e-02 4.58745867013931e-01 2.02970296144485e-01 4.58745867013931e-01 2.15346530079842e-01 4.58745867013931e-01 1.33663371205330e-01
+ 0.00000000000000e+00 -3.75198520487174e-04 0.00000000000000e+00 -2.24803457967937e-03 0.00000000000000e+00 1.85198104009032e-03 0.00000000000000e+00 2.19112867489457e-03 0.00000000000000e+00 -1.08944403473288e-03
+ 2.47524753212929e-02 -9.23957489430904e-03 2.47524753212929e-02 -8.34755878895521e-03 2.47524753212929e-02 -1.21426973491907e-02 2.47524753212929e-02 -9.19921137392521e-03 2.47524753212929e-02 -1.49771561846137e-02
diff --git a/data/data_tpgmr_trajectories.txt b/data/data_tpgmr_trajectories.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cbf512a44c991ee2c2338b7aaee66b5351539526
--- /dev/null
+++ b/data/data_tpgmr_trajectories.txt
@@ -0,0 +1,12 @@
+ARMA_CUB_TXT_FN008
+2 200 5
+ -1.37362637362637e-03 -1.61936053895853e-03 -1.86509470429068e-03 -2.11082886962284e-03 -2.35656303495499e-03 -2.73206692804683e-03 -3.22353525871114e-03 -3.64321608040201e-03 -3.88895024573417e-03 -4.13468441106632e-03 -4.38041857639848e-03 -4.62615274173063e-03 -4.46877243359655e-03 -4.22303826826440e-03 -3.97730410293224e-03 -3.73156993760009e-03 -3.48583577226793e-03 -3.24010160693578e-03 -2.99436744160362e-03 -2.74863327627147e-03 -2.50289911093931e-03 -2.25716494560716e-03 -2.01143078027500e-03 -1.76569661494285e-03 -1.51996244961069e-03 -1.27422828427854e-03 -1.02849411894638e-03 -7.82759953614225e-04 -5.37025788282070e-04 -2.91291622949914e-04 -4.55574576177593e-05 2.00176707714397e-04 4.45910873046551e-04 6.91645038378706e-04 9.37379203710862e-04 1.18311336904302e-03 1.42884753437517e-03 1.67458169970733e-03 1.92031586503948e-03 2.16605003037164e-03 2.41178419570379e-03 2.65751836103595e-03 2.90325252636810e-03 3.14898669170026e-03 3.39472085703241e-03 3.64045502236457e-03 3.88618918769673e-03 4.13192335302888e-03 4.37765751836104e-03 4.62339168369319e-03 4.86912584902535e-03 5.11486001435750e-03 5.36059417968966e-03 5.60632834502181e-03 5.85206251035397e-03 6.09779667568612e-03 6.34353084101828e-03 6.58926500635043e-03 6.83499917168259e-03 7.29333480589762e-03 7.78480313656193e-03 8.12165221712960e-03 8.36738638246176e-03 8.61312054779391e-03 8.85885471312607e-03 9.10458887845822e-03 9.35032304379038e-03 9.59605720912254e-03 1.00681981335248e-02 1.05596664641891e-02 1.08827102545695e-02 1.11284444199017e-02 1.13741785852339e-02 1.16199127505660e-02 1.18656469158982e-02 1.21113810812303e-02 1.23571152465625e-02 1.26028494118946e-02 1.28485835772268e-02 1.34587774035010e-02 1.41959798994975e-02 1.46846871721244e-02 1.49304213374565e-02 1.52424208956872e-02 1.57338892263515e-02 1.62502070793528e-02 1.74788779060136e-02 1.87075487326744e-02 2.09163951626263e-02 2.33737368159479e-02 2.48647081561654e-02 2.56019106521619e-02 2.62590424650726e-02 2.67505107957369e-02 2.72419791264012e-02 2.77334474570655e-02 2.82249157877299e-02 2.89262245292396e-02 2.96634270252361e-02 3.02487713291733e-02 3.07402396598377e-02 3.11378320172290e-02 3.13835661825612e-02 3.17010878568668e-02 3.24382903528632e-02 3.31754928488597e-02 3.39126953448561e-02 3.46498978408526e-02 3.52214368546027e-02 3.57129051852670e-02 3.60966922524711e-02 3.63424264178033e-02 3.65881605831355e-02 3.68338947484676e-02 3.70796289137998e-02 3.73253630791319e-02 3.75710972444641e-02 3.78168314097962e-02 3.80625655751284e-02 3.84297862941079e-02 3.89212546247722e-02 3.94127229554365e-02 3.99041912861008e-02 4.03956596167651e-02 4.08871279474295e-02 4.13785962780938e-02 4.16767905461373e-02 4.19225247114694e-02 4.21682588768016e-02 4.24139930421337e-02 4.26597272074659e-02 4.29054613727981e-02 4.31511955381302e-02 4.33969297034624e-02 4.36426638687945e-02 4.40954773869347e-02 4.45869457175990e-02 4.52275111822851e-02 4.59647136782815e-02 4.65196863438069e-02 4.67654205091391e-02 4.70111546744713e-02 4.72568888398034e-02 4.75026230051356e-02 4.77483571704677e-02 4.79940913357999e-02 4.82398255011320e-02 4.84855596664642e-02 4.87312938317963e-02 4.89770279971285e-02 4.92227621624607e-02 4.94684963277928e-02 4.97142304931250e-02 4.99599646584571e-02 5.02056988237893e-02 5.06281407035176e-02 5.11196090341819e-02 5.14923518692363e-02 5.17380860345684e-02 5.19838201999006e-02 5.22295543652328e-02 5.24752885305649e-02 5.27210226958971e-02 5.29667568612292e-02 5.34030040311447e-02 5.38944723618090e-02 5.42534099066762e-02 5.44991440720084e-02 5.45957811033188e-02 5.43500469379866e-02 5.41043127726545e-02 5.38585786073223e-02 5.36128444419902e-02 5.37757468661991e-02 5.40214810315313e-02 5.42672151968634e-02 5.45129493621956e-02 5.47586835275277e-02 5.50044176928599e-02 5.52501518581921e-02 5.54958860235242e-02 5.57416201888564e-02 5.55511071842730e-02 5.53053730189409e-02 5.48994974874372e-02 5.44080291567729e-02 5.40187199734938e-02 5.37729858081617e-02 5.35272516428295e-02 5.32815174774974e-02 5.30357833121652e-02 5.27900491468331e-02 5.25443149815009e-02 5.22985808161687e-02 5.20528466508366e-02 5.18071124855045e-02 5.15613783201723e-02 5.13156441548401e-02 5.10699099895080e-02 5.08241758241758e-02
+ -1.93069306930693e-01 -1.82737117932899e-01 -1.72404928935105e-01 -1.65455992835464e-01 -1.59551884836725e-01 -1.55011857969717e-01 -1.51690797220426e-01 -1.47076139774782e-01 -1.39326998026436e-01 -1.31909547738693e-01 -1.30064513989087e-01 -1.28219480239481e-01 -1.14570376635654e-01 -9.83340796391197e-02 -9.13685589664494e-02 -9.09995522165282e-02 -9.01164237026718e-02 -8.82713899530657e-02 -8.65507405011858e-02 -8.58127270013433e-02 -8.50747135015009e-02 -8.46600991757467e-02 -8.42910924258255e-02 -8.39220856759043e-02 -8.35530789259831e-02 -8.31840721760618e-02 -8.28150654261406e-02 -8.23216743784931e-02 -8.12146541287295e-02 -8.01076338789658e-02 -7.86564837388261e-02 -7.71804567391413e-02 -7.59614906214240e-02 -7.48544703716603e-02 -7.35774582483374e-02 -7.21014312486525e-02 -7.08741728444201e-02 -7.05051660944989e-02 -7.01361593445777e-02 -6.97671525946565e-02 -6.93981458447352e-02 -6.84735558983034e-02 -6.73665356485397e-02 -6.66409605784699e-02 -6.62719538285487e-02 -6.56956399157504e-02 -6.45886196659867e-02 -6.35147685622834e-02 -6.31457618123621e-02 -6.27767550624409e-02 -6.24077483125197e-02 -6.20387415625985e-02 -6.14582815065426e-02 -6.07202680067002e-02 -5.99822545068577e-02 -5.92442410070153e-02 -5.85435427964907e-02 -5.81745360465695e-02 -5.78055292966483e-02 -5.74365225467270e-02 -5.70675157968058e-02 -5.64663250244623e-02 -5.57283115246198e-02 -5.51354130387913e-02 -5.47664062888701e-02 -5.43393535333433e-02 -5.36013400335008e-02 -5.28633265336584e-02 -5.24652967809344e-02 -5.20962900310131e-02 -5.17272832810919e-02 -5.13582765311707e-02 -5.09892697812495e-02 -5.06202630313283e-02 -5.02512562814070e-02 -4.98822495314858e-02 -4.95132427815646e-02 -4.91442360316434e-02 -4.87752292817221e-02 -4.84062225318009e-02 -4.80372157818797e-02 -4.76682090319585e-02 -4.72992022820372e-02 -4.69301955321160e-02 -4.65611887821948e-02 -4.61921820322736e-02 -4.58231752823524e-02 -4.54541685324311e-02 -4.50851617825099e-02 -4.47161550325887e-02 -4.41398411197904e-02 -4.34018276199479e-02 -4.27840522745742e-02 -4.24150455246530e-02 -4.20460387747317e-02 -4.16770320248105e-02 -4.13080252748893e-02 -4.09390185249681e-02 -4.05700117750469e-02 -4.02010050251256e-02 -3.98319982752044e-02 -3.94629915252832e-02 -3.90939847753620e-02 -3.87249780254407e-02 -3.83559712755195e-02 -3.79869645255983e-02 -3.76179577756771e-02 -3.72489510257558e-02 -3.68799442758346e-02 -3.65109375259134e-02 -3.61419307759922e-02 -3.57729240260709e-02 -3.54039172761497e-02 -3.50349105262285e-02 -3.46659037763073e-02 -3.42968970263861e-02 -3.39278902764648e-02 -3.35588835265436e-02 -3.31898767766224e-02 -3.28208700267012e-02 -3.24518632767799e-02 -3.18921339370118e-02 -3.07851136872481e-02 -2.96946780105146e-02 -2.93256712605934e-02 -2.89566645106722e-02 -2.85876577607509e-02 -2.82186510108297e-02 -2.76464832412890e-02 -2.69084697414465e-02 -2.62865482528152e-02 -2.59175415028940e-02 -2.55485347529728e-02 -2.51795280030516e-02 -2.48105212531303e-02 -2.44415145032091e-02 -2.40725077532879e-02 -2.34796092674594e-02 -2.27415957676170e-02 -2.21404049952734e-02 -2.17713982453522e-02 -2.14023914954309e-02 -2.10333847455097e-02 -2.06643779955885e-02 -2.02953712456673e-02 -1.99263644957461e-02 -1.95573577458248e-02 -1.91883509959036e-02 -1.85042373584092e-02 -1.73972171086456e-02 -1.62901968588819e-02 -1.51831766091182e-02 -1.40761563593545e-02 -1.33215582864819e-02 -1.25835447866395e-02 -1.10494717813490e-02 -9.20443803174288e-03 -8.07254092243395e-03 -7.70353417251273e-03 -6.87845166426188e-03 -4.66441116473460e-03 -2.46280909497986e-03 -1.35578884521618e-03 -2.48768595452506e-04 2.00258719339273e-03 4.58563444284123e-03 6.17360731047978e-03 6.91162081032224e-03 1.02243892730982e-02 1.94495580211288e-02 2.88737416455211e-02 4.10509643929216e-02 5.32281871403220e-02 5.74282302602120e-02 6.00112775096605e-02 6.14955967958605e-02 6.22336102957029e-02 6.29716237955454e-02 6.37096372953878e-02 6.50405492810588e-02 7.05756505298772e-02 7.61107517786954e-02 8.32835796142428e-02 9.06637146126673e-02 9.37152760502181e-02 9.44532895500605e-02 9.54981176509611e-02 9.69741446506460e-02 9.83174950660895e-02 9.90555085659320e-02 9.97935220657744e-02 1.00531535565617e-01 1.01269549065459e-01 1.02268769590527e-01 1.03375789840291e-01 1.04134534056421e-01 1.04503540806342e-01 1.04959616564672e-01 1.05697630064514e-01 1.06435643564356e-01
+ 2.74725274725275e-04 1.68286487381965e-03 2.31238610635596e-03 2.78176597272075e-03 3.25114583908554e-03 3.72052570545033e-03 4.18990557181512e-03 4.65928543817991e-03 5.12866530454470e-03 5.59804517090949e-03 6.06742503727428e-03 6.10055773372356e-03 5.63117786735877e-03 5.16179800099398e-03 4.69241813462919e-03 3.77574686619913e-03 3.20420785244906e-03 2.73482798608427e-03 2.26544811971948e-03 1.79606825335469e-03 1.32668838698989e-03 8.57308520625103e-04 3.87928654260312e-04 -8.14512121044780e-05 -5.50831078469270e-04 -1.21624606549230e-03 -2.03904136064940e-03 -2.50842122701419e-03 -2.97780109337898e-03 -3.44718095974377e-03 -3.91656082610856e-03 -4.38594069247336e-03 -4.85532055883815e-03 -5.32470042520294e-03 -5.79408029156773e-03 -6.26346015793252e-03 -7.14699872991330e-03 -7.75167044011265e-03 -8.22105030647744e-03 -8.69043017284223e-03 -9.25368601247999e-03 -1.01786404550224e-02 -1.06480203213872e-02 -1.11174001877519e-02 -1.15867800541167e-02 -1.22991330277762e-02 -1.30749903362969e-02 -1.35443702026617e-02 -1.40137500690265e-02 -1.44831299353912e-02 -1.49525098017560e-02 -1.54218896681208e-02 -1.58912695344856e-02 -1.63606494008504e-02 -1.68300292672152e-02 -1.72994091335800e-02 -1.82298856921973e-02 -1.87876194157601e-02 -1.92569992821249e-02 -1.97263791484897e-02 -2.01957590148545e-02 -2.06651388812193e-02 -2.11345187475841e-02 -2.16038986139489e-02 -2.20732784803137e-02 -2.31224805345408e-02 -2.41109393119443e-02 -2.45803191783091e-02 -2.50496990446739e-02 -2.55190789110387e-02 -2.64274670053565e-02 -2.70072891932188e-02 -2.74766690595836e-02 -2.79460489259484e-02 -2.84154287923132e-02 -2.88848086586780e-02 -2.93541885250428e-02 -3.02515323872108e-02 -3.08423988072229e-02 -3.13117786735877e-02 -3.17811585399525e-02 -3.22505384063173e-02 -3.27199182726821e-02 -3.31892981390469e-02 -3.36586780054117e-02 -3.41280578717765e-02 -3.45974377381413e-02 -3.50668176045060e-02 -3.55361974708708e-02 -3.60055773372356e-02 -3.64749572036004e-02 -3.69443370699652e-02 -3.74137169363300e-02 -3.78830968026948e-02 -3.83524766690596e-02 -3.88218565354244e-02 -3.92912364017892e-02 -3.97606162681540e-02 -4.02299961345187e-02 -4.06993760008835e-02 -4.11687558672483e-02 -4.16381357336131e-02 -4.21075155999779e-02 -4.25768954663427e-02 -4.30462753327075e-02 -4.35156551990723e-02 -4.39850350654371e-02 -4.44544149318019e-02 -4.49237947981667e-02 -4.54566789993926e-02 -4.63954387321222e-02 -4.68813849467116e-02 -4.80962504831852e-02 -4.89190457783423e-02 -4.93884256447071e-02 -4.98578055110719e-02 -5.03796454801480e-02 -5.13184052128776e-02 -5.18153956596168e-02 -5.22847755259816e-02 -5.27541553923463e-02 -5.32235352587111e-02 -5.36929151250759e-02 -5.41622949914407e-02 -5.46316748578055e-02 -5.51010547241703e-02 -5.55704345905351e-02 -5.63103981445690e-02 -5.70586448727152e-02 -5.75280247390800e-02 -5.79974046054448e-02 -5.84667844718096e-02 -5.93558451598653e-02 -5.99549947539897e-02 -6.04243746203545e-02 -6.08937544867193e-02 -6.14625324424320e-02 -6.23819647688994e-02 -6.28513446352642e-02 -6.33207245016290e-02 -6.37901043679938e-02 -6.42594842343586e-02 -6.47288641007234e-02 -6.51982439670882e-02 -6.56676238334530e-02 -6.61370036998178e-02 -6.66063835661826e-02 -6.70757634325474e-02 -6.75451432989121e-02 -6.80145231652769e-02 -6.84839030316417e-02 -6.89532828980065e-02 -6.94226627643713e-02 -7.02785907559777e-02 -7.09108730465514e-02 -7.13802529129162e-02 -7.18496327792810e-02 -7.23190126456458e-02 -7.27883925120106e-02 -7.32577723783754e-02 -7.37271522447402e-02 -7.41965321111050e-02 -7.46659119774698e-02 -7.51352918438346e-02 -7.56046717101994e-02 -7.60740515765641e-02 -7.65434314429289e-02 -7.73772709702358e-02 -7.83160307029654e-02 -7.90504721409244e-02 -7.95198520072892e-02 -7.99892318736540e-02 -8.04586117400188e-02 -8.09279916063836e-02 -8.13973714727484e-02 -8.18667513391131e-02 -8.23361312054779e-02 -8.28055110718427e-02 -8.33080236346568e-02 -8.42467833673864e-02 -8.47631012203876e-02 -8.52324810867525e-02 -8.57018609531172e-02 -8.61712408194820e-02 -8.66406206858468e-02 -8.71100005522116e-02 -8.75793804185764e-02 -8.80487602849412e-02 -8.85181401513060e-02 -8.89875200176708e-02 -8.94568998840356e-02 -8.99262797504004e-02 -9.03956596167651e-02 -9.08650394831299e-02 -9.13344193494947e-02 -9.21240819482026e-02 -9.28226296316749e-02 -9.32920094980397e-02 -9.37613893644044e-02 -9.42307692307692e-02
+ -1.88118811881188e-01 -1.86709123173624e-01 -1.74191916679105e-01 -1.67748810056885e-01 -1.66322536776291e-01 -1.62930991591621e-01 -1.54597243643962e-01 -1.51073021875052e-01 -1.48237059886893e-01 -1.42577574340349e-01 -1.39816242930826e-01 -1.33543128182165e-01 -1.23343615768612e-01 -1.19288687662736e-01 -1.16469310247608e-01 -1.14321608040201e-01 -1.12207074978855e-01 -1.10092541917508e-01 -1.08288969600478e-01 -1.06688558303066e-01 -1.04714994112477e-01 -1.04010149758694e-01 -1.03305305404912e-01 -1.02600461051130e-01 -1.01481002371594e-01 -9.91881851501733e-02 -9.63605154485298e-02 -9.56556710947477e-02 -9.49508267409656e-02 -9.23304642021991e-02 -8.95110867870707e-02 -8.78857986301143e-02 -8.71809542763322e-02 -8.63185564787635e-02 -8.49088677711992e-02 -8.34991790636350e-02 -8.08456473788082e-02 -7.95313199661675e-02 -7.80635852529977e-02 -7.62102592168765e-02 -7.45186327677994e-02 -7.16992553526709e-02 -6.88798779375425e-02 -6.78765112692174e-02 -6.57163706320381e-02 -6.32618538235733e-02 -6.16365656666169e-02 -6.09317213128348e-02 -6.02227308157951e-02 -5.88130421082309e-02 -5.62258487155248e-02 -5.48120138647031e-02 -5.41071695109209e-02 -5.31742872779740e-02 -5.18723982951059e-02 -5.11675539413238e-02 -4.97703036635322e-02 -4.83606149559680e-02 -4.74028558634758e-02 -4.63663200490903e-02 -4.47451780353915e-02 -4.28130752773770e-02 -4.21082309235949e-02 -4.14033865698128e-02 -3.56982934474352e-02 -3.16972652039073e-02 -3.02129459177073e-02 -2.93132328308208e-02 -2.79035441232565e-02 -2.64938554156923e-02 -2.57434034860772e-02 -2.50385591322951e-02 -2.43337147785130e-02 -2.33303481101879e-02 -2.20989435626980e-02 -2.13940992089159e-02 -2.06892548551338e-02 -1.93417582964327e-02 -1.79320695888684e-02 -1.69245567772858e-02 -1.62197124235036e-02 -1.55148680697215e-02 -1.47685622833640e-02 -1.33588735757998e-02 -1.19491848682356e-02 -1.05394961606714e-02 -9.51539877605851e-03 -8.81055442227639e-03 -8.10571006849429e-03 -7.37598885516691e-03 -5.96630014760270e-03 -5.16609449889712e-03 -4.46125014511502e-03 -3.01839229149045e-03 -1.40139642104913e-03 -4.39491185299433e-04 1.66674958953183e-03 3.07643829709604e-03 3.89322851883179e-03 5.07073320397366e-03 6.12799973464683e-03 7.52939615569600e-03 1.01248818349172e-02 1.15345705424814e-02 1.22477071827786e-02 1.35288654493590e-02 1.44824783985936e-02 1.55231603562366e-02 1.71484485131931e-02 1.90722589846924e-02 1.97771033384746e-02 2.25218501749672e-02 2.42217689105594e-02 2.56314576181236e-02 2.67218932948571e-02 2.80237822777253e-02 3.07643829709604e-02 3.28789160323068e-02 3.43300661724464e-02 3.62828996467486e-02 3.79745260958257e-02 3.96868832611905e-02 4.14365557158731e-02 4.21414000696552e-02 4.28462444234373e-02 4.41978871253960e-02 4.50810156392524e-02 4.61921820322736e-02 4.73157868550674e-02 4.90157055906596e-02 5.37215781879696e-02 5.51312668955338e-02 5.71711693782444e-02 5.82657511982354e-02 5.93603330182264e-02 6.13090203492711e-02 6.38298754498566e-02 6.52105411546180e-02 6.59153855084001e-02 6.78474882664146e-02 6.89752392324660e-02 7.04263893726056e-02 7.20350929565318e-02 7.27399373103140e-02 7.34447816640961e-02 7.41496260178782e-02 7.60485596298323e-02 7.72094797419440e-02 7.79143240957262e-02 7.90918287808681e-02 8.09741778197920e-02 8.16790221735742e-02 8.37852629484054e-02 8.53193359536959e-02 8.62688027596730e-02 8.73136308605735e-02 8.85035739754880e-02 8.92084183292701e-02 8.99132626830522e-02 9.06181070368343e-02 9.13229513906165e-02 9.20277957443986e-02 9.27326400981807e-02 9.36406454715824e-02 9.50503341791466e-02 9.64600228867108e-02 9.72021825298108e-02 9.79070268835929e-02 9.86118712373750e-02 9.93167155911571e-02 1.00208136391529e-01 1.01551486806972e-01 1.02256331160754e-01 1.02961175514536e-01 1.03666019868318e-01 1.04781332404597e-01 1.06481251140189e-01 1.08595784201536e-01 1.10760070981973e-01 1.13579448397101e-01 1.15764465893826e-01 1.17364877191237e-01 1.19562333117734e-01 1.21834419622867e-01 1.23090701029902e-01 1.23861883675805e-01 1.25976416737151e-01 1.29326500489245e-01 1.32353185067250e-01 1.35549861518815e-01 1.38045839759855e-01 1.38750684113638e-01 1.39488697613480e-01 1.41603230674826e-01 1.43717763736173e-01 1.45832296797519e-01 1.47586115395459e-01 1.49476756720898e-01 1.51935419672621e-01 1.52640264026403e-01
+ -2.74725274725275e-04 2.60919984538075e-04 7.96565243801425e-04 1.07819316362030e-03 1.34601579325197e-03 1.61383842288365e-03 1.88166105251532e-03 2.14948368214700e-03 2.41730631177867e-03 2.89773041029322e-03 3.43337566955657e-03 3.77022475012425e-03 4.03804737975592e-03 4.30587000938760e-03 4.57369263901927e-03 4.84151526865095e-03 5.10933789828262e-03 5.53454083604837e-03 6.07018609531172e-03 6.46225633662820e-03 6.73007896625987e-03 6.73836214037219e-03 6.47053951074051e-03 6.20271688110884e-03 5.93489425147717e-03 5.66707162184549e-03 5.39924899221382e-03 5.13142636258214e-03 4.86360373295047e-03 4.59578110331879e-03 4.32795847368712e-03 3.99939256723176e-03 3.46374730796841e-03 2.97504003534154e-03 2.70721740570987e-03 2.43939477607819e-03 2.17157214644652e-03 1.88442211055276e-03 1.34877685128941e-03 8.18653708100944e-04 5.50831078469268e-04 2.83008448837594e-04 1.51858192059204e-05 -2.52636810425756e-04 -5.20459440057430e-04 -7.88282069689105e-04 -1.05610469932078e-03 -1.32392732895246e-03 -1.80987354354189e-03 -2.34551880280523e-03 -2.67684576729803e-03 -2.94466839692970e-03 -3.21249102656138e-03 -3.48031365619305e-03 -3.74813628582473e-03 -4.01595891545640e-03 -4.44668396929703e-03 -4.98232922856038e-03 -5.36887735380198e-03 -5.63669998343365e-03 -5.90452261306533e-03 -6.17234524269700e-03 -6.56165442597603e-03 -7.09729968523939e-03 -7.52526368104258e-03 -7.79308631067425e-03 -8.15478491357888e-03 -8.69043017284223e-03 -9.38621679827710e-03 -1.04575073168038e-02 -1.13300016566348e-02 -1.15978242862665e-02 -1.18656469158982e-02 -1.21334695455298e-02 -1.24786018002098e-02 -1.32820696891049e-02 -1.40358385333260e-02 -1.43036611629576e-02 -1.45935722568888e-02 -1.53970401457839e-02 -1.62005080346789e-02 -1.64738527803854e-02 -1.67416754100171e-02 -1.70094980396488e-02 -1.72773206692805e-02 -1.75451432989121e-02 -1.78129659285438e-02 -1.83044342592081e-02 -1.88400795184715e-02 -1.95855651885803e-02 -2.03890330774753e-02 -2.09964658457121e-02 -2.15321111049754e-02 -2.26144458556519e-02 -2.39535590038103e-02 -2.49558230714010e-02 -2.57592909602960e-02 -2.64081395990944e-02 -2.69437848583577e-02 -2.73386161577116e-02 -2.76064387873433e-02 -2.82552874261417e-02 -2.93265779446684e-02 -3.22091225357557e-02 -3.75655751283892e-02 -4.51087856866751e-02 -5.63573361312055e-02 -6.42677674084709e-02 -6.50712352973660e-02 -6.60182782042078e-02 -6.73573913523662e-02 -6.86385222817384e-02 -6.97098128002651e-02 -7.06927494615937e-02 -7.12283947208570e-02 -7.17944116185322e-02 -7.25978795074272e-02 -7.33682146998730e-02 -7.36360373295047e-02 -7.39038599591363e-02 -7.41716825887680e-02 -7.44395052183997e-02 -7.52208846429952e-02 -7.60243525318902e-02 -7.73137666353747e-02 -7.86528797835331e-02 -7.93044894803689e-02 -7.98401347396322e-02 -8.01604174719753e-02 -8.04282401016069e-02 -8.08976199679717e-02 -8.14332652272351e-02 -8.17811585399525e-02 -8.20489811695842e-02 -8.23168037992159e-02 -8.25846264288475e-02 -8.28524490584792e-02 -8.31202716881109e-02 -8.33880943177426e-02 -8.36559169473742e-02 -8.39237395770059e-02 -8.41915622066376e-02 -8.44593848362693e-02 -8.47272074659009e-02 -8.49950300955326e-02 -8.52628527251643e-02 -8.55306753547960e-02 -8.57984979844276e-02 -8.61436302391076e-02 -8.66792754983710e-02 -8.71514164227732e-02 -8.74192390524049e-02 -8.76870616820366e-02 -8.79548843116682e-02 -8.82227069412999e-02 -8.84905295709316e-02 -8.87804406648628e-02 -8.93160859241261e-02 -8.98434480092771e-02 -9.01112706389088e-02 -9.03790932685405e-02 -9.01223148710586e-02 -8.98544922414269e-02 -8.95866696117952e-02 -8.93188469821636e-02 -8.90510243525319e-02 -8.87832017229002e-02 -8.85153790932685e-02 -8.82475564636369e-02 -8.79797338340052e-02 -8.77119112043735e-02 -8.74440885747418e-02 -8.71762659451102e-02 -8.69084433154785e-02 -8.66406206858468e-02 -8.63727980562152e-02 -8.61049754265835e-02 -8.58371527969518e-02 -8.55693301673201e-02 -8.53015075376884e-02 -8.50336849080568e-02 -8.47658622784251e-02 -8.44980396487934e-02 -8.42302170191618e-02 -8.39623943895301e-02 -8.38878458225192e-02 -8.41556684521509e-02 -8.42578275995361e-02 -8.39900049699045e-02 -8.37221823402728e-02 -8.34543597106411e-02 -8.31865370810094e-02 -8.29187144513778e-02 -8.26508918217461e-02 -8.23830691921144e-02 -8.21152465624827e-02 -8.18474239328511e-02 -8.15796013032194e-02 -8.13117786735877e-02 -8.10439560439560e-02
+ -1.86468646864686e-01 -1.80838184320945e-01 -1.75207721777203e-01 -1.72628820671012e-01 -1.70215765295122e-01 -1.68524138846045e-01 -1.66915435262119e-01 -1.65986699172430e-01 -1.65182347380467e-01 -1.63739489526842e-01 -1.62130785942916e-01 -1.60820604673533e-01 -1.59614076985588e-01 -1.58685340895899e-01 -1.57880989103936e-01 -1.56562515548037e-01 -1.54953811964111e-01 -1.53817768711545e-01 -1.53013416919581e-01 -1.50699868981873e-01 -1.47080285918039e-01 -1.43655571587309e-01 -1.40438164419457e-01 -1.37569033285238e-01 -1.35155977909349e-01 -1.32589515232930e-01 -1.29774283961059e-01 -1.27622435610395e-01 -1.26818083818432e-01 -1.26013732026469e-01 -1.25209380234506e-01 -1.24405028442543e-01 -1.23600676650580e-01 -1.22796324858617e-01 -1.21991973066653e-01 -1.21137867555600e-01 -1.19931339867655e-01 -1.18782858185316e-01 -1.18380682289334e-01 -1.17970214106838e-01 -1.17165862314875e-01 -1.16361510522912e-01 -1.15946896197157e-01 -1.15544720301176e-01 -1.14773537655273e-01 -1.13969185863310e-01 -1.13164834071347e-01 -1.12360482279384e-01 -1.11556130487421e-01 -1.10751778695457e-01 -1.09947426903494e-01 -1.09143075111531e-01 -1.08624807204339e-01 -1.08222631308357e-01 -1.07555102243893e-01 -1.06750750451930e-01 -1.06191021112161e-01 -1.05788845216180e-01 -1.05386669320198e-01 -1.04984493424217e-01 -1.04175995488996e-01 -1.02969467801051e-01 -1.02127800719770e-01 -1.01725624823789e-01 -1.01323448927807e-01 -1.00921273031826e-01 -1.00519097135844e-01 -1.00116921239863e-01 -9.97147453438811e-02 -9.93125694478996e-02 -9.89103935519180e-02 -9.85082176559365e-02 -9.81060417599549e-02 -9.77038658639733e-02 -9.73016899679918e-02 -9.68995140720102e-02 -9.64973381760287e-02 -9.60951622800471e-02 -9.56929863840655e-02 -9.52908104880840e-02 -9.48886345921024e-02 -9.44864586961209e-02 -9.40842828001393e-02 -9.36821069041577e-02 -9.32799310081762e-02 -9.28777551121946e-02 -9.24755792162131e-02 -9.20734033202315e-02 -9.16712274242500e-02 -9.09539446406952e-02 -9.01495928487321e-02 -8.96396172280545e-02 -8.92374413320729e-02 -8.88352654360913e-02 -8.84330895401098e-02 -8.80309136441282e-02 -8.76287377481467e-02 -8.72265618521651e-02 -8.68243859561836e-02 -8.64222100602020e-02 -8.60200341642204e-02 -8.56178582682389e-02 -8.52156823722573e-02 -8.48135064762758e-02 -8.44113305802942e-02 -8.40091546843127e-02 -8.36069787883311e-02 -8.30762724513657e-02 -8.22719206594026e-02 -8.15753685921356e-02 -8.11731926961540e-02 -8.07710168001725e-02 -8.03688409041909e-02 -7.99666650082094e-02 -7.95644891122278e-02 -7.91623132162462e-02 -7.87601373202647e-02 -7.83579614242831e-02 -7.79557855283016e-02 -7.75536096323200e-02 -7.71514337363385e-02 -7.67492578403569e-02 -7.63470819443753e-02 -7.59449060483938e-02 -7.55427301524122e-02 -7.51405542564307e-02 -7.47383783604491e-02 -7.43362024644676e-02 -7.39340265684860e-02 -7.35318506725044e-02 -7.28270063187223e-02 -7.20226545267592e-02 -7.15002404763089e-02 -7.10980645803274e-02 -7.04346816591207e-02 -6.96303298671576e-02 -6.90664543841319e-02 -6.86642784881503e-02 -6.82621025921688e-02 -6.78599266961872e-02 -6.74577508002056e-02 -6.70555749042241e-02 -6.66533990082425e-02 -6.62512231122610e-02 -6.56914937724928e-02 -6.48871419805297e-02 -6.42196129160655e-02 -6.38174370200839e-02 -6.32991691128912e-02 -6.24948173209281e-02 -6.14997429391180e-02 -5.98910393551918e-02 -5.79838134567225e-02 -5.47664062888701e-02 -5.16028989833657e-02 -4.87876677114948e-02 -4.60056055856842e-02 -4.35925502097948e-02 -4.11670564041329e-02 -3.83518251322619e-02 -3.55365938603910e-02 -3.15397117601208e-02 -2.75179528003052e-02 -2.64814169859197e-02 -2.56770651939566e-02 -2.48727134019935e-02 -2.40683616100303e-02 -2.29323183574639e-02 -2.17257906695192e-02 -1.86534985156807e-02 -1.50339154518467e-02 -1.34459425842082e-02 -1.26415907922450e-02 -1.21067383120222e-02 -1.17045624160406e-02 -1.03073121382490e-02 -8.29643265834122e-03 -6.51359105759823e-03 -4.90488747367199e-03 -2.67426240111447e-03 1.40968870756449e-04 1.83674146309104e-03 2.23891735907259e-03 3.47032190656250e-03 5.88337728245186e-03 1.00378128265088e-02 1.72769789541769e-02 2.33966864023086e-02 2.70162694661426e-02 3.00139310413453e-02 3.12204587292900e-02 3.25099092823855e-02 3.41186128663118e-02 3.56651243013749e-02 3.68716519893195e-02 3.79952568121134e-02 3.83974327080949e-02 3.87996086040765e-02 3.92017845000580e-02 3.96039603960396e-02
+ 8.24175824175824e-04 1.08923739577006e-03 1.35429896736429e-03 1.61936053895853e-03 1.88442211055276e-03 2.14948368214700e-03 2.41454525374123e-03 2.26544811971948e-03 2.00038654812524e-03 1.73532497653101e-03 1.47026340493677e-03 1.20520183334254e-03 9.40140261748302e-04 9.73272958197581e-04 1.23833452979182e-03 1.50339610138605e-03 1.76845767298029e-03 2.03351924457452e-03 2.29858081616876e-03 2.56364238776299e-03 2.82870395935723e-03 3.09376553095146e-03 3.35882710254570e-03 3.67634877685129e-03 4.20647192003976e-03 4.70346236677895e-03 4.96852393837318e-03 5.23358550996742e-03 5.49864708156165e-03 5.76370865315589e-03 6.02877022475012e-03 6.29383179634436e-03 6.55889336793859e-03 6.82395493953283e-03 7.08901651112706e-03 7.35407808272130e-03 7.61913965431553e-03 7.88420122590977e-03 8.14926279750400e-03 8.41432436909824e-03 8.67938594069247e-03 8.94444751228671e-03 9.20950908388094e-03 9.47457065547518e-03 9.73963222706941e-03 1.00046937986636e-02 1.02697553702579e-02 1.05348169418521e-02 1.07998785134464e-02 1.10649400850406e-02 1.13300016566348e-02 1.15950632282291e-02 1.21886907062786e-02 1.43091832790325e-02 1.63192335302888e-02 1.73794798166657e-02 1.84148765807057e-02 1.86799381523000e-02 1.89449997238942e-02 1.97180959743774e-02 2.05132806891601e-02 2.08390855375780e-02 2.11041471091722e-02 2.30921088961290e-02 2.54776630404771e-02 2.68830415815340e-02 2.79432878679110e-02 2.91802418686841e-02 3.05055497266553e-02 3.19882378927605e-02 3.35786073223259e-02 3.53070296537633e-02 3.71624606549230e-02 4.09174995858413e-02 4.70139157325087e-02 5.14205643602628e-02 5.30109337898282e-02 5.42009498039649e-02 5.44660113755591e-02 5.47918162239770e-02 5.53219393671655e-02 5.59348942514772e-02 5.69951405378541e-02 5.80553868242310e-02 5.91156331106080e-02 6.01731183389475e-02 6.09683030537302e-02 6.17634877685129e-02 6.23101772599260e-02 6.28403004031145e-02 6.31412557291954e-02 6.34063173007897e-02 6.38812192832293e-02 6.44113424264178e-02 6.47509525650229e-02 6.50160141366171e-02 6.52810757082114e-02 6.55461372798056e-02 6.61149152355183e-02 6.69100999503009e-02 6.74402230934894e-02 6.77052846650837e-02 6.80835496162129e-02 6.86136727594014e-02 6.92376718758628e-02 7.00328565906455e-02 7.06789441714065e-02 7.09440057430007e-02 7.12090673145949e-02 7.14741288861892e-02 7.17391904577834e-02 7.20042520293776e-02 7.22858799491965e-02 7.28160030923850e-02 7.33461262355735e-02 7.36139488652051e-02 7.38790104367994e-02 7.41440720083936e-02 7.44091335799878e-02 7.48978408526147e-02 7.54279639958032e-02 7.57537688442211e-02 7.60188304158153e-02 7.62838919874096e-02 7.65489535590038e-02 7.68140151305980e-02 7.70790767021923e-02 7.74904743497708e-02 7.80205974929593e-02 7.84237119664255e-02 7.86887735380198e-02 7.89538351096140e-02 7.92188966812082e-02 7.94839582528025e-02 7.97490198243967e-02 8.00831078469269e-02 8.06132309901154e-02 8.10936550886299e-02 8.13587166602242e-02 8.16237782318184e-02 8.18888398034127e-02 8.21539013750069e-02 8.24189629466011e-02 8.26840245181954e-02 8.29490860897896e-02 8.32141476613838e-02 8.39541112154178e-02 8.47492959302004e-02 8.53263570600254e-02 8.58564802032139e-02 8.61878071677067e-02 8.64528687393009e-02 8.67179303108951e-02 8.69829918824894e-02 8.72480534540836e-02 8.75131150256778e-02 8.77781765972721e-02 8.80432381688663e-02 8.83082997404605e-02 8.85733613120548e-02 8.88384228836490e-02 8.91034844552432e-02 8.93685460268375e-02 8.96336075984317e-02 8.98986691700260e-02 9.01637307416202e-02 9.04287923132144e-02 9.06938538848086e-02 9.09589154564029e-02 9.12239770279971e-02 9.14890385995913e-02 9.17541001711856e-02 9.20191617427798e-02 9.22842233143741e-02 9.25492848859683e-02 9.28143464575625e-02 9.30794080291568e-02 9.33444696007510e-02 9.36095311723452e-02 9.38745927439395e-02 9.41396543155337e-02 9.44047158871279e-02 9.46697774587222e-02 9.49348390303164e-02 9.51999006019107e-02 9.54649621735049e-02 9.57300237450991e-02 9.59950853166933e-02 9.62601468882876e-02 9.65252084598818e-02 9.67902700314761e-02 9.70553316030703e-02 9.73203931746645e-02 9.75854547462588e-02 9.78505163178530e-02 9.81155778894472e-02 9.83806394610415e-02 9.86457010326357e-02 9.89107626042299e-02 9.91758241758242e-02
+ -1.91419141914191e-01 -1.78682189827023e-01 -1.65945237739854e-01 -1.64647494900244e-01 -1.64249465147520e-01 -1.57391744199546e-01 -1.49431149145065e-01 -1.39604789624691e-01 -1.29256016053867e-01 -1.25955686020863e-01 -1.25557656268139e-01 -1.22630479128315e-01 -1.18252151848351e-01 -1.10739340265685e-01 -1.00788596447584e-01 -9.49300960246778e-02 -9.33379770137818e-02 -9.17458580028857e-02 -9.01537389919896e-02 -8.84247972535947e-02 -8.64346484899746e-02 -8.45522994510506e-02 -8.29601804401546e-02 -8.16043915949384e-02 -8.12063618422144e-02 -8.07585783703999e-02 -7.99625188649518e-02 -7.91871900757915e-02 -7.87891603230675e-02 -7.83911305703435e-02 -7.79931008176195e-02 -7.75950710648954e-02 -7.71970413121714e-02 -7.67990115594474e-02 -7.64009818067234e-02 -7.60029520539994e-02 -7.56049223012754e-02 -7.52068925485513e-02 -7.48088627958273e-02 -7.44108330431033e-02 -7.40128032903793e-02 -7.36147735376553e-02 -7.32167437849313e-02 -7.28187140322072e-02 -7.24206842794832e-02 -7.20226545267592e-02 -7.16246247740352e-02 -7.12265950213112e-02 -7.08285652685872e-02 -7.04305355158631e-02 -7.00325057631391e-02 -6.96344760104151e-02 -6.92364462576911e-02 -6.88384165049671e-02 -6.83989253196676e-02 -6.76028658142196e-02 -6.68192447385442e-02 -6.64212149858202e-02 -6.60231852330962e-02 -6.56251554803721e-02 -6.52271257276481e-02 -6.48290959749241e-02 -6.44310662222001e-02 -6.37096372953878e-02 -6.29135777899398e-02 -6.24118944557772e-02 -6.20138647030532e-02 -6.16158349503292e-02 -6.12178051976052e-02 -6.08197754448812e-02 -6.04217456921572e-02 -5.98164087765561e-02 -5.90203492711080e-02 -5.84025739257343e-02 -5.80045441730103e-02 -5.76065144202863e-02 -5.72084846675622e-02 -5.68104549148382e-02 -5.64124251621142e-02 -5.59231802577243e-02 -5.51271207522762e-02 -5.43932533956913e-02 -5.39952236429673e-02 -5.35971938902433e-02 -5.31991641375193e-02 -5.28011343847953e-02 -5.24031046320712e-02 -5.20050748793472e-02 -5.16070451266232e-02 -5.12090153738992e-02 -5.08109856211752e-02 -5.04129558684512e-02 -5.00149261157272e-02 -4.96168963630031e-02 -4.92188666102791e-02 -4.88208368575551e-02 -4.84228071048311e-02 -4.80247773521071e-02 -4.76267475993831e-02 -4.72287178466590e-02 -4.66316732175730e-02 -4.58356137121250e-02 -4.52095460802362e-02 -4.48115163275122e-02 -4.44134865747881e-02 -4.40154568220641e-02 -4.36174270693401e-02 -4.32193973166161e-02 -4.28213675638921e-02 -4.24233378111681e-02 -4.20253080584440e-02 -4.16272783057200e-02 -4.12292485529960e-02 -4.08312188002720e-02 -4.04331890475480e-02 -4.00351592948240e-02 -3.96371295420999e-02 -3.92390997893759e-02 -3.88410700366519e-02 -3.81072026800670e-02 -3.73111431746190e-02 -3.68218982702290e-02 -3.64238685175050e-02 -3.60258387647810e-02 -3.56278090120570e-02 -3.52297792593330e-02 -3.48317495066090e-02 -3.39942285685855e-02 -3.28001393104135e-02 -3.19874952319353e-02 -3.15894654792112e-02 -3.11914357264872e-02 -3.07934059737632e-02 -3.02626996367979e-02 -2.94666401313498e-02 -2.87742342073403e-02 -2.83762044546163e-02 -2.79781747018923e-02 -2.75801449491683e-02 -2.71821151964443e-02 -2.67840854437203e-02 -2.63860556909962e-02 -2.59880259382722e-02 -2.55899961855482e-02 -2.44207837869214e-02 -2.32266945287494e-02 -2.20326052705773e-02 -2.08385160124053e-02 -2.02995173889248e-02 -1.99014876362008e-02 -1.95034578834768e-02 -1.91054281307528e-02 -1.81683997545483e-02 -1.69743104963763e-02 -1.60206975471416e-02 -1.52246380416936e-02 -1.44285785362456e-02 -1.36325190307976e-02 -1.26540292220177e-02 -1.14599399638456e-02 -1.05726653067317e-02 -1.01746355540077e-02 -9.65222150355741e-03 -8.85616199810937e-03 -7.77401860789093e-03 -5.78386984427089e-03 -3.99273595701279e-03 -3.19667645156477e-03 -2.10209463157375e-03 1.87820289566646e-03 5.78386984427085e-03 6.18189959699487e-03 6.57992934971888e-03 7.73255717531551e-03 8.92664643348757e-03 1.01207356916596e-02 1.13148249498317e-02 1.31474202696651e-02 1.51375690332852e-02 1.65472577408495e-02 1.77413469990215e-02 1.84130222067433e-02 1.88110519594673e-02 1.92090817121913e-02 1.96071114649153e-02 2.04114632568784e-02 2.16055525150505e-02 2.24513657395890e-02 2.28493954923130e-02 2.32474252450371e-02 2.36454549977611e-02 2.40434847504851e-02 2.44415145032091e-02 2.48395442559331e-02 2.52375740086571e-02 2.56356037613812e-02 2.60336335141052e-02 2.64316632668292e-02 2.68296930195532e-02 2.72277227722772e-02
+ 8.24175824175824e-04 7.05450328565907e-04 5.86724832955989e-04 4.67999337346071e-04 3.49273841736153e-04 2.30548346126236e-04 1.11822850516318e-04 -6.90264509359987e-06 -1.25628140703518e-04 -2.44353636313435e-04 -1.86371417527197e-04 -6.76459219172787e-05 5.10795736926391e-05 1.69805069302557e-04 2.88530564912474e-04 4.07256060522392e-04 5.25981556132310e-04 6.44707051742228e-04 7.63432547352145e-04 8.82158042962063e-04 1.00088353857198e-03 1.11960903418190e-03 1.23833452979182e-03 1.35706002540173e-03 1.47578552101165e-03 1.59451101662157e-03 1.71323651223149e-03 1.83196200784140e-03 1.89546634270252e-03 1.77674084709261e-03 1.65801535148269e-03 1.53928985587277e-03 1.42056436026285e-03 1.30183886465294e-03 1.18311336904302e-03 1.06438787343310e-03 9.45662377823182e-04 8.26936882213264e-04 7.08211386603346e-04 5.89485890993429e-04 4.70760395383511e-04 3.52034899773594e-04 2.33309404163675e-04 1.14583908553758e-04 -4.14158705615985e-06 -1.22867082666077e-04 -2.41592578275995e-04 -3.60318073885913e-04 -4.79043569495831e-04 -5.97769065105749e-04 -7.16494560715666e-04 -8.46264288475343e-04 -1.08371527969518e-03 -1.32116627091501e-03 -1.55861726213485e-03 -1.79606825335469e-03 -1.97829808382572e-03 -2.09702357943564e-03 -2.21574907504556e-03 -2.33447457065547e-03 -2.45320006626539e-03 -2.57192556187531e-03 -2.69065105748523e-03 -2.80937655309515e-03 -2.92810204870506e-03 -3.04682754431498e-03 -3.16555303992490e-03 -3.28427853553482e-03 -3.40300403114473e-03 -3.52172952675465e-03 -3.64045502236457e-03 -3.75918051797449e-03 -3.87790601358440e-03 -3.99663150919432e-03 -4.11535700480424e-03 -4.23408250041416e-03 -4.35280799602408e-03 -4.47153349163399e-03 -4.59025898724391e-03 -4.70898448285383e-03 -4.82770997846375e-03 -4.94643547407366e-03 -5.06516096968358e-03 -5.18388646529350e-03 -5.30261196090342e-03 -5.42133745651334e-03 -5.54006295212325e-03 -5.65878844773317e-03 -5.77751394334309e-03 -5.89623943895301e-03 -6.01496493456292e-03 -6.13369043017284e-03 -6.25241592578276e-03 -6.37114142139268e-03 -6.48986691700259e-03 -6.60859241261251e-03 -6.72731790822243e-03 -6.84604340383235e-03 -6.96476889944227e-03 -7.08349439505218e-03 -7.20221989066210e-03 -7.32094538627202e-03 -7.43967088188194e-03 -7.55839637749185e-03 -7.67712187310177e-03 -7.79584736871169e-03 -7.91457286432161e-03 -8.03329835993153e-03 -8.15202385554144e-03 -8.27074935115136e-03 -8.38947484676128e-03 -8.50820034237120e-03 -8.62692583798111e-03 -8.74565133359103e-03 -8.86437682920095e-03 -8.98310232481087e-03 -9.13772157490750e-03 -9.37517256612734e-03 -9.61262355734717e-03 -9.85007454856701e-03 -1.00875255397868e-02 -1.02449058479209e-02 -1.03636313435308e-02 -1.04823568391408e-02 -1.06010823347507e-02 -1.07198078303606e-02 -1.08385333259705e-02 -1.09572588215804e-02 -1.10759843171903e-02 -1.11947098128003e-02 -1.13134353084102e-02 -1.14321608040201e-02 -1.15508862996300e-02 -1.16696117952399e-02 -1.17883372908499e-02 -1.19070627864598e-02 -1.20257882820697e-02 -1.21445137776796e-02 -1.22632392732895e-02 -1.23819647688994e-02 -1.25006902645094e-02 -1.26194157601193e-02 -1.27381412557292e-02 -1.28568667513391e-02 -1.29755922469490e-02 -1.30943177425589e-02 -1.32130432381689e-02 -1.33317687337788e-02 -1.34504942293887e-02 -1.35692197249986e-02 -1.36879452206085e-02 -1.38066707162185e-02 -1.39253962118284e-02 -1.40441217074383e-02 -1.41628472030482e-02 -1.42815726986581e-02 -1.44002981942680e-02 -1.45190236898780e-02 -1.47150588105362e-02 -1.49525098017560e-02 -1.51899607929759e-02 -1.54274117841957e-02 -1.56621017173781e-02 -1.57808272129880e-02 -1.58995527085979e-02 -1.60182782042078e-02 -1.61370036998178e-02 -1.62557291954277e-02 -1.63744546910376e-02 -1.64931801866475e-02 -1.66119056822574e-02 -1.67306311778674e-02 -1.68493566734773e-02 -1.69680821690872e-02 -1.70868076646971e-02 -1.72055331603070e-02 -1.73242586559169e-02 -1.74429841515269e-02 -1.75617096471368e-02 -1.76804351427467e-02 -1.77991606383566e-02 -1.79178861339665e-02 -1.80366116295765e-02 -1.81553371251864e-02 -1.82740626207963e-02 -1.83927881164062e-02 -1.85115136120161e-02 -1.86302391076260e-02 -1.87489646032360e-02 -1.88676900988459e-02 -1.89256723176321e-02 -1.88069468220222e-02 -1.86882213264123e-02 -1.85694958308024e-02 -1.84507703351924e-02 -1.84811419736043e-02 -1.85998674692142e-02 -1.87185929648241e-02 -1.88373184604340e-02 -1.89560439560440e-02
+ -1.89768976897690e-01 -1.85490157055907e-01 -1.81211337214123e-01 -1.76932517372340e-01 -1.72653697530557e-01 -1.67976847936050e-01 -1.62628323133821e-01 -1.57279798331592e-01 -1.51931273529363e-01 -1.46582748727134e-01 -1.37121249813424e-01 -1.26245916048891e-01 -1.15370582284359e-01 -1.04495248519827e-01 -9.48637577325572e-02 -9.46854735724829e-02 -9.45071894124086e-02 -9.43289052523343e-02 -9.41506210922600e-02 -9.32757848649187e-02 -9.16712274242500e-02 -9.00666699835813e-02 -8.84621125429126e-02 -8.68575551022439e-02 -8.61734414647495e-02 -8.56385889845266e-02 -8.51037365043037e-02 -8.45688840240808e-02 -8.28731114317462e-02 -7.73463024694429e-02 -7.18194935071397e-02 -6.62926845448364e-02 -6.07658755825331e-02 -5.84730583611125e-02 -5.82947742010382e-02 -5.81164900409639e-02 -5.79382058808896e-02 -5.77599217208153e-02 -5.63626714430237e-02 -5.49363981624293e-02 -5.35101248818349e-02 -5.20838516012405e-02 -5.06575783206461e-02 -4.92313050400517e-02 -4.78050317594574e-02 -4.63787584788630e-02 -4.49524851982686e-02 -4.40403336816094e-02 -4.33271970413122e-02 -4.26140604010150e-02 -4.19009237607178e-02 -4.11712025473904e-02 -4.02797817470189e-02 -3.93883609466474e-02 -3.84969401462759e-02 -3.76055193459044e-02 -3.69628671409855e-02 -3.66062988208369e-02 -3.62497305006883e-02 -3.58931621805397e-02 -3.55365938603911e-02 -3.44337197538849e-02 -3.31857306333648e-02 -3.19377415128448e-02 -3.06897523923247e-02 -2.94044479824867e-02 -2.79781747018923e-02 -2.65519014212979e-02 -2.51256281407035e-02 -2.36993548601091e-02 -2.21694279980762e-02 -2.05648705574075e-02 -1.89603131167388e-02 -1.73557556760701e-02 -1.57511982354014e-02 -1.55065757832065e-02 -1.53282916231322e-02 -1.51500074630579e-02 -1.49717233029836e-02 -1.43290710980646e-02 -1.27245136573959e-02 -1.11199562167272e-02 -9.51539877605853e-03 -7.91084133538984e-03 -6.55505249017364e-03 -5.30706336965354e-03 -4.05907424913345e-03 -2.81108512861336e-03 -1.58797286763852e-03 -6.96552067267031e-04 1.94868733104463e-04 1.08628953347596e-03 1.97771033384745e-03 2.71157769043236e-03 3.24643017065525e-03 3.78128265087815e-03 4.31613513110105e-03 4.85098761132395e-03 5.09561006351892e-03 5.27389422359321e-03 5.45217838366751e-03 5.63046254374181e-03 5.87508499593678e-03 6.40993747615967e-03 6.94478995638257e-03 7.47964243660547e-03 8.01449491682836e-03 8.54934739705126e-03 9.08419987727415e-03 9.61905235749706e-03 1.01539048377200e-02 1.06887573179429e-02 1.10577640678641e-02 1.14143323880127e-02 1.17709007081613e-02 1.21274690283099e-02 1.26457369355026e-02 1.35371577358741e-02 1.44285785362456e-02 1.53199993366171e-02 1.62114201369886e-02 1.68623646284226e-02 1.73972171086455e-02 1.79320695888684e-02 1.84669220690913e-02 1.89851899762841e-02 1.91634741363584e-02 1.93417582964327e-02 1.95200424565070e-02 1.96983266165813e-02 2.01005025125628e-02 2.08136391528600e-02 2.15267757931572e-02 2.22399124334544e-02 2.29530490737516e-02 2.35252168432924e-02 2.40600693235153e-02 2.45949218037382e-02 2.51297742839611e-02 2.56065807585784e-02 2.57848649186527e-02 2.59631490787270e-02 2.61414332388013e-02 2.63197173988756e-02 2.67840854437202e-02 2.74972220840174e-02 2.82103587243146e-02 2.89234953646118e-02 2.96366320049090e-02 3.03497686452062e-02 3.10629052855034e-02 3.17760419258006e-02 3.24891785660978e-02 3.38491135545715e-02 3.68799442758346e-02 3.99107749970977e-02 4.29416057183608e-02 4.59724364396238e-02 4.96998192281539e-02 5.38003549098628e-02 5.79008905915717e-02 6.20014262732805e-02 6.60646466656716e-02 6.85606249067117e-02 7.10566031477519e-02 7.35525813887921e-02 7.60485596298323e-02 7.78396935170904e-02 7.85528301573876e-02 7.92659667976848e-02 7.99791034379820e-02 8.06922400782792e-02 8.15421994460753e-02 8.24336202464468e-02 8.33250410468183e-02 8.42164618471897e-02 8.50581289284707e-02 8.55929814086936e-02 8.61278338889165e-02 8.66626863691394e-02 8.71975388493623e-02 8.75499610262534e-02 8.77282451863277e-02 8.79065293464020e-02 8.80848135064763e-02 8.82630976665506e-02 8.92291490455578e-02 9.02988540060036e-02 9.13685589664494e-02 9.24382639268952e-02 9.33711461598421e-02 9.39059986400650e-02 9.44408511202879e-02 9.49757036005108e-02 9.55105560807337e-02 9.72768131084465e-02 9.97727913494867e-02 1.02268769590527e-01 1.04764747831567e-01 1.07260726072607e-01
diff --git a/src/demo_TPGMR01.cpp b/src/demo_TPGMR01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..586eb41a80c1a0468c71c05c95a3ca2a310f5ec9
--- /dev/null
+++ b/src/demo_TPGMR01.cpp
@@ -0,0 +1,1940 @@
+/*
+ * demo_TPGMR01.cpp
+ *
+ * TP-GMM with GMR (conditioning on the phase of the demonstration)
+ *
+ * @article{Calinon16JIST,
+ * author="Calinon, S.",
+ * title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
+ * journal="Intelligent Service Robotics",
+ * publisher="Springer Berlin Heidelberg",
+ * doi="10.1007/s11370-015-0187-9",
+ * year="2016",
+ * volume="9",
+ * number="1",
+ * pages="1--29"
+ * }
+ *
+ * Authors: Sylvain Calinon, Philip Abbet, Andras Kupcsik
+ */
+
+
+#include <stdio.h>
+#include <float.h>
+#include <armadillo>
+#include <chrono>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <window_utils.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui_internal.h>
+
+using namespace arma;
+
+
+/***************************** ALGORITHM SECTION *****************************/
+
+typedef std::vector<vec> vector_list_t;
+typedef std::vector<mat> matrix_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the parameters used by the algorithm. Some of them are
+// modifiable through the UI, others are hard-coded.
+//-----------------------------------------------------------------------------
+struct parameters_t {
+ int nb_states; // Number of components in the GMM
+ int nb_frames; // Number of candidate frames of reference
+ int nb_deriv; // Number of static and dynamic features
+ int nb_data; // Number of datapoints in a trajectory <==== not used for product of Gaussians!!!
+ float dt; // Time step duration <==== not used for product of Gaussians!!!
+};
+
+//-----------------------------------------------------------------------------
+// Model trained using the algorithm
+//-----------------------------------------------------------------------------
+struct model_t {
+ parameters_t parameters; // Parameters used to train the model
+
+ // These lists contain one element per GMM state and per frame (access them
+ // by doing: mu[state][frame])
+ std::vector<vector_list_t> mu;
+ std::vector<matrix_list_t> sigma;
+
+ int nb_var;
+ mat pix;
+ vec priors;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a coordinate system, aka a reference frame
+//-----------------------------------------------------------------------------
+struct coordinate_system_t {
+
+ coordinate_system_t(const arma::vec& position, const arma::mat& orientation,
+ const parameters_t& parameters) {
+
+ this->position = zeros(2 + (parameters.nb_deriv - 1) * 2);
+ this->position(span(0, 1)) = position(span(0, 1));
+
+ this->orientation = kron(eye(parameters.nb_deriv, parameters.nb_deriv),
+ orientation(span(0, 1), span(0, 1)));
+ }
+
+ vec position;
+ mat orientation;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of coordinate systems
+//-----------------------------------------------------------------------------
+typedef std::vector<coordinate_system_t> coordinate_system_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the needed informations about a demonstration, like:
+// - its reference frames
+// - the trajectory originally drawn by the user
+// - the resampled trajectory
+// - the trajectory expressed in each reference frame
+//-----------------------------------------------------------------------------
+class Demonstration
+{
+public:
+ Demonstration(coordinate_system_list_t coordinate_systems,
+ const std::vector<arma::vec>& points,
+ const parameters_t& parameters)
+ : coordinate_systems(coordinate_systems)
+ {
+ points_original = mat(3, points.size()); // added one for time (3rd dime)
+
+ for (size_t i = 0; i < points.size(); ++i) {
+ points_original(0, i) = points[i](0);
+ points_original(1, i) = points[i](1);
+ points_original(2, i) = points[i](2);
+ }
+
+ update(parameters);
+ }
+
+
+ //-------------------------------------------------------------------------
+ // Resample the trajectory and recompute it in each reference frame
+ // according to the provided parameters
+ //-------------------------------------------------------------------------
+ void update(const parameters_t& parameters)
+ {
+ // Resampling of the trajectory
+ arma::vec x = points_original.row(0).t();
+ arma::vec y = points_original.row(1).t();
+ arma::vec x2(parameters.nb_data);
+ arma::vec y2(parameters.nb_data);
+
+ arma::vec from_indices = arma::linspace<arma::vec>(0, points_original.n_cols - 1, points_original.n_cols);
+ arma::vec to_indices = arma::linspace<arma::vec>(0, points_original.n_cols - 1, parameters.nb_data);
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ points = mat(2 * parameters.nb_deriv, parameters.nb_data);
+ points(span(0), span::all) = x2.t();
+ points(span(1), span::all) = y2.t();
+// points = join_linspace(0, parameters.nb_data-1, parameters.nb_data);
+
+
+ // Compute the derivatives
+ mat D = (diagmat(ones(1, parameters.nb_data - 1), -1) -
+ eye(parameters.nb_data, parameters.nb_data)) / parameters.dt;
+
+ D(parameters.nb_data - 1, parameters.nb_data - 1) = 0.0;
+
+ points(span(2, 3), span::all) = points(span(0, 1), span::all) * pow(D, 1);
+
+ // Compute the points in each coordinate system
+ points_in_coordinate_systems.clear();
+
+
+ points = join_vert(points, linspace(0, points.n_cols-1, points.n_cols).t());
+
+ for (int m = 0; m < coordinate_systems.size(); ++m) {
+ points_in_coordinate_systems.push_back(
+ join_vert( pinv(coordinate_systems[m].orientation) *
+ (points.rows(0, points.n_rows-2) - repmat(coordinate_systems[m].position, 1, parameters.nb_data)), points.row(points.n_rows-1)));
+ }
+ }
+
+
+ //-------------------------------------------------------------------------
+ // Returns the coordinates of a point in a specific reference frame of
+ // the demonstration
+ //-------------------------------------------------------------------------
+ arma::vec convert_to_coordinate_system(const arma::vec& point, int frame) const {
+ vec original_point = zeros(points.n_rows);
+ original_point(span(0, 1)) = point(span(0, 1));
+
+ vec result = pinv(coordinate_systems[frame].orientation) *
+ (original_point - coordinate_systems[frame].position);
+
+ return result(span(0, 1));
+ }
+
+
+public:
+ coordinate_system_list_t coordinate_systems;
+ arma::mat points_original;
+ arma::mat points;
+ matrix_list_t points_in_coordinate_systems;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of demonstrations
+//-----------------------------------------------------------------------------
+typedef std::vector<Demonstration> demonstration_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Computes the factorial of a number
+//-----------------------------------------------------------------------------
+int factorial(int n) {
+ return (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
+}
+
+
+//-----------------------------------------------------------------------------
+// Return the likelihood of datapoint(s) to be generated by a Gaussian
+// parameterized by center and covariance
+//-----------------------------------------------------------------------------
+arma::vec gaussPDF(const mat& data, colvec mu, mat sigma) {
+
+ int nb_var = data.n_rows;
+ int nb_data = data.n_cols;
+
+ mat data2 = data.t() - repmat(mu.t(), nb_data, 1);
+
+ vec prob = sum((data2 * inv(sigma)) % data2, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nb_var) * det(sigma) + DBL_MIN);
+
+ return prob;
+}
+
+
+//-----------------------------------------------------------------------------
+// Initialization of Gaussian Mixture Model (GMM) parameters by clustering an
+// ordered dataset into equal bins
+//-----------------------------------------------------------------------------
+void init_tensorGMM_kbins(const demonstration_list_t& demos,
+ model_t &model) {
+
+ model.priors.resize(model.parameters.nb_states);
+ model.mu.clear();
+ model.sigma.clear();
+
+ model.nb_var = demos[0].points_in_coordinate_systems[0].n_rows;
+
+ // Initialize bins
+ uvec t_sep = linspace<uvec>(0, model.parameters.nb_data - 1,
+ model.parameters.nb_states + 1);
+
+ struct bin_t {
+ mat data;
+ vec mu;
+ mat sigma;
+ };
+
+ std::vector<bin_t> bins;
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ bin_t bin;
+ bin.data = zeros(model.nb_var * model.parameters.nb_frames + 1, //adding time as last dimension
+ demos.size() * (t_sep(i + 1) - t_sep(i) + 1));
+
+ bins.push_back(bin);
+ }
+
+
+ // Split each demonstration in K equal bins
+ for (int n = 0; n < demos.size(); ++n) {
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ int bin_size = t_sep(i + 1) - t_sep(i) + 1;
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ bins[i].data(span(m * model.nb_var, (m + 1) * model.nb_var - 1),
+ span(n * bin_size, (n + 1) * bin_size - 1)) =
+ demos[n].points_in_coordinate_systems[m](span::all, span(t_sep(i), t_sep(i + 1)));
+ }
+ }
+ }
+
+
+ // Calculate statistics on bin data
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ bins[i].mu = mean(bins[i].data, 1);
+ bins[i].sigma = cov(bins[i].data.t());
+ model.priors(i) = bins[i].data.n_elem;
+ }
+
+
+ // Reshape GMM into a tensor
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ model.mu.push_back(vector_list_t());
+ model.sigma.push_back(matrix_list_t());
+ }
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ model.mu[i].push_back(bins[i].mu(span(m * model.nb_var, (m + 1) * model.nb_var - 1)));
+
+ model.sigma[i].push_back(bins[i].sigma(span(m * model.nb_var, (m + 1) * model.nb_var - 1),
+ span(m * model.nb_var, (m + 1) * model.nb_var - 1)));
+
+ }
+
+ }
+
+ model.priors /= sum(model.priors);
+}
+
+
+//-----------------------------------------------------------------------------
+// Training of a task-parameterized Gaussian mixture model (GMM) with an
+// expectation-maximization (EM) algorithm.
+//
+// The approach allows the modulation of the centers and covariance matrices of
+// the Gaussians with respect to external parameters represented in the form of
+// candidate coordinate systems.
+//-----------------------------------------------------------------------------
+void train_EM_tensorGMM(const demonstration_list_t& demos,
+ model_t &model) {
+
+ const int nb_max_steps = 100; // Maximum number of iterations allowed
+ const int nb_min_steps = 5; // Minimum number of iterations allowed
+ const double max_diff_log_likelihood = 1e-5; // Likelihood increase threshold
+ // to stop the algorithm
+ const double diag_reg_fact = 1e-2; // Regularization term is optional
+
+
+ cube data(model.nb_var, model.parameters.nb_frames,
+ demos.size() * model.parameters.nb_data);
+
+
+ for (int n = 0; n < demos.size(); ++n) {
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ data(span::all, span(m), span(n * model.parameters.nb_data,
+ (n + 1) * model.parameters.nb_data - 1)) =
+ demos[n].points_in_coordinate_systems[m];
+ }
+ }
+
+
+ std::vector<double> log_likelihoods;
+
+ for (int iter = 0; iter < nb_max_steps; ++iter) {
+
+ // E-step
+ mat L = ones(model.parameters.nb_states, data.n_slices);
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+
+ // Matricization/flattening of tensor
+ mat data_mat(data(span::all, span(m), span::all));
+
+ vec gamma0 = gaussPDF(data_mat, model.mu[i][m], model.sigma[i][m]);
+
+ L(i, span::all) = L(i, span::all) % gamma0.t();
+ }
+
+ L(i, span::all) = L(i, span::all) * model.priors(i);
+ }
+
+ mat gamma = L / repmat(sum(L, 0) + DBL_MIN, L.n_rows, 1);
+ mat gamma2 = gamma / repmat(sum(gamma, 1), 1, data.n_slices);
+
+ model.pix = gamma2;
+
+
+ // M-step
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+
+ // Update priors
+ model.priors(i) = sum(gamma(i, span::all)) / data.n_slices;
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+
+ // Matricization/flattening of tensor
+ mat data_mat(data(span::all, span(m), span::all));
+
+ // Update mu
+ model.mu[i][m] = data_mat * gamma2(i, span::all).t();
+
+ // Update sigma
+ mat data_tmp = data_mat - repmat(model.mu[i][m], 1, data.n_slices);
+ model.sigma[i][m] = data_tmp * diagmat(gamma2(i, span::all)) * data_tmp.t() +
+ eye(data_tmp.n_rows, data_tmp.n_rows) * diag_reg_fact;
+ }
+ }
+
+ // Compute average log-likelihood
+ log_likelihoods.push_back(vec(sum(log(sum(L, 0)), 1))[0] / data.n_slices);
+
+ // Stop the algorithm if EM converged (small change of log-likelihood)
+ if (iter >= nb_min_steps) {
+ if (log_likelihoods[iter] - log_likelihoods[iter - 1] < max_diff_log_likelihood)
+ break;
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Training of the model
+//-----------------------------------------------------------------------------
+void learn(const demonstration_list_t& demos, model_t &model) {
+
+ init_tensorGMM_kbins(demos, model);
+ train_EM_tensorGMM(demos, model);
+
+}
+
+
+/*************************** DEMONSTRATION SECTION ***************************/
+
+static void error_callback(int error, const char* description) {
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+
+//-----------------------------------------------------------------------------
+// Contains all the informations about a viewport
+//-----------------------------------------------------------------------------
+struct viewport_t {
+ int x;
+ int y;
+ int width;
+ int height;
+
+ // Projection matrix parameters
+ arma::vec projection_top_left;
+ arma::vec projection_bottom_right;
+ double projection_near;
+ double projection_far;
+
+ // View matrix parameters
+ arma::fvec view;
+};
+
+
+//-----------------------------------------------------------------------------
+// Helper function to setup a viewport
+//-----------------------------------------------------------------------------
+void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
+ double near = -1.0, double far = 1.0,
+ const fvec& view_transforms = zeros<fvec>(3)) {
+
+ viewport->x = x;
+ viewport->y = y;
+ viewport->width = width;
+ viewport->height = height;
+ viewport->projection_top_left = vec({ (double) -width / 2,
+ (double) height / 2 });
+ viewport->projection_bottom_right = vec({ (double) width / 2,
+ (double) -height / 2 });
+ viewport->projection_near = near;
+ viewport->projection_far = far;
+ viewport->view = view_transforms;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from UI-space to OpenGL-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec ui2fb(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // ui -> viewport
+ result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width - viewport.x;
+ result(1) = (window_size.win_height - coords(1)) *
+ (float) window_size.fb_height / (float) window_size.win_height - viewport.y;
+
+ // viewport -> fb
+ result(0) = result(0) - (float) viewport.width * 0.5f;
+ result(1) = result(1) - (float) viewport.height * 0.5f;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from OpenGL-space to UI-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // fb -> viewport
+ result(0) = result(0) + (float) viewport.width * 0.5f;
+ result(1) = result(1) + (float) viewport.height * 0.5f;
+
+ // viewport -> ui
+ result(0) = (result(0) + viewport.x) * (float) window_size.win_width /
+ (float) window_size.fb_width;
+
+ result(1) = window_size.win_height - (result(1) + viewport.y) *
+ (float) window_size.win_height / (float) window_size.fb_height;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Colors used by the movable handlers
+//-----------------------------------------------------------------------------
+const arma::fmat HANDLER_COLORS({
+ { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ { 0.0, 0.75, 0.75 },
+ });
+
+
+//-----------------------------------------------------------------------------
+// Colors used by the fixed handlers
+//-----------------------------------------------------------------------------
+const arma::fmat HANDLER_FIXED_COLORS({
+ { 0.4, 0.4, 0.4 },
+ { 0.4, 0.4, 1.0 },
+ { 0.2, 0.5, 0.2 },
+ { 1.0, 0.4, 0.4 },
+ { 0.3, 0.75, 0.75 },
+ });
+
+
+//-----------------------------------------------------------------------------
+// Colors of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const arma::mat COLORS({
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ { 0.0, 0.75, 0.75 },
+ { 0.75, 0.0, 0.75 },
+ { 0.75, 0.75, 0.0 },
+ { 0.25, 0.25, 0.25 },
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ });
+
+
+//-----------------------------------------------------------------------------
+// An user-movable UI widget representing a coordinate system
+//
+// Can be "fixed", so the user can't move it anymore
+//-----------------------------------------------------------------------------
+class Handler
+{
+public:
+ Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
+ int index)
+ : viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
+ {
+ ui_position = position;
+ ui_y = y;
+
+ fvec color = HANDLER_COLORS.row(index).t();
+
+ models[0] = gfx2::create_rectangle(color, 25.0f, 5.0f);
+ models[0].transforms.parent = &transforms;
+ models[0].transforms.rotation = gfx2::rotate(arma::fvec({0.0f, 0.0f, 1.0f}),
+ gfx2::deg2rad(90.0f));
+
+ models[1] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[1].transforms.parent = &transforms;
+ models[1].transforms.position(0) = 30.0f;
+ models[1].transforms.position(1) = -10.0f;
+
+ models[2] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[2].transforms.parent = &transforms;
+ models[2].transforms.position(0) = 30.0f;
+ models[2].transforms.position(1) = 10.0f;
+ }
+
+
+ void update(const gfx2::window_size_t& window_size)
+ {
+ transforms.position.rows(0, 1) = arma::conv_to<arma::fvec>::from(
+ ui2fb(ui_position, window_size, *viewport)
+ );
+
+ arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
+
+ transforms.rotation(0, 0) = -delta(0);
+ transforms.rotation(1, 0) = delta(1);
+ transforms.rotation(0, 1) = -delta(1);
+ transforms.rotation(1, 1) = -delta(0);
+ }
+
+
+ void update()
+ {
+ transforms.position(0) = ui_position.x;
+ transforms.position(1) = ui_position.y;
+
+ arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
+
+ transforms.rotation(0, 0) = -delta(0);
+ transforms.rotation(1, 0) = delta(1);
+ transforms.rotation(0, 1) = -delta(1);
+ transforms.rotation(1, 1) = -delta(0);
+ }
+
+
+ void viewport_resized(const gfx2::window_size_t& window_size)
+ {
+ ui_position = fb2ui(arma::conv_to<arma::vec>::from(transforms.position.rows(0, 1)),
+ window_size, *viewport);
+ }
+
+
+ void fix()
+ {
+ fixed = true;
+
+ for (int i = 0; i < 3; ++i)
+ models[i].diffuse_color = HANDLER_FIXED_COLORS.row(index).t();
+ }
+
+
+ bool draw(const gfx2::window_size_t& window_size)
+ {
+ if (!fixed)
+ {
+ std::stringstream id;
+ id << "handler" << (uint64_t) this;
+
+ ImGuiWindow* window = ImGui::GetCurrentWindow();
+ if (window->SkipItems)
+ return false;
+
+ ImGui::PushID(id.str().c_str());
+
+ // Position
+ ImVec2 current_position = ui_position;
+
+ ui_position = ui::dragger(0, ui_position, false, ui::config.draggerSize );
+
+ moved = moved || (ui_position.x != current_position.x) || (ui_position.y != current_position.y);
+
+ hovered = ImGui::IsItemHovered();
+
+ // y-axis
+ ImVec2 py = ui_position + ui_y;
+ current_position = ui_y;
+
+ py = ui::dragger(1, py, false, ui::config.draggerSize * 0.7);
+ ui_y = py - ui_position;
+
+ moved = moved || ((ui_y.x != current_position.x) || (ui_y.y != current_position.y));
+
+ hovered = ImGui::IsItemHovered() || hovered;
+
+ window->DrawList->AddLine(ui_position, py, ui::config.lineColor);
+
+ ImGui::PopID();
+
+ update(window_size);
+ }
+
+ return hovered;
+ }
+
+
+ void draw_anchor() const
+ {
+ for (int i = 0; i < 3; ++i)
+ gfx2::draw(models[i]);
+ }
+
+
+public:
+ const viewport_t* viewport;
+ int index;
+
+ ImVec2 ui_position;
+ ImVec2 ui_y;
+
+ gfx2::transforms_t transforms;
+ gfx2::model_t models[3];
+
+ bool hovered;
+ bool fixed;
+ bool moved;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of handlers
+//-----------------------------------------------------------------------------
+typedef std::vector<Handler*> handler_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the needed infos about the state of the application (values of
+// the parameters modifiable via the UI, which action the user is currently
+// doing, ...)
+//-----------------------------------------------------------------------------
+struct gui_state_t {
+ // Indicates if the user can draw a new demonstration
+ bool can_draw_demonstration;
+
+ // Indicates if the user is currently drawing a new demonstration
+ bool is_drawing_demonstration;
+
+ // Indicates if the parameters dialog is displayed
+ bool is_parameters_dialog_displayed;
+
+ // Indicates if the parameters were modified through the UI
+ bool are_parameters_modified;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ //in parameters_t)
+ int parameter_nb_states;
+ int parameter_nb_frames;
+ int parameter_nb_data;
+
+ int parameter_nb_gmr_components;
+};
+
+
+//-----------------------------------------------------------------------------
+// Create a handler at a random position (within the given boundaries)
+//-----------------------------------------------------------------------------
+Handler* create_random_handler(const viewport_t* viewport, int index,
+ int min_x, int min_y, int max_x, int max_y) {
+ return new Handler(viewport,
+ ImVec2((randu() * 0.8f + 0.1f) * (max_x - min_x) + min_x,
+ (randu() * 0.5f + 0.1f) * (max_y - min_y) + min_y),
+ ImVec2((randu() - 0.5) * 10, randu() * -10 - 10),
+ index);
+}
+
+
+//-----------------------------------------------------------------------------
+// Create handlers to be used for a new demonstration at random positions
+//-----------------------------------------------------------------------------
+void create_new_demonstration_handlers(const viewport_t& viewport,
+ const gfx2::window_size_t& window_size,
+ int nb_frames,
+ handler_list_t &handlers) {
+
+ for (size_t i = 0; i < handlers.size(); ++i)
+ delete handlers[i];
+
+ handlers.clear();
+
+ handlers.push_back(
+ new Handler(&viewport, ImVec2(window_size.win_width / 6,
+ window_size.win_height - 50),
+ ImVec2(0, 30), 0)
+ );
+
+ for (int n = 1; n < nb_frames; ++n) {
+ handlers.push_back(
+ create_random_handler(&viewport, n, 10, 20,
+ window_size.win_width / 3 - 10,
+ window_size.win_height / 2 - 20)
+ );
+ };
+}
+
+
+//-----------------------------------------------------------------------------
+// Create handlers to be used for a new reproduction at random positions
+//-----------------------------------------------------------------------------
+void create_reproduction_handlers(const viewport_t& viewport,
+ const gfx2::window_size_t& window_size,
+ int nb_frames,
+ handler_list_t &handlers) {
+
+ for (size_t i = 0; i < handlers.size(); ++i)
+ delete handlers[i];
+
+ handlers.clear();
+
+ handlers.push_back(
+ new Handler(&viewport, ImVec2(window_size.win_width / 2,
+ window_size.win_height - 50),
+ ImVec2(0, 30), 0)
+ );
+
+ for (int n = 1; n < nb_frames; ++n) {
+ handlers.push_back(
+ create_random_handler(&viewport, n,
+ window_size.win_width / 3 + 20,
+ 20,
+ window_size.win_width * 2 / 3 - 20,
+ window_size.win_height / 2- 20)
+ );
+ };
+}
+
+//-----------------------------------------------------------------------------
+// Create handlers to be used for moving GMR window
+//-----------------------------------------------------------------------------
+void create_reproduction_handlers_gmrmoving(const viewport_t& viewport,
+ const gfx2::window_size_t& window_size,
+ int nb_frames,
+ handler_list_t &handlers) {
+
+ for (size_t i = 0; i < handlers.size(); ++i)
+ delete handlers[i];
+
+ handlers.clear();
+
+ handlers.push_back(
+ new Handler(&viewport, ImVec2(window_size.win_width *5 / 6,
+ window_size.win_height - 50),
+ ImVec2(0, 30), 0)
+ );
+
+ if (nb_frames > 1) {
+ for (int n = 1; n < nb_frames; ++n) {
+ handlers.push_back(
+ create_random_handler(&viewport, n,
+ window_size.win_width * 2 / 3 + 20,
+ 20,
+ window_size.win_width - 20,
+ window_size.win_height / 2 - 20)
+ );
+ };
+ };
+
+ for (int i = 0; i < handlers.size(); i++) {
+ handlers.at(i)->fix();
+ handlers.at(i)->update(window_size);
+ }
+
+
+}
+
+
+//-----------------------------------------------------------------------------
+// Extract a list of coordinate systems from a list of handlers
+//-----------------------------------------------------------------------------
+void convert(const handler_list_t& from, coordinate_system_list_t &to,
+ const parameters_t& parameters) {
+
+ to.clear();
+
+ for (int n = 0; n < from.size(); ++n) {
+ to.push_back(
+ coordinate_system_t(arma::conv_to<arma::vec>::from(from[n]->transforms.position),
+ arma::conv_to<arma::mat>::from(from[n]->transforms.rotation),
+ parameters)
+ );
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Extract a list of coordinate systems from a list of demonstrations
+//-----------------------------------------------------------------------------
+void convert(const demonstration_list_t& from, std::vector<coordinate_system_list_t> &to) {
+
+ to.clear();
+
+ for (int n = 0; n < from.size(); ++n) {
+ to.push_back(from[n].coordinate_systems);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Compute a view matrix centered on the given reference frame across all the
+// demonstrations
+//-----------------------------------------------------------------------------
+arma::fvec compute_centered_view_matrix(const demonstration_list_t& demonstrations,
+ int frame) {
+ vec top_left({-30, 0});
+ vec bottom_right({0, 0});
+
+ for (auto iter = demonstrations.begin(); iter != demonstrations.end(); ++iter) {
+ top_left(0) = fmin(top_left(0), iter->points_in_coordinate_systems[frame](0, span::all).min());
+ top_left(1) = fmax(top_left(1), iter->points_in_coordinate_systems[frame](1, span::all).max());
+
+ bottom_right(0) = fmax(bottom_right(0), iter->points_in_coordinate_systems[frame](0, span::all).max());
+ bottom_right(1) = fmin(bottom_right(1), iter->points_in_coordinate_systems[frame](1, span::all).min());
+ }
+
+ vec center = (bottom_right - top_left) / 2 + top_left;
+
+ return fvec({ (float) -center(0), (float) -center(1), 0.0f });
+}
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations" viewport
+//-----------------------------------------------------------------------------
+void draw_demos_viewport(const viewport_t& viewport,
+ const std::vector<arma::vec>& current_trajectory,
+ const demonstration_list_t& demonstrations,
+ const std::vector<handler_list_t> fixed_demonstration_handlers,
+ const handler_list_t current_demonstration_handlers) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.7f, 0.7f, 0.7f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+ // Draw the currently created demonstration (if any)
+ if (current_trajectory.size() > 1) {
+ std::vector<arma::vec> tmpvec;
+ for (int i = 0; i < current_trajectory.size(); i++) {
+ tmpvec.push_back(current_trajectory.at(i).subvec(0, 1));
+ }
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), tmpvec);
+ tmpvec.clear();
+ }
+
+ // Draw the demonstrations
+ int color_index = 0;
+ for (auto iter = demonstrations.begin(); iter != demonstrations.end(); ++iter) {
+ arma::mat datapoints = iter->points(span(0, 1), span::all);
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ ++color_index;
+ if (color_index >= demonstrations.size())
+ color_index = 0;
+ }
+
+ // Draw the handlers
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ fixed_demonstration_handlers[n][i]->draw_anchor();
+ }
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->draw_anchor();
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_reproductions_viewport(const viewport_t& viewport,
+ const std::vector<handler_list_t>& handlers,
+ const matrix_list_t& reproductions) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+ // Draw the handlers
+ for (size_t n = 0; n < handlers.size(); ++n) {
+ for (size_t i = 0; i < handlers[n].size(); ++i)
+ handlers[n][i]->draw_anchor();
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_reproductions_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers,
+ const matrix_list_t& reproductions) {
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ draw_reproductions_viewport(viewport, handler_list, reproductions);
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "GMMs" viewport
+//-----------------------------------------------------------------------------
+void draw_gmrmoving_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers, model_t model, int nb_gmr_components, int drawIndex) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ // Draw the handlers
+ for (size_t n = 0; n < handler_list.size(); ++n) {
+ for (size_t i = 0; i < handler_list[n].size(); ++i)
+ handler_list[n][i]->draw_anchor();
+ }
+
+ mat grey = {0.9, 0.9, 0.9};
+ mat black = {0.0, 0.0, 0.0};
+
+ if (model.mu.size() > 0) {
+ mat inputs = linspace(0, 199, nb_gmr_components);
+ inputs = inputs.t();
+ mat H(model.parameters.nb_states, inputs.size());
+
+ cube muGMR(2 * model.parameters.nb_deriv, inputs.size(), model.parameters.nb_frames);
+ muGMR = zeros(2 * model.parameters.nb_deriv, inputs.size(), model.parameters.nb_frames);
+ field<cube> sigmaGMR(model.parameters.nb_frames);
+
+ for (int i = 0; i < model.parameters.nb_frames; i++) {
+ sigmaGMR(i).set_size(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv, inputs.size());
+ sigmaGMR(i) = zeros(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv, inputs.size());
+ }
+
+ for (int m = 0; m < model.parameters.nb_frames; m++) {
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ H.row(i) = model.priors(i) * gaussPDF(inputs, model.mu[i][m].row(model.nb_var - 1),
+ model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)).t();
+ }
+ H = H / repmat(sum(H + 1e-300), model.parameters.nb_states, 1);
+
+ mat muTmp(2 * model.parameters.nb_deriv, model.parameters.nb_states);
+ mat sigmaTmp;
+
+ for (int t = 0; t < inputs.size(); t++) {
+ // Compute conditional means
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ muTmp.col(i) = model.mu[i][m].subvec(0, 2 * model.parameters.nb_deriv - 1) +
+ model.sigma[i][m].col(2 * model.parameters.nb_deriv).rows(0, 2 *
+ model.parameters.nb_deriv -
+ 1) *
+ inv(model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)) *
+ (inputs(t) - model.mu[i][m].row(model.nb_var - 1));
+ muGMR.slice(m).col(t) += H(i, t) * muTmp.col(i);
+ }
+
+ // Compute conditional covariances
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ sigmaTmp = model.sigma[i][m].submat(0, 0, model.nb_var - 2, model.nb_var - 2) -
+ model.sigma[i][m].col(2 * model.parameters.nb_deriv).rows(0,
+ 2 * model.parameters.nb_deriv -
+ 1) *
+ inv(model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)) *
+ model.sigma[i][m].row(2 * model.parameters.nb_deriv).cols(0,
+ 2 * model.parameters.nb_deriv -
+ 1);
+ sigmaGMR(m).slice(t) += H(i, t) * (sigmaTmp + muTmp.col(i) * muTmp.col(i).t());
+ }
+
+ sigmaGMR(m).slice(t) += -muGMR.slice(m).col(t) * muGMR.slice(m).col(t).t() +
+ eye(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv) * 1e-4;
+ }
+ }
+
+ // transform mu/sigma GMR components into coordinate systems
+ cube muGMRt = zeros(2, inputs.size(), model.parameters.nb_frames);
+ field<cube> sigmaGMRt(model.parameters.nb_frames);
+ for (int i = 0; i < model.parameters.nb_frames; i++) {
+ sigmaGMRt(i).resize(2, 2, inputs.size());
+ sigmaGMRt(i) = zeros(2, 2, inputs.size());
+ }
+
+ for (int f = 0; f < model.parameters.nb_frames; f++) {
+ muGMRt.slice(f) = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * muGMR.slice(f).rows(0, 1);
+ //muGMRt.slice(f).each_col() += handler_list[0][f]->transforms.position.subvec(0, 1);
+ vec b = {handler_list[0][f]->transforms.position(0), handler_list[0][f]->transforms.position(1)};
+ muGMRt.slice(f).each_col() += b;
+// mat dummy = repmat(b , 1, inputs.n_cols);
+// muGMR.slice(f) += tmpmat;
+ for (int t = 0; t < inputs.size(); t++ ) {
+ sigmaGMRt(f).slice(t) = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) *
+ sigmaGMR(f).slice(t).submat(0, 0, 1, 1) *
+ handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t();
+ }
+ }
+
+
+
+ // product
+ vec maxMuP = zeros(2, 1);
+ mat maxSigmaP = eye(2, 2);
+ for (int t = 0; t < inputs.size(); t++) {
+ vec muP = zeros(2, 1);
+ mat sigmaP = zeros(2, 2);
+
+ for (int m = 0; m < model.parameters.nb_frames; m++) {
+
+ sigmaP += inv(sigmaGMRt(m).slice(t) + eye(2, 2) * 1e-4);
+ muP += inv(sigmaGMRt(m).slice(t) + eye(2, 2) * 1e-4) * muGMRt.slice(m).col(t);
+ }
+
+ sigmaP = inv(sigmaP);
+ muP = sigmaP * muP;
+
+
+ if (t == drawIndex) {
+ maxMuP = muP;
+ maxSigmaP = sigmaP;
+ }
+
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(grey.row(0).t()), muP, sigmaP);
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(black.row(0).t()), maxMuP, maxSigmaP);
+
+ }
+
+
+}
+
+//-----------------------------------------------------------------------------
+// Render a "Product" viewport
+//-----------------------------------------------------------------------------
+void draw_gmr_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers, model_t model, int nb_gmr_components, vec mouse, int &drawIndex) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ // Draw the handlers
+ for (size_t n = 0; n < handler_list.size(); ++n) {
+ for (size_t i = 0; i < handler_list[n].size(); ++i)
+ handler_list[n][i]->draw_anchor();
+ }
+
+ mat grey = {0.9, 0.9, 0.9};
+ mat black = {0.0, 0.0, 0.0};
+
+ // Draw the Product GMM states
+
+ // GMR
+ //int numGauss = 20; // later load it from GUI
+ if (model.mu.size() > 0) {
+ mat inputs = linspace(0, 199, nb_gmr_components);
+ inputs = inputs.t();
+ mat H(model.parameters.nb_states, inputs.size());
+
+ cube muGMR(2 * model.parameters.nb_deriv, inputs.size(), model.parameters.nb_frames);
+ muGMR = zeros(2 * model.parameters.nb_deriv, inputs.size(), model.parameters.nb_frames);
+ field<cube> sigmaGMR(model.parameters.nb_frames);
+
+ for (int i = 0; i < model.parameters.nb_frames; i++) {
+ sigmaGMR(i).set_size(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv, inputs.size());
+ sigmaGMR(i) = zeros(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv, inputs.size());
+ }
+
+ for (int m = 0; m < model.parameters.nb_frames; m++) {
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ H.row(i) = model.priors(i) * gaussPDF(inputs, model.mu[i][m].row(model.nb_var - 1),
+ model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)).t();
+ }
+ H = H / repmat(sum(H + 1e-300), model.parameters.nb_states, 1);
+
+ mat muTmp(2 * model.parameters.nb_deriv, model.parameters.nb_states);
+ mat sigmaTmp;
+
+ for (int t = 0; t < inputs.size(); t++) {
+ // Compute conditional means
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ muTmp.col(i) = model.mu[i][m].subvec(0, 2 * model.parameters.nb_deriv - 1) +
+ model.sigma[i][m].col(2 * model.parameters.nb_deriv).rows(0, 2 *
+ model.parameters.nb_deriv -
+ 1) *
+ inv(model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)) *
+ (inputs(t) - model.mu[i][m].row(model.nb_var - 1));
+ muGMR.slice(m).col(t) += H(i, t) * muTmp.col(i);
+ }
+
+ // Compute conditional covariances
+ for (int i = 0; i < model.parameters.nb_states; i++) {
+ sigmaTmp = model.sigma[i][m].submat(0, 0, model.nb_var - 2, model.nb_var - 2) -
+ model.sigma[i][m].col(2 * model.parameters.nb_deriv).rows(0,
+ 2 * model.parameters.nb_deriv -
+ 1) *
+ inv(model.sigma[i][m].row(model.nb_var - 1).col(model.nb_var - 1)) *
+ model.sigma[i][m].row(2 * model.parameters.nb_deriv).cols(0,
+ 2 * model.parameters.nb_deriv -
+ 1);
+ sigmaGMR(m).slice(t) += H(i, t) * (sigmaTmp + muTmp.col(i) * muTmp.col(i).t());
+ }
+
+ sigmaGMR(m).slice(t) += -muGMR.slice(m).col(t) * muGMR.slice(m).col(t).t() +
+ eye(2 * model.parameters.nb_deriv, 2 * model.parameters.nb_deriv) * 1e-4;
+ }
+ }
+
+ // transform mu/sigma GMR components into coordinate systems
+ cube muGMRt = zeros(2, inputs.size(), model.parameters.nb_frames);
+ field<cube> sigmaGMRt(model.parameters.nb_frames);
+ for (int i = 0; i < model.parameters.nb_frames; i++) {
+ sigmaGMRt(i).resize(2, 2, inputs.size());
+ sigmaGMRt(i) = zeros(2, 2, inputs.size());
+ }
+
+ for (int f = 0; f < model.parameters.nb_frames; f++) {
+ muGMRt.slice(f) = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * muGMR.slice(f).rows(0, 1);
+ //muGMRt.slice(f).each_col() += handler_list[0][f]->transforms.position.subvec(0, 1);
+ vec b = {handler_list[0][f]->transforms.position(0), handler_list[0][f]->transforms.position(1)};
+ muGMRt.slice(f).each_col() += b;
+// mat dummy = repmat(b , 1, inputs.n_cols);
+// muGMR.slice(f) += tmpmat;
+ for (int t = 0; t < inputs.size(); t++ ) {
+ sigmaGMRt(f).slice(t) = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) *
+ sigmaGMR(f).slice(t).submat(0, 0, 1, 1) *
+ handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t();
+ }
+ }
+
+
+
+ // product
+ double maxLL = 0.0;
+ vec LLs = zeros(inputs.size(), 1);
+ vec maxMuP = zeros(2, 1);
+ mat maxSigmaP = eye(2, 2);
+ for (int t = 0; t < inputs.size(); t++) {
+ vec muP = zeros(2, 1);
+ mat sigmaP = zeros(2, 2);
+
+ for (int m = 0; m < model.parameters.nb_frames; m++) {
+
+ sigmaP += inv(sigmaGMRt(m).slice(t) + eye(2, 2) * 1e-4);
+ muP += inv(sigmaGMRt(m).slice(t) + eye(2, 2) * 1e-4) * muGMRt.slice(m).col(t);
+ }
+
+ sigmaP = inv(sigmaP);
+ muP = sigmaP * muP;
+
+ vec currLL = gaussPDF(mouse, muP, sigmaP);
+ LLs.at(t) = currLL(0);
+
+ if (LLs.at(t) >= maxLL) {
+ maxMuP = muP;
+ maxSigmaP = sigmaP;
+ maxLL =LLs.at(t);
+ drawIndex = t;
+ }
+
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(grey.row(0).t()), muP, sigmaP);
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(black.row(0).t()), maxMuP, maxSigmaP);
+
+ }
+
+
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+ arma_rng::set_seed_random();
+
+ // Model
+ model_t model;
+
+ // Parameters
+ model.parameters.nb_states = 4;
+ model.parameters.nb_frames = 2;
+ model.parameters.nb_deriv = 2;
+ model.parameters.nb_data = 200;
+ model.parameters.dt = 0.1f;
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 1200;
+ window_size.win_height = 400;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+ int viewport_width = 0;
+ int viewport_height = 0;
+
+
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
+
+ if (!glfwInit())
+ return -1;
+
+ glfwWindowHint(GLFW_SAMPLES, 4);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
+
+ // Open a window and create its OpenGL context
+ GLFWwindow* window = create_window_at_optimal_size(
+ "Demo - TPGMR",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+ // Setup GLSL
+ gfx2::init();
+ glEnable(GL_SCISSOR_TEST);
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_LINE_SMOOTH);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ // Setup ImGui
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+
+
+ // Viewports
+ viewport_t viewport_demos;
+ viewport_t viewport_gmr;
+ viewport_t viewport_gmrmoving;
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.can_draw_demonstration = false;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = false;
+ gui_state.parameter_nb_data = model.parameters.nb_data;
+ gui_state.parameter_nb_states = model.parameters.nb_states;
+ gui_state.parameter_nb_frames = model.parameters.nb_frames;
+ gui_state.parameter_nb_gmr_components = 20;
+ std::chrono::microseconds ms2 = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch());
+ long startTimeMsec = ms2.count();
+ ImVec2 initFramePos;
+ int drawIndex = 0;
+ bool reset_moving_handler = true;
+
+
+ // Initial handlers
+ std::vector<handler_list_t> fixed_demonstration_handlers;
+ handler_list_t current_demonstration_handlers;
+ handler_list_t reproduction_handlers;
+ handler_list_t reproduction_handlers_moving;
+
+ // create_new_demonstration_handlers(viewport_demos, window_size,
+ // model.parameters.nb_frames,
+ // current_demonstration_handlers
+ // );
+ //
+
+
+ // List of demonstrations and reproductions
+ demonstration_list_t demos;
+
+ // Main loop
+ std::vector<arma::vec> current_trajectory;
+
+ while (!glfwWindowShouldClose(window)) {
+ glfwPollEvents();
+
+ // Detect when the window was resized
+ if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
+ (ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+
+ bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+
+ window_size.win_width = ImGui::GetIO().DisplaySize.x;
+ window_size.win_height = ImGui::GetIO().DisplaySize.y;
+
+ glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+
+ viewport_width = window_size.fb_width / 3 - 1;
+ viewport_height = window_size.fb_height;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demos, 0, window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_gmr, viewport_width + 2,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_gmrmoving, window_size.fb_width - viewport_width,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+
+ // Update all the handlers
+ if (!first) {
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->viewport_resized(window_size);
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i)
+ reproduction_handlers[i]->viewport_resized(window_size);
+
+ for (size_t i = 0; i < reproduction_handlers_moving.size(); ++i)
+ reproduction_handlers_moving[i]->viewport_resized(window_size);
+ }
+
+ // At the very first frame: load initial data from files (can't be done
+ // outside the loop because we need to know the size of the OpenGL front
+ // buffer)
+ //
+ // Note: The loaded data was recorded in another demo with 3x2 viewports,
+ // so appropriate scaling must be applied
+ else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
+
+ cube loaded_trajectories;
+ mat loaded_frames;
+ //loaded_trajectories.load("data/data_tpgmm_product_trajectories.txt");
+ //loaded_frames.load("data/data_tpgmm_product_frames.txt");
+ loaded_trajectories.load("data/data_tpgmr_trajectories.txt");
+ loaded_frames.load("data/data_tpgmr_frames.txt");
+
+ // adding time as 3rd dimension
+ cube loaded_trajectories_tmp(loaded_trajectories.n_rows + 1, loaded_trajectories.n_cols, loaded_trajectories.n_slices);
+ for (int i = 0; i < loaded_trajectories.n_slices; i++) {
+ vec tmpvec = linspace(0, loaded_trajectories.n_cols-1, loaded_trajectories.n_cols);
+ loaded_trajectories_tmp.slice(i) = join_vert(loaded_trajectories.slice(i), tmpvec.t());
+ }
+ loaded_trajectories.resize(loaded_trajectories.n_rows + 1, loaded_trajectories.n_cols, loaded_trajectories.n_slices);
+ loaded_trajectories = loaded_trajectories_tmp;
+
+ for (int n = 0; n < loaded_frames.n_cols / 2; ++n) {
+ Handler* handler1 = new Handler(
+ &viewport_demos,
+ ImVec2(loaded_frames(0, n * 2) * window_size.win_width,
+ loaded_frames(1, n * 2) * window_size.win_height * 2),
+ ImVec2(loaded_frames(2, n * 2) * window_size.win_width,
+ loaded_frames(3, n * 2) * window_size.win_height * 2),
+ 0
+ );
+ handler1->update(window_size);
+ handler1->fix();
+
+ Handler* handler2 = new Handler(
+ &viewport_demos,
+ ImVec2(loaded_frames(0, n * 2 + 1) * window_size.win_width,
+ loaded_frames(1, n * 2 + 1) * window_size.win_height * 2),
+ ImVec2(loaded_frames(2, n * 2 + 1) * window_size.win_width,
+ loaded_frames(3, n * 2 + 1) * window_size.win_height * 2),
+ 1
+ );
+ handler2->update(window_size);
+ handler2->fix();
+
+ handler_list_t handlers;
+ handlers.push_back(handler1);
+ handlers.push_back(handler2);
+
+ fixed_demonstration_handlers.push_back(handlers);
+ }
+
+ Handler* handler_reproduction_1 = new Handler(
+ &viewport_gmr,
+ fixed_demonstration_handlers[0][0]->ui_position +
+ ImVec2(window_size.win_width / 3, 0),
+ fixed_demonstration_handlers[0][0]->ui_y,
+ 0
+ );
+ handler_reproduction_1->update(window_size);
+ reproduction_handlers.push_back(handler_reproduction_1);
+
+ Handler* handler_reproduction_2 = new Handler(
+ &viewport_gmr,
+ fixed_demonstration_handlers[0][1]->ui_position +
+ ImVec2(window_size.win_width / 3, 0),
+ fixed_demonstration_handlers[0][1]->ui_y,
+ 1
+ );
+ handler_reproduction_2->update(window_size);
+ reproduction_handlers.push_back(handler_reproduction_2);
+
+
+ // moving frames
+ Handler* handler_reproduction_moving1 = new Handler(
+ &viewport_gmrmoving,
+ fixed_demonstration_handlers[0][0]->ui_position +
+ ImVec2(window_size.win_width * 2 / 3, 0),
+ fixed_demonstration_handlers[0][0]->ui_y,
+ 0
+ );
+ handler_reproduction_moving1->fix();
+ handler_reproduction_moving1->update(window_size);
+ reproduction_handlers_moving.push_back(handler_reproduction_moving1);
+
+ Handler* handler_reproduction_moving2 = new Handler(
+ &viewport_gmrmoving,
+ fixed_demonstration_handlers[0][1]->ui_position +
+ ImVec2(window_size.win_width * 2 / 3, 0),
+ fixed_demonstration_handlers[0][1]->ui_y,
+ 1
+ );
+ handler_reproduction_moving2->fix();
+ handler_reproduction_moving2->update(window_size);
+ reproduction_handlers_moving.push_back(handler_reproduction_moving2);
+
+
+ for (int n = 0; n < loaded_trajectories.n_slices; ++n) {
+ vector_list_t trajectory;
+ for (int i = 0; i < loaded_trajectories.n_cols; ++i) {
+ mat t = loaded_trajectories(span::all, span(i), span(n));
+ t(0, span::all) *= window_size.fb_width;
+ t(1, span::all) *= window_size.fb_height * 2;
+ trajectory.push_back(t);
+ }
+
+ coordinate_system_list_t coordinate_systems;
+ convert(fixed_demonstration_handlers[n], coordinate_systems, model.parameters);
+
+ Demonstration demonstration(coordinate_systems, trajectory, model.parameters);
+ demos.push_back(demonstration);
+ }
+
+ // Initial learning from the loaded data
+ learn(demos, model);
+
+ }
+ }
+
+
+ // If the parameters changed, recompute
+ if (gui_state.are_parameters_modified) {
+
+ // If the number of frames changed, clear everything
+ if (model.parameters.nb_frames != gui_state.parameter_nb_frames) {
+ demos.clear();
+ model.mu.clear();
+ model.sigma.clear();
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ delete current_demonstration_handlers[i];
+
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ delete fixed_demonstration_handlers[n][i];
+ }
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i)
+ delete reproduction_handlers[i];
+
+ for (size_t i = 0; i < reproduction_handlers_moving.size(); ++i)
+ delete reproduction_handlers_moving[i];
+
+
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
+ reproduction_handlers.clear();
+ reproduction_handlers_moving.clear();
+
+
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ gui_state.parameter_nb_frames,
+ current_demonstration_handlers
+ );
+
+ create_reproduction_handlers(viewport_gmr, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers);
+
+
+ create_reproduction_handlers_gmrmoving(viewport_gmrmoving, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers_moving);
+
+ model.parameters.nb_frames = gui_state.parameter_nb_frames;
+
+ gui_state.can_draw_demonstration = true;
+ reset_moving_handler = true;
+ }
+
+ // If the number of states changed, recompute the model
+ if (model.parameters.nb_states != gui_state.parameter_nb_states) {
+
+ model.parameters.nb_states = gui_state.parameter_nb_states;
+
+ for (auto iter = demos.begin(); iter != demos.end(); ++iter)
+ iter->update(model.parameters);
+
+ if (!demos.empty()) {
+ learn(demos, model);
+ }
+ }
+
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glScissor(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT);
+
+ draw_demos_viewport(viewport_demos, current_trajectory, demos,
+ fixed_demonstration_handlers,
+ current_demonstration_handlers);
+
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+ vec mp = ui2fb({ mouse_x, mouse_y }, window_size, viewport_gmr);
+
+ draw_gmr_viewport(viewport_gmr, reproduction_handlers, model, gui_state.parameter_nb_gmr_components, mp, drawIndex);
+
+
+
+ if (reproduction_handlers.size() > 0) {
+ int moving_handler_ix;
+ if (model.parameters.nb_frames > 1) {
+ moving_handler_ix = 1;
+ } else {
+ moving_handler_ix = 0;
+ }
+
+
+ if ( reset_moving_handler) {
+ initFramePos.x = reproduction_handlers_moving.at(moving_handler_ix)->ui_position.x;
+ initFramePos.y = reproduction_handlers_moving.at(moving_handler_ix)->ui_position.y;
+ reset_moving_handler = false;
+ }
+ ImVec2 diff;
+ std::chrono::microseconds ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch());
+ long currTimeMsec = ms.count();
+ float phase = (currTimeMsec-startTimeMsec)/1E6 ;
+ diff.x = 50.0 * sin(phase);
+ diff.y = 10.0 * sin(2 * phase);
+ reproduction_handlers_moving.at(moving_handler_ix)->ui_position = initFramePos + diff;
+ reproduction_handlers_moving.at(moving_handler_ix)->update(window_size);
+ }
+
+ draw_gmrmoving_viewport(viewport_gmrmoving, reproduction_handlers_moving, model, gui_state.parameter_nb_gmr_components, drawIndex);
+
+
+ // Draw the UI controls of the handlers
+ ui::begin("handlers");
+
+ bool hovering_ui = false;
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i) {
+ hovering_ui = reproduction_handlers[i]->draw(window_size) || hovering_ui;
+ }
+
+
+ for (size_t i = 0; i < reproduction_handlers_moving.size(); ++i) {
+ hovering_ui = reproduction_handlers_moving[i]->draw(window_size) || hovering_ui;
+ }
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ hovering_ui = current_demonstration_handlers[i]->draw(window_size) || hovering_ui;
+
+ ui::end();
+
+
+ // Window: Demonstrations
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Demonstrations", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Demonstrations ");
+ ImGui::SameLine();
+
+ if (!gui_state.can_draw_demonstration) {
+ if (ImGui::Button("Add")) {
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ gui_state.parameter_nb_frames,
+ current_demonstration_handlers
+ );
+
+ gui_state.can_draw_demonstration = true;
+ }
+ }
+
+ ImGui::SameLine();
+
+ if (ImGui::Button("Clear")) {
+ demos.clear();
+ model.mu.clear();
+ model.sigma.clear();
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ delete current_demonstration_handlers[i];
+
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ delete fixed_demonstration_handlers[n][i];
+ }
+
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
+
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ model.parameters.nb_frames,
+ current_demonstration_handlers
+ );
+
+ gui_state.can_draw_demonstration = true;
+ }
+
+ ImGui::SameLine();
+ ImGui::Text(" ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ ImGui::SameLine();
+ if (ImGui::Button("Save")) {
+ cube tmpTrajs(2, model.parameters.nb_data, fixed_demonstration_handlers.size());
+ mat tmpFrames(4, 2 * fixed_demonstration_handlers.size());
+
+ for (int i = 0; i < fixed_demonstration_handlers.size(); i++) {
+ tmpFrames(0, i * 2) = fixed_demonstration_handlers.at(i).at(0)->ui_position.x / window_size.win_width ;
+ tmpFrames(1, i * 2) =fixed_demonstration_handlers.at(i).at(0)->ui_position.y / window_size.win_height / 2;
+ tmpFrames(2, i * 2) =fixed_demonstration_handlers.at(i).at(0)->ui_y.x / window_size.win_width ;
+ tmpFrames(3, i * 2) =fixed_demonstration_handlers.at(i).at(0)->ui_y.y / window_size.win_height / 2;
+
+ tmpFrames(0, i*2 + 1) = fixed_demonstration_handlers.at(i).at(1)->ui_position.x / window_size.win_width;
+ tmpFrames(1, i*2 + 1) =fixed_demonstration_handlers.at(i).at(1)->ui_position.y / window_size.win_height / 2;
+ tmpFrames(2, i*2 + 1) =fixed_demonstration_handlers.at(i).at(1)->ui_y.x / window_size.win_width;
+ tmpFrames(3, i*2 + 1) =fixed_demonstration_handlers.at(i).at(1)->ui_y.y / window_size.win_height / 2;
+
+ }
+ tmpFrames.save("data/data_tpgmr_frames.txt", arma_ascii);
+
+ for (int i = 0; i < demos.size(); i++ ) {
+ tmpTrajs.slice(i).row(0) = demos.at(i).points.row(0) / window_size.fb_width;
+ tmpTrajs.slice(i).row(1) = demos.at(i).points.row(1) / window_size.fb_height / 2;
+ }
+
+ tmpTrajs.save("data/data_tpgmr_trajectories.txt", arma_ascii);
+
+ }
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+
+
+ ImGui::End();
+
+ // Window: GMR
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width / 3, 0));
+ ImGui::Begin("TPGMR", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("TPGMR");
+ ImGui::SameLine();
+ ImGui::SliderInt("Nb components", &gui_state.parameter_nb_gmr_components, 10, 199);
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+ ImGui::End();
+
+ // Window: GMMs in global coordinates
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width * 2 / 3, 0));
+ ImGui::Begin("GMMs", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("TPGMR (changing TPs)");
+ ImGui::SameLine();
+ if (ImGui::Button("Randomize")) {
+ for (size_t i = 0; i < reproduction_handlers_moving.size(); ++i)
+ delete reproduction_handlers_moving[i];
+
+ reproduction_handlers_moving.clear();
+
+ create_reproduction_handlers_gmrmoving(viewport_gmrmoving, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers_moving);
+
+ reset_moving_handler = true;
+
+ }
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+ ImGui::End();
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(600, 100));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 600) / 2, (window_size.win_height - 100) / 2));
+ ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
+
+ if (gui_state.is_parameters_dialog_displayed)
+ ImGui::OpenPopup("Parameters");
+
+ if (ImGui::BeginPopupModal("Parameters", NULL,
+ ImGuiWindowFlags_NoResize |
+ ImGuiWindowFlags_NoSavedSettings)) {
+
+ ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 1, 10);
+ ImGui::SliderInt("Nb frames", &gui_state.parameter_nb_frames, 1, 5);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+
+ hovering_ui = true;
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (!gui_state.is_drawing_demonstration) {
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1) && gui_state.can_draw_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if (!hovering_ui && (mouse_x <= window_size.win_width / 3))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+ vec tmpvec = {0.0};
+ coords = join_vert(coords, tmpvec);
+
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+ vec tmpvec = {(float)current_trajectory.size() - 1};
+ coords = join_vert(coords, tmpvec);
+
+ vec last_point = current_trajectory[current_trajectory.size() - 1];
+ vec diff = abs(coords - last_point);
+
+ if ((diff(0) > 1e-6) && (diff(1) > 1e-6))
+ current_trajectory.push_back(coords);
+
+ // Left mouse button release: end the demonstration creation
+ if (!ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ gui_state.is_drawing_demonstration = false;
+
+ if (current_trajectory.size() > 1) {
+
+ coordinate_system_list_t coordinate_systems;
+ convert(current_demonstration_handlers, coordinate_systems, model.parameters);
+
+ Demonstration demonstration(coordinate_systems, current_trajectory,
+ model.parameters);
+
+ demos.push_back(demonstration);
+
+ for (int i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->fix();
+
+ fixed_demonstration_handlers.push_back(current_demonstration_handlers);
+ current_demonstration_handlers.clear();
+
+ learn(demos, model);
+
+ gui_state.can_draw_demonstration = false;
+ }
+
+ current_trajectory.clear();
+ }
+ }
+
+ ImGui::CaptureMouseFromApp();
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
+