diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9767a413e82b6dff4ba2d4ec64e18f18e94ed81c..c56797baab23ffe5bacb9887a0c969488eb0bb16 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -94,17 +94,19 @@ set(GFX2_SUPPORT_SOURCES
)
-# Declare the support files needed for OpenGL3 (using GLSL & gfx3.h) demos
-set(GFX3_SUPPORT_SOURCES
- ${PROJECT_SOURCE_DIR}/src/utils/imgui_impl_glfw_gl3.cpp
- ${PROJECT_SOURCE_DIR}/src/utils/imgui.cpp
- ${PROJECT_SOURCE_DIR}/src/utils/imgui_draw.cpp
- ${PROJECT_SOURCE_DIR}/src/utils/gfx_ui.cpp
- ${PROJECT_SOURCE_DIR}/src/utils/gfx3.cpp
+# Executables
+add_executable(demo_ergodicControl_2D01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_ergodicControl_2D01.cpp
+ ${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
+)
+target_link_libraries(demo_ergodicControl_2D01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
)
-
-# Executables
add_executable(demo_GMR01
${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_GMR01.cpp
@@ -116,19 +118,53 @@ target_link_libraries(demo_GMR01
${ARMADILLO_LIBRARIES}
)
-add_executable(demo_infHorLQR01_glsl
- ${GFX3_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_infHorLQR01_glsl.cpp
+add_executable(demo_GPR01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_GPR01.cpp
)
-target_link_libraries(demo_infHorLQR01_glsl
+target_link_libraries(demo_GPR01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+)
+
+add_executable(demo_HSMM_batchLQR01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_HSMM_batchLQR01.cpp
+)
+
+target_link_libraries(demo_HSMM_batchLQR01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
${GLEW_LIBRARIES}
)
+add_executable(demo_LWR_batch01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_LWR_batch01.cpp
+)
+target_link_libraries(demo_LWR_batch01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+)
+
+add_executable(demo_LWR_iterative01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_LWR_iterative01.cpp
+)
+target_link_libraries(demo_LWR_iterative01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+)
+
add_executable(demo_MPC_batch01
- ${GL2_SUPPORT_SOURCES}
+ ${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_MPC_batch01.cpp
${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
@@ -138,10 +174,11 @@ target_link_libraries(demo_MPC_batch01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
+ ${GLEW_LIBRARIES}
)
add_executable(demo_MPC_iterative01
- ${GL2_SUPPORT_SOURCES}
+ ${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_MPC_iterative01.cpp
${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
)
@@ -150,10 +187,11 @@ target_link_libraries(demo_MPC_iterative01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
+ ${GLEW_LIBRARIES}
)
add_executable(demo_MPC_semitied01
- ${GL2_SUPPORT_SOURCES}
+ ${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_MPC_semitied01.cpp
${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
@@ -162,10 +200,11 @@ target_link_libraries(demo_MPC_semitied01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
+ ${GLEW_LIBRARIES}
)
add_executable(demo_MPC_velocity01
- ${GL2_SUPPORT_SOURCES}
+ ${GFX2_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_MPC_velocity01.cpp
${PROJECT_SOURCE_DIR}/src/utils/mpc_utils.cpp
${PROJECT_SOURCE_DIR}/src/utils/gl2ps.c
@@ -174,6 +213,7 @@ target_link_libraries(demo_MPC_velocity01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
+ ${GLEW_LIBRARIES}
)
add_executable(demo_online_GMM01
diff --git a/README.md b/README.md
index e090864dae7cda59c4322138774d5984224068ce..274a7d01bbc2b7fa3e3c2e05cdf85d3107e888dc 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
-# PbDlib-cpp-sandbox
+# PbDlib-cpp
-PbDlib-cpp-sandbox is a set of tools in C++ combining statistical learning, optimal control and differential geometry for programming-by-demonstration applications.
+PbDlib-cpp is a set of tools in C++ combining statistical learning, optimal control and differential geometry for programming-by-demonstration applications.
Other versions of the library are available at http://www.idiap.ch/software/pbdlib/ (in Matlab, C++ and Python).
@@ -13,8 +13,14 @@ This work was in part supported by the DexROV project through the EC H2020 progr
### Prerequisite
-The program requires glfw3 (lightweight and portable library for managing OpenGL contexts, windows and inputs) and
-Armadillo (C++ library for linear algebra & scientific computing). Instructions are given below.
+The program requires:
+
+ - *glfw3* (lightweight and portable library for managing OpenGL contexts, windows and inputs)
+ - *GLEW* (The OpenGL Extension Wrangler Library)
+ - *LAPACK* (Linear Algebra PACKage)
+ - *Armadillo* (C++ library for linear algebra & scientific computing)
+
+Instructions are given below.
ImGui (graphical user interface library for C++, [https://github.com/ocornut/imgui](https://github.com/ocornut/imgui)) and
gfx_ui (a minimal geometry editing UI, [https://github.com/colormotor/gfx\_ui](https://github.com/colormotor/gfx_ui)), are also
@@ -34,10 +40,10 @@ make
### glfw3 installation
-**On Debian:**
+**On Debian and Ubuntu:**
```
-sudo apt-get install libglfw3-dev
+sudo apt-get install libglfw3-dev
```
**Installation from source**
@@ -59,6 +65,24 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
static library.
+### GLEW installation
+
+**On Debian and Ubuntu:**
+
+```
+sudo apt-get install libglew-dev
+```
+
+
+### LAPACK installation
+
+**On Debian and Ubuntu:**
+
+```
+sudo apt-get install liblapack-dev
+```
+
+
### Armadillo installation
See [http://arma.sourceforge.net/download.html](http://arma.sourceforge.net/download.html)
@@ -78,6 +102,12 @@ See [examples.md](./examples.md)
***
+[demo\_GPR01.cpp](./src/demo_GPR01.cpp)
+
+
+
+***
+
[demo\_MPC\_batch01.cpp](./src/demo_MPC_batch01.cpp)
diff --git a/data/data_4_frames.txt b/data/data_4_frames.txt
new file mode 100644
index 0000000000000000000000000000000000000000..afdac866d537cf1f778164540d8081e9957486f7
--- /dev/null
+++ b/data/data_4_frames.txt
@@ -0,0 +1,6 @@
+ARMA_MAT_TXT_FN008
+4 8
+ 9.52582433819771e-02 2.64691840857267e-02 9.51822921633720e-02 1.13339841365814e-01 9.53103303909302e-02 2.15707466006279e-01 9.53146666288376e-02 2.37892791628838e-01
+ 4.58216160535812e-01 1.68154299259186e-01 4.58710938692093e-01 7.65657573938370e-02 4.58629548549652e-01 1.57467454671860e-01 4.58499342203140e-01 3.23121756315231e-01
+ 0.00000000000000e+00 -1.08463540673256e-02 0.00000000000000e+00 9.67881933320314e-04 0.00000000000000e+00 1.50325521826744e-02 0.00000000000000e+00 1.82703994214535e-02
+ 2.50000003725290e-02 -1.44889326766133e-02 2.50000003725290e-02 -1.40039063990116e-02 2.50000003725290e-02 -1.41308596357703e-02 2.50000003725290e-02 1.55143225565553e-02
diff --git a/data/data_4_trajectories.txt b/data/data_4_trajectories.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e095c58e4c64fe88051b197417db5b4b2ed9604e
--- /dev/null
+++ b/data/data_4_trajectories.txt
@@ -0,0 +1,10 @@
+ARMA_CUB_TXT_FN008
+2 200 4
+ -7.11046006944445e-02 -7.09964427170575e-02 -7.11769132118928e-02 -7.11485269228085e-02 -7.09707500174483e-02 -7.07929731120882e-02 -7.07442813023451e-02 -7.09247517971803e-02 -7.11052222920156e-02 -7.13030538979620e-02 -7.14835243927973e-02 -7.16753363170017e-02 -7.18773555276382e-02 -7.20620136271636e-02 -7.21421669981854e-02 -7.20541727735902e-02 -7.19385556253490e-02 -7.17590338847013e-02 -7.15597082635399e-02 -7.15589885189838e-02 -7.17825564454216e-02 -7.20363863588777e-02 -7.24997491799274e-02 -7.28387161501954e-02 -7.31828412897823e-02 -7.34451445596036e-02 -7.39021823527359e-02 -7.44574325621161e-02 -7.47341634387214e-02 -7.51283980667225e-02 -7.57064619974874e-02 -7.61739033710218e-02 -7.65449098792574e-02 -7.68864831972362e-02 -7.72362572410664e-02 -7.74810249162479e-02 -7.78419659059185e-02 -7.80684673366834e-02 -7.83943371370743e-02 -7.85748076319095e-02 -7.87665323143495e-02 -7.89685515249860e-02 -7.91705707356226e-02 -7.94266580297320e-02 -7.97093649671971e-02 -8.00288770240089e-02 -8.04110177624232e-02 -8.05914882572585e-02 -8.08660162793132e-02 -8.11294100711893e-02 -8.13411131176717e-02 -8.17431122627024e-02 -8.20493962869905e-02 -8.23574578622278e-02 -8.27804495568118e-02 -8.33964418446399e-02 -8.37306105178671e-02 -8.43469735831937e-02 -8.50253219221105e-02 -8.56085985482970e-02 -8.60027677449749e-02 -8.64100341115299e-02 -8.71257001151591e-02 -8.75386917225014e-02 -8.79287714614740e-02 -8.85521683940536e-02 -8.90601989984645e-02 -8.93273005653266e-02 -8.98006743788386e-02 -9.00850498150475e-02 -9.03173964440257e-02 -9.06615324888331e-02 -9.09691360448074e-02 -9.11496065396427e-02 -9.13300770344779e-02 -9.15266436348409e-02 -9.19171922982970e-02 -9.21948392134282e-02 -9.24936749720826e-02 -9.31976396740648e-02 -9.39475698806533e-02 -9.45293852072864e-02 -9.49979498185371e-02 -9.55614007537688e-02 -9.59268783151870e-02 -9.63900666527080e-02 -9.68637458123953e-02 -9.70898219395589e-02 -9.74659320910106e-02 -9.78268730806812e-02 -9.84645885678392e-02 -9.92773982761027e-02 -1.00387560633026e-01 -1.00902308853294e-01 -1.01704998516890e-01 -1.02344938669040e-01 -1.03076984313931e-01 -1.03437925303601e-01 -1.04026000226829e-01 -1.04672199975572e-01 -1.05076063913317e-01 -1.05593494817839e-01 -1.06061743177694e-01 -1.06370579023590e-01 -1.06739273625070e-01 -1.07436346227666e-01 -1.07819206710637e-01 -1.08180147700307e-01 -1.08550030970826e-01 -1.09131846297460e-01 -1.09618873447097e-01 -1.10105769734087e-01 -1.10501323893774e-01 -1.10727400020938e-01 -1.11112550600223e-01 -1.11473491589894e-01 -1.11691955873116e-01 -1.11872426367951e-01 -1.12052896862786e-01 -1.12233367357621e-01 -1.12413837852457e-01 -1.12766741694584e-01 -1.13189787915271e-01 -1.13752955314768e-01 -1.14172915358040e-01 -1.14393364391401e-01 -1.14724959432580e-01 -1.14905429927415e-01 -1.15213033483389e-01 -1.15428575167504e-01 -1.15789516157175e-01 -1.16267252058906e-01 -1.16734028213987e-01 -1.17278155098409e-01 -1.17592334502373e-01 -1.17772804997208e-01 -1.18070179456309e-01 -1.18305448684394e-01 -1.19092184010329e-01 -1.19399634893216e-01 -1.19828711700865e-01 -1.20230787182440e-01 -1.20591728172111e-01 -1.20881065483668e-01 -1.21168210845896e-01 -1.21529151835567e-01 -1.21776111460078e-01 -1.22317522944584e-01 -1.22655813791178e-01 -1.22972915794249e-01 -1.23214760870324e-01 -1.23694797773590e-01 -1.24173776870463e-01 -1.24463114182021e-01 -1.24788983982412e-01 -1.25081974542853e-01 -1.25532054805276e-01 -1.25892995794947e-01 -1.26237862489531e-01 -1.26469478468733e-01 -1.26830419458403e-01 -1.27339802310162e-01 -1.27752172319933e-01 -1.28335961491485e-01 -1.28597534111530e-01 -1.29216830681184e-01 -1.29491522281547e-01 -1.29722069549135e-01 -1.30030414660106e-01 -1.30210885154941e-01 -1.30392086299553e-01 -1.30717225450168e-01 -1.30968568973339e-01 -1.31223565745394e-01 -1.31404036240229e-01 -1.31791280621859e-01 -1.32201175146566e-01 -1.32482475310581e-01 -1.32979317158710e-01 -1.33392690448772e-01 -1.33812814070352e-01 -1.34110428444305e-01 -1.34351706448911e-01 -1.34552635137493e-01 -1.34858046744137e-01 -1.35129117811279e-01 -1.35364387039363e-01 -1.35544857534199e-01 -1.35725328029034e-01 -1.35905798523869e-01 -1.36086269018705e-01 -1.36276968610413e-01 -1.36592609313931e-01 -1.36773079808766e-01 -1.36953550303601e-01 -1.37134020798437e-01 -1.37314491293272e-01 -1.37494961788107e-01 -1.37675432282942e-01 -1.37855902777778e-01
+ -2.01012369791667e-01 -1.99162364557161e-01 -1.97077297293760e-01 -1.93143647927136e-01 -1.83732513478853e-01 -1.81271445116206e-01 -1.76643553051717e-01 -1.68612087128350e-01 -1.64954296220687e-01 -1.60158392875838e-01 -1.58456965818677e-01 -1.55884729637772e-01 -1.53120190797739e-01 -1.50286474691164e-01 -1.40419267561767e-01 -1.20625736102387e-01 -7.82781583699749e-02 -6.87191327732412e-02 -5.62453053025545e-02 -4.82558953622278e-02 -4.29531937028894e-02 -3.35878448230737e-02 -2.85858491677136e-02 -2.67646337154523e-02 -2.51646415672111e-02 -2.35726974717337e-02 -2.22315843540620e-02 -2.06315922058208e-02 -1.92328013766750e-02 -1.73260671848828e-02 -1.58625811348409e-02 -1.48733086002931e-02 -1.38729945299414e-02 -1.30002715399916e-02 -1.21713875366415e-02 -1.15909626256281e-02 -1.11986036955611e-02 -1.05245792765913e-02 -9.96672817211055e-03 -9.51409063546901e-03 -8.92897102701005e-03 -8.25023555276382e-03 -7.61972296377722e-03 -6.74699997382747e-03 -6.17162963253769e-03 -5.48658003559464e-03 -4.37493456867672e-03 -3.46823636411223e-03 -2.91723919074540e-03 -2.04451620079565e-03 -1.38989216917923e-03 -4.36688651591295e-04 6.22480894053595e-04 1.49520388400335e-03 2.97660175879396e-03 4.88758898659965e-03 6.89565994032664e-03 9.13046678706030e-03 1.08106123063233e-02 1.23368633532245e-02 1.34148116886516e-02 1.47056408343802e-02 1.57710590713987e-02 1.69118869085008e-02 1.79013066635260e-02 1.92709641959799e-02 2.05535162793132e-02 2.14518883479899e-02 2.23464653999162e-02 2.30474965975712e-02 2.35889080820771e-02 2.43210191582915e-02 2.45917249005444e-02 2.48624306427973e-02 2.53183561034338e-02 2.56416686296064e-02 2.61107457862228e-02 2.63814515284757e-02 2.68297051664573e-02 2.76447668027638e-02 2.87696621126466e-02 2.95128637981575e-02 3.02945063860971e-02 3.09495230056533e-02 3.15606842807789e-02 3.22027618561558e-02 3.26679949225293e-02 3.32160804020100e-02 3.38063200115159e-02 3.43477314960218e-02 3.49260462468593e-02 3.59896651224874e-02 3.73619399078727e-02 3.81340622382747e-02 3.90136882328308e-02 3.93964614740369e-02 3.99735493875628e-02 4.04283788735343e-02 4.08715125104690e-02 4.14546691792295e-02 4.17579597204774e-02 4.20515664258794e-02 4.23379756857203e-02 4.26190195770519e-02 4.28919499842965e-02 4.31949788002513e-02 4.34832201371440e-02 4.37539258793970e-02 4.40259402481156e-02 4.43925355946399e-02 4.49523987123116e-02 4.55315967860134e-02 4.58626629239950e-02 4.61959373691374e-02 4.67567165253350e-02 4.71370197340871e-02 4.74077254763400e-02 4.76784312185930e-02 4.79491369608459e-02 4.82198427030988e-02 4.84905484453518e-02 4.90604552711474e-02 4.97794146513819e-02 5.01924662374372e-02 5.06701803287270e-02 5.10008538787688e-02 5.14982464405360e-02 5.17689521827889e-02 5.22303575167504e-02 5.25284626256281e-02 5.28769825690955e-02 5.34475927816164e-02 5.40304877250838e-02 5.46021284809464e-02 5.50733975868928e-02 5.54668524916248e-02 5.58329080297320e-02 5.63217127303182e-02 5.66623481993300e-02 5.71072648398241e-02 5.75656766907454e-02 5.78928169493300e-02 5.81635226915829e-02 5.84342284338358e-02 5.87049341760888e-02 5.89756399183417e-02 5.92463456605946e-02 5.95170514028476e-02 5.97877571451005e-02 6.00584628873534e-02 6.03291686296064e-02 6.05998743718593e-02 6.10325226392379e-02 6.13854264813652e-02 6.16561322236181e-02 6.19268379658710e-02 6.21975437081239e-02 6.24682494503769e-02 6.27630666352596e-02 6.32277598932161e-02 6.34984656354690e-02 6.39728263714405e-02 6.43004573649498e-02 6.47728224455611e-02 6.51651813756281e-02 6.57274000209380e-02 6.62652782139866e-02 6.65359839562395e-02 6.68066896984925e-02 6.70773954407454e-02 6.73481011829983e-02 6.76188069252513e-02 6.79958222100084e-02 6.83783173680905e-02 6.86490231103434e-02 6.89197288525963e-02 6.92890723146985e-02 6.96792392954355e-02 6.99499450376884e-02 7.07056768216080e-02 7.09763825638610e-02 7.14078039939280e-02 7.17619346733668e-02 7.20326404156198e-02 7.23866402324121e-02 7.30639525753769e-02 7.34990545173786e-02 7.39878592179648e-02 7.42585649602178e-02 7.45292707024707e-02 7.49808122644472e-02 7.55068801036432e-02 7.57775858458961e-02 7.62894060144472e-02 7.69283592441374e-02 7.72439999476549e-02 7.75147056899079e-02 7.78982968226549e-02 7.84397083071608e-02 7.87630208333333e-02
+ -7.08159722222222e-02 -7.07180869625907e-02 -7.07260804892518e-02 -7.08254488588777e-02 -7.09248172285036e-02 -7.10314266645729e-02 -7.11409150788666e-02 -7.12402834484925e-02 -7.13477653022055e-02 -7.14563812988554e-02 -7.15557496684813e-02 -7.16641039398381e-02 -7.17718475188442e-02 -7.18712158884701e-02 -7.19903008968453e-02 -7.21090369381630e-02 -7.22202701877443e-02 -7.23315034373255e-02 -7.24375021810441e-02 -7.25368705506700e-02 -7.24317333193746e-02 -7.23280901032942e-02 -7.22302048436628e-02 -7.21323195840313e-02 -7.20344343243998e-02 -7.19365490647683e-02 -7.18386638051368e-02 -7.17250750279174e-02 -7.16058155360134e-02 -7.14960653964266e-02 -7.13950394332775e-02 -7.12939262283640e-02 -7.11841760887772e-02 -7.10648293551089e-02 -7.09514150614182e-02 -7.08535298017867e-02 -7.07451755304299e-02 -7.06354253908431e-02 -7.05256752512563e-02 -7.04159251116695e-02 -7.03061749720826e-02 -7.01964248324958e-02 -7.00866746929090e-02 -6.99769245533222e-02 -6.98659530290341e-02 -6.97443380094919e-02 -6.96379030569514e-02 -6.95206501256281e-02 -6.93130147264098e-02 -6.91751945491346e-02 -6.90684106295366e-02 -6.89705253699051e-02 -6.88633052414852e-02 -6.87535551018984e-02 -6.86438049623116e-02 -6.85340548227247e-02 -6.84243046831379e-02 -6.83145545435511e-02 -6.82048044039643e-02 -6.81006377372976e-02 -6.80026652359017e-02 -6.78929150963149e-02 -6.77896208472920e-02 -6.76907759282524e-02 -6.75810257886656e-02 -6.74786039572864e-02 -6.73807186976549e-02 -6.72828334380235e-02 -6.71849481783920e-02 -6.70870629187605e-02 -6.69891776591290e-02 -6.68912923994975e-02 -6.67934071398660e-02 -6.66955218802345e-02 -6.65976366206030e-02 -6.64541457286432e-02 -6.62346454494696e-02 -6.61260294528196e-02 -6.60281441931882e-02 -6.59302589335567e-02 -6.58206832774986e-02 -6.57109331379118e-02 -6.56018809324400e-02 -6.55039956728085e-02 -6.54061104131770e-02 -6.53066548017867e-02 -6.51969046621999e-02 -6.50950935231714e-02 -6.49972082635399e-02 -6.48993230039084e-02 -6.48014377442769e-02 -6.47035524846454e-02 -6.46056672250140e-02 -6.45077819653825e-02 -6.44098967057510e-02 -6.43120114461195e-02 -6.42141261864880e-02 -6.41111809045226e-02 -6.40014307649358e-02 -6.38916806253490e-02 -6.37878629257398e-02 -6.36895414572864e-02 -6.35797913176996e-02 -6.34768460357342e-02 -6.33776521496371e-02 -6.32679020100503e-02 -6.31658291457286e-02 -6.30679438860972e-02 -6.29700586264657e-02 -6.28721733668342e-02 -6.27742881072027e-02 -6.26764028475712e-02 -6.25690954773869e-02 -6.24632712171971e-02 -6.23653859575656e-02 -6.22675006979341e-02 -6.21648171412619e-02 -6.20550670016750e-02 -6.18643565047460e-02 -6.17153693816304e-02 -6.16174841219989e-02 -6.15195988623674e-02 -6.14217136027359e-02 -6.13238283431044e-02 -6.12259430834729e-02 -6.11280578238414e-02 -6.10301725642099e-02 -6.09322873045784e-02 -6.08344020449470e-02 -6.07365167853155e-02 -6.06386315256840e-02 -6.05315858807928e-02 -6.04254998953099e-02 -6.03276146356784e-02 -6.02297293760469e-02 -6.01318441164154e-02 -6.00339588567839e-02 -5.99251683766052e-02 -5.98208272264098e-02 -5.97229419667783e-02 -5.96250567071469e-02 -5.95146086334450e-02 -5.93945639656616e-02 -5.92966787060302e-02 -5.91916396217197e-02 -5.90835470756561e-02 -5.89856618160246e-02 -5.88877765563931e-02 -5.87898912967616e-02 -5.86920060371301e-02 -5.85941207774986e-02 -5.84962355178671e-02 -5.83983502582356e-02 -5.82906939209939e-02 -5.81809437814070e-02 -5.80711936418202e-02 -5.79720869974874e-02 -5.78742017378559e-02 -5.77763164782245e-02 -5.76669153056951e-02 -5.75631848478504e-02 -5.74652995882189e-02 -5.73674143285874e-02 -5.72695290689559e-02 -5.71716438093244e-02 -5.70737585496929e-02 -5.69758732900614e-02 -5.68547381002233e-02 -5.66352378210497e-02 -5.65042661222781e-02 -5.64063808626466e-02 -5.63084956030151e-02 -5.62106103433836e-02 -5.61127250837521e-02 -5.60148398241206e-02 -5.59065727945282e-02 -5.57968226549414e-02 -5.56870725153546e-02 -5.54844753280290e-02 -5.52825978852596e-02 -5.50889865996650e-02 -5.49911013400335e-02 -5.48385154941374e-02 -5.46627342441374e-02 -5.46760822340871e-02 -5.46190915515075e-02 -5.45212062918760e-02 -5.44233210322446e-02 -5.43254357726131e-02 -5.42275505129816e-02 -5.41296652533501e-02 -5.40317799937186e-02 -5.39338947340871e-02 -5.38360094744556e-02 -5.37381242148241e-02 -5.36402389551926e-02 -5.35423536955611e-02 -5.34444684359297e-02 -5.33465831762982e-02 -5.32486979166667e-02
+ -2.00022786458333e-01 -1.90167520545436e-01 -1.79485922450796e-01 -1.68827945063861e-01 -1.67453102099037e-01 -1.66169225031407e-01 -1.65079858930067e-01 -1.64781753821189e-01 -1.63112976994347e-01 -1.60423258872487e-01 -1.56365469927764e-01 -1.52909174780151e-01 -1.45181866363065e-01 -1.26741618247487e-01 -1.19684032139866e-01 -1.15181277481156e-01 -1.11087893896566e-01 -1.00908170409338e-01 -9.12707908029732e-02 -8.31351909286014e-02 -8.23400367724037e-02 -8.01669316635260e-02 -7.66203740054439e-02 -7.45825808731156e-02 -7.31781629501675e-02 -7.21612129658710e-02 -7.07530000261725e-02 -6.97157826894891e-02 -6.89706507799414e-02 -6.86391756961893e-02 -6.69434575219849e-02 -6.39044342807789e-02 -5.85852602858040e-02 -4.57051697288527e-02 -3.54017646827889e-02 -3.33617468854690e-02 -2.92449388871441e-02 -2.39505306480318e-02 -1.74077909076633e-02 -1.19814731208124e-02 -7.36619294388611e-03 -3.59806846733668e-03 -1.97406302345059e-03 5.69416090871006e-04 4.26376675041875e-03 1.18476497068676e-02 1.75074591708543e-02 2.13608406616415e-02 2.18413682998325e-02 2.27947353957286e-02 2.38553444304858e-02 2.46673471524288e-02 2.62354742462312e-02 2.76493797110553e-02 2.83123298785595e-02 2.88183430171692e-02 2.95621008689280e-02 3.07055786746231e-02 3.25013740577889e-02 3.39288238065327e-02 3.48887831082496e-02 3.57007858301926e-02 3.62331677920854e-02 3.65460276643635e-02 3.70265553025544e-02 3.78753304281826e-02 3.88548536955611e-02 3.93020113588777e-02 4.04933848932161e-02 4.18474043394054e-02 4.26260370864740e-02 4.34046698335427e-02 4.40384376308626e-02 4.43365427397404e-02 4.50374430747487e-02 4.57726948544807e-02 4.64356450219849e-02 4.74028508427554e-02 4.80861828674623e-02 4.83842879763400e-02 4.91887333804439e-02 5.01737855946399e-02 5.12675355946399e-02 5.15656407035176e-02 5.19525197602596e-02 5.24435491258375e-02 5.32221818729062e-02 5.36792850973618e-02 5.45201430328727e-02 5.74544761568258e-02 5.87878847361809e-02 5.97331056323283e-02 6.11079485971524e-02 6.19961788107203e-02 6.25526067839196e-02 6.27016593383585e-02 6.36760789625209e-02 6.47034816007119e-02 6.56645368770938e-02 6.61627309725712e-02 6.83084203569933e-02 7.20674989530988e-02 7.44701371440536e-02 7.64262065536013e-02 7.80212586369347e-02 7.93827536118090e-02 8.01742108982412e-02 8.06547385364322e-02 8.11352661746231e-02 8.19620563756281e-02 8.29231116520100e-02 8.38841669283920e-02 8.47268569409548e-02 8.53688200115159e-02 8.56669251203936e-02 8.61233086002931e-02 8.67981672686348e-02 8.77592225450168e-02 8.80963247225712e-02 8.83944298314489e-02 8.86952176245812e-02 8.91757452627722e-02 8.94734741415410e-02 8.96912295854271e-02 9.06189148869347e-02 9.10599547215243e-02 9.12572301612228e-02 9.17043878245394e-02 9.23830905831240e-02 9.31188003821189e-02 9.37150105998744e-02 9.42219070613484e-02 9.47484656354690e-02 9.53780458280988e-02 9.57273345896147e-02 9.62091708542714e-02 9.69878036013400e-02 9.74924426821608e-02 9.81920343907035e-02 9.91530896670854e-02 1.01216090216709e-01 1.02814970032454e-01 1.04003906250000e-01 1.05332031250000e-01 1.06328844744556e-01 1.07387507197446e-01 1.10170563101968e-01 1.11474282218384e-01 1.12211856810092e-01 1.13471017195352e-01 1.14257992436139e-01 1.14939884971734e-01 1.15751887693677e-01 1.17082368221315e-01 1.18523951135888e-01 1.19965534050461e-01 1.21243473225502e-01 1.22085705218802e-01 1.22383810327680e-01 1.23325367069724e-01 1.23950072628769e-01 1.24285473591918e-01 1.25428951397613e-01 1.26766269498534e-01 1.28157339431533e-01 1.28902602203727e-01 1.29355239740368e-01 1.29960839353015e-01 1.31867377250837e-01 1.33061368038107e-01 1.33989053339615e-01 1.34916738641122e-01 1.36136836526382e-01 1.37193846838358e-01 1.37790057056114e-01 1.38444664729899e-01 1.39032499738275e-01 1.39513027376466e-01 1.41187905674204e-01 1.42388864897404e-01 1.43015157166039e-01 1.43495684804229e-01 1.43874270440745e-01 1.44218308338568e-01 1.44996941085636e-01 1.45471710767379e-01 1.45870645545435e-01 1.46764960872069e-01 1.48150796299204e-01 1.50078812028894e-01 1.53059863117672e-01 1.54530661118090e-01 1.55537452889447e-01 1.56465138190955e-01 1.58155621859296e-01 1.59499924753978e-01 1.60096134971734e-01 1.60416159573911e-01 1.62233285568467e-01 1.65810546875000e-01
+ -7.07400173611111e-02 -7.09941962416248e-02 -7.12598910350363e-02 -7.15010163665550e-02 -7.14564140145170e-02 -7.16003629257398e-02 -7.13614077331100e-02 -7.11106749022892e-02 -7.08661362367392e-02 -7.06269193188163e-02 -7.03708647403685e-02 -7.01258026242323e-02 -6.98755932439978e-02 -6.96366380513680e-02 -6.93976828587381e-02 -6.91587276661083e-02 -6.89197724734785e-02 -6.86571747627024e-02 -6.84071398659967e-02 -6.81681846733668e-02 -6.79228608319375e-02 -6.76729131769961e-02 -6.74339579843663e-02 -6.71417416945840e-02 -6.68063625418760e-02 -6.64132729620324e-02 -6.60310886027359e-02 -6.57675094221106e-02 -6.52730231016192e-02 -6.49756813581798e-02 -6.47205864391402e-02 -6.41641148450586e-02 -6.38961953866555e-02 -6.34686453098827e-02 -6.29972780569514e-02 -6.25089640912898e-02 -6.20813267727527e-02 -6.14411248953099e-02 -6.08975650823562e-02 -6.02499258445003e-02 -5.98896609785036e-02 -5.95480004187605e-02 -5.90697846873255e-02 -5.85935755164712e-02 -5.78464370463428e-02 -5.72527132188721e-02 -5.67748028336125e-02 -5.63211892797320e-02 -5.59390049204355e-02 -5.56870507049135e-02 -5.51737855946399e-02 -5.46022647962032e-02 -5.42323160943607e-02 -5.36117218034618e-02 -5.31177153126745e-02 -5.23429212032384e-02 -5.18220878699051e-02 -5.13288447445561e-02 -5.05697541527080e-02 -4.99065858807929e-02 -4.94430703866555e-02 -4.89571773799553e-02 -4.85035638260748e-02 -4.78879423157454e-02 -4.69539756072027e-02 -4.63571328866555e-02 -4.57761245463428e-02 -4.48244695700726e-02 -4.42053147682859e-02 -4.38340792504188e-02 -4.33561688651591e-02 -4.27330009422111e-02 -4.22223967057510e-02 -4.17444863204913e-02 -4.10886245463428e-02 -4.06107141610832e-02 -3.98502277010050e-02 -3.91210392238973e-02 -3.84232141610832e-02 -3.76660865089336e-02 -3.67514874720827e-02 -3.59160167155221e-02 -3.47977517797320e-02 -3.41485421901173e-02 -3.25638609715243e-02 -3.19419798646008e-02 -3.11436304787828e-02 -3.06309978713009e-02 -3.01248211543830e-02 -2.92782270728643e-02 -2.82684472710776e-02 -2.77905368858180e-02 -2.69995158082077e-02 -2.59767806044110e-02 -2.51068057300391e-02 -2.40041570700726e-02 -2.30155988274707e-02 -2.22408047180346e-02 -2.16274078726968e-02 -2.10419065815187e-02 -2.05639961962591e-02 -1.96216760887772e-02 -1.88315928601340e-02 -1.77023136515913e-02 -1.68129056742044e-02 -1.60381115647683e-02 -1.51130871370743e-02 -1.43839859017309e-02 -1.35010556253490e-02 -1.27262615159129e-02 -1.17735160175879e-02 -1.06206379117811e-02 -9.49103154662201e-03 -8.59224507956450e-03 -7.91045069095479e-03 -7.13565658151871e-03 -6.36086247208263e-03 -5.52312342964825e-03 -4.61388976130653e-03 -3.88389429787829e-03 -2.47609575656058e-03 -1.70130164712452e-03 -8.18000593244001e-04 2.77974071747617e-04 8.97172494416525e-04 1.55303426856504e-03 2.12896077610273e-03 3.53815518565045e-03 3.96932579564489e-03 4.82257206169738e-03 5.43998202819653e-03 5.91789241345617e-03 6.29191966778335e-03 7.16322497906197e-03 7.87937081239531e-03 8.43717284338358e-03 9.45859942071466e-03 1.01981260469012e-02 1.09765188791178e-02 1.22292233738135e-02 1.28970372696817e-02 1.34508043690676e-02 1.43349996510329e-02 1.51510809254606e-02 1.59583725921273e-02 1.68634404662200e-02 1.72876317350642e-02 1.76409390703517e-02 1.85236948632049e-02 1.90168071259073e-02 1.94947175111669e-02 2.02360544039642e-02 2.11037828029034e-02 2.16164154103852e-02 2.22266715522055e-02 2.30935711543830e-02 2.41561758445003e-02 2.48713402079844e-02 2.52060214265773e-02 2.55407026451703e-02 2.60186130304299e-02 2.64965234156895e-02 2.71135189838079e-02 2.75196948283082e-02 2.81429281825796e-02 2.87095416317699e-02 2.93451851270240e-02 3.01111023869347e-02 3.05549448632049e-02 3.11357787199888e-02 3.16484113274707e-02 3.23602168830262e-02 3.29774959868788e-02 3.33661580471803e-02 3.36886036083194e-02 3.42679979759911e-02 3.46026791945840e-02 3.52636882328308e-02 3.59255042573981e-02 3.63537086474037e-02 3.67790776800670e-02 3.72569880653266e-02 3.79916945840313e-02 3.85339893914014e-02 3.88722693327750e-02 3.94387082984366e-02 4.00663255513679e-02 4.04456309324400e-02 4.08306942699609e-02 4.12128786292574e-02 4.15929037548855e-02 4.28641471245114e-02 4.34149261934673e-02 4.37746021775544e-02 4.42021304438861e-02 4.45857542922948e-02 4.49211552554439e-02 4.54812910036293e-02 4.57851104480737e-02 4.61197916666667e-02
+ -1.99895833333333e-01 -1.83907885783082e-01 -1.79080738981365e-01 -1.33098238588777e-01 -1.21114131857203e-01 -9.12528953360553e-02 -6.07404372121022e-02 -4.68417936034338e-02 -4.09569003873534e-02 -3.76668008008794e-02 -3.53235088201424e-02 -3.40166162845477e-02 -3.23772999110134e-02 -3.14658088620184e-02 -3.06047326476131e-02 -2.89016043760469e-02 -2.79136568257956e-02 -2.65680289468174e-02 -2.51511954302764e-02 -2.27933940536013e-02 -2.13403770152848e-02 -2.06617396880235e-02 -1.96271396042714e-02 -1.86889852910385e-02 -1.73773326266750e-02 -1.63934091028057e-02 -1.56656819252513e-02 -1.44018922738693e-02 -1.36275452784757e-02 -1.26328910175879e-02 -1.16004501675042e-02 -9.96674452994138e-03 -8.79369176612228e-03 -7.63539376570352e-03 -6.88882236704355e-03 -5.86783199853434e-03 -4.78525439698493e-03 -3.53689018006700e-03 -1.66504724141542e-03 -2.20667137772261e-05 1.45233000942211e-03 2.27810602491624e-03 3.16545291561976e-03 3.91241690221943e-03 4.76619752407872e-03 5.66764486494975e-03 6.39537204250419e-03 7.05927096419598e-03 7.66269498534338e-03 8.63518438546901e-03 9.63227596314908e-03 1.04060013609715e-02 1.09342611494975e-02 1.15959681218593e-02 1.21779306689698e-02 1.27443369189698e-02 1.31725849298576e-02 1.36035810563233e-02 1.49739092598409e-02 1.55465805590452e-02 1.60562087782663e-02 1.66362738170017e-02 1.72242560458543e-02 1.75881196346315e-02 1.81409161693886e-02 1.86679491206030e-02 1.91975011777638e-02 2.02374175565327e-02 2.07374437290620e-02 2.14078367095896e-02 2.21355638871440e-02 2.26569697445561e-02 2.33726575586265e-02 2.39085891698074e-02 2.43445253611809e-02 2.49538872749162e-02 2.55291267535595e-02 2.60252270466918e-02 2.67505005496231e-02 2.71143641384003e-02 2.78698996283501e-02 2.84055367985762e-02 2.89579080297320e-02 2.93217716185092e-02 2.97116768739531e-02 3.00755404627303e-02 3.04394040515075e-02 3.08293093069514e-02 3.12143726444724e-02 3.15830781511725e-02 3.19469417399497e-02 3.23108053287270e-02 3.26746689175042e-02 3.32566314646147e-02 3.36204950533920e-02 3.42284992671692e-02 3.45923628559464e-02 3.49562264447236e-02 3.55381889918342e-02 3.59020525806114e-02 3.62659161693886e-02 3.66558214248325e-02 3.70457266802764e-02 3.74095902690536e-02 3.79915528161641e-02 3.83814580716080e-02 3.87673065850084e-02 3.91352269158291e-02 3.95298432265494e-02 3.99150374267169e-02 4.06754475502513e-02 4.17183901277219e-02 4.25330101025963e-02 4.31939973304020e-02 4.36169126884422e-02 4.42006745969430e-02 4.45819429177136e-02 4.50059542504188e-02 4.55886201842546e-02 4.59560170644891e-02 4.63198806532663e-02 4.69157964300670e-02 4.73285372173367e-02 4.80249227910385e-02 4.85826266750419e-02 4.89725319304858e-02 4.93879226863484e-02 5.01590962625628e-02 5.05524039468174e-02 5.11660352543970e-02 5.15503134160385e-02 5.19141770048157e-02 5.22780405935930e-02 5.28358916980737e-02 5.32238667294807e-02 5.35877303182580e-02 5.39515939070352e-02 5.43414991624791e-02 5.47053627512563e-02 5.50692263400335e-02 5.56511888871440e-02 5.60150524759213e-02 5.63789160646985e-02 5.67427796534757e-02 5.71129246492881e-02 5.75202346367253e-02 5.78864537531407e-02 5.82503173419179e-02 5.86379979323702e-02 5.90040861861390e-02 5.94227485081658e-02 5.99499123220268e-02 6.04680466132747e-02 6.11398790829146e-02 6.15037426716918e-02 6.25128408971943e-02 6.34977458909129e-02 6.38616094796901e-02 6.42254730684673e-02 6.45893366572445e-02 6.51570515337102e-02 6.56666797529313e-02 6.64204485971524e-02 6.69676998272613e-02 6.73331337677973e-02 6.78642888923785e-02 6.90561858773032e-02 6.99318859924623e-02 7.06854258270519e-02 7.13627381700167e-02 7.21055472675879e-02 7.27680884893216e-02 7.38767568310301e-02 7.42504841917923e-02 7.49595798000419e-02 7.56325082443467e-02 7.61333686662479e-02 7.69662766959799e-02 7.79069173994975e-02 7.85235094744556e-02 7.91712305014657e-02 7.98989576790201e-02 8.07222145885678e-02 8.13135992462311e-02 8.16774628350084e-02 8.20413264237856e-02 8.24051900125628e-02 8.27690536013400e-02 8.31329171901173e-02 8.34967807788945e-02 8.38606443676717e-02 8.50524268477805e-02 8.58109394629397e-02 8.61924695090033e-02 8.67744320561139e-02 8.71382956448911e-02 8.75021592336683e-02 8.78660228224456e-02 8.84968134945561e-02 8.88606770833333e-02
+ -7.05164930555556e-02 -7.02933722431602e-02 -7.03922389726410e-02 -7.06304307998325e-02 -7.08132459170854e-02 -7.08178370149358e-02 -7.10503581274428e-02 -7.13039045051647e-02 -7.14333494730598e-02 -7.12102286606644e-02 -7.09871078482691e-02 -7.07639870358738e-02 -7.05288268599944e-02 -7.02960222117532e-02 -7.00458564524009e-02 -7.02312888225852e-02 -7.04577902533501e-02 -7.06842916841150e-02 -7.09114910489950e-02 -7.11546556567560e-02 -7.10475663909827e-02 -7.08244455785874e-02 -7.05792525998046e-02 -7.03434817315745e-02 -7.01203609191792e-02 -6.98808386550810e-02 -6.95760595512284e-02 -6.92536794214126e-02 -6.86187556707147e-02 -6.81531463742323e-02 -6.77833721559185e-02 -6.75602513435232e-02 -6.73223866729481e-02 -6.69289917469291e-02 -6.65573854515634e-02 -6.60764216045505e-02 -6.56416958926577e-02 -6.52136005548576e-02 -6.43751417678671e-02 -6.40440592720547e-02 -6.36424418097432e-02 -6.32948051891401e-02 -6.28485635643495e-02 -6.25433918725572e-02 -6.20804652603294e-02 -6.14751164677554e-02 -6.10060175006979e-02 -6.05479109959520e-02 -5.98531175844500e-02 -5.88223343278894e-02 -5.81529718907035e-02 -5.19665493264936e-02 -4.28557828203517e-02 -3.98383955367113e-02 -3.20907815989671e-02 -3.08294837904802e-02 -2.71370197340871e-02 -2.44788504152708e-02 -2.31762218209101e-02 -2.16810724630095e-02 -2.00628904068956e-02 -1.84497029417923e-02 -1.74150592371580e-02 -1.64689441129257e-02 -1.57454917818258e-02 -1.38640795121441e-02 -1.26205354027080e-02 -1.17869403440815e-02 -1.08294619800391e-02 -1.02867745847292e-02 -9.19708132677275e-03 -8.80122182091010e-03 -8.07410533570630e-03 -7.05374746998883e-03 -6.18052283989390e-03 -4.81008558417085e-03 -3.90868186418202e-03 -2.91316609087102e-03 -1.93961343174204e-03 -9.31600275683963e-04 7.01914520519265e-04 1.88305895798437e-03 2.73796281756003e-03 3.73862585496929e-03 4.73928889237856e-03 5.66662522682859e-03 6.56266357830820e-03 7.59804883793970e-03 8.90469055346176e-03 9.51368168969850e-03 1.01736438267728e-02 1.08970961578727e-02 1.15642121196259e-02 1.20104537444165e-02 1.26346467580960e-02 1.32570513156058e-02 1.38025522578169e-02 1.45260045889168e-02 1.51544506386097e-02 1.62940898066723e-02 1.68789367846175e-02 1.75630430799832e-02 1.82554373429648e-02 1.88947449923227e-02 1.93803762737298e-02 2.03676913211893e-02 2.07549575132608e-02 2.12589095651870e-02 2.22364099141541e-02 2.30685436732272e-02 2.36999777533501e-02 2.42883798331938e-02 2.47468571154383e-02 2.51984641087381e-02 2.58377063267728e-02 2.63400007851759e-02 2.71561474909269e-02 2.77016484331379e-02 2.82793415864042e-02 2.89035346000837e-02 2.96499751360972e-02 3.01171984052205e-02 3.07431362541876e-02 3.12223552659129e-02 3.15548772508375e-02 3.20459175216360e-02 3.25554966673646e-02 3.29279535699330e-02 3.34366384875768e-02 3.42427523904244e-02 3.48064868613903e-02 3.51829132642379e-02 3.55391868195142e-02 3.59652537862926e-02 3.66628825551368e-02 3.71026464789224e-02 3.77248329320212e-02 3.85721685685371e-02 3.91918904417923e-02 3.96426032070073e-02 4.01062059429090e-02 4.06452946154383e-02 4.11959428217476e-02 4.16873320596036e-02 4.20577824015913e-02 4.22809032139866e-02 4.26472531930486e-02 4.30301572969012e-02 4.33973142622836e-02 4.37636642413456e-02 4.40825110797041e-02 4.45611412095198e-02 4.51182234959520e-02 4.54701785838917e-02 4.60419392971804e-02 4.64916705925461e-02 4.72222113170017e-02 4.76293250104690e-02 4.84193209973479e-02 4.88381578029034e-02 5.00059760608599e-02 5.06753384980458e-02 5.12014717685651e-02 5.20344779452820e-02 5.24316896984925e-02 5.29875069793412e-02 5.34337486041318e-02 5.37367610622557e-02 5.41830026870463e-02 5.47436836962591e-02 5.55051734366276e-02 5.60608162339475e-02 5.66841150195422e-02 5.72016113553881e-02 5.78551394123395e-02 5.82538778964266e-02 5.87310685371301e-02 5.89541893495254e-02 5.94025902079844e-02 5.99510355597432e-02 6.01964684533780e-02 6.04195892657733e-02 6.06516959798995e-02 6.12151687255723e-02 6.16614103503629e-02 6.19370725153546e-02 6.21601933277499e-02 6.25704477247348e-02 6.28928932858738e-02 6.32973897264098e-02 6.37688224106644e-02 6.43554142238973e-02 6.49448850153546e-02 6.53911266401452e-02 6.56941390982691e-02 6.61403807230597e-02 6.65866223478504e-02 6.69061889307649e-02 6.74157680764936e-02 6.77821180555556e-02
+ -1.98710937500000e-01 -1.91402356836265e-01 -1.85208725921273e-01 -1.80560910018844e-01 -1.72613392483250e-01 -1.56192044205402e-01 -1.51719682396357e-01 -1.48187699565536e-01 -1.40319239426298e-01 -1.34489930119347e-01 -1.33810425827052e-01 -1.32768595582077e-01 -1.31296259945561e-01 -1.30346000837521e-01 -1.27396896592337e-01 -1.21272279365578e-01 -1.20391115734925e-01 -1.18919777926089e-01 -1.16173069121650e-01 -8.34668295906617e-02 -3.49086742305276e-02 -2.76820462992043e-02 -2.51509991363065e-02 -2.40556787845477e-02 -2.25241605161223e-02 -2.10167536903266e-02 -2.01642489792714e-02 -1.93211336631072e-02 -1.79802986285595e-02 -1.67011816896985e-02 -1.57802358144891e-02 -1.51007315221943e-02 -1.43797601287688e-02 -1.33173190169598e-02 -1.22691092179648e-02 -1.15558914363484e-02 -1.04401892273869e-02 -9.74406537897822e-03 -9.00305564279732e-03 -8.48794755025126e-03 -8.14819540410385e-03 -7.61883964091290e-03 -7.25059215347571e-03 -6.91084000732831e-03 -6.57108786118090e-03 -6.23133571503350e-03 -5.89158356888610e-03 -5.55183142273869e-03 -5.19192642902010e-03 -4.60214483877722e-03 -4.26239269262981e-03 -3.91099377093802e-03 -3.99196503350084e-03 -4.32664625209380e-03 -4.66132747068677e-03 -4.99600868927973e-03 -5.35673157453936e-03 -5.69141279313233e-03 -6.03525439698492e-03 -6.41050303601340e-03 -6.77358144891122e-03 -7.13495864740369e-03 -7.52538735343384e-03 -7.90783343802345e-03 -8.24251465661642e-03 -8.57719587520938e-03 -8.91187709380235e-03 -9.48744372906198e-03 -9.82212494765494e-03 -1.01568061662479e-02 -1.04914873848409e-02 -1.08261686034338e-02 -1.11953975607203e-02 -1.17290881490787e-02 -1.20710976758794e-02 -1.24275020938023e-02 -1.28869608458961e-02 -1.33681100816583e-02 -1.37027913002513e-02 -1.40408749476549e-02 -1.44404640389447e-02 -1.48110016226968e-02 -1.51649196503350e-02 -1.55401682893635e-02 -1.59154169283920e-02 -1.62906655674204e-02 -1.66659142064489e-02 -1.73341315954774e-02 -1.79480409861809e-02 -1.82827222047739e-02 -1.86434450900335e-02 -1.89978865682580e-02 -1.95797346105528e-02 -1.99144158291457e-02 -2.02751387144054e-02 -2.08482843907035e-02 -2.11853865682580e-02 -2.15200677868509e-02 -2.18720228747906e-02 -2.22744909443049e-02 -2.28170801926298e-02 -2.32328635364322e-02 -2.37273280464824e-02 -2.40880509317420e-02 -2.46660385259632e-02 -2.52391842022613e-02 -2.56041274078727e-02 -2.61494320561139e-02 -2.67510403580402e-02 -2.70857215766332e-02 -2.77304818362647e-02 -2.82368548471524e-02 -2.86730200481575e-02 -2.93423824853434e-02 -2.99007406825796e-02 -3.07071490263819e-02 -3.13948322340871e-02 -3.18638766750419e-02 -3.23764983773032e-02 -3.30330362751256e-02 -3.35015899811558e-02 -3.41569173994975e-02 -3.46127447131491e-02 -3.51762065536013e-02 -3.57257315221943e-02 -3.62158775649079e-02 -3.68247814593802e-02 -3.71594626779732e-02 -3.77942773764657e-02 -3.84636398136516e-02 -3.90489230004188e-02 -3.93836042190117e-02 -3.97182854376047e-02 -4.00977543969849e-02 -4.07883165829146e-02 -4.11780582600503e-02 -4.17197314698493e-02 -4.23552331972362e-02 -4.29247474350921e-02 -4.33799204355109e-02 -4.40819331030151e-02 -4.45558848932161e-02 -4.52880614007538e-02 -4.62026604376047e-02 -4.66511267273869e-02 -4.71747408919598e-02 -4.75353329145729e-02 -4.81355998743719e-02 -4.87385495184255e-02 -4.93118260573702e-02 -4.98375994556114e-02 -5.02208634317421e-02 -5.07218056427973e-02 -5.11299008061139e-02 -5.16060118299832e-02 -5.21555367985762e-02 -5.24902180171692e-02 -5.29628938965662e-02 -5.34004658710218e-02 -5.37351470896148e-02 -5.40698283082077e-02 -5.44045095268007e-02 -5.47391907453936e-02 -5.50978198283082e-02 -5.54345948492462e-02 -5.57692760678392e-02 -5.61039572864322e-02 -5.64386385050251e-02 -5.67733197236181e-02 -5.71080009422111e-02 -5.74426821608040e-02 -5.77773633793970e-02 -5.83139329459799e-02 -5.86615695665829e-02 -5.89962507851759e-02 -5.93309320037689e-02 -5.96656132223618e-02 -6.00002944409548e-02 -6.03349756595477e-02 -6.08510325062814e-02 -6.12191818467337e-02 -6.17600698806533e-02 -6.22415462730318e-02 -6.29458817525125e-02 -6.34165946922111e-02 -6.38300879396985e-02 -6.41647691582915e-02 -6.47142941268844e-02 -6.50489753454774e-02 -6.53836565640704e-02 -6.59288630653266e-02 -6.62678627512563e-02 -6.66025439698493e-02 -6.69372251884422e-02 -6.72719064070352e-02 -6.76065876256281e-02 -6.79412688442211e-02 -6.82759500628141e-02 -6.86106312814070e-02 -6.89453125000000e-02
diff --git a/data/data_tpbatch_lqr_frames.txt b/data/data_tpbatch_lqr_frames.txt
deleted file mode 100644
index 522096031815fcc966785063e79fe01e2019fd04..0000000000000000000000000000000000000000
--- a/data/data_tpbatch_lqr_frames.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-ARMA_MAT_TXT_FN008
-4 8
- 1.66666671633720e-01 7.77777805924416e-02 1.66666671633720e-01 9.77777764201164e-02 1.66666671633720e-01 5.38888871669769e-02 1.66666671633720e-01 6.61111101508141e-02
- 4.58333343267441e-01 1.31666660308838e-01 4.58333343267441e-01 1.39166668057442e-01 4.58333343267441e-01 1.35000005364418e-01 4.58333343267441e-01 1.10833331942558e-01
- 0.00000000000000e+00 -5.55555569007993e-04 0.00000000000000e+00 -1.11111113801599e-03 0.00000000000000e+00 -1.11111113801599e-03 0.00000000000000e+00 -5.55555569007993e-04
- 2.50000003725290e-02 -2.08333339542150e-02 2.50000003725290e-02 -1.49999996647239e-02 2.50000003725290e-02 -1.16666667163372e-02 2.50000003725290e-02 -1.33333336561918e-02
diff --git a/data/data_tpbatch_lqr_trajectories.txt b/data/data_tpbatch_lqr_trajectories.txt
deleted file mode 100644
index 7f6a4588f891e665d9c85ebb6850a19df97eb92d..0000000000000000000000000000000000000000
--- a/data/data_tpbatch_lqr_trajectories.txt
+++ /dev/null
@@ -1,10 +0,0 @@
-ARMA_CUB_TXT_FN008
-2 200 4
- -2.14843750000000e-04 -2.77548768146287e-04 -3.40253786292575e-04 -4.02958804438861e-04 -4.65576580820769e-04 -5.23919510748186e-04 -5.82262440675600e-04 -6.40605370603014e-04 -6.98948300530431e-04 -7.57291230457844e-04 -8.15634160385258e-04 -8.73977090312675e-04 -9.43279766890005e-04 -1.18428514098269e-03 -1.42529051507538e-03 -1.66629588916806e-03 -1.91907890145170e-03 -2.30730475293132e-03 -2.69553060441094e-03 -3.08375645589056e-03 -3.46527559673367e-03 -3.78643434184814e-03 -4.10759308696258e-03 -4.42875183207706e-03 -4.74886367601897e-03 -5.06129824469569e-03 -5.37373281337242e-03 -5.68616738204914e-03 -5.97867811278614e-03 -6.14879955332217e-03 -6.31892099385817e-03 -6.48904243439419e-03 -6.69048366834169e-03 -7.05635381769961e-03 -7.42222396705750e-03 -7.78809411641542e-03 -8.18115098059742e-03 -8.69805843453378e-03 -9.21496588847014e-03 -9.73187334240647e-03 -1.02302419214126e-02 -1.06544550006979e-02 -1.10786680799833e-02 -1.15028811592686e-02 -1.19203766226968e-02 -1.23140550844500e-02 -1.27077335462032e-02 -1.31014120079564e-02 -1.35143272787549e-02 -1.39881591115299e-02 -1.44619909443049e-02 -1.49358227770798e-02 -1.53933513051368e-02 -1.58044781197655e-02 -1.62156049343942e-02 -1.66267317490229e-02 -1.70528205262423e-02 -1.75173829215522e-02 -1.79819453168621e-02 -1.84465077121720e-02 -1.89284094081519e-02 -1.94507694723618e-02 -1.99731295365718e-02 -2.04954896007817e-02 -2.11434778056951e-02 -2.20584258096036e-02 -2.29733738135120e-02 -2.38883218174204e-02 -2.46560711543830e-02 -2.51380819025684e-02 -2.56200926507538e-02 -2.61021033989391e-02 -2.66447689838079e-02 -2.72952653894472e-02 -2.79457617950867e-02 -2.85962582007258e-02 -2.93076711683417e-02 -3.01184743160244e-02 -3.09292774637075e-02 -3.17400806113903e-02 -3.25471759840872e-02 -3.33487096943047e-02 -3.41502434045225e-02 -3.49517771147403e-02 -3.57162112646567e-02 -3.64294126884422e-02 -3.71426141122278e-02 -3.78558155360133e-02 -3.86196389935789e-02 -3.94478904941372e-02 -4.02761419946956e-02 -4.11043934952542e-02 -4.18390891087380e-02 -4.24639582460917e-02 -4.30888273834450e-02 -4.37136965207983e-02 -4.44233646531267e-02 -4.52248983633444e-02 -4.60264320735622e-02 -4.68279657837800e-02 -4.75962385713289e-02 -4.83312504362089e-02 -4.90662623010889e-02 -4.98012741659686e-02 -5.05708773904244e-02 -5.13724111006422e-02 -5.21739448108597e-02 -5.29754785210775e-02 -5.37437404034058e-02 -5.44836596175322e-02 -5.52235788316583e-02 -5.59634980457844e-02 -5.66512030639308e-02 -5.72978826423786e-02 -5.79445622208264e-02 -5.85912417992742e-02 -5.95601815501117e-02 -6.07624821154383e-02 -6.19647826807650e-02 -6.31670832460917e-02 -6.41292508549692e-02 -6.49313298262144e-02 -6.57334087974594e-02 -6.65354877687047e-02 -6.73040986180906e-02 -6.80521967476269e-02 -6.88002948771636e-02 -6.95483930067003e-02 -7.02315832635400e-02 -7.08782628419878e-02 -7.15249424204356e-02 -7.21716219988833e-02 -7.27510054613345e-02 -7.32957212276661e-02 -7.38404369939978e-02 -7.43851527603294e-02 -7.47789620847292e-02 -7.51017566129258e-02 -7.54245511411222e-02 -7.57473456693189e-02 -7.61075451039922e-02 -7.64837752128700e-02 -7.68600053217475e-02 -7.72362354306253e-02 -7.75739919214125e-02 -7.78967864496092e-02 -7.82195809778056e-02 -7.85423755060022e-02 -7.88651700341989e-02 -7.91879645623953e-02 -7.95107590905919e-02 -7.98335536187883e-02 -8.01153227072864e-02 -8.03841363937745e-02 -8.06529500802625e-02 -8.09217637667503e-02 -8.11905774532383e-02 -8.14593911397264e-02 -8.17282048262145e-02 -8.19970185127025e-02 -8.23622343488275e-02 -8.27515507223617e-02 -8.31408670958961e-02 -8.35301834694305e-02 -8.37692695247069e-02 -8.39753781930486e-02 -8.41814868613903e-02 -8.43875955297319e-02 -8.45634749267170e-02 -8.47335963672531e-02 -8.49037178077889e-02 -8.50738392483250e-02 -8.52439606888611e-02 -8.54140821293969e-02 -8.55842035699331e-02 -8.57543250104689e-02 -8.59282850886378e-02 -8.61027686173925e-02 -8.62772521461475e-02 -8.64517356749022e-02 -8.66262192036572e-02 -8.68007027324119e-02 -8.69751862611669e-02 -8.71496697899219e-02 -8.72132908465942e-02 -8.72672716883025e-02 -8.73212525300111e-02 -8.73752333717197e-02 -8.74333145763539e-02 -8.74916575062814e-02 -8.75500004362089e-02 -8.76083433661361e-02 -8.76666862960636e-02 -8.77250292259911e-02 -8.77833721559183e-02 -8.78417150858458e-02 -8.78829586299553e-02 -8.79238532070072e-02 -8.79647477840592e-02 -8.80056423611111e-02
- -1.94430338541667e-01 -1.81771013269473e-01 -1.69111687997278e-01 -1.56452362725084e-01 -1.44025318650545e-01 -1.42980053261097e-01 -1.41934787871650e-01 -1.40889522482202e-01 -1.39838924439908e-01 -1.38660342729271e-01 -1.37481761018635e-01 -1.36303179307998e-01 -1.35124597597362e-01 -1.33946015886725e-01 -1.32767434176089e-01 -1.31588852465452e-01 -1.30335482752303e-01 -1.28222051010260e-01 -1.26108619268216e-01 -1.23995187526173e-01 -1.21947350685720e-01 -1.20489867959590e-01 -1.19032385233459e-01 -1.17574902507328e-01 -1.16134889944514e-01 -1.14822991912688e-01 -1.13511093880863e-01 -1.12199195849037e-01 -1.10941458595059e-01 -1.10016423262144e-01 -1.09091387929230e-01 -1.08166352596315e-01 -1.07215799047320e-01 -1.06131274863903e-01 -1.05046750680486e-01 -1.03962226497069e-01 -1.02845461029104e-01 -1.01581818598199e-01 -1.00318176167295e-01 -9.90545337363904e-02 -9.78267149549833e-02 -9.67421907715662e-02 -9.56576665881492e-02 -9.45731424047321e-02 -9.35429589353017e-02 -9.27054379972779e-02 -9.18679170592546e-02 -9.10303961212312e-02 -9.01624496178813e-02 -8.91981554909967e-02 -8.82338613641121e-02 -8.72695672372279e-02 -8.63052731103433e-02 -8.53409789834592e-02 -8.43766848565746e-02 -8.34123907296900e-02 -8.24705395466917e-02 -8.15863987908292e-02 -8.07022580349667e-02 -7.98181172791037e-02 -7.89641567211054e-02 -7.81806166247908e-02 -7.73970765284758e-02 -7.66135364321608e-02 -7.56096236390283e-02 -7.41374188651592e-02 -7.26652140912900e-02 -7.11930093174204e-02 -6.98823708385679e-02 -6.88853610500421e-02 -6.78883512615158e-02 -6.68913414729900e-02 -6.58345601706450e-02 -6.46715183992879e-02 -6.35084766279313e-02 -6.23454348565746e-02 -6.11671639447237e-02 -5.99640454878558e-02 -5.87609270309883e-02 -5.75578085741204e-02 -5.63550172738692e-02 -5.51527167085429e-02 -5.39504161432163e-02 -5.27481155778896e-02 -5.15623037060300e-02 -5.03992619346733e-02 -4.92362201633167e-02 -4.80731783919600e-02 -4.68284456134842e-02 -4.54797424623117e-02 -4.41310393111392e-02 -4.27823361599667e-02 -4.15799865211474e-02 -4.05494431794389e-02 -3.95188998377303e-02 -3.84883564960218e-02 -3.73753696869765e-02 -3.61730691216499e-02 -3.49707685563233e-02 -3.37684679909967e-02 -3.25158670958962e-02 -3.12129658710218e-02 -2.99100646461474e-02 -2.86071634212730e-02 -2.73565745393635e-02 -2.61542739740369e-02 -2.49519734087103e-02 -2.37496728433836e-02 -2.24895146304439e-02 -2.11800702732412e-02 -1.98706259160385e-02 -1.85611815588358e-02 -1.73296004763400e-02 -1.61591976811139e-02 -1.49887948858878e-02 -1.38183920906617e-02 -1.22808214248325e-02 -1.04773705768426e-02 -8.67391972885258e-03 -6.87046888086267e-03 -5.41936570875212e-03 -4.20316098722783e-03 -2.98695626570354e-03 -1.77075154417924e-03 -5.62660306742058e-04 6.40458150125613e-04 1.84357660699330e-03 3.04669506386096e-03 4.22887549727804e-03 5.39927829250417e-03 6.56968108773029e-03 7.74008388295646e-03 8.73450913421271e-03 9.63827928706029e-03 1.05420494399079e-02 1.14458195927554e-02 1.22822936296064e-02 1.30870989059883e-02 1.38919041823702e-02 1.46967094587521e-02 1.53893000157035e-02 1.60337985500419e-02 1.66782970843802e-02 1.73227956187186e-02 1.79672941530570e-02 1.86117926873953e-02 1.92562912217337e-02 1.99007897560720e-02 2.06046017849665e-02 2.13292536903266e-02 2.20539055956868e-02 2.27785575010469e-02 2.35641259683836e-02 2.43689312447655e-02 2.51737365211474e-02 2.59785417975293e-02 2.67833470739112e-02 2.75881523502931e-02 2.83929576266750e-02 2.91977629030569e-02 3.04206743352177e-02 3.17481123063233e-02 3.30755502774288e-02 3.44029882485343e-02 3.52146801716917e-02 3.59131595477387e-02 3.66116389237856e-02 3.73101182998325e-02 3.81989046796482e-02 3.91239400125628e-02 4.00489753454774e-02 4.09740106783920e-02 4.20136980475292e-02 4.30720497016333e-02 4.41304013557371e-02 4.51887530098408e-02 4.62355887510471e-02 4.72808541404942e-02 4.83261195299413e-02 4.93713849193887e-02 5.04166503088358e-02 5.14619156982829e-02 5.25071810877304e-02 5.35524464771775e-02 5.46097512301087e-02 5.56681028842129e-02 5.67264545383167e-02 5.77848061924204e-02 5.89561740996650e-02 6.01347558103017e-02 6.13133375209379e-02 6.24919192315746e-02 6.35425172738692e-02 6.45877826633167e-02 6.56330480527638e-02 6.66783134422108e-02 7.62038054857617e-02 8.59023633793967e-02 9.56009212730317e-02 1.05299479166667e-01
- -1.99652777777778e-04 -1.07394611948632e-04 -1.51364461194863e-05 -2.05279871580123e-04 -4.32850013958683e-04 -5.70124930206589e-04 -6.79809638470128e-04 -7.94816094360692e-04 -9.12701528475711e-04 -1.02186278615299e-03 -1.12334676856505e-03 -1.40601008514796e-03 -1.94418271915131e-03 -2.44510311976549e-03 -2.85821468453378e-03 -3.33658308905639e-03 -4.11052659128978e-03 -4.84900631630375e-03 -5.06735064209939e-03 -5.28569496789503e-03 -5.99126273729761e-03 -6.72420261027358e-03 -7.51073073701842e-03 -8.30825132607483e-03 -8.91507232342267e-03 -9.44504423157453e-03 -1.02186823876326e-02 -1.11576654976270e-02 -1.20853617043551e-02 -1.30007677275265e-02 -1.41204721524288e-02 -1.56007031686209e-02 -1.68672354829704e-02 -1.74740891959799e-02 -1.80809429089894e-02 -1.86877966219989e-02 -1.92957190466220e-02 -2.00030316513121e-02 -2.07103442560022e-02 -2.13278850851480e-02 -2.19347387981574e-02 -2.33074225293132e-02 -2.49004135259631e-02 -2.57091010608599e-02 -2.61129868090452e-02 -2.67910625174484e-02 -2.77003179962312e-02 -2.84808918725572e-02 -2.90877455855667e-02 -2.96945992985761e-02 -3.03014530115858e-02 -3.09083067245953e-02 -3.15151604376047e-02 -3.21496479794808e-02 -3.31275845372697e-02 -3.41055210950586e-02 -3.46152965347572e-02 -3.51042648136517e-02 -3.56923070212172e-02 -3.62991607342267e-02 -3.68207356225572e-02 -3.73097039014517e-02 -3.77837538386375e-02 -3.82481199399778e-02 -3.87044597989950e-02 -3.91524244486319e-02 -3.95343579878558e-02 -3.98049819409547e-02 -4.01661628454775e-02 -4.07914681916528e-02 -4.13677218558069e-02 -4.16383458089056e-02 -4.19089697620044e-02 -4.21795937151033e-02 -4.24502176682022e-02 -4.27968510085147e-02 -4.31515324015914e-02 -4.34206732446958e-02 -4.36666950202400e-02 -4.41847802379956e-02 -4.48367379431881e-02 -4.52216049867392e-02 -4.53907449574261e-02 -4.56010194200167e-02 -4.58644677379956e-02 -4.61249716464267e-02 -4.63791941478225e-02 -4.66195343034617e-02 -4.68050757258514e-02 -4.69978582146844e-02 -4.72684821677833e-02 -4.75391061208822e-02 -4.77045383165831e-02 -4.78665026521497e-02 -4.81593296342825e-02 -4.84750575795644e-02 -4.87396182300392e-02 -4.89856400055833e-02 -4.91374188651592e-02 -4.92307021217197e-02 -4.94546844465383e-02 -4.98093658396147e-02 -5.01707212276661e-02 -5.05428291631769e-02 -5.09307496684814e-02 -5.13623128664153e-02 -5.17547917539083e-02 -5.19239317245953e-02 -5.20930716952819e-02 -5.22622116659686e-02 -5.24313516366556e-02 -5.26783330716081e-02 -5.29325555730039e-02 -5.31867780743997e-02 -5.34410005757956e-02 -5.37482660699331e-02 -5.40803954669181e-02 -5.43639748220267e-02 -5.46099965975711e-02 -5.47804451947236e-02 -5.48573269995811e-02 -5.49592253803742e-02 -5.51119638993578e-02 -5.52862947550250e-02 -5.55405172564211e-02 -5.57931694060581e-02 -5.60309904557511e-02 -5.62688115054439e-02 -5.63571437918761e-02 -5.64422263225853e-02 -5.68126221384700e-02 -5.72288089754328e-02 -5.75243840731436e-02 -5.77786065745394e-02 -5.79897207391122e-02 -5.81752621615019e-02 -5.83916871684814e-02 -5.86377089440256e-02 -5.88538722257119e-02 -5.90240372871300e-02 -5.91942023485483e-02 -5.93643674099664e-02 -5.95332238449192e-02 -5.96951881804858e-02 -5.98607730492742e-02 -6.01078199155500e-02 -6.03548667818258e-02 -6.06095036816025e-02 -6.08647512737297e-02 -6.10445783605528e-02 -6.12065426961194e-02 -6.13096733668342e-02 -6.13865551716917e-02 -6.15517692629817e-02 -6.17824146775544e-02 -6.19816421517308e-02 -6.21436064872975e-02 -6.22793982935511e-02 -6.23644808242603e-02 -6.24495633549692e-02 -6.25346458856783e-02 -6.26270349141542e-02 -6.27807985238694e-02 -6.29345621335847e-02 -6.30203753140703e-02 -6.31054578447794e-02 -6.33103669388611e-02 -6.35328116275822e-02 -6.38133266157175e-02 -6.41126531092964e-02 -6.42699936313511e-02 -6.43468754362089e-02 -6.44681087730319e-02 -6.46300731085986e-02 -6.47576423785594e-02 -6.48345241834169e-02 -6.49090504606364e-02 -6.49777315396428e-02 -6.50856714126186e-02 -6.53849979061978e-02 -6.56770179020100e-02 -6.58389822375767e-02 -6.60009465731436e-02 -6.62348853643217e-02 -6.64737315047458e-02 -6.67125776451703e-02 -6.69514237855947e-02 -6.70765502861531e-02 -6.71534320910106e-02 -6.72303138958683e-02 -6.73071957007258e-02 -6.73802388679508e-02 -6.74489199469569e-02 -6.75233589824119e-02 -6.76084415131211e-02 -6.76916047250139e-02 -6.77684865298717e-02 -6.78453683347292e-02 -6.79222501395869e-02 -6.79991319444444e-02
- -1.92304687500000e-01 -1.89469286536851e-01 -1.86633885573702e-01 -1.70560910018844e-01 -1.52733557108459e-01 -1.44218782715662e-01 -1.38549518425461e-01 -1.33992831998534e-01 -1.30038031956658e-01 -1.26336778292504e-01 -1.22858645440745e-01 -1.19590399916248e-01 -1.16618149340452e-01 -1.14013230213568e-01 -1.12274163787688e-01 -1.10134379580193e-01 -1.06179579538317e-01 -1.02414497618300e-01 -1.01431948152220e-01 -1.00449398686139e-01 -9.75378520205192e-02 -9.45179347257121e-02 -9.23120485238692e-02 -9.02731430590454e-02 -8.84479526538946e-02 -8.67088862280150e-02 -8.47325658239113e-02 -8.25952516488692e-02 -8.05050480265912e-02 -7.84661425617671e-02 -7.60256850659546e-02 -7.28766063389867e-02 -7.01278364478642e-02 -6.86148025282663e-02 -6.71017686086683e-02 -6.55887346890704e-02 -6.40789069043133e-02 -6.28672496597571e-02 -6.16555924152008e-02 -6.01746198440117e-02 -5.86615859244137e-02 -5.58827830559046e-02 -5.27398548733250e-02 -5.12703327837104e-02 -5.06645041614321e-02 -4.93195305956867e-02 -4.73513563913317e-02 -4.57051042975292e-02 -4.44934470529733e-02 -4.32353008532246e-02 -4.18729552711475e-02 -4.05106096890704e-02 -3.91482641069933e-02 -3.77030170383166e-02 -3.52274229219012e-02 -3.27518288054858e-02 -3.15334975659548e-02 -3.03710446765075e-02 -2.91672391907454e-02 -2.79555819461893e-02 -2.67795193414992e-02 -2.56170664520519e-02 -2.41469227648660e-02 -2.24770499633585e-02 -2.12338221053183e-02 -2.04357889708962e-02 -1.95862286693886e-02 -1.86498082862228e-02 -1.74617390337102e-02 -1.55396939122697e-02 -1.37539585950586e-02 -1.28175382118928e-02 -1.18811178287270e-02 -1.09446974455611e-02 -1.00082770623953e-02 -9.07185667922950e-03 -8.13543629606367e-03 -7.31764290201004e-03 -6.53191085636517e-03 -4.73006307579567e-03 -2.42822183835848e-03 -1.18963960427135e-03 -8.09843488274704e-04 -2.16086945142382e-05 1.29460780464824e-03 2.43317826109715e-03 3.19277049309045e-03 3.95764630443886e-03 4.74337835008375e-03 5.54193493509212e-03 6.47835531825796e-03 7.41477570142379e-03 8.03413224979063e-03 8.64303614426300e-03 9.64191072550250e-03 1.07090301769263e-02 1.18867612803601e-02 1.31045690693048e-02 1.41487548419179e-02 1.50851752250837e-02 1.60215956082496e-02 1.69580159914154e-02 1.81753330454355e-02 1.98452058469430e-02 2.15220307265494e-02 2.32180433417085e-02 2.47745890912898e-02 2.55341813232831e-02 2.62889643530151e-02 2.68978682474875e-02 2.75067721419598e-02 2.82535398345896e-02 2.90131320665829e-02 2.97727242985762e-02 3.05323165305695e-02 3.17242789468174e-02 3.31189148869347e-02 3.40705316949330e-02 3.46794355894053e-02 3.52210106522194e-02 3.56792262091708e-02 3.61871368561558e-02 3.67960407506281e-02 3.75178136777638e-02 3.86572020257537e-02 3.97457993090452e-02 4.03547032035176e-02 4.09636070979900e-02 4.17199931951425e-02 4.24795854271358e-02 4.37863961735762e-02 4.51810321136933e-02 4.61027631647821e-02 4.68623553967754e-02 4.77329355108879e-02 4.86693558940538e-02 4.95288290410383e-02 5.03145610866833e-02 5.11002931323283e-02 5.18860251779733e-02 5.26645270623954e-02 5.34241192943887e-02 5.41596655150754e-02 5.47685694095479e-02 5.54195456448913e-02 5.70171495498325e-02 5.86147534547737e-02 6.01397776643633e-02 6.16589621283500e-02 6.24421750680487e-02 6.30510789625208e-02 6.36599828569933e-02 6.42688867514658e-02 6.51551540253350e-02 6.62468756543133e-02 6.71177338515496e-02 6.77266377460217e-02 6.83355416404942e-02 6.89444455349667e-02 6.95870138452679e-02 7.03466060772613e-02 7.10741369608458e-02 7.15323525177975e-02 7.19905680747488e-02 7.25978689018008e-02 7.32067727962729e-02 7.37740950847992e-02 7.43353322602596e-02 7.50696189279733e-02 7.58599638819096e-02 7.67464928810721e-02 7.76875261725292e-02 7.84554281825796e-02 7.90643320770521e-02 7.96123194095479e-02 8.00705349664992e-02 8.04925342860133e-02 8.08246636829983e-02 8.12737842860133e-02 8.22932370184254e-02 8.33030386306533e-02 8.41410503036013e-02 8.49790619765496e-02 8.58170736494975e-02 8.66550853224454e-02 8.80561171744138e-02 8.95814521827887e-02 9.02690045278475e-02 9.06011339248325e-02 9.18995203884004e-02 9.38830709537271e-02 9.51641508322862e-02 9.56469685667921e-02 9.65740486285596e-02 9.83223408710217e-02 9.97687002721942e-02 1.00226915829146e-01 1.00808387641332e-01 1.02319883924833e-01 1.03831380208333e-01
- 3.40711805555556e-04 2.42891977247348e-04 1.45072148939140e-04 -3.13743195142378e-05 -3.04419231574539e-04 -5.77464143634842e-04 -7.81042800809603e-04 -9.76682457426020e-04 -1.23945465173088e-03 -1.62903275055835e-03 -2.01861084938582e-03 -2.17627852805695e-03 -2.27409835636516e-03 -2.49064332077052e-03 -3.07586107621442e-03 -3.66107883165828e-03 -4.33227334589614e-03 -5.04167975293133e-03 -5.71497897473478e-03 -6.16835261376325e-03 -6.62172625279172e-03 -7.53610718523172e-03 -8.77118884352317e-03 -9.97568135817978e-03 -1.04154452819654e-02 -1.08552092057510e-02 -1.12850275683976e-02 -1.17043769193188e-02 -1.21237262702401e-02 -1.26999035978504e-02 -1.32868225676996e-02 -1.39853019437465e-02 -1.48622779697097e-02 -1.57392539956728e-02 -1.71804661327471e-02 -1.87345254920436e-02 -2.01252028371022e-02 -2.10999986913735e-02 -2.20747945456449e-02 -2.27860330297320e-02 -2.34001714300670e-02 -2.40566002756840e-02 -2.49063568711614e-02 -2.57561134666387e-02 -2.66844203657175e-02 -2.76592162199888e-02 -2.86821040968733e-02 -3.02820962451144e-02 -3.18820883933556e-02 -3.38616803636236e-02 -3.61838380269403e-02 -3.85059956902569e-02 -3.96098221140425e-02 -4.06815871894194e-02 -4.17123268250978e-02 -4.26871226793692e-02 -4.36619185336403e-02 -4.46063106330261e-02 -4.55462315919878e-02 -4.64861525509492e-02 -4.74260735099106e-02 -4.83659944688722e-02 -4.93327422703797e-02 -5.03075381246511e-02 -5.12823339789225e-02 -5.22571298331936e-02 -5.32319256874650e-02 -5.53329254780850e-02 -5.79970272368789e-02 -6.04099708612506e-02 -6.08973687883864e-02 -6.13847667155219e-02 -6.24767500697933e-02 -6.40359130723058e-02 -6.55875841883025e-02 -6.65623800425739e-02 -6.75371758968453e-02 -6.90223360727247e-02 -7.11029212904803e-02 -7.31835065082356e-02 -7.41119769856225e-02 -7.49268586857900e-02 -7.57444885015355e-02 -7.65670256665272e-02 -7.73895628315186e-02 -7.82120999965103e-02 -7.90346371615019e-02 -7.97701506665272e-02 -8.02532955576494e-02 -8.07364404487717e-02 -8.12237838498044e-02 -8.17128829913456e-02 -8.22042067978783e-02 -8.27077662618650e-02 -8.32113257258514e-02 -8.40859680346175e-02 -8.52053671133444e-02 -8.62824539363483e-02 -8.65767640284758e-02 -8.68710741206031e-02 -8.71972819828308e-02 -8.75553876151592e-02 -8.79134932474875e-02 -8.82715988798158e-02 -8.86297045121442e-02 -8.89293254466778e-02 -8.91402760329425e-02 -8.93512266192072e-02 -8.96982089265772e-02 -9.00699242741486e-02 -9.04682265494136e-02 -9.09301062604689e-02 -9.13919859715242e-02 -9.20986442629817e-02 -9.28897089614739e-02 -9.36336631072028e-02 -9.41797529313233e-02 -9.47258427554439e-02 -9.50701859994417e-02 -9.53015511585706e-02 -9.55373438372417e-02 -9.58180442141261e-02 -9.60987445910105e-02 -9.66776155080961e-02 -9.75120611739253e-02 -9.83465068397544e-02 -9.86208712835008e-02 -9.88879619451425e-02 -9.91609850467617e-02 -9.94416854236461e-02 -9.97223858005306e-02 -9.99910468139308e-02 -1.00258137475572e-01 -1.00534301280709e-01 -1.00828611372836e-01 -1.01122921464964e-01 -1.01286848740229e-01 -1.01414439820631e-01 -1.01545171604551e-01 -1.01686372400195e-01 -1.01827573195840e-01 -1.02325963585287e-01 -1.02992839632189e-01 -1.03614677118230e-01 -1.03930252390424e-01 -1.04245827662619e-01 -1.04420071276521e-01 -1.04490671674344e-01 -1.04565045278476e-01 -1.04782800722362e-01 -1.05000556166248e-01 -1.05291343872139e-01 -1.05663059219710e-01 -1.06034774567281e-01 -1.06193707251536e-01 -1.06334908047180e-01 -1.06449859976968e-01 -1.06520460374791e-01 -1.06591060772613e-01 -1.06881510416667e-01 -1.07220052083333e-01 -1.07484623639028e-01 -1.07548419179229e-01 -1.07612214719431e-01 -1.07676010259631e-01 -1.07739805799832e-01 -1.07802467197097e-01 -1.07859457879676e-01 -1.07916448562256e-01 -1.08016885643495e-01 -1.08144476723897e-01 -1.08276157262004e-01 -1.08467543882608e-01 -1.08658930503211e-01 -1.08784885800530e-01 -1.08848681340731e-01 -1.08912476880932e-01 -1.09044157419040e-01 -1.09178553357063e-01 -1.09283985029313e-01 -1.09347780569514e-01 -1.09411576109715e-01 -1.09481216848129e-01 -1.09551817245952e-01 -1.09622417643774e-01 -1.09693018041597e-01 -1.09763618439419e-01 -1.09823924309045e-01 -1.09880914991625e-01 -1.09940697410664e-01 -1.10011297808487e-01 -1.10081898206309e-01 -1.10152498604132e-01 -1.10223099001954e-01 -1.10293699399777e-01 -1.10364299797599e-01 -1.10434900195422e-01 -1.10497997801507e-01 -1.10554988484087e-01 -1.10611979166667e-01
- -1.90696614583333e-01 -1.80070829407454e-01 -1.69445044231575e-01 -1.59639686060511e-01 -1.50842281066792e-01 -1.42044876073074e-01 -1.39305920880444e-01 -1.37259359950796e-01 -1.35206174099665e-01 -1.33140474507957e-01 -1.31074774916248e-01 -1.28843763086265e-01 -1.26570090033501e-01 -1.24351739164573e-01 -1.22305178234925e-01 -1.20258617305276e-01 -1.18162590295226e-01 -1.16044578360553e-01 -1.13995302030988e-01 -1.12364688023450e-01 -1.10734074015913e-01 -1.08054530464824e-01 -1.04645296796483e-01 -1.01311685380025e-01 -9.98686302606783e-02 -9.84255751413317e-02 -9.71348114269263e-02 -9.60043544545646e-02 -9.48738974822025e-02 -9.37434405098408e-02 -9.26129835374792e-02 -9.12989917033083e-02 -8.96913440902429e-02 -8.80836964771775e-02 -8.56647822445562e-02 -8.30836146880233e-02 -8.08180551193467e-02 -7.93558613379396e-02 -7.78936675565325e-02 -7.65452261306533e-02 -7.52386934673367e-02 -7.39136110238692e-02 -7.25037295854271e-02 -7.10938481469850e-02 -6.95597289834587e-02 -6.79520813703938e-02 -6.62834844796900e-02 -6.38834962573283e-02 -6.14835080349667e-02 -5.85905765808208e-02 -5.52527939175042e-02 -5.19150112541875e-02 -5.04047254501675e-02 -4.89425316687604e-02 -4.74803378873533e-02 -4.60181441059462e-02 -4.45559503245396e-02 -4.32572694200167e-02 -4.19826345268008e-02 -4.07079996335846e-02 -3.94333647403685e-02 -3.81587298471524e-02 -3.67398188860972e-02 -3.52776251046901e-02 -3.38154313232831e-02 -3.23532375418760e-02 -3.08910437604690e-02 -2.80449120603015e-02 -2.45068114007538e-02 -2.12925957914573e-02 -2.05614989007538e-02 -1.98304020100502e-02 -1.83572484819933e-02 -1.63106875523450e-02 -1.42716185092127e-02 -1.28094247278057e-02 -1.13472309463987e-02 -9.11006857202683e-03 -5.96877617252933e-03 -2.82748377303185e-03 -1.43294597989951e-03 -2.10623429648250e-04 9.70935406197650e-04 2.07970189489112e-03 3.18846838358458e-03 4.29723487227804e-03 5.40600136097150e-03 6.44835505653267e-03 7.29811165201004e-03 8.14786824748742e-03 9.07859610552762e-03 1.00431846733668e-02 1.10140546482412e-02 1.20194723618090e-02 1.30248900753769e-02 1.46376740473199e-02 1.66510612960637e-02 1.86010455925460e-02 1.93780752721943e-02 2.01551049518425e-02 2.08121990159129e-02 2.13493574644054e-02 2.18865159128978e-02 2.24236743613903e-02 2.29608328098828e-02 2.35350090295226e-02 2.41653089667085e-02 2.47956089038945e-02 2.54528992619347e-02 2.61150969692211e-02 2.67494536484506e-02 2.73172339562395e-02 2.78850142640285e-02 2.91700527376465e-02 3.07024216132747e-02 3.21862077313651e-02 3.34659462677973e-02 3.47456848042295e-02 3.54275446241625e-02 3.57745923628560e-02 3.61470601706449e-02 3.67773601078308e-02 3.74076600450167e-02 3.86652173628559e-02 4.04604238641122e-02 4.22556303653683e-02 4.27774288107204e-02 4.32826894891121e-02 4.38424544597992e-02 4.44727543969850e-02 4.51030543341708e-02 4.56227426193467e-02 4.61280032977388e-02 4.67238536432162e-02 4.75008833228642e-02 4.82779130025125e-02 4.87605835165413e-02 4.91612195090033e-02 4.96148548733250e-02 5.02451548105108e-02 5.08754547476967e-02 5.22903580402008e-02 5.40753572550250e-02 5.57203334380233e-02 5.64131530046062e-02 5.71059725711892e-02 5.75809058312396e-02 5.78960557998325e-02 5.82198427030988e-02 5.88718331239529e-02 5.95238235448075e-02 6.04542242462313e-02 6.16931336369346e-02 6.29320430276383e-02 6.36091590766333e-02 6.42394590138192e-02 6.48232699958125e-02 6.53285306742042e-02 6.58337913525962e-02 6.68740185301508e-02 6.80312696293971e-02 6.90305040829146e-02 6.96008362123117e-02 7.01711683417083e-02 7.05298955716079e-02 7.08054923052763e-02 7.10494039206450e-02 7.11348899445142e-02 7.12203759683838e-02 7.19277212887354e-02 7.30237286693887e-02 7.40982255025125e-02 7.48586683417083e-02 7.56191111809046e-02 7.61845686767171e-02 7.65647900963150e-02 7.69450115159129e-02 7.74454629920438e-02 7.79507236704354e-02 7.86721694409546e-02 7.97043812814071e-02 8.07365931218592e-02 8.22170586002929e-02 8.37711179595896e-02 8.54739681480317e-02 8.75115977020521e-02 8.95492272560721e-02 9.02540371126467e-02 9.05296338463150e-02 9.13111455716079e-02 9.40530778894471e-02 9.67950102072862e-02 9.92366127512562e-02 1.01519217179648e-01 1.03600031407035e-01 1.03915181375628e-01 1.04230331344221e-01 1.04675607856993e-01 1.05226801324330e-01 1.05777994791667e-01
- 1.12847222222222e-04 2.08791352596315e-04 3.04735482970408e-04 2.75814838079285e-04 1.64483441513121e-04 1.30862646566331e-07 -3.70068659268564e-04 -7.40268181183697e-04 -1.08051106225572e-03 -1.41360011864880e-03 -1.74668917504188e-03 -2.07977823143495e-03 -2.41367427414852e-03 -2.81374319514239e-03 -3.21381211613622e-03 -3.83189820630933e-03 -4.58677938302625e-03 -5.58074661501953e-03 -7.43808015773311e-03 -9.29541370044667e-03 -1.03229472012842e-02 -1.11674365752373e-02 -1.20768120114461e-02 -1.30751740473199e-02 -1.40712677973199e-02 -1.49754959694305e-02 -1.58797241415410e-02 -1.66586295191234e-02 -1.73628232307370e-02 -1.81194383375209e-02 -1.90526307754048e-02 -1.99858232132887e-02 -2.19032336159966e-02 -2.40203404173646e-02 -2.57326890529034e-02 -2.69166034163875e-02 -2.81063738833053e-02 -2.94523071084589e-02 -3.07982403336125e-02 -3.21441735587661e-02 -3.34901067839197e-02 -3.47969993195142e-02 -3.59809136829983e-02 -3.71648280464825e-02 -3.83426354864600e-02 -3.95193087835008e-02 -4.08351872208264e-02 -4.23241315082356e-02 -4.38252896426578e-02 -4.55676712555833e-02 -4.73100528685092e-02 -4.85695513156058e-02 -4.95697236180906e-02 -5.05382925914294e-02 -5.14135564977667e-02 -5.22888204041039e-02 -5.31640843104411e-02 -5.40393482167783e-02 -5.48976217895031e-02 -5.57357752303183e-02 -5.66045178147683e-02 -5.79504510399219e-02 -5.92963842650753e-02 -5.99867501570353e-02 -6.03433726793692e-02 -6.07199081344222e-02 -6.11516567211056e-02 -6.15834053077889e-02 -6.21823963567839e-02 -6.28069383375208e-02 -6.35573701842547e-02 -6.44498316233947e-02 -6.53165131211614e-02 -6.58523520379675e-02 -6.63881909547739e-02 -6.73301402847572e-02 -6.84678928496650e-02 -6.94385992462311e-02 -6.99735330297319e-02 -7.05084668132328e-02 -7.10306632991344e-02 -7.15511149497489e-02 -7.20789821503350e-02 -7.26148210671414e-02 -7.31418812814069e-02 -7.35736298680906e-02 -7.40053784547739e-02 -7.45745001046900e-02 -7.52062831518706e-02 -7.57781311069236e-02 -7.62026386271636e-02 -7.66271461474036e-02 -7.76899798471525e-02 -7.88304478119767e-02 -7.96029409198772e-02 -7.99984841743439e-02 -8.03846925600222e-02 -8.06833865508097e-02 -8.09820805415969e-02 -8.12605344430486e-02 -8.15302641680625e-02 -8.18043559812953e-02 -8.20885678391958e-02 -8.23727796970967e-02 -8.29203526312117e-02 -8.34960174134561e-02 -8.38866424134561e-02 -8.40966333403128e-02 -8.43178130234506e-02 -8.46309891471245e-02 -8.49441652707983e-02 -8.53055969953936e-02 -8.56866581169736e-02 -8.59931493404522e-02 -8.61361604027081e-02 -8.62791714649636e-02 -8.64835134875767e-02 -8.66935044144333e-02 -8.70938786816025e-02 -8.76713537304578e-02 -8.82054260015356e-02 -8.84226579948353e-02 -8.86398899881350e-02 -8.87975794772472e-02 -8.89324443397544e-02 -8.90893922738694e-02 -8.92921421342825e-02 -8.94948919946958e-02 -8.96355039084311e-02 -8.97712739042433e-02 -9.00447550251256e-02 -9.04402982795925e-02 -9.08023734122000e-02 -9.09453844744555e-02 -9.10883955367114e-02 -9.14249851689000e-02 -9.18313900230320e-02 -9.21235954075936e-02 -9.21914804054997e-02 -9.22593654034058e-02 -9.23272504013122e-02 -9.23951353992183e-02 -9.24958996370742e-02 -9.26244285664433e-02 -9.27626413316583e-02 -9.29581501256281e-02 -9.31536589195981e-02 -9.32538342755445e-02 -9.33217192734506e-02 -9.34107931148800e-02 -9.35393220442492e-02 -9.36678509736180e-02 -9.37317664712450e-02 -9.37924104027081e-02 -9.38946904662200e-02 -9.40304604620325e-02 -9.41555978678114e-02 -9.42234828657175e-02 -9.42913678636236e-02 -9.43537566303742e-02 -9.44144005618369e-02 -9.45561793341708e-02 -9.48412963253769e-02 -9.51264133165831e-02 -9.51951707321328e-02 -9.52558146635958e-02 -9.54435698457567e-02 -9.57286868369625e-02 -9.59759409024289e-02 -9.60365848338917e-02 -9.60972287653544e-02 -9.61690396426578e-02 -9.62441657070072e-02 -9.63473509038247e-02 -9.64976030325236e-02 -9.66478551612228e-02 -9.67247914921831e-02 -9.67999175565328e-02 -9.68708560161919e-02 -9.69387410140983e-02 -9.70079346384703e-02 -9.70830607028197e-02 -9.71581867671692e-02 -9.72978172110553e-02 -9.74553104061978e-02 -9.75782558626467e-02 -9.76461408605528e-02 -9.77140258584589e-02 -9.77819108563653e-02 -9.78497958542714e-02 -9.79134060057231e-02 -9.79740499371858e-02 -9.80346938686489e-02 -9.80953378001117e-02 -9.81559817315744e-02 -9.82166256630375e-02 -9.82772695945003e-02 -9.83379135259631e-02 -9.83985574574261e-02 -9.84592013888889e-02
- -1.92968750000000e-01 -1.84181715871022e-01 -1.75394681742043e-01 -1.65925526067839e-01 -1.56006170173785e-01 -1.45711123979271e-01 -1.33957515441792e-01 -1.22203906904313e-01 -1.17790040698283e-01 -1.15128948780360e-01 -1.12467856862437e-01 -1.09806764944514e-01 -1.07154162740787e-01 -1.05197717101131e-01 -1.03241271461474e-01 -1.01000346786013e-01 -9.85809254606367e-02 -9.54583791352596e-02 -8.97967703098829e-02 -8.41351614845058e-02 -8.14468174204354e-02 -7.94143405831242e-02 -7.72564809725712e-02 -7.49266678444304e-02 -7.26131144001254e-02 -7.09580781511725e-02 -6.93030419022196e-02 -6.75569906825796e-02 -6.57566805381071e-02 -6.39414520519263e-02 -6.20759723094642e-02 -6.02104925670017e-02 -5.68686858773033e-02 -5.32273345896146e-02 -5.03951070456450e-02 -4.86192355004188e-02 -4.68345798000417e-02 -4.48156799623117e-02 -4.27967801245813e-02 -4.07778802868509e-02 -3.87589804491206e-02 -3.67986416457287e-02 -3.50227701005025e-02 -3.32468985552764e-02 -3.13301860866834e-02 -2.93873174466080e-02 -2.73839248324958e-02 -2.53052861966080e-02 -2.32008676193468e-02 -2.05872951999581e-02 -1.79737227805695e-02 -1.61781073335427e-02 -1.48217650753769e-02 -1.34764152795226e-02 -1.21635194200167e-02 -1.08506235605109e-02 -9.53772770100504e-03 -8.22483184149917e-03 -7.00331082495812e-03 -5.88999685929650e-03 -4.67330140284758e-03 -1.84385469011726e-03 9.85592022613058e-04 2.29457836055276e-03 2.82951214405360e-03 3.49471969221105e-03 4.52114086055275e-03 5.54756202889446e-03 6.61167163944721e-03 7.68153920644892e-03 9.00276120184254e-03 1.06075625523450e-02 1.21833123953099e-02 1.33862345582077e-02 1.45891567211055e-02 1.64021605422948e-02 1.85093108773032e-02 2.03256353381491e-02 2.13832836055276e-02 2.24409318729062e-02 2.34711153423367e-02 2.44975365106784e-02 2.55448957024707e-02 2.66147632694724e-02 2.76809666823702e-02 2.87073878507119e-02 2.97338090190536e-02 3.08049852125209e-02 3.18965759788526e-02 3.29724632275963e-02 3.40097459956030e-02 3.50470287636097e-02 3.68033363431742e-02 3.86470928863065e-02 3.99281564070352e-02 4.06328026853015e-02 4.13341773974037e-02 4.20048811767171e-02 4.26755849560300e-02 4.31603329145729e-02 4.35649275020938e-02 4.40095987751254e-02 4.45472479585425e-02 4.50848971419600e-02 4.60077732412058e-02 4.69717402114742e-02 4.77199801088779e-02 4.82576292922950e-02 4.88266364373954e-02 4.96534757118929e-02 5.04803149863904e-02 5.11257132014237e-02 5.16973048837942e-02 5.22582639761308e-02 5.27959131595479e-02 5.33335623429650e-02 5.38712115263821e-02 5.44088607097992e-02 5.52208307160804e-02 5.62879828831658e-02 5.72913395100504e-02 5.78289886934675e-02 5.83666378768846e-02 5.89042870603017e-02 5.94419362437187e-02 5.99036196608042e-02 6.02077444514237e-02 6.05118692420433e-02 6.09092009526800e-02 6.13137955402008e-02 6.18774536746229e-02 6.25820999528896e-02 6.32646140860554e-02 6.38022632694725e-02 6.43399124528896e-02 6.52258034966500e-02 6.62372899654521e-02 6.69757969535175e-02 6.71780942472779e-02 6.73803915410383e-02 6.77727995445979e-02 6.81773941321187e-02 6.85216283239113e-02 6.88148915148658e-02 6.91374025073283e-02 6.96329629920433e-02 7.01285234767588e-02 7.06920343907033e-02 7.12785607726129e-02 7.17626217022612e-02 7.20558848932163e-02 7.23491480841708e-02 7.26424112751254e-02 7.29356744660804e-02 7.32906393948912e-02 7.36952339824121e-02 7.40823911222779e-02 7.43756543132329e-02 7.46689175041875e-02 7.49621806951425e-02 7.52554438860971e-02 7.59044898974037e-02 7.71820855579983e-02 7.84596812185929e-02 7.85935373481992e-02 7.86845032453937e-02 7.92183083385679e-02 8.00913094116417e-02 8.09384323963567e-02 8.16580133741625e-02 8.23775943519683e-02 8.35944533867254e-02 8.49589418446400e-02 8.63817459694304e-02 8.79023699225292e-02 8.94229938756279e-02 9.01141776591288e-02 9.07848814384421e-02 9.15356731574537e-02 9.23448623324958e-02 9.39951711683417e-02 9.94585557998325e-02 1.04921940431323e-01 1.10229025989322e-01 1.15492828726968e-01 1.19039190091080e-01 1.19848379266122e-01 1.20657568441164e-01 1.21806117174414e-01 1.22958804438861e-01 1.23657823623325e-01 1.24042052711474e-01 1.24404820325586e-01 1.24677718017169e-01 1.24950615708752e-01 1.25456710636516e-01 1.26022871519054e-01 1.26689436767169e-01 1.27508129841918e-01 1.28326822916667e-01
diff --git a/data/data_tpgmr_frames.txt b/data/data_tpgmr_frames.txt
deleted file mode 100644
index cfab7a6acd784b50924a512a9ba75b4448eb38ac..0000000000000000000000000000000000000000
--- a/data/data_tpgmr_frames.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-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
deleted file mode 100644
index cbf512a44c991ee2c2338b7aaee66b5351539526..0000000000000000000000000000000000000000
--- a/data/data_tpgmr_trajectories.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-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/images/demo_GMR01.gif b/images/demo_GMR01.gif
index 04a140c5fa7cf06c8b24337cfe36d30229bd0375..1a0358f1d03da2b3f675b787fac2dc9eb4fc4769 100644
Binary files a/images/demo_GMR01.gif and b/images/demo_GMR01.gif differ
diff --git a/images/demo_GPR01.gif b/images/demo_GPR01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..4a75c8f6c3dc0fdd559713237b4730c59973bc91
Binary files /dev/null and b/images/demo_GPR01.gif differ
diff --git a/images/demo_Riemannian_quat_infHorLQR01.png b/images/demo_Riemannian_quat_infHorLQR01.png
index 3c4c9b0b698546a6cd7880c4ad1e8cfc02b63b28..533c01811e3ce56361df2c4b25e30ee4cb10f2c1 100644
Binary files a/images/demo_Riemannian_quat_infHorLQR01.png and b/images/demo_Riemannian_quat_infHorLQR01.png differ
diff --git a/images/demo_TPGMMProduct01.gif b/images/demo_TPGMMProduct01.gif
index 163e82c4db08d73b9953c947693e5ceb0a07312c..824d935a70aef2ebe3c05527a6541160a39f8df5 100644
Binary files a/images/demo_TPGMMProduct01.gif and b/images/demo_TPGMMProduct01.gif differ
diff --git a/images/demo_TPbatchLQR01.gif b/images/demo_TPbatchLQR01.gif
index 40b4af0f12080c8aafb5f2d2cebdc5ce65bad3e9..e1fa02c9ff96190f8dc5061eb61a7a93b962b87e 100644
Binary files a/images/demo_TPbatchLQR01.gif and b/images/demo_TPbatchLQR01.gif differ
diff --git a/include/gfx2.h b/include/gfx2.h
index c4960423d749700967cade511b6318a2878070aa..a16db3b362f425f5da9da8ad503c59dca7c6eaf1 100644
--- a/include/gfx2.h
+++ b/include/gfx2.h
@@ -105,8 +105,15 @@ namespace gfx2
// Converts some coordinates from UI-space to OpenGL-space
//
// UI coordinates range from (0, 0) to (win_width, win_height)
- // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
- // (fb_width / 2, -fb_height / 2)
+ // OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+ //-------------------------------------------------------------------------
+ arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size);
+
+ //-------------------------------------------------------------------------
+ // Converts some coordinates from UI-space to OpenGL-space
+ //
+ // UI coordinates range from (0, 0) to (win_width, win_height)
+ // OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
//-------------------------------------------------------------------------
arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
int fb_width, int fb_height);
@@ -118,7 +125,17 @@ namespace gfx2
// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
// (fb_width / 2, -fb_height / 2)
//-------------------------------------------------------------------------
- arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size);
+ arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size);
+
+ //-------------------------------------------------------------------------
+ // Converts some coordinates from UI-space to OpenGL-space
+ //
+ // UI coordinates range from (0, 0) to (win_width, win_height)
+ // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
+ // (fb_width / 2, -fb_height / 2)
+ //-------------------------------------------------------------------------
+ arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
+ int fb_width, int fb_height);
//-------------------------------------------------------------------------
// Converts some coordinates from OpenGL-space to UI-space
@@ -130,6 +147,23 @@ namespace gfx2
arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
int fb_width, int fb_height);
+ //-------------------------------------------------------------------------
+ // Converts some coordinates from OpenGL-space to UI-space
+ //
+ // OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+ // UI coordinates range from (0, 0) to (win_width, win_height)
+ //-------------------------------------------------------------------------
+ arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size);
+
+ //-------------------------------------------------------------------------
+ // Converts some coordinates from OpenGL-space to UI-space
+ //
+ // OpenGL coordinates range from (0, fb_height) to (fb_width, 0)
+ // UI coordinates range from (0, 0) to (win_width, win_height)
+ //-------------------------------------------------------------------------
+ arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
+ int fb_width, int fb_height);
+
//-------------------------------------------------------------------------
// Converts some coordinates from OpenGL-space to UI-space
//
@@ -137,7 +171,7 @@ namespace gfx2
// (fb_width / 2, -fb_height / 2)
// UI coordinates range from (0, 0) to (win_width, win_height)
//-------------------------------------------------------------------------
- arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size);
+ arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size);
/*********************** PROJECTION & VIEW MATRICES **********************/
@@ -240,6 +274,35 @@ namespace gfx2
typedef std::vector<point_light_t> light_list_t;
+ /******************************** TEXTURES *******************************/
+
+ //-------------------------------------------------------------------------
+ // Holds all the needed informations about a texture
+ //-------------------------------------------------------------------------
+ struct texture_t {
+ GLuint id;
+ GLuint width;
+ GLuint height;
+ GLenum format;
+ GLenum type;
+
+ union {
+ float* pixels_f;
+ unsigned char* pixels_b;
+ };
+ };
+
+ //-------------------------------------------------------------------------
+ // Create a texture
+ //-------------------------------------------------------------------------
+ texture_t create_texture(int width, int height, GLenum format, GLenum type);
+
+ //-------------------------------------------------------------------------
+ // Create a texture
+ //-------------------------------------------------------------------------
+ void destroy(texture_t &texture);
+
+
/********************************* MESHES ********************************/
//-------------------------------------------------------------------------
@@ -262,12 +325,18 @@ namespace gfx2
arma::fvec diffuse_color;
arma::fvec specular_color;
float specular_power;
+ texture_t texture;
// Other
bool lightning_enabled;
bool use_one_minus_src_alpha_blending;
};
+ //-------------------------------------------------------------------------
+ // Represent a list of models
+ //-------------------------------------------------------------------------
+ typedef std::vector<gfx2::model_t> model_list_t;
+
//-------------------------------------------------------------------------
// Create a rectangular mesh, colored (no lightning)
//-------------------------------------------------------------------------
@@ -276,6 +345,14 @@ namespace gfx2
const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
transforms_t* parent_transforms = 0);
+ //-------------------------------------------------------------------------
+ // Create a rectangular mesh, textured (no lightning)
+ //-------------------------------------------------------------------------
+ model_t create_rectangle(const texture_t& texture, float width, float height,
+ const arma::fvec& position = arma::zeros<arma::fvec>(3),
+ const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+ transforms_t* parent_transforms = 0);
+
//-------------------------------------------------------------------------
// Create a square mesh, colored (no lightning)
//-------------------------------------------------------------------------
@@ -342,7 +419,7 @@ namespace gfx2
//-------------------------------------------------------------------------
// Release the OpenGL resources used by the model
//-------------------------------------------------------------------------
- void destroy(const model_t& model);
+ void destroy(model_t &model);
/******************************* RENDERING *******************************/
@@ -368,6 +445,13 @@ namespace gfx2
const arma::fvec& position = arma::zeros<arma::fvec>(3),
const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+ //-------------------------------------------------------------------------
+ // Render a rectangular mesh, textured (no lightning)
+ //-------------------------------------------------------------------------
+ bool draw_rectangle(const texture_t& texture, float width, float height,
+ const arma::fvec& position = arma::zeros<arma::fvec>(3),
+ const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
+
//-------------------------------------------------------------------------
// Render a line, colored (no lightning), from a matrix containing the
// point coordinates
diff --git a/include/gfx3.h b/include/gfx3.h
deleted file mode 100644
index 93ce949664f6d45d8d02a05bd24c5cc14096af12..0000000000000000000000000000000000000000
--- a/include/gfx3.h
+++ /dev/null
@@ -1,612 +0,0 @@
-/*
- * gfx3.h
- *
- * Rendering utility structures and functions based on OpenGL 3.3+
- *
- * Authors: Philip Abbet
- */
-
-#pragma once
-
-#define ARMA_DONT_PRINT_ERRORS
-#include <armadillo>
-#include <map>
-
-// Detect the platform
-#ifdef _WIN32
- #define GFX_WINDOWS
- #include <windows.h>
-#elif __APPLE__
- #define GFX_OSX
-#elif __linux__ || __unix__ || defined(_POSIX_VERSION)
- #define GFX_LINUX
-#else
- #error "Unknown platform"
-#endif
-
-// OpenGL includes
-#ifdef GFX_WINDOWS
- // NOT TESTED HERE
- #include <windows.h>
- #include <GL/glu.h>
- #ifndef GL_CLAMP_TO_EDGE
- #define GL_CLAMP_TO_EDGE 0x812F
- #endif
-#elif defined GFX_LINUX
- #ifndef __glew_h__
- #define GL_GLEXT_PROTOTYPES
- #include <GL/glew.h>
- #include <GL/glu.h>
- #include <GL/gl.h>
- #include <GL/glx.h>
- #endif
-#elif defined GFX_OSX
- #ifndef __glew_h__
- #define GL_GLEXT_PROTOTYPES
- #include <GL/glew.h>
- #endif
- #include <OpenGL/glu.h>
- #include <OpenGL/OpenGL.h>
- #include <OpenGL/gl.h>
- #include <OpenGL/glext.h>
-#endif
-
-
-
-namespace gfx3
-{
- /**************************** UTILITY FUNCTIONS **************************/
-
- //-------------------------------------------------------------------------
- // Holds the sizes of the window and of the OpenGL front-buffer (they might
- // be different, for instance on a 4K screen)
- //-------------------------------------------------------------------------
- struct window_size_t {
- int win_width;
- int win_height;
- int fb_width;
- int fb_height;
-
- inline float scale_x() const {
- return (float) fb_width / (float) win_width;
- }
-
- inline float scale_y() const {
- return (float) fb_height / (float) win_height;
- }
- };
-
- //-------------------------------------------------------------------------
- // Initialisations
- //-------------------------------------------------------------------------
- void init();
-
- //-------------------------------------------------------------------------
- // Convert radian to degrees
- //-------------------------------------------------------------------------
- double deg2rad(double deg);
-
- //-------------------------------------------------------------------------
- // Returns the sinus of an angle in degrees
- //-------------------------------------------------------------------------
- double sin_deg(double deg);
-
- //-------------------------------------------------------------------------
- // Returns the cosinus of an angle in degrees
- //-------------------------------------------------------------------------
- double cos_deg(double deg);
-
- //-------------------------------------------------------------------------
- // Indicates if two values are close enough
- //-------------------------------------------------------------------------
- bool is_close(float a, float b, float epsilon = 1e-6f);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from UI-space to OpenGL-space
- //
- // UI coordinates range from (0, 0) to (win_width, win_height)
- // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
- // (fb_width / 2, -fb_height / 2)
- //-------------------------------------------------------------------------
- arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from UI-space to OpenGL-space
- //
- // UI coordinates range from (0, 0) to (win_width, win_height)
- // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
- // (fb_width / 2, -fb_height / 2)
- //-------------------------------------------------------------------------
- arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from OpenGL-space to UI-space
- //
- // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
- // (fb_width / 2, -fb_height / 2)
- // UI coordinates range from (0, 0) to (win_width, win_height)
- //-------------------------------------------------------------------------
- arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from OpenGL-space to UI-space
- //
- // OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
- // (fb_width / 2, -fb_height / 2)
- // UI coordinates range from (0, 0) to (win_width, win_height)
- //-------------------------------------------------------------------------
- arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from UI-space to shader-space
- //
- // UI coordinates range from (0, 0) to (win_width, win_height)
- // Shader coordinates range from (sh_left, sh_top) to (sh_right, sh_bottom)
- // by default: (-1, 1) to (1, -1)
- //-------------------------------------------------------------------------
- arma::vec ui2shader(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height, float sh_left = -1.0f,
- float sh_top = 1.0f, float sh_right = 1.0f,
- float sh_bottom = -1.0f);
-
- //-------------------------------------------------------------------------
- // Converts some coordinates from UI-space to shader-space
- //
- // UI coordinates range from (0, 0) to (win_width, win_height)
- // Shader coordinates range from (sh_left, sh_top) to (sh_right, sh_bottom)
- // by default: (-1, 1) to (1, -1)
- //-------------------------------------------------------------------------
- arma::vec ui2shader(const arma::vec& coords, const window_size_t& window_size,
- float sh_left = -1.0f, float sh_top = 1.0f,
- float sh_right = 1.0f, float sh_bottom = -1.0f);
-
-
- /*********************** PROJECTION & VIEW MATRICES **********************/
-
- //-------------------------------------------------------------------------
- // Returns a perspective projection matrix
- //-------------------------------------------------------------------------
- arma::fmat perspective(float fovy, float aspect, float zNear, float zFar);
-
- //-------------------------------------------------------------------------
- // Returns a orthographic projection matrix
- //-------------------------------------------------------------------------
- arma::fmat orthographic(float width, float height, float zNear, float zFar);
-
- //-------------------------------------------------------------------------
- // Returns a view matrix
- //-------------------------------------------------------------------------
- arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
- const arma::fvec& up);
-
-
- /**************************** TRANSFORMATIONS ****************************/
-
- //-------------------------------------------------------------------------
- // Holds all the transformations needed for a 3D entity
- //
- // Can be organised in a hierarchy, where the parent transforms affect the
- // children ones
- //-------------------------------------------------------------------------
- struct transforms_t {
-
- // Constructor
- transforms_t()
- : parent(0)
- {
- position.zeros(3);
- rotation.eye(4, 4);
- }
-
- transforms_t* parent;
-
- arma::fvec position;
- arma::fmat rotation;
- };
-
- //-------------------------------------------------------------------------
- // Returns a rotation matrix given an axis and an angle (in radian)
- //-------------------------------------------------------------------------
- arma::fmat rotate(const arma::fvec& axis, float angle);
-
- //-------------------------------------------------------------------------
- // Returns the rotation matrix to go from one direction to another one
- //-------------------------------------------------------------------------
- arma::fmat rotation(const arma::fvec& from, const arma::fvec& to);
-
- //-------------------------------------------------------------------------
- // Compute the translation and rotation to apply to a list of 3D points A to
- // obtain the list of 3D points B.
- //
- // Points are organised in columns:
- // [ x0 x1 x2 ...
- // y0 y1 y2
- // z0 z1 z2 ]
- //-------------------------------------------------------------------------
- void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
- arma::fmat &rotation, arma::fvec &translation);
-
- //-------------------------------------------------------------------------
- // Returns the full world transformation matrix corresponding to the given
- // transforms structure, taking all its parent hierarchy into account
- //-------------------------------------------------------------------------
- arma::fmat worldTransforms(const transforms_t* transforms);
-
- //-------------------------------------------------------------------------
- // Returns the full world position corresponding to the given transforms
- // structure, taking all its parent hierarchy into account
- //-------------------------------------------------------------------------
- arma::fvec worldPosition(const transforms_t* transforms);
-
- //-------------------------------------------------------------------------
- // Returns the full world rotation matrix corresponding to the given
- // transforms structure, taking all its parent hierarchy into account
- //-------------------------------------------------------------------------
- arma::fmat worldRotation(const transforms_t* transforms);
-
-
- /******************************** SHADERS ********************************/
-
- struct shader_fmat_uniform_t {
- GLint handle;
- arma::fmat value;
- };
-
-
- struct shader_fvec_uniform_t {
- GLint handle;
- arma::fvec value;
- };
-
-
- struct shader_float_uniform_t {
- GLint handle;
- float value;
- };
-
-
- struct shader_bool_uniform_t {
- GLint handle;
- bool value;
- };
-
-
- //-------------------------------------------------------------------------
- // Holds all the needed informations about a GLSL program
- //-------------------------------------------------------------------------
- struct shader_t {
- GLuint id;
-
- bool use_lightning;
-
- // Matrices
- GLint model_matrix_handle;
- GLint view_matrix_handle;
- GLint projection_matrix_handle;
-
- // Material
- GLint ambiant_color_handle; // valid if lightning is used
- GLint diffuse_color_handle;
- GLint specular_color_handle; // valid if lightning is used
- GLint specular_power_handle; // valid if lightning is used
-
- // Textures
- GLint diffuse_texture_handle; // valid if textures are used
-
- // Light
- GLint light_position_handle; // valid if lightning is used
- GLint light_color_handle; // valid if lightning is used
- GLint light_power_handle; // valid if lightning is used
-
- // RTT-specific
- GLint backbuffer_handle;
-
- // Application-dependant uniforms
- std::map<std::string, shader_fmat_uniform_t> fmat_uniforms;
- std::map<std::string, shader_fvec_uniform_t> fvec_uniforms;
- std::map<std::string, shader_float_uniform_t> float_uniforms;
- std::map<std::string, shader_bool_uniform_t> bool_uniforms;
-
- void setUniform(const std::string& name, const arma::fmat& value);
- void setUniform(const std::string& name, const arma::mat& value);
- void setUniform(const std::string& name, const arma::fvec& value);
- void setUniform(const std::string& name, const arma::vec& value);
- void setUniform(const std::string& name, float value);
- void setUniform(const std::string& name, bool value);
- };
-
-
- //-------------------------------------------------------------------------
- // Convert to string (handy to create a shader, but messes up line numbers,
- // which makes it tricky with errors)
- //-------------------------------------------------------------------------
- #define STRINGIFY( expr ) #expr
-
- //-------------------------------------------------------------------------
- // Load and compile a vertex and a fragment shaders into a GLSL program
- //-------------------------------------------------------------------------
- shader_t loadShader(const std::string& vertex_shader,
- const std::string& fragment_shader,
- const std::string& version = "330 core");
-
- //-------------------------------------------------------------------------
- // Vertex and fragment shaders for a mesh with normals and one point light
- // source
- //-------------------------------------------------------------------------
- extern const char* VERTEX_SHADER_ONE_LIGHT;
- extern const char* FRAGMENT_SHADER_ONE_LIGHT;
-
- //-------------------------------------------------------------------------
- // Vertex and fragment shaders without any lightning
- //-------------------------------------------------------------------------
- extern const char* VERTEX_SHADER_COLORED;
- extern const char* FRAGMENT_SHADER_COLORED;
-
- //-------------------------------------------------------------------------
- // Vertex and fragment shaders with a texture, without any lightning
- //-------------------------------------------------------------------------
- extern const char* VERTEX_SHADER_TEXTURED;
- extern const char* FRAGMENT_SHADER_ONE_TEXTURE;
- extern const char* FRAGMENT_SHADER_GAUSSIAN;
-
- //-------------------------------------------------------------------------
- // Vertex and fragment shaders to do render-to-texture
- //-------------------------------------------------------------------------
- extern const char* RTT_VERTEX_SHADER;
- extern const char* RTT_FRAGMENT_SHADER_GAUSSIAN;
- extern const char* RTT_FRAGMENT_SHADER_LIC;
-
-
- /******************************* LIGHTNING *******************************/
-
- //-------------------------------------------------------------------------
- // Holds all the needed informations about a point light
- //-------------------------------------------------------------------------
- struct point_light_t {
- transforms_t transforms;
- arma::fvec color;
- float power;
- };
-
- //-------------------------------------------------------------------------
- // A list of lights
- //-------------------------------------------------------------------------
- typedef std::vector<point_light_t> light_list_t;
-
-
- /*************************** RENDER-TO-TEXTURE ***************************/
-
- struct render_to_texture_buffer_t {
- GLuint framebuffer;
- GLuint texture;
- };
-
- struct render_to_texture_t {
-
- // Textures
- render_to_texture_buffer_t buffers[2];
- unsigned int nb_buffers;
- unsigned int current_buffer;
- unsigned int width;
- unsigned int height;
-
- // Rectangular mesh
- GLuint nb_vertices;
- GLuint vertex_buffer;
-
- // Shader
- shader_t const* shader;
-
- inline GLuint texture() const {
- return buffers[current_buffer].texture;
- };
-
- inline GLuint previous_texture() const {
- unsigned int previous_buffer = current_buffer + 1;
- if (previous_buffer >= nb_buffers)
- previous_buffer = 0;
-
- return buffers[previous_buffer].texture;
- };
- };
-
- //-------------------------------------------------------------------------
- // Create a render-to-texture object
- //-------------------------------------------------------------------------
- render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
- unsigned int height, const arma::fvec& color);
-
- //-------------------------------------------------------------------------
- // Render a render-to-texture object
- //-------------------------------------------------------------------------
- bool draw(render_to_texture_t &rtt);
-
-
- /********************************* MESHES ********************************/
-
- //-------------------------------------------------------------------------
- // Holds all the needed informations about a mesh
- //-------------------------------------------------------------------------
- struct model_t {
- GLenum mode;
-
- // Vertex data
- GLuint nb_vertices;
- GLuint vertex_buffer;
- GLuint normal_buffer;
- GLuint uv_buffer;
-
- // Transforms
- transforms_t transforms;
-
- // Shader
- shader_t const* shader;
-
- // Material
- arma::fvec ambiant_color;
- arma::fvec diffuse_color;
- arma::fvec specular_color;
- float specular_power;
-
- // Textures
- GLuint diffuse_texture;
- };
-
- //-------------------------------------------------------------------------
- // Create a rectangular mesh, colored (no lightning)
- //-------------------------------------------------------------------------
- model_t create_rectangle(const shader_t& shader, const arma::fvec& color,
- float width, float height,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Create a square mesh, colored (no lightning)
- //-------------------------------------------------------------------------
- model_t create_square(const shader_t& shader, const arma::fvec& color, float size,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Create a sphere mesh, either colored or that can be lighted (it depends
- // on the shader)
- //-------------------------------------------------------------------------
- model_t create_sphere(const shader_t& shader, float radius = 1.0f,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Create a line mesh, colored (no lightning), from a matrix containing the
- // point coordinates
- //-------------------------------------------------------------------------
- model_t create_line(const shader_t& shader, const arma::fvec& color,
- const arma::mat& points,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Create a line mesh, colored (no lightning), from an array containing the
- // point coordinates
- //-------------------------------------------------------------------------
- model_t create_line(const shader_t& shader, const arma::fvec& color,
- const std::vector<arma::vec>& points,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Create a mesh, colored (no lightning), from a matrix containing the
- // vertex coordinates
- //-------------------------------------------------------------------------
- model_t create_mesh(const shader_t& shader, const arma::fvec& color,
- const arma::mat& vertices,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
-
- //-------------------------------------------------------------------------
- // Release the OpenGL resources used by the model
- //-------------------------------------------------------------------------
- void destroy(const model_t& model);
-
-
- /******************************* RENDERING *******************************/
-
- //-------------------------------------------------------------------------
- // Render a mesh
- //-------------------------------------------------------------------------
- bool draw(const model_t& model, const arma::fmat& view,
- const arma::fmat& projection,
- const light_list_t& lights);
-
- //-------------------------------------------------------------------------
- // Render a mesh (shortcut when lights aren't used by the shaders)
- //-------------------------------------------------------------------------
- inline bool draw(const model_t& model, const arma::fmat& view,
- const arma::fmat& projection)
- {
- light_list_t lights;
- return draw(model, view, projection, lights);
- }
-
- //-------------------------------------------------------------------------
- // Render a rectangular mesh, colored (no lightning)
- //-------------------------------------------------------------------------
- bool draw_rectangle(const shader_t& shader, const arma::fvec& color,
- float width, float height, const arma::fmat& view,
- const arma::fmat& projection,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
-
- //-------------------------------------------------------------------------
- // Render a line, colored (no lightning), from a matrix containing the
- // point coordinates
- //-------------------------------------------------------------------------
- bool draw_line(const shader_t& shader, const arma::fvec& color,
- const arma::mat& points, const arma::fmat& view,
- const arma::fmat& projection,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
-
- //-------------------------------------------------------------------------
- // Render a line, colored (no lightning), from an array containing the
- // point coordinates
- //-------------------------------------------------------------------------
- bool draw_line(const shader_t& shader, const arma::fvec& color,
- const std::vector<arma::vec>& points, const arma::fmat& view,
- const arma::fmat& projection,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
-
- //-------------------------------------------------------------------------
- // Render a mesh, colored (no lightning), from a matrix containing the
- // vertex coordinates
- //-------------------------------------------------------------------------
- bool draw_mesh(const shader_t& shader, const arma::fvec& color,
- const arma::mat& vertices,const arma::fmat& view,
- const arma::fmat& projection,
- const arma::fvec& position = arma::zeros<arma::fvec>(3),
- const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4));
-
- //-------------------------------------------------------------------------
- // Render a gaussian, colored (no lightning)
- //-------------------------------------------------------------------------
- bool draw_gaussian(shader_t* shader, const arma::fvec& color,
- const arma::vec& mu, const arma::mat& sigma,
- const arma::fmat& view, const arma::fmat& projection,
- float viewport_width, float viewport_height);
-
- //-------------------------------------------------------------------------
- // Render the border of a gaussian, colored (no lightning)
- //-------------------------------------------------------------------------
- bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
- const arma::vec& mu, const arma::mat& sigma,
- const arma::fmat& view,
- const arma::fmat& projection,
- float viewport_width, float viewport_height);
-
-
- /****************************** RAY CASTING ******************************/
-
- //-------------------------------------------------------------------------
- // Represents a 3D ray (in world coordinates)
- //-------------------------------------------------------------------------
- struct ray_t {
- arma::fvec origin;
- arma::fvec direction;
- };
-
- ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
- const arma::fmat& view, const arma::fmat& projection,
- int window_width, int window_height);
-
- bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
- arma::fvec &result);
-
-};
diff --git a/include/imgui_impl_glfw_gl3.h b/include/imgui_impl_glfw_gl3.h
deleted file mode 100644
index 33b5329db9e3b0c4a669dd436488d5a304ec17c9..0000000000000000000000000000000000000000
--- a/include/imgui_impl_glfw_gl3.h
+++ /dev/null
@@ -1,31 +0,0 @@
-// ImGui GLFW binding with OpenGL3 + shaders
-// (GLFW is a cross-platform general purpose library for handling windows, inputs, OpenGL/Vulkan graphics context creation, etc.)
-// (GL3W is a helper library to access OpenGL functions since there is no standard header to access modern OpenGL functions easily. Alternatives are GLEW, Glad, etc.)
-
-// Implemented features:
-// [X] User texture binding. Cast 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID in imgui.cpp.
-// [X] Gamepad navigation mapping. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'.
-
-// You can copy and use unmodified imgui_impl_* files in your project. See main.cpp for an example of using this.
-// If you use this binding you'll need to call 4 functions: ImGui_ImplXXXX_Init(), ImGui_ImplXXXX_NewFrame(), ImGui::Render() and ImGui_ImplXXXX_Shutdown().
-// If you are new to ImGui, see examples/README.txt and documentation at the top of imgui.cpp.
-// https://github.com/ocornut/imgui
-
-struct GLFWwindow;
-
-IMGUI_API bool ImGui_ImplGlfwGL3_Init(GLFWwindow* window, bool install_callbacks, const char* glsl_version = NULL);
-IMGUI_API void ImGui_ImplGlfwGL3_Shutdown();
-IMGUI_API void ImGui_ImplGlfwGL3_NewFrame();
-IMGUI_API void ImGui_ImplGlfwGL3_RenderDrawData(ImDrawData* draw_data);
-
-// Use if you want to reset your rendering device without losing ImGui state.
-IMGUI_API void ImGui_ImplGlfwGL3_InvalidateDeviceObjects();
-IMGUI_API bool ImGui_ImplGlfwGL3_CreateDeviceObjects();
-
-// GLFW callbacks (installed by default if you enable 'install_callbacks' during initialization)
-// Provided here if you want to chain callbacks.
-// You can also handle inputs yourself and use those as a reference.
-IMGUI_API void ImGui_ImplGlfw_MouseButtonCallback(GLFWwindow* window, int button, int action, int mods);
-IMGUI_API void ImGui_ImplGlfw_ScrollCallback(GLFWwindow* window, double xoffset, double yoffset);
-IMGUI_API void ImGui_ImplGlfw_KeyCallback(GLFWwindow* window, int key, int scancode, int action, int mods);
-IMGUI_API void ImGui_ImplGlfw_CharCallback(GLFWwindow* window, unsigned int c);
diff --git a/src/demo_GMR01.cpp b/src/demo_GMR01.cpp
index 1dda2827899402d0090c94548a6052030b8f4518..da4e073874286cbecd62094b4a819ad4824705a5 100644
--- a/src/demo_GMR01.cpp
+++ b/src/demo_GMR01.cpp
@@ -67,8 +67,18 @@ struct model_t {
};
-//-----------------------------------------------
-
+//-----------------------------------------------------------------------------
+// Likelihood of datapoint(s) to be generated by a Gaussian parameterized by
+// center and covariance.
+//
+// Inputs:
+// - Data: D x N array representing N datapoints of D dimensions.
+// - Mu: D x 1 vector representing the center of the Gaussian.
+// - Sigma: D x D array representing the covariance matrix of the Gaussian.
+//
+// Output:
+// - prob: 1 x N vector representing the likelihood of the N datapoints.
+//-----------------------------------------------------------------------------
arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
int nbVar = Data.n_rows;
@@ -77,7 +87,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
@@ -330,16 +340,13 @@ arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
// Colors of the displayed lines and gaussians
//-----------------------------------------------------------------------------
const 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.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 },
});
@@ -454,7 +461,7 @@ void draw_demos_viewport(const viewport_t& viewport,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
}
@@ -488,7 +495,9 @@ void draw_GMR_viewport(const viewport_t& viewport, const mat& points,
glClear(GL_DEPTH_BUFFER_BIT);
- gfx2::draw_line(arma::fvec({0.0f, 0.0f, 0.0f}), points);
+ glLineWidth(4.0f);
+ gfx2::draw_line(arma::fvec({0.0f, 0.4f, 0.0f}), points);
+ glLineWidth(1.0f);
}
}
@@ -616,7 +625,8 @@ void draw_timeline_viewport(const gfx2::window_size_t& window_size,
sigma(0, 1) = sigma(0, 1) * scale_x;
sigma(1, 0) = sigma(1, 0) * scale_x;
- gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % 10).t()), mu, sigma);
+ gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % 10).t()), mu, sigma,
+ true, false);
}
glClear(GL_DEPTH_BUFFER_BIT);
@@ -634,7 +644,7 @@ void draw_timeline_viewport(const gfx2::window_size_t& window_size,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
@@ -646,7 +656,9 @@ void draw_timeline_viewport(const gfx2::window_size_t& window_size,
points(0, span::all) = points(0, span::all) * scale_x - plot_dimensions(0) / 2;
points(1, span::all) *= scale_y;
- gfx2::draw_line(arma::fvec({0.0f, 0.0f, 0.0f}), points);
+ glLineWidth(4.0f);
+ gfx2::draw_line(arma::fvec({0.0f, 0.4f, 0.0f}), points);
+ glLineWidth(1.0f);
}
@@ -970,7 +982,7 @@ int main(int argc, char **argv) {
break;
- if (!gui_state.is_drawing_demonstration) {
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
// Left click: start a new demonstration (only if not on the UI and in the
// demonstrations viewport)
if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
@@ -986,7 +998,7 @@ int main(int argc, char **argv) {
current_trajectory.push_back(coords);
}
}
- } else {
+ } else if (gui_state.is_drawing_demonstration) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
diff --git a/src/demo_GPR01.cpp b/src/demo_GPR01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee63177892295aeac4a432ad500c909cb64dcfb6
--- /dev/null
+++ b/src/demo_GPR01.cpp
@@ -0,0 +1,968 @@
+/*
+ * demo_GPR01.cpp
+ *
+ * Gaussian process regression (GPR) with RBF kernel
+ *
+ * @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
+ */
+
+
+#include <stdio.h>
+#include <armadillo>
+#include <mvn.h>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <imgui_internal.h>
+#include <window_utils.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_data; // Number of datapoints in a trajectory
+ int nb_data_reproduction; // Number of datapoints for reproduction
+ vec p; // GPR parameters
+};
+
+
+//-----------------------------------------------------------------------------
+// Create a demonstration (with a length of 'time_steps.size()') from a
+// trajectory (of any length)
+//-----------------------------------------------------------------------------
+mat sample_trajectory(const mat& trajectory, const uvec& time_steps) {
+
+ // Resampling of the trajectory
+ vec x = trajectory.row(0).t();
+ vec y = trajectory.row(1).t();
+ vec x2;
+ vec y2;
+
+ vec from_indices = linspace<vec>(0, trajectory.n_cols - 1, trajectory.n_cols);
+ vec to_indices = linspace<vec>(0, trajectory.n_cols - 1, time_steps[time_steps.size() - 1] + 1);
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ // Create the demonstration
+ mat demo(3, time_steps.size());
+ for (int i = 0; i < time_steps.size(); ++i) {
+ int j = time_steps[i];
+ demo(0, i) = j;
+ demo(1, i) = x2[j];
+ demo(2, i) = y2[j];
+ }
+
+ return demo;
+}
+
+
+//-----------------------------------------------------------------------------
+// Compute pairwise distance between two sets of vectors.
+//-----------------------------------------------------------------------------
+mat pdist2(const vec& x, const vec& y) {
+ mat result(x.n_rows, y.n_rows);
+
+ for (int i = 0; i < x.n_rows; ++i) {
+ for (int j = 0; j < y.n_rows; ++j) {
+ result(i, j) = fabsl(x(i) - y(j));
+ }
+ }
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Gaussian mixture regression (GPR)
+//-----------------------------------------------------------------------------
+void compute_GPR(const parameters_t& parameters, const matrix_list_t& demos,
+ mat &points, matrix_list_t &sigma_out) {
+
+ const int nb_var = demos[0].n_rows;
+
+ sigma_out.clear();
+
+ mat data(nb_var, demos[0].n_cols * demos.size());
+ for (unsigned int m = 0; m < demos.size(); ++m) {
+ data(span::all, span(m * demos[0].n_cols,
+ (m + 1) * demos[0].n_cols - 1)) =
+ demos[m];
+ }
+
+ vec all_time_steps = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data_reproduction);
+
+ mat K = parameters.p(0) * exp(-1.0 / parameters.p(1) * pow(pdist2(data.row(0).t(), data.row(0).t()), 2.0)) +
+ parameters.p(2) * eye(data.n_cols, data.n_cols);
+
+ mat Kd = parameters.p(0) * exp(-1.0 / parameters.p(1) * pow(pdist2(all_time_steps, data.row(0).t()), 2.0));
+
+ points = mat(nb_var, all_time_steps.n_rows);
+ points(0, span::all) = all_time_steps.t();
+
+ points(span(1, nb_var - 1), span::all) = (Kd * solve(K, data(span(1, nb_var - 1), span::all).t())).t();
+
+ mat Kdd = parameters.p(0) * exp(-1.0 / parameters.p(1) * pow(pdist2(all_time_steps, all_time_steps), 2));
+
+ mat S = Kdd - Kd * solve(K, Kd.t());
+
+ for (unsigned int t = 0; t < parameters.nb_data_reproduction; ++t) {
+ mat sigma = eye(nb_var - 1, nb_var - 1) * S(t, t);
+ sigma_out.push_back(sigma);
+ }
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+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;
+};
+
+
+//-----------------------------------------------------------------------------
+// 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) {
+
+ 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;
+}
+
+
+//-----------------------------------------------------------------------------
+// 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) = coords(0) + (float) viewport.width * 0.5f;
+ result(1) = coords(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 of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const 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 },
+});
+
+
+//-----------------------------------------------------------------------------
+// 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 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;
+
+ // Indicates if the reproductions must be recomputed
+ bool must_recompute_GPR;
+
+ // Offset of the first missing point in the demonstrations
+ int parameter_missing_data_offset;
+
+ // Number of missing points in the demonstrations
+ int parameter_missing_data_length;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ // in parameters_t)
+ int parameter_nb_data;
+ int parameter_nb_data_reproduction;
+ fvec parameter_p;
+};
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations & model" viewport
+//-----------------------------------------------------------------------------
+void draw_demos_viewport(const viewport_t& viewport,
+ const vector_list_t& current_trajectory,
+ const matrix_list_t& original_trajectories,
+ const matrix_list_t& demonstrations) {
+
+ 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();
+
+ // Draw the currently created demonstration (if any)
+ if (current_trajectory.size() > 1)
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
+
+ // Draw the demonstrations
+ int color_index = 0;
+ for (size_t i = 0; i < demonstrations.size(); ++i) {
+ arma::fvec color = conv_to<fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, original_trajectories[i]);
+
+ for (size_t j = 0; j < demonstrations[i].n_cols; ++j) {
+ fvec position = zeros<fvec>(3);
+ position(span(0, 1)) = conv_to<fvec>::from(demonstrations[i](span(1, 2), j));
+
+ gfx2::draw_rectangle(color, 10.0f, 10.0f, position,
+ gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), datum::pi / 4));
+ }
+
+ ++color_index;
+ if (color_index >= COLORS.n_rows)
+ color_index = 0;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Sample the GPR result points at the given time steps
+//-----------------------------------------------------------------------------
+mat sample_GPR_points(const mat& points, const vec& time_steps) {
+
+ mat result(3, time_steps.n_rows);
+
+ for (size_t i = 0; i < time_steps.n_rows; ++i) {
+ for (size_t j = 0; j < points.n_cols - 1; ++j) {
+ if ((points(0, j) <= time_steps(i)) && (time_steps(i) <= points(0, j + 1))) {
+ result(0, i) = time_steps(i);
+ result(1, i) = points(1, j) + (time_steps(i) - points(0, j)) *
+ (points(1, j + 1) - points(1, j)) / (points(0, j + 1) - points(0, j));
+ result(2, i) = points(2, j) + (time_steps(i) - points(0, j)) *
+ (points(2, j + 1) - points(2, j)) / (points(0, j + 1) - points(0, j));
+ break;
+ }
+ }
+ }
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_GPR_viewport(const viewport_t& viewport, const mat& points,
+ const std::vector<gfx2::model_t>& models,
+ const matrix_list_t& demonstrations) {
+
+ 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();
+
+ if (!models.empty()) {
+ for (int i = 0; i < models.size(); ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw(models[i]);
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ glLineWidth(4.0f);
+ gfx2::draw_line(arma::fvec({0.0f, 0.4f, 0.0f}), points(span(1, 2), span::all));
+ glLineWidth(1.0f);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ mat sampled_points = sample_GPR_points(points, vec(demonstrations[0].row(0).t()));
+
+ for (size_t j = 0; j < sampled_points.n_cols; ++j) {
+ fvec position = zeros<fvec>(3);
+ position(span(0, 1)) = conv_to<fvec>::from(sampled_points(span(1, 2), j));
+
+ gfx2::draw_rectangle(fvec({ 0.0f, 0.0f, 0.0f }), 10.0f, 10.0f, position,
+ gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), datum::pi / 4));
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns the dimensions that a plot should have inside the provided viewport
+//-----------------------------------------------------------------------------
+ivec get_plot_dimensions(const viewport_t& viewport) {
+
+ const int MARGIN = 50;
+
+ ivec result(2);
+ result(0) = viewport.width - 2 * MARGIN;
+ result(1) = viewport.height - 2 * MARGIN;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "timeline" viewport
+//-----------------------------------------------------------------------------
+void draw_timeline_viewport(const gfx2::window_size_t& window_size,
+ const viewport_t& viewport,
+ const matrix_list_t& original_trajectories,
+ const matrix_list_t& demonstrations,
+ const mat& GPR_points,
+ matrix_list_t GPR_sigma,
+ unsigned int dimension) {
+
+ 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();
+
+ ivec plot_dimensions = get_plot_dimensions(viewport);
+
+ ivec plot_top_left({ -plot_dimensions(0) / 2, plot_dimensions(1) / 2 });
+ ivec plot_bottom_right({ plot_dimensions(0) / 2, -plot_dimensions(1) / 2 });
+
+ // Axis labels
+ ui::begin("Text");
+
+ vec coords = fb2ui(vec({ -20.0, double(-viewport.height / 2 + 45) }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ std::stringstream label;
+ label << "x" << dimension;
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, -20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), label.str(), ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ // Check if there is something to display
+ if (demonstrations.empty())
+ return;
+
+ // Draw the GPR
+ double scale_x = (double) plot_dimensions(0) / GPR_points(0, GPR_points.n_cols - 1);
+ double scale_y = (double) plot_dimensions(1) / viewport.height;
+
+ mat top_vertices(2, GPR_points.n_cols);
+ mat bottom_vertices(2, GPR_points.n_cols);
+
+ for (int j = 0; j < GPR_points.n_cols; ++j) {
+ top_vertices(0, j) = GPR_points(0, j) * scale_x - plot_dimensions(0) / 2;
+ top_vertices(1, j) = (GPR_points(dimension, j) +
+ sqrt(GPR_sigma[j](dimension - 1, dimension - 1)) * 20.0) * scale_y;
+
+ bottom_vertices(0, j) = top_vertices(0, j);
+ bottom_vertices(1, j) = (GPR_points(dimension, j) -
+ sqrt(GPR_sigma[j](dimension - 1, dimension - 1)) * 20.0) * scale_y;
+ }
+
+ mat gmr_points(2, (GPR_points.n_cols - 1) * 6);
+
+ for (int j = 0; j < GPR_points.n_cols - 1; ++j) {
+ gmr_points(span::all, j * 6 + 0) = top_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 1) = bottom_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 2) = top_vertices(span::all, j + 1);
+
+ gmr_points(span::all, j * 6 + 3) = top_vertices(span::all, j + 1);
+ gmr_points(span::all, j * 6 + 4) = bottom_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 5) = bottom_vertices(span::all, j + 1);
+ }
+
+ gfx2::model_t gmr_model = gfx2::create_mesh(fvec({ 0.0f, 0.8f, 0.0f, 0.05f }), gmr_points);
+ gmr_model.use_one_minus_src_alpha_blending = true;
+ gfx2::draw(gmr_model);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ // Draw the demonstrations
+ int color_index = 0;
+
+ for (size_t i = 0; i < original_trajectories.size(); ++i) {
+ uvec time_steps = linspace<uvec>(0, GPR_points(0, GPR_points.n_cols - 1),
+ original_trajectories[i].n_cols);
+
+ mat datapoints = sample_trajectory(original_trajectories[i], time_steps).rows(uvec({ 0, dimension }));
+
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ datapoints(1, span::all) *= scale_y;
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ ++color_index;
+ if (color_index >= COLORS.n_rows)
+ color_index = 0;
+ }
+
+ // Draw the GPR result
+ mat points(2, GPR_points.n_cols);
+ points(0, span::all) = GPR_points(0, span::all);
+ points(1, span::all) = GPR_points(dimension, span::all);
+
+ points(0, span::all) = points(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ points(1, span::all) *= scale_y;
+
+ glLineWidth(4.0f);
+ gfx2::draw_line(arma::fvec({0.0f, 0.4f, 0.0f}), points);
+ glLineWidth(1.0f);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ mat sampled_points = sample_GPR_points(GPR_points, vec(demonstrations[0].row(0).t()));
+
+ for (size_t j = 0; j < sampled_points.n_cols; ++j) {
+ fvec position = zeros<fvec>(3);
+ position(span(0, 1)) = conv_to<fmat>::from(sampled_points.rows(uvec({ 0, dimension }))).col(j);
+
+ position(0, span::all) = position(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ position(1, span::all) *= scale_y;
+
+ gfx2::draw_rectangle(fvec({ 0.0f, 0.0f, 0.0f }), 10.0f, 10.0f, position,
+ gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), datum::pi / 4));
+ }
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_data = 20;
+ parameters.nb_data_reproduction = 100;
+ parameters.p = vec({ 100.0, 10.0, 1.0 });
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ 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 Gaussian process regression (GPR) with RBF kernel",
+ 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_GPR;
+ viewport_t viewport_x1;
+ viewport_t viewport_x2;
+
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ gui_state.must_recompute_GPR = false;
+ gui_state.parameter_nb_data = parameters.nb_data;
+ gui_state.parameter_nb_data_reproduction = parameters.nb_data_reproduction;
+ gui_state.parameter_missing_data_offset = parameters.nb_data / 2 - 1;
+ gui_state.parameter_missing_data_length = parameters.nb_data / 4;
+ gui_state.parameter_p = conv_to<fvec>::from(parameters.p);
+
+
+ // List of demonstrations and GMr results
+ uvec time_steps;
+ matrix_list_t demos;
+ mat GPR_points;
+ matrix_list_t GPR_sigma;
+ std::vector<gfx2::model_t> GPR_models;
+
+
+ // Main loop
+ vector_list_t current_trajectory;
+ matrix_list_t original_trajectories;
+
+ 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)) {
+
+ 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 / 2 - 1;
+ viewport_height = window_size.fb_height / 2 - 1;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demos, 0, window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_GPR, window_size.fb_width - viewport_width,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_x1, 0, 0, viewport_width, viewport_height);
+
+ setup_viewport(&viewport_x2, window_size.fb_width - viewport_width, 0,
+ viewport_width, viewport_height);
+ }
+
+
+ // If the parameters changed, resample the trajectories and trigger a
+ // recomputation
+ if (gui_state.are_parameters_modified) {
+
+ if (gui_state.parameter_missing_data_offset >= gui_state.parameter_nb_data - 1) {
+ if (gui_state.parameter_missing_data_length > gui_state.parameter_nb_data / 2)
+ gui_state.parameter_missing_data_length = gui_state.parameter_nb_data / 2;
+
+ gui_state.parameter_missing_data_offset = gui_state.parameter_nb_data - (gui_state.parameter_missing_data_length + 1);
+
+ } else if (gui_state.parameter_missing_data_length > gui_state.parameter_nb_data - (gui_state.parameter_missing_data_offset + 1)) {
+ gui_state.parameter_missing_data_length = gui_state.parameter_nb_data - (gui_state.parameter_missing_data_offset + 1);
+ }
+
+ demos.clear();
+
+ uvec all_time_steps = linspace<uvec>(
+ 0, gui_state.parameter_nb_data - 1, gui_state.parameter_nb_data
+ );
+
+ int offset1 = gui_state.parameter_missing_data_offset - 1;
+ int offset2 = offset1 + gui_state.parameter_missing_data_length + 1;
+
+ time_steps = uvec(gui_state.parameter_nb_data - (offset2 - offset1) + 1);
+
+ time_steps(span(0, offset1)) = all_time_steps(span(0, offset1));
+ time_steps(span(offset1 + 1, time_steps.n_rows - 1)) =
+ all_time_steps(span(offset2, gui_state.parameter_nb_data - 1));
+
+ for (size_t i = 0; i < original_trajectories.size(); ++i) {
+ demos.push_back(sample_trajectory(original_trajectories[i],
+ time_steps)
+ );
+ }
+
+ parameters.nb_data = gui_state.parameter_nb_data;
+ parameters.nb_data_reproduction = gui_state.parameter_nb_data_reproduction;
+ parameters.p = conv_to<vec>::from(gui_state.parameter_p);
+
+ gui_state.must_recompute_GPR = !demos.empty();
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Recompute the GPR (if necessary)
+ if (gui_state.must_recompute_GPR) {
+
+ compute_GPR(parameters, demos, GPR_points, GPR_sigma);
+
+ // Create one big mesh for the GPR viewport (for performance reasons)
+ for (int i = 0; i < GPR_models.size(); ++i)
+ gfx2::destroy(GPR_models[i]);
+
+ GPR_models.clear();
+
+ const int NB_POINTS = 60;
+
+ mat vertices(2, NB_POINTS * 3 * GPR_sigma.size());
+ mat lines(2, NB_POINTS * 2 * GPR_sigma.size());
+
+ for (int j = 0; j < GPR_sigma.size(); ++j) {
+
+ mat v = gfx2::get_gaussian_background_vertices(GPR_points(span(1, 2), j),
+ GPR_sigma[j] * 20.0, NB_POINTS);
+
+ vertices(span::all, span(j * NB_POINTS * 3, (j + 1) * NB_POINTS * 3 - 1)) = v;
+
+ mat p = gfx2::get_gaussian_border_vertices(GPR_points(span(1, 2), j),
+ GPR_sigma[j] * 20.0, NB_POINTS, false);
+
+ lines(span::all, span(j * NB_POINTS * 2, (j + 1) * NB_POINTS * 2 - 1)) = p;
+ }
+
+ GPR_models.push_back(
+ gfx2::create_mesh(fvec({ 0.0f, 0.8f, 0.0f, 0.1f }), vertices)
+ );
+
+ GPR_models[0].use_one_minus_src_alpha_blending = true;
+
+ GPR_models.push_back(
+ gfx2::create_line(fvec({ 0.0f, 0.4f, 0.0f, 0.1f }), lines,
+ arma::zeros<arma::fvec>(3),
+ arma::eye<arma::fmat>(4, 4), 0, false)
+ );
+
+ gui_state.must_recompute_GPR = 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, original_trajectories,
+ demos);
+
+ draw_GPR_viewport(viewport_GPR, GPR_points, GPR_models, demos);
+
+ draw_timeline_viewport(window_size, viewport_x1, original_trajectories, demos,
+ GPR_points, GPR_sigma, 1);
+
+ draw_timeline_viewport(window_size, viewport_x2, original_trajectories, demos,
+ GPR_points, GPR_sigma, 2);
+
+
+ // Window: Demonstrations
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 84));
+ 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 (ImGui::Button("Clear")) {
+ original_trajectories.clear();
+ demos.clear();
+ GPR_points = mat();
+ GPR_sigma.clear();
+ GPR_models.clear();
+ }
+
+ ImGui::SameLine();
+ ImGui::Text(" ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ int previous_offset = gui_state.parameter_missing_data_offset;
+ ImGui::SliderInt("Missing data offset", &gui_state.parameter_missing_data_offset,
+ std::max(parameters.nb_data / 8 - 1, 1),
+ std::min(parameters.nb_data - (gui_state.parameter_missing_data_length + 1), parameters.nb_data - 2));
+
+ int previous_length = gui_state.parameter_missing_data_length;
+ ImGui::SliderInt("Missing data length", &gui_state.parameter_missing_data_length,
+ 1,
+ std::min(parameters.nb_data / 2, parameters.nb_data - (gui_state.parameter_missing_data_offset + 1)));
+
+ if ((gui_state.parameter_missing_data_offset != previous_offset) ||
+ (gui_state.parameter_missing_data_length != previous_length)) {
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::End();
+
+
+ // Window: GPR
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - window_size.win_width / 2, 0));
+ ImGui::Begin("GPR", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("GPR");
+
+ ImGui::End();
+
+
+ // Window: Timeline x1
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, window_size.win_height / 2));
+ ImGui::Begin("Timeline: x1", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline: x1");
+
+ ImGui::End();
+
+
+ // Window: Timeline x2
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - window_size.win_width / 2,
+ window_size.win_height / 2));
+ ImGui::Begin("Timeline: x2", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline: x2");
+
+ ImGui::End();
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(440, 170));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2, (window_size.win_height - 170) / 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 data", &gui_state.parameter_nb_data, 10, 30);
+ ImGui::SliderInt("Nb data for reproduction", &gui_state.parameter_nb_data_reproduction, 100, 300);
+ ImGui::SliderFloat("RBF parameter 1", &gui_state.parameter_p[0], 1, 1000);
+ ImGui::SliderFloat("RBF parameter 2", &gui_state.parameter_p[1], 1, 100);
+ ImGui::SliderFloat("RBF parameter 3", &gui_state.parameter_p[2], .01, 10);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if ((mouse_x <= window_size.win_width / 2) &&
+ (mouse_y > 84) && (mouse_y <= window_size.win_height / 2))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else if (gui_state.is_drawing_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+
+ 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) {
+
+ mat trajectory(2, current_trajectory.size());
+ for (size_t i = 0; i < current_trajectory.size(); ++i) {
+ trajectory(0, i) = current_trajectory[i](0);
+ trajectory(1, i) = current_trajectory[i](1);
+ }
+
+ demos.push_back(sample_trajectory(trajectory, time_steps));
+
+ original_trajectories.push_back(trajectory);
+
+ gui_state.must_recompute_GPR = true;
+ }
+
+ current_trajectory.clear();
+ }
+ }
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_HSMM_batchLQR01.cpp b/src/demo_HSMM_batchLQR01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5dc42041e8ba337400c434f9dd0efb76ba0be7a
--- /dev/null
+++ b/src/demo_HSMM_batchLQR01.cpp
@@ -0,0 +1,795 @@
+/*
+ * demo_HSMM_batch01.cpp
+ *
+ * Online hsmm learning.
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+#include <stdio.h>
+#include <armadillo>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <window_utils.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_data; // Number of datapoints in a trajectory
+ int nb_states; // Number of hidden states in the HSMM
+ double rfactor; // Control cost in LQR
+ double dt; // Time step duration
+};
+
+
+//-----------------------------------------------------------------------------
+// Model trained using the algorithm
+//-----------------------------------------------------------------------------
+struct model_t {
+ parameters_t parameters; // Parameters used to train the model
+
+ vector_list_t mu;
+ matrix_list_t sigma;
+ mat transitions;
+ vec states_priors;
+ mat H;
+};
+
+
+//-----------------------------------------------------------------------------
+// Likelihood of datapoint(s) to be generated by a Gaussian parameterized by
+// center and covariance.
+//
+// Inputs:
+// - Data: D x N array representing N datapoints of D dimensions.
+// - Mu: D x 1 vector representing the center of the Gaussian.
+// - Sigma: D x D array representing the covariance matrix of the Gaussian.
+//
+// Output:
+// - prob: 1 x N vector representing the likelihood of the N datapoints.
+//-----------------------------------------------------------------------------
+arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
+
+ int nbVar = Data.n_rows;
+ int nbData = Data.n_cols;
+ Data = Data.t() - repmat(Mu.t(), nbData, 1);
+
+ vec prob = sum((Data * inv(Sigma)) % Data, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
+
+ return prob;
+}
+
+
+//-----------------------------------------------------------------------------
+// Initialization of Gaussian Mixture Model (GMM) parameters by clustering
+// an ordered dataset into equal bins
+//-----------------------------------------------------------------------------
+void init_GMM_kbins(const matrix_list_t& data, model_t &model) {
+
+ // Regularization term to avoid numerical instability
+ const double diag_reg_fact = 1e-4;
+
+ model.mu.clear();
+ model.sigma.clear();
+
+ // Delimit the cluster bins
+ uvec timing_sep = conv_to<uvec>::from(
+ round(linspace<vec>(0, model.parameters.nb_data, model.parameters.nb_states + 1))
+ );
+
+ // Compute statistics for each bin
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ span id(timing_sep(i), timing_sep(i + 1) - 1);
+ int nb_ids = id.b - id.a + 1;
+
+ mat values(data[0].n_rows, data.size() * nb_ids);
+ for (int n = 0; n < data.size(); ++n)
+ values(span::all, span(n * nb_ids, (n + 1) * nb_ids - 1)) = data[n](span::all, id);
+
+ model.mu.push_back(mean(values, 1));
+ model.sigma.push_back(cov(values.t()) + eye(values.n_rows, values.n_rows) * diag_reg_fact);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Estimation of HMM parameters with an EM algorithm
+//-----------------------------------------------------------------------------
+void EM_HMM(const matrix_list_t& data, model_t &model) {
+
+ const int nb_var = data[0].n_rows;
+ const int nb_samples = data.size();
+ const int nb_data = nb_samples * model.parameters.nb_data;
+
+ const int nb_max_steps = 50; // Maximum number of iterations allowed
+ const int nb_min_steps = 5; // Minimum number of iterations allowed
+ const double max_diff_log_likelihood = 1e-4; // Likelihood increase threshold
+ // to stop the algorithm
+ const double diag_reg_fact = 1e-8; //Regularization term
+
+ mat all_data(nb_var, nb_data);
+ for (int i = 0; i < nb_samples; ++i)
+ all_data(span::all, span(i * model.parameters.nb_data, (i + 1) * model.parameters.nb_data - 1)) = data[i];
+
+ std::vector<double> log_likelihoods;
+
+ for (int iter = 0; iter < nb_max_steps; ++iter) {
+
+ vector_list_t c;
+ mat GAMMA = zeros(model.parameters.nb_states, nb_samples * model.parameters.nb_data);
+ mat GAMMA_INIT = zeros(model.parameters.nb_states, nb_samples);
+ mat GAMMA_TRK = zeros(model.parameters.nb_states, nb_samples * (model.parameters.nb_data - 1));
+ cube ZETA = zeros(model.parameters.nb_states, model.parameters.nb_states,
+ nb_samples * (model.parameters.nb_data - 1));
+
+ // E-step
+ for (int n = 0; n < nb_samples; ++n) {
+
+ // Emission probabilities
+ mat B(model.parameters.nb_states, model.parameters.nb_data);
+ for (int i = 0; i < model.parameters.nb_states; ++i)
+ B(i, span::all) = gaussPDF(data[n], model.mu[i], model.sigma[i]).t();
+
+ // Forward variable ALPHA (rescaled, to avoid underflow issues)
+ mat ALPHA(model.parameters.nb_states, model.parameters.nb_data);
+ vec c_(model.parameters.nb_data);
+
+ ALPHA(span::all, 0) = model.states_priors % B(span::all, 0);
+ c_(0) = 1.0 / sum(ALPHA(span::all, 0) + DBL_MIN);
+ ALPHA(span::all, 0) = ALPHA(span::all, 0) * c_(0);
+
+ for (int t = 1; t < model.parameters.nb_data; ++t) {
+ ALPHA(span::all, t) = (ALPHA(span::all, t - 1).t() * model.transitions).t() % B(span::all, t);
+ c_(t) = 1.0 / sum(ALPHA(span::all, t) + DBL_MIN);
+ ALPHA(span::all, t) = ALPHA(span::all, t) * c_(t);
+ }
+
+ c.push_back(c_);
+
+ // Backward variable BETA (rescaled)
+ mat BETA(model.parameters.nb_states, model.parameters.nb_data);
+
+ BETA(span::all, model.parameters.nb_data - 1) =
+ ones(model.parameters.nb_states, 1) * c_(model.parameters.nb_data - 1);
+
+ for (int t = model.parameters.nb_data - 2; t >= 0; --t) {
+ BETA(span::all, t) = model.transitions * (BETA(span::all, t + 1) % B(span::all, t + 1));
+ BETA(span::all, t) = min(BETA(span::all, t) * c_(t), ones(BETA.n_rows) * DBL_MAX);
+ }
+
+ // Intermediate variable GAMMA
+ mat GAMMA_ = (ALPHA % BETA) / repmat(sum(ALPHA % BETA) + DBL_MIN, model.parameters.nb_states, 1);
+
+ GAMMA(span::all, span(n * model.parameters.nb_data, (n + 1) * model.parameters.nb_data - 1)) = GAMMA_;
+ GAMMA_INIT(span::all, n) = GAMMA_(span::all, 0);
+ GAMMA_TRK(span::all, span(n * (model.parameters.nb_data - 1), (n + 1) * (model.parameters.nb_data - 1) - 1)) =
+ GAMMA_(span::all, span(0, model.parameters.nb_data - 2));
+
+ // Intermediate variable ZETA (fast version, by considering scaling factor)
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ for (int j = 0; j < model.parameters.nb_states; ++j) {
+ ZETA(span(i), span(j), span(n * (model.parameters.nb_data - 1),
+ (n + 1) * (model.parameters.nb_data - 1) - 1)) =
+ model.transitions(i, j) *
+ (ALPHA(i, span(0, model.parameters.nb_data - 2)) %
+ B(j, span(1, model.parameters.nb_data - 1)) %
+ BETA(j, span(1, model.parameters.nb_data - 1))
+ );
+ }
+ }
+ }
+
+ model.H = GAMMA / repmat(sum(GAMMA, 1) + DBL_MIN, 1, GAMMA.n_cols);
+
+ // M-step
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+
+ // Update the centers
+ model.mu[i] = all_data * model.H(i, span::all).t();
+
+ // Update the covariance matrices
+ mat data_tmp = all_data - repmat(model.mu[i], 1, nb_data);
+ model.sigma[i] = data_tmp * diagmat(model.H(i, span::all)) * data_tmp.t() + // Eq. (54) Rabiner
+ eye(nb_var, nb_var) * diag_reg_fact; // Regularization term
+ }
+
+ // Update initial state probability vector
+ model.states_priors = mean(GAMMA_INIT, 1);
+
+ // Update transition probabilities
+ model.transitions = mat(sum(ZETA, 2)) / repmat(sum(GAMMA_TRK, 1) + DBL_MIN, 1, model.parameters.nb_states);
+
+ // Compute the average log-likelihood through the ALPHA scaling factors
+ log_likelihoods.push_back(0.0);
+ for (int n = 0; n < nb_samples; ++n)
+ log_likelihoods[iter] = log_likelihoods[iter] - sum(log(c[n]));
+
+ log_likelihoods[iter] = log_likelihoods[iter] / nb_samples;
+
+ // Stop the algorithm if EM converged
+ if (iter >= nb_min_steps) {
+ if (log_likelihoods[iter] - log_likelihoods[iter - 1] < max_diff_log_likelihood)
+ break;
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Learn the model from the demonstrations
+//-----------------------------------------------------------------------------
+void learn(const matrix_list_t& data, model_t &model) {
+
+ init_GMM_kbins(data, model);
+
+ // Left-right model initialization
+ model.transitions = zeros(model.parameters.nb_states, model.parameters.nb_states);
+
+ for (int i = 0; i < model.parameters.nb_states - 1; ++i) {
+ model.transitions(i, i) = 1.0 - (double) model.parameters.nb_states / model.parameters.nb_data;
+ model.transitions(i, i + 1) = (double) model.parameters.nb_states / model.parameters.nb_data;
+ }
+
+ model.transitions(model.parameters.nb_states - 1, model.parameters.nb_states - 1) = 1.0;
+
+ model.states_priors = zeros(model.parameters.nb_states);
+ model.states_priors(0) = 1.0;
+
+ EM_HMM(data, model);
+
+ // Removal of self-transition (for HSMM representation) and normalization
+ model.transitions = model.transitions - diagmat(model.transitions) +
+ eye(model.parameters.nb_states, model.parameters.nb_states) * DBL_MIN;
+
+ model.transitions(model.parameters.nb_states - 1, model.parameters.nb_states - 1) = 1.0;
+
+ model.transitions = model.transitions / repmat(sum(model.transitions, 1), 1, model.parameters.nb_states);
+}
+
+
+//-----------------------------------------------------------------------------
+// Compute a reproduction using batch LQR
+//-----------------------------------------------------------------------------
+mat compute_LQR(const model_t& model, const vec& start_point) {
+
+ // Minimum variance of state duration (regularization term)
+ const double min_sigma_Pd = 2e-1;
+
+ // Number of maximum duration step to consider in the HSMM (2.5 is a safety factor)
+ const int nbD = round(2.5f * (float) model.parameters.nb_data / model.parameters.nb_states);
+
+ // Dimension of position data (here: x1,x2)
+ const int nb_var_pos = 2;
+
+
+ // Post-estimation of the state duration from data (for HSMM representation)
+ //--------------------------------------------------------------------------
+ std::vector< std::vector<double> > st(model.parameters.nb_states);
+
+ urowvec hmax = index_max(model.H, 0);
+
+ unsigned int current_state = hmax(0);
+ unsigned int count = 1;
+
+ for (int t = 0; t < hmax.size(); ++t) {
+ if (hmax(t) == current_state) {
+ ++count;
+ } else {
+ st[current_state].push_back(log(count));
+ count = 1;
+ current_state = hmax(t);
+ }
+ }
+ st[current_state].push_back(log(count));
+
+ // Compute state duration as Gaussian distribution
+ vector_list_t Mu_Pd;
+ matrix_list_t Sigma_Pd;
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ if (!st[i].empty()) {
+ vec st_(st[i].size());
+ for (int j = 0; j < st[i].size(); ++j)
+ st_(j) = st[i][j];
+
+ Mu_Pd.push_back(vec({ mean(st_) }));
+ Sigma_Pd.push_back(cov(st_) + min_sigma_Pd);
+ } else {
+ Mu_Pd.push_back(vec({ 0.0 }));
+ Sigma_Pd.push_back(cov(vec({ 0.0 })) + min_sigma_Pd);
+ }
+ }
+
+
+ // Reconstruction of states probability sequence
+ //----------------------------------------------
+
+ // Precomputation of duration probabilities
+ mat Pd(model.parameters.nb_states, nbD);
+
+ vec logs = log(linspace<vec>(0, nbD - 1, nbD));
+
+ for (int i = 0; i < model.parameters.nb_states; ++i)
+ Pd(i, span::all) = gaussPDF(logs.t(), Mu_Pd[i], Sigma_Pd[i]).t();
+
+ // Reconstruction of states sequence
+ mat h = zeros(model.parameters.nb_states, model.parameters.nb_data);
+
+ for (int t = 0; t < model.parameters.nb_data; ++t) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ if (t < nbD)
+ h(i, t) = model.states_priors(i) * Pd(i, t);
+
+ for (int d = 0; d < std::min(t - 1, nbD); ++d)
+ h(i, t) = h(i, t) + mat(h(span::all, t - d).t() * model.transitions(span::all, i) * Pd(i, d))(0, 0);
+ }
+ }
+
+ h = h / repmat(sum(h, 0) + DBL_MIN, model.parameters.nb_states, 1);
+
+
+ // Batch LQR reproduction
+ //-----------------------
+
+ // Dynamical System settings (discrete version), see Eq. (33)
+ mat A = kron(mat({{ 1.0, model.parameters.dt }, { 0.0, 1.0 }}), eye(nb_var_pos, nb_var_pos));
+ mat B = kron(mat({{ 0.0, model.parameters.dt }}).t(), eye(nb_var_pos, nb_var_pos));
+ mat C = kron(mat({{ 1.0, 0.0 }}), eye(nb_var_pos, nb_var_pos));
+
+ // Control cost matrix
+ mat R = eye(nb_var_pos, nb_var_pos) * model.parameters.rfactor;
+ R = kron(eye(model.parameters.nb_data - 1, model.parameters.nb_data - 1), R);
+
+ // Build CSx and CSu matrices for batch LQR, see Eq. (35)
+ mat CSu = zeros(nb_var_pos * model.parameters.nb_data, nb_var_pos * (model.parameters.nb_data - 1));
+ mat CSx = kron(ones(model.parameters.nb_data, 1), eye(nb_var_pos, nb_var_pos * 2));
+
+ mat M = zeros(B.n_rows, 2 * model.parameters.nb_data);
+
+ int n = 2 * model.parameters.nb_data - 2;
+ M(span::all, span(n, n + 1)) = B;
+
+ for (int n = 1; n < model.parameters.nb_data; ++n) {
+ span id1(n * nb_var_pos, (n + 1) * nb_var_pos - 1);
+ span id2(0, n * nb_var_pos - 1);
+ int n2 = 2 * model.parameters.nb_data - n * 2;
+
+ CSx.rows(id1) = CSx.rows(id1) * A;
+ CSu(id1, id2) = C * M(span::all, span(n2, n2 + n * 2 - 1));
+
+ M(span::all, span(n2 - 2, n2 - 1)) = A * M(span::all, span(n2, n2 + 1));
+ }
+
+ // Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (27)
+ urowvec qList = index_max(h, 0);
+
+ mat MuQ(nb_var_pos, qList.size());
+ mat sigma_(nb_var_pos, nb_var_pos * qList.size());
+
+ for (int i = 0; i < qList.size(); ++i) {
+ MuQ(span::all, i) = model.mu[qList(i)];
+ sigma_(span::all, span(i * nb_var_pos, (i + 1) * nb_var_pos - 1)) = model.sigma[qList(i)];
+ }
+
+ MuQ = reshape(MuQ, MuQ.n_elem, 1);
+
+ mat SigmaQ = (kron(ones(model.parameters.nb_data, 1), eye(nb_var_pos, nb_var_pos)) * sigma_) %
+ kron(eye(model.parameters.nb_data, model.parameters.nb_data), ones(nb_var_pos, nb_var_pos));
+
+ // Set matrices to compute the damped weighted least squares estimate
+ mat CSuInvSigmaQ = (pinv(mat(SigmaQ.t())) * CSu).t();
+ mat Rq = CSuInvSigmaQ * CSu + R;
+
+ // Reproductions
+ vec X = zeros(nb_var_pos * 2);
+ X(span(0, nb_var_pos - 1)) = start_point;
+
+ mat rq = CSuInvSigmaQ * (MuQ - CSx * X);
+ mat u = pinv(Rq) * rq;
+
+ return reshape(CSx * X + CSu * u, nb_var_pos, model.parameters.nb_data);
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+
+//-----------------------------------------------------------------------------
+// Colors of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const 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 },
+});
+
+
+//-----------------------------------------------------------------------------
+// Create a demonstration (with a length of 'timestamps.size()') from a
+// trajectory (of any length)
+//-----------------------------------------------------------------------------
+mat sample_trajectory(const vector_list_t& trajectory, const vec& time_steps) {
+
+ // Resampling of the trajectory
+ vec x(trajectory.size());
+ vec y(trajectory.size());
+ vec x2(trajectory.size());
+ vec y2(trajectory.size());
+
+ for (size_t i = 0; i < trajectory.size(); ++i) {
+ x(i) = trajectory[i](0);
+ y(i) = trajectory[i](1);
+ }
+
+ vec from_indices = linspace<vec>(0, trajectory.size() - 1, trajectory.size());
+ vec to_indices = linspace<vec>(0, trajectory.size() - 1, time_steps.size());
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ // Create the demonstration
+ mat demo(2, time_steps.size());
+ for (int i = 0; i < time_steps.size(); ++i) {
+ demo(0, i) = x2[i];
+ demo(1, i) = y2[i];
+ }
+
+ return demo;
+}
+
+
+//-----------------------------------------------------------------------------
+// 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 is currently drawing a new demonstration
+ bool is_drawing_demonstration;
+
+ // Indicates if the parameters were modified through the UI
+ bool are_parameters_modified;
+
+ // Indicates if the reproductions must be recomputed
+ bool must_recompute;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ // in parameters_t)
+ int parameter_nb_data;
+ int parameter_nb_states;
+};
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv){
+ arma_rng::set_seed_random();
+
+ // Model
+ model_t model;
+
+ // Parameters
+ model.parameters.nb_data = 200;
+ model.parameters.nb_states = 6;
+ model.parameters.rfactor = 1e-3;
+ model.parameters.dt = 0.01;
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 1200;
+ window_size.win_height = 600;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+
+
+ // 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);
+
+ GLFWwindow* window = create_window_at_optimal_size(
+ "Demo HSMM batch", window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+
+ // Setup OpenGL
+ gfx2::init();
+ 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);
+
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.are_parameters_modified = true;
+ gui_state.must_recompute = false;
+ gui_state.parameter_nb_data = model.parameters.nb_data;
+ gui_state.parameter_nb_states = model.parameters.nb_states;
+
+
+ // Main loop
+ vec time_steps;
+ vector_list_t current_trajectory;
+ std::vector<vector_list_t> original_trajectories;
+ matrix_list_t demonstrations;
+ mat reproduction;
+
+ 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);
+
+ int previous_fb_width = window_size.fb_width;
+ int previous_fb_height = window_size.fb_height;
+
+ // Retrieve the new window size
+ glfwGetWindowSize(window, &window_size.win_width, &window_size.win_height);
+
+ // Retrieve the new framebuffer size
+ glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+
+ // Rescale the demonstrations so they stay in the window
+ if (!first) {
+ float scale_x = (float) window_size.fb_width / previous_fb_width;
+ float scale_y = (float) window_size.fb_height / previous_fb_height;
+
+ for (size_t i = 0; i < original_trajectories.size(); ++i) {
+ for (size_t j = 0; j < original_trajectories[i].size(); ++j) {
+ original_trajectories[i][j](0) *= scale_x;
+ original_trajectories[i][j](1) *= scale_y;
+ }
+ }
+
+ gui_state.are_parameters_modified = true;
+ time_steps.clear();
+ }
+ }
+
+
+ // If the parameters changed, learn the model again
+ if (gui_state.are_parameters_modified) {
+
+ if (time_steps.size() != gui_state.parameter_nb_data) {
+ demonstrations.clear();
+
+ time_steps = linspace<vec>(
+ 0, gui_state.parameter_nb_data - 1, gui_state.parameter_nb_data
+ );
+
+ for (size_t i = 0; i < original_trajectories.size(); ++i) {
+ mat sampled_trajectory = sample_trajectory(original_trajectories[i],
+ time_steps);
+ sampled_trajectory.row(0) /= window_size.fb_width;
+ sampled_trajectory.row(1) /= window_size.fb_height;
+
+ demonstrations.push_back(sampled_trajectory);
+ }
+ }
+
+ model.parameters.nb_data = gui_state.parameter_nb_data;
+ model.parameters.nb_states = gui_state.parameter_nb_states;
+
+ gui_state.are_parameters_modified = false;
+ gui_state.must_recompute = !demonstrations.empty();
+ }
+
+ if (!demonstrations.empty() && gui_state.must_recompute) {
+ learn(demonstrations, model);
+
+ mat all_start_points(2, demonstrations.size());
+ for (int i = 0; i < demonstrations.size(); ++i)
+ all_start_points(span::all, i) = demonstrations[i](span::all, 0);
+
+ reproduction = compute_LQR(model, mean(all_start_points, 1));
+
+ gui_state.must_recompute = false;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode( GL_PROJECTION );
+ glLoadIdentity();
+ glOrtho(0, window_size.fb_width, 0, window_size.fb_height, -1., 1.);
+ glMatrixMode( GL_MODELVIEW );
+ glLoadIdentity();
+
+ glPushMatrix();
+
+ // Draw the GMM states
+ if (!model.mu.empty()) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ vec mu(2);
+ mu(0) = model.mu[i](0) * window_size.fb_width;
+ mu(1) = model.mu[i](1) * window_size.fb_height;
+
+ mat scaling({
+ { (double) window_size.fb_width, 0.0 },
+ { 0.0, (double) window_size.fb_height }
+ });
+
+ gfx2::draw_gaussian(
+ conv_to<fvec>::from(COLORS.row(i % 10).t()), mu,
+ scaling * model.sigma[i] * scaling.t()
+ );
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ }
+
+ // Draw the currently created demonstration (if any)
+ if (current_trajectory.size() > 1)
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}), current_trajectory);
+
+ // Draw the demonstrations
+ for (size_t i = 0; i < demonstrations.size(); ++i) {
+ mat datapoints = demonstrations[i];
+ datapoints.row(0) *= window_size.fb_width;
+ datapoints.row(1) *= window_size.fb_height;
+
+ gfx2::draw_line(fvec({0.3f, 0.3f, 0.3f}), datapoints);
+ }
+
+ // Draw the reproduction
+ if (!demonstrations.empty()) {
+ mat scaled_reproduction = reproduction;
+ scaled_reproduction.row(0) *= window_size.fb_width;
+ scaled_reproduction.row(1) *= window_size.fb_height;
+
+ glLineWidth(4.0f);
+ gfx2::draw_line(fvec({1.0f, 0.0f, 0.0f}), scaled_reproduction);
+ glLineWidth(1.0f);
+ }
+
+ glPopMatrix();
+
+
+ // Control panel GUI
+ ImGui::SetNextWindowPos(ImVec2(2,2));
+ ImGui::SetNextWindowSize(ImVec2(300, 116));
+
+ ImGui::Begin("Control Panel", NULL,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings
+ );
+
+ ImGui::Text("Left-click to collect demonstrations");
+ ImGui::Text("");
+ ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 2, 20);
+ ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+
+ if (ImGui::Button("Apply"))
+ gui_state.are_parameters_modified = true;
+
+ ImGui::SameLine();
+
+ if (ImGui::Button("Clear")) {
+ demonstrations.clear();
+ original_trajectories.clear();
+ model.mu.clear();
+ model.sigma.clear();
+ model.transitions.clear();
+ model.states_priors.clear();
+ model.H.clear();
+ }
+
+ ImGui::End();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ if (!gui_state.is_drawing_demonstration) {
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1) && !ImGui::GetIO().WantCaptureMouse) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ gui_state.is_drawing_demonstration = true;
+
+ vec coords = gfx2::ui2fb({ mouse_x, mouse_y }, window_size);
+ current_trajectory.push_back(coords);
+ }
+ } else if (gui_state.is_drawing_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = gfx2::ui2fb({ mouse_x, mouse_y }, window_size);
+
+ 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) {
+ mat sampled_trajectory = sample_trajectory(current_trajectory, time_steps);
+ sampled_trajectory.row(0) /= window_size.fb_width;
+ sampled_trajectory.row(1) /= window_size.fb_height;
+
+ demonstrations.push_back(sampled_trajectory);
+
+ original_trajectories.push_back(current_trajectory);
+
+ gui_state.must_recompute = true;
+ }
+
+ current_trajectory.clear();
+ }
+ }
+ }
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_LWR_batch01.cpp b/src/demo_LWR_batch01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..61143d4ede1aa9ce43479d652a59202236941451
--- /dev/null
+++ b/src/demo_LWR_batch01.cpp
@@ -0,0 +1,718 @@
+/*
+ * demo_LWR_batch01.cpp
+ *
+ * Locally weighted regression (LWR) with radial basis functions (RBF), using batch
+ * computation
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+
+#include <stdio.h>
+#include <armadillo>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <imgui_internal.h>
+#include <window_utils.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_RBF; // Number of radial basis functions
+ int nb_data; // Number of datapoints in a trajectory
+};
+
+
+//-----------------------------------------------------------------------------
+// Likelihood of datapoint(s) to be generated by a Gaussian parameterized by
+// center and covariance.
+//-----------------------------------------------------------------------------
+arma::vec gaussPDF(vec Data, double Mu, double Sigma) {
+
+ int nb_data = Data.n_rows;
+ Data = Data - repmat(mat({ Mu }), nb_data, 1);
+
+ vec prob = sum((Data / Sigma) % Data, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(2 * datum::pi * Sigma + DBL_MIN);
+
+ return prob;
+}
+
+
+//-----------------------------------------------------------------------------
+// Locally weighted regression (LWR) with radial basis functions (RBF)
+//-----------------------------------------------------------------------------
+std::tuple<mat, mat> compute_LWR(const parameters_t& parameters, const mat& demonstration) {
+
+ // Set centroids equally spread in time
+ vec mu_RBF = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_RBF);
+
+ // Set constant homogeneous covariance
+ double sigma_RBF = 100.0;
+
+ mat H = zeros(parameters.nb_RBF, parameters.nb_data);
+ for (int i = 0; i < parameters.nb_RBF; ++i) {
+ H(i, span::all) = gaussPDF(linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data),
+ mu_RBF(i), sigma_RBF).t();
+ }
+
+ // Batch estimate (Least squares estimate of weights)
+ mat w = solve(H.t(), demonstration.t());
+
+ // Reconstruction of signal by weighted sum of radial basis functions
+ mat reproduction = w.t() * H;
+
+ return std::make_tuple(reproduction, H);
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+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;
+};
+
+
+//-----------------------------------------------------------------------------
+// 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) {
+
+ 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;
+}
+
+
+//-----------------------------------------------------------------------------
+// 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) = coords(0) + (float) viewport.width * 0.5f;
+ result(1) = coords(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 of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const 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 },
+});
+
+
+//-----------------------------------------------------------------------------
+// Create a demonstration (with a length of 'parameters.nb_data') from a
+// trajectory (of any length)
+//-----------------------------------------------------------------------------
+mat sample_trajectory(const vector_list_t& trajectory, const parameters_t& parameters) {
+
+ // Resampling of the trajectory
+ vec x(trajectory.size());
+ vec y(trajectory.size());
+
+ for (size_t i = 0; i < trajectory.size(); ++i) {
+ x(i) = trajectory[i](0);
+ y(i) = trajectory[i](1);
+ }
+
+ vec from_indices = linspace<vec>(0, trajectory.size() - 1, trajectory.size());
+ vec to_indices = linspace<vec>(0, trajectory.size() - 1, parameters.nb_data);
+
+ vec x2;
+ vec y2;
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ // Create the demonstration
+ mat demo(2, x2.size());
+ for (int i = 0; i < x2.size(); ++i) {
+ demo(0, i) = x2[i];
+ demo(1, i) = y2[i];
+ }
+
+ return demo;
+}
+
+
+//-----------------------------------------------------------------------------
+// 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 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;
+
+ // Indicates if the reproductions must be recomputed
+ bool must_recompute_LWR;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ // in parameters_t)
+ int parameter_nb_RBF;
+ int parameter_nb_data;
+};
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations & model" viewport
+//-----------------------------------------------------------------------------
+void draw_demo_viewport(const viewport_t& viewport,
+ const vector_list_t& current_trajectory,
+ const mat& demonstration,
+ const mat& reproduction) {
+
+ 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();
+
+ // Draw the demonstration
+ if (demonstration.n_cols > 0)
+ gfx2::draw_line({0.0f, 0.0f, 0.0f}, demonstration);
+ else if (current_trajectory.size() > 1)
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
+
+ // Draw the reproduction
+ if (reproduction.n_cols > 0) {
+ glLineWidth(4.0f);
+ gfx2::draw_line({1.0f, 0.0f, 0.0f}, reproduction);
+ glLineWidth(1.0f);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns the dimensions that a plot should have inside the provided viewport
+//-----------------------------------------------------------------------------
+ivec get_plot_dimensions(const viewport_t& viewport) {
+
+ const int MARGIN = 50;
+
+ ivec result(2);
+ result(0) = viewport.width - 2 * MARGIN;
+ result(1) = viewport.height / 2 - 2 * MARGIN;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "timeline" viewport
+//-----------------------------------------------------------------------------
+void draw_timeline_viewport(const gfx2::window_size_t& window_size,
+ const viewport_t& viewport,
+ const mat& demonstration, const mat& reproduction,
+ const mat& H, int dimension) {
+
+ 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();
+
+ ivec plot_dimensions = get_plot_dimensions(viewport);
+
+ const int MARGIN = 50;
+ const int TOP_OFFSET = 40;
+ const int MARGIN_TIMELINE_TOP = MARGIN + TOP_OFFSET;
+ const int MARGIN_TIMELINE_BOTTOM = MARGIN - TOP_OFFSET;
+
+
+ //_____ Timeline plot __________
+
+ ivec plot_top_left({ -plot_dimensions(0) / 2, viewport.height / 2 - MARGIN_TIMELINE_TOP });
+ ivec plot_bottom_right({ plot_dimensions(0) / 2, MARGIN_TIMELINE_BOTTOM });
+
+ // Axis labels
+ ui::begin("Text");
+
+ vec coords = fb2ui(vec({ -20.0, MARGIN_TIMELINE_BOTTOM - 5.0 }), window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ std::stringstream label;
+ label << "x" << dimension;
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, viewport.height / 4 - TOP_OFFSET + 20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), label.str(), ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ double scale_x = (double) plot_dimensions(0) / (demonstration.n_cols - 1);
+
+ // Check if there is something to display
+ if (demonstration.n_cols > 0) {
+
+ double scale_y;
+
+ if (dimension == 1)
+ scale_y = (double) plot_dimensions(1) / viewport.width;
+ else
+ scale_y = (double) plot_dimensions(1) / viewport.height;
+
+ // Draw the demonstration
+ arma::mat datapoints(2, demonstration.n_cols);
+ datapoints.row(0) = linspace<vec>(0, demonstration.n_cols - 1, demonstration.n_cols).t();
+ datapoints.row(1) = demonstration.row(dimension - 1);
+
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ datapoints(1, span::all) *= scale_y;
+ datapoints(1, span::all) += viewport.height / 4 - TOP_OFFSET;
+
+ glLineWidth(2.0f);
+ gfx2::draw_line({0.7f, 0.7f, 0.7f}, datapoints);
+ glLineWidth(1.0f);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ // Draw the reproduction
+ datapoints.row(1) = reproduction.row(dimension - 1);
+ datapoints(1, span::all) *= scale_y;
+ datapoints(1, span::all) += viewport.height / 4 - TOP_OFFSET;
+
+ glLineWidth(4.0f);
+ gfx2::draw_line({1.0f, 0.0f, 0.0f}, datapoints);
+ glLineWidth(1.0f);
+ }
+
+
+ //_____ RBFs plot __________
+
+ plot_top_left = ivec({ -plot_dimensions(0) / 2, -MARGIN });
+ plot_bottom_right = ivec({ plot_dimensions(0) / 2, -viewport.height / 2 + MARGIN });
+
+ // Axis labels
+ ui::begin("Text");
+
+ coords = fb2ui(vec({ -20.0, -viewport.height / 2 + (MARGIN - 5.0) }), window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, -viewport.height / 4 - 20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "h", ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ // Check if there is something to display
+ if (demonstration.n_cols > 0) {
+
+ double max_value = max(max(H));
+
+ int color_index = 0;
+ for (int i = 0; i < H.n_rows; ++i) {
+ arma::mat datapoints(2, H.n_cols);
+ datapoints.row(0) = linspace<vec>(0, H.n_cols - 1, H.n_cols).t();
+ datapoints.row(1) = H.row(i) / max_value;
+
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ datapoints(1, span::all) *= (double) plot_dimensions(1);
+ datapoints(1, span::all) -= viewport.height / 2 - MARGIN;
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ ++color_index;
+ if (color_index >= COLORS.n_rows)
+ color_index = 0;
+ }
+ }
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_RBF = 8;
+ parameters.nb_data = 100;
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ 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 Locally weighted regression (LWR)",
+ 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_demo;
+ viewport_t viewport_timeline;
+
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = false;
+ gui_state.must_recompute_LWR = false;
+ gui_state.parameter_nb_RBF = parameters.nb_RBF;
+ gui_state.parameter_nb_data = parameters.nb_data;
+
+
+ // Main loop
+ vector_list_t current_trajectory;
+ mat demonstration;
+ mat reproduction;
+ mat H;
+ int dimension = 1;
+
+ 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)) {
+
+ 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 / 2 - 1;
+ viewport_height = window_size.fb_height / 2 - 1;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demo, 0, window_size.fb_height - viewport_height,
+ window_size.fb_width, viewport_height);
+
+ setup_viewport(&viewport_timeline, 0, 0, window_size.fb_width, viewport_height);
+ }
+
+
+ // If the parameters changed, learn the model again
+ if (gui_state.are_parameters_modified) {
+
+ if (parameters.nb_data != gui_state.parameter_nb_data) {
+ parameters.nb_data = gui_state.parameter_nb_data;
+ demonstration = sample_trajectory(current_trajectory, parameters);
+ }
+
+ parameters.nb_RBF = gui_state.parameter_nb_RBF;
+
+ gui_state.must_recompute_LWR = true;
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Recompute the LWR (if necessary)
+ if (gui_state.must_recompute_LWR) {
+ std::tie(reproduction, H) = compute_LWR(parameters, demonstration);
+ gui_state.must_recompute_LWR = 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_demo_viewport(viewport_demo, current_trajectory,
+ demonstration, reproduction);
+
+ draw_timeline_viewport(window_size, viewport_timeline,
+ demonstration, reproduction, H, dimension);
+
+
+ // Window: Demonstration & reproduction
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Demonstration & reproduction", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Demonstration & reproduction ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ ImGui::End();
+
+
+ // Window: Timeline
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, window_size.win_height / 2));
+ ImGui::Begin("Timeline", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline ");
+ ImGui::SameLine();
+
+ ImGui::RadioButton("1", &dimension, 1);
+ ImGui::SameLine();
+ ImGui::RadioButton("2", &dimension, 2);
+
+ ImGui::End();
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(440, 106));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2,
+ (window_size.win_height - 106) / 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 RBF", &gui_state.parameter_nb_RBF, 2, 20);
+ ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if ((mouse_y > 36) && (mouse_y <= window_size.win_height / 2))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ current_trajectory.clear();
+ demonstration.clear();
+ reproduction.clear();
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demo);
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else if (gui_state.is_drawing_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demo);
+
+ 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) {
+ demonstration = sample_trajectory(current_trajectory, parameters);
+ gui_state.must_recompute_LWR = true;
+ }
+ }
+ }
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_LWR_iterative01.cpp b/src/demo_LWR_iterative01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cda48b99ccf0c9b56bad4d0bad55005e64e793fd
--- /dev/null
+++ b/src/demo_LWR_iterative01.cpp
@@ -0,0 +1,755 @@
+/*
+ * demo_LWR_iterative01.cpp
+ *
+ * Locally weighted regression (LWR) with radial basis functions (RBF), using iterative
+ * computation
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+
+#include <stdio.h>
+#include <armadillo>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <imgui_internal.h>
+#include <window_utils.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_RBF; // Number of radial basis functions
+ int nb_data; // Number of datapoints in a trajectory
+};
+
+
+//-----------------------------------------------------------------------------
+// Likelihood of datapoint(s) to be generated by a Gaussian parameterized by
+// center and covariance.
+//-----------------------------------------------------------------------------
+arma::vec gaussPDF(vec Data, double Mu, double Sigma) {
+
+ int nb_data = Data.n_rows;
+ Data = Data - repmat(mat({ Mu }), nb_data, 1);
+
+ vec prob = sum((Data / Sigma) % Data, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(2 * datum::pi * Sigma + DBL_MIN);
+
+ return prob;
+}
+
+
+//-----------------------------------------------------------------------------
+// Locally weighted regression (LWR) with radial basis functions (RBF)
+//-----------------------------------------------------------------------------
+std::tuple<matrix_list_t, mat> compute_LWR(const parameters_t& parameters,
+ const mat& demonstration) {
+
+ // Set centroids equally spread in time
+ vec mu_RBF = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_RBF);
+
+ // Set constant homogeneous covariance
+ double sigma_RBF = 100.0;
+
+ mat H = zeros(parameters.nb_RBF, parameters.nb_data);
+ for (int i = 0; i < parameters.nb_RBF; ++i) {
+ H(i, span::all) = gaussPDF(linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data),
+ mu_RBF(i), sigma_RBF).t();
+ }
+
+ // Incremental estimate (does not require matrix inversion)
+ mat w = zeros(parameters.nb_RBF, 2); // Initial estimate of w
+ mat iB = eye(parameters.nb_RBF, parameters.nb_RBF) * 1e3; // Initial estimate of iB (set to big value)
+
+ matrix_list_t reconstructions;
+
+ for (int t = 0; t < demonstration.n_cols; ++t) {
+
+ // New input data
+ mat V = H.col(t).t();
+
+ // New output data
+ mat C = demonstration.col(t).t();
+
+ // Kalman gain
+ mat K = iB * V.t() / mat(1.0 + V * iB * V.t())(0, 0);
+
+ // Update w
+ w = w + K * (C - V * w);
+
+ // Update iB
+ iB = iB - iB * V.t() / mat(1.0 + V * iB * V.t())(0, 0) * V * iB;
+
+ // Reconstruction of signal by weighted sum of radial basis functions
+ reconstructions.push_back(w.t() * H);
+ }
+
+ return std::make_tuple(reconstructions, H);
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+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;
+};
+
+
+//-----------------------------------------------------------------------------
+// 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) {
+
+ 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;
+}
+
+
+//-----------------------------------------------------------------------------
+// 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) = coords(0) + (float) viewport.width * 0.5f;
+ result(1) = coords(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 of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const 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 },
+});
+
+
+//-----------------------------------------------------------------------------
+// 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 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;
+
+ // Indicates if the reproductions must be recomputed
+ bool must_recompute_LWR;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ // in parameters_t)
+ int parameter_nb_RBF;
+ int parameter_nb_data;
+};
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations & model" viewport
+//-----------------------------------------------------------------------------
+void draw_demo_viewport(const viewport_t& viewport,
+ const vector_list_t& current_trajectory,
+ const mat& demonstration,
+ 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.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();
+
+ // Draw the demonstration
+ if (current_trajectory.size() > 1)
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
+ else if (demonstration.n_cols > 0)
+ gfx2::draw_line({0.0f, 0.0f, 0.0f}, demonstration);
+
+ // Draw the reproduction
+ if (!reproductions.empty()) {
+ glLineWidth(4.0f);
+ gfx2::draw_line({1.0f, 0.0f, 0.0f}, reproductions[reproductions.size() - 1]);
+ glLineWidth(1.0f);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns the dimensions that a plot should have inside the provided viewport
+//-----------------------------------------------------------------------------
+ivec get_plot_dimensions(const viewport_t& viewport) {
+
+ const int MARGIN = 50;
+
+ ivec result(2);
+ result(0) = viewport.width - 2 * MARGIN;
+ result(1) = viewport.height / 2 - 2 * MARGIN;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "timeline" viewport
+//-----------------------------------------------------------------------------
+void draw_timeline_viewport(const gfx2::window_size_t& window_size,
+ const viewport_t& viewport,
+ const parameters_t& parameters,
+ const mat& demonstration,
+ const matrix_list_t& reproductions,
+ const mat& H, int dimension,
+ gfx2::model_list_t &models) {
+
+ 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();
+
+ ivec plot_dimensions = get_plot_dimensions(viewport);
+
+ const int MARGIN = 50;
+ const int TOP_OFFSET = 40;
+ const int MARGIN_TIMELINE_TOP = MARGIN + TOP_OFFSET;
+ const int MARGIN_TIMELINE_BOTTOM = MARGIN - TOP_OFFSET;
+
+
+ //_____ Timeline plot __________
+
+ ivec plot_top_left({ -plot_dimensions(0) / 2, viewport.height / 2 - MARGIN_TIMELINE_TOP });
+ ivec plot_bottom_right({ plot_dimensions(0) / 2, MARGIN_TIMELINE_BOTTOM });
+
+ // Axis labels
+ ui::begin("Text");
+
+ vec coords = fb2ui(vec({ -20.0, MARGIN_TIMELINE_BOTTOM - 5.0 }), window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ std::stringstream label;
+ label << "x" << dimension;
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, viewport.height / 4 - TOP_OFFSET + 20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), label.str(), ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ double scale_x = (double) plot_dimensions(0) / (parameters.nb_data - 1);
+
+ // Check if there is something to display
+ if (demonstration.n_cols > 0) {
+
+ double scale_y;
+
+ if (dimension == 1)
+ scale_y = (double) plot_dimensions(1) / viewport.width;
+ else
+ scale_y = (double) plot_dimensions(1) / viewport.height;
+
+ arma::mat datapoints(2, parameters.nb_data);
+ datapoints.row(0) = linspace<vec>(0, parameters.nb_data - 1, parameters.nb_data).t();
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+
+ // Create 3D models that will be reused during further renderings for performance
+ // reasons
+ if (models.empty()) {
+ for (int t = 0; t < reproductions.size() - 1; ++t) {
+ datapoints.row(1) = reproductions[t].row(dimension - 1);
+
+ datapoints(1, span::all) *= scale_y;
+ datapoints(1, span::all) += viewport.height / 4 - TOP_OFFSET;
+
+ models.push_back(
+ gfx2::create_line(
+ fvec({0.8f, 0.8f, 0.8f}) - fvec({0.0f, 0.8f, 0.8f}) * t / parameters.nb_data,
+ datapoints
+ )
+ );
+ }
+ }
+
+ // Draw the iterative reproductions
+ for (int t = 0; t < models.size(); ++t) {
+ gfx2::draw(models[t]);
+ glClear(GL_DEPTH_BUFFER_BIT);
+ }
+
+ // Draw the demonstration
+ datapoints(1, span(0, demonstration.n_cols - 1)) = demonstration.row(dimension - 1);
+ datapoints(1, span::all) *= scale_y;
+ datapoints(1, span::all) += viewport.height / 4 - TOP_OFFSET;
+
+ glLineWidth(4.0f);
+ gfx2::draw_line({0.0f, 0.0f, 0.7f}, datapoints(span::all, span(0, demonstration.n_cols - 1)));
+ glLineWidth(1.0f);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ // Draw the final reproduction
+ datapoints.row(1) = reproductions[reproductions.size() - 1].row(dimension - 1);
+ datapoints(1, span::all) *= scale_y;
+ datapoints(1, span::all) += viewport.height / 4 - TOP_OFFSET;
+
+ glLineWidth(4.0f);
+ gfx2::draw_line({1.0f, 0.0f, 0.0f}, datapoints);
+ glLineWidth(1.0f);
+ }
+
+
+ //_____ RBFs plot __________
+
+ plot_top_left = ivec({ -plot_dimensions(0) / 2, -MARGIN });
+ plot_bottom_right = ivec({ plot_dimensions(0) / 2, -viewport.height / 2 + MARGIN });
+
+ // Axis labels
+ ui::begin("Text");
+
+ coords = fb2ui(vec({ -20.0, -viewport.height / 2 + (MARGIN - 5.0) }), window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, -viewport.height / 4 - 20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "h", ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ // Check if there is something to display
+ if (demonstration.n_cols > 0) {
+
+ double max_value = max(max(H));
+
+ int color_index = 0;
+ for (int i = 0; i < H.n_rows; ++i) {
+ arma::mat datapoints(2, H.n_cols);
+ datapoints.row(0) = linspace<vec>(0, H.n_cols - 1, H.n_cols).t();
+ datapoints.row(1) = H.row(i) / max_value;
+
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ datapoints(1, span::all) *= (double) plot_dimensions(1);
+ datapoints(1, span::all) -= viewport.height / 2 - MARGIN;
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ ++color_index;
+ if (color_index >= COLORS.n_rows)
+ color_index = 0;
+ }
+ }
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_RBF = 8;
+ parameters.nb_data = 100;
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ 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 Locally weighted regression (LWR)",
+ 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_demo;
+ viewport_t viewport_timeline;
+
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = false;
+ gui_state.must_recompute_LWR = false;
+ gui_state.parameter_nb_RBF = parameters.nb_RBF;
+ gui_state.parameter_nb_data = parameters.nb_data;
+
+
+ // Main loop
+ vector_list_t current_trajectory;
+ mat demonstration;
+ matrix_list_t reproductions;
+ mat H;
+ int dimension = 1;
+ gfx2::model_list_t models; // For performance reasons
+
+ 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)) {
+
+ 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 / 2 - 1;
+ viewport_height = window_size.fb_height / 2 - 1;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demo, 0, window_size.fb_height - viewport_height,
+ window_size.fb_width, viewport_height);
+
+ setup_viewport(&viewport_timeline, 0, 0, window_size.fb_width, viewport_height);
+ }
+
+
+ // If the parameters changed, learn the model again
+ if (gui_state.are_parameters_modified) {
+
+ if (parameters.nb_data > gui_state.parameter_nb_data)
+ demonstration = demonstration(span::all, span(0, gui_state.parameter_nb_data - 1));
+
+ parameters.nb_RBF = gui_state.parameter_nb_RBF;
+ parameters.nb_data = gui_state.parameter_nb_data;
+
+ gui_state.must_recompute_LWR = true;
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Recompute the LWR (if necessary)
+ if (gui_state.must_recompute_LWR) {
+ std::tie(reproductions, H) = compute_LWR(parameters, demonstration);
+
+ for (int i = 0; i < models.size(); ++i)
+ gfx2::destroy(models[i]);
+
+ models.clear();
+
+ gui_state.must_recompute_LWR = 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_demo_viewport(viewport_demo, current_trajectory,
+ demonstration, reproductions);
+
+ draw_timeline_viewport(window_size, viewport_timeline, parameters,
+ demonstration, reproductions, H, dimension, models);
+
+
+ // Window: Demonstration & reproduction
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Demonstration & reproduction", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Demonstration & reproduction ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ ImGui::End();
+
+
+ // Window: Timeline
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, window_size.win_height / 2));
+ ImGui::Begin("Timeline", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline ");
+ ImGui::SameLine();
+
+ int previous_dimension = dimension;
+
+ ImGui::RadioButton("1", &dimension, 1);
+ ImGui::SameLine();
+ ImGui::RadioButton("2", &dimension, 2);
+
+ ImGui::End();
+
+ if (dimension != previous_dimension) {
+ for (int i = 0; i < models.size(); ++i)
+ gfx2::destroy(models[i]);
+
+ models.clear();
+ }
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(440, 106));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2,
+ (window_size.win_height - 106) / 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 RBF", &gui_state.parameter_nb_RBF, 2, 20);
+ ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if ((mouse_y > 36) && (mouse_y <= window_size.win_height / 2))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ current_trajectory.clear();
+ demonstration.clear();
+ reproductions.clear();
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demo);
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else if (gui_state.is_drawing_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demo);
+
+ 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);
+
+ demonstration = mat(2, current_trajectory.size());
+ for (int i = 0; i < current_trajectory.size(); ++i) {
+ demonstration(0, i) = current_trajectory[i][0];
+ demonstration(1, i) = current_trajectory[i][1];
+ }
+
+ gui_state.must_recompute_LWR = true;
+ }
+
+ // Left mouse button release OR number of points reached: end the
+ // demonstration creation
+ if (!ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1) ||
+ (current_trajectory.size() >= parameters.nb_data)) {
+
+ current_trajectory.clear();
+ gui_state.is_drawing_demonstration = false;
+ }
+ }
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_MPC_batch01.cpp b/src/demo_MPC_batch01.cpp
index 1b2487fd2d35e91f21529e9342cb78d10c87ea86..cf7876ad6f621bd7d7175cc8dce7b54ba01c07cf 100644
--- a/src/demo_MPC_batch01.cpp
+++ b/src/demo_MPC_batch01.cpp
@@ -1,247 +1,261 @@
/*
* demo_MPC_batch01.cpp
*
- * Interactive MPC demo, with batch LQT and repulsive field test
+ * Interactive MPC demo, with batch LQR and repulsive field test
*
- * Fabien Crépon, Sylvain Calinon, 2017
+ * Fabien Crépon, Philip Abbet, Sylvain Calinon, 2017
*/
#include <stdio.h>
-#include <imgui.h>
-#include <imgui_impl_glfw_gl2.h>
+#include <armadillo>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
#include <gfx_ui.h>
#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
#include <window_utils.h>
-
-#include <armadillo>
-#include <mpc_utils.h>
#include <gl2ps.h>
using namespace arma;
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
- int win_width;
- int win_height;
- int fb_width;
- int fb_height;
+/***************************** ALGORITHM SECTION *****************************/
+
+//-----------------------------------------------------------------------------
+// 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_targets;
+ int nb_dimensions;
+ int order;
+ double global_scale; // global scale, avoids numerical issues in batch method
+ double end_weight; // forces movement to a stop (see stepwiseReference function)
+ float maximum_displacement; // maximum displacement, used to compute R diagonal
+ float stroke_duration; // duration of a stroke
+ double dt;
};
-//-------------------------------------------------------------------------
-// Converts some coordinates from UI-space to OpenGL-space
-//
-// UI coordinates range from (0, 0) to (win_width, win_height)
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-//-------------------------------------------------------------------------
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+//-----------------------------------------------
- result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
- result(1) = coords(1) * (float) window_size.fb_height / (float) window_size.win_height;
+mat compute_LQR(const parameters_t& parameters, const mat& Mu, const cube& Sigma) {
- return result;
-}
+ const double duration = parameters.stroke_duration * parameters.nb_targets;
+ const int n = (int) (duration / parameters.dt);
+ const int cDim = parameters.nb_dimensions * parameters.order;
-//-------------------------------------------------------------------------
-// Converts some coordinates from OpenGL-space to UI-space
-//
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-// UI coordinates range from (0, 0) to (win_width, win_height)
-//-------------------------------------------------------------------------
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+ // Integration with higher order Taylor series expansion
+ mat A, B;
+ makeIntegratorChain(&A, &B, parameters.order);
+ discretizeSystem(&A, &B, A, B, parameters.dt);
+ A = kron(A, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+ B = kron(B, eye(parameters.nb_dimensions, parameters.nb_dimensions));
- result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
- result(1) = coords(1) * (float) window_size.win_height / (float) window_size.fb_height;
+ // Reference
+ mat Q, MuQ;
+ stepwiseReference(&MuQ, &Q, Mu, Sigma, n, parameters.order,
+ parameters.nb_dimensions, parameters.end_weight);
+ MuQ *= parameters.global_scale;
+ Q /= parameters.global_scale * parameters.global_scale;
- return result;
-}
+ // r based on oscillatory movement displacement
+ double r = SHM_r(parameters.maximum_displacement, parameters.stroke_duration,
+ parameters.order);
-//-----------------------------------------------
+ mat R = kron(eye(n - 1, n - 1), eye(parameters.nb_dimensions, parameters.nb_dimensions) * r);
-static void error_callback(int error, const char* description) {
- fprintf(stderr, "Error %d: %s\n", error, description);
-}
+ ////////////////////////////////////
+ // Batch LQR
-//-----------------------------------------------
+ // Sx and Su matrices for batch LQR
+ mat Su = zeros(cDim * n, parameters.nb_dimensions * (n - 1));
+ mat Sx = kron(ones(n, 1),
+ eye(parameters.nb_dimensions * parameters.order,
+ parameters.nb_dimensions * parameters.order)
+ );
+ mat M = B;
+ for (int i = 1; i < n; i++) {
+ Sx.rows(i * cDim, n * cDim - 1) = Sx.rows(i * cDim, n * cDim - 1) * A;
+ Su.submat(i * cDim, 0, (i + 1) * cDim - 1, i * parameters.nb_dimensions - 1) = M;
+ M = join_horiz(A * M.cols(0, parameters.nb_dimensions - 1), M);
+ }
+
+ arma::vec x0 = MuQ.col(0);
+
+ // Flatten Mu's
+ mat Xi_hat = reshape(MuQ, cDim * n, 1);
+
+ mat SuInvSigmaQ = Su.t() * Q;
+
+ mat Rq, rq;
-void setOrtho( float w, float h )
-{
- glMatrixMode( GL_PROJECTION );
- glLoadIdentity();
- glOrtho( 0, 0+w, 0+h, 0, -1., 1.);
- glMatrixMode( GL_MODELVIEW );
- glLoadIdentity();
+ Rq = SuInvSigmaQ * Su + R;
+ rq = SuInvSigmaQ * (Xi_hat - Sx * x0);
+
+ // Least squares solution
+ vec u = pinv(Rq) * rq;;
+ mat Y = reshape(Sx * x0 + Su * u, cDim, n);
+
+ return Y.rows(0, parameters.nb_dimensions - 1) / parameters.global_scale;
}
-//-----------------------------------------------
-void trans2d_to_gauss(arma::vec* mu, arma::mat* Sigma, const ui::Trans2d& t2d,
- const window_size_t& window_size)
-{
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
+/****************************** HELPER FUNCTIONS *****************************/
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
- *Sigma = basis * basis.t();
- *mu = ui2fb(arma::vec({t2d.pos.x, t2d.pos.y}), window_size);
+static void error_callback(int error, const char* description) {
+ fprintf(stderr, "Error %d: %s\n", error, description);
}
//-----------------------------------------------
-void gauss_to_trans2d(ui::Trans2d *t2d, const arma::vec& mu, const arma::mat& Sigma,
- const window_size_t& window_size)
-{
- arma::vec ui_mu = fb2ui(mu, window_size);
+std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
+ const gfx2::window_size_t& window_size) {
+
+ vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
- t2d->pos.x = ui_mu(0);
- t2d->pos.y = ui_mu(1);
+ vec t_x({
+ gaussian_transforms.x.x * window_size.scale_x(),
+ gaussian_transforms.x.y * window_size.scale_y()
+ });
- arma::mat V;
- arma::vec d;
- arma::eig_sym(d, V, Sigma);
- arma::mat VD = V*diagmat(sqrt(d));
+ vec t_y({
+ gaussian_transforms.y.x * window_size.scale_x(),
+ gaussian_transforms.y.y * window_size.scale_y()
+ });
- arma::vec t_x = fb2ui(arma::vec({VD.col(0)(0), VD.col(1)(0)}), window_size);
- arma::vec t_y = fb2ui(arma::vec({VD.col(0)(1), VD.col(1)(1)}), window_size);
+ mat RG = {
+ { t_x(0), t_y(0) },
+ { -t_x(1), -t_y(1) }
+ };
- t2d->x.x = t_x(0);
- t2d->x.y = t_x(1);
- t2d->y.x = t_y(0);
- t2d->y.y = t_y(1);
+ mat sigma = RG * RG.t();
+
+ return std::make_tuple(mu, sigma);
}
//-----------------------------------------------
-void draw(const arma::mat& pts, int primitive=GL_LINE_STRIP)
-{
- glBegin(primitive);
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+void gauss_to_trans2d(const vec& mu, const mat& sigma,
+ const gfx2::window_size_t& window_size, ui::Trans2d &t2d) {
+
+ vec ui_mu = gfx2::fb2ui_centered(mu, window_size);
+
+ t2d.pos.x = ui_mu(0);
+ t2d.pos.y = ui_mu(1);
+
+ mat V;
+ vec d;
+ eig_sym(d, V, sigma);
+ mat VD = V * diagmat(sqrt(d));
+
+ t2d.x.x = VD.col(0)(0) / window_size.scale_x();
+ t2d.x.y = VD.col(1)(0) / window_size.scale_y();
+ t2d.y.x = VD.col(0)(1) / window_size.scale_x();
+ t2d.y.y = VD.col(1)(1) / window_size.scale_y();
}
//-----------------------------------------------
-arma::mat gradient_2d( arma::mat X, int order=1 )
-{
+arma::mat gradient_2d(arma::mat X, int order = 1) {
mat df = diff(X, 1, 1);
- mat db = fliplr( -diff(fliplr(X), 1, 1) );
- mat d = (df+db)/2;
- d = join_horiz(zeros(2,1), d);
+ mat db = fliplr(-diff(fliplr(X), 1, 1));
+
+ mat d = (df + db) / 2;
+ d = join_horiz(zeros(2, 1), d);
d.col(0) = df.col(0);
- d.col(d.n_cols-1) = db.col(db.n_cols-1);
- if(order>1)
- return gradient_2d(d, order-1);
+ d.col(d.n_cols - 1) = db.col(db.n_cols - 1);
+
+ if(order > 1)
+ return gradient_2d(d, order - 1);
+
return d;
}
//-----------------------------------------------
-void plot( float x, float y, float w, float h, const vec& v )
-{
+void plot(float x, float y, float w, float h, const vec& v, const fvec& color) {
float v_min = v.min();
float v_max = v.max();
- float inc = w / v.n_rows;
- glBegin(GL_LINE_STRIP);
- for (int i=0; i < v.n_rows; i++)
- {
- glVertex2f(x + inc*i, y - h * ((v[i] - v_min) / (v_max - v_min)));
- }
- glEnd();
-}
-
-//-----------------------------------------------
-
-void drawGauss(ui::Trans2d& t2d, const window_size_t& window_size)
-{
- static arma::mat circ(2,35);
- static bool circ_init = true;
- if(circ_init)
- {
- circ = join_cols(cos(linspace<rowvec>(0,2*datum::pi,35)),
- sin(linspace<rowvec>(0,2*datum::pi,35)));
- circ_init = false;
- }
-
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
-
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
-
- vec mu = ui2fb(vec({t2d.pos.x, t2d.pos.y}), window_size);
- arma::mat pts = basis * circ + arma::repmat(mu, 1, 35);
+ mat points(2, v.n_rows);
+ points(0, span::all) = linspace<vec>(0, w, v.n_rows).t() + x;
+ points(1, span::all) = (v.t() - v_min) / (v_max - v_min) * h + y;
- glBegin(GL_TRIANGLE_FAN);
- glVertex2f(mu(0), mu(1));
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+ gfx2::draw_line(color, points);
}
-//-----------------------------------------------
+
+/******************************* MAIN FUNCTION *******************************/
int main(int argc, char **argv){
arma_rng::set_seed_random();
- //--------------- Setup parameters ---------------
-
- double dt = 0.01;
- int order = 4;
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_targets = 5;
+ parameters.nb_dimensions = 2;
+ parameters.order = 4;
+ parameters.global_scale = 0.0001;
+ parameters.end_weight = 1.;
+ parameters.maximum_displacement = 10.1;
+ parameters.stroke_duration = 0.3;
+ parameters.dt = 0.01;
- int m = 5; // Number of targets
- double globScale = 0.0001; // global scale, avoids numerical issues in batch method
- double endWeight = 1.; //1e-10; // forces movement to a stop (see stepwiseReference function)
-
- float d=10.1; // maximum displacement, used to compute R diagonal
- float stroke_duration=0.3; // duration of a stroke
-
-
- //--------------- Setup of the rendering ---------------
// Take 4k screens into account (framebuffer size != window size)
- window_size_t window_size;
+ gfx2::window_size_t window_size;
window_size.win_width = 800;
window_size.win_height = 800;
window_size.fb_width = -1; // Will be known later
window_size.fb_height = -1;
- // Setup GUI
+
+ // Initialise GLFW
glfwSetErrorCallback(error_callback);
+
if (!glfwInit())
- exit(1);
+ 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 Batch MPC", window_size.win_width, window_size.win_height
);
glfwMakeContextCurrent(window);
+
+ // Setup GLSL
+ gfx2::init();
+ 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);
- ImVec4 clear_color = ImColor(255, 255, 255);
- //--------------- Main loop ---------------
-
- FILE* eps_file = NULL;
- bool saving_eps = false;
-
// Covariances
- mat Mu;
- cube Sigma;
+ mat Mu = zeros(2, parameters.nb_targets);
+ cube Sigma = zeros(2, 2, parameters.nb_targets);
// Gaussian widgets
- std::vector<ui::Trans2d> t2ds(m, ui::Trans2d());
+ std::vector<ui::Trans2d> t2ds(parameters.nb_targets, ui::Trans2d());
+
+ // Main loop
+ FILE* eps_file = NULL;
+ bool saving_eps = false;
+ bool must_recompute = true;
+ mat trajectory;
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
@@ -262,28 +276,46 @@ int main(int argc, char **argv){
// screens into account)
if (first && (window_size.fb_width > 0)) {
- Mu = arma::randu(2, m);
- Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) + 100;
- Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) + 100;
+ Mu = randu(2, parameters.nb_targets);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
randomCovariances(
&Sigma, Mu,
- arma::vec({ 50 * (float) window_size.fb_width / window_size.win_width,
- 50 * (float) window_size.fb_height / window_size.win_height
+ vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+ 100 * (float) window_size.fb_height / window_size.win_height
}),
- false, 0.0, 0.8
+ true, 0.0, 0.2
);
- for( int i = 0; i < m; i++ )
- gauss_to_trans2d(&t2ds[i], Mu.col(i), Sigma.slice(i), window_size);
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
+
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
+
+ fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+ t2ds[i].x.x = x(0);
+ t2ds[i].x.y = x(1);
+
+ fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+ t2ds[i].y.x = y(0);
+ t2ds[i].y.y = y(1);
+ }
}
}
+
+ // Recompute the LQR when needed
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ trajectory = compute_LQR(parameters, Mu, Sigma);
+ must_recompute = false;
+ }
+
+
// Start of rendering
ImGui_ImplGlfwGL2_NewFrame();
- if (saving_eps)
- {
+ if (saving_eps) {
eps_file = fopen("out.eps", "wb");
gl2psBeginPage("grab", "gl2psTestSimple", NULL, GL2PS_EPS, GL2PS_SIMPLE_SORT,
GL2PS_DRAW_BACKGROUND | GL2PS_USE_CURRENT_VIEWPORT,
@@ -291,181 +323,135 @@ int main(int argc, char **argv){
}
glViewport(0, 0, window_size.fb_width, window_size.fb_height);
- glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
- glClear(GL_COLOR_BUFFER_BIT);
+ glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- setOrtho(window_size.fb_width, window_size.fb_height);
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2, -1.0f, 1.0f);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
- //Model rendering
glPushMatrix();
- // Gauss UI
- ui::begin("Gaussian");
- arma::vec mu;
- arma::mat Sigm;
-
- for( int i = 0; i < m; i++ )
- {
- t2ds[i] = ui::affineSimple(i, t2ds[i]);
- trans2d_to_gauss(&mu, &Sigm, t2ds[i], window_size);
- Mu.col(i) = mu;
- Sigma.slice(i) = Sigm;
+ // Draw the gaussians
+ for( int i = 0; i < parameters.nb_targets; i++ ) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian_background(fvec({ 0.0f, 0.5f, 1.0f }), Mu.col(i), Sigma.slice(i));
}
- ui::end();
+ glClear(GL_DEPTH_BUFFER_BIT);
- // Parameters window
- int winw = 400;
- ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
- ImGui::Begin("Params", NULL, ImVec2(winw, 86), 0.5f,
- ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
- ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
+ // Draw the motor plan
+ glColor3f(0.5, 0.5 ,0.5);
+ gfx2::draw_line(fvec({ 0.5f, 0.5f, 0.5f }), Mu);
- if(ImGui::Button("Save EPS"))
- saving_eps = true;
-
- ImGui::SliderInt("Order", &order, 2, 7);
- ImGui::SliderFloat("Max Displacement", &d, 0.1, 100.);
- ImGui::End();
+ // Draw the trajectory
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), trajectory);
- // Recompute LQR
- int dim = 2;
- double duration = stroke_duration * m;
- int n = (int)(duration/dt);
- int cDim = dim * order;
-
- // system matrices
- mat A, B;
- makeIntegratorChain(&A, &B, order);
- discretizeSystem(&A, &B, A, B, dt);
- // make bi-variate
- A = kron(A, eye(dim, dim));
- B = kron( B, eye(dim, dim) );
-
- // reference
- mat Q, MuQ;
- stepwiseReference( &MuQ, &Q, Mu, Sigma, n, order, dim, endWeight );
- MuQ *= globScale;
- Q /= globScale*globScale;
-
- // r based on oscillatory movement displacement
- double r = SHM_r(d*globScale, stroke_duration, order);
- mat R = kron( eye(n-1, n-1),
- eye(dim, dim) * r
- );
+ // Plot derivatives magnitude
+ int plot_width = 200 * window_size.fb_width / window_size.win_width;
+ int plot_height = 100 * window_size.fb_height / window_size.win_height;
+ int plot_x = -window_size.fb_width / 2;
+ int plot_y = -window_size.fb_height / 2 + 25 * window_size.fb_height / window_size.win_height;
- ////////////////////////////////////
- // Batch LQR
+ mat dx = gradient_2d(trajectory, 1);
+ vec speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x, plot_y, plot_width, plot_height, speed, {1.0f, 0.0f, 0.0f});
- // Sx and Su matrices for batch LQR
- mat Su = zeros(cDim*n, dim*(n-1));
- mat Sx = kron( ones(n,1),
- eye(dim*order, dim*order)
- );
- mat M = B;
- for( int i = 1; i < n; i++ )
- {
- Sx.rows( i*cDim, n*cDim-1 ) =
- Sx.rows( i*cDim, n*cDim-1 ) * A;
- Su.submat( i*cDim, 0,
- (i+1)*cDim-1, i*dim-1 ) = M;
- M = join_horiz( A*M.cols(0, dim-1), M );
- }
+ ui::begin("Text");
+ ui::text(ImVec2(10, window_size.win_height - 25), "velocity magnitude",
+ ImVec4(1, 0, 0, 1));
+ ui::end();
- arma::vec x0 = MuQ.col(0);
+ dx = gradient_2d(trajectory, 2);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.0f, 1.0f});
- // Flatten Mu's
- mat Xi_hat = reshape(MuQ, cDim*n, 1);
+ ui::begin("Text");
+ ui::text(ImVec2(200 + 10, window_size.win_height - 25), "acceleration magnitude",
+ ImVec4(0, 0, 1, 1));
+ ui::end();
- mat SuInvSigmaQ = Su.t()*Q;
+ dx = gradient_2d(trajectory, 3);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + 2 * plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.5f, 0.0f});
- mat Rq, rq;
+ ui::begin("Text");
+ ui::text(ImVec2(400 + 10, window_size.win_height - 25), "jerk magnitude",
+ ImVec4(0, 0.5, 0, 1));
+ ui::end();
- Rq = SuInvSigmaQ*Su + R;
- rq = SuInvSigmaQ*(Xi_hat - Sx*x0);
+ glPopMatrix();
- // least squares solution
- vec u = solve(Rq,rq);
- mat Y = reshape( Sx*x0 + Su*u, cDim, n );
- mat Xi = Y.submat(0,0, dim-1, n-1) / globScale;
- //////////////////////////////////////
+ // Gaussians UI
+ ui::begin("Gaussian");
- glEnable( GL_BLEND );
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ t2ds[i] = ui::affineSimple(i, t2ds[i]);
- // draw gaussians
- glColor4f(0.0, 0.5f, 1.0f, 0.2f);
- for (int i = 0; i < m; i++)
- drawGauss(t2ds[i], window_size);
+ vec mu;
+ mat sigma;
+ std::tie(mu, sigma) = trans2d_to_gauss(t2ds[i], window_size);
- // draw motor plan
- glColor3f(0.5,0.5,0.5);
- draw(Mu);
+ must_recompute = must_recompute ||
+ (norm(mu - Mu.col(i)) > 1e-6) ||
+ (norm(sigma - Sigma.slice(i)) > 1e-6);
- // draw trajectory
- glColor3f(0, 0, 1);
- draw(Xi);
+ Mu.col(i) = mu;
+ Sigma.slice(i) = sigma;
+ }
- // plot derivatives magnitude
- int plot_width = 200 * window_size.fb_width / window_size.win_width;
- int plot_height = 100 * window_size.fb_height / window_size.win_height;
- int plot_y = window_size.fb_height - 25 * window_size.fb_height / window_size.win_height;
+ ui::end();
- glColor3f(1,0,0);
- mat dx = gradient_2d(Xi, 1);
- vec speed = sqrt(sum(dx%dx, 0)).t();
- plot(0, plot_y, plot_width, plot_height, speed);
- ui::begin("Text");
- ui::text(ImVec2(10, window_size.win_height - 25), "velocity magnitude",
- ImVec4(1,0,0,1));
- ui::end();
+ // Parameters window
+ int winw = 400;
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
+ ImGui::Begin("Params", NULL, ImVec2(winw, 86), 0.5f,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings
+ );
- glColor3f(0,0,1);
- dx = gradient_2d(Xi, 2);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(plot_width, plot_y, plot_width, plot_height, speed);
+ if (ImGui::Button("Save EPS"))
+ saving_eps = true;
- ui::begin("Text");
- ui::text(ImVec2(200 + 10, window_size.win_height - 25), "acceleration magnitude",
- ImVec4(0,0,1,1));
- ui::end();
+ int previous_order = parameters.order;
+ float previous_maximum_displacement = parameters.maximum_displacement;
- glColor3f(0,0.5,0);
- dx = gradient_2d(Xi, 3);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(2 * plot_width, plot_y, plot_width, plot_height, speed);
+ ImGui::SliderInt("Order", ¶meters.order, 2, 6);
+ ImGui::SliderFloat("Max Displacement", ¶meters.maximum_displacement, 0.1, 100.);
+ ImGui::End();
- ui::begin("Text");
- ui::text(ImVec2(400 + 10, window_size.win_height - 25), "jerk magnitude",
- ImVec4(0,0.5,0,1));
- ui::end();
+ must_recompute = must_recompute ||
+ (parameters.order != previous_order) ||
+ !gfx2::is_close(parameters.maximum_displacement, previous_maximum_displacement);
- glPopMatrix();
- // Render UI
+ // GUI rendering
ImGui::Render();
ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
- // save eps
- if(eps_file)
- {
+ // Save eps
+ if (eps_file) {
glFlush();
gl2psEndPage();
fclose(eps_file);
- eps_file=NULL;
- saving_eps=false;
+ eps_file = NULL;
+ saving_eps = false;
}
glfwSwapBuffers(window);
- //Keyboard input
+ // Keyboard input
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
}
- //Cleanup
+ // Cleanup
ImGui_ImplGlfwGL2_Shutdown();
glfwTerminate();
+
return 0;
}
diff --git a/src/demo_MPC_iterative01.cpp b/src/demo_MPC_iterative01.cpp
index 51b841a395cf7756672e0c559926495fc58e4cc0..2f8abb67fe15ae9f5e485d71a0ffdb220ea4234a 100644
--- a/src/demo_MPC_iterative01.cpp
+++ b/src/demo_MPC_iterative01.cpp
@@ -1,67 +1,115 @@
/*
* demo_MPC_iterative01.cpp
*
- * Interactive MPC demo, with iterative LQT
+ * Interactive MPC demo, with iterative LQR
*
- * Fabien Crépon, Sylvain Calinon, 2017
+ * Fabien Crépon, Philip Abbet, Sylvain Calinon, 2017
*/
#include <stdio.h>
-#include <imgui.h>
-#include <imgui_impl_glfw_gl2.h>
+#include <armadillo>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
#include <gfx_ui.h>
#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
#include <window_utils.h>
-#include <armadillo>
-#include <mpc_utils.h>
-
using namespace arma;
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
- int win_width;
- int win_height;
- int fb_width;
- int fb_height;
+/***************************** ALGORITHM SECTION *****************************/
+
+//-----------------------------------------------------------------------------
+// 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_targets;
+ int nb_dimensions;
+ int order;
+ double global_scale; // global scale, avoids numerical issues in batch method
+ double end_weight; // forces movement to a stop (see stepwiseReference function)
+ float maximum_displacement; // maximum displacement, used to compute R diagonal
+ float stroke_duration; // duration of a stroke
+ double dt;
};
-//-------------------------------------------------------------------------
-// Converts some coordinates from UI-space to OpenGL-space
-//
-// UI coordinates range from (0, 0) to (win_width, win_height)
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-//-------------------------------------------------------------------------
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
-
- result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
- result(1) = coords(1) * (float) window_size.fb_height / (float) window_size.win_height;
-
- return result;
-}
+//-----------------------------------------------
-//-------------------------------------------------------------------------
-// Converts some coordinates from OpenGL-space to UI-space
-//
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-// UI coordinates range from (0, 0) to (win_width, win_height)
-//-------------------------------------------------------------------------
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+mat compute_LQR(const parameters_t& parameters, const mat& Mu, const cube& Sigma) {
+
+ const double duration = parameters.stroke_duration * parameters.nb_targets;
+ const int n = (int) (duration / parameters.dt);
+ const int cDim = parameters.nb_dimensions * parameters.order;
+
+ // Integration with higher order Taylor series expansion
+ mat A, B;
+ makeIntegratorChain(&A, &B, parameters.order);
+ discretizeSystem(&A, &B, A, B, parameters.dt);
+ A = kron(A, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+ B = kron(B, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+
+ // Reference
+ mat Q, MuQ;
+ stepwiseReference(&MuQ, &Q, Mu, Sigma, n, parameters.order,
+ parameters.nb_dimensions, parameters.end_weight);
+ MuQ *= parameters.global_scale;
+ Q /= parameters.global_scale * parameters.global_scale;
+
+ // r based on oscillatory movement displacement
+ double r = SHM_r(parameters.maximum_displacement, parameters.stroke_duration,
+ parameters.order);
+
+ ////////////////////////////////////
+ // Iterative LQR
+ mat R = eye(parameters.nb_dimensions, parameters.nb_dimensions) * r;
+
+ cube P = zeros(cDim, cDim, n);
+ int i = n - 1;
+ P.slice(i) = Q.submat(span(i * cDim, (i + 1) * cDim - 1), span(i * cDim, (i + 1) * cDim - 1));
+
+ mat D = zeros(cDim, n);
+
+ // Riccati recursion
+ for (int i = n - 2; i >= 0; --i) {
+ mat Qi = Q.submat(span(i * cDim, (i + 1) * cDim - 1),span(i * cDim, (i + 1) * cDim - 1));
+ mat P2 = P.slice(i + 1);
+ P.slice(i) = Qi - (A.t() * (P2 * B * inv(B.t() * P2 * B + R) * B.t() * P2 - P2 ) * A);
+ D.col(i) = (A.t() - A.t() * P2 * B * inv(B.t() * P2 * B + R) * B.t()) *
+ (P2 * (A * MuQ.col(i) - MuQ.col(i + 1)) + D.col(i + 1));
+ }
- result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
- result(1) = coords(1) * (float) window_size.win_height / (float) window_size.fb_height;
+ // Compute trajectory
+ arma::vec x0 = MuQ.col(0);
+ mat Y = zeros(cDim, n);
+ vec x = x0;
+
+ for (int i = 0; i < n; ++i) {
+ Y.col(i) = x;
+ mat p = P.slice(i);
+ vec mu = MuQ.col(i);
+
+ mat G = inv(B.t() * p * B + R) * B.t();
+
+ // Feedback gain
+ mat K = G * p * A;
+ // Feedforward term
+ mat M = -G * (p * (A * mu - mu) + D.col(i));
+ // Control command (highest order derivative of system)
+ vec u = K * (mu - x) + M;
+ // New state
+ x = A * x + B * u;
+ }
- return result;
+ return Y.rows(0, parameters.nb_dimensions - 1) / parameters.global_scale;
}
-//-----------------------------------------------
+
+/****************************** HELPER FUNCTIONS *****************************/
static void error_callback(int error, const char* description){
fprintf(stderr, "Error %d: %s\n", error, description);
@@ -69,176 +117,152 @@ static void error_callback(int error, const char* description){
//-----------------------------------------------
-void setOrtho( float w, float h )
-{
- glMatrixMode( GL_PROJECTION );
- glLoadIdentity();
- glOrtho( 0, 0+w, 0+h, 0, -1., 1.);
- glMatrixMode( GL_MODELVIEW );
- glLoadIdentity();
-}
+std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
+ const gfx2::window_size_t& window_size) {
-//-----------------------------------------------
+ vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
-void trans2d_to_gauss(arma::vec* mu, arma::mat* Sigma, const ui::Trans2d& t2d,
- const window_size_t& window_size)
-{
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
+ vec t_x({
+ gaussian_transforms.x.x * window_size.scale_x(),
+ gaussian_transforms.x.y * window_size.scale_y()
+ });
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
- *Sigma = basis * basis.t();
- *mu = ui2fb(arma::vec({t2d.pos.x, t2d.pos.y}), window_size);
-}
+ vec t_y({
+ gaussian_transforms.y.x * window_size.scale_x(),
+ gaussian_transforms.y.y * window_size.scale_y()
+ });
-//-----------------------------------------------
+ mat RG = {
+ { t_x(0), t_y(0) },
+ { -t_x(1), -t_y(1) }
+ };
-void gauss_to_trans2d(ui::Trans2d *t2d, const arma::vec& mu, const arma::mat& Sigma,
- const window_size_t& window_size)
-{
- arma::vec ui_mu = fb2ui(mu, window_size);
+ mat sigma = RG * RG.t();
- t2d->pos.x = ui_mu(0);
- t2d->pos.y = ui_mu(1);
+ return std::make_tuple(mu, sigma);
+}
- arma::mat V;
- arma::vec d;
- arma::eig_sym(d, V, Sigma);
- arma::mat VD = V*diagmat(sqrt(d));
+//-----------------------------------------------
- arma::vec t_x = fb2ui(arma::vec({VD.col(0)(0), VD.col(1)(0)}), window_size);
- arma::vec t_y = fb2ui(arma::vec({VD.col(0)(1), VD.col(1)(1)}), window_size);
+void gauss_to_trans2d(const vec& mu, const mat& sigma,
+ const gfx2::window_size_t& window_size, ui::Trans2d &t2d) {
- t2d->x.x = t_x(0);
- t2d->x.y = t_x(1);
- t2d->y.x = t_y(0);
- t2d->y.y = t_y(1);
-}
+ vec ui_mu = gfx2::fb2ui_centered(mu, window_size);
-//-----------------------------------------------
+ t2d.pos.x = ui_mu(0);
+ t2d.pos.y = ui_mu(1);
-void draw(const arma::mat& pts, int primitive=GL_LINE_STRIP)
-{
- glBegin(primitive);
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+ mat V;
+ vec d;
+ eig_sym(d, V, sigma);
+ mat VD = V * diagmat(sqrt(d));
+
+ t2d.x.x = VD.col(0)(0) / window_size.scale_x();
+ t2d.x.y = VD.col(1)(0) / window_size.scale_y();
+ t2d.y.x = VD.col(0)(1) / window_size.scale_x();
+ t2d.y.y = VD.col(1)(1) / window_size.scale_y();
}
//-----------------------------------------------
-arma::mat gradient_2d( arma::mat X, int order=1 )
-{
+arma::mat gradient_2d(arma::mat X, int order = 1) {
mat df = diff(X, 1, 1);
- mat db = fliplr( -diff(fliplr(X), 1, 1) );
- mat d = (df+db)/2;
- d = join_horiz(zeros(2,1), d);
+ mat db = fliplr(-diff(fliplr(X), 1, 1));
+
+ mat d = (df + db) / 2;
+ d = join_horiz(zeros(2, 1), d);
d.col(0) = df.col(0);
- d.col(d.n_cols-1) = db.col(db.n_cols-1);
- if(order>1)
- return gradient_2d(d, order-1);
+ d.col(d.n_cols - 1) = db.col(db.n_cols - 1);
+
+ if(order > 1)
+ return gradient_2d(d, order - 1);
+
return d;
}
//-----------------------------------------------
-void plot( float x, float y, float w, float h, const vec& v )
-{
+void plot(float x, float y, float w, float h, const vec& v, const fvec& color) {
float v_min = v.min();
float v_max = v.max();
- float inc = w / v.n_rows;
- glBegin(GL_LINE_STRIP);
- for (int i=0; i < v.n_rows; i++)
- {
- glVertex2f(x + inc*i, y - h * ((v[i] - v_min) / (v_max - v_min)));
- }
- glEnd();
-}
-
-//-----------------------------------------------
-
-void drawGauss(ui::Trans2d& t2d, const window_size_t& window_size)
-{
- static arma::mat circ(2,35);
- static bool circ_init = true;
- if(circ_init)
- {
- circ = join_cols(cos(linspace<rowvec>(0,2*datum::pi,35)),
- sin(linspace<rowvec>(0,2*datum::pi,35)));
- circ_init = false;
- }
-
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
-
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
- vec mu = ui2fb(vec({t2d.pos.x, t2d.pos.y}), window_size);
+ mat points(2, v.n_rows);
+ points(0, span::all) = linspace<vec>(0, w, v.n_rows).t() + x;
+ points(1, span::all) = (v.t() - v_min) / (v_max - v_min) * h + y;
- arma::mat pts = basis * circ + arma::repmat(mu, 1, 35);
-
- glBegin(GL_TRIANGLE_FAN);
- glVertex2f(mu(0), mu(1));
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+ gfx2::draw_line(color, points);
}
-//-----------------------------------------------
+
+/******************************* MAIN FUNCTION *******************************/
int main(int argc, char **argv){
arma_rng::set_seed_random();
- //--------------- Setup parameters ---------------
-
- double dt = 0.01;
- int order = 5;
-
- int m = 3; // Number of targets
- double globScale = 0.001; // global scale, avoids numerical issues in batch method
- double endWeight = 1.; //1e-10; // forces movement to a stop (see stepwiseReference function)
-
- float d=0.1; // maximum displacement, used to compute R diagonal
- float stroke_duration=0.3; // duration of a stroke
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_targets = 3;
+ parameters.nb_dimensions = 2;
+ parameters.order = 5;
+ parameters.global_scale = 0.001;
+ parameters.end_weight = 1.;
+ parameters.maximum_displacement = 0.1;
+ parameters.stroke_duration = 0.3;
+ parameters.dt = 0.01;
//--------------- Setup of the rendering ---------------
// Take 4k screens into account (framebuffer size != window size)
- window_size_t window_size;
+ gfx2::window_size_t window_size;
window_size.win_width = 800;
window_size.win_height = 800;
window_size.fb_width = -1; // Will be known later
window_size.fb_height = -1;
- //Setup GUI
+
+ // Initialise GLFW
glfwSetErrorCallback(error_callback);
+
if (!glfwInit())
- exit(1);
+ 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 Iterative MPC", window_size.win_width, window_size.win_height
);
glfwMakeContextCurrent(window);
- //Setup ImGui
+
+ // Setup GLSL
+ gfx2::init();
+ 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);
- ImVec4 clear_color = ImColor(255, 255, 255);
- //--------------- Main loop ---------------
-
// Covariances
- mat Mu;
- cube Sigma;
+ mat Mu = zeros(2, parameters.nb_targets);
+ cube Sigma = zeros(2, 2, parameters.nb_targets);
// Gaussian widgets
- std::vector<ui::Trans2d> t2ds(m, ui::Trans2d());
+ std::vector<ui::Trans2d> t2ds(parameters.nb_targets, ui::Trans2d());
+
+ // Main loop
+ bool must_recompute = true;
+ mat trajectory;
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
@@ -259,191 +283,163 @@ int main(int argc, char **argv){
// screens into account)
if (first && (window_size.fb_width > 0)) {
- Mu = arma::randu(2, m);
- Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) + 100;
- Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) + 100;
+ Mu = randu(2, parameters.nb_targets);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
randomCovariances(
- &Sigma, Mu,
- arma::vec({ 50 * (float) window_size.fb_width / window_size.win_width,
- 50 * (float) window_size.fb_height / window_size.win_height
- }),
- false, 0.0, 0.8
+ &Sigma, Mu,
+ vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+ 100 * (float) window_size.fb_height / window_size.win_height
+ }),
+ true, 0.0, 0.2
);
- for( int i = 0; i < m; i++ )
- gauss_to_trans2d(&t2ds[i], Mu.col(i), Sigma.slice(i), window_size);
- }
- }
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
- // Start of rendering
- ImGui_ImplGlfwGL2_NewFrame();
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
- glViewport(0, 0, window_size.fb_width, window_size.fb_height);
- glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
- glClear(GL_COLOR_BUFFER_BIT);
+ fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+ t2ds[i].x.x = x(0);
+ t2ds[i].x.y = x(1);
- setOrtho(window_size.fb_width, window_size.fb_height);
-
- //Model rendering
- glPushMatrix();
+ fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+ t2ds[i].y.x = y(0);
+ t2ds[i].y.y = y(1);
+ }
+ }
+ }
- // Gauss UI
- ui::begin("Gaussian");
- arma::vec mu;
- arma::mat Sigm;
- for( int i = 0; i < m; i++ )
- {
- t2ds[i] = ui::affineSimple(i, t2ds[i]);
- trans2d_to_gauss(&mu, &Sigm, t2ds[i], window_size);
- Mu.col(i) = mu;
- Sigma.slice(i) = Sigm;
+ // Recompute the LQR when needed
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ trajectory = compute_LQR(parameters, Mu, Sigma);
+ must_recompute = false;
}
- ui::end();
-
- // Parameters window
- int winw = 400;
- ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
- ImGui::Begin("Params", NULL, ImVec2(winw,60), 0.5f,
- ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
- ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
- ImGui::SliderInt("Order", &order, 2, 7);
- ImGui::SliderFloat("Max Displacement", &d, 0.05, 5.);
- ImGui::End();
- // Recompute LQR
- int dim = 2;
- double duration = stroke_duration * m;
- int n = (int)(duration/dt);
- int cDim = dim * order;
-
- // system matrices
- mat A, B;
- makeIntegratorChain(&A, &B, order);
- discretizeSystem(&A, &B, A, B, dt);
- // make bi-variate
- A = kron(A, eye(dim, dim));
- B = kron( B, eye(dim, dim) );
-
- // reference
- mat Q, MuQ, Q0, MuQ0;
- stepwiseReference( &MuQ, &Q, Mu, Sigma, n, order, dim, endWeight );
- MuQ *= globScale;
- Q /= globScale*globScale;
-
- // r based on oscillatory movement displacement
- double r = SHM_r(d, stroke_duration, order);
-
- ////////////////////////////////////
- // Iterative LQR
- mat R = eye(dim, dim) * r;
-
- cube P = zeros(cDim, cDim, n);
- int i = n-1;
- P.slice(i) = Q.submat(span(i*cDim,i*cDim+cDim-1),span(i*cDim,i*cDim+cDim-1));
-
- mat D = zeros(cDim, n);
- // Riccati recursion
- for( int i = n-2; i >=0; i-- )
- {
- mat Qi = Q.submat(span(i*cDim,i*cDim+cDim-1),span(i*cDim,i*cDim+cDim-1));
- mat P2 = P.slice(i+1);
- P.slice(i) = Qi - (A.t() * (P2 * B * inv(B.t() * P2 * B + R) * B.t() * P2 - P2 ) * A);
- D.col(i) = (A.t() - A.t() * P2 * B * inv(B.t() * P2 * B + R) * B.t())
- * (P2 * (A * MuQ.col(i) - MuQ.col(i+1)) + D.col(i+1));
- }
+ // Start of rendering
+ ImGui_ImplGlfwGL2_NewFrame();
- // compute trajectory
- arma::vec x0 = MuQ.col(0);
- mat Y = zeros(cDim, n);
- vec x = x0;
-
- for( int i = 0; i < n; i++ )
- {
- Y.col(i) = x;
- mat p = P.slice(i);
- vec mu = MuQ.col(i);
-
- mat G = inv( B.t() * p * B + R ) * B.t();
-
- // feedback gain
- mat K = G * p * A;
- // Feedforward term
- mat M = -G * (p * (A * mu - mu) + D.col(i));
- // Control command (highest order derivative of system)
- vec u = K * (mu - x) + M;
- // New state
- x = A*x + B*u;
- }
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- mat Xi = Y.submat(0,0, dim-1, n-1) / globScale;
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2, -1.0f, 1.0f);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
- //////////////////////////////////////
+ glPushMatrix();
- glEnable( GL_BLEND );
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ // Draw the gaussians
+ for( int i = 0; i < parameters.nb_targets; i++ ) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian_background(fvec({ 0.0f, 0.5f, 1.0f }), Mu.col(i), Sigma.slice(i));
+ }
+ glClear(GL_DEPTH_BUFFER_BIT);
- // draw gaussians
- glColor4f(0.0, 0.5f, 1.0f, 0.2f);
- for( int i = 0; i < m; i++ )
- drawGauss(t2ds[i], window_size);
+ // Draw the motor plan
+ glColor3f(0.5, 0.5 ,0.5);
+ gfx2::draw_line(fvec({ 0.5f, 0.5f, 0.5f }), Mu);
- // draw trajectory
- glColor3f(0, 0, 1);
- draw(Xi);
+ // Draw the trajectory
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), trajectory);
- // plot derivatives magnitude
+ // Plot derivatives magnitude
int plot_width = 200 * window_size.fb_width / window_size.win_width;
int plot_height = 100 * window_size.fb_height / window_size.win_height;
- int plot_y = window_size.fb_height - 25 * window_size.fb_height / window_size.win_height;
+ int plot_x = -window_size.fb_width / 2;
+ int plot_y = -window_size.fb_height / 2 + 25 * window_size.fb_height / window_size.win_height;
- glColor3f(1,0,0);
- mat dx = gradient_2d(Xi, 1);
- vec speed = sqrt(sum(dx%dx, 0)).t();
- plot(0, plot_y, plot_width, plot_height, speed);
+ mat dx = gradient_2d(trajectory, 1);
+ vec speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x, plot_y, plot_width, plot_height, speed, {1.0f, 0.0f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(10, window_size.win_height - 25), "velocity magnitude",
- ImVec4(1,0,0,1));
+ ImVec4(1, 0, 0, 1));
ui::end();
- glColor3f(0,0,1);
- dx = gradient_2d(Xi, 2);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 2);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.0f, 1.0f});
ui::begin("Text");
ui::text(ImVec2(200 + 10, window_size.win_height - 25), "acceleration magnitude",
- ImVec4(0,0,1,1));
+ ImVec4(0, 0, 1, 1));
ui::end();
- glColor3f(0,0.5,0);
- dx = gradient_2d(Xi, 3);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(2 * plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 3);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + 2 * plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.5f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(400 + 10, window_size.win_height - 25), "jerk magnitude",
- ImVec4(0,0.5,0,1));
+ ImVec4(0, 0.5, 0, 1));
ui::end();
glPopMatrix();
- // Render UI
+
+ // Gaussians UI
+ ui::begin("Gaussian");
+
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ t2ds[i] = ui::affineSimple(i, t2ds[i]);
+
+ vec mu;
+ mat sigma;
+ std::tie(mu, sigma) = trans2d_to_gauss(t2ds[i], window_size);
+
+ must_recompute = must_recompute ||
+ (norm(mu - Mu.col(i)) > 1e-6) ||
+ (norm(sigma - Sigma.slice(i)) > 1e-6);
+
+ Mu.col(i) = mu;
+ Sigma.slice(i) = sigma;
+ }
+
+ ui::end();
+
+
+ // Parameters window
+ int winw = 400;
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
+ ImGui::Begin("Params", NULL, ImVec2(winw, 60), 0.5f,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings
+ );
+
+ int previous_order = parameters.order;
+ float previous_maximum_displacement = parameters.maximum_displacement;
+
+ ImGui::SliderInt("Order", ¶meters.order, 2, 6);
+ ImGui::SliderFloat("Max Displacement", ¶meters.maximum_displacement, 0.1, 100.);
+ ImGui::End();
+
+ must_recompute = must_recompute ||
+ (parameters.order != previous_order) ||
+ !gfx2::is_close(parameters.maximum_displacement, previous_maximum_displacement);
+
+
+ // GUI rendering
ImGui::Render();
ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
glfwSwapBuffers(window);
- //Keyboard input
+ // Keyboard input
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
}
- //Cleanup
+ // Cleanup
ImGui_ImplGlfwGL2_Shutdown();
glfwTerminate();
+
return 0;
}
diff --git a/src/demo_MPC_semitied01.cpp b/src/demo_MPC_semitied01.cpp
index 610f5388cbd805399d550392c0d55d746a89cce4..d39005976ae1fad604ff2258fd8d4636833a2d0b 100644
--- a/src/demo_MPC_semitied01.cpp
+++ b/src/demo_MPC_semitied01.cpp
@@ -3,180 +3,144 @@
*
* Interactive MPC demo, demonstrates a GUI to edit semi-tied covariances (2d)
*
- * Fabien Crépon, Sylvain Calinon, 2017
+ * Fabien Crépon, Philip Abbet, Sylvain Calinon, 2017
*/
#include <stdio.h>
-#include <imgui.h>
-#include <imgui_impl_glfw_gl2.h>
+#include <armadillo>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
#include <gfx_ui.h>
#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
#include <window_utils.h>
-
-#include <armadillo>
-#include <mpc_utils.h>
#include <gl2ps.h>
using namespace arma;
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
- int win_width;
- int win_height;
- int fb_width;
- int fb_height;
+/***************************** ALGORITHM SECTION *****************************/
+
+//-----------------------------------------------------------------------------
+// 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_targets;
+ int nb_dimensions;
+ int order;
+ double global_scale; // global scale, avoids numerical issues in batch method
+ double end_weight; // forces movement to a stop (see stepwiseReference function)
+ float maximum_displacement; // maximum displacement, used to compute R diagonal
+ float stroke_duration; // duration of a stroke
+ double dt;
};
-//-------------------------------------------------------------------------
-// Converts some coordinates from UI-space to OpenGL-space
-//
-// UI coordinates range from (0, 0) to (win_width, win_height)
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-//-------------------------------------------------------------------------
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+//-----------------------------------------------
- result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
- result(1) = coords(1) * (float) window_size.fb_height / (float) window_size.win_height;
+mat compute_LQR(const parameters_t& parameters, const mat& Mu, const cube& Sigma) {
- return result;
-}
+ const double duration = parameters.stroke_duration * parameters.nb_targets;
+ const int n = (int) (duration / parameters.dt);
+ const int cDim = parameters.nb_dimensions * parameters.order;
-//-------------------------------------------------------------------------
-// Converts some coordinates from OpenGL-space to UI-space
-//
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-// UI coordinates range from (0, 0) to (win_width, win_height)
-//-------------------------------------------------------------------------
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+ // Integration with higher order Taylor series expansion
+ mat A, B;
+ makeIntegratorChain(&A, &B, parameters.order);
+ discretizeSystem(&A, &B, A, B, parameters.dt);
+ A = kron(A, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+ B = kron(B, eye(parameters.nb_dimensions, parameters.nb_dimensions));
- result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
- result(1) = coords(1) * (float) window_size.win_height / (float) window_size.fb_height;
+ // Reference
+ mat Q, MuQ;
+ stepwiseReference(&MuQ, &Q, Mu, Sigma, n, parameters.order,
+ parameters.nb_dimensions, parameters.end_weight);
+ MuQ *= parameters.global_scale;
+ Q /= parameters.global_scale * parameters.global_scale;
- return result;
-}
+ // r based on oscillatory movement displacement
+ double r = SHM_r(parameters.maximum_displacement, parameters.stroke_duration,
+ parameters.order);
-//-----------------------------------------------
+ mat R = kron(eye(n - 1, n - 1), eye(parameters.nb_dimensions, parameters.nb_dimensions) * r);
-static void error_callback(int error, const char* description){
- fprintf(stderr, "Error %d: %s\n", error, description);
-}
+ ////////////////////////////////////
+ // Batch LQR
-//-----------------------------------------------
+ // Sx and Su matrices for batch LQR
+ mat Su = zeros(cDim * n, parameters.nb_dimensions * (n - 1));
+ mat Sx = kron(ones(n, 1),
+ eye(parameters.nb_dimensions * parameters.order,
+ parameters.nb_dimensions * parameters.order)
+ );
+ mat M = B;
+ for (int i = 1; i < n; i++) {
+ Sx.rows(i * cDim, n * cDim - 1) = Sx.rows(i * cDim, n * cDim - 1) * A;
+ Su.submat(i * cDim, 0, (i + 1) * cDim - 1, i * parameters.nb_dimensions - 1) = M;
+ M = join_horiz(A * M.cols(0, parameters.nb_dimensions - 1), M);
+ }
-void setOrtho( float w, float h )
-{
- glMatrixMode( GL_PROJECTION );
- glLoadIdentity();
- glOrtho( 0, 0+w, 0+h, 0, -1., 1.);
- glMatrixMode( GL_MODELVIEW );
- glLoadIdentity();
-}
+ arma::vec x0 = MuQ.col(0);
-//-----------------------------------------------
+ // Flatten Mu's
+ mat Xi_hat = reshape(MuQ, cDim * n, 1);
-void gauss_to_trans2d(ui::Trans2d *t2d, const arma::vec& mu, const arma::mat& Sigma,
- const window_size_t& window_size)
-{
- arma::vec ui_mu = fb2ui(mu, window_size);
+ mat SuInvSigmaQ = Su.t() * Q;
- t2d->pos.x = ui_mu(0);
- t2d->pos.y = ui_mu(1);
+ mat Rq, rq;
- arma::mat V;
- arma::vec d;
- arma::eig_sym(d, V, Sigma);
- arma::mat VD = V*diagmat(sqrt(d));
+ Rq = SuInvSigmaQ * Su + R;
+ rq = SuInvSigmaQ * (Xi_hat - Sx * x0);
- arma::vec t_x = fb2ui(arma::vec({VD.col(0)(0), VD.col(1)(0)}), window_size);
- arma::vec t_y = fb2ui(arma::vec({VD.col(0)(1), VD.col(1)(1)}), window_size);
+ // Least squares solution
+ vec u = pinv(Rq) * rq;;
+ mat Y = reshape(Sx * x0 + Su * u, cDim, n);
- t2d->x.x = t_x(0);
- t2d->x.y = t_x(1);
- t2d->y.x = t_y(0);
- t2d->y.y = t_y(1);
+ return Y.rows(0, parameters.nb_dimensions - 1) / parameters.global_scale;
}
-//-----------------------------------------------
-void draw(const arma::mat& pts, int primitive=GL_LINE_STRIP)
-{
- glBegin(primitive);
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
}
//-----------------------------------------------
-arma::mat gradient_2d( arma::mat X, int order=1 )
-{
+arma::mat gradient_2d(arma::mat X, int order = 1) {
mat df = diff(X, 1, 1);
- mat db = fliplr( -diff(fliplr(X), 1, 1) );
- mat d = (df+db)/2;
- d = join_horiz(zeros(2,1), d);
+ mat db = fliplr(-diff(fliplr(X), 1, 1));
+
+ mat d = (df + db) / 2;
+ d = join_horiz(zeros(2, 1), d);
d.col(0) = df.col(0);
- d.col(d.n_cols-1) = db.col(db.n_cols-1);
- if(order>1)
- return gradient_2d(d, order-1);
+ d.col(d.n_cols - 1) = db.col(db.n_cols - 1);
+
+ if(order > 1)
+ return gradient_2d(d, order - 1);
+
return d;
}
//-----------------------------------------------
-void plot( float x, float y, float w, float h, const vec& v )
-{
+void plot(float x, float y, float w, float h, const vec& v, const fvec& color) {
float v_min = v.min();
float v_max = v.max();
- float inc = w / v.n_rows;
- glBegin(GL_LINE_STRIP);
- for (int i=0; i < v.n_rows; i++)
- {
- glVertex2f(x + inc*i, y - h * ((v[i] - v_min) / (v_max - v_min)));
- }
- glEnd();
-}
-
-//-----------------------------------------------
-
-void drawGauss(ui::Trans2d& t2d, const window_size_t& window_size)
-{
- static arma::mat circ(2,35);
- static bool circ_init = true;
- if(circ_init)
- {
- circ = join_cols(cos(linspace<rowvec>(0,2*datum::pi,35)),
- sin(linspace<rowvec>(0,2*datum::pi,35)));
- circ_init = false;
- }
-
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
+ mat points(2, v.n_rows);
+ points(0, span::all) = linspace<vec>(0, w, v.n_rows).t() + x;
+ points(1, span::all) = (v.t() - v_min) / (v_max - v_min) * h + y;
- vec mu = ui2fb(vec({t2d.pos.x, t2d.pos.y}), window_size);
-
- arma::mat pts = basis * circ + arma::repmat(mu, 1, 35);
-
- glBegin(GL_TRIANGLE_FAN);
- glVertex2f(mu(0), mu(1));
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+ gfx2::draw_line(color, points);
}
//-----------------------------------------------
-arma::mat semitiedBasis( const arma::vec& params )
-{
+arma::mat semitiedBasis(const arma::vec& params) {
float t1 = params[0];
float t2 = params[1];
return {{cos(t1), cos(t2)}, {sin(t1), sin(t2)}};
@@ -184,40 +148,46 @@ arma::mat semitiedBasis( const arma::vec& params )
//-----------------------------------------------
-arma::mat semitiedSigma( const arma::vec& params )
-{
+arma::mat semitiedSigma(const arma::vec& params) {
float h = params[2];
arma::mat H = semitiedBasis(params);
- return H*H.t()*h*h;
+
+ arma::mat sigma = H * H.t() * h * h;
+
+ // This is needed to compensate the fact that the coordinates system of the UI
+ // and OpenGL are inverted on the Y axis
+ sigma(0, 1) = -sigma(0, 1);
+ sigma(1, 0) = -sigma(1, 0);
+
+ return sigma;
}
//-----------------------------------------------
-arma::cube semitiedCovariances( const arma::mat& Mu, const arma::vec& params )
-{
+arma::cube semitiedCovariances(const arma::mat& Mu, const arma::vec& params) {
int m = Mu.n_cols;
arma::cube Sigma = arma::zeros(2,2,m);
arma::mat sigm = semitiedSigma(params);
- for( int i = 0; i < m; i++ )
- {
+
+ for (int i = 0; i < m; i++)
Sigma.slice(i) = sigm;
- }
+
return Sigma;
}
//-----------------------------------------------
arma::vec semitiedWidget(int id, arma::vec params, const arma::vec& pos,
- const window_size_t& window_size, float minLength=2.0,
- float maxLength=500.0)
-{
+ const gfx2::window_size_t& window_size, float minLength=2.0,
+ float maxLength=500.0) {
+
float h = params[2] * (float) window_size.win_height / window_size.fb_height;
arma::vec theta = params.subvec(0,1);
- for( int i = 0; i < 2; i++ )
- {
- ImVec2 theta_length = ui::lengthHandle(id+i, ImVec2(theta[i], h), 0.0, pos,
- ImVec2(-1000.0, minLength),
- ImVec2(1000.0, maxLength)
+
+ for (int i = 0; i < 2; i++) {
+ ImVec2 theta_length = ui::lengthHandle(
+ id + i, ImVec2(theta[i], h), 0.0, pos,
+ ImVec2(-1000.0, minLength), ImVec2(1000.0, maxLength)
);
params[i] = theta_length.x;
h = theta_length.y;
@@ -228,76 +198,98 @@ arma::vec semitiedWidget(int id, arma::vec params, const arma::vec& pos,
params[2] = h;
// draw ellipse
- static arma::mat circ(2,35);
+ static arma::mat circ(2, 35);
static bool circ_init = true;
- if(circ_init)
- {
- circ = join_cols(cos(linspace<rowvec>(0,2*datum::pi,35)),
- sin(linspace<rowvec>(0,2*datum::pi,35)));
+ if (circ_init) {
+ circ = join_cols(cos(linspace<rowvec>(0, 2 * datum::pi, 35)),
+ sin(linspace<rowvec>(0, 2 * datum::pi, 35)));
circ_init = false;
}
+
arma::mat basis = semitiedBasis(params);
- arma::mat pts = basis*circ*h + arma::repmat(ui2fb(pos, window_size), 1, 35);
- glColor4f(1.0,0.0,0.0,1.0);
- draw(pts);
- return params;
-}
-//-----------------------------------------------
+ arma::mat pts = basis * circ * h;
-int main(int argc, char **argv){
+ // This is needed to compensate the fact that the coordinates system of the UI
+ // and OpenGL are inverted on the Y axis
+ pts.row(1) = -pts.row(1);
- arma_rng::set_seed_random();
+ pts = pts + arma::repmat(ui2fb_centered(pos, window_size), 1, 35);
- //--------------- Setup parameters ---------------
+ gfx2::draw_line(fvec({ 1.0f, 0.0f, 0.0f }), pts);
- double dt = 0.01;
- int order = 4;
+ return params;
+}
- int m = 4; // Number of targets
- double globScale = 0.001; // global scale, avoids numerical issues in batch method
- double endWeight = 1.; //1e-10; // forces movement to a stop (see stepwiseReference function)
- float d=0.05; // maximum displacement, used to compute R diagonal
- float stroke_duration=0.3; // duration of a stroke
+/******************************* MAIN FUNCTION *******************************/
+int main(int argc, char **argv){
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_targets = 4;
+ parameters.nb_dimensions = 2;
+ parameters.order = 4;
+ parameters.global_scale = 0.001;
+ parameters.end_weight = 1.;
+ parameters.maximum_displacement = 0.05;
+ parameters.stroke_duration = 0.3;
+ parameters.dt = 0.01;
- //--------------- Setup of the rendering ---------------
// Take 4k screens into account (framebuffer size != window size)
- window_size_t window_size;
+ gfx2::window_size_t window_size;
window_size.win_width = 800;
window_size.win_height = 800;
window_size.fb_width = -1; // Will be known later
window_size.fb_height = -1;
- //Setup GUI
+
+ // Initialise GLFW
glfwSetErrorCallback(error_callback);
+
if (!glfwInit())
- exit(1);
+ 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 Semitied MPC", window_size.win_width, window_size.win_height
);
glfwMakeContextCurrent(window);
- //Setup ImGui
+
+ // Setup GLSL
+ gfx2::init();
+ 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);
- ImVec4 clear_color = ImColor(255, 255, 255);
- //--------------- Main loop ---------------
-
// Targets
mat Mu;
+ cube Sigma;
- // semi tied parameters (theta1, theta2, h)
+ // Semi-tied parameters (theta1, theta2, h)
arma::vec semitied_params;
+ // Main loop
FILE* eps_file = NULL;
bool saving_eps = false;
+ bool must_recompute = true;
+ mat trajectory;
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
@@ -318,22 +310,23 @@ int main(int argc, char **argv){
// screens into account)
if (first && (window_size.fb_width > 0)) {
- Mu = arma::randu(2, m);
- Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) + 100;
- Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) + 100;
+ Mu = randu(2, parameters.nb_targets);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
semitied_params = {
0.0,
- datum::pi/2,
+ datum::pi / 2.0,
100.0 * (float) window_size.fb_height / window_size.win_height
};
}
}
+
// Start of rendering
ImGui_ImplGlfwGL2_NewFrame();
- if(saving_eps)
+ if (saving_eps)
{
eps_file = fopen("out.eps", "wb");
gl2psBeginPage("grab", "gl2psTestSimple", NULL, GL2PS_EPS, GL2PS_SIMPLE_SORT,
@@ -342,159 +335,125 @@ int main(int argc, char **argv){
}
glViewport(0, 0, window_size.fb_width, window_size.fb_height);
- glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
- glClear(GL_COLOR_BUFFER_BIT);
+ glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- setOrtho(window_size.fb_width, window_size.fb_height);
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2, -1.0f, 1.0f);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
- //Model rendering
glPushMatrix();
- // Gauss UI
+
+ // Gaussian UI
ui::begin("Gaussian");
arma::vec mu;
arma::mat Sigm;
// Drag only target means
- for( int i = 0; i < m; i++ )
- Mu.col(i) = ui2fb((arma::vec) ui::dragger(i, fb2ui(Mu.col(i), window_size)), window_size);
-
- // manipulate semitied covariance
- semitied_params = semitiedWidget(m, semitied_params, {100, 100}, window_size);
- // Setup cov states
- cube Sigma = semitiedCovariances(Mu, semitied_params);
+ for (int i = 0; i < parameters.nb_targets; i++) {
+ vec mu = ui2fb_centered((arma::vec) ui::dragger(i, fb2ui_centered(Mu.col(i), window_size)), window_size);
+ must_recompute = must_recompute || (norm(mu - Mu.col(i)) > 1e-6);
+ Mu.col(i) = mu;
+ }
- std::vector<ui::Trans2d> t2ds(m, ui::Trans2d());
- for( int i = 0; i < m; i++ )
- gauss_to_trans2d(&t2ds[i], Mu.col(i), Sigma.slice(i), window_size);
+ // Manipulate semitied covariance
+ semitied_params = semitiedWidget(parameters.nb_targets, semitied_params, {100, 100}, window_size);
- ui::end();
+ // Setup cov states
+ cube new_sigma = semitiedCovariances(Mu, semitied_params);
+ for (int i = 0; !must_recompute && (i < parameters.nb_targets); i++)
+ must_recompute = (norm(new_sigma.slice(i) - Sigma.slice(i)) > 1e-6);
- // Parameters window
- int winw = 360;
- ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
- ImGui::Begin("Params", NULL, ImVec2(winw,84), 0.5f,
- ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
- ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
+ Sigma = new_sigma;
- if(ImGui::Button("Save EPS"))
- saving_eps = true;
-
- ImGui::SliderInt("Order", &order, 2, 7);
- ImGui::SliderFloat("Max Displacement", &d, 0.01, 5.);
- ImGui::End();
+ ui::end();
- // Recompute LQR
- int dim = 2;
- double duration = stroke_duration * m;
- int n = (int)(duration/dt);
- int cDim = dim * order;
-
- // system matrices
- mat A, B;
- makeIntegratorChain(&A, &B, order);
- discretizeSystem(&A, &B, A, B, dt);
- // make bi-variate
- A = kron(A, eye(dim, dim));
- B = kron( B, eye(dim, dim) );
-
- // reference
- mat Q, MuQ;
- stepwiseReference( &MuQ, &Q, Mu, Sigma, n, order, dim, endWeight );
- MuQ *= globScale;
- Q /= globScale*globScale;
-
- // r based on oscillatory movement displacement
- double r = SHM_r(d, stroke_duration, order);
- mat R = kron( eye(n-1, n-1),
- eye(dim, dim) * r );
-
- ////////////////////////////////////
- // Batch LQR
-
- // Sx and Su matrices for batch LQR
- mat Su = zeros(cDim*n, dim*(n-1));
- mat Sx = kron( ones(n,1),
- eye(dim*order, dim*order));
- mat M = B;
- for( int i = 1; i < n; i++ )
- {
- Sx.rows( i*cDim, n*cDim-1 ) =
- Sx.rows( i*cDim, n*cDim-1 ) * A;
- Su.submat( i*cDim, 0,
- (i+1)*cDim-1, i*dim-1 ) = M;
- M = join_horiz( A*M.cols(0, dim-1), M );
+ // Recompute the LQR when needed
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ trajectory = compute_LQR(parameters, Mu, Sigma);
+ must_recompute = false;
}
- arma::vec x0 = MuQ.col(0);
-
- // Flatten Mu's
- mat Xi_hat = reshape(MuQ, cDim*n, 1);
- mat SuInvSigmaQ = Su.t()*Q;
-
- mat Rq, rq;
- Rq = SuInvSigmaQ*Su + R;
- rq = SuInvSigmaQ*(Xi_hat - Sx*x0);
-
- // least squares solution
- vec u = solve(Rq,rq);
- mat Y = reshape( Sx*x0 + Su*u, cDim, n );
- mat Xi = Y.submat(0,0, dim-1, n-1) / globScale;
-
- //////////////////////////////////////
-
- glEnable( GL_BLEND );
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ // Draw the gaussians
+ for( int i = 0; i < parameters.nb_targets; i++ ) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian_background(fvec({ 0.0f, 0.5f, 1.0f }), Mu.col(i), Sigma.slice(i));
+ }
+ glClear(GL_DEPTH_BUFFER_BIT);
- // draw gaussians
- glColor4f(0.0, 0.5f, 1.0f, 0.2f);
- for( int i = 0; i < m; i++ )
- drawGauss(t2ds[i], window_size);
+ // Draw the motor plan
+ glColor3f(0.5, 0.5 ,0.5);
+ gfx2::draw_line(fvec({ 0.5f, 0.5f, 0.5f }), Mu);
- // draw trajectory
- glColor3f(0, 0, 1);
- draw(Xi);
+ // Draw the trajectory
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), trajectory);
- // plot derivatives magnitude
+ // Plot derivatives magnitude
int plot_width = 200 * window_size.fb_width / window_size.win_width;
int plot_height = 100 * window_size.fb_height / window_size.win_height;
- int plot_y = window_size.fb_height - 25 * window_size.fb_height / window_size.win_height;
+ int plot_x = -window_size.fb_width / 2;
+ int plot_y = -window_size.fb_height / 2 + 25 * window_size.fb_height / window_size.win_height;
- glColor3f(1,0,0);
- mat dx = gradient_2d(Xi, 1);
- vec speed = sqrt(sum(dx%dx, 0)).t();
- plot(0, plot_y, plot_width, plot_height, speed);
+ mat dx = gradient_2d(trajectory, 1);
+ vec speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x, plot_y, plot_width, plot_height, speed, {1.0f, 0.0f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(10, window_size.win_height - 25), "velocity magnitude",
- ImVec4(1,0,0,1));
+ ImVec4(1, 0, 0, 1));
ui::end();
- glColor3f(0,0,1);
- dx = gradient_2d(Xi, 2);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 2);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.0f, 1.0f});
ui::begin("Text");
ui::text(ImVec2(200 + 10, window_size.win_height - 25), "acceleration magnitude",
- ImVec4(0,0,1,1));
+ ImVec4(0, 0, 1, 1));
ui::end();
- glColor3f(0,0.5,0);
- dx = gradient_2d(Xi, 3);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(2 * plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 3);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + 2 * plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.5f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(400 + 10, window_size.win_height - 25), "jerk magnitude",
- ImVec4(0,0.5,0,1));
+ ImVec4(0, 0.5, 0, 1));
ui::end();
glPopMatrix();
+
+ // Parameters window
+ int winw = 360;
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
+ ImGui::Begin("Params", NULL, ImVec2(winw, 86), 0.5f,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings
+ );
+
+ if (ImGui::Button("Save EPS"))
+ saving_eps = true;
+
+ int previous_order = parameters.order;
+ float previous_maximum_displacement = parameters.maximum_displacement;
+
+ ImGui::SliderInt("Order", ¶meters.order, 2, 7);
+ ImGui::SliderFloat("Max Displacement", ¶meters.maximum_displacement, 0.01, 5.);
+ ImGui::End();
+
+ must_recompute = must_recompute ||
+ (parameters.order != previous_order) ||
+ !gfx2::is_close(parameters.maximum_displacement, previous_maximum_displacement);
+
+
// Render UI
ImGui::Render();
ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
@@ -504,19 +463,20 @@ int main(int argc, char **argv){
glFlush();
gl2psEndPage();
fclose(eps_file);
- eps_file=NULL;
- saving_eps=false;
+ eps_file = NULL;
+ saving_eps = false;
}
glfwSwapBuffers(window);
- //Keyboard input
+ // Keyboard input
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
}
- //Cleanup
+ // Cleanup
ImGui_ImplGlfwGL2_Shutdown();
glfwTerminate();
+
return 0;
}
diff --git a/src/demo_MPC_velocity01.cpp b/src/demo_MPC_velocity01.cpp
index f4eb36c4aa6051cdc522203d300d9ea02ec47b87..69677ca359e5645121f8b2bcb8b7f5a18b347e29 100644
--- a/src/demo_MPC_velocity01.cpp
+++ b/src/demo_MPC_velocity01.cpp
@@ -3,251 +3,298 @@
*
* Interactive MPC demo, with batch LQT and repulsive field test
*
- * Fabien Crépon, Sylvain Calinon, 2017
+ * Fabien Crépon, Philip Abbet, Sylvain Calinon, 2017
*/
#include <stdio.h>
-#include <imgui.h>
-#include <imgui_impl_glfw_gl2.h>
+#include <armadillo>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
#include <gfx_ui.h>
#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
#include <window_utils.h>
-#include <armadillo>
-#include <mpc_utils.h>
-
using namespace arma;
-//-------------------------------------------------------------------------
-// Holds the sizes of the window and of the OpenGL front-buffer (they might
-// be different, for instance on a 4K screen)
-//-------------------------------------------------------------------------
-struct window_size_t {
- int win_width;
- int win_height;
- int fb_width;
- int fb_height;
+/***************************** ALGORITHM SECTION *****************************/
+
+//-----------------------------------------------------------------------------
+// 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_targets; // Number of gaussians
+ int nb_repulsive_gaussians; // Number of repulsive gaussians
+ int nb_dimensions;
+ int order;
+ double global_scale; // global scale, avoids numerical issues in batch method
+ double end_weight; // forces movement to a stop (see stepwiseReference function)
+ float maximum_displacement; // maximum displacement, used to compute R diagonal
+ float stroke_duration; // duration of a stroke
+ float covscale; // hack, field covariance scaling factor
+ int field_derivative; // derivative used for additional field
+ bool use_field;
+ bool stepwise;
+ double dt;
};
-//-------------------------------------------------------------------------
-// Converts some coordinates from UI-space to OpenGL-space
-//
-// UI coordinates range from (0, 0) to (win_width, win_height)
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-//-------------------------------------------------------------------------
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+//-----------------------------------------------
- result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
- result(1) = coords(1) * (float) window_size.fb_height / (float) window_size.win_height;
+mat compute_LQR(const parameters_t& parameters, const mat& Mu, const cube& Sigma,
+ const mat& Mu_repulsive, const cube& Sigma_repulsive) {
+
+ const double duration = parameters.stroke_duration * parameters.nb_targets;
+ const int n = (int) (duration / parameters.dt);
+ const int cDim = parameters.nb_dimensions * parameters.order;
+
+ // Integration with higher order Taylor series expansion
+ mat A, B;
+ makeIntegratorChain(&A, &B, parameters.order);
+ discretizeSystem(&A, &B, A, B, parameters.dt);
+ A = kron(A, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+ B = kron(B, eye(parameters.nb_dimensions, parameters.nb_dimensions));
+
+ // Reference
+ mat Q, MuQ;
+
+ if (parameters.stepwise) {
+ stepwiseReference(&MuQ, &Q, Mu, Sigma, n, parameters.order,
+ parameters.nb_dimensions, parameters.end_weight);
+ } else {
+ viaPointReference(&MuQ, &Q, Mu, Sigma, n, parameters.order,
+ parameters.nb_dimensions, parameters.end_weight);
+ }
- return result;
-}
+ MuQ *= parameters.global_scale;
+ Q /= parameters.global_scale * parameters.global_scale;
-//-------------------------------------------------------------------------
-// Converts some coordinates from OpenGL-space to UI-space
-//
-// OpenGL coordinates range from (0, 0) to (fb_width, fb_height)
-// UI coordinates range from (0, 0) to (win_width, win_height)
-//-------------------------------------------------------------------------
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
+ // r based on oscillatory movement displacement
+ double r = SHM_r(parameters.maximum_displacement, parameters.stroke_duration,
+ parameters.order);
- result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
- result(1) = coords(1) * (float) window_size.win_height / (float) window_size.fb_height;
+ mat R = kron(eye(n - 1, n - 1), eye(parameters.nb_dimensions, parameters.nb_dimensions) * r);
- return result;
-}
+ ////////////////////////////////////
+ // Batch LQR
-//-----------------------------------------------
+ // Sx and Su matrices for batch LQR
+ mat Su = zeros(cDim * n, parameters.nb_dimensions * (n - 1));
+ mat Sx = kron(ones(n, 1),
+ eye(parameters.nb_dimensions * parameters.order,
+ parameters.nb_dimensions * parameters.order)
+ );
+ mat M = B;
+ for (int i = 1; i < n; i++) {
+ Sx.rows(i * cDim, n * cDim - 1) = Sx.rows(i * cDim, n * cDim - 1) * A;
+ Su.submat(i * cDim, 0, (i + 1) * cDim - 1, i * parameters.nb_dimensions - 1) = M;
+ M = join_horiz(A * M.cols(0, parameters.nb_dimensions - 1), M);
+ }
-static void error_callback(int error, const char* description){
- fprintf(stderr, "Error %d: %s\n", error, description);
-}
+ arma::vec x0 = MuQ.col(0);
-//-----------------------------------------------
+ // Flatten Mu's
+ mat Xi_hat = reshape(MuQ, cDim * n, 1);
-void setOrtho( float w, float h )
-{
- glMatrixMode( GL_PROJECTION );
- glLoadIdentity();
- glOrtho( 0, 0+w, 0+h, 0, -1., 1.);
- glMatrixMode( GL_MODELVIEW );
- glLoadIdentity();
-}
+ mat SuInvSigmaQ = Su.t() * Q;
-//-----------------------------------------------
+ mat Rq = SuInvSigmaQ * Su + R;
+ mat rq = SuInvSigmaQ * (Xi_hat - Sx * x0);
-void trans2d_to_gauss(arma::vec* mu, arma::mat* Sigma, const ui::Trans2d& t2d,
- const window_size_t& window_size)
-{
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
+ // Repulsive field
+ if (parameters.use_field) {
+ int deriv = std::min(parameters.field_derivative, parameters.order - 2);
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
- *Sigma = basis * basis.t();
- *mu = ui2fb(arma::vec({t2d.pos.x, t2d.pos.y}), window_size);
-}
+ for (int i = 0; i < Sigma_repulsive.n_slices; ++i) {
+ mat MuQ_repulsive = repmat(Mu_repulsive.col(i), 1, n);
+ mat invSigma_repulsive = zeros(cDim, cDim);
-//-----------------------------------------------
+ invSigma_repulsive(span(parameters.nb_dimensions * deriv, parameters.nb_dimensions * deriv + 1),
+ span(parameters.nb_dimensions * deriv, parameters.nb_dimensions * deriv + 1)) =
+ inv(Sigma_repulsive.slice(i) * parameters.covscale * parameters.covscale);
-void gauss_to_trans2d(ui::Trans2d *t2d, const arma::vec& mu, const arma::mat& Sigma,
- const window_size_t& window_size)
-{
- arma::vec ui_mu = fb2ui(mu, window_size);
+ mat Q_repulsive = kron(eye(n, n), invSigma_repulsive);
- t2d->pos.x = ui_mu(0);
- t2d->pos.y = ui_mu(1);
+ mat Xi_hat_repulsive = reshape(MuQ_repulsive, cDim * n, 1);
+ mat SuInvSigmaQ_repulsive = Su.t() * Q_repulsive;
- arma::mat V;
- arma::vec d;
- arma::eig_sym(d, V, Sigma);
- arma::mat VD = V*diagmat(sqrt(d));
+ Rq = Rq + SuInvSigmaQ_repulsive * Su;
+ rq = rq + SuInvSigmaQ_repulsive * (Xi_hat_repulsive - Sx * x0);
+ }
+ }
- arma::vec t_x = fb2ui(arma::vec({VD.col(0)(0), VD.col(1)(0)}), window_size);
- arma::vec t_y = fb2ui(arma::vec({VD.col(0)(1), VD.col(1)(1)}), window_size);
+ // Least squares solution
+ vec u = pinv(Rq) * rq;;
+ mat Y = reshape(Sx * x0 + Su * u, cDim, n);
- t2d->x.x = t_x(0);
- t2d->x.y = t_x(1);
- t2d->y.x = t_y(0);
- t2d->y.y = t_y(1);
+ return Y.rows(0, parameters.nb_dimensions - 1) / parameters.global_scale;
}
-//-----------------------------------------------
-void draw(const arma::mat& pts, int primitive=GL_LINE_STRIP)
-{
- glBegin(primitive);
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
}
//-----------------------------------------------
-arma::mat gradient_2d( arma::mat X, int order=1 )
-{
- mat df = diff(X, 1, 1);
- mat db = fliplr( -diff(fliplr(X), 1, 1) );
- mat d = (df+db)/2;
- d = join_horiz(zeros(2,1), d);
- d.col(0) = df.col(0);
- d.col(d.n_cols-1) = db.col(db.n_cols-1);
- if(order>1)
- return gradient_2d(d, order-1);
- return d;
-}
+std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
+ const gfx2::window_size_t& window_size) {
-//-----------------------------------------------
+ vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
-void plot( float x, float y, float w, float h, const vec& v )
-{
- float v_min = v.min();
- float v_max = v.max();
- float inc = w / v.n_rows;
- glBegin(GL_LINE_STRIP);
- for (int i=0; i < v.n_rows; i++)
- {
- glVertex2f(x + inc*i, y - h * ((v[i] - v_min) / (v_max - v_min)));
- }
- glEnd();
+ vec t_x({
+ gaussian_transforms.x.x * window_size.scale_x(),
+ gaussian_transforms.x.y * window_size.scale_y()
+ });
+
+ vec t_y({
+ gaussian_transforms.y.x * window_size.scale_x(),
+ gaussian_transforms.y.y * window_size.scale_y()
+ });
+
+ mat RG = {
+ { t_x(0), t_y(0) },
+ { -t_x(1), -t_y(1) }
+ };
+
+ mat sigma = RG * RG.t();
+
+ return std::make_tuple(mu, sigma);
}
//-----------------------------------------------
-void drawGauss(ui::Trans2d& t2d, const window_size_t& window_size)
-{
- static arma::mat circ(2,35);
- static bool circ_init = true;
- if(circ_init)
- {
- circ = join_cols(cos(linspace<rowvec>(0,2*datum::pi,35)),
- sin(linspace<rowvec>(0,2*datum::pi,35)));
- circ_init = false;
- }
+void gauss_to_trans2d(const vec& mu, const mat& sigma,
+ const gfx2::window_size_t& window_size, ui::Trans2d &t2d) {
- arma::vec t_x = ui2fb(arma::vec({t2d.x.x, t2d.x.y}), window_size);
- arma::vec t_y = ui2fb(arma::vec({t2d.y.x, t2d.y.y}), window_size);
+ vec ui_mu = gfx2::fb2ui_centered(mu, window_size);
- arma::mat basis = arma::mat{{t_x(0), t_y(0)}, {t_x(1), t_y(1)}};
+ t2d.pos.x = ui_mu(0);
+ t2d.pos.y = ui_mu(1);
- vec mu = ui2fb(vec({t2d.pos.x, t2d.pos.y}), window_size);
+ mat V;
+ vec d;
+ eig_sym(d, V, sigma);
+ mat VD = V * diagmat(sqrt(d));
- arma::mat pts = basis * circ + arma::repmat(mu, 1, 35);
+ t2d.x.x = VD.col(0)(0) / window_size.scale_x();
+ t2d.x.y = VD.col(1)(0) / window_size.scale_y();
+ t2d.y.x = VD.col(0)(1) / window_size.scale_x();
+ t2d.y.y = VD.col(1)(1) / window_size.scale_y();
+}
- glBegin(GL_TRIANGLE_FAN);
- glVertex2f(mu(0), mu(1));
- for (uint t=0; t<pts.n_cols; t++){
- glVertex2f(pts(0,t), pts(1,t));
- }
- glEnd();
+//-----------------------------------------------
+
+arma::mat gradient_2d(arma::mat X, int order = 1) {
+ mat df = diff(X, 1, 1);
+ mat db = fliplr(-diff(fliplr(X), 1, 1));
+
+ mat d = (df + db) / 2;
+ d = join_horiz(zeros(2, 1), d);
+ d.col(0) = df.col(0);
+ d.col(d.n_cols - 1) = db.col(db.n_cols - 1);
+
+ if(order > 1)
+ return gradient_2d(d, order - 1);
+
+ return d;
}
//-----------------------------------------------
-int main(int argc, char **argv){
+void plot(float x, float y, float w, float h, const vec& v, const fvec& color) {
+ float v_min = v.min();
+ float v_max = v.max();
- arma_rng::set_seed_random();
+ mat points(2, v.n_rows);
+ points(0, span::all) = linspace<vec>(0, w, v.n_rows).t() + x;
+ points(1, span::all) = (v.t() - v_min) / (v_max - v_min) * h + y;
- //--------------- Setup parameters ---------------
+ gfx2::draw_line(color, points);
+}
- double dt = 0.01;
- int order = 3;
- int m = 4; // Number of targets
- double globScale = 0.001; // global scale, avoids numerical issues in batch method
- double endWeight = 1.; //1e-10; // forces movement to a stop (see stepwiseReference function)
+/******************************* MAIN FUNCTION *******************************/
- float d=0.1; // maximum displacement, used to compute R diagonal
- float stroke_duration=0.3; // duration of a stroke
- float covscale = 1.0; // hack, field covariance scaling factor
- int fieldDeriv=1; // derivative used for additional field
+int main(int argc, char **argv){
- // Gui settings
- bool useField = true;
- bool stepwise = false;
+ arma_rng::set_seed_random();
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_targets = 4;
+ parameters.nb_repulsive_gaussians = 1;
+ parameters.nb_dimensions = 2;
+ parameters.order = 3;
+ parameters.global_scale = 0.001;
+ parameters.end_weight = 1.;
+ parameters.maximum_displacement = 0.1;
+ parameters.stroke_duration = 0.3;
+ parameters.covscale = 1.0;
+ parameters.field_derivative = 1;
+ parameters.use_field = true;
+ parameters.stepwise = false;
+ parameters.dt = 0.01;
- //--------------- Setup of the rendering ---------------
// Take 4k screens into account (framebuffer size != window size)
- window_size_t window_size;
+ gfx2::window_size_t window_size;
window_size.win_width = 800;
window_size.win_height = 800;
window_size.fb_width = -1; // Will be known later
window_size.fb_height = -1;
- //Setup GUI
+
+ // Initialise GLFW
glfwSetErrorCallback(error_callback);
+
if (!glfwInit())
- exit(1);
+ 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 Velocity MPC", window_size.win_width, window_size.win_height
);
glfwMakeContextCurrent(window);
- //Setup ImGui
+
+ // Setup GLSL
+ gfx2::init();
+ 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);
- ImVec4 clear_color = ImColor(255, 255, 255);
- //--------------- Main loop ---------------
-
// Covariances
- mat Mu;
- cube Sigma;
+ mat Mu = zeros(2, parameters.nb_targets);
+ cube Sigma = zeros(2, 2, parameters.nb_targets);
- mat Mu0;
- cube Sigma0;
+ mat Mu0 = zeros(2, parameters.nb_repulsive_gaussians);
+ cube Sigma0 = zeros(2, 2, parameters.nb_repulsive_gaussians);
// Gaussian widgets
- std::vector<ui::Trans2d> t2ds(m, ui::Trans2d());
- std::vector<ui::Trans2d> t2ds0(1, ui::Trans2d());
+ std::vector<ui::Trans2d> t2ds(parameters.nb_targets, ui::Trans2d());
+ std::vector<ui::Trans2d> t2ds0(parameters.nb_repulsive_gaussians, ui::Trans2d());
+
+ // Main loop
+ bool must_recompute = true;
+ mat trajectory;
while (!glfwWindowShouldClose(window)) {
glfwPollEvents();
@@ -267,247 +314,225 @@ int main(int argc, char **argv){
// At the very first frame: initialise the UI widgets of the gaussians
if (first && (window_size.fb_width > 0)) {
- Mu = arma::randu(2, m);
- Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) + 100;
- Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) + 100;
+ Mu = randu(2, parameters.nb_targets);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
randomCovariances(
&Sigma, Mu,
- arma::vec({ 50 * (float) window_size.fb_width / window_size.win_width,
- 50 * (float) window_size.fb_height / window_size.win_height
+ vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+ 100 * (float) window_size.fb_height / window_size.win_height
}),
- false, 0.0, 0.8
+ true, 0.0, 0.2
);
- Mu0 = arma::randu(2, 1);
- Mu0.row(0) = Mu0.row(0) * (window_size.fb_width - 200) + 100;
- Mu0.row(1) = Mu0.row(1) * (window_size.fb_height - 200) + 100;
+ Mu0 = randu(2, parameters.nb_repulsive_gaussians);
+ Mu0.row(0) = Mu0.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu0.row(1) = Mu0.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
randomCovariances(
&Sigma0, Mu0,
- arma::vec({ 120 * (float) window_size.fb_width / window_size.win_width,
- 120 * (float) window_size.fb_height / window_size.win_height
+ vec({ 150 * (float) window_size.fb_width / window_size.win_width,
+ 150 * (float) window_size.fb_height / window_size.win_height
}),
- false, 0.0, 0.8
+ true, 0.0, 0.2
);
- for( int i = 0; i < t2ds.size(); i++ )
- gauss_to_trans2d(&t2ds[i], Mu.col(i), Sigma.slice(i), window_size);
-
- for( int i = 0; i < t2ds0.size(); i++ )
- gauss_to_trans2d(&t2ds0[i], Mu0.col(i), Sigma0.slice(i), window_size);
- }
- }
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, t2ds[i]);
- // Start of rendering
- ImGui_ImplGlfwGL2_NewFrame();
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
- glViewport(0, 0, window_size.fb_width, window_size.fb_height);
- glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
- glClear(GL_COLOR_BUFFER_BIT);
+ fvec x = rotation * fvec({ t2ds[i].x.x, t2ds[i].x.y, 0.0f, 0.0f });
+ t2ds[i].x.x = x(0);
+ t2ds[i].x.y = x(1);
- setOrtho(window_size.fb_width, window_size.fb_height);
+ fvec y = rotation * fvec({ t2ds[i].y.x, t2ds[i].y.y, 0.0f, 0.0f });
+ t2ds[i].y.x = y(0);
+ t2ds[i].y.y = y(1);
+ }
- //Model rendering
- glPushMatrix();
+ for (int i = 0; i < parameters.nb_repulsive_gaussians; ++i) {
+ gauss_to_trans2d(Mu0.col(i), Sigma0.slice(i), window_size, t2ds0[i]);
- // Gauss UI
- ui::begin("Gaussian");
- arma::vec mu;
- arma::mat Sigm;
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
- for( int i = 0; i < t2ds.size(); i++ )
- {
- t2ds[i] = ui::affineSimple(i, t2ds[i]);
- trans2d_to_gauss(&mu, &Sigm, t2ds[i], window_size);
- Mu.col(i) = mu;
- Sigma.slice(i) = Sigm;
- }
+ fvec x = rotation * fvec({ t2ds0[i].x.x, t2ds0[i].x.y, 0.0f, 0.0f });
+ t2ds0[i].x.x = x(0);
+ t2ds0[i].x.y = x(1);
- // Velocity field Gaussians
- for( int i = 0; i < t2ds0.size(); i++ )
- {
- t2ds0[i] = ui::affineSimple(i+m, t2ds0[i]);
- trans2d_to_gauss(&mu, &Sigm, t2ds0[i], window_size);
- Mu0.col(i) = mu;
- Sigma0.slice(i) = Sigm;
+ fvec y = rotation * fvec({ t2ds0[i].y.x, t2ds0[i].y.y, 0.0f, 0.0f });
+ t2ds0[i].y.x = y(0);
+ t2ds0[i].y.y = y(1);
+ }
+ }
}
- ui::end();
-
-
- // Parameters window
- int winw = 400;
- ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
- ImGui::Begin("Params", NULL, ImVec2(winw,154), 0.5f,
- ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
- ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
- ImGui::SliderInt("Order", &order, 2, 7);
- ImGui::SliderInt("Field deriv", &fieldDeriv, 0, 4);
- ImGui::SliderFloat("covscale", &covscale, 0.1, 1.0);
- ImGui::SliderFloat("Max Displacement", &d, 0.01, 2.);
- ImGui::Checkbox("Use Field", &useField);
- ImGui::Checkbox("Stepwise", &stepwise);
- ImGui::End();
- // Recompute LQR
- int dim = 2;
- double duration = stroke_duration * m;
- int n = (int)(duration/dt);
- int cDim = dim * order;
-
- // system matrices
- mat A, B;
- makeIntegratorChain(&A, &B, order);
- discretizeSystem(&A, &B, A, B, dt);
- // make bi-variate
- A = kron(A, eye(dim, dim));
- B = kron( B, eye(dim, dim) );
-
- // reference
- mat Q, MuQ, Q0, MuQ0;
- if(stepwise)
- stepwiseReference( &MuQ, &Q, Mu, Sigma, n, order, dim, endWeight );
- else
- viaPointReference( &MuQ, &Q, Mu, Sigma, n, order, dim, endWeight );
- MuQ *= globScale;
- Q /= globScale*globScale;
-
- // velocity field
- int deriv=std::min(fieldDeriv, order-2);
- // Mu0 = zeros(cDim,1);
- MuQ0 = repmat(Mu0.col(0), 1, n);// * globScale;
- mat invSigma0 = zeros(cDim, cDim);
- invSigma0(span(dim*deriv, dim*deriv+1), span(dim*deriv, dim*deriv+1)) = inv(Sigma0.slice(0)*covscale*covscale);
- Q0 = kron(eye(n,n), invSigma0);// / (globScale*globScale);
-
- /*
- Q0 = zeros(cDim, cDim);
- Q0(span(0, dim-1), span(0, dim-1)) = inv(Sigma0.slice(0));
- Q0 = kron(eye(n,n), Q0) / (globScale*globScale);
- Q0 = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(model0.invSigma(:,:,qList0), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
- */
- // Q0 = (kron(ones(n,1), eye(cDim, cDim)) * kron(ones(1,n), invSigma0)) % kron(eye(n,n), ones(cDim,cDim));
-
-
- // r based on oscillatory movement displacement
- double r = SHM_r(d, stroke_duration, order);
- mat R = kron( eye(n-1, n-1),
- eye(dim, dim) * r );
-
- ////////////////////////////////////
- // Batch LQR
-
- // Sx and Su matrices for batch LQR
- mat Su = zeros(cDim*n, dim*(n-1));
- mat Sx = kron( ones(n,1),
- eye(dim*order, dim*order));
- mat M = B;
- for( int i = 1; i < n; i++ )
- {
- Sx.rows( i*cDim, n*cDim-1 ) =
- Sx.rows( i*cDim, n*cDim-1 ) * A;
- Su.submat( i*cDim, 0,
- (i+1)*cDim-1, i*dim-1 ) = M;
- M = join_horiz( A*M.cols(0, dim-1), M );
+ // Recompute the LQR when needed
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ trajectory = compute_LQR(parameters, Mu, Sigma, Mu0, Sigma0);
+ must_recompute = false;
}
- arma::vec x0 = MuQ.col(0);
- // Flatten Mu's
- mat Xi_hat = reshape(MuQ, cDim*n, 1);
- mat Xi_hat0 = reshape(MuQ0, cDim*n, 1);
-
- mat SuInvSigmaQ = Su.t()*Q;
- mat SuInvSigmaQ0 = Su.t()*Q0;
+ // Start of rendering
+ ImGui_ImplGlfwGL2_NewFrame();
- mat Rq, rq;
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- if(useField)
- {
- Rq = SuInvSigmaQ*Su + SuInvSigmaQ0*Su + R;
- rq = SuInvSigmaQ*(Xi_hat - Sx*x0) + SuInvSigmaQ0*(Xi_hat0 - Sx*x0);
- }
- else
- {
- Rq = SuInvSigmaQ*Su + R;
- rq = SuInvSigmaQ*(Xi_hat - Sx*x0);
- }
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2, -1.0f, 1.0f);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
- // least squares solution
- vec u = solve(Rq,rq);
- mat Y = reshape( Sx*x0 + Su*u, cDim, n );
- mat Xi = Y.submat(0,0, dim-1, n-1) / globScale;
+ glPushMatrix();
- //////////////////////////////////////
- glEnable( GL_BLEND );
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ // Draw the gaussians
+ for( int i = 0; i < parameters.nb_targets; i++ ) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian_background(fvec({ 0.0f, 0.5f, 1.0f }), Mu.col(i), Sigma.slice(i));
+ }
+ glClear(GL_DEPTH_BUFFER_BIT);
- // draw gaussians
- glColor4f(0.0, 0.5f, 1.0f, 0.2f);
- for( int i = 0; i < t2ds.size(); i++ )
- drawGauss(t2ds[i], window_size);
+ // Draw the repulsive gaussians
+ for( int i = 0; i < parameters.nb_repulsive_gaussians; i++ ) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian_background(fvec({ 1.0f, 0.5f, 0.0f }), Mu0.col(i), Sigma0.slice(i));
+ }
+ glClear(GL_DEPTH_BUFFER_BIT);
- // repulsive gaussians
- glColor4f(1.0, 0.5f, 0.0f, 0.2f);
- for( int i = 0; i < t2ds0.size(); i++ )
- drawGauss(t2ds0[i], window_size);
+ // Draw the motor plan
+ glColor3f(0.5, 0.5 ,0.5);
+ gfx2::draw_line(fvec({ 0.5f, 0.5f, 0.5f }), Mu);
- // draw trajectory
- glColor3f(0, 0, 1);
- draw(Xi);
+ // Draw the trajectory
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), trajectory);
- // plot derivatives magnitude
+ // Plot derivatives magnitude
int plot_width = 200 * window_size.fb_width / window_size.win_width;
int plot_height = 100 * window_size.fb_height / window_size.win_height;
- int plot_y = window_size.fb_height - 25 * window_size.fb_height / window_size.win_height;
+ int plot_x = -window_size.fb_width / 2;
+ int plot_y = -window_size.fb_height / 2 + 25 * window_size.fb_height / window_size.win_height;
- glColor3f(1,0,0);
- mat dx = gradient_2d(Xi, 1);
- vec speed = sqrt(sum(dx%dx, 0)).t();
- plot(0, plot_y, plot_width, plot_height, speed);
+ mat dx = gradient_2d(trajectory, 1);
+ vec speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x, plot_y, plot_width, plot_height, speed, {1.0f, 0.0f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(10, window_size.win_height - 25), "velocity magnitude",
- ImVec4(1,0,0,1));
+ ImVec4(1, 0, 0, 1));
ui::end();
- glColor3f(0,0,1);
- dx = gradient_2d(Xi, 2);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 2);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.0f, 1.0f});
ui::begin("Text");
ui::text(ImVec2(200 + 10, window_size.win_height - 25), "acceleration magnitude",
- ImVec4(0,0,1,1));
+ ImVec4(0, 0, 1, 1));
ui::end();
- glColor3f(0,0.5,0);
- dx = gradient_2d(Xi, 3);
- speed = sqrt(sum(dx%dx, 0)).t();
- plot(2 * plot_width, plot_y, plot_width, plot_height, speed);
+ dx = gradient_2d(trajectory, 3);
+ speed = sqrt(sum(dx % dx, 0)).t();
+ plot(plot_x + 2 * plot_width, plot_y, plot_width, plot_height, speed, {0.0f, 0.5f, 0.0f});
ui::begin("Text");
ui::text(ImVec2(400 + 10, window_size.win_height - 25), "jerk magnitude",
- ImVec4(0,0.5,0,1));
+ ImVec4(0, 0.5, 0, 1));
ui::end();
glPopMatrix();
- // Render UI
+
+ // Gaussians UI
+ ui::begin("Gaussian");
+
+ for (int i = 0; i < parameters.nb_targets; ++i) {
+ t2ds[i] = ui::affineSimple(i, t2ds[i]);
+
+ vec mu;
+ mat sigma;
+ std::tie(mu, sigma) = trans2d_to_gauss(t2ds[i], window_size);
+
+ must_recompute = must_recompute ||
+ (norm(mu - Mu.col(i)) > 1e-6) ||
+ (norm(sigma - Sigma.slice(i)) > 1e-6);
+
+ Mu.col(i) = mu;
+ Sigma.slice(i) = sigma;
+ }
+
+ for (int i = 0; i < parameters.nb_repulsive_gaussians; ++i) {
+ t2ds0[i] = ui::affineSimple(i + parameters.nb_targets, t2ds0[i]);
+
+ vec mu;
+ mat sigma;
+ std::tie(mu, sigma) = trans2d_to_gauss(t2ds0[i], window_size);
+
+ must_recompute = must_recompute ||
+ (norm(mu - Mu0.col(i)) > 1e-6) ||
+ (norm(sigma - Sigma0.slice(i)) > 1e-6);
+
+ Mu0.col(i) = mu;
+ Sigma0.slice(i) = sigma;
+ }
+
+ ui::end();
+
+
+ // Parameters window
+ int winw = 400;
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - winw, 2));
+ ImGui::Begin("Params", NULL, ImVec2(winw, 154), 0.5f,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings
+ );
+
+ int previous_order = parameters.order;
+ int previous_field_derivative = parameters.field_derivative;
+ float previous_covscale = parameters.covscale;
+ float previous_maximum_displacement = parameters.maximum_displacement;
+ bool previous_use_field = parameters.use_field;
+ bool previous_stepwise = parameters.stepwise;
+
+ ImGui::SliderInt("Order", ¶meters.order, 2, 7);
+ ImGui::SliderInt("Field deriv", ¶meters.field_derivative, 0, 4);
+ ImGui::SliderFloat("covscale", ¶meters.covscale, 0.1, 1.0);
+ ImGui::SliderFloat("Max Displacement", ¶meters.maximum_displacement, 0.1, 100.);
+ ImGui::Checkbox("Use Field", ¶meters.use_field);
+ ImGui::Checkbox("Stepwise", ¶meters.stepwise);
+
+ ImGui::End();
+
+ must_recompute = must_recompute ||
+ (parameters.order != previous_order) ||
+ (parameters.field_derivative != previous_field_derivative) ||
+ !gfx2::is_close(parameters.covscale, previous_covscale) ||
+ !gfx2::is_close(parameters.maximum_displacement, previous_maximum_displacement) ||
+ (parameters.use_field != previous_use_field) ||
+ (parameters.stepwise != previous_stepwise);
+
+
+ // GUI rendering
ImGui::Render();
ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
glfwSwapBuffers(window);
- //Keyboard input
+ // Keyboard input
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
}
- //Cleanup
+ // Cleanup
ImGui_ImplGlfwGL2_Shutdown();
glfwTerminate();
+
return 0;
}
diff --git a/src/demo_Riemannian_cov_interp02.cpp b/src/demo_Riemannian_cov_interp02.cpp
index aaa1773cb3df3e933c23067181ef39a26bef6428..40e145cc6d310cc0b42e64b720fb8f76bbb37220 100644
--- a/src/demo_Riemannian_cov_interp02.cpp
+++ b/src/demo_Riemannian_cov_interp02.cpp
@@ -28,8 +28,8 @@ void trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
const gfx2::window_size_t& window_size,
arma::vec &mu, arma::mat &sigma) {
- mu = gfx2::ui2fb(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
- window_size);
+ mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
vec t_x({
gaussian_transforms.x.x * window_size.scale_x(),
diff --git a/src/demo_Riemannian_pose_GMM01.cpp b/src/demo_Riemannian_pose_GMM01.cpp
index c7ac8af034a1c675aeb83eda9452db5293c1b753..6898975b279aae026ed0ea5fdf16a73433b32b9d 100644
--- a/src/demo_Riemannian_pose_GMM01.cpp
+++ b/src/demo_Riemannian_pose_GMM01.cpp
@@ -9,6 +9,7 @@
#include <stdio.h>
#include <armadillo>
+#include <cfloat>
using namespace arma;
@@ -197,7 +198,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
diff --git a/src/demo_Riemannian_pose_TPGMM01.cpp b/src/demo_Riemannian_pose_TPGMM01.cpp
index 8af677450609cd6662030b0a04337e656682a963..9c9b2ee75eb502d1018ea45bec028dfd5248912c 100644
--- a/src/demo_Riemannian_pose_TPGMM01.cpp
+++ b/src/demo_Riemannian_pose_TPGMM01.cpp
@@ -197,7 +197,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma)); // + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma)); // + DBL_MIN);
return prob;
}
diff --git a/src/demo_Riemannian_pose_batchLQR01.cpp b/src/demo_Riemannian_pose_batchLQR01.cpp
index d8ee3303fede4faea3924805ccfb6f0657572793..eb0205e4a40cda0c0ad83bc10db75d66bb67627b 100644
--- a/src/demo_Riemannian_pose_batchLQR01.cpp
+++ b/src/demo_Riemannian_pose_batchLQR01.cpp
@@ -222,7 +222,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
diff --git a/src/demo_Riemannian_pose_infHorLQR01.cpp b/src/demo_Riemannian_pose_infHorLQR01.cpp
index fb8337621695e0c7080e92016199b106288a9862..35138820e3d9ea8b249c6ee2af5366b148c0d53c 100644
--- a/src/demo_Riemannian_pose_infHorLQR01.cpp
+++ b/src/demo_Riemannian_pose_infHorLQR01.cpp
@@ -210,7 +210,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
diff --git a/src/demo_Riemannian_quat_TPGMM01.cpp b/src/demo_Riemannian_quat_TPGMM01.cpp
index e35f527e5c2b997958d96c49438a0c54ea0d64cc..db8d3d96d3ae1599b8946eb65bece949279afa19 100644
--- a/src/demo_Riemannian_quat_TPGMM01.cpp
+++ b/src/demo_Riemannian_quat_TPGMM01.cpp
@@ -185,7 +185,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
diff --git a/src/demo_Riemannian_quat_infHorLQR01.cpp b/src/demo_Riemannian_quat_infHorLQR01.cpp
index fd93161360269e08e4c71c19e0d2091e094334ed..9923e972e6971395d8306ae19e407268bc8bf446 100644
--- a/src/demo_Riemannian_quat_infHorLQR01.cpp
+++ b/src/demo_Riemannian_quat_infHorLQR01.cpp
@@ -14,6 +14,10 @@
#include <math.h>
#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <gfx_ui.h>
#include <window_utils.h>
namespace linmath{
@@ -35,6 +39,9 @@ using namespace arma;
#define DIST_BALL (RADIUS * 2.f + RADIUS * 0.1f)
#define VIEW_SCENE_DIST (DIST_BALL * 3.f + 200.f)
+#define MARGIN 50
+
+
/* Vertex type */
typedef struct {float x; float y; float z;} vertex_t;
@@ -219,36 +226,112 @@ arma::mat logmap(arma::mat x, arma::vec mu)
//-----------------------------------------------
-void DrawTarget(int nbData)
+void DrawResults(int nbData, int nbRepros, const arma::mat& x_y,
+ const window_size_t& window_size)
{
+ const int plot_width = (window_size.win_width - 3 * MARGIN) / 2;
+ const int plot_height = (window_size.win_height - 3 * MARGIN) / 2;
+
+ const float scale_x = (float) plot_width / (nbData - 1);
+ const float scale_y = (float) plot_height / 2;
+
+ // Draw the plot backgrounds
+ glColor3f(0.95f, 0.95f, 0.95f);
+
+ glBegin (GL_QUADS);
+ glVertex2f(MARGIN, window_size.win_height - MARGIN);
+ glVertex2f(MARGIN, window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(MARGIN + plot_width, window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(MARGIN + plot_width, window_size.win_height - MARGIN);
+ glEnd();
+
+ glBegin (GL_QUADS);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), window_size.win_height - MARGIN);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + plot_width, window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + plot_width, window_size.win_height - MARGIN);
+ glEnd();
+
+ glBegin (GL_QUADS);
+ glVertex2f(MARGIN, MARGIN + plot_height);
+ glVertex2f(MARGIN, MARGIN);
+ glVertex2f(MARGIN + plot_width, MARGIN);
+ glVertex2f(MARGIN + plot_width, MARGIN + plot_height);
+ glEnd();
+
+ glBegin (GL_QUADS);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), MARGIN + plot_height);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), MARGIN);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + plot_width, MARGIN);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + plot_width, MARGIN + plot_height);
+ glEnd();
+
+ // Draw the axes
+ glColor3f(0.0f, 0.0f, 0.0f);
+ glLineWidth(2);
+
+ glBegin(GL_LINES);
+ glVertex2f(MARGIN, window_size.win_height - MARGIN);
+ glVertex2f(MARGIN, window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(MARGIN, window_size.win_height - (MARGIN + plot_height / 2));
+ glVertex2f(MARGIN + plot_width, window_size.win_height - (MARGIN + plot_height / 2));
+ glEnd();
+
+ glBegin(GL_LINES);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), window_size.win_height - MARGIN);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), window_size.win_height - (MARGIN + plot_height));
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), window_size.win_height - (MARGIN + plot_height / 2));
+ glVertex2f(window_size.win_width - MARGIN, window_size.win_height - (MARGIN + plot_height / 2));
+ glEnd();
+
+ glBegin(GL_LINES);
+ glVertex2f(MARGIN, MARGIN + plot_height);
+ glVertex2f(MARGIN, MARGIN);
+ glVertex2f(MARGIN, MARGIN + plot_height / 2);
+ glVertex2f(MARGIN + plot_width, MARGIN + plot_height / 2);
+ glEnd();
+
+ glBegin(GL_LINES);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), MARGIN + plot_height);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), MARGIN);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width), MARGIN + plot_height / 2);
+ glVertex2f(window_size.win_width - MARGIN, MARGIN + plot_height / 2);
+ glEnd();
+
+ // Draw the targets
glColor3f(0.98, 0.349, 0.329);
- glLineWidth(1.5);
+ glLineWidth(4);
+
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(t, 1);
+ glVertex2f(MARGIN + t * scale_x,
+ window_size.win_height - (MARGIN + plot_height / 2) + 1 * scale_y
+ );
}
glEnd();
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(nbData + 0.05 + t, 0);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + t * scale_x,
+ window_size.win_height - (MARGIN + plot_height / 2) + 0 * scale_y
+ );
}
glEnd();
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(t, -2.10);
+ glVertex2f(MARGIN + t * scale_x,
+ MARGIN + plot_height / 2 + 0 * scale_y
+ );
}
glEnd();
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(nbData + 0.05 + t, -2.10);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + t * scale_x,
+ MARGIN + plot_height / 2 + 0 * scale_y
+ );
}
glEnd();
-}
-//-----------------------------------------------
-
-void DrawResults(int nbData, int nbRepros, arma::mat x_y)
-{
+ // Draw the results
const arma::fmat COLORS({
{ 0.00f, 0.00f, 1.00f },
{ 0.00f, 0.50f, 0.00f },
@@ -262,14 +345,16 @@ void DrawResults(int nbData, int nbRepros, arma::mat x_y)
{ 1.00f, 0.00f, 0.00f },
});
+ glLineWidth(1);
- glLineWidth(1.3);
for (int n = 0; n < nbRepros; n++) {
glColor3f(COLORS(n, 0), COLORS(n, 1), COLORS(n, 2));
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(t, x_y(0, n * nbData + t));
+ glVertex2f(MARGIN + t * scale_x,
+ window_size.win_height - (MARGIN + plot_height / 2) + x_y(0, n * nbData + t) * scale_y
+ );
}
glEnd();
}
@@ -279,7 +364,9 @@ void DrawResults(int nbData, int nbRepros, arma::mat x_y)
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(nbData + 0.05 + t, x_y(1, n * nbData + t));
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + t * scale_x,
+ window_size.win_height - (MARGIN + plot_height / 2) + x_y(1, n * nbData + t) * scale_y
+ );
}
glEnd();
}
@@ -289,7 +376,9 @@ void DrawResults(int nbData, int nbRepros, arma::mat x_y)
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(t, x_y(2, n * nbData + t) - 2.10);
+ glVertex2f(MARGIN + t * scale_x,
+ MARGIN + plot_height / 2 + x_y(2, n * nbData + t) * scale_y
+ );
}
glEnd();
}
@@ -299,7 +388,9 @@ void DrawResults(int nbData, int nbRepros, arma::mat x_y)
glBegin(GL_LINE_STRIP);
for (int t = 0; t < nbData; t++) {
- glVertex2f(nbData + 0.05 + t, x_y(3, n * nbData + t) - 2.10);
+ glVertex2f(window_size.win_width - (MARGIN + plot_width) + t * scale_x,
+ MARGIN + plot_height / 2 + x_y(3, n * nbData + t) * scale_y
+ );
}
glEnd();
}
@@ -422,7 +513,7 @@ int main(int argc, char **argv){
//---------------- Iterative discrete LQR with infinite horizon ----------------
- arma:: mat duTar = zeros(nbVarPos*(nbDeriv-1),1);
+ arma::mat duTar = zeros(nbVarPos*(nbDeriv-1),1);
arma::mat Q = inv(uCov);
Q.resize(6,6);
@@ -477,6 +568,13 @@ int main(int argc, char **argv){
glfwMakeContextCurrent(window);
+ // Setup ImGui binding
+ if (!REFERENTIALS)
+ {
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+ }
+
glfwSetInputMode(window, GLFW_STICKY_KEYS, 1);
while (!glfwWindowShouldClose(window)) {
@@ -489,7 +587,8 @@ int main(int argc, char **argv){
glfwGetWindowSize(window, ¤t_win_width, ¤t_win_height);
if ((current_win_width != window_size.win_width) ||
- (current_win_height != window_size.win_height)) {
+ (current_win_height != window_size.win_height) ||
+ (window_size.fb_width == -1)) {
// Retrieve the new window size
window_size.win_width = current_win_width;
@@ -499,6 +598,37 @@ int main(int argc, char **argv){
glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
}
+ if (!REFERENTIALS)
+ {
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ ui::begin("demo");
+ ImGuiWindow *win = ImGui::GetCurrentWindow();
+ ui::end();
+
+ ImVec4 clip_rect(0, 0, window_size.win_width, window_size.win_height);
+
+ const int plot_height = (window_size.win_height - 3 * MARGIN) / 2;
+
+ win->DrawList->AddText(ImGui::GetFont(), ImGui::GetFontSize() * 1.5f,
+ ImVec2(10, MARGIN + plot_height / 2 - 10),
+ ImColor(0, 0, 0, 255), "qw", NULL, 0.0f, &clip_rect);
+
+ win->DrawList->AddText(ImGui::GetFont(), ImGui::GetFontSize() * 1.5f,
+ ImVec2(window_size.win_width / 2 - 10,
+ MARGIN + plot_height / 2 - 10),
+ ImColor(0, 0, 0, 255), "qx", NULL, 0.0f, &clip_rect);
+
+ win->DrawList->AddText(ImGui::GetFont(), ImGui::GetFontSize() * 1.5f,
+ ImVec2(10, MARGIN * 2 + plot_height + plot_height / 2 - 10),
+ ImColor(0, 0, 0, 255), "qy", NULL, 0.0f, &clip_rect);
+
+ win->DrawList->AddText(ImGui::GetFont(), ImGui::GetFontSize() * 1.5f,
+ ImVec2(window_size.win_width / 2 - 10,
+ MARGIN * 2 + plot_height + plot_height / 2 - 10),
+ ImColor(0, 0, 0, 255), "qz", NULL, 0.0f, &clip_rect);
+ }
+
glViewport(0, 0, window_size.fb_width, window_size.fb_height);
glClearColor(1.f, 1.f, 1.f, 1.f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
@@ -506,11 +636,17 @@ int main(int argc, char **argv){
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
if (!REFERENTIALS)
- glOrtho(0, 2 * nbData + 0.1, -3.15, 1.05, 0, 1);
+ glOrtho(0, window_size.win_width, 0, window_size.win_height, 0, 1);
glMatrixMode( GL_MODELVIEW );
glLoadIdentity();
+ if (!REFERENTIALS)
+ {
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+ }
+
glPushMatrix();
//------------ Draw referentials in sphere ---------------
@@ -525,8 +661,7 @@ int main(int argc, char **argv){
//--------------- Draw Q1,Q2,Q3,Q4 ---------------
else
{
- DrawTarget(nbData);
- DrawResults(nbData, nbRepros, x_y);
+ DrawResults(nbData, nbRepros, x_y, window_size);
}
glPopMatrix();
@@ -539,7 +674,11 @@ int main(int argc, char **argv){
}
//Cleanup
+ if (!REFERENTIALS)
+ ImGui_ImplGlfwGL2_Shutdown();
+
glfwTerminate();
+
return 0;
}
diff --git a/src/demo_Riemannian_sphere_TPGMM01.cpp b/src/demo_Riemannian_sphere_TPGMM01.cpp
index a261a49c19937e3610d8a1cbef1063223a08b33c..3c91a78489a144273e908bd84329d65a1a2e4c58 100644
--- a/src/demo_Riemannian_sphere_TPGMM01.cpp
+++ b/src/demo_Riemannian_sphere_TPGMM01.cpp
@@ -180,7 +180,7 @@ arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
vec prob = sum((Data * inv(Sigma)) % Data, 1);
- prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + DBL_MIN);
return prob;
}
diff --git a/src/demo_TPGMMProduct01.cpp b/src/demo_TPGMMProduct01.cpp
index 49274e7e7a0eb005de40d009118ff51cfab7feda..5fb08709f22ec35ee6bc41e165107de376a823a0 100644
--- a/src/demo_TPGMMProduct01.cpp
+++ b/src/demo_TPGMMProduct01.cpp
@@ -110,7 +110,7 @@ public:
Demonstration(coordinate_system_list_t coordinate_systems,
const std::vector<arma::vec>& points,
const parameters_t& parameters)
- : coordinate_systems(coordinate_systems)
+ : coordinate_systems(coordinate_systems)
{
points_original = mat(2, points.size());
@@ -525,9 +525,6 @@ const arma::mat COLORS({
{ 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 },
});
@@ -540,28 +537,42 @@ 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)
+ int index, bool small)
+ : 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));
+ if (small) {
+ models[0] = gfx2::create_rectangle(color, 12.0f, 2.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[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[1].transforms.position(0) = 15.0f;
+ models[1].transforms.position(1) = -5.0f;
- models[2] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[2] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[2].transforms.position(0) = 15.0f;
+ models[2].transforms.position(1) = 5.0f;
+ } else {
+ models[0] = gfx2::create_rectangle(color, 25.0f, 5.0f);
+
+ models[1] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ 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.position(0) = 30.0f;
+ models[2].transforms.position(1) = 10.0f;
+ }
+
+ models[0].transforms.parent = &transforms;
+ models[1].transforms.parent = &transforms;
models[2].transforms.parent = &transforms;
- models[2].transforms.position(0) = 30.0f;
- models[2].transforms.position(1) = 10.0f;
+
+ models[0].transforms.rotation = gfx2::rotate(arma::fvec({0.0f, 0.0f, 1.0f}),
+ gfx2::deg2rad(90.0f));
}
@@ -712,13 +723,15 @@ struct gui_state_t {
//-----------------------------------------------------------------------------
// 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) {
+Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
+ int min_y, int max_x, int max_y, bool small) {
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);
+ index,
+ small
+ );
}
@@ -738,14 +751,15 @@ void create_new_demonstration_handlers(const viewport_t& viewport,
handlers.push_back(
new Handler(&viewport, ImVec2(window_size.win_width / 6,
window_size.win_height - 50),
- ImVec2(0, 30), 0)
+ ImVec2(0, 30), 0, (window_size.fb_width == window_size.win_width))
);
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)
+ window_size.win_height / 2 - 20,
+ (window_size.fb_width == window_size.win_width))
);
};
}
@@ -767,7 +781,7 @@ void create_reproduction_handlers(const viewport_t& viewport,
handlers.push_back(
new Handler(&viewport, ImVec2(window_size.win_width / 2,
window_size.win_height - 50),
- ImVec2(0, 30), 0)
+ ImVec2(0, 30), 0, (window_size.fb_width == window_size.win_width))
);
for (int n = 1; n < nb_frames; ++n) {
@@ -776,7 +790,8 @@ void create_reproduction_handlers(const viewport_t& viewport,
window_size.win_width / 3 + 20,
window_size.win_height / 2 + 20,
window_size.win_width * 2 / 3 - 20,
- window_size.win_height - 20)
+ window_size.win_height - 20,
+ (window_size.fb_width == window_size.win_width))
);
};
}
@@ -874,7 +889,7 @@ void draw_demos_viewport(const viewport_t& viewport,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
@@ -937,7 +952,7 @@ void draw_reproductions_viewport(const viewport_t& viewport,
// Render a "GMMs" viewport
//-----------------------------------------------------------------------------
void draw_GMMs_viewport(const viewport_t& viewport,
- const handler_list_t& handlers, model_t model) {
+ const handler_list_t& handlers, model_t model) {
glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
@@ -964,8 +979,8 @@ void draw_GMMs_viewport(const viewport_t& viewport,
handler_list[n][i]->draw_anchor();
}
- // Draw the GMM states
if (model.mu.size() > 0 ) {
+ // Draw the GMM states
for (int f = 0; f < model.parameters.nb_frames; f++) {
for (int i = 0; i < model.parameters.nb_states; ++i) {
@@ -975,9 +990,27 @@ void draw_GMMs_viewport(const viewport_t& viewport,
handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.sigma[i][f].submat(0, 0, 1, 1) * handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t());
}
}
- }
+ // Draw the product of the GMM states
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ vec mu = zeros(2, 1);
+ mat cov = zeros(2, 2);
+ for (int f = 0; f < model.parameters.nb_frames; f++) {
+ vec mutmp = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.mu[i][f].subvec(0, 1) + handler_list[0][f]->transforms.position.subvec(0, 1);
+ mat covtmp = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.sigma[i][f].submat(0, 0, 1, 1) * handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t();
+ mat icovtmp = arma::inv(covtmp);
+ cov = cov + icovtmp;
+ mu = mu + icovtmp * mutmp;
+
+ }
+
+ cov = arma::inv(cov);
+ mu = cov * mu;
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(fvec({0.0f, 0.0f, 0.0f, 0.6f}), mu, cov);
+ }
+ }
}
//-----------------------------------------------------------------------------
@@ -1034,8 +1067,6 @@ void draw_product_viewport(const viewport_t& viewport,
gfx2::draw_gaussian(conv_to<fvec>::from(black.row(0).t()), mu, cov);
}
}
-
-
}
@@ -1048,15 +1079,15 @@ int main(int argc, char **argv) {
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;
+ 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_width = 800;
window_size.win_height = 400;
window_size.fb_width = -1; // Will be known later
window_size.fb_height = -1;
@@ -1097,12 +1128,11 @@ int main(int argc, char **argv) {
// Viewports
viewport_t viewport_demos;
- viewport_t viewport_product;
viewport_t viewport_GMMs;
// GUI state
gui_state_t gui_state;
- gui_state.can_draw_demonstration = true;
+ 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;
@@ -1116,15 +1146,6 @@ int main(int argc, char **argv) {
handler_list_t current_demonstration_handlers;
handler_list_t reproduction_handlers;
- // create_new_demonstration_handlers(viewport_demos, window_size,
- // model.parameters.nb_frames,
- // current_demonstration_handlers
- // );
- //
- // create_reproduction_handlers(viewport_product, window_size,
- // model.parameters.nb_frames,
- // reproduction_handlers);
-
// List of demonstrations and reproductions
demonstration_list_t demos;
@@ -1146,17 +1167,13 @@ int main(int argc, char **argv) {
glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
- viewport_width = window_size.fb_width / 3 - 1;
+ viewport_width = window_size.fb_width / 2 - 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_product, viewport_width + 2,
- window_size.fb_height - viewport_height,
- viewport_width, viewport_height);
-
setup_viewport(&viewport_GMMs, window_size.fb_width - viewport_width,
window_size.fb_height - viewport_height,
viewport_width, viewport_height);
@@ -1180,28 +1197,30 @@ int main(int argc, char **argv) {
else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
cube loaded_trajectories;
mat loaded_frames;
- loaded_trajectories.load("data/data_tpbatch_lqr_trajectories.txt");
- loaded_frames.load("data/data_tpbatch_lqr_frames.txt");
+ loaded_trajectories.load("data/data_4_trajectories.txt");
+ loaded_frames.load("data/data_4_frames.txt");
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,
+ ImVec2(loaded_frames(0, n * 2) * window_size.win_width * 3 / 2,
loaded_frames(1, n * 2) * window_size.win_height * 2),
- ImVec2(loaded_frames(2, n * 2) * window_size.win_width,
+ ImVec2(loaded_frames(2, n * 2) * window_size.win_width * 3 / 2,
loaded_frames(3, n * 2) * window_size.win_height * 2),
- 0
+ 0,
+ (window_size.fb_width == window_size.win_width)
);
handler1->update(window_size);
handler1->fix();
Handler* handler2 = new Handler(
&viewport_demos,
- ImVec2(loaded_frames(0, n * 2 + 1) * window_size.win_width,
+ ImVec2(loaded_frames(0, n * 2 + 1) * window_size.win_width * 3 / 2,
loaded_frames(1, n * 2 + 1) * window_size.win_height * 2),
- ImVec2(loaded_frames(2, n * 2 + 1) * window_size.win_width,
+ ImVec2(loaded_frames(2, n * 2 + 1) * window_size.win_width * 3 / 2,
loaded_frames(3, n * 2 + 1) * window_size.win_height * 2),
- 1
+ 1,
+ (window_size.fb_width == window_size.win_width)
);
handler2->update(window_size);
handler2->fix();
@@ -1214,21 +1233,23 @@ int main(int argc, char **argv) {
}
Handler* handler_reproduction_1 = new Handler(
- &viewport_product,
+ &viewport_GMMs,
fixed_demonstration_handlers[0][0]->ui_position +
- ImVec2(window_size.win_width / 3, 0),
+ ImVec2(window_size.win_width / 2, 0),
fixed_demonstration_handlers[0][0]->ui_y,
- 0
+ 0,
+ (window_size.fb_width == window_size.win_width)
);
handler_reproduction_1->update(window_size);
reproduction_handlers.push_back(handler_reproduction_1);
Handler* handler_reproduction_2 = new Handler(
- &viewport_product,
- fixed_demonstration_handlers[0][1]->ui_position +
- ImVec2(window_size.win_width / 3, 0),
- fixed_demonstration_handlers[0][1]->ui_y,
- 1
+ &viewport_GMMs,
+ fixed_demonstration_handlers[2][1]->ui_position +
+ ImVec2(window_size.win_width / 2, 0),
+ fixed_demonstration_handlers[2][1]->ui_y,
+ 1,
+ (window_size.fb_width == window_size.win_width)
);
handler_reproduction_2->update(window_size);
reproduction_handlers.push_back(handler_reproduction_2);
@@ -1237,7 +1258,7 @@ int main(int argc, char **argv) {
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(0, span::all) *= window_size.fb_width * 3 / 2;
t(1, span::all) *= window_size.fb_height * 2;
trajectory.push_back(t);
}
@@ -1285,7 +1306,7 @@ int main(int argc, char **argv) {
current_demonstration_handlers
);
- create_reproduction_handlers(viewport_product, window_size,
+ create_reproduction_handlers(viewport_GMMs, window_size,
gui_state.parameter_nb_frames,
reproduction_handlers);
@@ -1323,8 +1344,6 @@ int main(int argc, char **argv) {
fixed_demonstration_handlers,
current_demonstration_handlers);
- draw_product_viewport(viewport_product, reproduction_handlers, model);
-
draw_GMMs_viewport(viewport_GMMs, reproduction_handlers, model);
@@ -1344,7 +1363,7 @@ int main(int argc, char **argv) {
// Window: Demonstrations
- ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
ImGui::SetNextWindowPos(ImVec2(0, 0));
ImGui::Begin("Demonstrations", NULL,
ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
@@ -1403,24 +1422,9 @@ int main(int argc, char **argv) {
ImGui::End();
- // Window: Product
- ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
- ImGui::SetNextWindowPos(ImVec2(window_size.win_width / 3, 0));
- ImGui::Begin("Product", NULL,
- ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
- ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
- ImGuiWindowFlags_NoTitleBar
- );
-
- ImGui::Text("Product");
-
- 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::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width / 2, 0));
ImGui::Begin("GMMs", NULL,
ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
@@ -1482,7 +1486,7 @@ int main(int argc, char **argv) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
- if (!hovering_ui && (mouse_x <= window_size.win_width / 3))
+ if (!hovering_ui && (mouse_x <= window_size.win_width / 2))
{
gui_state.is_drawing_demonstration = true;
diff --git a/src/demo_TPGMR01.cpp b/src/demo_TPGMR01.cpp
index 586eb41a80c1a0468c71c05c95a3ca2a310f5ec9..987d0c2db44e58e81165d6922e999cb3ce98294e 100644
--- a/src/demo_TPGMR01.cpp
+++ b/src/demo_TPGMR01.cpp
@@ -48,27 +48,27 @@ typedef std::vector<mat> matrix_list_t;
// 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!!!
+ 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
+ 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;
+ // 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;
+ int nb_var;
+ mat pix;
+ vec priors;
};
@@ -77,18 +77,18 @@ struct model_t {
//-----------------------------------------------------------------------------
struct coordinate_system_t {
- coordinate_system_t(const arma::vec& position, const arma::mat& orientation,
- const parameters_t& parameters) {
+ 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->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)));
- }
+ this->orientation = kron(eye(parameters.nb_deriv, parameters.nb_deriv),
+ orientation(span(0, 1), span(0, 1)));
+ }
- vec position;
- mat orientation;
+ vec position;
+ mat orientation;
};
@@ -108,89 +108,88 @@ typedef std::vector<coordinate_system_t> coordinate_system_list_t;
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)
+ 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);
- }
+ 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);
- }
+ 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);
+ //-------------------------------------------------------------------------
+ // 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);
+ 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");
+ 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);
+ points = mat(2 * parameters.nb_deriv, parameters.nb_data);
+ points(span(0), span::all) = x2.t();
+ points(span(1), span::all) = y2.t();
- // Compute the derivatives
- mat D = (diagmat(ones(1, parameters.nb_data - 1), -1) -
- eye(parameters.nb_data, parameters.nb_data)) / parameters.dt;
+ // 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;
+ 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);
+ 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();
+ // 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());
+ 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)));
- }
- }
+ 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));
+ //-------------------------------------------------------------------------
+ // 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);
+ vec result = pinv(coordinate_systems[frame].orientation) *
+ (original_point - coordinate_systems[frame].position);
- return result(span(0, 1));
- }
+ 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;
+ coordinate_system_list_t coordinate_systems;
+ arma::mat points_original;
+ arma::mat points;
+ matrix_list_t points_in_coordinate_systems;
};
@@ -204,7 +203,7 @@ 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 (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
}
@@ -214,16 +213,16 @@ int factorial(int n) {
//-----------------------------------------------------------------------------
arma::vec gaussPDF(const mat& data, colvec mu, mat sigma) {
- int nb_var = data.n_rows;
- int nb_data = data.n_cols;
+ int nb_var = data.n_rows;
+ int nb_data = data.n_cols;
- mat data2 = data.t() - repmat(mu.t(), nb_data, 1);
+ mat data2 = data.t() - repmat(mu.t(), nb_data, 1);
- vec prob = sum((data2 * inv(sigma)) % data2, 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);
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nb_var) * det(sigma) + DBL_MIN);
- return prob;
+ return prob;
}
@@ -232,75 +231,75 @@ arma::vec gaussPDF(const mat& data, colvec mu, mat sigma) {
// ordered dataset into equal bins
//-----------------------------------------------------------------------------
void init_tensorGMM_kbins(const demonstration_list_t& demos,
- model_t &model) {
+ model_t &model) {
- model.priors.resize(model.parameters.nb_states);
- model.mu.clear();
- model.sigma.clear();
+ 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;
+ 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);
+ // 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;
- };
+ 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));
+ 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);
- }
+ bins.push_back(bin);
+ }
- // Split each demonstration in K equal bins
- for (int n = 0; n < demos.size(); ++n) {
+ // 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 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)));
- }
- }
- }
+ 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;
- }
+ // 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());
- }
+ // 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)));
+ 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.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);
+ model.priors /= sum(model.priors);
}
@@ -313,85 +312,85 @@ void init_tensorGMM_kbins(const demonstration_list_t& demos,
// candidate coordinate systems.
//-----------------------------------------------------------------------------
void train_EM_tensorGMM(const demonstration_list_t& demos,
- model_t &model) {
+ 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
+ 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);
+ 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];
- }
- }
+ 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;
+ std::vector<double> log_likelihoods;
- for (int iter = 0; iter < nb_max_steps; ++iter) {
+ for (int iter = 0; iter < nb_max_steps; ++iter) {
- // E-step
- mat L = ones(model.parameters.nb_states, data.n_slices);
+ // 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) {
+ 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));
+ // 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]);
+ 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) % gamma0.t();
+ }
- L(i, span::all) = L(i, span::all) * model.priors(i);
- }
+ 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);
+ 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;
+ model.pix = gamma2;
- // M-step
- for (int i = 0; i < model.parameters.nb_states; ++i) {
+ // 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;
+ // Update priors
+ model.priors(i) = sum(gamma(i, span::all)) / data.n_slices;
- for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
- // Matricization/flattening of tensor
- mat data_mat(data(span::all, span(m), span::all));
+ // 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 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;
- }
- }
+ // 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);
+ // 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;
- }
- }
+ // 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;
+ }
+ }
}
@@ -400,8 +399,8 @@ void train_EM_tensorGMM(const demonstration_list_t& demos,
//-----------------------------------------------------------------------------
void learn(const demonstration_list_t& demos, model_t &model) {
- init_tensorGMM_kbins(demos, model);
- train_EM_tensorGMM(demos, model);
+ init_tensorGMM_kbins(demos, model);
+ train_EM_tensorGMM(demos, model);
}
@@ -409,7 +408,7 @@ void learn(const demonstration_list_t& demos, model_t &model) {
/*************************** DEMONSTRATION SECTION ***************************/
static void error_callback(int error, const char* description) {
- fprintf(stderr, "Error %d: %s\n", error, description);
+ fprintf(stderr, "Error %d: %s\n", error, description);
}
@@ -417,19 +416,19 @@ static void error_callback(int error, const char* 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;
+ 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;
};
@@ -437,20 +436,20 @@ struct viewport_t {
// 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;
+ 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;
}
@@ -459,19 +458,19 @@ void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
// 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;
+ 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;
+ // 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;
+ // viewport -> fb
+ result(0) = result(0) - (float) viewport.width * 0.5f;
+ result(1) = result(1) - (float) viewport.height * 0.5f;
- return result;
+ return result;
}
@@ -480,21 +479,21 @@ arma::vec ui2fb(const arma::vec& coords, const gfx2::window_size_t& window_size,
// 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;
+ 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;
+ // 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;
+ // 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;
+ result(1) = window_size.win_height - (result(1) + viewport.y) *
+ (float) window_size.win_height / (float) window_size.fb_height;
- return result;
+ return result;
}
@@ -502,41 +501,38 @@ arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
// 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 },
- });
+ { 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 },
- });
+ { 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 },
- });
+ { 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 },
+});
//-----------------------------------------------------------------------------
@@ -547,141 +543,155 @@ const arma::mat COLORS({
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;
+ Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
+ int index, bool small)
+ : viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
+ {
+ ui_position = position;
+ ui_y = y;
- fvec color = HANDLER_COLORS.row(index).t();
+ 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));
+ if (small) {
+ models[0] = gfx2::create_rectangle(color, 12.0f, 2.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[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[1].transforms.position(0) = 15.0f;
+ models[1].transforms.position(1) = -5.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;
- }
+ models[2] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[2].transforms.position(0) = 15.0f;
+ models[2].transforms.position(1) = 5.0f;
+ } else {
+ models[0] = gfx2::create_rectangle(color, 25.0f, 5.0f);
+ models[1] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[1].transforms.position(0) = 30.0f;
+ models[1].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)
- );
+ models[2] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[2].transforms.position(0) = 30.0f;
+ models[2].transforms.position(1) = 10.0f;
+ }
- arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
+ models[0].transforms.parent = &transforms;
+ models[1].transforms.parent = &transforms;
+ models[2].transforms.parent = &transforms;
- transforms.rotation(0, 0) = -delta(0);
- transforms.rotation(1, 0) = delta(1);
- transforms.rotation(0, 1) = -delta(1);
- transforms.rotation(1, 1) = -delta(0);
- }
+ models[0].transforms.rotation = gfx2::rotate(arma::fvec({0.0f, 0.0f, 1.0f}),
+ gfx2::deg2rad(90.0f));
+ }
- void update()
- {
- transforms.position(0) = ui_position.x;
- transforms.position(1) = ui_position.y;
+ 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));
+ 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);
- }
+ 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 update()
+ {
+ transforms.position(0) = ui_position.x;
+ transforms.position(1) = ui_position.y;
+ arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
- void fix()
- {
- fixed = true;
+ transforms.rotation(0, 0) = -delta(0);
+ transforms.rotation(1, 0) = delta(1);
+ transforms.rotation(0, 1) = -delta(1);
+ transforms.rotation(1, 1) = -delta(0);
+ }
- for (int i = 0; i < 3; ++i)
- models[i].diffuse_color = HANDLER_FIXED_COLORS.row(index).t();
- }
+ 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);
+ }
- 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;
+ void fix()
+ {
+ fixed = true;
- ImGui::PushID(id.str().c_str());
+ for (int i = 0; i < 3; ++i)
+ models[i].diffuse_color = HANDLER_FIXED_COLORS.row(index).t();
+ }
- // Position
- ImVec2 current_position = ui_position;
- ui_position = ui::dragger(0, ui_position, false, ui::config.draggerSize );
+ bool draw(const gfx2::window_size_t& window_size)
+ {
+ if (!fixed)
+ {
+ std::stringstream id;
+ id << "handler" << (uint64_t) this;
- moved = moved || (ui_position.x != current_position.x) || (ui_position.y != current_position.y);
+ ImGuiWindow* window = ImGui::GetCurrentWindow();
+ if (window->SkipItems)
+ return false;
- hovered = ImGui::IsItemHovered();
+ ImGui::PushID(id.str().c_str());
- // y-axis
- ImVec2 py = ui_position + ui_y;
- current_position = ui_y;
+ // Position
+ ImVec2 current_position = ui_position;
- py = ui::dragger(1, py, false, ui::config.draggerSize * 0.7);
- ui_y = py - ui_position;
+ ui_position = ui::dragger(0, ui_position, false, ui::config.draggerSize );
- moved = moved || ((ui_y.x != current_position.x) || (ui_y.y != current_position.y));
+ moved = moved || (ui_position.x != current_position.x) || (ui_position.y != current_position.y);
- hovered = ImGui::IsItemHovered() || hovered;
+ hovered = ImGui::IsItemHovered();
- window->DrawList->AddLine(ui_position, py, ui::config.lineColor);
+ // y-axis
+ ImVec2 py = ui_position + ui_y;
+ current_position = ui_y;
- ImGui::PopID();
+ py = ui::dragger(1, py, false, ui::config.draggerSize * 0.7);
+ ui_y = py - ui_position;
- update(window_size);
- }
+ moved = moved || ((ui_y.x != current_position.x) || (ui_y.y != current_position.y));
- return hovered;
- }
+ hovered = ImGui::IsItemHovered() || hovered;
+ window->DrawList->AddLine(ui_position, py, ui::config.lineColor);
- void draw_anchor() const
- {
- for (int i = 0; i < 3; ++i)
- gfx2::draw(models[i]);
- }
+ 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;
+ const viewport_t* viewport;
+ int index;
- ImVec2 ui_position;
- ImVec2 ui_y;
+ ImVec2 ui_position;
+ ImVec2 ui_y;
- gfx2::transforms_t transforms;
- gfx2::model_t models[3];
+ gfx2::transforms_t transforms;
+ gfx2::model_t models[3];
- bool hovered;
- bool fixed;
- bool moved;
+ bool hovered;
+ bool fixed;
+ bool moved;
};
@@ -697,38 +707,40 @@ typedef std::vector<Handler*> handler_list_t;
// doing, ...)
//-----------------------------------------------------------------------------
struct gui_state_t {
- // Indicates if the user can draw a new demonstration
- bool can_draw_demonstration;
+ // 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 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 dialog is displayed
+ bool is_parameters_dialog_displayed;
- // Indicates if the parameters were modified through the UI
- bool are_parameters_modified;
+ // 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;
+ // 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;
+ 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);
+Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
+ int min_y, int max_x, int max_y, bool small) {
+ 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,
+ small
+ );
}
@@ -736,28 +748,29 @@ Handler* create_random_handler(const viewport_t* viewport, int 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)
- );
- };
+ 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 / 2 - 50),
+ ImVec2(0, 30), 0, (window_size.fb_width == window_size.win_width))
+ );
+
+ 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,
+ (window_size.fb_width == window_size.win_width))
+ );
+ };
}
@@ -765,67 +778,69 @@ void create_new_demonstration_handlers(const viewport_t& viewport,
// 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)
- );
- };
+ 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, (window_size.fb_width == window_size.win_width))
+ );
+
+ 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,
+ (window_size.fb_width == window_size.win_width))
+ );
+ };
}
//-----------------------------------------------------------------------------
// 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);
- }
+ 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, (window_size.fb_width == window_size.win_width))
+ );
+
+ 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,
+ (window_size.fb_width == window_size.win_width))
+ );
+ };
+ };
+
+ for (int i = 0; i < handlers.size(); i++) {
+ handlers.at(i)->fix();
+ handlers.at(i)->update(window_size);
+ }
}
@@ -835,17 +850,17 @@ void create_reproduction_handlers_gmrmoving(const viewport_t& viewport,
// 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) {
+ const parameters_t& parameters) {
- to.clear();
+ 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)
- );
- }
+ 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)
+ );
+ }
}
@@ -854,11 +869,11 @@ void convert(const handler_list_t& from, coordinate_system_list_t &to,
//-----------------------------------------------------------------------------
void convert(const demonstration_list_t& from, std::vector<coordinate_system_list_t> &to) {
- to.clear();
+ to.clear();
- for (int n = 0; n < from.size(); ++n) {
- to.push_back(from[n].coordinate_systems);
- }
+ for (int n = 0; n < from.size(); ++n) {
+ to.push_back(from[n].coordinate_systems);
+ }
}
@@ -867,21 +882,21 @@ void convert(const demonstration_list_t& from, std::vector<coordinate_system_lis
// demonstrations
//-----------------------------------------------------------------------------
arma::fvec compute_centered_view_matrix(const demonstration_list_t& demonstrations,
- int frame) {
- vec top_left({-30, 0});
- vec bottom_right({0, 0});
+ 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());
+ 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());
- }
+ 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;
+ vec center = (bottom_right - top_left) / 2 + top_left;
- return fvec({ (float) -center(0), (float) -center(1), 0.0f });
+ return fvec({ (float) -center(0), (float) -center(1), 0.0f });
}
@@ -889,58 +904,58 @@ arma::fvec compute_centered_view_matrix(const demonstration_list_t& demonstratio
// 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();
+ 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 >= COLORS.n_rows)
+ 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();
}
@@ -948,29 +963,29 @@ void draw_demos_viewport(const viewport_t& viewport,
// 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();
- }
+ 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();
+ }
}
@@ -978,13 +993,13 @@ void draw_reproductions_viewport(const viewport_t& viewport,
// Render a "reproduction" viewport
//-----------------------------------------------------------------------------
void draw_reproductions_viewport(const viewport_t& viewport,
- const handler_list_t& handlers,
- const matrix_list_t& reproductions) {
+ const handler_list_t& handlers,
+ const matrix_list_t& reproductions) {
- std::vector<handler_list_t> handler_list;
- handler_list.push_back(handlers);
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
- draw_reproductions_viewport(viewport, handler_list, reproductions);
+ draw_reproductions_viewport(viewport, handler_list, reproductions);
}
@@ -992,949 +1007,927 @@ void draw_reproductions_viewport(const viewport_t& viewport,
// 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);
-
- }
-
-
+ 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);
+ vec b = {handler_list[0][f]->transforms.position(0), handler_list[0][f]->transforms.position(1)};
+ muGMRt.slice(f).each_col() += b;
+ 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);
-
- }
-
-
+ 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);
+ vec b = {handler_list[0][f]->transforms.position(0), handler_list[0][f]->transforms.position(1)};
+ muGMRt.slice(f).each_col() += b;
+ 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);
+ 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;
- 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
- );
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
- 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];
+ if (!glfwInit())
+ return -1;
- for (size_t i = 0; i < reproduction_handlers_moving.size(); ++i)
- delete reproduction_handlers_moving[i];
+ 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
+ );
- current_demonstration_handlers.clear();
- fixed_demonstration_handlers.clear();
- reproduction_handlers.clear();
- reproduction_handlers_moving.clear();
+ 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;
+
+
+ // 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_4_trajectories.txt");
+ loaded_frames.load("data/data_4_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,
+ (window_size.fb_width == window_size.win_width)
+ );
+ 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,
+ (window_size.fb_width == window_size.win_width)
+ );
+ 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,
+ (window_size.fb_width == window_size.win_width)
+ );
+ handler_reproduction_1->update(window_size);
+ reproduction_handlers.push_back(handler_reproduction_1);
+
+ Handler* handler_reproduction_2 = new Handler(
+ &viewport_gmr,
+ fixed_demonstration_handlers[2][1]->ui_position +
+ ImVec2(window_size.win_width / 3, 0),
+ fixed_demonstration_handlers[2][1]->ui_y,
+ 1,
+ (window_size.fb_width == window_size.win_width)
+ );
+ 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,
+ (window_size.fb_width == window_size.win_width)
+ );
+ 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,
+ (window_size.fb_width == window_size.win_width)
+ );
+ 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];
- create_new_demonstration_handlers(viewport_demos, window_size,
- gui_state.parameter_nb_frames,
- current_demonstration_handlers
- );
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
+ reproduction_handlers.clear();
+ reproduction_handlers_moving.clear();
- create_reproduction_handlers(viewport_gmr, window_size,
- gui_state.parameter_nb_frames,
- reproduction_handlers);
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ gui_state.parameter_nb_frames,
+ current_demonstration_handlers
+ );
- create_reproduction_handlers_gmrmoving(viewport_gmrmoving, window_size,
- gui_state.parameter_nb_frames,
- reproduction_handlers_moving);
+ create_reproduction_handlers(viewport_gmr, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers);
- model.parameters.nb_frames = gui_state.parameter_nb_frames;
- gui_state.can_draw_demonstration = true;
- reset_moving_handler = true;
- }
+ create_reproduction_handlers_gmrmoving(viewport_gmrmoving, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers_moving);
- // If the number of states changed, recompute the model
- if (model.parameters.nb_states != gui_state.parameter_nb_states) {
+ model.parameters.nb_frames = gui_state.parameter_nb_frames;
- model.parameters.nb_states = gui_state.parameter_nb_states;
+ gui_state.can_draw_demonstration = true;
+ reset_moving_handler = true;
+ }
- for (auto iter = demos.begin(); iter != demos.end(); ++iter)
- iter->update(model.parameters);
+ // If the number of states changed, recompute the model
+ if (model.parameters.nb_states != gui_state.parameter_nb_states) {
- if (!demos.empty()) {
- learn(demos, model);
- }
- }
+ model.parameters.nb_states = gui_state.parameter_nb_states;
- gui_state.are_parameters_modified = false;
- }
+ for (auto iter = demos.begin(); iter != demos.end(); ++iter)
+ iter->update(model.parameters);
+ if (!demos.empty()) {
+ learn(demos, model);
+ }
+ }
- // Start the rendering
- ImGui_ImplGlfwGL2_NewFrame();
+ gui_state.are_parameters_modified = false;
+ }
- 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);
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
- double mouse_x, mouse_y;
- glfwGetCursorPos(window, &mouse_x, &mouse_y);
- vec mp = ui2fb({ mouse_x, mouse_y }, window_size, viewport_gmr);
+ 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_gmr_viewport(viewport_gmr, reproduction_handlers, model, gui_state.parameter_nb_gmr_components, mp, drawIndex);
+ 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);
- }
+ if (reproduction_handlers.size() > 0) {
+ int moving_handler_ix;
+ if (model.parameters.nb_frames > 1)
+ moving_handler_ix = 1;
+ else
+ moving_handler_ix = 0;
- draw_gmrmoving_viewport(viewport_gmrmoving, reproduction_handlers_moving, model, gui_state.parameter_nb_gmr_components, drawIndex);
+ 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 the UI controls of the handlers
- ui::begin("handlers");
+ draw_gmrmoving_viewport(viewport_gmrmoving, reproduction_handlers_moving, model, gui_state.parameter_nb_gmr_components, drawIndex);
- bool hovering_ui = false;
- for (size_t i = 0; i < reproduction_handlers.size(); ++i) {
- hovering_ui = reproduction_handlers[i]->draw(window_size) || hovering_ui;
- }
+ // Draw the UI controls of the handlers
+ ui::begin("handlers");
+ bool hovering_ui = false;
- 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 < reproduction_handlers.size(); ++i) {
+ hovering_ui = reproduction_handlers[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();
+ 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;
- // 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
- );
+ ui::end();
- 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
- );
+ // 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
+ );
- gui_state.can_draw_demonstration = true;
- }
- }
+ ImGui::Text("Demonstrations ");
+ ImGui::SameLine();
- 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
+ );
- if (ImGui::Button("Clear")) {
- demos.clear();
- model.mu.clear();
- model.sigma.clear();
+ gui_state.can_draw_demonstration = true;
+ }
+ }
- for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
- delete current_demonstration_handlers[i];
+ ImGui::SameLine();
- 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];
- }
+ if (ImGui::Button("Clear")) {
+ demos.clear();
+ model.mu.clear();
+ model.sigma.clear();
- current_demonstration_handlers.clear();
- fixed_demonstration_handlers.clear();
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ delete current_demonstration_handlers[i];
- create_new_demonstration_handlers(viewport_demos, window_size,
- model.parameters.nb_frames,
- current_demonstration_handlers
- );
+ 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];
+ }
- gui_state.can_draw_demonstration = true;
- }
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
- ImGui::SameLine();
- ImGui::Text(" ");
- ImGui::SameLine();
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ model.parameters.nb_frames,
+ current_demonstration_handlers
+ );
- if (ImGui::Button("Parameters"))
- gui_state.is_parameters_dialog_displayed = true;
+ gui_state.can_draw_demonstration = 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());
+ ImGui::SameLine();
+ ImGui::Text(" ");
+ ImGui::SameLine();
- 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;
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
- 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;
+ 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());
- }
- tmpFrames.save("data/data_tpgmr_frames.txt", arma_ascii);
+ 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;
- 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;
- }
+ 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;
- tmpTrajs.save("data/data_tpgmr_trajectories.txt", arma_ascii);
+ }
+ 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;
+ }
- hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+ 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::End();
- ImGui::Text("TPGMR");
- ImGui::SameLine();
- ImGui::SliderInt("Nb components", &gui_state.parameter_nb_gmr_components, 10, 199);
+ // 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
+ );
- hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+ ImGui::Text("TPGMR");
+ ImGui::SameLine();
+ ImGui::SliderInt("Nb components", &gui_state.parameter_nb_gmr_components, 10, 199);
- ImGui::End();
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
- // 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::End();
- 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];
+ // 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
+ );
- reproduction_handlers_moving.clear();
+ 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];
- create_reproduction_handlers_gmrmoving(viewport_gmrmoving, window_size,
- gui_state.parameter_nb_frames,
- reproduction_handlers_moving);
+ reproduction_handlers_moving.clear();
- reset_moving_handler = true;
+ 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;
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
- ImGui::End();
+ 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));
+ // 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 (gui_state.is_parameters_dialog_displayed)
+ ImGui::OpenPopup("Parameters");
- if (ImGui::BeginPopupModal("Parameters", NULL,
- ImGuiWindowFlags_NoResize |
- ImGuiWindowFlags_NoSavedSettings)) {
+ 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);
+ 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;
- }
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
- ImGui::EndPopup();
+ ImGui::EndPopup();
- hovering_ui = true;
- }
+ hovering_ui = true;
+ }
- ImGui::PopStyleColor();
+ ImGui::PopStyleColor();
- // GUI rendering
- ImGui::Render();
- ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
- // Swap buffers
- glfwSwapBuffers(window);
+ // Swap buffers
+ glfwSwapBuffers(window);
- // Keyboard input
- if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
- break;
+ // 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);
+ // 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;
+ 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);
+ 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);
+ 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 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);
+ 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);
+ 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;
+ // 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) {
+ if (current_trajectory.size() > 1) {
- coordinate_system_list_t coordinate_systems;
- convert(current_demonstration_handlers, coordinate_systems, model.parameters);
+ coordinate_system_list_t coordinate_systems;
+ convert(current_demonstration_handlers, coordinate_systems, model.parameters);
- Demonstration demonstration(coordinate_systems, current_trajectory,
- model.parameters);
+ Demonstration demonstration(coordinate_systems, current_trajectory,
+ model.parameters);
- demos.push_back(demonstration);
+ demos.push_back(demonstration);
- for (int i = 0; i < current_demonstration_handlers.size(); ++i)
- current_demonstration_handlers[i]->fix();
+ 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();
+ fixed_demonstration_handlers.push_back(current_demonstration_handlers);
+ current_demonstration_handlers.clear();
- learn(demos, model);
+ learn(demos, model);
- gui_state.can_draw_demonstration = false;
- }
+ gui_state.can_draw_demonstration = false;
+ }
- current_trajectory.clear();
- }
- }
+ current_trajectory.clear();
+ }
+ }
- ImGui::CaptureMouseFromApp();
- }
+ ImGui::CaptureMouseFromApp();
+ }
- // Cleanup
- ImGui_ImplGlfwGL2_Shutdown();
- glfwTerminate();
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
- return 0;
+ return 0;
}
diff --git a/src/demo_TPbatchLQR01.cpp b/src/demo_TPbatchLQR01.cpp
index a15a5c16a7f300e91d6cfaee6a30d256060ef8d5..7e1832f6d00ddeeb965dfa056bef24510e685910 100644
--- a/src/demo_TPbatchLQR01.cpp
+++ b/src/demo_TPbatchLQR01.cpp
@@ -782,10 +782,10 @@ arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
// 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.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 },
});
@@ -794,10 +794,10 @@ const arma::fmat HANDLER_COLORS({
// 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.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 },
});
@@ -806,16 +806,13 @@ const arma::fmat HANDLER_FIXED_COLORS({
// 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.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 },
});
@@ -828,7 +825,7 @@ class Handler
{
public:
Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
- int index)
+ int index, bool small)
: viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
{
ui_position = position;
@@ -836,20 +833,34 @@ public:
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));
+ if (small) {
+ models[0] = gfx2::create_rectangle(color, 12.0f, 2.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[1] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[1].transforms.position(0) = 15.0f;
+ models[1].transforms.position(1) = -5.0f;
+
+ models[2] = gfx2::create_rectangle(color, 30.0f, 2.0f);
+ models[2].transforms.position(0) = 15.0f;
+ models[2].transforms.position(1) = 5.0f;
+ } else {
+ models[0] = gfx2::create_rectangle(color, 25.0f, 5.0f);
- models[2] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[1] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ 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.position(0) = 30.0f;
+ models[2].transforms.position(1) = 10.0f;
+ }
+
+ models[0].transforms.parent = &transforms;
+ models[1].transforms.parent = &transforms;
models[2].transforms.parent = &transforms;
- models[2].transforms.position(0) = 30.0f;
- models[2].transforms.position(1) = 10.0f;
+
+ models[0].transforms.rotation = gfx2::rotate(arma::fvec({0.0f, 0.0f, 1.0f}),
+ gfx2::deg2rad(90.0f));
}
@@ -1007,13 +1018,15 @@ struct gui_state_t {
//-----------------------------------------------------------------------------
// 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) {
+Handler* create_random_handler(const viewport_t* viewport, int index, int min_x,
+ int min_y, int max_x, int max_y, bool small) {
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);
+ index,
+ small
+ );
}
@@ -1033,14 +1046,15 @@ void create_new_demonstration_handlers(const viewport_t& viewport,
handlers.push_back(
new Handler(&viewport, ImVec2(window_size.win_width / 6,
window_size.win_height / 2 - 50),
- ImVec2(0, 30), 0)
+ ImVec2(0, 30), 0, (window_size.fb_width == window_size.win_width))
);
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)
+ window_size.win_height / 2 - 20,
+ (window_size.fb_width == window_size.win_width))
);
};
}
@@ -1062,7 +1076,7 @@ void create_reproduction_handlers(const viewport_t& viewport,
handlers.push_back(
new Handler(&viewport, ImVec2(window_size.win_width / 2,
window_size.win_height - 50),
- ImVec2(0, 30), 0)
+ ImVec2(0, 30), 0, (window_size.fb_width == window_size.win_width))
);
for (int n = 1; n < nb_frames; ++n) {
@@ -1071,7 +1085,8 @@ void create_reproduction_handlers(const viewport_t& viewport,
window_size.win_width / 3 + 20,
window_size.win_height / 2 + 20,
window_size.win_width * 2 / 3 - 20,
- window_size.win_height - 20)
+ window_size.win_height - 20,
+ (window_size.fb_width == window_size.win_width))
);
};
}
@@ -1169,7 +1184,7 @@ void draw_demos_viewport(const viewport_t& viewport,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
@@ -1188,6 +1203,7 @@ void draw_demos_viewport(const viewport_t& viewport,
// Render a "model" viewport
//-----------------------------------------------------------------------------
void draw_model_viewport(const viewport_t& viewport,
+ const gfx2::window_size_t window_size,
const demonstration_list_t& demonstrations,
int perspective,
const model_t& model,
@@ -1241,12 +1257,13 @@ void draw_model_viewport(const viewport_t& viewport,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
// Draw the handler
- Handler handler(&viewport, ImVec2(0, 0), ImVec2(-30, 0), perspective);
+ Handler handler(&viewport, ImVec2(0, 0), ImVec2(-30, 0), perspective,
+ (window_size.fb_width == window_size.win_width));
handler.update();
handler.draw_anchor();
@@ -1308,7 +1325,7 @@ void draw_reproductions_viewport(const viewport_t& viewport,
gfx2::draw_line(color, *iter);
++color_index;
- if (color_index >= reproductions.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
@@ -1343,7 +1360,7 @@ int main(int argc, char **argv) {
model_t model;
// Parameters
- model.parameters.nb_states = 6;
+ model.parameters.nb_states = 3;
model.parameters.nb_frames = 2;
model.parameters.nb_deriv = 2;
model.parameters.nb_data = 200;
@@ -1404,7 +1421,7 @@ int main(int argc, char **argv) {
// GUI state
gui_state_t gui_state;
- gui_state.can_draw_demonstration = true;
+ 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;
@@ -1413,7 +1430,7 @@ int main(int argc, char **argv) {
gui_state.parameter_nb_frames = model.parameters.nb_frames;
gui_state.parameter_nb_data = model.parameters.nb_data;
gui_state.parameter_nb_stoch_repros = model.parameters.nb_stoch_repros;
- gui_state.displayed_frame = 2;
+ gui_state.displayed_frame = 1;
// Demonstration and reproduction handlers
@@ -1487,8 +1504,8 @@ int main(int argc, char **argv) {
else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
cube loaded_trajectories;
mat loaded_frames;
- loaded_trajectories.load("data/data_tpbatch_lqr_trajectories.txt");
- loaded_frames.load("data/data_tpbatch_lqr_frames.txt");
+ loaded_trajectories.load("data/data_4_trajectories.txt");
+ loaded_frames.load("data/data_4_frames.txt");
for (int n = 0; n < loaded_frames.n_cols / 2; ++n) {
Handler* handler1 = new Handler(
@@ -1497,7 +1514,8 @@ int main(int argc, char **argv) {
loaded_frames(1, n * 2) * window_size.win_height),
ImVec2(loaded_frames(2, n * 2) * window_size.win_width,
loaded_frames(3, n * 2) * window_size.win_height),
- 0
+ 0,
+ (window_size.fb_width == window_size.win_width)
);
handler1->update(window_size);
handler1->fix();
@@ -1508,7 +1526,8 @@ int main(int argc, char **argv) {
loaded_frames(1, n * 2 + 1) * window_size.win_height),
ImVec2(loaded_frames(2, n * 2 + 1) * window_size.win_width,
loaded_frames(3, n * 2 + 1) * window_size.win_height),
- 1
+ 1,
+ (window_size.fb_width == window_size.win_width)
);
handler2->update(window_size);
handler2->fix();
@@ -1525,7 +1544,8 @@ int main(int argc, char **argv) {
fixed_demonstration_handlers[0][0]->ui_position +
ImVec2(window_size.win_width / 3, window_size.win_height / 2),
fixed_demonstration_handlers[0][0]->ui_y,
- 0
+ 0,
+ (window_size.fb_width == window_size.win_width)
);
handler_reproduction_1->update(window_size);
reproduction_handlers.push_back(handler_reproduction_1);
@@ -1535,7 +1555,8 @@ int main(int argc, char **argv) {
fixed_demonstration_handlers[0][1]->ui_position +
ImVec2(window_size.win_width / 3, window_size.win_height / 2),
fixed_demonstration_handlers[0][1]->ui_y,
- 1
+ 1,
+ (window_size.fb_width == window_size.win_width)
);
handler_reproduction_2->update(window_size);
reproduction_handlers.push_back(handler_reproduction_2);
@@ -1715,13 +1736,14 @@ int main(int argc, char **argv) {
draw_reproductions_viewport(viewport_repros, fixed_demonstration_handlers,
reproductions);
- draw_model_viewport(viewport_model, demos, gui_state.displayed_frame - 1, model);
+ draw_model_viewport(viewport_model, window_size, demos,
+ gui_state.displayed_frame - 1, model);
draw_reproductions_viewport(viewport_new_repros, reproduction_handlers,
new_reproductions);
- draw_model_viewport(viewport_stochastic_model, demos, gui_state.displayed_frame - 1,
- model, &stochastic_mu);
+ draw_model_viewport(viewport_stochastic_model, window_size, demos,
+ gui_state.displayed_frame - 1, model, &stochastic_mu);
draw_reproductions_viewport(viewport_stochastic_repros, reproduction_handlers,
stochastic_reproductions);
@@ -1947,10 +1969,34 @@ int main(int argc, char **argv) {
if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
break;
+ // When the 'S' key is pressed, save the demonstrations in files
+ if (ImGui::IsKeyPressed(GLFW_KEY_S)) {
+ mat frames(4, fixed_demonstration_handlers.size() * gui_state.parameter_nb_frames);
+ for (size_t i = 0; i < fixed_demonstration_handlers.size(); ++i) {
+ for (size_t j = 0; j < fixed_demonstration_handlers[i].size(); ++j) {
+ frames(0, i * gui_state.parameter_nb_frames + j) = (float) fixed_demonstration_handlers[i][j]->ui_position.x / window_size.win_width;
+ frames(1, i * gui_state.parameter_nb_frames + j) = (float) fixed_demonstration_handlers[i][j]->ui_position.y / window_size.win_height;
+ frames(2, i * gui_state.parameter_nb_frames + j) = (float) fixed_demonstration_handlers[i][j]->ui_y.x / window_size.win_width;
+ frames(3, i * gui_state.parameter_nb_frames + j) = (float) fixed_demonstration_handlers[i][j]->ui_y.y / window_size.win_height;
+ }
+ }
+
+ frames.save("data_tpbatch_lqr_frames.txt", arma_ascii);
+
+ cube trajectories(2, gui_state.parameter_nb_data, demos.size());
+ for (size_t i = 0; i < demos.size(); ++i) {
+ for (size_t j = 0; j < gui_state.parameter_nb_data; ++j) {
+ trajectories(0, j, i) = demos[i].points(0, j) / window_size.fb_width;
+ trajectories(1, j, i) = demos[i].points(1, j) / window_size.fb_height;
+ }
+ }
+
+ trajectories.save("data_tpbatch_lqr_trajectories.txt", arma_ascii);
+ }
// Left click: start a new demonstration (only if not on the UI and in the
// demonstrations viewport)
- if (!gui_state.is_drawing_demonstration) {
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1) && gui_state.can_draw_demonstration) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
@@ -1964,7 +2010,7 @@ int main(int argc, char **argv) {
current_trajectory.push_back(coords);
}
}
- } else {
+ } else if (gui_state.is_drawing_demonstration) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
diff --git a/src/demo_ergodicControl_2D01.cpp b/src/demo_ergodicControl_2D01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6bc3f9cec810aa4f9e8a241a523e96fe7675db6
--- /dev/null
+++ b/src/demo_ergodicControl_2D01.cpp
@@ -0,0 +1,562 @@
+/*
+ * demo_ergodicControl_2D01.cpp
+ *
+ * 2D ergodic control with spectral multiscale coverage (SMC) algorithm, based on
+ * "Spectral Multiscale Coverage: A Uniform Coverage Algorithm for Mobile Sensor Networks"
+ * by George Mathew and Igor Mezic (http://www.geoggy.net/resources/SMC_CDC09.pdf)
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+#include <stdio.h>
+#include <armadillo>
+#include <mvn.h>
+#include <tuple>
+#include <mpc_utils.h>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <GLFW/glfw3.h>
+#include <window_utils.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_gaussians; // Number of gaussians that control the trajectory
+ int nb_data; // Number of datapoints
+ int nb_fct[2]; // Number of basis functions along x and y
+ int nb_res; // Resolution of discretization for the computation of
+ // Fourier coefficients of coverage distribution
+ float dt; // Time step
+ mat xlim; // Domain limits
+};
+
+
+//-----------------------------------------------------------------------------
+// Implementation of the algorithm
+//-----------------------------------------------------------------------------
+std::tuple<mat, mat> compute(const parameters_t& parameters, const mat& mu,
+ const cube& sigma) {
+
+ const int nb_var = 2; // Dimension of datapoint
+ const float sp = ((float) nb_var + 1.0f) / 2.0f; // Sobolev norm parameter
+
+ vec xsiz({ parameters.xlim(0, 1) - parameters.xlim(0, 0),
+ parameters.xlim(1, 1) - parameters.xlim(1, 0)
+ }); // Domain size
+
+ vec dx = xsiz / parameters.nb_res; // Spatial increments
+ vec x({0.1f, 0.3f}); // Initial position
+
+
+ // Basis functions (Fourier coefficients of coverage distribution)
+ //----------------------------------------------------------------
+ vector_list_t rg;
+ rg.push_back(linspace<vec>(0, parameters.nb_fct[0] - 1, parameters.nb_fct[0]));
+ rg.push_back(linspace<vec>(0, parameters.nb_fct[1] - 1, parameters.nb_fct[1]));
+
+ mat KX1 = repmat(rg[0], 1, parameters.nb_fct[1]);
+ mat KX2 = repmat(rg[1].t(), parameters.nb_fct[0], 1);
+
+ mat LK = pow(pow(KX1, 2) + pow(KX2, 2) + 1, -sp); // Weighting matrix
+
+ mat HK = join_vert(mat({1.0}), sqrt(0.5) * ones(parameters.nb_fct[0] - 1)) *
+ join_vert(mat({1.0}), sqrt(0.5) * ones(parameters.nb_fct[1] - 1)).t() *
+ sqrt(xsiz(0) * xsiz(1)); // Normalizing matrix
+
+ mat X = repmat(linspace<vec>(parameters.xlim(0, 0), parameters.xlim(0, 1) - dx(0), parameters.nb_res).t(),
+ parameters.nb_res, 1);
+
+ mat Y = repmat(linspace<vec>(parameters.xlim(1, 0), parameters.xlim(1, 1) - dx(1), parameters.nb_res),
+ 1, parameters.nb_res);
+
+ // Desired spatial distribution as mixture of Gaussians
+ cube G_(X.n_rows, X.n_cols, mu.n_cols);
+
+ mat XY(X.n_elem, 2);
+ XY(span(0, X.n_elem - 1), 0) = reshape(X, X.n_elem, 1);
+ XY(span(0, X.n_elem - 1), 1) = reshape(Y, Y.n_elem, 1);
+
+ for (int k = 0; k < mu.n_cols; ++k) {
+ G_.slice(k) = reshape(
+ mvn::getPDFValue(mu(span::all, k), sigma.slice(k), XY.t()),
+ X.n_rows, X.n_cols
+ ).t() / mu.n_cols;
+ }
+
+ mat G = sum(G_, 2);
+ G /= accu(G); // Spatial distribution
+
+ // Computation of phi_k by discretization
+ // mat phi = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+ // for (int kx = 0; kx < parameters.nb_fct[0]; ++kx) {
+ // for (int ky = 0; ky < parameters.nb_fct[1]; ++ky) {
+ // phi(kx, ky) = accu(G % cos(X * kx * datum::pi / xsiz(0)) % cos(Y * ky * datum::pi / xsiz(1))) /
+ // HK(kx, ky); // Fourier coefficients of spatial distribution
+ // }
+ // }
+
+ // Explicit description of phi_k by exploiting the Fourier transform properties
+ // of Gaussians
+ mat w1 = KX1 * datum::pi / xsiz(0);
+ mat w2 = KX2 * datum::pi / xsiz(1);
+
+ mat w(2, w1.n_elem);
+ w(0, span::all) = reshape(w1, 1, w1.n_elem);
+ w(1, span::all) = reshape(w2, 1, w1.n_elem);
+
+ // Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1])
+ mat op({ {-1, 1, -1, 1}, {-1, -1, 1, 1} });
+
+ // Compute phi_k
+ mat phi = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+ for (int k = 0; k < mu.n_cols; ++k) {
+ for (int n = 0; n < op.n_cols; ++n) {
+ mat MuTmp = diagmat(op(span::all, n)) * mu(span::all, k);
+ mat SigmaTmp = diagmat(op(span::all, n)) * sigma.slice(k) * diagmat(op(span::all, n)).t();
+ phi = phi + reshape(cos(w.t() * MuTmp) % diagvec(exp(-0.5 * w.t() * SigmaTmp * w)), size(HK));
+ }
+ }
+ phi = phi / HK / mu.n_cols / op.n_cols;
+
+
+ // Ergodic control with spectral multiscale coverage (SMC) algorithm
+ //------------------------------------------------------------------
+ mat Ck = zeros(parameters.nb_fct[0], parameters.nb_fct[1]);
+
+ mat result = zeros(nb_var, parameters.nb_data);
+
+ for (int t = 0; t < parameters.nb_data; ++t) {
+
+ // Log data
+ result(span::all, t) = x;
+
+ // Updating Fourier cosine coefficients of coverage distribution for each dimension
+ vector_list_t cx;
+ vector_list_t dcx;
+
+ for (int i = 0; i < nb_var; ++i) {
+ cx.push_back(cos(rg[i] * datum::pi * (x(i) - parameters.xlim(i, 0)) / xsiz(i)));
+ dcx.push_back(sin(rg[i] * datum::pi * (x(i) - parameters.xlim(i, 0)) / xsiz(i)) % rg[i] * datum::pi / xsiz(i));
+ }
+
+ // Fourier cosine coefficients along trajectory
+ Ck = Ck + (repmat(cx[0], 1, parameters.nb_fct[1]) % repmat(cx[1].t(), parameters.nb_fct[0], 1)) / HK * parameters.dt;
+
+ // SMC feedback control law
+ dx(0) = accu((LK / HK) % (Ck - phi * (t + 1) * parameters.dt) %
+ (repmat(dcx[0], 1, parameters.nb_fct[1]) % repmat(cx[1].t(), parameters.nb_fct[0], 1)));
+
+ dx(1) = accu((LK / HK) % (Ck - phi * (t + 1) * parameters.dt) %
+ (repmat(cx[0], 1, parameters.nb_fct[1]) % repmat(dcx[1].t(), parameters.nb_fct[0], 1)));
+
+ x = x + dx * parameters.dt;
+ }
+
+ return std::make_tuple(result, G);
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description){
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+//-----------------------------------------------
+
+std::tuple<vec, mat> trans2d_to_gauss(const ui::Trans2d& gaussian_transforms,
+ const gfx2::window_size_t& window_size) {
+
+ vec mu = gfx2::ui2fb_centered(vec({ gaussian_transforms.pos.x, gaussian_transforms.pos.y }),
+ window_size);
+
+ vec t_x({
+ gaussian_transforms.x.x * window_size.scale_x(),
+ gaussian_transforms.x.y * window_size.scale_y()
+ });
+
+ vec t_y({
+ gaussian_transforms.y.x * window_size.scale_x(),
+ gaussian_transforms.y.y * window_size.scale_y()
+ });
+
+ mat RG = {
+ { t_x(0), t_y(0) },
+ { -t_x(1), -t_y(1) }
+ };
+
+ mat sigma = RG * RG.t();
+
+ return std::make_tuple(mu, sigma);
+}
+
+//-----------------------------------------------
+
+void gauss_to_trans2d(const vec& mu, const mat& sigma,
+ const gfx2::window_size_t& window_size, ui::Trans2d &t2d) {
+
+ vec ui_mu = gfx2::fb2ui_centered(mu, window_size);
+
+ t2d.pos.x = ui_mu(0);
+ t2d.pos.y = ui_mu(1);
+
+ mat V;
+ vec d;
+ eig_sym(d, V, sigma);
+ mat VD = V * diagmat(sqrt(d));
+
+ t2d.x.x = VD.col(0)(0) / window_size.scale_x();
+ t2d.x.y = VD.col(1)(0) / window_size.scale_y();
+ t2d.y.x = VD.col(0)(1) / window_size.scale_x();
+ t2d.y.y = VD.col(1)(1) / window_size.scale_y();
+}
+
+//-----------------------------------------------
+
+std::tuple<mat, cube, std::vector<ui::Trans2d> > create_random_gaussians(
+ const parameters_t& parameters, const gfx2::window_size_t& window_size) {
+
+ mat Mu = randu(2, parameters.nb_gaussians);
+ Mu.row(0) = Mu.row(0) * (window_size.fb_width - 200) - (window_size.fb_width / 2 - 100);
+ Mu.row(1) = Mu.row(1) * (window_size.fb_height - 200) - (window_size.fb_height / 2 - 100);
+
+ cube Sigma;
+ randomCovariances(
+ &Sigma, Mu,
+ vec({ 100 * (float) window_size.fb_width / window_size.win_width,
+ 100 * (float) window_size.fb_height / window_size.win_height
+ }),
+ true, 0.0, 0.2
+ );
+
+ std::vector<ui::Trans2d> gaussians(parameters.nb_gaussians, ui::Trans2d());
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ gauss_to_trans2d(Mu.col(i), Sigma.slice(i), window_size, gaussians[i]);
+
+ fmat rotation = gfx2::rotate(fvec({ 0.0f, 0.0f, 1.0f }), randu() * 2 * datum::pi);
+
+ fvec x = rotation * fvec({ gaussians[i].x.x, gaussians[i].x.y, 0.0f, 0.0f });
+ gaussians[i].x.x = x(0);
+ gaussians[i].x.y = x(1);
+
+ fvec y = rotation * fvec({ gaussians[i].y.x, gaussians[i].y.y, 0.0f, 0.0f });
+ gaussians[i].y.x = y(0);
+ gaussians[i].y.y = y(1);
+
+ vec mu_;
+ mat sigma_;
+ std::tie(mu_, sigma_) = trans2d_to_gauss(gaussians[i], window_size);
+
+ Sigma.slice(i) = sigma_;
+ }
+
+ return std::make_tuple(Mu, Sigma, gaussians);
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv){
+
+ arma_rng::set_seed_random();
+
+ // Parameters
+ parameters_t parameters;
+ parameters.nb_gaussians = 2;
+ parameters.nb_data = 2000;
+ parameters.nb_fct[0] = 20;
+ parameters.nb_fct[1] = 20;
+ parameters.nb_res = 100;
+ parameters.dt = 0.01f;
+ parameters.xlim = mat({ { 0.0f, 1.0f }, { 0.0f, 1.0f } } );
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+
+
+ // 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 2D ergodic control",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+
+ // Setup GLSL
+ gfx2::init();
+ 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);
+
+
+ // Gaussian widgets
+ std::vector<ui::Trans2d> gaussians;
+
+ // Covariances
+ mat Mu;
+ cube Sigma;
+
+ // Main loop
+ mat result;
+ mat G;
+ const float speed = 1.0f / 20.0f;
+ float t = 0.0f;
+ bool must_recompute = false;
+
+ gfx2::texture_t background_texture = {0};
+
+ 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);
+
+ int previous_win_width = window_size.win_width;
+ int previous_win_height = window_size.win_height;
+ int previous_fb_width = window_size.fb_width;
+ int previous_fb_height = window_size.fb_height;
+
+ 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);
+
+ // Move and rescale the various objects so they stay in the window
+ if (!first) {
+ float scale = std::min((float) window_size.fb_width / previous_fb_width,
+ (float) window_size.fb_height / previous_fb_height);
+
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ arma::vec target = gfx2::fb2ui_centered(gfx2::ui2fb_centered({gaussians[i].pos.x, gaussians[i].pos.y},
+ previous_win_width, previous_win_height,
+ previous_fb_width, previous_fb_height) * scale,
+ window_size.win_width, window_size.win_height,
+ window_size.fb_width, window_size.fb_height);
+ gaussians[i].pos.x = target(0);
+ gaussians[i].pos.y = target(1);
+
+ gaussians[i].x.x *= scale;
+ gaussians[i].x.y *= scale;
+ gaussians[i].y.x *= scale;
+ gaussians[i].y.y *= scale;
+ }
+ }
+
+ // At the very first frame: random initialisation of the gaussians (taking 4K
+ // screens into account)
+ else if ((window_size.fb_width > 0) && (window_size.win_width > 0)) {
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(parameters, window_size);
+ }
+
+ must_recompute = true;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-window_size.fb_width / 2, window_size.fb_width / 2,
+ -window_size.fb_height / 2, window_size.fb_height / 2,
+ -1.0f, 1.0f
+ );
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ if (result.n_cols > 0) {
+
+ double max = G.max();
+ double min = G.min();
+
+ if (background_texture.width == 0) {
+ background_texture = gfx2::create_texture(
+ parameters.nb_res, parameters.nb_res, GL_RGB, GL_FLOAT
+ );
+
+ for (int j = 0; j < parameters.nb_res; ++j) {
+ for (int i = 0; i < parameters.nb_res; ++i) {
+ float color = 1.0f - (G(i, j) - min) / (max - min);
+
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3] = color;
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3 + 1] = 1.0f;
+ background_texture.pixels_f[(j * parameters.nb_res + i) * 3 + 2] = color;
+ }
+ }
+ }
+
+ gfx2::draw_rectangle(background_texture, window_size.fb_width, window_size.fb_height);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ mat result2(result.n_rows, result.n_cols);
+ result2(0, span::all) = (result(0, span::all) - 0.5) * window_size.fb_width;
+ result2(1, span::all) = (result(1, span::all) - 0.5) * window_size.fb_height;
+
+ gfx2::draw_line(fvec({ 0.0f, 0.0f, 1.0f }), result2);
+
+ int current_index = t * result2.n_cols;
+
+ if (current_index > 0) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ glLineWidth(4.0f);
+ gfx2::draw_line(fvec({ 1.0f, 0.0f, 0.0f }), result2(span::all, span(0, current_index)));
+ glLineWidth(1.0f);
+ }
+
+ fvec current_position = zeros<fvec>(3);
+ current_position(0) = result2(0, current_index);
+ current_position(1) = result2(1, current_index);
+
+ gfx2::draw_rectangle(fvec({ 1.0f, 0.0f, 0.0f }), 10.0f, 10.0f, current_position);
+ }
+
+
+ // Control panel GUI
+ ImGui::SetNextWindowSize(ImVec2(650, 130));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Control Panel", NULL,
+ ImGuiWindowFlags_NoTitleBar|ImGuiWindowFlags_NoResize|
+ ImGuiWindowFlags_NoMove|ImGuiWindowFlags_NoSavedSettings);
+
+ int previous_nb_gaussians = parameters.nb_gaussians;
+ int previous_nb_data = parameters.nb_data;
+ int previous_nb_fct_x = parameters.nb_fct[0];
+ int previous_nb_fct_y = parameters.nb_fct[1];
+ int previous_nb_res = parameters.nb_res;
+
+ ImGui::SliderInt("Nb gaussians", ¶meters.nb_gaussians, 1, 10);
+ ImGui::SliderInt("Nb data", ¶meters.nb_data, 500, 10000);
+ ImGui::SliderInt("Nb basis functions along X", ¶meters.nb_fct[0], 5, 100);
+ ImGui::SliderInt("Nb basis functions along Y", ¶meters.nb_fct[1], 5, 100);
+ ImGui::SliderInt("Resolution of discretization", ¶meters.nb_res, 20, 500);
+
+ if ((parameters.nb_gaussians != previous_nb_gaussians) ||
+ (parameters.nb_data != previous_nb_data) ||
+ (parameters.nb_fct[0] != previous_nb_fct_x) ||
+ (parameters.nb_fct[1] != previous_nb_fct_y) ||
+ (parameters.nb_res != previous_nb_res)) {
+
+ must_recompute = true;
+ }
+
+ ImGui::End();
+
+
+ // Gaussian widgets
+ if (parameters.nb_gaussians != gaussians.size())
+ std::tie(Mu, Sigma, gaussians) = create_random_gaussians(parameters, window_size);
+
+ ui::begin("Gaussian");
+
+ if (!gaussians.empty()) {
+ for (int i = 0; i < parameters.nb_gaussians; ++i) {
+ ui::Trans2d previous_gaussian = gaussians[i];
+
+ gaussians[i] = ui::affineSimple(i, gaussians[i]);
+
+ vec mu;
+ mat sigma;
+ std::tie(mu, sigma) = trans2d_to_gauss(gaussians[i], window_size);
+
+ Mu.col(i) = mu;
+ Sigma.slice(i) = sigma;
+
+ must_recompute = must_recompute ||
+ !gfx2::is_close(gaussians[i].pos.x, previous_gaussian.pos.x) ||
+ !gfx2::is_close(gaussians[i].pos.y, previous_gaussian.pos.y) ||
+ !gfx2::is_close(gaussians[i].x.x, previous_gaussian.x.x) ||
+ !gfx2::is_close(gaussians[i].x.y, previous_gaussian.x.y) ||
+ !gfx2::is_close(gaussians[i].y.x, previous_gaussian.y.x) ||
+ !gfx2::is_close(gaussians[i].y.y, previous_gaussian.y.y);
+ }
+ }
+
+ ui::end();
+
+
+ // Redo the computation when necessary
+ if (must_recompute && !ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ mat mu = Mu;
+ mu(0, span::all) = mu(0, span::all) / window_size.fb_width + 0.5;
+ mu(1, span::all) = mu(1, span::all) / window_size.fb_height + 0.5;
+
+ std::tie(result, G) = compute(parameters, mu, Sigma / (window_size.fb_width * window_size.fb_height));
+ t = 0.0f;
+
+ if (background_texture.width > 0)
+ gfx2::destroy(background_texture);
+
+ must_recompute = false;
+ }
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glPopMatrix();
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ t += speed * ImGui::GetIO().DeltaTime;
+ if (t >= 1.0f)
+ t = 0.0f;
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_infHorLQR01_glsl.cpp b/src/demo_infHorLQR01_glsl.cpp
deleted file mode 100644
index 7a4774a2526ec4f1d8b1bf176a19e9162a79c6b8..0000000000000000000000000000000000000000
--- a/src/demo_infHorLQR01_glsl.cpp
+++ /dev/null
@@ -1,341 +0,0 @@
-/*
- * demo_infHorLQR01_glsl.cpp
- *
- * Discrete infinite horizon linear quadratic regulation
- *
- * Fabien Crépon, Philip Abbet, Sylvain Calinon, 2017
- */
-
-#include <stdio.h>
-#include <imgui.h>
-#include <imgui_impl_glfw_gl3.h>
-#include <gfx3.h>
-#include <gfx_ui.h>
-#include <GLFW/glfw3.h>
-#include <lqr.h>
-#include <window_utils.h>
-#include <armadillo>
-
-using namespace arma;
-
-
-/****************************** HELPER FUNCTIONS *****************************/
-
-static void error_callback(int error, const char* description){
- fprintf(stderr, "Error %d: %s\n", error, description);
-}
-
-//-----------------------------------------------
-
-int factorial(int n) {
- return (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
-}
-
-//-----------------------------------------------------------------------------
-// Computes RG (in OpenGL-space) from gaussian transformations (in UI-space)
-//
-// UI coordinates range from (0, 0) to (win_width, win_height)
-// OpenGL coordinates range from (-fb_width / 2, fb_height / 2) to
-// (fb_width / 2, -fb_height / 2)
-//-----------------------------------------------------------------------------
-arma::mat trans2RG(const ui::Trans2d& trans, const gfx3::window_size_t& window_size) {
-
- float scale_x = (float) window_size.fb_width / (float) window_size.win_width;
- float scale_y = (float) window_size.fb_height / (float) window_size.win_height;
-
- arma::mat RG = {
- { trans.x.x * scale_x, trans.y.x * scale_x },
- { -trans.x.y * scale_y, -trans.y.y * scale_y }
- };
-
- return RG;
-}
-
-
-/******************************* MAIN FUNCTION *******************************/
-
-int main(int argc, char **argv){
- arma_rng::set_seed_random();
-
- //--------------- Setup parameters ---------------
-
- int nbData = 100; //Number of datapoints
- int nbRepros = 10; //Number of reproductions
- int nbVarPos = 2; //Dimension of position data (here: x1,x2)
- int nbDeriv = 2; //Number of static & dynamic features (D=2 for [x,dx])
- int nbVar = nbVarPos * nbDeriv; //Dimension of state vector in the tangent space
- double dt = 1E-2; //Time step duration
- double rfactor = 1E-8; //Control cost in LQR
-
- // Control cost
- arma::mat R = eye(nbVarPos,nbVarPos) * rfactor;
-
- // Target
- arma::vec Utar = {100.0, 300.0, 0, 0};
-
- // Initial positions
- arma:mat U0(nbVarPos, 0);
- arma::mat pts0(2, 35), pts(3, 35, fill::zeros);
- pts0 = join_cols(cos(linspace<rowvec>(0, 2 * datum::pi, 35)), sin(linspace<rowvec>(0, 2 * datum::pi, 35)));
- arma::mat RG(2, 2, fill::eye);
-
-
- //--------------- Discrete dynamical system settings ---------------
-
- arma::mat A, B;
- arma::mat A1d(zeros(nbDeriv, nbDeriv));
- for (int i = 0; i <= nbDeriv - 1 ; i++) {
- A1d = A1d + diagmat(ones(nbDeriv - i, 1), i) * pow(dt, i) * 1. / factorial(i);
- }
- arma::mat B1d(zeros(nbDeriv, 1));
- for (int i = 1; i <= nbDeriv ; i++) {
- B1d(nbDeriv - i, 0) = pow(dt, i) * 1. / factorial(i);
- }
- A = kron(A1d, eye(nbVarPos, nbVarPos));
- B = kron(B1d, eye(nbVarPos, nbVarPos));
-
-
- //--------------- Setup of the rendering ---------------
-
- // Take 4k screens into account (framebuffer size != window size)
- gfx3::window_size_t window_size;
- window_size.win_width = 800;
- window_size.win_height = 800;
- window_size.fb_width = -1; // Will be known later
- window_size.fb_height = -1;
-
- // Setup GUI
- glfwSetErrorCallback(error_callback);
- if (!glfwInit())
- return -1;
-
- glfwWindowHint(GLFW_SAMPLES, 4);
- glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
- glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
- glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
- glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
-
- GLFWwindow* window = create_window_at_optimal_size(
- "Demo LQR", window_size.win_width, window_size.win_height
- );
-
- glfwMakeContextCurrent(window);
-
- // Setup GLSL
- gfx3::init();
- glEnable(GL_DEPTH_TEST);
- glEnable(GL_CULL_FACE);
- glEnable(GL_LINE_SMOOTH);
- glDepthFunc(GL_LESS);
-
- // Creation of the Vertex Array Object (VAO)
- GLuint vertexArrayID;
- glGenVertexArrays(1, &vertexArrayID);
- glBindVertexArray(vertexArrayID);
-
- // Setup ImGui
- ImGui::CreateContext();
- ImGui_ImplGlfwGL3_Init(window, true);
- ImVec4 clear_color = ImColor(0, 0, 40);
-
-
- //--------------- Rendering matrices, shaders, models ---------------
-
- // Gaussian UI widget
- ui::Trans2d trans(ImVec2(80.0f, 0.0f), ImVec2(0.0f, 100.0f),
- ImVec2((float) Utar(0),(float) Utar(1)));
-
-
- // Projection matrix
- arma::fmat projection;
-
- // Camera matrix
- arma::fmat view = gfx3::lookAt(
- arma::fvec({0, 0, 3}), // Position of the camera
- arma::fvec({0, 0, 0}), // Look at the origin
- arma::fvec({0, 1, 0}) // Head is up
- );
-
-
- // Loading of the shaders
- gfx3::shader_t rtt_shader = gfx3::loadShader(gfx3::RTT_VERTEX_SHADER,
- gfx3::RTT_FRAGMENT_SHADER_LIC);
-
- rtt_shader.setUniform("Resolution", arma::fvec({512, 512}));
-
- gfx3::shader_t background_shader = gfx3::loadShader(gfx3::VERTEX_SHADER_TEXTURED,
- gfx3::FRAGMENT_SHADER_ONE_TEXTURE);
-
- gfx3::shader_t colored_shader = gfx3::loadShader(gfx3::VERTEX_SHADER_COLORED,
- gfx3::FRAGMENT_SHADER_COLORED);
-
- // Creation of the RTTs
- gfx3::render_to_texture_t background_rtt = gfx3::createRTT(rtt_shader, 512, 512,
- arma::fvec({ 0.5f, 0.5f, 0.5f }));
-
- // Background model
- gfx3::model_t background;
-
-
- //--------------- Main loop ---------------
-
- 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);
-
- int previous_win_width = window_size.win_width;
- int previous_win_height = window_size.win_height;
- int previous_fb_width = window_size.fb_width;
- int previous_fb_height = window_size.fb_height;
-
- // Retrieve the new window size
- window_size.win_width = ImGui::GetIO().DisplaySize.x;
- window_size.win_height = ImGui::GetIO().DisplaySize.y;
-
- // Retrieve the new framebuffer size
- glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
-
- // Update the projection matrix
- projection = gfx3::orthographic(
- (float) window_size.fb_width, (float) window_size.fb_height, 0.1f, 10.0f);
-
- // Recreate the background model
- gfx3::destroy(background);
-
- background = gfx3::create_rectangle(background_shader,
- arma::fvec({ 0.5f, 0.0f, 0.0f }),
- (float) window_size.fb_width,
- (float) window_size.fb_height);
-
- background.diffuse_texture = background_rtt.texture();
-
- // Move and rescale the various objects so they stay in the window
- if (!first) {
- float scale = std::min((float) window_size.fb_width / previous_fb_width,
- (float) window_size.fb_height / previous_fb_height);
-
- U0 *= scale;
-
- arma::vec target = gfx3::fb2ui(gfx3::ui2fb({trans.pos.x, trans.pos.y},
- previous_win_width, previous_win_height,
- previous_fb_width, previous_fb_height) * scale,
- window_size.win_width, window_size.win_height,
- window_size.fb_width, window_size.fb_height);
- trans.pos.x = target(0);
- trans.pos.y = target(1);
-
- trans.x.x *= scale;
- trans.x.y *= scale;
- trans.y.x *= scale;
- trans.y.y *= scale;
- }
- }
-
- // Detect if the number of reproductions has changed
- if (nbRepros > U0.n_cols) {
- unsigned int nb = U0.n_cols;
- U0.reshape(nbVarPos, nbRepros);
-
- for (int i = nb; i < nbRepros; ++i) {
- U0.col(i) = arma::vec({ (randu() - 0.5) * window_size.fb_width,
- (randu() - 0.5) * window_size.fb_height });
- }
- }
- else if (nbRepros < U0.n_cols) {
- U0.reshape(nbVarPos, nbRepros);
- }
-
- // Start of rendering
- ImGui_ImplGlfwGL3_NewFrame();
- glViewport(0, 0, window_size.fb_width, window_size.fb_height);
- glClearColor(clear_color.x, clear_color.y, clear_color.z, clear_color.w);
- glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
- // Gaussian UI widget
- ui::begin("Gaussian");
- trans = ui::affineSimple(0, trans);
- Utar = gfx3::ui2fb({ trans.pos.x, trans.pos.y, 0, 0 }, window_size);
- RG = trans2RG(trans, window_size);
- ui::end();
-
- // Recompute LQR
- arma::mat Q = inv(RG * RG.t());
- Q.resize(nbVar, nbVar);
- arma::mat P = lqr::solve_algebraic_Riccati_discrete(A, B, Q, R);
- arma::mat L = inv(B.t() * P * B + R) * B.t() * P * A;
- arma::vec U(nbVar, fill::zeros);
- arma::vec ddu(nbVarPos);
- std::vector<arma::mat> reproductions;
-
- for (int n = 0; n < nbRepros; n++) {
- arma::mat reproduction(nbVar, nbData);
- U = { U0(0, n), U0(1, n), 0, 0 };
-
- for (int t = 0; t < nbData; t++) {
- reproduction.col(t) = U;
- ddu = L * (Utar - U);
- U = A * U + B * ddu;
- }
-
- reproduction.rows(nbVarPos, nbVar-1).zeros();
-
- reproductions.push_back(reproduction);
- }
-
- // RTT rendering
- rtt_shader.setUniform("Time", (float) glfwGetTime());
- rtt_shader.setUniform("Sigma", conv_to<fmat>::from(inv(RG * RG.t())));
- rtt_shader.setUniform("Target", arma::vec(gfx3::ui2shader({ trans.pos.x, trans.pos.y },
- window_size)));
-
- gfx3::draw(background_rtt);
-
- background.diffuse_texture = background_rtt.texture();
-
- // Draw repros
- for (size_t i = 0; i < reproductions.size(); ++i) {
- gfx3::draw_line(colored_shader, arma::fvec({0.33f, 0.97f, 0.33f}),
- reproductions[i], view, projection);
- }
-
- // Draw the gaussian
- pts.rows(0, 1) = arma::mat(RG * pts0).each_col() + Utar.subvec(0, 1);
-
- gfx3::draw_line(colored_shader, arma::fvec({1.0f, 0.33f, 0.33f}), pts, view,
- projection);
-
- // Draw the background
- gfx3::draw(background, view, projection);
-
- // Parameter window
- ImGui::SetNextWindowSize(ImVec2(400, 56));
- ImGui::Begin("Parameters", NULL,
- ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
- ImGuiWindowFlags_NoMove
- );
- ImGui::SliderInt("Nb reproductions", &nbRepros, 1, 10);
- ImGui::End();
-
- // UI rendering
- ImGui::Render();
- ImGui_ImplGlfwGL3_RenderDrawData(ImGui::GetDrawData());
-
- // End of rendering
- glfwSwapBuffers(window);
-
- // Keyboard input
- if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
- break;
- }
-
- // Cleanup
- ImGui_ImplGlfwGL3_Shutdown();
- glfwTerminate();
-
- return 0;
-}
diff --git a/src/demo_proMP01.cpp b/src/demo_proMP01.cpp
index 4ce11e89a62d843f47f1d8127a2b1f4479282bcf..0b0e94c91594f37436b5ba7bbdf10abc7f450c0c 100644
--- a/src/demo_proMP01.cpp
+++ b/src/demo_proMP01.cpp
@@ -55,6 +55,9 @@ struct parameters_t {
float dt; // Time step (without rescaling, large values such
// as 1 has the advantage of creating clusers based
// on position information)
+ float rbf_width;
+ float regularized_pseudoinverse_parameter;
+ float minimum_variance_parameter;
};
@@ -88,13 +91,13 @@ void learn(const matrix_list_t& demos, model_t &model) {
timesteps(0), timesteps(timesteps.n_rows - 1), model.parameters.nb_states
);
- double sigma = 0.01;
-
// Compute basis functions activation
model.H = mat(model.parameters.nb_states, model.parameters.nb_data);
- for (unsigned int i = 0; i < model.parameters.nb_states; ++i)
- model.H.row(i) = mvn::getPDFValue(colvec{ mu(i) }, mat({ sigma }), timesteps.t()).t();
+ for (unsigned int i = 0; i < model.parameters.nb_states; ++i) {
+ model.H.row(i) = mvn::getPDFValue(colvec{ mu(i) }, mat({ model.parameters.rbf_width }),
+ timesteps.t()).t();
+ }
model.H = model.H / repmat(sum(model.H, 0), model.parameters.nb_states, 1);
@@ -110,13 +113,21 @@ void learn(const matrix_list_t& demos, model_t &model) {
}
mat w(model.nb_var * model.parameters.nb_states, demos.size());
- for (size_t i = 0; i < demos.size(); ++i)
- w.col(i) = pinv(model.psi) * (mat) vectorise(demos[i]);
+ for (size_t i = 0; i < demos.size(); ++i) {
+ w.col(i) = solve(model.psi.t() * model.psi +
+ eye(
+ model.nb_var * model.parameters.nb_states,
+ model.nb_var * model.parameters.nb_states
+ ) * model.parameters.regularized_pseudoinverse_parameter,
+ model.psi.t()
+ ) * (mat) vectorise(demos[i]);
+ }
model.mu_w = mean(w, 1);
model.sigma_w = eye(model.nb_var * model.parameters.nb_states,
- model.nb_var * model.parameters.nb_states);
+ model.nb_var * model.parameters.nb_states
+ ) * model.parameters.minimum_variance_parameter;
if (w.n_cols == 1)
model.sigma_w += rowvec(cov(w.t()))[0];
@@ -253,16 +264,13 @@ arma::vec ui2fb(const arma::vec& coords, const gfx2::window_size_t& window_size,
// Colors of the displayed lines and gaussians
//-----------------------------------------------------------------------------
const 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.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 },
});
@@ -323,6 +331,9 @@ struct gui_state_t {
int parameter_nb_states;
int parameter_nb_data;
int parameter_nb_reproductions;
+ float parameter_rbf_width;
+ float parameter_regularized_pseudoinverse_parameter;
+ float parameter_minimum_variance_parameter;
bool display_colored_points;
};
@@ -363,7 +374,7 @@ void draw_demos_viewport(const viewport_t& viewport,
gfx2::draw_line(color, datapoints);
++color_index;
- if (color_index >= demonstrations.size())
+ if (color_index >= COLORS.n_rows)
color_index = 0;
}
}
@@ -473,10 +484,13 @@ int main(int argc, char **argv) {
model_t model;
// Parameters
- model.parameters.nb_states = 10;
- model.parameters.nb_data = 200;
- model.parameters.nb_reproductions = 10;
- model.parameters.dt = 0.01f;
+ model.parameters.nb_states = 10;
+ model.parameters.nb_data = 200;
+ model.parameters.nb_reproductions = 10;
+ model.parameters.dt = 0.01f;
+ model.parameters.rbf_width = 1e-2f;
+ model.parameters.regularized_pseudoinverse_parameter = 3e-2f;
+ model.parameters.minimum_variance_parameter = 1.0f;
// Take 4k screens into account (framebuffer size != window size)
@@ -536,7 +550,10 @@ int main(int argc, char **argv) {
gui_state.parameter_nb_states = model.parameters.nb_states;
gui_state.parameter_nb_data = model.parameters.nb_data;
gui_state.parameter_nb_reproductions = model.parameters.nb_reproductions;
- gui_state.display_colored_points = true;
+ gui_state.parameter_rbf_width = model.parameters.rbf_width;
+ gui_state.parameter_regularized_pseudoinverse_parameter = model.parameters.regularized_pseudoinverse_parameter;
+ gui_state.parameter_minimum_variance_parameter = model.parameters.minimum_variance_parameter;
+ gui_state.display_colored_points = false;
// List of demonstrations and reproductions
@@ -590,6 +607,9 @@ int main(int argc, char **argv) {
model.parameters.nb_data = gui_state.parameter_nb_data;
model.parameters.nb_states = gui_state.parameter_nb_states;
+ model.parameters.rbf_width = gui_state.parameter_rbf_width;
+ model.parameters.regularized_pseudoinverse_parameter = gui_state.parameter_regularized_pseudoinverse_parameter;
+ model.parameters.minimum_variance_parameter = gui_state.parameter_minimum_variance_parameter;
if (!demos.empty()) {
learn(demos, model);
@@ -694,8 +714,9 @@ int main(int argc, char **argv) {
// Window: Parameters
- ImGui::SetNextWindowSize(ImVec2(440, 146));
- ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2, (window_size.win_height - 146) / 2));
+ ImGui::SetNextWindowSize(ImVec2(800, 216));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 800) / 2,
+ (window_size.win_height - 216) / 2));
ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
if (gui_state.is_parameters_dialog_displayed)
@@ -708,6 +729,12 @@ int main(int argc, char **argv) {
ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 2, 20);
ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
ImGui::SliderInt("Nb reproductions", &gui_state.parameter_nb_reproductions, 2, 10);
+ ImGui::SliderFloat("RBF width", &gui_state.parameter_rbf_width, 1e-3f, 3e-2f);
+ ImGui::SliderFloat("Regularized pseudoinverse parameter",
+ &gui_state.parameter_regularized_pseudoinverse_parameter,
+ 3e-2f, 1e-1f);
+ ImGui::SliderFloat("Minimum variance parameter",
+ &gui_state.parameter_minimum_variance_parameter, 1e-3f, 1e0f);
ImGui::Checkbox("Show colored points in reproductions", &gui_state.display_colored_points);
if (ImGui::Button("Close")) {
@@ -734,7 +761,7 @@ int main(int argc, char **argv) {
break;
- if (!gui_state.is_drawing_demonstration) {
+ if (!gui_state.is_drawing_demonstration && !gui_state.is_parameters_dialog_displayed) {
// Left click: start a new demonstration (only if not on the UI and in the
// demonstrations viewport)
if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
@@ -749,7 +776,7 @@ int main(int argc, char **argv) {
current_trajectory.push_back(coords);
}
}
- } else {
+ } else if (gui_state.is_drawing_demonstration) {
double mouse_x, mouse_y;
glfwGetCursorPos(window, &mouse_x, &mouse_y);
diff --git a/src/utils/gfx2.cpp b/src/utils/gfx2.cpp
index b8cfbc302c0f7a6136c853aea16ed47e432e093b..28d014e6b38dc5224541f5a00e4cb95706ea7e63 100644
--- a/src/utils/gfx2.cpp
+++ b/src/utils/gfx2.cpp
@@ -53,6 +53,31 @@ arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
int fb_width, int fb_height) {
arma::vec result = coords;
+ result(0) = coords(0) * (float) fb_width / (float) win_width;
+ result(1) = ((float) win_height - coords(1)) * (float) fb_height / (float) win_height;
+
+ return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
+ arma::vec result = coords;
+
+ result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width;
+
+ result(1) = ((float) window_size.win_height - coords(1)) *
+ (float) window_size.fb_height / (float) window_size.win_height;
+
+ return result;
+}
+
+//-----------------------------------------------
+
+arma::vec ui2fb_centered(const arma::vec& coords, int win_width, int win_height,
+ int fb_width, int fb_height) {
+ arma::vec result = coords;
+
result(0) = (coords(0) - (float) win_width * 0.5f) * (float) fb_width / (float) win_width;
result(1) = ((float) win_height * 0.5f - coords(1)) * (float) fb_height / (float) win_height;
@@ -61,7 +86,7 @@ arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
//-----------------------------------------------
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
+arma::vec ui2fb_centered(const arma::vec& coords, const window_size_t& window_size) {
arma::vec result = coords;
result(0) = (coords(0) - (float) window_size.win_width * 0.5f) *
@@ -79,6 +104,30 @@ arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
int fb_width, int fb_height) {
arma::vec result = coords;
+ result(0) = coords(0) * (float) win_width / (float) fb_width;
+ result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height);
+
+ return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
+ arma::vec result = coords;
+
+ result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width;
+ result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
+ (float) window_size.win_height);
+
+ return result;
+}
+
+//-----------------------------------------------
+
+arma::vec fb2ui_centered(const arma::vec& coords, int win_width, int win_height,
+ int fb_width, int fb_height) {
+ arma::vec result = coords;
+
result(0) = coords(0) * (float) win_width / (float) fb_width + (float) win_width * 0.5f;
result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height * 0.5f);
@@ -87,7 +136,7 @@ arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
//-----------------------------------------------
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
+arma::vec fb2ui_centered(const arma::vec& coords, const window_size_t& window_size) {
arma::vec result = coords;
result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width +
@@ -240,7 +289,6 @@ arma::fmat worldTransforms(const transforms_t* transforms)
return result;
}
-
//-----------------------------------------------
arma::fvec worldPosition(const transforms_t* transforms)
@@ -259,7 +307,6 @@ arma::fvec worldPosition(const transforms_t* transforms)
return transforms->position;
}
-
//-----------------------------------------------
arma::fmat worldRotation(const transforms_t* transforms)
@@ -274,6 +321,46 @@ arma::fmat worldRotation(const transforms_t* transforms)
}
+/********************************** TEXTURES *********************************/
+
+texture_t create_texture(int width, int height, GLenum format, GLenum type)
+{
+ texture_t texture = { 0 };
+
+ texture.width = (GLuint) width;
+ texture.height = (GLuint) height;
+ texture.format = format;
+ texture.type = type;
+
+ glGenTextures(1, &texture.id);
+
+ glBindTexture(GL_TEXTURE_2D, texture.id);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+ glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+
+ if (texture.type == GL_FLOAT)
+ texture.pixels_f = new float[width * height * 3];
+ else
+ texture.pixels_b = new unsigned char[width * height * 3];
+
+ return texture;
+}
+
+//-----------------------------------------------
+
+void destroy(texture_t &texture)
+{
+ if (texture.type == GL_FLOAT)
+ delete[] texture.pixels_f;
+ else
+ delete[] texture.pixels_b;
+
+ glDeleteTextures(1, &texture.id);
+
+ texture = {0};
+}
+
+
/*********************************** MESHES **********************************/
model_t create_rectangle(const arma::fvec& color, float width, float height,
@@ -332,6 +419,21 @@ model_t create_rectangle(const arma::fvec& color, float width, float height,
//-----------------------------------------------
+model_t create_rectangle(const texture_t& texture, float width, float height,
+ const arma::fvec& position, const arma::fmat& rotation,
+ transforms_t* parent_transforms)
+{
+ model_t model = create_rectangle(arma::fvec({1.0f, 1.0f, 1.0f, 1.0f}),
+ width, height, position, rotation,
+ parent_transforms);
+
+ model.texture = texture;
+
+ return model;
+}
+
+//-----------------------------------------------
+
model_t create_square(const arma::fvec& color, float size, const arma::fvec& position,
const arma::fmat& rotation, transforms_t* parent_transforms)
{
@@ -579,32 +681,7 @@ model_t create_gaussian_background(const arma::fvec& color, const arma::vec& mu,
const arma::fvec& position, const arma::fmat& rotation,
transforms_t* parent_transforms)
{
- const int NB_POINTS = 60;
-
- arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
- arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
- );
-
- arma::vec eigval(2);
- arma::mat eigvec(2, 2);
- eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
-
- arma::mat R = eigvec * diagmat(sqrt(eigval));
-
- arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
-
- arma::mat vertices(2, NB_POINTS * 3);
-
- for (int i = 0; i < NB_POINTS - 1; ++i)
- {
- vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
- vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
- vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
- }
-
- vertices(arma::span::all, (NB_POINTS - 1) * 3) = mu(arma::span(0, 1));
- vertices(arma::span::all, (NB_POINTS - 1) * 3 + 1) = pts(arma::span::all, 0);
- vertices(arma::span::all, (NB_POINTS - 1) * 3 + 2) = pts(arma::span::all, NB_POINTS - 1);
+ arma::mat vertices = get_gaussian_background_vertices(mu, sigma, 60);
model_t model = create_mesh(color, vertices, position, rotation, parent_transforms);
@@ -620,26 +697,14 @@ model_t create_gaussian_border(const arma::fvec& color, const arma::vec& mu,
const arma::fvec& position, const arma::fmat& rotation,
transforms_t* parent_transforms)
{
- const int NB_POINTS = 60;
-
- arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
- arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
- );
-
- arma::vec eigval(2);
- arma::mat eigvec(2, 2);
- eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
-
- arma::mat R = eigvec * diagmat(sqrt(eigval));
-
- arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
+ arma::mat pts = get_gaussian_border_vertices(mu, sigma, 60, true);
return create_line(color, pts, position, rotation, parent_transforms);
}
//-----------------------------------------------
-void destroy(const model_t& model)
+void destroy(model_t &model)
{
if (model.vertex_buffer)
delete[] model.vertex_buffer;
@@ -649,6 +714,8 @@ void destroy(const model_t& model)
if (model.uv_buffer)
delete[] model.uv_buffer;
+
+ model = {0};
}
@@ -667,7 +734,9 @@ bool draw(const model_t& model, const light_list_t& lights)
glMaterialfv(GL_FRONT, GL_SPECULAR, model.specular_color.memptr());
glMaterialf(GL_FRONT, GL_SHININESS, model.specular_power);
} else {
- if (model.diffuse_color.n_rows == 3)
+ if (model.texture.width > 0)
+ glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
+ else if (model.diffuse_color.n_rows == 3)
glColor3fv(model.diffuse_color.memptr());
else
glColor4fv(model.diffuse_color.memptr());
@@ -699,6 +768,26 @@ bool draw(const model_t& model, const light_list_t& lights)
glTexCoordPointer(2, GL_FLOAT, 0, model.uv_buffer);
}
+ // Texturing
+ if (model.texture.width > 0) {
+ glEnable(GL_TEXTURE_2D);
+ glBindTexture(GL_TEXTURE_2D, model.texture.id);
+
+ if (model.texture.type == GL_FLOAT) {
+ glTexImage2D(GL_TEXTURE_2D, 0, model.texture.format,
+ model.texture.width, model.texture.height,
+ 0, model.texture.format, GL_FLOAT,
+ model.texture.pixels_f
+ );
+ } else {
+ glTexImage2D(GL_TEXTURE_2D, 0, model.texture.format,
+ model.texture.width, model.texture.height,
+ 0, model.texture.format, GL_UNSIGNED_BYTE,
+ model.texture.pixels_b
+ );
+ }
+ }
+
// Apply the model matrix
arma::fmat model_matrix = worldTransforms(&model.transforms);
@@ -751,6 +840,9 @@ bool draw(const model_t& model, const light_list_t& lights)
glDisableClientState(GL_NORMAL_ARRAY);
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+ if (model.texture.width > 0)
+ glDisable(GL_TEXTURE_2D);
+
return true;
}
@@ -770,6 +862,20 @@ bool draw_rectangle(const arma::fvec& color, float width, float height,
//-----------------------------------------------
+bool draw_rectangle(const texture_t& texture, float width, float height,
+ const arma::fvec& position, const arma::fmat& rotation)
+{
+ model_t rect = create_rectangle(texture, width, height, position, rotation);
+
+ bool result = draw(rect);
+
+ destroy(rect);
+
+ return result;
+}
+
+//-----------------------------------------------
+
bool draw_line(const arma::fvec& color, const arma::mat& points,
const arma::fvec& position, const arma::fmat& rotation)
{
@@ -817,17 +923,7 @@ bool draw_gaussian(const arma::fvec& color, const arma::vec& mu, const arma::mat
{
const int NB_POINTS = 60;
- arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
- arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
- );
-
- arma::vec eigval(2);
- arma::mat eigvec(2, 2);
- eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
-
- arma::mat R = eigvec * diagmat(sqrt(eigval));
-
- arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
+ arma::mat pts = get_gaussian_border_vertices(mu, sigma, NB_POINTS, true);
arma::mat vertices(2, NB_POINTS * 3);
@@ -939,31 +1035,38 @@ bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
int nb_points)
{
- arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points)),
- arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points))
- );
+ arma::mat pts = get_gaussian_border_vertices(mu, sigma, nb_points, true);
- arma::vec eigval(2);
- arma::mat eigvec(2, 2);
- eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+ arma::mat vertices(2, nb_points * 3);
- arma::mat R = eigvec * diagmat(sqrt(eigval));
+ // We need to ensure that the vertices will be in a counter-clockwise order
+ arma::vec v1 = pts(arma::span::all, 0) - mu(arma::span(0, 1));
+ arma::vec v2 = pts(arma::span::all, 1) - mu(arma::span(0, 1));
- arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, nb_points);
+ if (atan2(v1(1), v1(0)) - atan2(v2(1), v2(0)) > 0.0) {
+ for (int i = 0; i < nb_points - 1; ++i)
+ {
+ vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
+ vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
+ }
- arma::mat vertices(2, nb_points * 3);
+ vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, 0);
+ vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, nb_points - 1);
+ } else {
+ for (int i = 0; i < nb_points - 1; ++i)
+ {
+ vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i);
+ vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i + 1);
+ }
- for (int i = 0; i < nb_points - 1; ++i)
- {
- vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
- vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
- vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
+ vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, nb_points - 1);
+ vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, 0);
}
- vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
- vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, 0);
- vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, nb_points - 1);
-
return vertices;
}
diff --git a/src/utils/gfx3.cpp b/src/utils/gfx3.cpp
deleted file mode 100644
index f9c12835b359dcddeff3343e7a8a1d1dd337a4c0..0000000000000000000000000000000000000000
--- a/src/utils/gfx3.cpp
+++ /dev/null
@@ -1,1641 +0,0 @@
-/*
- * gfx3.cpp
- *
- * Rendering utility structures and functions based on OpenGL 3.3+
- *
- * Authors: Philip Abbet
- */
-
-#include <gfx3.h>
-
-namespace gfx3 {
-
-
-/****************************** UTILITY FUNCTIONS ****************************/
-
-void init()
-{
- glewExperimental = GL_TRUE;
- glewInit();
-}
-
-//-----------------------------------------------
-
-double deg2rad(double deg)
-{
- return deg / 360.0 * (2.0 * M_PI);
-}
-
-//-----------------------------------------------
-
-double sin_deg(double deg)
-{
- return sin(deg2rad(deg));
-}
-
-//-----------------------------------------------
-
-double cos_deg(double deg)
-{
- return cos(deg2rad(deg));
-}
-
-//-----------------------------------------------
-
-bool is_close(float a, float b, float epsilon)
-{
- return fabs(a - b) < epsilon;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height) {
- arma::vec result = coords;
-
- result(0) = (coords(0) - (float) win_width * 0.5f) * (float) fb_width / (float) win_width;
- result(1) = ((float) win_height * 0.5f - coords(1)) * (float) fb_height / (float) win_height;
-
- return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2fb(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
-
- result(0) = (coords(0) - (float) window_size.win_width * 0.5f) *
- (float) window_size.fb_width / (float) window_size.win_width;
-
- result(1) = ((float) window_size.win_height * 0.5f - coords(1)) *
- (float) window_size.fb_height / (float) window_size.win_height;
-
- return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height) {
- arma::vec result = coords;
-
- result(0) = coords(0) * (float) win_width / (float) fb_width + (float) win_width * 0.5f;
- result(1) = -(coords(1) * (float) win_height / (float) fb_height - (float) win_height * 0.5f);
-
- return result;
-}
-
-//-----------------------------------------------
-
-arma::vec fb2ui(const arma::vec& coords, const window_size_t& window_size) {
- arma::vec result = coords;
-
- result(0) = coords(0) * (float) window_size.win_width / (float) window_size.fb_width +
- (float) window_size.win_width * 0.5f;
- result(1) = -(coords(1) * (float) window_size.win_height / (float) window_size.fb_height -
- (float) window_size.win_height * 0.5f);
-
- return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2shader(const arma::vec& coords, int win_width, int win_height,
- int fb_width, int fb_height, float sh_left, float sh_top,
- float sh_right, float sh_bottom) {
- arma::vec result = ui2fb(coords, win_width, win_height, fb_width, fb_height);
-
- result(0) = result(0) * (sh_right - sh_left) / (float) fb_width + (1.0f + sh_left);
- result(1) = result(1) * (sh_top - sh_bottom) / (float) fb_height + (1.0f + sh_bottom);
-
- return result;
-}
-
-//-----------------------------------------------
-
-arma::vec ui2shader(const arma::vec& coords, const window_size_t& window_size,
- float sh_left, float sh_top, float sh_right, float sh_bottom) {
- arma::vec result = ui2fb(coords, window_size);
-
- result(0) = result(0) * (sh_right - sh_left) / (float) window_size.fb_width + (1.0f + sh_left);
- result(1) = result(1) * (sh_top - sh_bottom) / (float) window_size.fb_height + (1.0f + sh_bottom);
-
- return result;
-}
-
-
-/************************* PROJECTION & VIEW MATRICES ************************/
-
-arma::fmat perspective(float fovy, float aspect, float zNear, float zFar)
-{
- const float top = zNear * tan(fovy / 2.0f);
- const float bottom = -top;
- const float right = top * aspect;
- const float left = -right;
-
- arma::fmat projection = arma::zeros<arma::fmat>(4,4);
-
- projection(0, 0) = 2.0f * zNear / (right - left);
- projection(0, 2) = (right + left) / (right - left);
- projection(1, 1) = 2.0f * zNear / (top - bottom);
- projection(1, 2) = (top + bottom) / (top - bottom);
- projection(2, 2) = -(zFar + zNear) / (zFar - zNear);
- projection(2, 3) = -(2.0f * zFar * zNear) / (zFar - zNear);
- projection(3, 2) = -1.0f;
-
- return projection;
-}
-
-//-----------------------------------------------
-
-arma::fmat orthographic(float width, float height, float zNear, float zFar)
-{
- const float top = height / 2.0f;
- const float bottom = -top;
- const float right = width / 2.0f;
- const float left = -right;
-
- arma::fmat projection = arma::zeros<arma::fmat>(4,4);
-
- projection(0, 0) = 2.0f / (right - left);
- projection(0, 3) = -(right + left) / (right - left);
- projection(1, 1) = 2.0f / (top - bottom);
- projection(1, 3) = -(top + bottom) / (top - bottom);
- projection(2, 2) = -2.0f / (zFar - zNear);
- projection(2, 3) = -(zFar + zNear) / (zFar - zNear);
- projection(3, 3) = 1.0f;
-
- return projection;
-}
-
-//-----------------------------------------------
-
-arma::fmat lookAt(const arma::fvec& position, const arma::fvec& target,
- const arma::fvec& up)
-{
- const arma::fvec f(arma::normalise(target - position));
- const arma::fvec s(arma::normalise(arma::cross(f, up)));
- const arma::fvec u(arma::cross(s, f));
-
- arma::fmat result = arma::zeros<arma::fmat>(4,4);
-
- result(0, 0) = s(0);
- result(0, 1) = s(1);
- result(0, 2) = s(2);
- result(1, 0) = u(0);
- result(1, 1) = u(1);
- result(1, 2) = u(2);
- result(2, 0) =-f(0);
- result(2, 1) =-f(1);
- result(2, 2) =-f(2);
- result(0, 3) =-arma::dot(s, position);
- result(1, 3) =-arma::dot(u, position);
- result(2, 3) = arma::dot(f, position);
- result(3, 3) = 1.0f;
-
- return result;
-}
-
-
-/****************************** TRANSFORMATIONS ******************************/
-
-arma::fmat rotate(const arma::fvec& axis, float angle)
-{
- float rcos = cos(angle);
- float rsin = sin(angle);
-
- arma::fmat matrix = arma::zeros<arma::fmat>(4, 4);
-
- matrix(0, 0) = rcos + axis(0) * axis(0) * (1.0f - rcos);
- matrix(1, 0) = axis(2) * rsin + axis(1) * axis(0) * (1.0f - rcos);
- matrix(2, 0) = -axis(1) * rsin + axis(2) * axis(0) * (1.0f - rcos);
- matrix(0, 1) = -axis(2) * rsin + axis(0) * axis(1) * (1.0f - rcos);
- matrix(1, 1) = rcos + axis(1) * axis(1) * (1.0f - rcos);
- matrix(2, 1) = axis(0) * rsin + axis(2) * axis(1) * (1.0f - rcos);
- matrix(0, 2) = axis(1) * rsin + axis(0) * axis(2) * (1.0f - rcos);
- matrix(1, 2) = -axis(0) * rsin + axis(1) * axis(2) * (1.0f - rcos);
- matrix(2, 2) = rcos + axis(2) * axis(2) * (1.0f - rcos);
- matrix(3, 3) = 1.0f;
-
- return matrix;
-}
-
-//-----------------------------------------------
-
-arma::fmat rotation(const arma::fvec& from, const arma::fvec& to)
-{
- const float dot = arma::dot(from, to);
- const arma::fvec cross = arma::cross(from, to);
- const float norm = arma::norm(cross);
-
- arma::fmat g({
- { dot, -norm, 0.0f },
- { norm, dot, 0.0f },
- { 0.0f, 0.0f, 1.0f },
- });
-
- arma::fmat fi(3, 3);
- fi.rows(0, 0) = from.t();
- fi.rows(1, 1) = arma::normalise(to - dot * from).t();
- fi.rows(2, 2) = arma::cross(to, from).t();
-
- arma::fmat result = arma::eye<arma::fmat>(4, 4);
-
- arma::fmat u;
- if (arma::inv(u, fi))
- {
- u = u * g * fi;
- result.submat(0, 0, 2, 2) = u;
- }
-
- return result;
-}
-
-//-----------------------------------------------
-
-void rigid_transform_3D(const arma::fmat& A, const arma::fmat& B,
- arma::fmat &rotation, arma::fvec &translation) {
-
- arma::fvec centroidsA = arma::mean(A, 1);
- arma::fvec centroidsB = arma::mean(B, 1);
-
- int n = A.n_cols;
-
- arma::fmat H = (A - repmat(centroidsA, 1, n)) * (B - repmat(centroidsB, 1, n)).t();
-
- arma::fmat U, V;
- arma::fvec s;
- arma::svd(U, s, V, H);
-
- rotation = V * U.t();
-
- if (arma::det(rotation) < 0.0f)
- rotation.col(2) *= -1.0f;
-
- translation = -rotation * centroidsA + centroidsB;
-}
-
-//-----------------------------------------------
-
-arma::fmat worldTransforms(const transforms_t* transforms)
-{
- arma::fmat result = arma::eye<arma::fmat>(4, 4);
- result(0, 3, arma::size(3, 1)) = worldPosition(transforms);
-
- result = result * worldRotation(transforms);
-
- return result;
-}
-
-
-//-----------------------------------------------
-
-arma::fvec worldPosition(const transforms_t* transforms)
-{
- if (transforms->parent)
- {
- arma::fvec position(4);
- position.rows(0, 2) = transforms->position;
- position(3) = 1.0f;
-
- position = worldRotation(transforms->parent) * position;
-
- return worldPosition(transforms->parent) + position.rows(0, 2);
- }
-
- return transforms->position;
-}
-
-
-//-----------------------------------------------
-
-arma::fmat worldRotation(const transforms_t* transforms)
-{
- if (transforms->parent)
- {
- arma::fmat result = worldRotation(transforms->parent) * transforms->rotation;
- return arma::normalise(result);
- }
-
- return transforms->rotation;
-}
-
-
-/********************************** SHADERS **********************************/
-
-void shader_t::setUniform(const std::string& name, const arma::fmat& value)
-{
- auto iter = fmat_uniforms.find(name);
-
- if (iter == fmat_uniforms.end())
- {
- shader_fmat_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = value;
- fmat_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = value;
- }
-}
-
-//-----------------------------------------------
-
-void shader_t::setUniform(const std::string& name, const arma::mat& value)
-{
- auto iter = fmat_uniforms.find(name);
-
- if (iter == fmat_uniforms.end())
- {
- shader_fmat_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = arma::conv_to<arma::fmat>::from(value);
- fmat_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = arma::conv_to<arma::fmat>::from(value);
- }
-}
-
-//-----------------------------------------------
-
-void shader_t::setUniform(const std::string& name, const arma::fvec& value)
-{
- auto iter = fvec_uniforms.find(name);
-
- if (iter == fvec_uniforms.end())
- {
- shader_fvec_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = value;
- fvec_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = value;
- }
-}
-
-//-----------------------------------------------
-
-void shader_t::setUniform(const std::string& name, const arma::vec& value)
-{
- auto iter = fvec_uniforms.find(name);
-
- if (iter == fvec_uniforms.end())
- {
- shader_fvec_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = arma::conv_to<arma::fvec>::from(value);
- fvec_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = arma::conv_to<arma::fvec>::from(value);
- }
-}
-
-//-----------------------------------------------
-
-void shader_t::setUniform(const std::string& name, float value)
-{
- auto iter = float_uniforms.find(name);
-
- if (iter == float_uniforms.end())
- {
- shader_float_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = value;
- float_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = value;
- }
-}
-
-//-----------------------------------------------
-
-void shader_t::setUniform(const std::string& name, bool value)
-{
- auto iter = bool_uniforms.find(name);
-
- if (iter == bool_uniforms.end())
- {
- shader_bool_uniform_t entry;
- entry.handle = glGetUniformLocation(this->id, name.c_str());
- entry.value = value;
- bool_uniforms[name] = entry;
- }
- else
- {
- iter->second.value = value;
- }
-}
-
-//-----------------------------------------------
-
-GLuint compileShader(GLenum shader_type, const char* shader_source)
-{
- GLuint id = glCreateShader(shader_type);
- GLint compiled;
-
- glShaderSource(id, 1, (const GLchar**) &shader_source, NULL);
- glCompileShader(id);
- glGetShaderiv(id, GL_COMPILE_STATUS, &compiled);
-
- if (!compiled)
- {
- char errors[1024];
- GLsizei len;
-
- glGetShaderInfoLog(id, 1024, &len, errors);
- printf("shader errors: %s\n", errors);
- return 0;
- }
-
- return id;
-}
-
-//-----------------------------------------------
-
-GLuint linkShader(GLuint vertex_shader, GLuint fragment_shader)
-{
- GLuint id = glCreateProgram();
-
- glAttachShader(id, vertex_shader);
- glAttachShader(id, fragment_shader);
- glLinkProgram(id);
-
- GLint linked;
-
- glGetProgramiv(id, GL_LINK_STATUS, &linked);
- if (!linked)
- {
- char errors[1024];
- GLsizei len;
-
- glGetProgramInfoLog(id, 1024, &len, errors);
- printf("GLSL Shader linker error:%s\n",errors);
- return 0;
- }
-
- return id;
-}
-
-//-----------------------------------------------
-
-shader_t loadShader(const std::string& vertex_shader,
- const std::string& fragment_shader,
- const std::string& version)
-{
- shader_t shader = { 0 };
- shader.id = 0;
-
- // Compile the vertex shader
- GLuint vs_id = compileShader(
- GL_VERTEX_SHADER, (std::string("#version ") + version + "\n\n" + vertex_shader).c_str());
-
- if (vs_id == 0)
- return shader;
-
- // Compile the fragment shader
- GLuint fg_id = compileShader(
- GL_FRAGMENT_SHADER, (std::string("#version ") + version + "\n\n" + fragment_shader).c_str());
-
- if (fg_id == 0)
- return shader;
-
- // Link the shaders into a GLSL program
- shader.id = linkShader(vs_id, fg_id);
-
- // Transformation matrices-related uniforms
- shader.model_matrix_handle = glGetUniformLocation(shader.id, "ModelMatrix");
- shader.view_matrix_handle = glGetUniformLocation(shader.id, "ViewMatrix");
- shader.projection_matrix_handle = glGetUniformLocation(shader.id, "ProjectionMatrix");
-
- // Material-related uniforms
- shader.ambiant_color_handle = glGetUniformLocation(shader.id, "AmbiantColor");
- shader.diffuse_color_handle = glGetUniformLocation(shader.id, "DiffuseColor");
- shader.specular_color_handle = glGetUniformLocation(shader.id, "SpecularColor");
- shader.specular_power_handle = glGetUniformLocation(shader.id, "SpecularPower");
-
- // Texture-related uniforms
- shader.diffuse_texture_handle = glGetUniformLocation(shader.id, "DiffuseTexture");
-
- // Light-related uniforms
- shader.light_position_handle = glGetUniformLocation(shader.id, "LightPosition");
- shader.light_color_handle = glGetUniformLocation(shader.id, "LightColor");
- shader.light_power_handle = glGetUniformLocation(shader.id, "LightPower");
-
- // Determine if lightning is used by the shaders
- shader.use_lightning = (shader.light_position_handle != -1);
-
- // Backbuffer
- shader.backbuffer_handle = glGetUniformLocation(shader.id, "BackBuffer");
-
- return shader;
-}
-
-//-----------------------------------------------
-
-void sendApplicationUniforms(const shader_t* shader)
-{
- // Send the application-specific uniforms to the shader
- for (auto iter = shader->fmat_uniforms.begin(), iterEnd = shader->fmat_uniforms.end();
- iter != iterEnd; ++iter)
- {
- if (iter->second.value.n_rows == 4)
- glUniformMatrix4fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
- else if (iter->second.value.n_rows == 2)
- glUniformMatrix2fv(iter->second.handle, 1, GL_FALSE, iter->second.value.memptr());
- }
-
- for (auto iter = shader->fvec_uniforms.begin(), iterEnd = shader->fvec_uniforms.end();
- iter != iterEnd; ++iter)
- {
- if (iter->second.value.n_rows == 4)
- glUniform4fv(iter->second.handle, 1, iter->second.value.memptr());
- else if (iter->second.value.n_rows == 3)
- glUniform3fv(iter->second.handle, 1, iter->second.value.memptr());
- else if (iter->second.value.n_rows == 2)
- glUniform2fv(iter->second.handle, 1, iter->second.value.memptr());
- }
-
- for (auto iter = shader->float_uniforms.begin(), iterEnd = shader->float_uniforms.end();
- iter != iterEnd; ++iter)
- {
- glUniform1f(iter->second.handle, iter->second.value);
- }
-
- for (auto iter = shader->bool_uniforms.begin(), iterEnd = shader->bool_uniforms.end();
- iter != iterEnd; ++iter)
- {
- glUniform1i(iter->second.handle, iter->second.value);
- }
-}
-
-//-----------------------------------------------
-
-const char* VERTEX_SHADER_ONE_LIGHT = STRINGIFY(
- // Input vertex data
- layout(location = 0) in vec3 vertex_position;
- layout(location = 1) in vec3 normal;
-
- // Values that stay constant for the whole mesh
- uniform mat4 ModelMatrix;
- uniform mat4 ViewMatrix;
- uniform mat4 ProjectionMatrix;
- uniform vec3 LightPosition;
-
- // Output data ; will be interpolated for each fragment
- out vec3 position_worldspace;
- out vec3 eye_direction_cameraspace;
- out vec3 light_direction_cameraspace;
- out vec3 normal_cameraspace;
-
- void main() {
- // Position of the vertex, in clip space
- gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
-
- // Position of the vertex, in worldspace
- position_worldspace = (ModelMatrix * vec4(vertex_position, 1)).xyz;
-
- // Vector that goes from the vertex to the camera, in camera space.
- // In camera space, the camera is at the origin (0,0,0).
- vec3 vertex_position_cameraspace = (ViewMatrix * vec4(position_worldspace, 1)).xyz;
- eye_direction_cameraspace = vec3(0,0,0) - vertex_position_cameraspace;
-
- // Vector that goes from the vertex to the light, in camera space
- vec3 light_position_cameraspace = (ViewMatrix * vec4(LightPosition, 1)).xyz;
- light_direction_cameraspace = light_position_cameraspace + eye_direction_cameraspace;
-
- // Normal of the the vertex, in camera space (Only correct if ModelMatrix does
- // not scale the model)
- normal_cameraspace = (ViewMatrix * ModelMatrix * vec4(normal, 0)).xyz;
- }
-);
-
-//-----------------------------------------------
-
-const char* FRAGMENT_SHADER_ONE_LIGHT = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform vec3 AmbiantColor;
- uniform vec3 DiffuseColor;
- uniform vec3 SpecularColor;
- uniform float SpecularPower;
- uniform vec3 LightPosition;
- uniform vec3 LightColor;
- uniform float LightPower;
-
- // Interpolated values from the vertex shaders
- in vec3 position_worldspace;
- in vec3 eye_direction_cameraspace;
- in vec3 light_direction_cameraspace;
- in vec3 normal_cameraspace;
-
- // Output data
- out vec3 color;
-
- void main() {
- // Normal of the computed fragment, in camera space
- vec3 n = normalize(normal_cameraspace);
-
- // Direction of the light (from the fragment to the light)
- vec3 l = normalize(light_direction_cameraspace);
-
- // Eye vector (towards the camera)
- vec3 E = normalize(eye_direction_cameraspace);
-
- // Direction in which the triangle reflects the light
- vec3 R = reflect(-l, n);
-
- // Cosine of the angle between the normal and the light direction,
- // clamped above 0
- // - light is at the vertical of the triangle -> 1
- // - light is perpendicular to the triangle -> 0
- // - light is behind the triangle -> 0
- float cos_theta = clamp(dot(n, l), 0, 1);
-
- // Cosine of the angle between the Eye vector and the Reflect vector,
- // clamped to 0
- // - Looking into the reflection -> 1
- // - Looking elsewhere -> < 1
- float cos_alpha = clamp(dot(E, R), 0, 1);
-
- // Distance to the light
- float distance = length(LightPosition - position_worldspace);
-
- // Computation of the color of the fragment
- color = AmbiantColor +
- DiffuseColor * LightColor * LightPower * cos_theta / (distance * distance) +
- SpecularColor * LightColor * LightPower * pow(cos_alpha, SpecularPower) / (distance * distance);
- }
-);
-
-//-----------------------------------------------
-
-const char* VERTEX_SHADER_COLORED = STRINGIFY(
- // Input vertex data
- layout(location = 0) in vec3 vertex_position;
-
- // Values that stay constant for the whole mesh
- uniform mat4 ModelMatrix;
- uniform mat4 ViewMatrix;
- uniform mat4 ProjectionMatrix;
-
- void main() {
- // Position of the vertex, in clip space
- gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
- }
-);
-
-//-----------------------------------------------
-
-const char* FRAGMENT_SHADER_COLORED = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform vec3 DiffuseColor;
-
- // Output data
- out vec3 color;
-
- void main() {
- color = DiffuseColor;
- }
-);
-
-//-----------------------------------------------
-
-const char* VERTEX_SHADER_TEXTURED = STRINGIFY(
- // Input vertex data
- layout(location = 0) in vec3 vertex_position;
- layout(location = 2) in vec2 vertex_UV;
-
- // Values that stay constant for the whole mesh
- uniform mat4 ModelMatrix;
- uniform mat4 ViewMatrix;
- uniform mat4 ProjectionMatrix;
-
- // Output data ; will be interpolated for each fragment
- out vec2 UV;
-
- void main() {
- // Position of the vertex, in clip space
- gl_Position = ProjectionMatrix * ViewMatrix * ModelMatrix * vec4(vertex_position, 1);
-
- UV = vertex_UV;
- }
-);
-
-//-----------------------------------------------
-
-const char* FRAGMENT_SHADER_ONE_TEXTURE = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform sampler2D DiffuseTexture;
-
- // Input data
- in vec2 UV;
-
- // Output data
- out vec3 color;
-
- void main() {
- color = texture(DiffuseTexture, UV).rgb;
- }
-);
-
-//-----------------------------------------------
-
-char const* FRAGMENT_SHADER_GAUSSIAN = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform vec2 Mu;
- uniform mat2 Sigma;
- uniform vec4 GaussianColor;
-
- // Input data
- in vec2 UV;
-
- // Output data
- out vec4 color;
-
- void main() {
- // vec2 UV2 = vec2(UV.x, 1.0 - UV.y);
- // vec2 e = UV2 - Mu;
- vec2 e = UV - Mu;
-
- float p = clamp(exp(-(Sigma[0][0] * e.x * e.x + 2 * Sigma[0][1] * e.x * e.y +
- Sigma[1][1] * e.y * e.y)), 0, 0.5f);
-
- color = vec4(GaussianColor.x, GaussianColor.y, GaussianColor.z,
- clamp(GaussianColor.a * 2 * p, 0.0, 1.0));
- }
-);
-
-//-----------------------------------------------
-
-const char* RTT_VERTEX_SHADER = STRINGIFY(
- // Input vertex data
- layout(location = 0) in vec3 vertex_position;
-
- // Output data ; will be interpolated for each fragment
- out vec2 coords;
-
- void main() {
- gl_Position = vec4(vertex_position, 1);
- coords = vertex_position.xy;
- }
-);
-
-//-----------------------------------------------
-
-char const* RTT_FRAGMENT_SHADER_GAUSSIAN = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform vec2 Scale;
- uniform vec2 Mu;
- uniform mat2 Sigma;
- uniform vec3 GaussianColor;
- uniform vec3 BackgroundColor;
-
- // Input data
- in vec2 coords;
-
- // Output data
- layout(location = 0) out vec3 color;
-
- void main() {
- vec2 e = coords * Scale - Mu;
- float a = clamp(exp(-(Sigma[0][0] * e.x * e.x + 2 * Sigma[0][1] * e.x * e.y +
- Sigma[1][1] * e.y * e.y)), 0, 1.0f);
-
- color = GaussianColor * a + (1.0f - a) * BackgroundColor;
- }
-);
-
-//-----------------------------------------------
-
-const char* RTT_FRAGMENT_SHADER_LIC = STRINGIFY(
- // Values that stay constant for the whole mesh
- uniform vec2 Resolution;
- uniform float Time;
- uniform vec2 Target;
- uniform mat2 Sigma;
- uniform sampler2D BackBuffer;
-
- // Input data
- in vec2 coords;
-
- // Output data
- layout(location = 0) out vec3 color;
-
- // Constants
- const int Length = 3;
- const int nbPass = 5;
-
- void main() {
- vec2 uvUnit = 1.0 / Resolution.xy;
- vec2 uv = (coords.xy + 1.0) * 0.5;
-
- vec2 dTarget = -Sigma * (coords - Target);
- dTarget = dTarget / length(dTarget);
-
- color = vec3(0);
-
- // Random noise
- color += vec3(fract(sin(dot(uv.xy, vec2(12.9898, 78.233) * sin(Time))) * 43758.5453));
-
- for (int n = 0; n < nbPass; n++) {
- for (int i = 0; i < Length; i++) {
- color += texture(BackBuffer, uv - dTarget * uvUnit * float(i + 4)).rgb;
- }
- }
- color /= 1.0 * float(Length) * float(nbPass) + 1.0;
- }
-);
-
-
-/***************************** RENDER-TO-TEXTURE *****************************/
-
-render_to_texture_t createRTT(const shader_t& shader, unsigned int width,
- unsigned int height, const arma::fvec& color)
-{
- render_to_texture_t rtt = { 0 };
- rtt.width = width;
- rtt.height = height;
-
- rtt.shader = &shader;
-
- rtt.current_buffer = 0;
- rtt.nb_buffers = (shader.backbuffer_handle >= 0 ? 2 : 1);
-
- // Preparation of the default color buffer
- unsigned char* pixels = new unsigned char[width * height * 3];
- unsigned char* pDst = pixels;
- for (unsigned int y = 0; y < height; ++y)
- {
- for (unsigned int x = 0; x < width; ++x)
- {
- pDst[0] = (unsigned char) (color(0) * 255);
- pDst[1] = (unsigned char) (color(1) * 255);
- pDst[2] = (unsigned char) (color(2) * 255);
-
- pDst += 3;
- }
- }
-
- for (unsigned int i = 0; i < rtt.nb_buffers; ++i) {
- // Creates the framebuffer
- glGenFramebuffers(1, &rtt.buffers[i].framebuffer);
- glBindFramebuffer(GL_FRAMEBUFFER, rtt.buffers[i].framebuffer);
-
- // Creates the texture we're going to render to
- glActiveTexture(GL_TEXTURE0);
- glGenTextures(1, &rtt.buffers[i].texture);
- glBindTexture(GL_TEXTURE_2D, rtt.buffers[i].texture);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, pixels);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-
- // Link the two
- glFramebufferTexture(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, rtt.buffers[i].texture, 0);
- }
-
- delete[] pixels;
-
- // The rectangular mesh on which the shader is applied
- rtt.nb_vertices = 6;
-
- const GLfloat vertex_buffer_data[] = {
- -1.0f, -1.0f, 0.0f,
- 1.0f, -1.0f, 0.0f,
- -1.0f, 1.0f, 0.0f,
- -1.0f, 1.0f, 0.0f,
- 1.0f, -1.0f, 0.0f,
- 1.0f, 1.0f, 0.0f,
- };
-
- // Vertex buffer
- glGenBuffers(1, &rtt.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, rtt.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, rtt.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- glBindFramebuffer(GL_FRAMEBUFFER, 0);
-
- return rtt;
-}
-
-//-----------------------------------------------
-
-bool draw(render_to_texture_t &rtt)
-{
- // Various checks
- if (rtt.shader->id == 0)
- return false;
-
- // Retrieve the current viewport
- GLint previous_viewport[4];
- glGetIntegerv(GL_VIEWPORT, previous_viewport);
-
- // Activate the GLSL program
- glUseProgram(rtt.shader->id);
-
- // Backbuffer management
- if (rtt.shader->backbuffer_handle >= 0) {
- unsigned int previous_buffer = rtt.current_buffer;
-
- rtt.current_buffer++;
- if (rtt.current_buffer >= rtt.nb_buffers)
- rtt.current_buffer = 0;
-
- glActiveTexture(GL_TEXTURE0);
- glBindTexture(GL_TEXTURE_2D, rtt.buffers[previous_buffer].texture);
- glUniform1i(rtt.shader->backbuffer_handle, 0);
- }
-
- glBindFramebuffer(GL_FRAMEBUFFER, rtt.buffers[rtt.current_buffer].framebuffer);
-
- glViewport(0, 0, rtt.width, rtt.height);
-
- // Send the application-specific uniforms to the shader
- sendApplicationUniforms(rtt.shader);
-
- // Specify the vertices for the shader
- glEnableVertexAttribArray(0);
- glBindBuffer(GL_ARRAY_BUFFER, rtt.vertex_buffer);
- glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
-
- // Set the list of draw buffers.
- GLenum draw_buffers[1] = { GL_COLOR_ATTACHMENT0 };
- glDrawBuffers(1, draw_buffers);
-
- // Draw the mesh
- glDrawArrays(GL_TRIANGLES, 0, rtt.nb_vertices);
-
- // Cleanup
- glDisableVertexAttribArray(0);
- glBindFramebuffer(GL_FRAMEBUFFER, 0);
-
- // Restore the previous viewport
- glViewport(previous_viewport[0], previous_viewport[1], previous_viewport[2],
- previous_viewport[3]);
-
- return true;
-}
-
-
-/*********************************** MESHES **********************************/
-
-model_t create_rectangle(const shader_t& shader, const arma::fvec& color,
- float width, float height, const arma::fvec& position,
- const arma::fmat& rotation, transforms_t* parent_transforms)
-{
- model_t model = { 0 };
-
- if (shader.use_lightning)
- return model;
-
- model.mode = GL_TRIANGLES;
- model.shader = &shader;
-
- // Position & rotation
- model.transforms.position = position;
- model.transforms.rotation = rotation;
- model.transforms.parent = parent_transforms;
-
- // Material
- model.diffuse_color = color;
-
- // Create the mesh
- model.nb_vertices = 6;
-
- //-- Vertex buffer
- float half_width = 0.5f * width;
- float half_height = 0.5f * height;
-
- const GLfloat vertex_buffer_data[] = {
- half_width, half_height, 0.0f,
- -half_width, half_height, 0.0f,
- -half_width, -half_height, 0.0f,
- -half_width, -half_height, 0.0f,
- half_width, -half_height, 0.0f,
- half_width, half_height, 0.0f,
- };
-
- glGenBuffers(1, &model.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- //-- UVs buffer
- const GLfloat uv_buffer_data[] = {
- 1.0f, 1.0f,
- 0.0f, 1.0f,
- 0.0f, 0.0f,
- 0.0f, 0.0f,
- 1.0f, 0.0f,
- 1.0f, 1.0f,
- };
-
- glGenBuffers(1, &model.uv_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 2 * sizeof(GLfloat),
- uv_buffer_data, GL_STATIC_DRAW);
-
- return model;
-}
-
-//-----------------------------------------------
-
-model_t create_square(const shader_t& shader, const arma::fvec& color, float size,
- const arma::fvec& position, const arma::fmat& rotation,
- transforms_t* parent_transforms)
-{
- model_t model = { 0 };
-
- if (shader.use_lightning)
- return model;
-
- model.mode = GL_TRIANGLES;
- model.shader = &shader;
-
- // Position & rotation
- model.transforms.position = position;
- model.transforms.rotation = rotation;
- model.transforms.parent = parent_transforms;
-
- // Material
- model.diffuse_color = color;
-
- // Create the mesh
- model.nb_vertices = 6;
-
- //-- Vertex buffer
- float half_size = 0.5f * size;
-
- const GLfloat vertex_buffer_data[] = {
- half_size, half_size, 0.0f,
- -half_size, half_size, 0.0f,
- -half_size, -half_size, 0.0f,
- -half_size, -half_size, 0.0f,
- half_size, -half_size, 0.0f,
- half_size, half_size, 0.0f,
- };
-
- glGenBuffers(1, &model.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- //-- UVs buffer
- const GLfloat uv_buffer_data[] = {
- 1.0f, 1.0f,
- 0.0f, 1.0f,
- 0.0f, 0.0f,
- 0.0f, 0.0f,
- 1.0f, 0.0f,
- 1.0f, 1.0f,
- };
-
- glGenBuffers(1, &model.uv_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 2 * sizeof(GLfloat),
- uv_buffer_data, GL_STATIC_DRAW);
-
- return model;
-}
-
-//-----------------------------------------------
-
-model_t create_sphere(const shader_t& shader, float radius, const arma::fvec& position,
- const arma::fmat& rotation, transforms_t* parent_transforms)
-{
- model_t model = { 0 };
-
- model.mode = GL_TRIANGLES;
- model.shader = &shader;
-
- // Position & rotation
- model.transforms.position = position;
- model.transforms.rotation = rotation;
- model.transforms.parent = parent_transforms;
-
- // Material
- model.ambiant_color = arma::fvec({0.2f, 0.2f, 0.2f});
- model.diffuse_color = arma::fvec({0.8f, 0.8f, 0.8f});
- model.specular_color = arma::fvec({0.0f, 0.0f, 0.0f});
- model.specular_power = 5;
-
- // Create the mesh
- const int NB_STEPS = 72;
- const float STEP_SIZE = 360.0f / NB_STEPS;
-
- model.nb_vertices = NB_STEPS / 2 * NB_STEPS * 6;
-
- GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
-
- GLfloat* normal_buffer_data = (shader.use_lightning ?
- new GLfloat[model.nb_vertices * 3] : 0);
-
- GLfloat* dst_vertex = vertex_buffer_data;
- GLfloat* dst_normal = normal_buffer_data;
-
- for (int i = 0; i < NB_STEPS / 2; ++i)
- {
- GLfloat latitude_lo = (float) i * STEP_SIZE;
- GLfloat latitude_hi = latitude_lo + STEP_SIZE;
-
- for (int j = 0; j < NB_STEPS; ++j)
- {
- GLfloat longitude_lo = (float) j * STEP_SIZE;
- GLfloat longitude_hi = longitude_lo + STEP_SIZE;
-
- arma::fvec vert_ne(3);
- arma::fvec vert_nw(3);
- arma::fvec vert_sw(3);
- arma::fvec vert_se(3);
-
- // Assign each X,Z with sin,cos values scaled by latitude radius indexed by longitude.
- vert_ne(1) = vert_nw(1) = (float) -cos_deg(latitude_hi) * radius;
- vert_sw(1) = vert_se(1) = (float) -cos_deg(latitude_lo) * radius;
-
- vert_nw(0) = (float) cos_deg(longitude_lo) * (radius * (float) sin_deg(latitude_hi));
- vert_sw(0) = (float) cos_deg(longitude_lo) * (radius * (float) sin_deg(latitude_lo));
- vert_ne(0) = (float) cos_deg(longitude_hi) * (radius * (float) sin_deg(latitude_hi));
- vert_se(0) = (float) cos_deg(longitude_hi) * (radius * (float) sin_deg(latitude_lo));
-
- vert_nw(2) = (float) -sin_deg(longitude_lo) * (radius * (float) sin_deg(latitude_hi));
- vert_sw(2) = (float) -sin_deg(longitude_lo) * (radius * (float) sin_deg(latitude_lo));
- vert_ne(2) = (float) -sin_deg(longitude_hi) * (radius * (float) sin_deg(latitude_hi));
- vert_se(2) = (float) -sin_deg(longitude_hi) * (radius * (float) sin_deg(latitude_lo));
-
- dst_vertex[0] = vert_ne(0); dst_vertex[1] = vert_ne(1); dst_vertex[2] = vert_ne(2); dst_vertex += 3;
- dst_vertex[0] = vert_nw(0); dst_vertex[1] = vert_nw(1); dst_vertex[2] = vert_nw(2); dst_vertex += 3;
- dst_vertex[0] = vert_sw(0); dst_vertex[1] = vert_sw(1); dst_vertex[2] = vert_sw(2); dst_vertex += 3;
-
- dst_vertex[0] = vert_sw(0); dst_vertex[1] = vert_sw(1); dst_vertex[2] = vert_sw(2); dst_vertex += 3;
- dst_vertex[0] = vert_se(0); dst_vertex[1] = vert_se(1); dst_vertex[2] = vert_se(2); dst_vertex += 3;
- dst_vertex[0] = vert_ne(0); dst_vertex[1] = vert_ne(1); dst_vertex[2] = vert_ne(2); dst_vertex += 3;
-
- if (shader.use_lightning)
- {
- arma::fvec normal_ne = arma::normalise(vert_ne);
- arma::fvec normal_nw = arma::normalise(vert_nw);
- arma::fvec normal_sw = arma::normalise(vert_sw);
- arma::fvec normal_se = arma::normalise(vert_se);
-
- dst_normal[0] = normal_ne(0); dst_normal[1] = normal_ne(1); dst_normal[2] = normal_ne(2); dst_normal += 3;
- dst_normal[0] = normal_nw(0); dst_normal[1] = normal_nw(1); dst_normal[2] = normal_nw(2); dst_normal += 3;
- dst_normal[0] = normal_sw(0); dst_normal[1] = normal_sw(1); dst_normal[2] = normal_sw(2); dst_normal += 3;
-
- dst_normal[0] = normal_sw(0); dst_normal[1] = normal_sw(1); dst_normal[2] = normal_sw(2); dst_normal += 3;
- dst_normal[0] = normal_se(0); dst_normal[1] = normal_se(1); dst_normal[2] = normal_se(2); dst_normal += 3;
- dst_normal[0] = normal_ne(0); dst_normal[1] = normal_ne(1); dst_normal[2] = normal_ne(2); dst_normal += 3;
- }
- }
- }
-
- // Vertex buffer
- glGenBuffers(1, &model.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- // Normal buffer
- if (shader.use_lightning)
- {
- glGenBuffers(1, &model.normal_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.normal_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- normal_buffer_data, GL_STATIC_DRAW);
- }
-
- // Cleanup
- delete[] vertex_buffer_data;
-
- if (shader.use_lightning)
- delete[] normal_buffer_data;
-
- return model;
-}
-
-//-----------------------------------------------
-
-model_t create_line(const shader_t& shader, const arma::fvec& color,
- const arma::mat& points, const arma::fvec& position,
- const arma::fmat& rotation, transforms_t* parent_transforms)
-{
- model_t model = { 0 };
-
- if (shader.use_lightning)
- return model;
-
- model.mode = GL_LINE_STRIP;
- model.shader = &shader;
-
- // Position & rotation
- model.transforms.position = position;
- model.transforms.rotation = rotation;
- model.transforms.parent = parent_transforms;
-
- // Material
- model.diffuse_color = color;
-
- // Create the mesh
- model.nb_vertices = points.n_cols;
-
- GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
-
- GLfloat* dst = vertex_buffer_data;
-
- for (int i = 0; i < points.n_cols; ++i) {
- dst[0] = (float) points(0, i);
- dst[1] = (float) points(1, i);
-
- if (points.n_rows == 3)
- dst[2] = (float) points(2, i);
- else
- dst[2] = 0.0f;
-
- dst += 3;
- }
-
- // Vertex buffer
- glGenBuffers(1, &model.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- // Cleanup
- delete[] vertex_buffer_data;
-
- return model;
-}
-
-//-----------------------------------------------
-
-model_t create_line(const shader_t& shader, const arma::fvec& color,
- const std::vector<arma::vec>& points, const arma::fvec& position,
- const arma::fmat& rotation, transforms_t* parent_transforms)
-{
- arma::mat points_mat(points[0].n_rows, points.size());
-
- for (size_t i = 0; i < points.size(); ++i)
- points_mat.col(i) = points[i];
-
- return create_line(shader, color, points_mat, position, rotation, parent_transforms);
-}
-
-//-----------------------------------------------
-
-model_t create_mesh(const shader_t& shader, const arma::fvec& color,
- const arma::mat& vertices,
- const arma::fvec& position, const arma::fmat& rotation,
- transforms_t* parent_transforms)
-{
- model_t model = { 0 };
-
- if (shader.use_lightning)
- return model;
-
- model.mode = GL_TRIANGLES;
- model.shader = &shader;
-
- // Position & rotation
- model.transforms.position = position;
- model.transforms.rotation = rotation;
- model.transforms.parent = parent_transforms;
-
- // Material
- model.diffuse_color = color;
-
- // Create the mesh
- model.nb_vertices = vertices.n_cols;
-
- GLfloat* vertex_buffer_data = new GLfloat[model.nb_vertices * 3];
-
- GLfloat* dst = vertex_buffer_data;
-
- for (int i = 0; i < vertices.n_cols; ++i) {
- dst[0] = (float) vertices(0, i);
- dst[1] = (float) vertices(1, i);
-
- if (vertices.n_rows == 3)
- dst[2] = (float) vertices(2, i);
- else
- dst[2] = 0.0f;
-
- dst += 3;
- }
-
- // Vertex buffer
- glGenBuffers(1, &model.vertex_buffer);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
-
- glBufferData(GL_ARRAY_BUFFER, model.nb_vertices * 3 * sizeof(GLfloat),
- vertex_buffer_data, GL_STATIC_DRAW);
-
- // Cleanup
- delete[] vertex_buffer_data;
-
- return model;
-}
-
-//-----------------------------------------------
-
-void destroy(const model_t& model)
-{
- if (model.vertex_buffer)
- glDeleteBuffers(1, &model.vertex_buffer);
-
- if (model.normal_buffer)
- glDeleteBuffers(1, &model.normal_buffer);
-
- if (model.uv_buffer)
- glDeleteBuffers(1, &model.uv_buffer);
-}
-
-
-/********************************** RENDERING ********************************/
-
-bool draw(const model_t& model, const arma::fmat& view,
- const arma::fmat& projection, const light_list_t& lights)
-{
- // Various checks
- if (model.shader->id == 0)
- return false;
-
- if (model.shader->use_lightning && lights.empty())
- return false;
-
- // Activate the GLSL program
- glUseProgram(model.shader->id);
-
- // Send the model, view and projection matrices to the shader
- arma::fmat model_matrix = worldTransforms(&model.transforms);
-
- glUniformMatrix4fv(model.shader->model_matrix_handle, 1, GL_FALSE, model_matrix.memptr());
- glUniformMatrix4fv(model.shader->view_matrix_handle, 1, GL_FALSE, view.memptr());
- glUniformMatrix4fv(model.shader->projection_matrix_handle, 1, GL_FALSE, projection.memptr());
-
- // Send the material parameters to the shader
- glUniform3fv(model.shader->diffuse_color_handle, 1, model.diffuse_color.memptr());
-
- if (model.shader->use_lightning) {
- glUniform3fv(model.shader->ambiant_color_handle, 1, model.ambiant_color.memptr());
- glUniform3fv(model.shader->specular_color_handle, 1, model.specular_color.memptr());
- glUniform1f(model.shader->specular_power_handle, model.specular_power);
- }
-
- // Send the texture parameters to the shader
- if (model.shader->diffuse_texture_handle > -1) {
- glActiveTexture(GL_TEXTURE0);
- glBindTexture(GL_TEXTURE_2D, model.diffuse_texture);
- glUniform1i(model.shader->diffuse_texture_handle, 0);
- }
-
- // Send the light parameters to the shader
- if (model.shader->use_lightning) {
- glUniform3fv(model.shader->light_position_handle, 1, lights[0].transforms.position.memptr());
- glUniform3fv(model.shader->light_color_handle, 1, lights[0].color.memptr());
- glUniform1f(model.shader->light_power_handle, lights[0].power);
- }
-
- // Send the application-specific uniforms to the shader
- sendApplicationUniforms(model.shader);
-
- // Specify the vertices for the shader
- glEnableVertexAttribArray(0);
- glBindBuffer(GL_ARRAY_BUFFER, model.vertex_buffer);
- glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
-
- // Specify the normals for the shader
- if (model.shader->use_lightning) {
- glEnableVertexAttribArray(1);
- glBindBuffer(GL_ARRAY_BUFFER, model.normal_buffer);
- glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 0, (void*) 0);
- }
-
- // Specify the UVs for the shader
- if (model.uv_buffer) {
- glEnableVertexAttribArray(2);
- glBindBuffer(GL_ARRAY_BUFFER, model.uv_buffer);
- glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 0, (void*) 0);
- }
-
- // Draw the mesh
- glDrawArrays(model.mode, 0, model.nb_vertices);
-
- glDisableVertexAttribArray(0);
-
- if (model.shader->use_lightning)
- glDisableVertexAttribArray(1);
-
- if (model.uv_buffer)
- glDisableVertexAttribArray(2);
-
- return true;
-}
-
-//-----------------------------------------------
-
-bool draw_rectangle(const shader_t& shader, const arma::fvec& color, float width,
- float height, const arma::fmat& view, const arma::fmat& projection,
- const arma::fvec& position, const arma::fmat& rotation)
-{
- model_t rect = create_rectangle(shader, color, width, height, position, rotation);
-
- bool result = draw(rect, view, projection);
-
- destroy(rect);
-
- return result;
-}
-
-//-----------------------------------------------
-
-bool draw_line(const shader_t& shader, const arma::fvec& color, const arma::mat& points,
- const arma::fmat& view, const arma::fmat& projection,
- const arma::fvec& position, const arma::fmat& rotation)
-{
- model_t line = create_line(shader, color, points, position, rotation);
-
- bool result = draw(line, view, projection);
-
- destroy(line);
-
- return result;
-}
-
-//-----------------------------------------------
-
-bool draw_line(const shader_t& shader, const arma::fvec& color, const std::vector<arma::vec>& points,
- const arma::fmat& view, const arma::fmat& projection,
- const arma::fvec& position, const arma::fmat& rotation)
-{
- model_t line = create_line(shader, color, points, position, rotation);
-
- bool result = draw(line, view, projection);
-
- destroy(line);
-
- return result;
-}
-
-//-----------------------------------------------
-
-bool draw_mesh(const shader_t& shader, const arma::fvec& color, const arma::mat& vertices,
- const arma::fmat& view, const arma::fmat& projection,
- const arma::fvec& position, const arma::fmat& rotation)
-{
- model_t mesh = create_mesh(shader, color, vertices, position, rotation);
-
- bool result = draw(mesh, view, projection);
-
- destroy(mesh);
-
- return result;
-}
-
-//-----------------------------------------------
-
-bool draw_gaussian(shader_t* shader, const arma::fvec& color,
- const arma::vec& mu, const arma::mat& sigma,
- const arma::fmat& view, const arma::fmat& projection,
- float viewport_width, float viewport_height)
-{
- float square_size = fmax(viewport_width, viewport_height) +
- fmax(fabs(fabs(mu(0)) - fabs(view(0, 3))),
- fabs(fabs(mu(1)) - fabs(view(1, 3)))) * 2;
-
- gfx3::model_t square = gfx3::create_rectangle(*shader, color, square_size, square_size);
-
- square.transforms.position = { (float) mu(0), (float) mu(1), 0.0f };
-
- glDisable(GL_DEPTH_TEST);
- glEnable(GL_BLEND);
-
- arma::mat scaled_sigma = sigma(arma::span(0, 1), arma::span(0, 1)) /
- (double) (square_size * square_size);
-
- arma::vec gaussian_color;
- if (color.n_rows == 4)
- gaussian_color = arma::conv_to<arma::vec>::from(color);
- else
- gaussian_color = arma::vec({ color(0), color(1), color(2), 0.5 });
-
- shader->setUniform("Mu", arma::vec{0.5, 0.5});
- shader->setUniform("Sigma", arma::mat(arma::inv(scaled_sigma)));
- shader->setUniform("GaussianColor", gaussian_color);
-
- bool result = gfx3::draw(square, view, projection);
-
- glDisable(GL_BLEND);
- glEnable(GL_DEPTH_TEST);
-
- gfx3::destroy(square);
-
- return result;
-}
-
-
-//-----------------------------------------------
-
-bool draw_gaussian_border(shader_t* shader, const arma::fvec& color,
- const arma::vec& mu, const arma::mat& sigma,
- const arma::fmat& view, const arma::fmat& projection,
- float viewport_width, float viewport_height)
-{
- const int NB_POINTS = 60;
-
- arma::mat pts0 = arma::join_cols(
- arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
- arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
- );
-
- arma::vec eigval(2);
- arma::mat eigvec(2, 2);
- eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
-
- arma::mat R = eigvec * diagmat(sqrt(eigval));
-
- arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
-
- arma::fvec gaussian_color = color(arma::span(0, 2));
-
- return draw_line(*shader, gaussian_color, pts, view, projection);
-}
-
-
-/******************************** RAY CASTING ********************************/
-
-ray_t create_ray(const arma::fvec& origin, int mouse_x, int mouse_y,
- const arma::fmat& view, const arma::fmat& projection,
- int window_width, int window_height)
-{
- ray_t ray;
-
- ray.origin = origin;
-
- // Compute the ray in homogeneous clip coordinates (range [-1:1, -1:1, -1:1, -1:1])
- arma::fvec ray_clip(4);
- ray_clip(0) = (2.0f * mouse_x) / window_width - 1.0f;
- ray_clip(1) = 1.0f - (2.0f * mouse_y) / window_height;
- ray_clip(2) = -1.0f;
- ray_clip(3) = 1.0f;
-
- // Compute the ray in camera coordinates
- arma::fvec ray_eye = arma::inv(projection) * ray_clip;
- ray_eye(2) = -1.0f;
- ray_eye(3) = 0.0f;
-
- // Compute the ray in world coordinates
- arma::fvec ray_world = arma::inv(view) * ray_eye;
- ray.direction = arma::fvec(arma::normalise(ray_world)).rows(0, 2);
-
- return ray;
-}
-
-//-----------------------------------------------
-
-bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
- arma::fvec &result)
-{
- arma::fvec O_C = ray.origin - center;
- float b = arma::dot(ray.direction, O_C);
- float c = arma::dot(O_C, O_C) - radius * radius;
-
- float det = b * b - c;
-
- if (det < 0.0f)
- return false;
-
- float t;
-
- if (det > 0.0f)
- {
- float t1 = -b + sqrtf(det);
- float t2 = -b - sqrtf(det);
-
- t = (t1 < t2 ? t1 : t2);
- }
- else
- {
- t = -b + sqrtf(det);
- }
-
- result = ray.origin + ray.direction * t;
-
- return true;
-}
-
-}
diff --git a/src/utils/imgui_impl_glfw_gl3.cpp b/src/utils/imgui_impl_glfw_gl3.cpp
deleted file mode 100644
index 9620870f7c29c4e28180b256a698b18df9c3f297..0000000000000000000000000000000000000000
--- a/src/utils/imgui_impl_glfw_gl3.cpp
+++ /dev/null
@@ -1,530 +0,0 @@
-// ImGui GLFW binding with OpenGL3 + shaders
-// (GLFW is a cross-platform general purpose library for handling windows, inputs, OpenGL/Vulkan graphics context creation, etc.)
-// (GL3W is a helper library to access OpenGL functions since there is no standard header to access modern OpenGL functions easily. Alternatives are GLEW, Glad, etc.)
-
-// Implemented features:
-// [X] User texture binding. Cast 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID in imgui.cpp.
-// [X] Gamepad navigation mapping. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'.
-
-// You can copy and use unmodified imgui_impl_* files in your project. See main.cpp for an example of using this.
-// If you use this binding you'll need to call 4 functions: ImGui_ImplXXXX_Init(), ImGui_ImplXXXX_NewFrame(), ImGui::Render() and ImGui_ImplXXXX_Shutdown().
-// If you are new to ImGui, see examples/README.txt and documentation at the top of imgui.cpp.
-// https://github.com/ocornut/imgui
-
-// CHANGELOG
-// (minor and older changes stripped away, please see git history for details)
-// 2018-03-20: Misc: Setup io.BackendFlags ImGuiBackendFlags_HasMouseCursors and ImGuiBackendFlags_HasSetMousePos flags + honor ImGuiConfigFlags_NoMouseCursorChange flag.
-// 2018-03-06: OpenGL: Added const char* glsl_version parameter to ImGui_ImplGlfwGL3_Init() so user can override the GLSL version e.g. "#version 150".
-// 2018-02-23: OpenGL: Create the VAO in the render function so the setup can more easily be used with multiple shared GL context.
-// 2018-02-20: Inputs: Added support for mouse cursors (ImGui::GetMouseCursor() value and WM_SETCURSOR message handling).
-// 2018-02-20: Inputs: Renamed GLFW callbacks exposed in .h to not include GL3 in their name.
-// 2018-02-16: Misc: Obsoleted the io.RenderDrawListsFn callback and exposed ImGui_ImplGlfwGL3_RenderDrawData() in the .h file so you can call it yourself.
-// 2018-02-06: Misc: Removed call to ImGui::Shutdown() which is not available from 1.60 WIP, user needs to call CreateContext/DestroyContext themselves.
-// 2018-02-06: Inputs: Added mapping for ImGuiKey_Space.
-// 2018-01-25: Inputs: Added gamepad support if ImGuiConfigFlags_NavEnableGamepad is set.
-// 2018-01-25: Inputs: Honoring the io.WantSetMousePos flag by repositioning the mouse (ImGuiConfigFlags_NavEnableSetMousePos is set).
-// 2018-01-20: Inputs: Added Horizontal Mouse Wheel support.
-// 2018-01-18: Inputs: Added mapping for ImGuiKey_Insert.
-// 2018-01-07: OpenGL: Changed GLSL shader version from 330 to 150. (Also changed GL context from 3.3 to 3.2 in example's main.cpp)
-// 2017-09-01: OpenGL: Save and restore current bound sampler. Save and restore current polygon mode.
-// 2017-08-25: Inputs: MousePos set to -FLT_MAX,-FLT_MAX when mouse is unavailable/missing (instead of -1,-1).
-// 2017-05-01: OpenGL: Fixed save and restore of current blend function state.
-// 2016-10-15: Misc: Added a void* user_data parameter to Clipboard function handlers.
-// 2016-09-05: OpenGL: Fixed save and restore of current scissor rectangle.
-// 2016-04-30: OpenGL: Fixed save and restore of current GL_ACTIVE_TEXTURE.
-
-#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
-#define _CRT_SECURE_NO_WARNINGS
-#endif
-
-#include "imgui.h"
-#include "imgui_impl_glfw_gl3.h"
-
-// GL3W/GLFW
-#include <GL/glew.h>
-#include <GLFW/glfw3.h>
-#ifdef _WIN32
-#undef APIENTRY
-#define GLFW_EXPOSE_NATIVE_WIN32
-#define GLFW_EXPOSE_NATIVE_WGL
-#include <GLFW/glfw3native.h>
-#endif
-
-// GLFW data
-static GLFWwindow* g_Window = NULL;
-static double g_Time = 0.0f;
-static bool g_MouseJustPressed[3] = { false, false, false };
-static GLFWcursor* g_MouseCursors[ImGuiMouseCursor_COUNT] = { 0 };
-
-// OpenGL3 data
-static char g_GlslVersion[32] = "#version 150";
-static GLuint g_FontTexture = 0;
-static int g_ShaderHandle = 0, g_VertHandle = 0, g_FragHandle = 0;
-static int g_AttribLocationTex = 0, g_AttribLocationProjMtx = 0;
-static int g_AttribLocationPosition = 0, g_AttribLocationUV = 0, g_AttribLocationColor = 0;
-static unsigned int g_VboHandle = 0, g_ElementsHandle = 0;
-
-// OpenGL3 Render function.
-// (this used to be set in io.RenderDrawListsFn and called by ImGui::Render(), but you can now call this directly from your main loop)
-// Note that this implementation is little overcomplicated because we are saving/setting up/restoring every OpenGL state explicitly, in order to be able to run within any OpenGL engine that doesn't do so.
-void ImGui_ImplGlfwGL3_RenderDrawData(ImDrawData* draw_data)
-{
- // Avoid rendering when minimized, scale coordinates for retina displays (screen coordinates != framebuffer coordinates)
- ImGuiIO& io = ImGui::GetIO();
- int fb_width = (int)(io.DisplaySize.x * io.DisplayFramebufferScale.x);
- int fb_height = (int)(io.DisplaySize.y * io.DisplayFramebufferScale.y);
- if (fb_width == 0 || fb_height == 0)
- return;
- draw_data->ScaleClipRects(io.DisplayFramebufferScale);
-
- // Backup GL state
- GLenum last_active_texture; glGetIntegerv(GL_ACTIVE_TEXTURE, (GLint*)&last_active_texture);
- glActiveTexture(GL_TEXTURE0);
- GLint last_program; glGetIntegerv(GL_CURRENT_PROGRAM, &last_program);
- GLint last_texture; glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
- GLint last_sampler; glGetIntegerv(GL_SAMPLER_BINDING, &last_sampler);
- GLint last_array_buffer; glGetIntegerv(GL_ARRAY_BUFFER_BINDING, &last_array_buffer);
- GLint last_element_array_buffer; glGetIntegerv(GL_ELEMENT_ARRAY_BUFFER_BINDING, &last_element_array_buffer);
- GLint last_vertex_array; glGetIntegerv(GL_VERTEX_ARRAY_BINDING, &last_vertex_array);
- GLint last_polygon_mode[2]; glGetIntegerv(GL_POLYGON_MODE, last_polygon_mode);
- GLint last_viewport[4]; glGetIntegerv(GL_VIEWPORT, last_viewport);
- GLint last_scissor_box[4]; glGetIntegerv(GL_SCISSOR_BOX, last_scissor_box);
- GLenum last_blend_src_rgb; glGetIntegerv(GL_BLEND_SRC_RGB, (GLint*)&last_blend_src_rgb);
- GLenum last_blend_dst_rgb; glGetIntegerv(GL_BLEND_DST_RGB, (GLint*)&last_blend_dst_rgb);
- GLenum last_blend_src_alpha; glGetIntegerv(GL_BLEND_SRC_ALPHA, (GLint*)&last_blend_src_alpha);
- GLenum last_blend_dst_alpha; glGetIntegerv(GL_BLEND_DST_ALPHA, (GLint*)&last_blend_dst_alpha);
- GLenum last_blend_equation_rgb; glGetIntegerv(GL_BLEND_EQUATION_RGB, (GLint*)&last_blend_equation_rgb);
- GLenum last_blend_equation_alpha; glGetIntegerv(GL_BLEND_EQUATION_ALPHA, (GLint*)&last_blend_equation_alpha);
- GLboolean last_enable_blend = glIsEnabled(GL_BLEND);
- GLboolean last_enable_cull_face = glIsEnabled(GL_CULL_FACE);
- GLboolean last_enable_depth_test = glIsEnabled(GL_DEPTH_TEST);
- GLboolean last_enable_scissor_test = glIsEnabled(GL_SCISSOR_TEST);
-
- // Setup render state: alpha-blending enabled, no face culling, no depth testing, scissor enabled, polygon fill
- glEnable(GL_BLEND);
- glBlendEquation(GL_FUNC_ADD);
- glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
- glDisable(GL_CULL_FACE);
- glDisable(GL_DEPTH_TEST);
- glEnable(GL_SCISSOR_TEST);
- glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
-
- // Setup viewport, orthographic projection matrix
- glViewport(0, 0, (GLsizei)fb_width, (GLsizei)fb_height);
- const float ortho_projection[4][4] =
- {
- { 2.0f/io.DisplaySize.x, 0.0f, 0.0f, 0.0f },
- { 0.0f, 2.0f/-io.DisplaySize.y, 0.0f, 0.0f },
- { 0.0f, 0.0f, -1.0f, 0.0f },
- {-1.0f, 1.0f, 0.0f, 1.0f },
- };
- glUseProgram(g_ShaderHandle);
- glUniform1i(g_AttribLocationTex, 0);
- glUniformMatrix4fv(g_AttribLocationProjMtx, 1, GL_FALSE, &ortho_projection[0][0]);
- glBindSampler(0, 0); // Rely on combined texture/sampler state.
-
- // Recreate the VAO every time
- // (This is to easily allow multiple GL contexts. VAO are not shared among GL contexts, and we don't track creation/deletion of windows so we don't have an obvious key to use to cache them.)
- GLuint vao_handle = 0;
- glGenVertexArrays(1, &vao_handle);
- glBindVertexArray(vao_handle);
- glBindBuffer(GL_ARRAY_BUFFER, g_VboHandle);
- glEnableVertexAttribArray(g_AttribLocationPosition);
- glEnableVertexAttribArray(g_AttribLocationUV);
- glEnableVertexAttribArray(g_AttribLocationColor);
- glVertexAttribPointer(g_AttribLocationPosition, 2, GL_FLOAT, GL_FALSE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, pos));
- glVertexAttribPointer(g_AttribLocationUV, 2, GL_FLOAT, GL_FALSE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, uv));
- glVertexAttribPointer(g_AttribLocationColor, 4, GL_UNSIGNED_BYTE, GL_TRUE, sizeof(ImDrawVert), (GLvoid*)IM_OFFSETOF(ImDrawVert, col));
-
- // Draw
- for (int n = 0; n < draw_data->CmdListsCount; n++)
- {
- const ImDrawList* cmd_list = draw_data->CmdLists[n];
- const ImDrawIdx* idx_buffer_offset = 0;
-
- glBindBuffer(GL_ARRAY_BUFFER, g_VboHandle);
- glBufferData(GL_ARRAY_BUFFER, (GLsizeiptr)cmd_list->VtxBuffer.Size * sizeof(ImDrawVert), (const GLvoid*)cmd_list->VtxBuffer.Data, GL_STREAM_DRAW);
-
- glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, g_ElementsHandle);
- glBufferData(GL_ELEMENT_ARRAY_BUFFER, (GLsizeiptr)cmd_list->IdxBuffer.Size * sizeof(ImDrawIdx), (const GLvoid*)cmd_list->IdxBuffer.Data, GL_STREAM_DRAW);
-
- for (int cmd_i = 0; cmd_i < cmd_list->CmdBuffer.Size; cmd_i++)
- {
- const ImDrawCmd* pcmd = &cmd_list->CmdBuffer[cmd_i];
- if (pcmd->UserCallback)
- {
- pcmd->UserCallback(cmd_list, pcmd);
- }
- else
- {
- glBindTexture(GL_TEXTURE_2D, (GLuint)(intptr_t)pcmd->TextureId);
- glScissor((int)pcmd->ClipRect.x, (int)(fb_height - pcmd->ClipRect.w), (int)(pcmd->ClipRect.z - pcmd->ClipRect.x), (int)(pcmd->ClipRect.w - pcmd->ClipRect.y));
- glDrawElements(GL_TRIANGLES, (GLsizei)pcmd->ElemCount, sizeof(ImDrawIdx) == 2 ? GL_UNSIGNED_SHORT : GL_UNSIGNED_INT, idx_buffer_offset);
- }
- idx_buffer_offset += pcmd->ElemCount;
- }
- }
- glDeleteVertexArrays(1, &vao_handle);
-
- // Restore modified GL state
- glUseProgram(last_program);
- glBindTexture(GL_TEXTURE_2D, last_texture);
- glBindSampler(0, last_sampler);
- glActiveTexture(last_active_texture);
- glBindVertexArray(last_vertex_array);
- glBindBuffer(GL_ARRAY_BUFFER, last_array_buffer);
- glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, last_element_array_buffer);
- glBlendEquationSeparate(last_blend_equation_rgb, last_blend_equation_alpha);
- glBlendFuncSeparate(last_blend_src_rgb, last_blend_dst_rgb, last_blend_src_alpha, last_blend_dst_alpha);
- if (last_enable_blend) glEnable(GL_BLEND); else glDisable(GL_BLEND);
- if (last_enable_cull_face) glEnable(GL_CULL_FACE); else glDisable(GL_CULL_FACE);
- if (last_enable_depth_test) glEnable(GL_DEPTH_TEST); else glDisable(GL_DEPTH_TEST);
- if (last_enable_scissor_test) glEnable(GL_SCISSOR_TEST); else glDisable(GL_SCISSOR_TEST);
- glPolygonMode(GL_FRONT_AND_BACK, (GLenum)last_polygon_mode[0]);
- glViewport(last_viewport[0], last_viewport[1], (GLsizei)last_viewport[2], (GLsizei)last_viewport[3]);
- glScissor(last_scissor_box[0], last_scissor_box[1], (GLsizei)last_scissor_box[2], (GLsizei)last_scissor_box[3]);
-}
-
-static const char* ImGui_ImplGlfwGL3_GetClipboardText(void* user_data)
-{
- return glfwGetClipboardString((GLFWwindow*)user_data);
-}
-
-static void ImGui_ImplGlfwGL3_SetClipboardText(void* user_data, const char* text)
-{
- glfwSetClipboardString((GLFWwindow*)user_data, text);
-}
-
-void ImGui_ImplGlfw_MouseButtonCallback(GLFWwindow*, int button, int action, int /*mods*/)
-{
- if (action == GLFW_PRESS && button >= 0 && button < 3)
- g_MouseJustPressed[button] = true;
-}
-
-void ImGui_ImplGlfw_ScrollCallback(GLFWwindow*, double xoffset, double yoffset)
-{
- ImGuiIO& io = ImGui::GetIO();
- io.MouseWheelH += (float)xoffset;
- io.MouseWheel += (float)yoffset;
-}
-
-void ImGui_ImplGlfw_KeyCallback(GLFWwindow*, int key, int, int action, int mods)
-{
- ImGuiIO& io = ImGui::GetIO();
- if (action == GLFW_PRESS)
- io.KeysDown[key] = true;
- if (action == GLFW_RELEASE)
- io.KeysDown[key] = false;
-
- (void)mods; // Modifiers are not reliable across systems
- io.KeyCtrl = io.KeysDown[GLFW_KEY_LEFT_CONTROL] || io.KeysDown[GLFW_KEY_RIGHT_CONTROL];
- io.KeyShift = io.KeysDown[GLFW_KEY_LEFT_SHIFT] || io.KeysDown[GLFW_KEY_RIGHT_SHIFT];
- io.KeyAlt = io.KeysDown[GLFW_KEY_LEFT_ALT] || io.KeysDown[GLFW_KEY_RIGHT_ALT];
- io.KeySuper = io.KeysDown[GLFW_KEY_LEFT_SUPER] || io.KeysDown[GLFW_KEY_RIGHT_SUPER];
-}
-
-void ImGui_ImplGlfw_CharCallback(GLFWwindow*, unsigned int c)
-{
- ImGuiIO& io = ImGui::GetIO();
- if (c > 0 && c < 0x10000)
- io.AddInputCharacter((unsigned short)c);
-}
-
-bool ImGui_ImplGlfwGL3_CreateFontsTexture()
-{
- // Build texture atlas
- ImGuiIO& io = ImGui::GetIO();
- unsigned char* pixels;
- int width, height;
- io.Fonts->GetTexDataAsRGBA32(&pixels, &width, &height); // Load as RGBA 32-bits (75% of the memory is wasted, but default font is so small) because it is more likely to be compatible with user's existing shaders. If your ImTextureId represent a higher-level concept than just a GL texture id, consider calling GetTexDataAsAlpha8() instead to save on GPU memory.
-
- // Upload texture to graphics system
- GLint last_texture;
- glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
- glGenTextures(1, &g_FontTexture);
- glBindTexture(GL_TEXTURE_2D, g_FontTexture);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
- glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
- glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, pixels);
-
- // Store our identifier
- io.Fonts->TexID = (void *)(intptr_t)g_FontTexture;
-
- // Restore state
- glBindTexture(GL_TEXTURE_2D, last_texture);
-
- return true;
-}
-
-bool ImGui_ImplGlfwGL3_CreateDeviceObjects()
-{
- // Backup GL state
- GLint last_texture, last_array_buffer, last_vertex_array;
- glGetIntegerv(GL_TEXTURE_BINDING_2D, &last_texture);
- glGetIntegerv(GL_ARRAY_BUFFER_BINDING, &last_array_buffer);
- glGetIntegerv(GL_VERTEX_ARRAY_BINDING, &last_vertex_array);
-
- const GLchar* vertex_shader =
- "uniform mat4 ProjMtx;\n"
- "in vec2 Position;\n"
- "in vec2 UV;\n"
- "in vec4 Color;\n"
- "out vec2 Frag_UV;\n"
- "out vec4 Frag_Color;\n"
- "void main()\n"
- "{\n"
- " Frag_UV = UV;\n"
- " Frag_Color = Color;\n"
- " gl_Position = ProjMtx * vec4(Position.xy,0,1);\n"
- "}\n";
-
- const GLchar* fragment_shader =
- "uniform sampler2D Texture;\n"
- "in vec2 Frag_UV;\n"
- "in vec4 Frag_Color;\n"
- "out vec4 Out_Color;\n"
- "void main()\n"
- "{\n"
- " Out_Color = Frag_Color * texture( Texture, Frag_UV.st);\n"
- "}\n";
-
- const GLchar* vertex_shader_with_version[2] = { g_GlslVersion, vertex_shader };
- const GLchar* fragment_shader_with_version[2] = { g_GlslVersion, fragment_shader };
-
- g_ShaderHandle = glCreateProgram();
- g_VertHandle = glCreateShader(GL_VERTEX_SHADER);
- g_FragHandle = glCreateShader(GL_FRAGMENT_SHADER);
- glShaderSource(g_VertHandle, 2, vertex_shader_with_version, NULL);
- glShaderSource(g_FragHandle, 2, fragment_shader_with_version, NULL);
- glCompileShader(g_VertHandle);
- glCompileShader(g_FragHandle);
- glAttachShader(g_ShaderHandle, g_VertHandle);
- glAttachShader(g_ShaderHandle, g_FragHandle);
- glLinkProgram(g_ShaderHandle);
-
- g_AttribLocationTex = glGetUniformLocation(g_ShaderHandle, "Texture");
- g_AttribLocationProjMtx = glGetUniformLocation(g_ShaderHandle, "ProjMtx");
- g_AttribLocationPosition = glGetAttribLocation(g_ShaderHandle, "Position");
- g_AttribLocationUV = glGetAttribLocation(g_ShaderHandle, "UV");
- g_AttribLocationColor = glGetAttribLocation(g_ShaderHandle, "Color");
-
- glGenBuffers(1, &g_VboHandle);
- glGenBuffers(1, &g_ElementsHandle);
-
- ImGui_ImplGlfwGL3_CreateFontsTexture();
-
- // Restore modified GL state
- glBindTexture(GL_TEXTURE_2D, last_texture);
- glBindBuffer(GL_ARRAY_BUFFER, last_array_buffer);
- glBindVertexArray(last_vertex_array);
-
- return true;
-}
-
-void ImGui_ImplGlfwGL3_InvalidateDeviceObjects()
-{
- if (g_VboHandle) glDeleteBuffers(1, &g_VboHandle);
- if (g_ElementsHandle) glDeleteBuffers(1, &g_ElementsHandle);
- g_VboHandle = g_ElementsHandle = 0;
-
- if (g_ShaderHandle && g_VertHandle) glDetachShader(g_ShaderHandle, g_VertHandle);
- if (g_VertHandle) glDeleteShader(g_VertHandle);
- g_VertHandle = 0;
-
- if (g_ShaderHandle && g_FragHandle) glDetachShader(g_ShaderHandle, g_FragHandle);
- if (g_FragHandle) glDeleteShader(g_FragHandle);
- g_FragHandle = 0;
-
- if (g_ShaderHandle) glDeleteProgram(g_ShaderHandle);
- g_ShaderHandle = 0;
-
- if (g_FontTexture)
- {
- glDeleteTextures(1, &g_FontTexture);
- ImGui::GetIO().Fonts->TexID = 0;
- g_FontTexture = 0;
- }
-}
-
-static void ImGui_ImplGlfw_InstallCallbacks(GLFWwindow* window)
-{
- glfwSetMouseButtonCallback(window, ImGui_ImplGlfw_MouseButtonCallback);
- glfwSetScrollCallback(window, ImGui_ImplGlfw_ScrollCallback);
- glfwSetKeyCallback(window, ImGui_ImplGlfw_KeyCallback);
- glfwSetCharCallback(window, ImGui_ImplGlfw_CharCallback);
-}
-
-bool ImGui_ImplGlfwGL3_Init(GLFWwindow* window, bool install_callbacks, const char* glsl_version)
-{
- g_Window = window;
-
- // Store GL version string so we can refer to it later in case we recreate shaders.
- if (glsl_version == NULL)
- glsl_version = "#version 150";
- IM_ASSERT((int)strlen(glsl_version) + 2 < IM_ARRAYSIZE(g_GlslVersion));
- strcpy(g_GlslVersion, glsl_version);
- strcat(g_GlslVersion, "\n");
-
- // Setup back-end capabilities flags
- ImGuiIO& io = ImGui::GetIO();
- io.BackendFlags |= ImGuiBackendFlags_HasMouseCursors; // We can honor GetMouseCursor() values (optional)
- io.BackendFlags |= ImGuiBackendFlags_HasSetMousePos; // We can honor io.WantSetMousePos requests (optional, rarely used)
-
- // Keyboard mapping. ImGui will use those indices to peek into the io.KeyDown[] array.
- io.KeyMap[ImGuiKey_Tab] = GLFW_KEY_TAB;
- io.KeyMap[ImGuiKey_LeftArrow] = GLFW_KEY_LEFT;
- io.KeyMap[ImGuiKey_RightArrow] = GLFW_KEY_RIGHT;
- io.KeyMap[ImGuiKey_UpArrow] = GLFW_KEY_UP;
- io.KeyMap[ImGuiKey_DownArrow] = GLFW_KEY_DOWN;
- io.KeyMap[ImGuiKey_PageUp] = GLFW_KEY_PAGE_UP;
- io.KeyMap[ImGuiKey_PageDown] = GLFW_KEY_PAGE_DOWN;
- io.KeyMap[ImGuiKey_Home] = GLFW_KEY_HOME;
- io.KeyMap[ImGuiKey_End] = GLFW_KEY_END;
- io.KeyMap[ImGuiKey_Insert] = GLFW_KEY_INSERT;
- io.KeyMap[ImGuiKey_Delete] = GLFW_KEY_DELETE;
- io.KeyMap[ImGuiKey_Backspace] = GLFW_KEY_BACKSPACE;
- io.KeyMap[ImGuiKey_Space] = GLFW_KEY_SPACE;
- io.KeyMap[ImGuiKey_Enter] = GLFW_KEY_ENTER;
- io.KeyMap[ImGuiKey_Escape] = GLFW_KEY_ESCAPE;
- io.KeyMap[ImGuiKey_A] = GLFW_KEY_A;
- io.KeyMap[ImGuiKey_C] = GLFW_KEY_C;
- io.KeyMap[ImGuiKey_V] = GLFW_KEY_V;
- io.KeyMap[ImGuiKey_X] = GLFW_KEY_X;
- io.KeyMap[ImGuiKey_Y] = GLFW_KEY_Y;
- io.KeyMap[ImGuiKey_Z] = GLFW_KEY_Z;
-
- io.SetClipboardTextFn = ImGui_ImplGlfwGL3_SetClipboardText;
- io.GetClipboardTextFn = ImGui_ImplGlfwGL3_GetClipboardText;
- io.ClipboardUserData = g_Window;
-#ifdef _WIN32
- io.ImeWindowHandle = glfwGetWin32Window(g_Window);
-#endif
-
- // Load cursors
- // FIXME: GLFW doesn't expose suitable cursors for ResizeAll, ResizeNESW, ResizeNWSE. We revert to arrow cursor for those.
- g_MouseCursors[ImGuiMouseCursor_Arrow] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_TextInput] = glfwCreateStandardCursor(GLFW_IBEAM_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_ResizeAll] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_ResizeNS] = glfwCreateStandardCursor(GLFW_VRESIZE_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_ResizeEW] = glfwCreateStandardCursor(GLFW_HRESIZE_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_ResizeNESW] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
- g_MouseCursors[ImGuiMouseCursor_ResizeNWSE] = glfwCreateStandardCursor(GLFW_ARROW_CURSOR);
-
- if (install_callbacks)
- ImGui_ImplGlfw_InstallCallbacks(window);
-
- return true;
-}
-
-void ImGui_ImplGlfwGL3_Shutdown()
-{
- // Destroy GLFW mouse cursors
- for (ImGuiMouseCursor cursor_n = 0; cursor_n < ImGuiMouseCursor_COUNT; cursor_n++)
- glfwDestroyCursor(g_MouseCursors[cursor_n]);
- memset(g_MouseCursors, 0, sizeof(g_MouseCursors));
-
- // Destroy OpenGL objects
- ImGui_ImplGlfwGL3_InvalidateDeviceObjects();
-}
-
-void ImGui_ImplGlfwGL3_NewFrame()
-{
- if (!g_FontTexture)
- ImGui_ImplGlfwGL3_CreateDeviceObjects();
-
- ImGuiIO& io = ImGui::GetIO();
-
- // Setup display size (every frame to accommodate for window resizing)
- int w, h;
- int display_w, display_h;
- glfwGetWindowSize(g_Window, &w, &h);
- glfwGetFramebufferSize(g_Window, &display_w, &display_h);
- io.DisplaySize = ImVec2((float)w, (float)h);
- io.DisplayFramebufferScale = ImVec2(w > 0 ? ((float)display_w / w) : 0, h > 0 ? ((float)display_h / h) : 0);
-
- // Setup time step
- double current_time = glfwGetTime();
- io.DeltaTime = g_Time > 0.0 ? (float)(current_time - g_Time) : (float)(1.0f/60.0f);
- g_Time = current_time;
-
- // Setup inputs
- // (we already got mouse wheel, keyboard keys & characters from glfw callbacks polled in glfwPollEvents())
- if (glfwGetWindowAttrib(g_Window, GLFW_FOCUSED))
- {
- // Set OS mouse position if requested (only used when ImGuiConfigFlags_NavEnableSetMousePos is enabled by user)
- if (io.WantSetMousePos)
- {
- glfwSetCursorPos(g_Window, (double)io.MousePos.x, (double)io.MousePos.y);
- }
- else
- {
- double mouse_x, mouse_y;
- glfwGetCursorPos(g_Window, &mouse_x, &mouse_y);
- io.MousePos = ImVec2((float)mouse_x, (float)mouse_y);
- }
- }
- else
- {
- io.MousePos = ImVec2(-FLT_MAX,-FLT_MAX);
- }
-
- for (int i = 0; i < 3; i++)
- {
- // If a mouse press event came, always pass it as "mouse held this frame", so we don't miss click-release events that are shorter than 1 frame.
- io.MouseDown[i] = g_MouseJustPressed[i] || glfwGetMouseButton(g_Window, i) != 0;
- g_MouseJustPressed[i] = false;
- }
-
- // Update OS/hardware mouse cursor if imgui isn't drawing a software cursor
- if ((io.ConfigFlags & ImGuiConfigFlags_NoMouseCursorChange) == 0 && glfwGetInputMode(g_Window, GLFW_CURSOR) != GLFW_CURSOR_DISABLED)
- {
- ImGuiMouseCursor cursor = ImGui::GetMouseCursor();
- if (io.MouseDrawCursor || cursor == ImGuiMouseCursor_None)
- {
- glfwSetInputMode(g_Window, GLFW_CURSOR, GLFW_CURSOR_HIDDEN);
- }
- else
- {
- glfwSetCursor(g_Window, g_MouseCursors[cursor] ? g_MouseCursors[cursor] : g_MouseCursors[ImGuiMouseCursor_Arrow]);
- glfwSetInputMode(g_Window, GLFW_CURSOR, GLFW_CURSOR_NORMAL);
- }
- }
-
- // Gamepad navigation mapping [BETA]
- memset(io.NavInputs, 0, sizeof(io.NavInputs));
- if (io.ConfigFlags & ImGuiConfigFlags_NavEnableGamepad)
- {
- // Update gamepad inputs
- #define MAP_BUTTON(NAV_NO, BUTTON_NO) { if (buttons_count > BUTTON_NO && buttons[BUTTON_NO] == GLFW_PRESS) io.NavInputs[NAV_NO] = 1.0f; }
- #define MAP_ANALOG(NAV_NO, AXIS_NO, V0, V1) { float v = (axes_count > AXIS_NO) ? axes[AXIS_NO] : V0; v = (v - V0) / (V1 - V0); if (v > 1.0f) v = 1.0f; if (io.NavInputs[NAV_NO] < v) io.NavInputs[NAV_NO] = v; }
- int axes_count = 0, buttons_count = 0;
- const float* axes = glfwGetJoystickAxes(GLFW_JOYSTICK_1, &axes_count);
- const unsigned char* buttons = glfwGetJoystickButtons(GLFW_JOYSTICK_1, &buttons_count);
- MAP_BUTTON(ImGuiNavInput_Activate, 0); // Cross / A
- MAP_BUTTON(ImGuiNavInput_Cancel, 1); // Circle / B
- MAP_BUTTON(ImGuiNavInput_Menu, 2); // Square / X
- MAP_BUTTON(ImGuiNavInput_Input, 3); // Triangle / Y
- MAP_BUTTON(ImGuiNavInput_DpadLeft, 13); // D-Pad Left
- MAP_BUTTON(ImGuiNavInput_DpadRight, 11); // D-Pad Right
- MAP_BUTTON(ImGuiNavInput_DpadUp, 10); // D-Pad Up
- MAP_BUTTON(ImGuiNavInput_DpadDown, 12); // D-Pad Down
- MAP_BUTTON(ImGuiNavInput_FocusPrev, 4); // L1 / LB
- MAP_BUTTON(ImGuiNavInput_FocusNext, 5); // R1 / RB
- MAP_BUTTON(ImGuiNavInput_TweakSlow, 4); // L1 / LB
- MAP_BUTTON(ImGuiNavInput_TweakFast, 5); // R1 / RB
- MAP_ANALOG(ImGuiNavInput_LStickLeft, 0, -0.3f, -0.9f);
- MAP_ANALOG(ImGuiNavInput_LStickRight,0, +0.3f, +0.9f);
- MAP_ANALOG(ImGuiNavInput_LStickUp, 1, +0.3f, +0.9f);
- MAP_ANALOG(ImGuiNavInput_LStickDown, 1, -0.3f, -0.9f);
- #undef MAP_BUTTON
- #undef MAP_ANALOG
- if (axes_count > 0 && buttons_count > 0)
- io.BackendFlags |= ImGuiBackendFlags_HasGamepad;
- else
- io.BackendFlags &= ~ImGuiBackendFlags_HasGamepad;
- }
-
- // Start the frame. This call will update the io.WantCaptureMouse, io.WantCaptureKeyboard flag that you can use to dispatch inputs (or not) to your application.
- ImGui::NewFrame();
-}
diff --git a/src/utils/mpc_utils.cpp b/src/utils/mpc_utils.cpp
index 95ef2934a9b5ad4732308b5629034f202ddb3e64..3e9d2bd94078a8a5d9b35f0728e32c7ea4ccb2f6 100644
--- a/src/utils/mpc_utils.cpp
+++ b/src/utils/mpc_utils.cpp
@@ -129,7 +129,6 @@ void randomCovariances(arma::cube* Sigma, const arma::mat& Mu, const arma::vec&
vec s = (minRnd+(randu(2)*(1.-minRnd))) % covscale;
if(!semiTied)
theta = -PI + arma::randu()*2.*PI;
- mat sigm = makeSigma(theta, s);
Sigma->slice(i) = makeSigma(theta, s);
}
}