diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9083196d9f7b21db71e89e73bf68d9fcb8562ec2
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
+PROJECT(pbdlib)
+
+find_package(Armadillo 4.4 REQUIRED)
+set(CMAKE_MAJOR_VERSION 1)
+set(CMAKE_MINOR_VERSION 0)
+set(CMAKE_PATCH_VERSION 0)
+
+include_directories(include)
+include_directories(${ARMADILLO_INCLUDE_DIRS})
+
+ADD_LIBRARY(pbd
+ src/datapoints.cpp
+ src/demonstration.cpp
+ src/tpdemonstration.cpp
+ src/taskparameters.cpp
+ src/mvn.cpp
+ src/gmm.cpp
+ src/gmr.cpp
+ src/tpgmm.cpp
+ src/lqr.cpp
+ src/hmm.cpp
+ src/hsmm.cpp
+)
+
+#Uncomment for debugging
+#add_definitions(-g)
+
+################################################################################
+### samples
+################################################################################
+OPTION(PBDLIB_BUILD_TEST "Build PbDLib examples" 0)
+IF (PBDLIB_BUILD_TEST)
+ ADD_SUBDIRECTORY(examples)
+ENDIF ()
+
+target_link_libraries(pbd ${ARMADILLO_LIBRARIES})
+
+################################################################################
+### install
+################################################################################
+set(ROOT_INSTALL_LIBDIR lib)
+set(ROOT_INSTALL_INCLUDEDIR include)
+file(GLOB headers ${CMAKE_CURRENT_SOURCE_DIR}/include/pbdlib/*.h)
+
+install(TARGETS pbd DESTINATION ${ROOT_INSTALL_LIBDIR})
+install(FILES ${headers} DESTINATION ${ROOT_INSTALL_INCLUDEDIR}/pbdlib/)
+
diff --git a/README.md b/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..b4e69f9ed4d409e49e1255045f49de1e75357613
--- /dev/null
+++ b/README.md
@@ -0,0 +1,29 @@
+## PbDLib C++ Library
+
+### Dependencies
+
+PbDLib requires Armadillo for linear algebra operations, which runs faster if Lapack is also installed. PbDLib can be compiled with CMake.
+```
+sudo apt-get install cmake liblapack3 liblapack-dev libarmadillo4 libarmadillo-dev
+```
+
+### Installation instructions
+
+```
+cd pbdlib-sandbox
+mkdir build
+cd build
+cmake ..
+make
+sudo make install
+```
+
+A GUI can be used after installation of the library:
+https://gitlab.idiap.ch/rli/pbdlib_gui
+
+### Test (assuming a build folder was created for cmake install)
+
+```
+cd examples
+./test_gmm
+```
diff --git a/data/HSMM_test_durMu.txt b/data/HSMM_test_durMu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a3579fb62119aad84f16ebe5c9e0a7d31dc1e319
--- /dev/null
+++ b/data/HSMM_test_durMu.txt
@@ -0,0 +1 @@
+32.5 34 33.75
diff --git a/data/HSMM_test_durSigma.txt b/data/HSMM_test_durSigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2bd29ebc9006c32469db047bb5bab022a96df13f
--- /dev/null
+++ b/data/HSMM_test_durSigma.txt
@@ -0,0 +1 @@
+1.3333 3 4.5833
diff --git a/data/HSMM_test_mu.txt b/data/HSMM_test_mu.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b2fc22f387a772f2d94e527f9e25842382c66f1
--- /dev/null
+++ b/data/HSMM_test_mu.txt
@@ -0,0 +1,4 @@
+-17.283 -24.539 -17.462
+40.324 18.767 -0.6093
+113.93 -104.75 112.44
+-10.878 -102.07 -5.8334
diff --git a/data/HSMM_test_priors.txt b/data/HSMM_test_priors.txt
new file mode 100644
index 0000000000000000000000000000000000000000..427172728db3b671b77957ff24c2ecfca154e0ae
--- /dev/null
+++ b/data/HSMM_test_priors.txt
@@ -0,0 +1 @@
+1 0 0
diff --git a/data/HSMM_test_sigma.txt b/data/HSMM_test_sigma.txt
new file mode 100644
index 0000000000000000000000000000000000000000..251d99797217b5bd37459422fb35aa2e8c415b4b
--- /dev/null
+++ b/data/HSMM_test_sigma.txt
@@ -0,0 +1,4 @@
+179.07 -2.9226 -316.98 -256.51 141.99 116.77 -384.41 20.704 158.3 2.0917 -101.99 206.92
+-2.9226 7.5428 58.59 17.033 116.77 107.01 -409.21 -2.0213 2.0917 0.77987 -21.862 -6.8527
+-316.98 58.59 5311.3 1290.9 -384.41 -409.21 2961.5 247.41 -101.99 -21.862 1623 358.92
+-256.51 17.033 1290.9 716.09 20.704 -2.0213 247.41 270.36 206.92 -6.8527 358.92 530.29
diff --git a/data/HSMM_test_trans.txt b/data/HSMM_test_trans.txt
new file mode 100644
index 0000000000000000000000000000000000000000..93b786e0f50e2589accdd0ded0050831798c5f82
--- /dev/null
+++ b/data/HSMM_test_trans.txt
@@ -0,0 +1,3 @@
+3.231e-11 1 0
+0 3.3928e-11 1
+0 0 1
diff --git a/data/data_txyz.txt b/data/data_txyz.txt
new file mode 100644
index 0000000000000000000000000000000000000000..bf192d366db8ac7026cbd2f2226ddf0006c3396a
--- /dev/null
+++ b/data/data_txyz.txt
@@ -0,0 +1,4 @@
+ 2.558000000000e+00 2.588000000000e+00 2.618000000000e+00 2.648000000000e+00 2.678000000000e+00 2.708000000000e+00 2.738000000000e+00 2.768000000000e+00 2.798000000000e+00 2.828000000000e+00 2.858000000000e+00 2.888000000000e+00 2.918000000000e+00 2.948000000000e+00 2.978000000000e+00 3.008000000000e+00 3.038000000000e+00 3.068000000000e+00 3.098000000000e+00 3.128000000000e+00 3.158000000000e+00 3.188000000000e+00 3.218000000000e+00 3.248000000000e+00 3.278000000000e+00 3.308000000000e+00 3.338000000000e+00 3.368000000000e+00 3.398000000000e+00 3.428000000000e+00 3.458000000000e+00 3.488000000000e+00 3.518000000000e+00 3.548000000000e+00 3.578000000000e+00 3.608000000000e+00 3.638000000000e+00 3.668000000000e+00 3.698000000000e+00 3.728000000000e+00 3.758000000000e+00 3.788000000000e+00 3.818000000000e+00 3.848000000000e+00 3.878000000000e+00 3.908000000000e+00 3.938000000000e+00 3.968000000000e+00 3.998000000000e+00 4.028000000000e+00 4.058000000000e+00 4.088000000000e+00 4.118000000000e+00 4.148000000000e+00 4.178000000000e+00 4.208000000000e+00 4.238000000000e+00 4.268000000000e+00 4.298000000000e+00 4.328000000000e+00 4.358000000000e+00 4.388000000000e+00 4.418000000000e+00 4.448000000000e+00 4.478000000000e+00 4.508000000000e+00 4.538000000000e+00 4.568000000000e+00 4.598000000000e+00 4.628000000000e+00 4.658000000000e+00 4.688000000000e+00 4.718000000000e+00 4.748000000000e+00 4.778000000000e+00 4.808000000000e+00 4.838000000000e+00 4.868000000000e+00 4.898000000000e+00 4.928000000000e+00 4.958000000000e+00 4.988000000000e+00 5.018000000000e+00 5.048000000000e+00 5.078000000000e+00 5.108000000000e+00 5.138000000000e+00 5.168000000000e+00 5.198000000000e+00 5.228000000000e+00 5.258000000000e+00 5.288000000000e+00 5.318000000000e+00 5.348000000000e+00 5.378000000000e+00 5.408000000000e+00 5.438000000000e+00 5.468000000000e+00 5.498000000000e+00 5.528000000000e+00 5.558000000000e+00 5.588000000000e+00 5.618000000000e+00 5.648000000000e+00 5.678000000000e+00 5.708000000000e+00 5.738000000000e+00 5.768000000000e+00 5.798000000000e+00 5.828000000000e+00 5.858000000000e+00 5.888000000000e+00 5.918000000000e+00 5.948000000000e+00 5.978000000000e+00 6.008000000000e+00 6.038000000000e+00 6.068000000000e+00 6.098000000000e+00 6.128000000000e+00 6.158000000000e+00 6.188000000000e+00 6.218000000000e+00 6.248000000000e+00 6.278000000000e+00 6.308000000000e+00 6.338000000000e+00 6.368000000000e+00 6.398000000000e+00 6.428000000000e+00 6.458000000000e+00 6.488000000000e+00 6.518000000000e+00 6.548000000000e+00 6.578000000000e+00 6.608000000000e+00 6.638000000000e+00 6.668000000000e+00 6.698000000000e+00 6.728000000000e+00 6.758000000000e+00 6.788000000000e+00 6.818000000000e+00 6.848000000000e+00 6.878000000000e+00 6.908000000000e+00 6.938000000000e+00 6.968000000000e+00 6.998000000000e+00 7.028000000000e+00 7.058000000000e+00 7.088000000000e+00 7.118000000000e+00 7.148000000000e+00 7.178000000000e+00 7.208000000000e+00 7.238000000000e+00 7.268000000000e+00 7.298000000000e+00 7.328000000000e+00 7.358000000000e+00 7.388000000000e+00 7.418000000000e+00 7.448000000000e+00 7.478000000000e+00 7.508000000000e+00 7.538000000000e+00 7.568000000000e+00 7.598000000000e+00 7.628000000000e+00 7.658000000000e+00 7.688000000000e+00 7.718000000000e+00 7.748000000000e+00 7.778000000000e+00 7.808000000000e+00 7.838000000000e+00 7.868000000000e+00 7.898000000000e+00 7.928000000000e+00 7.958000000000e+00 7.988000000000e+00 8.018000000000e+00 8.048000000000e+00 8.078000000000e+00 8.108000000000e+00 8.138000000000e+00 8.168000000000e+00 8.198000000000e+00 8.228000000000e+00 8.258000000000e+00 8.288000000000e+00 8.318000000000e+00 8.348000000000e+00 8.378000000000e+00 8.408000000000e+00 8.438000000000e+00 8.468000000000e+00 8.498000000000e+00 8.528000000000e+00
+ -4.138280000000e-02 -4.128380000000e-02 -4.118460000000e-02 -4.111850000000e-02 -4.109110000000e-02 -4.122540000000e-02 -4.119490000000e-02 -4.109150000000e-02 -4.098660000000e-02 -4.093660000000e-02 -4.081370000000e-02 -4.081520000000e-02 -4.085460000000e-02 -4.085480000000e-02 -4.080270000000e-02 -4.087280000000e-02 -4.082000000000e-02 -4.082000000000e-02 -4.085010000000e-02 -4.079540000000e-02 -4.091310000000e-02 -4.112710000000e-02 -4.124570000000e-02 -4.148960000000e-02 -4.176360000000e-02 -4.201620000000e-02 -4.237480000000e-02 -4.270680000000e-02 -4.314970000000e-02 -4.384060000000e-02 -4.463840000000e-02 -4.555840000000e-02 -4.670290000000e-02 -4.799070000000e-02 -4.947540000000e-02 -5.110290000000e-02 -5.276330000000e-02 -5.465380000000e-02 -5.675500000000e-02 -5.900450000000e-02 -6.144370000000e-02 -6.413990000000e-02 -6.696350000000e-02 -6.986060000000e-02 -7.277410000000e-02 -7.567440000000e-02 -7.858980000000e-02 -8.150870000000e-02 -8.439960000000e-02 -8.721500000000e-02 -9.004500000000e-02 -9.273480000000e-02 -9.543380000000e-02 -9.809190000000e-02 -1.008210000000e-01 -1.036000000000e-01 -1.063640000000e-01 -1.091690000000e-01 -1.120610000000e-01 -1.149260000000e-01 -1.179100000000e-01 -1.209620000000e-01 -1.239490000000e-01 -1.270040000000e-01 -1.299380000000e-01 -1.328710000000e-01 -1.358440000000e-01 -1.388130000000e-01 -1.420140000000e-01 -1.452510000000e-01 -1.484770000000e-01 -1.518810000000e-01 -1.551660000000e-01 -1.585760000000e-01 -1.621550000000e-01 -1.656510000000e-01 -1.691000000000e-01 -1.728500000000e-01 -1.767700000000e-01 -1.806790000000e-01 -1.845660000000e-01 -1.886760000000e-01 -1.930510000000e-01 -1.975430000000e-01 -2.020890000000e-01 -2.066010000000e-01 -2.110510000000e-01 -2.154470000000e-01 -2.198470000000e-01 -2.242830000000e-01 -2.287530000000e-01 -2.333150000000e-01 -2.378820000000e-01 -2.425840000000e-01 -2.472870000000e-01 -2.520370000000e-01 -2.569040000000e-01 -2.618170000000e-01 -2.668180000000e-01 -2.719420000000e-01 -2.771900000000e-01 -2.824270000000e-01 -2.876570000000e-01 -2.928140000000e-01 -2.979500000000e-01 -3.030480000000e-01 -3.081280000000e-01 -3.131070000000e-01 -3.180290000000e-01 -3.228730000000e-01 -3.276080000000e-01 -3.322330000000e-01 -3.368240000000e-01 -3.413440000000e-01 -3.459410000000e-01 -3.504940000000e-01 -3.549890000000e-01 -3.592840000000e-01 -3.634110000000e-01 -3.674900000000e-01 -3.715730000000e-01 -3.754790000000e-01 -3.791470000000e-01 -3.826810000000e-01 -3.860500000000e-01 -3.892400000000e-01 -3.922160000000e-01 -3.949410000000e-01 -3.974520000000e-01 -3.997970000000e-01 -4.019670000000e-01 -4.039920000000e-01 -4.059120000000e-01 -4.077690000000e-01 -4.094900000000e-01 -4.111720000000e-01 -4.127280000000e-01 -4.141410000000e-01 -4.154280000000e-01 -4.166760000000e-01 -4.177700000000e-01 -4.187070000000e-01 -4.195150000000e-01 -4.201580000000e-01 -4.206350000000e-01 -4.209200000000e-01 -4.210500000000e-01 -4.212330000000e-01 -4.213320000000e-01 -4.215270000000e-01 -4.217630000000e-01 -4.222220000000e-01 -4.228060000000e-01 -4.235410000000e-01 -4.242920000000e-01 -4.249860000000e-01 -4.254730000000e-01 -4.258540000000e-01 -4.260700000000e-01 -4.261890000000e-01 -4.262420000000e-01 -4.261060000000e-01 -4.258510000000e-01 -4.255640000000e-01 -4.252600000000e-01 -4.249640000000e-01 -4.247250000000e-01 -4.246590000000e-01 -4.247490000000e-01 -4.248800000000e-01 -4.250620000000e-01 -4.251540000000e-01 -4.253910000000e-01 -4.258290000000e-01 -4.262490000000e-01 -4.267580000000e-01 -4.273600000000e-01 -4.280980000000e-01 -4.288740000000e-01 -4.295480000000e-01 -4.301330000000e-01 -4.304160000000e-01 -4.304660000000e-01 -4.304140000000e-01 -4.301310000000e-01 -4.298390000000e-01 -4.293690000000e-01 -4.290000000000e-01 -4.286180000000e-01 -4.283170000000e-01 -4.280460000000e-01 -4.280630000000e-01 -4.282210000000e-01 -4.285090000000e-01 -4.288780000000e-01 -4.291940000000e-01 -4.295450000000e-01 -4.299310000000e-01 -4.302640000000e-01 -4.304370000000e-01
+ -5.470480000000e-01 -5.475560000000e-01 -5.481030000000e-01 -5.487480000000e-01 -5.493600000000e-01 -5.499890000000e-01 -5.507110000000e-01 -5.513950000000e-01 -5.522130000000e-01 -5.530300000000e-01 -5.538640000000e-01 -5.547440000000e-01 -5.557010000000e-01 -5.567200000000e-01 -5.577410000000e-01 -5.587940000000e-01 -5.599220000000e-01 -5.610460000000e-01 -5.622010000000e-01 -5.633260000000e-01 -5.643390000000e-01 -5.653070000000e-01 -5.661110000000e-01 -5.670120000000e-01 -5.679550000000e-01 -5.689480000000e-01 -5.699700000000e-01 -5.709140000000e-01 -5.716770000000e-01 -5.723790000000e-01 -5.730960000000e-01 -5.738260000000e-01 -5.746060000000e-01 -5.753780000000e-01 -5.760880000000e-01 -5.767600000000e-01 -5.774490000000e-01 -5.780960000000e-01 -5.786930000000e-01 -5.792780000000e-01 -5.797710000000e-01 -5.802710000000e-01 -5.807040000000e-01 -5.810770000000e-01 -5.814400000000e-01 -5.817660000000e-01 -5.820170000000e-01 -5.822400000000e-01 -5.824390000000e-01 -5.825710000000e-01 -5.826880000000e-01 -5.827830000000e-01 -5.828460000000e-01 -5.828550000000e-01 -5.828150000000e-01 -5.827290000000e-01 -5.826320000000e-01 -5.824680000000e-01 -5.822770000000e-01 -5.820650000000e-01 -5.818050000000e-01 -5.814990000000e-01 -5.811170000000e-01 -5.807800000000e-01 -5.803840000000e-01 -5.799910000000e-01 -5.795700000000e-01 -5.790970000000e-01 -5.785830000000e-01 -5.780320000000e-01 -5.774140000000e-01 -5.767530000000e-01 -5.760510000000e-01 -5.753140000000e-01 -5.744910000000e-01 -5.736740000000e-01 -5.728100000000e-01 -5.717760000000e-01 -5.706800000000e-01 -5.695310000000e-01 -5.683300000000e-01 -5.670160000000e-01 -5.655920000000e-01 -5.640050000000e-01 -5.623920000000e-01 -5.607500000000e-01 -5.591230000000e-01 -5.574120000000e-01 -5.557230000000e-01 -5.539420000000e-01 -5.521180000000e-01 -5.502060000000e-01 -5.482140000000e-01 -5.461790000000e-01 -5.440680000000e-01 -5.418440000000e-01 -5.395900000000e-01 -5.372250000000e-01 -5.347190000000e-01 -5.321310000000e-01 -5.294550000000e-01 -5.266450000000e-01 -5.238390000000e-01 -5.209370000000e-01 -5.180510000000e-01 -5.150470000000e-01 -5.120160000000e-01 -5.089810000000e-01 -5.059080000000e-01 -5.028280000000e-01 -4.997460000000e-01 -4.966700000000e-01 -4.935760000000e-01 -4.903780000000e-01 -4.870790000000e-01 -4.837260000000e-01 -4.803210000000e-01 -4.769300000000e-01 -4.735900000000e-01 -4.702860000000e-01 -4.668170000000e-01 -4.634490000000e-01 -4.601970000000e-01 -4.569380000000e-01 -4.537370000000e-01 -4.506480000000e-01 -4.476730000000e-01 -4.448370000000e-01 -4.421060000000e-01 -4.394290000000e-01 -4.368390000000e-01 -4.342900000000e-01 -4.317770000000e-01 -4.292560000000e-01 -4.267870000000e-01 -4.242550000000e-01 -4.217530000000e-01 -4.192480000000e-01 -4.168140000000e-01 -4.143290000000e-01 -4.118770000000e-01 -4.095260000000e-01 -4.072010000000e-01 -4.049520000000e-01 -4.028830000000e-01 -4.010130000000e-01 -3.992910000000e-01 -3.977090000000e-01 -3.963780000000e-01 -3.952120000000e-01 -3.944030000000e-01 -3.939610000000e-01 -3.937110000000e-01 -3.936850000000e-01 -3.937510000000e-01 -3.938700000000e-01 -3.939440000000e-01 -3.938900000000e-01 -3.937050000000e-01 -3.934050000000e-01 -3.929420000000e-01 -3.923180000000e-01 -3.915000000000e-01 -3.905960000000e-01 -3.895940000000e-01 -3.885580000000e-01 -3.876200000000e-01 -3.868580000000e-01 -3.862240000000e-01 -3.857820000000e-01 -3.853890000000e-01 -3.849920000000e-01 -3.848210000000e-01 -3.848840000000e-01 -3.850920000000e-01 -3.853750000000e-01 -3.858430000000e-01 -3.864830000000e-01 -3.871970000000e-01 -3.879600000000e-01 -3.885290000000e-01 -3.887750000000e-01 -3.889270000000e-01 -3.888950000000e-01 -3.888480000000e-01 -3.885840000000e-01 -3.883660000000e-01 -3.879370000000e-01 -3.876550000000e-01 -3.873570000000e-01 -3.871920000000e-01 -3.871290000000e-01 -3.872690000000e-01 -3.875000000000e-01 -3.878450000000e-01 -3.882720000000e-01 -3.886360000000e-01 -3.890310000000e-01 -3.892670000000e-01 -3.893340000000e-01
+ -2.514030000000e-01 -2.503000000000e-01 -2.490490000000e-01 -2.477160000000e-01 -2.462920000000e-01 -2.448610000000e-01 -2.432370000000e-01 -2.415620000000e-01 -2.397740000000e-01 -2.378910000000e-01 -2.358950000000e-01 -2.337450000000e-01 -2.314530000000e-01 -2.289930000000e-01 -2.264170000000e-01 -2.237300000000e-01 -2.209010000000e-01 -2.179550000000e-01 -2.148730000000e-01 -2.117570000000e-01 -2.086180000000e-01 -2.053530000000e-01 -2.021090000000e-01 -1.987730000000e-01 -1.954750000000e-01 -1.921470000000e-01 -1.888380000000e-01 -1.856150000000e-01 -1.824310000000e-01 -1.792880000000e-01 -1.762260000000e-01 -1.731780000000e-01 -1.701980000000e-01 -1.672030000000e-01 -1.642840000000e-01 -1.614020000000e-01 -1.583950000000e-01 -1.553480000000e-01 -1.523420000000e-01 -1.492320000000e-01 -1.461700000000e-01 -1.431500000000e-01 -1.400680000000e-01 -1.369310000000e-01 -1.338380000000e-01 -1.307780000000e-01 -1.277800000000e-01 -1.248810000000e-01 -1.220200000000e-01 -1.192180000000e-01 -1.165110000000e-01 -1.138970000000e-01 -1.113670000000e-01 -1.089320000000e-01 -1.066310000000e-01 -1.044120000000e-01 -1.022080000000e-01 -9.996620000000e-02 -9.782950000000e-02 -9.574580000000e-02 -9.370660000000e-02 -9.168980000000e-02 -8.970820000000e-02 -8.778180000000e-02 -8.589490000000e-02 -8.404420000000e-02 -8.221080000000e-02 -8.032250000000e-02 -7.841050000000e-02 -7.655680000000e-02 -7.475370000000e-02 -7.300030000000e-02 -7.138830000000e-02 -6.981720000000e-02 -6.835770000000e-02 -6.707550000000e-02 -6.587930000000e-02 -6.482570000000e-02 -6.390060000000e-02 -6.322260000000e-02 -6.275050000000e-02 -6.246190000000e-02 -6.231650000000e-02 -6.231340000000e-02 -6.231340000000e-02 -6.231340000000e-02 -6.228920000000e-02 -6.229950000000e-02 -6.228920000000e-02 -6.228980000000e-02 -6.227520000000e-02 -6.227090000000e-02 -6.229950000000e-02 -6.230310000000e-02 -6.228920000000e-02 -6.231840000000e-02 -6.232150000000e-02 -6.230310000000e-02 -6.233240000000e-02 -6.233240000000e-02 -6.232150000000e-02 -6.235010000000e-02 -6.233980000000e-02 -6.238240000000e-02 -6.237210000000e-02 -6.241470000000e-02 -6.247930000000e-02 -6.252990000000e-02 -6.257680000000e-02 -6.264070000000e-02 -6.271930000000e-02 -6.282580000000e-02 -6.303730000000e-02 -6.341230000000e-02 -6.392100000000e-02 -6.457260000000e-02 -6.543600000000e-02 -6.652940000000e-02 -6.771790000000e-02 -6.896010000000e-02 -7.039470000000e-02 -7.191040000000e-02 -7.353000000000e-02 -7.529710000000e-02 -7.720660000000e-02 -7.925710000000e-02 -8.141310000000e-02 -8.375550000000e-02 -8.629870000000e-02 -8.909270000000e-02 -9.201760000000e-02 -9.515990000000e-02 -9.837280000000e-02 -1.018350000000e-01 -1.054360000000e-01 -1.091750000000e-01 -1.130820000000e-01 -1.171560000000e-01 -1.213540000000e-01 -1.256670000000e-01 -1.301290000000e-01 -1.347130000000e-01 -1.394150000000e-01 -1.443240000000e-01 -1.493140000000e-01 -1.544690000000e-01 -1.596990000000e-01 -1.648670000000e-01 -1.700940000000e-01 -1.751760000000e-01 -1.802100000000e-01 -1.849830000000e-01 -1.896530000000e-01 -1.941550000000e-01 -1.984960000000e-01 -2.027890000000e-01 -2.069810000000e-01 -2.110880000000e-01 -2.152030000000e-01 -2.192400000000e-01 -2.231620000000e-01 -2.269910000000e-01 -2.307290000000e-01 -2.343060000000e-01 -2.377300000000e-01 -2.409770000000e-01 -2.441080000000e-01 -2.469330000000e-01 -2.495310000000e-01 -2.519620000000e-01 -2.541220000000e-01 -2.560980000000e-01 -2.579000000000e-01 -2.595560000000e-01 -2.611170000000e-01 -2.625230000000e-01 -2.639600000000e-01 -2.653660000000e-01 -2.668090000000e-01 -2.683740000000e-01 -2.698980000000e-01 -2.716740000000e-01 -2.735650000000e-01 -2.756140000000e-01 -2.779150000000e-01 -2.802500000000e-01 -2.827200000000e-01 -2.852560000000e-01 -2.877360000000e-01 -2.901260000000e-01 -2.924020000000e-01 -2.944650000000e-01 -2.964280000000e-01 -2.982830000000e-01 -2.999970000000e-01 -3.015580000000e-01 -3.030000000000e-01 -3.043260000000e-01 -3.055140000000e-01 -3.065700000000e-01
diff --git a/data/pgmm/Data01.txt b/data/pgmm/Data01.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1ada89d12d87c72c94a063adc9515fb7eb11bf16
--- /dev/null
+++ b/data/pgmm/Data01.txt
@@ -0,0 +1,200 @@
+ 1.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 2.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 3.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 4.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 5.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 6.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 7.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 8.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 9.0000000e-02 -8.0985915e-01 -7.9732739e-01
+ 1.0000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.1000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.2000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.3000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.4000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.5000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.6000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.7000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.8000000e-01 -8.0985915e-01 -7.9732739e-01
+ 1.9000000e-01 -8.0985915e-01 -7.9732739e-01
+ 2.0000000e-01 -8.0985915e-01 -7.9732739e-01
+ 2.1000000e-01 -8.0985915e-01 -7.9732739e-01
+ 2.2000000e-01 -8.0985915e-01 -7.9732739e-01
+ 2.3000000e-01 -8.0985915e-01 -7.9732739e-01
+ 2.4000000e-01 -8.0985915e-01 -7.9732731e-01
+ 2.5000000e-01 -8.0985915e-01 -7.9732663e-01
+ 2.6000000e-01 -8.0985915e-01 -7.9732257e-01
+ 2.7000000e-01 -8.0985915e-01 -7.9701514e-01
+ 2.8000000e-01 -8.0985915e-01 -7.9671056e-01
+ 2.9000000e-01 -8.0985915e-01 -7.9624689e-01
+ 3.0000000e-01 -8.0985915e-01 -7.9564155e-01
+ 3.1000000e-01 -8.0985915e-01 -7.9503423e-01
+ 3.2000000e-01 -8.0985915e-01 -7.9412461e-01
+ 3.3000000e-01 -8.0985915e-01 -7.9321288e-01
+ 3.4000000e-01 -8.0985914e-01 -7.9200985e-01
+ 3.5000000e-01 -8.0985900e-01 -7.9049191e-01
+ 3.6000000e-01 -8.0985682e-01 -7.8884567e-01
+ 3.7000000e-01 -8.0982504e-01 -7.8689284e-01
+ 3.8000000e-01 -8.1016849e-01 -7.8474658e-01
+ 3.9000000e-01 -8.1049382e-01 -7.8232046e-01
+ 4.0000000e-01 -8.1081772e-01 -7.7986147e-01
+ 4.1000000e-01 -8.1114151e-01 -7.7731340e-01
+ 4.2000000e-01 -8.1146529e-01 -7.7443780e-01
+ 4.3000000e-01 -8.1178907e-01 -7.7144076e-01
+ 4.4000000e-01 -8.1211286e-01 -7.6811585e-01
+ 4.5000000e-01 -8.1243664e-01 -7.6473636e-01
+ 4.6000000e-01 -8.1276042e-01 -7.6128863e-01
+ 4.7000000e-01 -8.1308420e-01 -7.5751106e-01
+ 4.8000000e-01 -8.1340798e-01 -7.5359952e-01
+ 4.9000000e-01 -8.1373176e-01 -7.4954152e-01
+ 5.0000000e-01 -8.1405555e-01 -7.4535161e-01
+ 5.1000000e-01 -8.1437933e-01 -7.4089499e-01
+ 5.2000000e-01 -8.1470311e-01 -7.3621773e-01
+ 5.3000000e-01 -8.1502689e-01 -7.3147160e-01
+ 5.4000000e-01 -8.1535067e-01 -7.2655177e-01
+ 5.5000000e-01 -8.1567445e-01 -7.2147090e-01
+ 5.6000000e-01 -8.1599824e-01 -7.1671791e-01
+ 5.7000000e-01 -8.1632202e-01 -7.1179394e-01
+ 5.8000000e-01 -8.1664580e-01 -7.0687434e-01
+ 5.9000000e-01 -8.1696958e-01 -7.0194267e-01
+ 6.0000000e-01 -8.1729336e-01 -6.9685784e-01
+ 6.1000000e-01 -8.1761715e-01 -6.9190140e-01
+ 6.2000000e-01 -8.1794093e-01 -6.8681056e-01
+ 6.3000000e-01 -8.1826472e-01 -6.8186475e-01
+ 6.4000000e-01 -8.1858865e-01 -6.7693140e-01
+ 6.5000000e-01 -8.1891461e-01 -6.7181273e-01
+ 6.6000000e-01 -8.1927017e-01 -6.6685083e-01
+ 6.7000000e-01 -8.1925050e-01 -6.6177948e-01
+ 6.8000000e-01 -8.1924896e-01 -6.5668155e-01
+ 6.9000000e-01 -8.1924884e-01 -6.5147312e-01
+ 7.0000000e-01 -8.1924883e-01 -6.4619305e-01
+ 7.1000000e-01 -8.1924883e-01 -6.4107867e-01
+ 7.2000000e-01 -8.1924883e-01 -6.3609836e-01
+ 7.3000000e-01 -8.1924883e-01 -6.3127416e-01
+ 7.4000000e-01 -8.1924890e-01 -6.2637877e-01
+ 7.5000000e-01 -8.1924994e-01 -6.2140812e-01
+ 7.6000000e-01 -8.1926525e-01 -6.1675046e-01
+ 7.7000000e-01 -8.1909426e-01 -6.1193052e-01
+ 7.8000000e-01 -8.1893165e-01 -6.0713775e-01
+ 7.9000000e-01 -8.1876971e-01 -6.0227530e-01
+ 8.0000000e-01 -8.1860781e-01 -5.9752182e-01
+ 8.1000000e-01 -8.1844592e-01 -5.9283302e-01
+ 8.2000000e-01 -8.1828400e-01 -5.8803937e-01
+ 8.3000000e-01 -8.1812178e-01 -5.8336205e-01
+ 8.4000000e-01 -8.1795529e-01 -5.7864090e-01
+ 8.5000000e-01 -8.1770524e-01 -5.7356121e-01
+ 8.6000000e-01 -8.1738575e-01 -5.6822999e-01
+ 8.7000000e-01 -8.1706228e-01 -5.6295315e-01
+ 8.8000000e-01 -8.1673853e-01 -5.5750006e-01
+ 8.9000000e-01 -8.1641475e-01 -5.5208236e-01
+ 9.0000000e-01 -8.1609096e-01 -5.4652006e-01
+ 9.1000000e-01 -8.1576718e-01 -5.4078981e-01
+ 9.2000000e-01 -8.1544340e-01 -5.3490218e-01
+ 9.3000000e-01 -8.1511962e-01 -5.2901046e-01
+ 9.4000000e-01 -8.1479587e-01 -5.2308339e-01
+ 9.5000000e-01 -8.1447246e-01 -5.1682139e-01
+ 9.6000000e-01 -8.1415347e-01 -5.1029716e-01
+ 9.7000000e-01 -8.1389373e-01 -5.0359239e-01
+ 9.8000000e-01 -8.1387184e-01 -4.9643560e-01
+ 9.9000000e-01 -8.1422558e-01 -4.8888262e-01
+ 1.0000000e+00 -8.1459539e-01 -4.8118030e-01
+ 1.0100000e+00 -8.1527369e-01 -4.7314964e-01
+ 1.0200000e+00 -8.1626940e-01 -4.6467383e-01
+ 1.0300000e+00 -8.1759162e-01 -4.5588506e-01
+ 1.0400000e+00 -8.1938125e-01 -4.4630174e-01
+ 1.0500000e+00 -8.2147599e-01 -4.3629473e-01
+ 1.0600000e+00 -8.2407415e-01 -4.2566992e-01
+ 1.0700000e+00 -8.2698660e-01 -4.1410151e-01
+ 1.0800000e+00 -8.3021505e-01 -4.0212887e-01
+ 1.0900000e+00 -8.3345174e-01 -3.8956860e-01
+ 1.1000000e+00 -8.3668944e-01 -3.7578888e-01
+ 1.1100000e+00 -8.3992727e-01 -3.6094636e-01
+ 1.1200000e+00 -8.4316542e-01 -3.4557505e-01
+ 1.1300000e+00 -8.4640784e-01 -3.2959604e-01
+ 1.1400000e+00 -8.4973387e-01 -3.1336105e-01
+ 1.1500000e+00 -8.5313005e-01 -2.9712267e-01
+ 1.1600000e+00 -8.5654077e-01 -2.8061971e-01
+ 1.1700000e+00 -8.5964507e-01 -2.6369291e-01
+ 1.1800000e+00 -8.6272266e-01 -2.4665200e-01
+ 1.1900000e+00 -8.6579393e-01 -2.2940095e-01
+ 1.2000000e+00 -8.6875575e-01 -2.1165413e-01
+ 1.2100000e+00 -8.7158537e-01 -1.9386289e-01
+ 1.2200000e+00 -8.7417836e-01 -1.7589023e-01
+ 1.2300000e+00 -8.7649000e-01 -1.5790873e-01
+ 1.2400000e+00 -8.7866349e-01 -1.3987923e-01
+ 1.2500000e+00 -8.8069538e-01 -1.2223612e-01
+ 1.2600000e+00 -8.8235350e-01 -1.0449712e-01
+ 1.2700000e+00 -8.8334985e-01 -8.6936264e-02
+ 1.2800000e+00 -8.8361892e-01 -6.9364808e-02
+ 1.2900000e+00 -8.8364779e-01 -5.1801712e-02
+ 1.3000000e+00 -8.8330772e-01 -3.4482358e-02
+ 1.3100000e+00 -8.8241089e-01 -1.7776761e-02
+ 1.3200000e+00 -8.8090586e-01 -1.5096533e-03
+ 1.3300000e+00 -8.7885115e-01 1.3819423e-02
+ 1.3400000e+00 -8.7622614e-01 2.8707949e-02
+ 1.3500000e+00 -8.7291635e-01 4.2682432e-02
+ 1.3600000e+00 -8.6867300e-01 5.5594019e-02
+ 1.3700000e+00 -8.6341526e-01 6.7899799e-02
+ 1.3800000e+00 -8.5733624e-01 7.9460191e-02
+ 1.3900000e+00 -8.5080345e-01 8.9645125e-02
+ 1.4000000e+00 -8.4399450e-01 9.8592766e-02
+ 1.4100000e+00 -8.3685566e-01 1.0694580e-01
+ 1.4200000e+00 -8.2922416e-01 1.1480942e-01
+ 1.4300000e+00 -8.2110889e-01 1.2239527e-01
+ 1.4400000e+00 -8.1268032e-01 1.2955484e-01
+ 1.4500000e+00 -8.0392108e-01 1.3664312e-01
+ 1.4600000e+00 -7.9531205e-01 1.4297955e-01
+ 1.4700000e+00 -7.8656767e-01 1.4938945e-01
+ 1.4800000e+00 -7.7783011e-01 1.5526978e-01
+ 1.4900000e+00 -7.6920211e-01 1.6034971e-01
+ 1.5000000e+00 -7.6070630e-01 1.6508155e-01
+ 1.5100000e+00 -7.5244713e-01 1.6932540e-01
+ 1.5200000e+00 -7.4446928e-01 1.7321069e-01
+ 1.5300000e+00 -7.3662923e-01 1.7655189e-01
+ 1.5400000e+00 -7.2892635e-01 1.7988540e-01
+ 1.5500000e+00 -7.2153769e-01 1.8261617e-01
+ 1.5600000e+00 -7.1456861e-01 1.8509107e-01
+ 1.5700000e+00 -7.0782568e-01 1.8716674e-01
+ 1.5800000e+00 -7.0106689e-01 1.8890754e-01
+ 1.5900000e+00 -6.9371648e-01 1.9112322e-01
+ 1.6000000e+00 -6.8608045e-01 1.9397107e-01
+ 1.6100000e+00 -6.7782487e-01 1.9709268e-01
+ 1.6200000e+00 -6.6914963e-01 2.0049927e-01
+ 1.6300000e+00 -6.6025647e-01 2.0399113e-01
+ 1.6400000e+00 -6.5143314e-01 2.0785683e-01
+ 1.6500000e+00 -6.4298363e-01 2.1166716e-01
+ 1.6600000e+00 -6.3480294e-01 2.1584583e-01
+ 1.6700000e+00 -6.2729350e-01 2.2000315e-01
+ 1.6800000e+00 -6.1980563e-01 2.2428476e-01
+ 1.6900000e+00 -6.1224527e-01 2.2870396e-01
+ 1.7000000e+00 -6.0441593e-01 2.3351359e-01
+ 1.7100000e+00 -5.9682539e-01 2.3806914e-01
+ 1.7200000e+00 -5.8950013e-01 2.4250348e-01
+ 1.7300000e+00 -5.8218191e-01 2.4701313e-01
+ 1.7400000e+00 -5.7466447e-01 2.5177900e-01
+ 1.7500000e+00 -5.6718789e-01 2.5687946e-01
+ 1.7600000e+00 -5.5953407e-01 2.6164217e-01
+ 1.7700000e+00 -5.5177861e-01 2.6640368e-01
+ 1.7800000e+00 -5.4388583e-01 2.7116521e-01
+ 1.7900000e+00 -5.3626910e-01 2.7592675e-01
+ 1.8000000e+00 -5.2865971e-01 2.8068834e-01
+ 1.8100000e+00 -5.2105081e-01 2.8545066e-01
+ 1.8200000e+00 -5.1344194e-01 2.9022433e-01
+ 1.8300000e+00 -5.0583309e-01 2.9482698e-01
+ 1.8400000e+00 -4.9822453e-01 2.9944947e-01
+ 1.8500000e+00 -4.9062032e-01 3.0391061e-01
+ 1.8600000e+00 -4.8314161e-01 3.0820346e-01
+ 1.8700000e+00 -4.7260761e-01 3.1433026e-01
+ 1.8800000e+00 -4.6340785e-01 3.1901122e-01
+ 1.8900000e+00 -4.5559882e-01 3.2333458e-01
+ 1.9000000e+00 -4.4809979e-01 3.2740764e-01
+ 1.9100000e+00 -4.4090948e-01 3.3133089e-01
+ 1.9200000e+00 -4.3433480e-01 3.3555629e-01
+ 1.9300000e+00 -4.2948710e-01 3.3902441e-01
+ 1.9400000e+00 -4.2537026e-01 3.4238487e-01
+ 1.9500000e+00 -4.2360514e-01 3.4299080e-01
+ 1.9600000e+00 -4.2394069e-01 3.4298445e-01
+ 1.9700000e+00 -4.2488038e-01 3.4298441e-01
+ 1.9800000e+00 -4.2488262e-01 3.4298441e-01
+ 1.9900000e+00 -4.2488263e-01 3.4298441e-01
+ 2.0000000e+00 -4.2488263e-01 3.4298441e-01
diff --git a/data/pgmm/Data02.txt b/data/pgmm/Data02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c4f85ba6a95fb2412aed3e5513b3380dd9a06968
--- /dev/null
+++ b/data/pgmm/Data02.txt
@@ -0,0 +1,200 @@
+ 1.0000000e-02 -8.0516432e-01 -8.1514477e-01
+ 2.0000000e-02 -8.0516432e-01 -8.1514451e-01
+ 3.0000000e-02 -8.0516432e-01 -8.1514218e-01
+ 4.0000000e-02 -8.0516432e-01 -8.1512784e-01
+ 5.0000000e-02 -8.0516432e-01 -8.1509607e-01
+ 6.0000000e-02 -8.0516432e-01 -8.1312802e-01
+ 7.0000000e-02 -8.0516432e-01 -8.0972361e-01
+ 8.0000000e-02 -8.0516432e-01 -8.0441274e-01
+ 9.0000000e-02 -8.0516432e-01 -7.9710038e-01
+ 1.0000000e-01 -8.0516432e-01 -7.8957855e-01
+ 1.1000000e-01 -8.0516432e-01 -7.8014449e-01
+ 1.2000000e-01 -8.0516432e-01 -7.7034821e-01
+ 1.3000000e-01 -8.0516432e-01 -7.6079571e-01
+ 1.4000000e-01 -8.0516432e-01 -7.5073747e-01
+ 1.5000000e-01 -8.0516432e-01 -7.4044782e-01
+ 1.6000000e-01 -8.0516432e-01 -7.3280782e-01
+ 1.7000000e-01 -8.0516432e-01 -7.2491963e-01
+ 1.8000000e-01 -8.0516432e-01 -7.1678495e-01
+ 1.9000000e-01 -8.0516432e-01 -7.0821244e-01
+ 2.0000000e-01 -8.0516432e-01 -6.9915479e-01
+ 2.1000000e-01 -8.0516432e-01 -6.8989378e-01
+ 2.2000000e-01 -8.0516432e-01 -6.8051526e-01
+ 2.3000000e-01 -8.0516432e-01 -6.7097298e-01
+ 2.4000000e-01 -8.0516432e-01 -6.6118580e-01
+ 2.5000000e-01 -8.0516432e-01 -6.5157162e-01
+ 2.6000000e-01 -8.0516432e-01 -6.4160968e-01
+ 2.7000000e-01 -8.0516432e-01 -6.3149211e-01
+ 2.8000000e-01 -8.0516432e-01 -6.2144330e-01
+ 2.9000000e-01 -8.0516432e-01 -6.1135522e-01
+ 3.0000000e-01 -8.0516432e-01 -6.0145612e-01
+ 3.1000000e-01 -8.0516432e-01 -5.9189509e-01
+ 3.2000000e-01 -8.0516432e-01 -5.8213943e-01
+ 3.3000000e-01 -8.0516432e-01 -5.7236539e-01
+ 3.4000000e-01 -8.0516432e-01 -5.6234912e-01
+ 3.5000000e-01 -8.0516430e-01 -5.5281263e-01
+ 3.6000000e-01 -8.0516439e-01 -5.4351346e-01
+ 3.7000000e-01 -8.0516407e-01 -5.3425397e-01
+ 3.8000000e-01 -8.0516503e-01 -5.2489999e-01
+ 3.9000000e-01 -8.0516256e-01 -5.1540797e-01
+ 4.0000000e-01 -8.0516743e-01 -5.0599885e-01
+ 4.1000000e-01 -8.0516399e-01 -4.9691680e-01
+ 4.2000000e-01 -8.0500989e-01 -4.8809576e-01
+ 4.3000000e-01 -8.0485254e-01 -4.7954448e-01
+ 4.4000000e-01 -8.0458761e-01 -4.7092240e-01
+ 4.5000000e-01 -8.0416896e-01 -4.6202202e-01
+ 4.6000000e-01 -8.0359257e-01 -4.5287017e-01
+ 4.7000000e-01 -8.0293762e-01 -4.4348411e-01
+ 4.8000000e-01 -8.0227329e-01 -4.3406288e-01
+ 4.9000000e-01 -8.0141914e-01 -4.2443366e-01
+ 5.0000000e-01 -8.0027679e-01 -4.1447142e-01
+ 5.1000000e-01 -7.9898991e-01 -4.0408113e-01
+ 5.2000000e-01 -7.9757799e-01 -3.9343165e-01
+ 5.3000000e-01 -7.9588739e-01 -3.8251676e-01
+ 5.4000000e-01 -7.9399709e-01 -3.7136103e-01
+ 5.5000000e-01 -7.9197976e-01 -3.6034954e-01
+ 5.6000000e-01 -7.8981651e-01 -3.4938570e-01
+ 5.7000000e-01 -7.8750080e-01 -3.3832655e-01
+ 5.8000000e-01 -7.8506328e-01 -3.2724223e-01
+ 5.9000000e-01 -7.8263562e-01 -3.1620772e-01
+ 6.0000000e-01 -7.8020709e-01 -3.0493488e-01
+ 6.1000000e-01 -7.7778319e-01 -2.9393306e-01
+ 6.2000000e-01 -7.7533230e-01 -2.8295041e-01
+ 6.3000000e-01 -7.7306154e-01 -2.7206850e-01
+ 6.4000000e-01 -7.7118886e-01 -2.6083939e-01
+ 6.5000000e-01 -7.6975107e-01 -2.4933435e-01
+ 6.6000000e-01 -7.6875641e-01 -2.3791204e-01
+ 6.7000000e-01 -7.6816369e-01 -2.2650775e-01
+ 6.8000000e-01 -7.6786013e-01 -2.1522494e-01
+ 6.9000000e-01 -7.6798381e-01 -2.0389666e-01
+ 7.0000000e-01 -7.6854800e-01 -1.9256966e-01
+ 7.1000000e-01 -7.6970975e-01 -1.8128356e-01
+ 7.2000000e-01 -7.7120205e-01 -1.6983371e-01
+ 7.3000000e-01 -7.7309585e-01 -1.5848556e-01
+ 7.4000000e-01 -7.7536199e-01 -1.4720988e-01
+ 7.5000000e-01 -7.7795095e-01 -1.3602701e-01
+ 7.6000000e-01 -7.8093084e-01 -1.2487547e-01
+ 7.7000000e-01 -7.8419812e-01 -1.1366826e-01
+ 7.8000000e-01 -7.8780031e-01 -1.0290392e-01
+ 7.9000000e-01 -7.9169834e-01 -9.1742788e-02
+ 8.0000000e-01 -7.9573523e-01 -8.0398350e-02
+ 8.1000000e-01 -7.9990210e-01 -6.8652200e-02
+ 8.2000000e-01 -8.0433695e-01 -5.6887360e-02
+ 8.3000000e-01 -8.0900323e-01 -4.4953786e-02
+ 8.4000000e-01 -8.1370201e-01 -3.2770902e-02
+ 8.5000000e-01 -8.1825043e-01 -2.0675258e-02
+ 8.6000000e-01 -8.2282672e-01 -8.3180410e-03
+ 8.7000000e-01 -8.2738260e-01 3.9052880e-03
+ 8.8000000e-01 -8.3164638e-01 1.6099362e-02
+ 8.9000000e-01 -8.3563062e-01 2.8416131e-02
+ 9.0000000e-01 -8.3923260e-01 4.0796047e-02
+ 9.1000000e-01 -8.4236025e-01 5.3252324e-02
+ 9.2000000e-01 -8.4461153e-01 6.5711646e-02
+ 9.3000000e-01 -8.4551935e-01 7.7870723e-02
+ 9.4000000e-01 -8.4499315e-01 8.9944666e-02
+ 9.5000000e-01 -8.4318559e-01 1.0214662e-01
+ 9.6000000e-01 -8.4039801e-01 1.1420543e-01
+ 9.7000000e-01 -8.3705258e-01 1.2628684e-01
+ 9.8000000e-01 -8.3271096e-01 1.3823674e-01
+ 9.9000000e-01 -8.2759187e-01 1.5027057e-01
+ 1.0000000e+00 -8.2154958e-01 1.6236317e-01
+ 1.0100000e+00 -8.1477787e-01 1.7439468e-01
+ 1.0200000e+00 -8.0754624e-01 1.8622932e-01
+ 1.0300000e+00 -7.9961112e-01 1.9777585e-01
+ 1.0400000e+00 -7.9090055e-01 2.0919282e-01
+ 1.0500000e+00 -7.8085851e-01 2.2015137e-01
+ 1.0600000e+00 -7.6962934e-01 2.3057619e-01
+ 1.0700000e+00 -7.5776372e-01 2.4075401e-01
+ 1.0800000e+00 -7.4548739e-01 2.4999931e-01
+ 1.0900000e+00 -7.3276295e-01 2.5850901e-01
+ 1.1000000e+00 -7.1964490e-01 2.6621290e-01
+ 1.1100000e+00 -7.0598212e-01 2.7334988e-01
+ 1.1200000e+00 -6.9161609e-01 2.7996564e-01
+ 1.1300000e+00 -6.7709920e-01 2.8599699e-01
+ 1.1400000e+00 -6.6258720e-01 2.9175390e-01
+ 1.1500000e+00 -6.4789649e-01 2.9693138e-01
+ 1.1600000e+00 -6.3320227e-01 3.0206405e-01
+ 1.1700000e+00 -6.1853424e-01 3.0728253e-01
+ 1.1800000e+00 -6.0392688e-01 3.1206517e-01
+ 1.1900000e+00 -5.8957412e-01 3.1647407e-01
+ 1.2000000e+00 -5.7557451e-01 3.2054412e-01
+ 1.2100000e+00 -5.6212391e-01 3.2424646e-01
+ 1.2200000e+00 -5.4948225e-01 3.2781476e-01
+ 1.2300000e+00 -5.3769774e-01 3.3095142e-01
+ 1.2400000e+00 -5.2631607e-01 3.3397290e-01
+ 1.2500000e+00 -5.1530115e-01 3.3672450e-01
+ 1.2600000e+00 -5.0448815e-01 3.3952274e-01
+ 1.2700000e+00 -4.9385721e-01 3.4235745e-01
+ 1.2800000e+00 -4.8332230e-01 3.4502804e-01
+ 1.2900000e+00 -4.7328024e-01 3.4774477e-01
+ 1.3000000e+00 -4.6345758e-01 3.5043766e-01
+ 1.3100000e+00 -4.5367599e-01 3.5305587e-01
+ 1.3200000e+00 -4.4438354e-01 3.5562520e-01
+ 1.3300000e+00 -4.3570065e-01 3.5791851e-01
+ 1.3400000e+00 -4.2804875e-01 3.6021722e-01
+ 1.3500000e+00 -4.2120774e-01 3.6253406e-01
+ 1.3600000e+00 -4.1467493e-01 3.6480047e-01
+ 1.3700000e+00 -4.0850141e-01 3.6724286e-01
+ 1.3800000e+00 -4.0270502e-01 3.6984579e-01
+ 1.3900000e+00 -3.9696663e-01 3.7254751e-01
+ 1.4000000e+00 -3.9151390e-01 3.7580445e-01
+ 1.4100000e+00 -3.8659330e-01 3.7903541e-01
+ 1.4200000e+00 -3.8179919e-01 3.8222327e-01
+ 1.4300000e+00 -3.7686950e-01 3.8582050e-01
+ 1.4400000e+00 -3.7202737e-01 3.8952652e-01
+ 1.4500000e+00 -3.6711695e-01 3.9315304e-01
+ 1.4600000e+00 -3.6221082e-01 3.9668473e-01
+ 1.4700000e+00 -3.5742006e-01 4.0029458e-01
+ 1.4800000e+00 -3.5256454e-01 4.0415017e-01
+ 1.4900000e+00 -3.4757102e-01 4.0801901e-01
+ 1.5000000e+00 -3.4258797e-01 4.1210719e-01
+ 1.5100000e+00 -3.3760422e-01 4.1634992e-01
+ 1.5200000e+00 -3.3251423e-01 4.2087802e-01
+ 1.5300000e+00 -3.2742227e-01 4.2551953e-01
+ 1.5400000e+00 -3.2227580e-01 4.3042085e-01
+ 1.5500000e+00 -3.1709005e-01 4.3508741e-01
+ 1.5600000e+00 -3.1213085e-01 4.3973432e-01
+ 1.5700000e+00 -3.0727662e-01 4.4441546e-01
+ 1.5800000e+00 -3.0226654e-01 4.4876493e-01
+ 1.5900000e+00 -2.9729365e-01 4.5319499e-01
+ 1.6000000e+00 -2.9215079e-01 4.5794140e-01
+ 1.6100000e+00 -2.8685506e-01 4.6269371e-01
+ 1.6200000e+00 -2.8129794e-01 4.6762343e-01
+ 1.6300000e+00 -2.7552994e-01 4.7290195e-01
+ 1.6400000e+00 -2.6966878e-01 4.7826727e-01
+ 1.6500000e+00 -2.6367656e-01 4.8375169e-01
+ 1.6600000e+00 -2.5745316e-01 4.8935629e-01
+ 1.6700000e+00 -2.5100400e-01 4.9509821e-01
+ 1.6800000e+00 -2.4444627e-01 5.0129966e-01
+ 1.6900000e+00 -2.3771001e-01 5.0724902e-01
+ 1.7000000e+00 -2.3092931e-01 5.1321153e-01
+ 1.7100000e+00 -2.2416339e-01 5.1945465e-01
+ 1.7200000e+00 -2.1756101e-01 5.2537677e-01
+ 1.7300000e+00 -2.1096188e-01 5.3133181e-01
+ 1.7400000e+00 -2.0439394e-01 5.3736387e-01
+ 1.7500000e+00 -1.9779187e-01 5.4312231e-01
+ 1.7600000e+00 -1.9118427e-01 5.4870998e-01
+ 1.7700000e+00 -1.8476888e-01 5.5406906e-01
+ 1.7800000e+00 -1.7864023e-01 5.5943556e-01
+ 1.7900000e+00 -1.7264754e-01 5.6451373e-01
+ 1.8000000e+00 -1.6679243e-01 5.6917165e-01
+ 1.8100000e+00 -1.6118526e-01 5.7355746e-01
+ 1.8200000e+00 -1.5601592e-01 5.7754572e-01
+ 1.8300000e+00 -1.5111112e-01 5.8127458e-01
+ 1.8400000e+00 -1.4631581e-01 5.8490780e-01
+ 1.8500000e+00 -1.4167350e-01 5.8837742e-01
+ 1.8600000e+00 -1.3717546e-01 5.9175597e-01
+ 1.8700000e+00 -1.3100048e-01 5.9664709e-01
+ 1.8800000e+00 -1.2486289e-01 6.0129246e-01
+ 1.8900000e+00 -1.1875733e-01 6.0584966e-01
+ 1.9000000e+00 -1.1272614e-01 6.1042258e-01
+ 1.9100000e+00 -1.0720577e-01 6.1449307e-01
+ 1.9200000e+00 -1.0316566e-01 6.1659048e-01
+ 1.9300000e+00 -9.9827050e-02 6.1825147e-01
+ 1.9400000e+00 -9.7342728e-02 6.1921673e-01
+ 1.9500000e+00 -9.6232908e-02 6.1915695e-01
+ 1.9600000e+00 -9.6245101e-02 6.1915339e-01
+ 1.9700000e+00 -9.6244479e-02 6.1915357e-01
+ 1.9800000e+00 -9.6244185e-02 6.1915366e-01
+ 1.9900000e+00 -9.6244137e-02 6.1915367e-01
+ 2.0000000e+00 -9.6244131e-02 6.1915367e-01
diff --git a/data/pgmm/Data03.txt b/data/pgmm/Data03.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cd7a531f36d9bd996d29af9a04e903c99cd7c8de
--- /dev/null
+++ b/data/pgmm/Data03.txt
@@ -0,0 +1,200 @@
+ 1.0000000e-02 -7.9577465e-01 -7.8841871e-01
+ 2.0000000e-02 -7.9577465e-01 -7.8841840e-01
+ 3.0000000e-02 -7.9577465e-01 -7.8841316e-01
+ 4.0000000e-02 -7.9577465e-01 -7.8843035e-01
+ 5.0000000e-02 -7.9577465e-01 -7.8844456e-01
+ 6.0000000e-02 -7.9577465e-01 -7.8793582e-01
+ 7.0000000e-02 -7.9577465e-01 -7.8591327e-01
+ 8.0000000e-02 -7.9577465e-01 -7.8363014e-01
+ 9.0000000e-02 -7.9577465e-01 -7.8096242e-01
+ 1.0000000e-01 -7.9577465e-01 -7.7763999e-01
+ 1.1000000e-01 -7.9577465e-01 -7.7415495e-01
+ 1.2000000e-01 -7.9577465e-01 -7.7043891e-01
+ 1.3000000e-01 -7.9577464e-01 -7.6679941e-01
+ 1.4000000e-01 -7.9577469e-01 -7.6330546e-01
+ 1.5000000e-01 -7.9577471e-01 -7.5938055e-01
+ 1.6000000e-01 -7.9577524e-01 -7.5609742e-01
+ 1.7000000e-01 -7.9577311e-01 -7.5248950e-01
+ 1.8000000e-01 -7.9577469e-01 -7.4862370e-01
+ 1.9000000e-01 -7.9578304e-01 -7.4466446e-01
+ 2.0000000e-01 -7.9575642e-01 -7.4058210e-01
+ 2.1000000e-01 -7.9576506e-01 -7.3626265e-01
+ 2.2000000e-01 -7.9599510e-01 -7.3164629e-01
+ 2.3000000e-01 -7.9635975e-01 -7.2674411e-01
+ 2.4000000e-01 -7.9668519e-01 -7.2151042e-01
+ 2.5000000e-01 -7.9696724e-01 -7.1594258e-01
+ 2.6000000e-01 -7.9737403e-01 -7.1039728e-01
+ 2.7000000e-01 -7.9800578e-01 -7.0491584e-01
+ 2.8000000e-01 -7.9868168e-01 -6.9912215e-01
+ 2.9000000e-01 -7.9932136e-01 -6.9314373e-01
+ 3.0000000e-01 -7.9996678e-01 -6.8719478e-01
+ 3.1000000e-01 -8.0061665e-01 -6.8125801e-01
+ 3.2000000e-01 -8.0126363e-01 -6.7529673e-01
+ 3.3000000e-01 -8.0191071e-01 -6.6924354e-01
+ 3.4000000e-01 -8.0255955e-01 -6.6316218e-01
+ 3.5000000e-01 -8.0320611e-01 -6.5703526e-01
+ 3.6000000e-01 -8.0384960e-01 -6.5080513e-01
+ 3.7000000e-01 -8.0451114e-01 -6.4456830e-01
+ 3.8000000e-01 -8.0515199e-01 -6.3830783e-01
+ 3.9000000e-01 -8.0568062e-01 -6.3193762e-01
+ 4.0000000e-01 -8.0614645e-01 -6.2544989e-01
+ 4.1000000e-01 -8.0663235e-01 -6.1870999e-01
+ 4.2000000e-01 -8.0713394e-01 -6.1154066e-01
+ 4.3000000e-01 -8.0758527e-01 -6.0424107e-01
+ 4.4000000e-01 -8.0791907e-01 -5.9710578e-01
+ 4.5000000e-01 -8.0812911e-01 -5.9007426e-01
+ 4.6000000e-01 -8.0828162e-01 -5.8316494e-01
+ 4.7000000e-01 -8.0843947e-01 -5.7631641e-01
+ 4.8000000e-01 -8.0859604e-01 -5.6929093e-01
+ 4.9000000e-01 -8.0878383e-01 -5.6206354e-01
+ 5.0000000e-01 -8.0893680e-01 -5.5476712e-01
+ 5.1000000e-01 -8.0886888e-01 -5.4756867e-01
+ 5.2000000e-01 -8.0866609e-01 -5.4044080e-01
+ 5.3000000e-01 -8.0850251e-01 -5.3321363e-01
+ 5.4000000e-01 -8.0838235e-01 -5.2583513e-01
+ 5.5000000e-01 -8.0813747e-01 -5.1817546e-01
+ 5.6000000e-01 -8.0766765e-01 -5.1016521e-01
+ 5.7000000e-01 -8.0715350e-01 -5.0217562e-01
+ 5.8000000e-01 -8.0667574e-01 -4.9426675e-01
+ 5.9000000e-01 -8.0619288e-01 -4.8595315e-01
+ 6.0000000e-01 -8.0570328e-01 -4.7718700e-01
+ 6.1000000e-01 -8.0521777e-01 -4.6820744e-01
+ 6.2000000e-01 -8.0474193e-01 -4.5898988e-01
+ 6.3000000e-01 -8.0423642e-01 -4.4935895e-01
+ 6.4000000e-01 -8.0373671e-01 -4.3931765e-01
+ 6.5000000e-01 -8.0344100e-01 -4.2906496e-01
+ 6.6000000e-01 -8.0329403e-01 -4.1859244e-01
+ 6.7000000e-01 -8.0314807e-01 -4.0770674e-01
+ 6.8000000e-01 -8.0309643e-01 -3.9627225e-01
+ 6.9000000e-01 -8.0311805e-01 -3.8446097e-01
+ 7.0000000e-01 -8.0311892e-01 -3.7270429e-01
+ 7.1000000e-01 -8.0310169e-01 -3.6122059e-01
+ 7.2000000e-01 -8.0313677e-01 -3.4977832e-01
+ 7.3000000e-01 -8.0329095e-01 -3.3809349e-01
+ 7.4000000e-01 -8.0354807e-01 -3.2621340e-01
+ 7.5000000e-01 -8.0370138e-01 -3.1411850e-01
+ 7.6000000e-01 -8.0343925e-01 -3.0170252e-01
+ 7.7000000e-01 -8.0272078e-01 -2.8915774e-01
+ 7.8000000e-01 -8.0159474e-01 -2.7666533e-01
+ 7.9000000e-01 -7.9995806e-01 -2.6411903e-01
+ 8.0000000e-01 -7.9733828e-01 -2.5127293e-01
+ 8.1000000e-01 -7.9327904e-01 -2.3816308e-01
+ 8.2000000e-01 -7.8779779e-01 -2.2521172e-01
+ 8.3000000e-01 -7.8105159e-01 -2.1254629e-01
+ 8.4000000e-01 -7.7316411e-01 -2.0019466e-01
+ 8.5000000e-01 -7.6420140e-01 -1.8837089e-01
+ 8.6000000e-01 -7.5450646e-01 -1.7676856e-01
+ 8.7000000e-01 -7.4434889e-01 -1.6515686e-01
+ 8.8000000e-01 -7.3316369e-01 -1.5406110e-01
+ 8.9000000e-01 -7.2086392e-01 -1.4353890e-01
+ 9.0000000e-01 -7.0797670e-01 -1.3338549e-01
+ 9.1000000e-01 -6.9428338e-01 -1.2377764e-01
+ 9.2000000e-01 -6.7964573e-01 -1.1483364e-01
+ 9.3000000e-01 -6.6434369e-01 -1.0647833e-01
+ 9.4000000e-01 -6.4834465e-01 -9.8524232e-02
+ 9.5000000e-01 -6.3163123e-01 -9.1001500e-02
+ 9.6000000e-01 -6.1422358e-01 -8.4122191e-02
+ 9.7000000e-01 -5.9605632e-01 -7.7865447e-02
+ 9.8000000e-01 -5.7701955e-01 -7.1992823e-02
+ 9.9000000e-01 -5.5695998e-01 -6.6390224e-02
+ 1.0000000e+00 -5.3620854e-01 -6.1008571e-02
+ 1.0100000e+00 -5.1520220e-01 -5.5739595e-02
+ 1.0200000e+00 -4.9383098e-01 -5.0512011e-02
+ 1.0300000e+00 -4.7202129e-01 -4.5227907e-02
+ 1.0400000e+00 -4.4996109e-01 -3.9953913e-02
+ 1.0500000e+00 -4.2788812e-01 -3.4994919e-02
+ 1.0600000e+00 -4.0585196e-01 -3.0179337e-02
+ 1.0700000e+00 -3.8389567e-01 -2.5194608e-02
+ 1.0800000e+00 -3.6231267e-01 -2.0462434e-02
+ 1.0900000e+00 -3.4159232e-01 -1.6222362e-02
+ 1.1000000e+00 -3.2199669e-01 -1.2087125e-02
+ 1.1100000e+00 -3.0349868e-01 -7.7302710e-03
+ 1.1200000e+00 -2.8594376e-01 -3.1836708e-03
+ 1.1300000e+00 -2.6910543e-01 1.4706196e-03
+ 1.1400000e+00 -2.5296003e-01 6.3406214e-03
+ 1.1500000e+00 -2.3724960e-01 1.1362648e-02
+ 1.1600000e+00 -2.2169367e-01 1.6443250e-02
+ 1.1700000e+00 -2.0683784e-01 2.1765216e-02
+ 1.1800000e+00 -1.9278624e-01 2.7420628e-02
+ 1.1900000e+00 -1.7907257e-01 3.3310894e-02
+ 1.2000000e+00 -1.6600801e-01 3.9331105e-02
+ 1.2100000e+00 -1.5381994e-01 4.5444729e-02
+ 1.2200000e+00 -1.4210287e-01 5.1759180e-02
+ 1.2300000e+00 -1.3067527e-01 5.8224580e-02
+ 1.2400000e+00 -1.1968961e-01 6.4687301e-02
+ 1.2500000e+00 -1.0939715e-01 7.1165622e-02
+ 1.2600000e+00 -9.9890102e-02 7.7529004e-02
+ 1.2700000e+00 -9.1238791e-02 8.3679141e-02
+ 1.2800000e+00 -8.3626966e-02 8.9826277e-02
+ 1.2900000e+00 -7.6690869e-02 9.5932325e-02
+ 1.3000000e+00 -6.9896192e-02 1.0177252e-01
+ 1.3100000e+00 -6.3400069e-02 1.0743834e-01
+ 1.3200000e+00 -5.7382267e-02 1.1298129e-01
+ 1.3300000e+00 -5.1794536e-02 1.1823177e-01
+ 1.3400000e+00 -4.6579257e-02 1.2338455e-01
+ 1.3500000e+00 -4.1660652e-02 1.2850139e-01
+ 1.3600000e+00 -3.6938111e-02 1.3340257e-01
+ 1.3700000e+00 -3.2320244e-02 1.3833713e-01
+ 1.3800000e+00 -2.7834183e-02 1.4335530e-01
+ 1.3900000e+00 -2.3429840e-02 1.4822000e-01
+ 1.4000000e+00 -1.8974916e-02 1.5298099e-01
+ 1.4100000e+00 -1.4525168e-02 1.5776975e-01
+ 1.4200000e+00 -1.0285081e-02 1.6266231e-01
+ 1.4300000e+00 -6.1987757e-03 1.6768882e-01
+ 1.4400000e+00 -2.0853786e-03 1.7280433e-01
+ 1.4500000e+00 2.0002836e-03 1.7803024e-01
+ 1.4600000e+00 6.1255826e-03 1.8349344e-01
+ 1.4700000e+00 1.0507375e-02 1.8922990e-01
+ 1.4800000e+00 1.4933058e-02 1.9487892e-01
+ 1.4900000e+00 1.9270587e-02 2.0038002e-01
+ 1.5000000e+00 2.3734531e-02 2.0619733e-01
+ 1.5100000e+00 2.8132970e-02 2.1209336e-01
+ 1.5200000e+00 3.2298288e-02 2.1776171e-01
+ 1.5300000e+00 3.6441490e-02 2.2339172e-01
+ 1.5400000e+00 4.0685562e-02 2.2914385e-01
+ 1.5500000e+00 4.5038194e-02 2.3510294e-01
+ 1.5600000e+00 4.9555456e-02 2.4125314e-01
+ 1.5700000e+00 5.4357671e-02 2.4762809e-01
+ 1.5800000e+00 5.9464369e-02 2.5432696e-01
+ 1.5900000e+00 6.4712418e-02 2.6129172e-01
+ 1.6000000e+00 7.0069884e-02 2.6826619e-01
+ 1.6100000e+00 7.5559154e-02 2.7513939e-01
+ 1.6200000e+00 8.1075906e-02 2.8195183e-01
+ 1.6300000e+00 8.6557321e-02 2.8865140e-01
+ 1.6400000e+00 9.2032017e-02 2.9523497e-01
+ 1.6500000e+00 9.7455228e-02 3.0167778e-01
+ 1.6600000e+00 1.0297979e-01 3.0810476e-01
+ 1.6700000e+00 1.0883973e-01 3.1473719e-01
+ 1.6800000e+00 1.1471475e-01 3.2141764e-01
+ 1.6900000e+00 1.2027734e-01 3.2791279e-01
+ 1.7000000e+00 1.2551868e-01 3.3411851e-01
+ 1.7100000e+00 1.3056892e-01 3.3998542e-01
+ 1.7200000e+00 1.3551115e-01 3.4559173e-01
+ 1.7300000e+00 1.4035578e-01 3.5108853e-01
+ 1.7400000e+00 1.4506499e-01 3.5642367e-01
+ 1.7500000e+00 1.4938941e-01 3.6140176e-01
+ 1.7600000e+00 1.5313002e-01 3.6585888e-01
+ 1.7700000e+00 1.5658733e-01 3.7013983e-01
+ 1.7800000e+00 1.5998481e-01 3.7443363e-01
+ 1.7900000e+00 1.6315878e-01 3.7832556e-01
+ 1.8000000e+00 1.6620903e-01 3.8193817e-01
+ 1.8100000e+00 1.6928580e-01 3.8561790e-01
+ 1.8200000e+00 1.7227892e-01 3.8934646e-01
+ 1.8300000e+00 1.7515050e-01 3.9297089e-01
+ 1.8400000e+00 1.7792142e-01 3.9638078e-01
+ 1.8500000e+00 1.8052438e-01 3.9960205e-01
+ 1.8600000e+00 1.8285841e-01 4.0258715e-01
+ 1.8700000e+00 1.8567172e-01 4.0623728e-01
+ 1.8800000e+00 1.8810799e-01 4.0950464e-01
+ 1.8900000e+00 1.9078180e-01 4.1294008e-01
+ 1.9000000e+00 1.9352778e-01 4.1663420e-01
+ 1.9100000e+00 1.9582221e-01 4.2014988e-01
+ 1.9200000e+00 1.9715058e-01 4.2272454e-01
+ 1.9300000e+00 1.9868381e-01 4.2506911e-01
+ 1.9400000e+00 1.9959170e-01 4.2710056e-01
+ 1.9500000e+00 1.9951606e-01 4.2766208e-01
+ 1.9600000e+00 1.9953160e-01 4.2761356e-01
+ 1.9700000e+00 1.9953100e-01 4.2761541e-01
+ 1.9800000e+00 1.9953029e-01 4.2761765e-01
+ 1.9900000e+00 1.9953050e-01 4.2761697e-01
+ 2.0000000e+00 1.9953052e-01 4.2761693e-01
diff --git a/data/pgmm/Data04.txt b/data/pgmm/Data04.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cbd5da926fbaa7e2c1a52c0077e824937f0b1905
--- /dev/null
+++ b/data/pgmm/Data04.txt
@@ -0,0 +1,200 @@
+ 1.0000000e-02 -8.0985915e-01 -7.9287305e-01
+ 2.0000000e-02 -8.0985915e-01 -7.9287320e-01
+ 3.0000000e-02 -8.0985915e-01 -7.9287348e-01
+ 4.0000000e-02 -8.0985915e-01 -7.9287169e-01
+ 5.0000000e-02 -8.0985915e-01 -7.9287823e-01
+ 6.0000000e-02 -8.0985915e-01 -7.9286070e-01
+ 7.0000000e-02 -8.0985915e-01 -7.9286978e-01
+ 8.0000000e-02 -8.0985916e-01 -7.9165143e-01
+ 9.0000000e-02 -8.0985914e-01 -7.8887708e-01
+ 1.0000000e-01 -8.0985922e-01 -7.8500293e-01
+ 1.1000000e-01 -8.0985893e-01 -7.8055834e-01
+ 1.2000000e-01 -8.0985944e-01 -7.7630560e-01
+ 1.3000000e-01 -8.0986148e-01 -7.7199963e-01
+ 1.4000000e-01 -8.0983804e-01 -7.6786272e-01
+ 1.5000000e-01 -8.1004268e-01 -7.6345857e-01
+ 1.6000000e-01 -8.1039417e-01 -7.6001205e-01
+ 1.7000000e-01 -8.1073239e-01 -7.5639499e-01
+ 1.8000000e-01 -8.1104682e-01 -7.5254983e-01
+ 1.9000000e-01 -8.1137021e-01 -7.4841500e-01
+ 2.0000000e-01 -8.1169611e-01 -7.4400223e-01
+ 2.1000000e-01 -8.1201942e-01 -7.3932103e-01
+ 2.2000000e-01 -8.1234288e-01 -7.3436680e-01
+ 2.3000000e-01 -8.1266682e-01 -7.2913747e-01
+ 2.4000000e-01 -8.1299062e-01 -7.2363421e-01
+ 2.5000000e-01 -8.1331444e-01 -7.1786353e-01
+ 2.6000000e-01 -8.1363815e-01 -7.1181739e-01
+ 2.7000000e-01 -8.1396162e-01 -7.0547387e-01
+ 2.8000000e-01 -8.1428619e-01 -6.9889880e-01
+ 2.9000000e-01 -8.1461056e-01 -6.9222161e-01
+ 3.0000000e-01 -8.1492999e-01 -6.8551404e-01
+ 3.1000000e-01 -8.1525660e-01 -6.7882274e-01
+ 3.2000000e-01 -8.1559745e-01 -6.7215492e-01
+ 3.3000000e-01 -8.1588178e-01 -6.6539589e-01
+ 3.4000000e-01 -8.1605980e-01 -6.5844302e-01
+ 3.5000000e-01 -8.1620491e-01 -6.5131548e-01
+ 3.6000000e-01 -8.1636830e-01 -6.4404175e-01
+ 3.7000000e-01 -8.1653290e-01 -6.3659909e-01
+ 3.8000000e-01 -8.1669386e-01 -6.2894243e-01
+ 3.9000000e-01 -8.1685784e-01 -6.2109828e-01
+ 4.0000000e-01 -8.1701587e-01 -6.1310127e-01
+ 4.1000000e-01 -8.1717079e-01 -6.0492391e-01
+ 4.2000000e-01 -8.1736086e-01 -5.9658444e-01
+ 4.3000000e-01 -8.1752122e-01 -5.8822407e-01
+ 4.4000000e-01 -8.1748311e-01 -5.7997553e-01
+ 4.5000000e-01 -8.1728960e-01 -5.7174413e-01
+ 4.6000000e-01 -8.1711284e-01 -5.6338851e-01
+ 4.7000000e-01 -8.1697890e-01 -5.5492636e-01
+ 4.8000000e-01 -8.1679366e-01 -5.4633250e-01
+ 4.9000000e-01 -8.1649605e-01 -5.3754093e-01
+ 5.0000000e-01 -8.1616843e-01 -5.2868013e-01
+ 5.1000000e-01 -8.1584843e-01 -5.1983709e-01
+ 5.2000000e-01 -8.1543628e-01 -5.1090135e-01
+ 5.3000000e-01 -8.1494210e-01 -5.0190347e-01
+ 5.4000000e-01 -8.1445772e-01 -4.9294066e-01
+ 5.5000000e-01 -8.1394194e-01 -4.8381105e-01
+ 5.6000000e-01 -8.1331368e-01 -4.7438059e-01
+ 5.7000000e-01 -8.1253850e-01 -4.6485554e-01
+ 5.8000000e-01 -8.1151133e-01 -4.5519495e-01
+ 5.9000000e-01 -8.1009713e-01 -4.4514596e-01
+ 6.0000000e-01 -8.0831745e-01 -4.3483970e-01
+ 6.1000000e-01 -8.0625047e-01 -4.2445717e-01
+ 6.2000000e-01 -8.0395738e-01 -4.1393314e-01
+ 6.3000000e-01 -8.0148235e-01 -4.0333645e-01
+ 6.4000000e-01 -7.9871961e-01 -3.9280176e-01
+ 6.5000000e-01 -7.9533851e-01 -3.8219645e-01
+ 6.6000000e-01 -7.9105289e-01 -3.7137762e-01
+ 6.7000000e-01 -7.8584693e-01 -3.6054351e-01
+ 6.8000000e-01 -7.7981860e-01 -3.4983425e-01
+ 6.9000000e-01 -7.7308937e-01 -3.3912444e-01
+ 7.0000000e-01 -7.6575275e-01 -3.2850363e-01
+ 7.1000000e-01 -7.5784654e-01 -3.1819048e-01
+ 7.2000000e-01 -7.4932200e-01 -3.0811377e-01
+ 7.3000000e-01 -7.3994025e-01 -2.9821773e-01
+ 7.4000000e-01 -7.2933384e-01 -2.8870281e-01
+ 7.5000000e-01 -7.1749264e-01 -2.7969362e-01
+ 7.6000000e-01 -7.0467429e-01 -2.7119826e-01
+ 7.7000000e-01 -6.9111627e-01 -2.6330487e-01
+ 7.8000000e-01 -6.7703916e-01 -2.5606901e-01
+ 7.9000000e-01 -6.6247330e-01 -2.4936362e-01
+ 8.0000000e-01 -6.4694017e-01 -2.4333047e-01
+ 8.1000000e-01 -6.2991311e-01 -2.3833687e-01
+ 8.2000000e-01 -6.1154286e-01 -2.3418630e-01
+ 8.3000000e-01 -5.9221107e-01 -2.3059872e-01
+ 8.4000000e-01 -5.7222992e-01 -2.2789453e-01
+ 8.5000000e-01 -5.5166909e-01 -2.2620674e-01
+ 8.6000000e-01 -5.3038821e-01 -2.2516512e-01
+ 8.7000000e-01 -5.0848311e-01 -2.2475728e-01
+ 8.8000000e-01 -4.8611185e-01 -2.2530062e-01
+ 8.9000000e-01 -4.6315885e-01 -2.2675014e-01
+ 9.0000000e-01 -4.3971976e-01 -2.2884627e-01
+ 9.1000000e-01 -4.1613525e-01 -2.3159827e-01
+ 9.2000000e-01 -3.9216094e-01 -2.3520714e-01
+ 9.3000000e-01 -3.6739049e-01 -2.3975815e-01
+ 9.4000000e-01 -3.4205905e-01 -2.4532759e-01
+ 9.5000000e-01 -3.1665811e-01 -2.5185035e-01
+ 9.6000000e-01 -2.9142931e-01 -2.5897155e-01
+ 9.7000000e-01 -2.6618934e-01 -2.6665139e-01
+ 9.8000000e-01 -2.4066172e-01 -2.7502761e-01
+ 9.9000000e-01 -2.1490368e-01 -2.8375645e-01
+ 1.0000000e+00 -1.8913307e-01 -2.9252231e-01
+ 1.0100000e+00 -1.6355499e-01 -3.0146051e-01
+ 1.0200000e+00 -1.3820373e-01 -3.1057291e-01
+ 1.0300000e+00 -1.1310049e-01 -3.1961720e-01
+ 1.0400000e+00 -8.8185837e-02 -3.2847412e-01
+ 1.0500000e+00 -6.3307237e-02 -3.3714329e-01
+ 1.0600000e+00 -3.8470719e-02 -3.4562172e-01
+ 1.0700000e+00 -1.3692275e-02 -3.5393656e-01
+ 1.0800000e+00 1.0969280e-02 -3.6207620e-01
+ 1.0900000e+00 3.4861728e-02 -3.6968606e-01
+ 1.1000000e+00 5.7238374e-02 -3.7642208e-01
+ 1.1100000e+00 7.8327754e-02 -3.8250188e-01
+ 1.1200000e+00 9.8722278e-02 -3.8800784e-01
+ 1.1300000e+00 1.1874852e-01 -3.9266613e-01
+ 1.1400000e+00 1.3825751e-01 -3.9668884e-01
+ 1.1500000e+00 1.5697619e-01 -4.0036955e-01
+ 1.1600000e+00 1.7501407e-01 -4.0343899e-01
+ 1.1700000e+00 1.9235365e-01 -4.0575505e-01
+ 1.1800000e+00 2.0873172e-01 -4.0747533e-01
+ 1.1900000e+00 2.2443111e-01 -4.0879819e-01
+ 1.2000000e+00 2.3992126e-01 -4.0988134e-01
+ 1.2100000e+00 2.5481077e-01 -4.1048905e-01
+ 1.2200000e+00 2.6853861e-01 -4.1032654e-01
+ 1.2300000e+00 2.8108442e-01 -4.0937807e-01
+ 1.2400000e+00 2.9271946e-01 -4.0769784e-01
+ 1.2500000e+00 3.0370823e-01 -4.0545568e-01
+ 1.2600000e+00 3.1389235e-01 -4.0284822e-01
+ 1.2700000e+00 3.2308046e-01 -3.9985994e-01
+ 1.2800000e+00 3.3149798e-01 -3.9645497e-01
+ 1.2900000e+00 3.3948806e-01 -3.9265935e-01
+ 1.3000000e+00 3.4725260e-01 -3.8841825e-01
+ 1.3100000e+00 3.5454359e-01 -3.8378398e-01
+ 1.3200000e+00 3.6092106e-01 -3.7888868e-01
+ 1.3300000e+00 3.6624608e-01 -3.7380094e-01
+ 1.3400000e+00 3.7068006e-01 -3.6851309e-01
+ 1.3500000e+00 3.7448382e-01 -3.6296424e-01
+ 1.3600000e+00 3.7775684e-01 -3.5708367e-01
+ 1.3700000e+00 3.8061454e-01 -3.5092872e-01
+ 1.3800000e+00 3.8335299e-01 -3.4479061e-01
+ 1.3900000e+00 3.8614027e-01 -3.3874186e-01
+ 1.4000000e+00 3.8888948e-01 -3.3256979e-01
+ 1.4100000e+00 3.9145328e-01 -3.2633054e-01
+ 1.4200000e+00 3.9380583e-01 -3.2023320e-01
+ 1.4300000e+00 3.9619138e-01 -3.1414998e-01
+ 1.4400000e+00 3.9865226e-01 -3.0792909e-01
+ 1.4500000e+00 4.0090031e-01 -3.0180885e-01
+ 1.4600000e+00 4.0305290e-01 -2.9593471e-01
+ 1.4700000e+00 4.0544449e-01 -2.9009971e-01
+ 1.4800000e+00 4.0796602e-01 -2.8406763e-01
+ 1.4900000e+00 4.1044965e-01 -2.7787626e-01
+ 1.5000000e+00 4.1304432e-01 -2.7172452e-01
+ 1.5100000e+00 4.1588645e-01 -2.6538429e-01
+ 1.5200000e+00 4.1894345e-01 -2.5860881e-01
+ 1.5300000e+00 4.2207527e-01 -2.5184071e-01
+ 1.5400000e+00 4.2517258e-01 -2.4533853e-01
+ 1.5500000e+00 4.2829869e-01 -2.3867473e-01
+ 1.5600000e+00 4.3163325e-01 -2.3161740e-01
+ 1.5700000e+00 4.3521534e-01 -2.2441366e-01
+ 1.5800000e+00 4.3876661e-01 -2.1737237e-01
+ 1.5900000e+00 4.4206854e-01 -2.1053673e-01
+ 1.6000000e+00 4.4518988e-01 -2.0372204e-01
+ 1.6100000e+00 4.4828598e-01 -1.9693476e-01
+ 1.6200000e+00 4.5148879e-01 -1.9025896e-01
+ 1.6300000e+00 4.5477720e-01 -1.8350323e-01
+ 1.6400000e+00 4.5804277e-01 -1.7663841e-01
+ 1.6500000e+00 4.6128329e-01 -1.7001262e-01
+ 1.6600000e+00 4.6452109e-01 -1.6371056e-01
+ 1.6700000e+00 4.6768812e-01 -1.5750312e-01
+ 1.6800000e+00 4.7073930e-01 -1.5139385e-01
+ 1.6900000e+00 4.7368964e-01 -1.4556002e-01
+ 1.7000000e+00 4.7656819e-01 -1.4004391e-01
+ 1.7100000e+00 4.7934568e-01 -1.3478924e-01
+ 1.7200000e+00 4.8191500e-01 -1.2971409e-01
+ 1.7300000e+00 4.8433336e-01 -1.2483875e-01
+ 1.7400000e+00 4.8676900e-01 -1.2024926e-01
+ 1.7500000e+00 4.8914024e-01 -1.1588549e-01
+ 1.7600000e+00 4.9129396e-01 -1.1169107e-01
+ 1.7700000e+00 4.9323726e-01 -1.0777091e-01
+ 1.7800000e+00 4.9499966e-01 -1.0410136e-01
+ 1.7900000e+00 4.9657367e-01 -1.0055736e-01
+ 1.8000000e+00 4.9794496e-01 -9.7388610e-02
+ 1.8100000e+00 4.9912234e-01 -9.4814760e-02
+ 1.8200000e+00 5.0016355e-01 -9.2486689e-02
+ 1.8300000e+00 5.0113293e-01 -9.0186519e-02
+ 1.8400000e+00 5.0204532e-01 -8.8106077e-02
+ 1.8500000e+00 5.0275756e-01 -8.6410808e-02
+ 1.8600000e+00 5.0315460e-01 -8.5096940e-02
+ 1.8700000e+00 5.0368017e-01 -8.3074794e-02
+ 1.8800000e+00 5.0435450e-01 -8.1412376e-02
+ 1.8900000e+00 5.0470043e-01 -8.0150630e-02
+ 1.9000000e+00 5.0469041e-01 -8.0199941e-02
+ 1.9100000e+00 5.0469618e-01 -8.0171555e-02
+ 1.9200000e+00 5.0469454e-01 -8.0179622e-02
+ 1.9300000e+00 5.0469488e-01 -8.0177953e-02
+ 1.9400000e+00 5.0469484e-01 -8.0178176e-02
+ 1.9500000e+00 5.0469483e-01 -8.0178187e-02
+ 1.9600000e+00 5.0469484e-01 -8.0178168e-02
+ 1.9700000e+00 5.0469484e-01 -8.0178175e-02
+ 1.9800000e+00 5.0469484e-01 -8.0178173e-02
+ 1.9900000e+00 5.0469484e-01 -8.0178174e-02
+ 2.0000000e+00 5.0469484e-01 -8.0178174e-02
diff --git a/data/pgmm/Mu02.txt b/data/pgmm/Mu02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9bf9fa8f75ebe4646f4196d42dac248b20560a22
--- /dev/null
+++ b/data/pgmm/Mu02.txt
@@ -0,0 +1,3 @@
+ 4.485639096467e-01 1.065745313882e+00 1.654644003908e+00 4.485639096467e-01 1.065745313882e+00 1.654644003908e+00
+ -1.969177896820e-02 8.869020197680e-01 2.360049073794e+00 3.486291646411e-01 -6.933679411279e-01 -7.579016605593e-03
+ 6.432404719102e-01 2.280510698337e+00 3.157233653573e+00 3.399613596060e+00 2.016810079537e+00 5.686262001576e-01
diff --git a/data/pgmm/Param01.txt b/data/pgmm/Param01.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d95f420b4d2160d3fbc91069180a4987230befd
--- /dev/null
+++ b/data/pgmm/Param01.txt
@@ -0,0 +1,8 @@
+ 0.0000000e+00 -8.0000000e-01 -8.0000000e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 3.1622777e-01 0.0000000e+00
+ 0.0000000e+00 0.0000000e+00 3.1622777e-01
+ 0.0000000e+00 -4.0000000e-01 3.4531101e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 1.2101513e-01 -2.9215636e-01
+ 0.0000000e+00 -2.9215636e-01 -1.2101513e-01
diff --git a/data/pgmm/Param02.txt b/data/pgmm/Param02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6c677f350214b52b0ce31eccc5567dc69d40d3ff
--- /dev/null
+++ b/data/pgmm/Param02.txt
@@ -0,0 +1,8 @@
+ 0.0000000e+00 -8.0000000e-01 -8.0000000e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 3.1622777e-01 0.0000000e+00
+ 0.0000000e+00 0.0000000e+00 3.1622777e-01
+ 0.0000000e+00 -1.0000000e-01 5.9799599e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 1.9250727e-01 -2.5088035e-01
+ 0.0000000e+00 -2.5088035e-01 -1.9250727e-01
diff --git a/data/pgmm/Param03.txt b/data/pgmm/Param03.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8527b335ad31e08f756d58a6b48642e244fed43f
--- /dev/null
+++ b/data/pgmm/Param03.txt
@@ -0,0 +1,8 @@
+ 0.0000000e+00 -8.0000000e-01 -8.0000000e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 3.1622777e-01 0.0000000e+00
+ 0.0000000e+00 0.0000000e+00 3.1622777e-01
+ 0.0000000e+00 2.0000000e-01 4.2245856e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 2.5088035e-01 -1.9250727e-01
+ 0.0000000e+00 -1.9250727e-01 -2.5088035e-01
diff --git a/data/pgmm/Param04.txt b/data/pgmm/Param04.txt
new file mode 100644
index 0000000000000000000000000000000000000000..498b4abfaf1677a4a388dbaca52c614a926ea467
--- /dev/null
+++ b/data/pgmm/Param04.txt
@@ -0,0 +1,8 @@
+ 0.0000000e+00 -8.0000000e-01 -8.0000000e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 3.1622777e-01 0.0000000e+00
+ 0.0000000e+00 0.0000000e+00 3.1622777e-01
+ 0.0000000e+00 5.0000000e-01 -8.7103994e-02
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 2.9215636e-01 -1.2101513e-01
+ 0.0000000e+00 -1.2101513e-01 -2.9215636e-01
diff --git a/data/pgmm/ParamRepro01.txt b/data/pgmm/ParamRepro01.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d95f420b4d2160d3fbc91069180a4987230befd
--- /dev/null
+++ b/data/pgmm/ParamRepro01.txt
@@ -0,0 +1,8 @@
+ 0.0000000e+00 -8.0000000e-01 -8.0000000e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 3.1622777e-01 0.0000000e+00
+ 0.0000000e+00 0.0000000e+00 3.1622777e-01
+ 0.0000000e+00 -4.0000000e-01 3.4531101e-01
+ 1.0000000e+00 0.0000000e+00 0.0000000e+00
+ 0.0000000e+00 1.2101513e-01 -2.9215636e-01
+ 0.0000000e+00 -2.9215636e-01 -1.2101513e-01
diff --git a/data/pgmm/Priors02.txt b/data/pgmm/Priors02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f36f69267a1b743cb36531a3e2be7e7f5a6ceb69
--- /dev/null
+++ b/data/pgmm/Priors02.txt
@@ -0,0 +1 @@
+ 4.298909705102e-01 2.227226242722e-01 3.473864052176e-01
diff --git a/data/pgmm/Sigma02.txt b/data/pgmm/Sigma02.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7eb88884c60cf84d6ad3d844fddb36415d71fe70
--- /dev/null
+++ b/data/pgmm/Sigma02.txt
@@ -0,0 +1,3 @@
+ 7.414909825973e-02 -6.552296853168e-04 1.172433990850e-01 2.845639265182e-02 3.313532990183e-02 3.387831659530e-02 4.224658215071e-02 2.996507843336e-02 6.698878861219e-02 7.414909825973e-02 4.991767979253e-02 -1.186819553979e-01 2.845639265182e-02 1.444835784944e-01 -4.671081429346e-02 4.224658215071e-02 7.208864774419e-04 -8.560857362038e-02
+ -6.552296853168e-04 1.414250531205e-03 9.889086063240e-03 3.313532990183e-02 1.167924680726e+00 -3.319261210536e-01 2.996507843336e-02 1.552932708762e+00 -6.170382767560e-01 4.991767979253e-02 4.550608152165e+00 -4.905572724478e-01 1.444835784944e-01 1.379407418008e+00 6.278905969488e-02 7.208864774419e-04 2.129417429227e-03 3.824000507890e-03
+ 1.172433990850e-01 9.889086063240e-03 3.417866204667e-01 3.387831659530e-02 -3.319261210536e-01 4.866514490086e-01 6.698878861219e-02 -6.170382767560e-01 7.595055717684e-01 -1.186819553979e-01 -4.905572724478e-01 7.418548736374e-01 -4.671081429346e-02 6.278905969488e-02 3.214758143930e-01 -8.560857362038e-02 3.824000507890e-03 2.066932376973e-01
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv.txt b/data/pgmm/timeInvModel/Zmu_timeInv.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b4673a1cf23982aa151d85d63745e15b5210363f
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zmu_timeInv.txt
@@ -0,0 +1,7 @@
+0 0 0.5 0.5
+0.5 0.5 0 0
+0.2 0.2 0.2 0.2
+0 0 0 0
+0 0 0 0
+0 0 0 0
+
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv_P1.txt b/data/pgmm/timeInvModel/Zmu_timeInv_P1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a3da09a622ea1a54f5171d6c805946122056490a
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zmu_timeInv_P1.txt
@@ -0,0 +1,7 @@
+0 0
+0.5 0.5
+0 0
+0 0
+0 0
+0 0
+
diff --git a/data/pgmm/timeInvModel/Zmu_timeInv_P2.txt b/data/pgmm/timeInvModel/Zmu_timeInv_P2.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ae5bdfc83da7ad3974afe6c3bbbd704008add008
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zmu_timeInv_P2.txt
@@ -0,0 +1,7 @@
+0 0
+-0.5 -0.5
+0 0
+0 0
+0 0
+0 0
+
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv.txt b/data/pgmm/timeInvModel/Zsigma_timeInv.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c0c3b64079cc4f5c7ac943173802913684a34fb6
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zsigma_timeInv.txt
@@ -0,0 +1,7 @@
+ 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001 0 0 0 0 0
+ 0 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001 0 0 0 0
+ 0 0 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001 0 0 0
+ 0 0 0 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001 0 0
+ 0 0 0 0 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001 0
+ 0 0 0 0 0 0.0001 0 0 0 0 0 0.01 0 0 0 0 0 0.01 0 0 0 0 0 0.0001
+
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv_P1.txt b/data/pgmm/timeInvModel/Zsigma_timeInv_P1.txt
new file mode 100644
index 0000000000000000000000000000000000000000..62370b699fefa2f0726817305c0eea26fe1bb595
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zsigma_timeInv_P1.txt
@@ -0,0 +1,7 @@
+ 0.001 0 0 0 0 0 0.1 0 0 0 0 0
+ 0 0.001 0 0 0 0 0 0.1 0 0 0 0
+ 0 0 0.001 0 0 0 0 0 0.1 0 0 0
+ 0 0 0 0.001 0 0 0 0 0 0.1 0 0
+ 0 0 0 0 0.001 0 0 0 0 0 0.1 0
+ 0 0 0 0 0 0.001 0 0 0 0 0 0.1
+
diff --git a/data/pgmm/timeInvModel/Zsigma_timeInv_P2.txt b/data/pgmm/timeInvModel/Zsigma_timeInv_P2.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fb87e90d9095ee5b8f64a17149e7061a4b3352a8
--- /dev/null
+++ b/data/pgmm/timeInvModel/Zsigma_timeInv_P2.txt
@@ -0,0 +1,7 @@
+0.1 0 0 0 0 0 0.001 0 0 0 0 0
+0 0.1 0 0 0 0 0 0.001 0 0 0 0
+0 0 0.1 0 0 0 0 0 0.001 0 0 0
+0 0 0 0.1 0 0 0 0 0 0.001 0 0
+0 0 0 0 0.1 0 0 0 0 0 0.001 0
+0 0 0 0 0 0.1 0 0 0 0 0 0.001
+
diff --git a/data/pgmm/timeInvModel/pgmm_timeInv.txt b/data/pgmm/timeInvModel/pgmm_timeInv.txt
new file mode 100644
index 0000000000000000000000000000000000000000..675283eb7c761a3bfb051cfbac1db40887af0792
--- /dev/null
+++ b/data/pgmm/timeInvModel/pgmm_timeInv.txt
@@ -0,0 +1,6 @@
+ 6.0000000e+00
+ 2.0000000e+00
+ 2.0000000e+00
+ 5.0000000e+02
+ 6.0000000e+01
+ 2.0000000e-03
diff --git a/data/pgmm/timeInvModel/priors_timeInv.txt b/data/pgmm/timeInvModel/priors_timeInv.txt
new file mode 100644
index 0000000000000000000000000000000000000000..ee2f391a160b2f13d4d15bbdd524f8059c15e051
--- /dev/null
+++ b/data/pgmm/timeInvModel/priors_timeInv.txt
@@ -0,0 +1,2 @@
+0.5 0.5
+
diff --git a/data/pgmm/timeInvModel/vars_timeInv.txt b/data/pgmm/timeInvModel/vars_timeInv.txt
new file mode 100644
index 0000000000000000000000000000000000000000..46a7d7c25806904da4e50183d26cf7625ae7eeb2
--- /dev/null
+++ b/data/pgmm/timeInvModel/vars_timeInv.txt
@@ -0,0 +1,2 @@
+x_in y_in z_in x_out y_out z_out
+
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dfefe4c7417587d34f96980f60f7ab95641aa7f3
--- /dev/null
+++ b/examples/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8)
+PROJECT(pbdlib)
+
+add_executable(test_gmm test_gmm.cpp)
+target_link_libraries(test_gmm pbd)
+
+add_executable(test_gmr test_gmr.cpp)
+target_link_libraries(test_gmr pbd)
+
+add_executable(test_gmr2 test_gmr2.cpp)
+target_link_libraries(test_gmr2 pbd)
+
+add_executable(test_tpgmm_timeInvariant test_tpgmm_timeInvariant.cpp)
+target_link_libraries(test_tpgmm_timeInvariant pbd)
+
+add_executable(test_lqr test_lqr.cpp)
+target_link_libraries(test_lqr pbd)
+
+add_executable(test_onlineDP test_onlineDP.cpp)
+target_link_libraries(test_onlineDP pbd)
+
+add_executable(test_rewardWeightedRefinement test_rewardWeightedRefinement.cpp)
+target_link_libraries(test_rewardWeightedRefinement pbd)
+
+add_executable(test_datapoints test_datapoints.cpp)
+target_link_libraries(test_datapoints pbd)
+
+add_executable(test_demonstration test_demonstration.cpp)
+target_link_libraries(test_demonstration pbd)
+
+add_executable(test_tpdemonstration test_tpdemonstration.cpp)
+target_link_libraries(test_tpdemonstration pbd)
+
+add_executable(test_mvn test_mvn.cpp)
+target_link_libraries(test_mvn pbd)
+
+add_executable(test_HSMM test_hsmm.cpp)
+target_link_libraries(test_HSMM pbd)
diff --git a/examples/data.csv b/examples/data.csv
new file mode 100755
index 0000000000000000000000000000000000000000..84d53aace5d576ba3e57a541c313e97e0e7beb22
--- /dev/null
+++ b/examples/data.csv
@@ -0,0 +1,200 @@
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068151
+-0.082394,-0.068152
+-0.082378,-0.068155
+-0.081687,-0.068319
+-0.080945,-0.068468
+-0.079977,-0.068612
+-0.079434,-0.068601
+-0.078752,-0.068765
+-0.078346,-0.068913
+-0.077863,-0.069059
+-0.077419,-0.069043
+-0.076856,-0.069042
+-0.076383,-0.069042
+-0.075905,-0.069042
+-0.075352,-0.069042
+-0.074704,-0.069042
+-0.073753,-0.069042
+-0.073005,-0.069042
+-0.072234,-0.069042
+-0.071674,-0.069042
+-0.070825,-0.069042
+-0.069984,-0.069042
+-0.069139,-0.069042
+-0.068414,-0.069042
+-0.067672,-0.069042
+-0.066948,-0.069042
+-0.066062,-0.069042
+-0.065174,-0.069042
+-0.064223,-0.069042
+-0.063224,-0.069043
+-0.062107,-0.068989
+-0.060982,-0.068842
+-0.059842,-0.068692
+-0.058749,-0.068598
+-0.057629,-0.068597
+-0.056719,-0.068597
+-0.055622,-0.068597
+-0.054388,-0.068597
+-0.052765,-0.068597
+-0.051501,-0.068597
+-0.050344,-0.068597
+-0.049266,-0.068597
+-0.048116,-0.068597
+-0.046853,-0.068597
+-0.045609,-0.068597
+-0.044176,-0.068597
+-0.042627,-0.068597
+-0.040775,-0.068597
+-0.038567,-0.068597
+-0.03649,-0.068597
+-0.034641,-0.068596
+-0.032726,-0.068645
+-0.030486,-0.068793
+-0.027966,-0.068942
+-0.025945,-0.069041
+-0.02427,-0.069107
+-0.022706,-0.069255
+-0.021146,-0.069404
+-0.019209,-0.069488
+-0.017422,-0.069503
+-0.014873,-0.069356
+-0.012387,-0.069224
+-0.0096192,-0.068913
+-0.0075533,-0.068668
+-0.0055986,-0.068337
+-0.0038396,-0.068058
+-0.0019648,-0.067687
+-0.00029778,-0.067273
+0.0012646,-0.066657
+0.0027773,-0.066071
+0.0041089,-0.065458
+0.0054553,-0.065006
+0.0064648,-0.064555
+0.0075943,-0.064139
+0.0087145,-0.063551
+0.010193,-0.062832
+0.011628,-0.061999
+0.012856,-0.061119
+0.013874,-0.059991
+0.01482,-0.05887
+0.015683,-0.057894
+0.016577,-0.057079
+0.017276,-0.056179
+0.017999,-0.054902
+0.018637,-0.053539
+0.019352,-0.052029
+0.019969,-0.050515
+0.020415,-0.048875
+0.020889,-0.047214
+0.021206,-0.045725
+0.021367,-0.044161
+0.021194,-0.0425
+0.020888,-0.04068
+0.020426,-0.038966
+0.019996,-0.03732
+0.019536,-0.035751
+0.01907,-0.034337
+0.018527,-0.032988
+0.017891,-0.031691
+0.017125,-0.030414
+0.016234,-0.029054
+0.015385,-0.027795
+0.014421,-0.026378
+0.013562,-0.024958
+0.012522,-0.023521
+0.011731,-0.022419
+0.010841,-0.02157
+0.009797,-0.0206
+0.0086814,-0.019597
+0.0076521,-0.018633
+0.0069458,-0.01797
+0.0060823,-0.017282
+0.0050601,-0.016597
+0.0038789,-0.015775
+0.0027138,-0.014959
+0.0012034,-0.013984
+-0.00028179,-0.013176
+-0.0019616,-0.012486
+-0.0035161,-0.011811
+-0.0051711,-0.011096
+-0.0069248,-0.010243
+-0.0088179,-0.0092795
+-0.011021,-0.0080833
+-0.013347,-0.0067082
+-0.015722,-0.0052943
+-0.017779,-0.0040924
+-0.019596,-0.0028025
+-0.021325,-0.0015108
+-0.023298,0.0002239
+-0.025116,0.0019443
+-0.026929,0.0038161
+-0.028583,0.0056918
+-0.030099,0.007467
+-0.031415,0.0093155
+-0.03277,0.011482
+-0.034095,0.013725
+-0.035137,0.015862
+-0.035669,0.017311
+-0.036206,0.019018
+-0.036742,0.020935
+-0.037485,0.022875
+-0.038168,0.024782
+-0.038749,0.026619
+-0.039053,0.028823
+-0.039037,0.030941
+-0.038638,0.033282
+-0.038014,0.035871
+-0.037128,0.038602
+-0.036277,0.040889
+-0.035166,0.042934
+-0.03399,0.044689
+-0.032421,0.046423
+-0.03068,0.04795
+-0.028419,0.049379
+-0.026156,0.05066
+-0.023731,0.051782
+-0.02125,0.052903
+-0.018634,0.053978
+-0.015666,0.055105
+-0.013159,0.055795
+-0.010508,0.056446
+-0.0079871,0.056865
+-0.0048888,0.057307
+-0.0018382,0.057606
+0.0014204,0.057749
+0.00481,0.057909
+0.0086031,0.057902
+0.012409,0.057756
+0.016489,0.057528
+0.020271,0.057084
+0.02406,0.056718
+0.027224,0.056199
+0.030641,0.055525
+0.034283,0.054689
+0.038111,0.053857
+0.042136,0.053344
+0.045952,0.053059
+0.049879,0.053006
+0.053413,0.053007
+0.056465,0.053007
+0.05906,0.053011
+0.061465,0.053174
+0.063735,0.05331
+0.066084,0.053637
+0.068435,0.054192
+0.071132,0.055055
+0.073568,0.056049
+0.075369,0.056731
+0.076145,0.057216
+0.076309,0.057399
+0.076291,0.057038
+0.076291,0.056125
diff --git a/examples/test_datapoints.cpp b/examples/test_datapoints.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d3365962b83c60ad6798a77e04afe4fb2ecde606
--- /dev/null
+++ b/examples/test_datapoints.cpp
@@ -0,0 +1,48 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_datapoint.cpp
+\brief testing datapoint class
+Testing basic features of the Datapoint class
+
+\author Davide De Tommaso and Milad Malekzadeh
+\bug No known bugs.
+*/
+
+#include "pbdlib/datapoints.h"
+#include <sstream>
+
+
+using namespace pbdlib;
+using namespace arma;
+
+int main(int argc, char **argv)
+{
+ mat A = randu<mat>(3,100);
+ Datapoints datapoint(3,100);
+ datapoint.setData(A);
+
+ cout << "\n Data \n " << datapoint.getData();
+ datapoint.saveInFile("data01.txt");
+
+ datapoint.loadFromFile("data01.txt");
+ cout << "\n Data \n " << datapoint.getData();
+
+ return 0;
+}
diff --git a/examples/test_demonstration.cpp b/examples/test_demonstration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42907415700bc0997e811e395740088e51ec300e
--- /dev/null
+++ b/examples/test_demonstration.cpp
@@ -0,0 +1,66 @@
+/**
+Copyright (C) 2014, Milad Malekzadeh, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_demonstration.cpp
+\brief testing Demonstration class
+Testing basic features of the Demonstration class
+
+\author Milad Malekzadeh and Davide De Tommaso
+\bug No known bugs.
+*/
+
+#include "pbdlib/demonstration.h"
+#include <sstream>
+
+using namespace pbdlib;
+using namespace arma;
+
+int main(int argc, char **argv)
+{
+ mat A = randu<mat>(3,100);
+ Datapoints datapoint(3,100);
+ datapoint.setData(A);
+
+ cout << "\n Data \n " << datapoint.getData();
+
+ datapoint.saveInFile("data02.txt");
+
+ Demonstration demo = Demonstration(4,200);
+ std::vector<Demonstration> demos;
+ std::vector<std::string> vars;
+
+ vars.push_back("t");
+ vars.push_back("x");
+ vars.push_back("y");
+ vars.push_back("z");
+
+ demo.getDatapoints().setVarNames(vars);
+
+ demo.getDatapoints().loadFromFile("data02.txt");
+
+ cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
+ cout << endl << "nbVar is: " << demo.getDatapoints().getNumVARS() << endl;
+ cout << endl << "nbDatapoints is: " << demo.getDatapoints().getNumPOINTS() << endl;
+
+ demos.push_back(demo);
+
+ demo.saveInFile("demo01.txt");
+
+ return 0;
+}
diff --git a/examples/test_dmp.cpp b/examples/test_dmp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a96b49f4c401b9c7d4d396771decaf4a9418dc1
--- /dev/null
+++ b/examples/test_dmp.cpp
@@ -0,0 +1,43 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file repro_gmr.cpp
+\brief Learning GMM model and testing reproductions with GMR
+Learning a GMM model from a demonstration saved in the file data_txyz.txt and after
+
+\author Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include <sstream>
+
+#define nbStates 3
+#define nbVar 4
+#define nbData 200
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+
+ return 0;
+}
+
diff --git a/examples/test_gmm.cpp b/examples/test_gmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..279b36ecf91fae0636633abf26b2c573467f47b8
--- /dev/null
+++ b/examples/test_gmm.cpp
@@ -0,0 +1,80 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file learn_gmm.cpp
+\brief Learning GMM model
+Learning a GMM model from a demonstration saved in the file data_txyz.txt
+
+\author Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include <sstream>
+
+#define nbStates 3
+#define nbVar 4
+#define nbData 200
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ std::vector<Demonstration> demos;
+ Demonstration demo = Demonstration(nbVar,nbData);
+ std::vector<std::string> vars;
+ vars.push_back("t");
+ vars.push_back("x");
+ vars.push_back("y");
+ vars.push_back("z");
+
+ demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().setVarNames(vars);
+
+ cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
+ cout << endl << "nbVar is: " << demo.getDatapoints().getNumVARS() << endl;
+ cout << endl << "nbDatapoints is: " << demo.getDatapoints().getNumPOINTS() << endl;
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ demos.push_back(demo);
+
+ GMM_Model *gmm;
+ gmm = new GMM_Model(demos,nbStates);
+ gmm->setVARSNames(vars);
+
+ cout << "\n Number of EM iterations: " << gmm->EM_learn();
+ for (int i=0;i<nbStates;i++){
+ cout << "\n Mu_" << i << " = \n" << gmm->getMU(i);
+ cout << "\n Sigma_" << i << " = \n" << gmm->getSIGMA(i);
+ }
+ //Save GMM in files GMM_priors.txt, GMM_mu.txt, GMM_sigma.txt and GMM_vars.txt
+ gmm->saveInFiles();
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ //Loading the GMM model from files...
+ GMM_Model *gmm2 = new GMM_Model("GMM_priors.txt", "GMM_mu.txt", "GMM_sigma.txt", "GMM_vars.txt");
+
+ for (int i=0;i<nbStates;i++)
+ gmm2->setMU(i, gmm2->getMU(i)*10.0);
+ for (int i=0;i<nbStates;i++)
+ cout << "\n Mu_" << i << " = \n" << gmm2->getMU(i);
+ return 0;
+}
+
diff --git a/examples/test_gmr.cpp b/examples/test_gmr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f0616f50d1e0c9f6e8ad8060c2d01b4a57c41e76
--- /dev/null
+++ b/examples/test_gmr.cpp
@@ -0,0 +1,88 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file repro_gmr.cpp
+\brief Learning GMM model and testing reproductions with GMR
+Learning a GMM model from a demonstration saved in the file data_txyz.txt and after
+
+\author Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include <sstream>
+
+#define nbStates 3
+#define nbVar 4
+#define nbData 200
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ std::vector<Demonstration> demos;
+ Demonstration demo = Demonstration(nbVar,nbData);
+ std::vector<std::string> vars,vars2;
+ vars.push_back("t");
+ vars.push_back("x");
+ vars.push_back("y");
+ vars.push_back("z");
+
+ demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().setVarNames(vars);
+
+ cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
+ cout << endl << "nbVar is: " << demo.getDatapoints().getNumVARS() << endl;
+ cout << endl << "nbDatapoints is: " << demo.getDatapoints().getNumPOINTS() << endl;
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ demos.push_back(demo);
+
+ GMM_Model* gmm;
+ gmm = new GMM_Model(demos,nbStates);
+ gmm->setVARSNames(vars);
+
+ cout << "\n Number of EM iterations: " << gmm->EM_learn();
+ for (int i=0;i<nbStates;i++){
+ cout << "\n Mu_" << i << " = \n" << gmm->getMU(i);
+ cout << "\n Sigma_" << i << " = \n" << gmm->getSIGMA(i);
+ }
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ Datapoints *Repros;
+ Repros = new Datapoints(1,nbData);
+
+ vars2.push_back("t");
+ Repros->setVarNames(vars2);
+
+ mat data = demo.getDatapoints().getData().submat(0,0,0,nbData-1);
+ Repros->setData(data);
+ GMR* reg = new GMR(gmm);
+ GMM_Model* gmmOut = reg->regression(Repros);
+
+ cout << "\n\n Reproduction with GMR:\n";
+ for (int t=0;t<nbData;t+=40){
+ cout << "\n MU_" << t << " = \n" << gmmOut->getMU(t);
+ cout << "\n SIGMA_" << t << " = \n" << gmmOut->getSIGMA(t);
+ }
+
+ return 0;
+}
+
diff --git a/examples/test_gmr2.cpp b/examples/test_gmr2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b45aa0e5b295ef058c7707d0ea0dad9731290331
--- /dev/null
+++ b/examples/test_gmr2.cpp
@@ -0,0 +1,104 @@
+/**
+Copyright (C) 2015, Martijn Zeestraten, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file repro_gmr.cpp
+\brief Learning GMM model and testing reproductions with GMR
+Learning a GMM model from a demonstration saved in the file data_txyz.txt and after
+
+
+In this example of GMR we set the input and output variables of the regression
+by index and not by name. This operation is faster, and allows for more flexibility when
+performing online regression. In addition, we show that we can perform regression on only part
+of the GMM (i.e we use x and y and input and z as output. t is not used)
+
+\author Martijn Zeestraten, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+\bug No known bugs.
+*/
+
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include <sstream>
+
+#define nbStates 3
+#define nbVar 4
+#define nbData 200
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ // Get Demonstration data
+ std::vector<Demonstration> demos;
+ Demonstration demo = Demonstration(nbVar,nbData);
+ std::vector<std::string> vars,vars2;
+ vars.push_back("t");
+ vars.push_back("x");
+ vars.push_back("y");
+ vars.push_back("z");
+
+ demo.getDatapoints().loadFromFile("../../data/data_txyz.txt");
+ demo.getDatapoints().setVarNames(vars);
+
+ cout << endl << "Data is: " << demo.getDatapoints().getData() << endl;
+ cout << endl << "nbVar is: " << demo.getDatapoints().getNumVARS() << endl;
+ cout << endl << "nbDatapoints is: " << demo.getDatapoints().getNumPOINTS() << endl;
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ demos.push_back(demo);
+ // Create GMM
+ GMM_Model* gmm;
+ gmm = new GMM_Model(demos,nbStates);
+ gmm->setVARSNames(vars);
+ cout << "\n Number of EM iterations: " << gmm->EM_learn();
+
+ for (int i=0;i<nbStates;i++){
+ cout << "\n Mu_" << i << " = \n" << gmm->getMU(i);
+ cout << "\n Sigma_" << i << " = \n" << gmm->getSIGMA(i);
+ }
+ cout << "\nPress enter to continue..." << endl; getchar();
+
+ // Perform GMR
+ // In this example of GMR we set the input and output variables of the regression
+ // by index and not by name. This operation is faster, and allows for more flexibility when
+ // performing online regression. In addition, we show that we can perform regression on only part
+ // of the GMM (i.e we use x and y and input and z as output. t is not used)
+
+ // Input data: We select x, and y as input data
+ cout<< "Get Data" << endl;
+ mat dataIn = demo.getDatapoints().getData().submat(1,0,2,nbData-1);
+ cout << "Data In" << endl << dataIn << endl;
+ GMR* reg = new GMR(gmm);
+
+ // Perform Regression
+ urowvec in, out;
+ in << 1 << 2; // The input variables for regression (x, y)
+ out << 3; // The output variables for regression (z)
+ GMM_Model* gmmOut = reg->regression(dataIn,in,out);
+
+ cout << "\n\n Reproduction with GMR, output: z, input : x,y :\n";
+
+ for (int t=0;t<nbData;t+=40){
+ cout << "\n MU_" << t << " = \n" << gmmOut->getMU(t);
+ cout << "\n SIGMA_" << t << " = \n" << gmmOut->getSIGMA(t);
+ }
+
+ return 0;
+}
+
diff --git a/examples/test_hsmm.cpp b/examples/test_hsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19961634f4f0fe51b2c2bf40214e82f654fde10d
--- /dev/null
+++ b/examples/test_hsmm.cpp
@@ -0,0 +1,108 @@
+/**
+Copyright (C) 2015, Martijn Zeestraten,Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_hsmm.cpp
+\brief Learning GMM model
+Learning a GMM model from a demonstration saved in the file data_txyz.txt
+
+\author Martijn Zeestraten, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "pbdlib/hsmm.h"
+#include "armadillo"
+
+using namespace arma;
+using namespace pbdlib;
+using namespace std;
+
+int main()
+{
+
+ // Create Object
+ HSMM* myHSMM;
+
+ // Load data from file
+ string muPath = "./data/HSMM_test_mu.txt";
+ string sigmaPath = "./data/HSMM_test_sigma.txt";
+ string priorsPath = "./data/HSMM_test_priors.txt";
+ string transPath = "./data/HSMM_test_trans.txt";
+ string durMu = "./data/HSMM_test_durMu.txt";
+ string durSigma = "./data/HSMM_test_durSigma.txt";
+
+ myHSMM = new HSMM(priorsPath,muPath,sigmaPath,transPath,durMu,durSigma);
+
+
+ colvec X = myHSMM->getMU(0);
+ // Display Data
+
+ cout << "--- HSMM components: " << endl;
+ cout << "nStates " << myHSMM->getNumSTATES() <<endl;
+ for (uint i=0;i<myHSMM->getNumSTATES();i++)
+ {
+ cout << "Priors" << endl;
+ cout << myHSMM->getPRIORS(i) << endl;
+ cout << "Mu" << endl;
+ cout << myHSMM->getMU(i) << endl;
+ cout << "Sigma" << endl;
+ cout << myHSMM->getSIGMA(i) << endl;
+ cout << "DurMu" << endl;
+ cout << myHSMM->getDurMU(i) << endl;
+ cout << "DurSigma" << endl;
+ cout << myHSMM->getDurSIGMA(i) << endl;
+ }
+
+ // Test alpha variable (Let the model step based on observations)
+ cout << "--> Forward variable steps: " << endl;
+ urowvec in;
+ in << 0 << 1;
+ cout << "Observations (constant): " << X.t() << endl;
+
+ cout << "Alpha values:" << endl;
+ for (uint i = 1;i<100;i++)
+ {
+ myHSMM->stepForwardVariable();
+
+ if (i%10 ==0)
+ {
+ cout << "Alpha " << i << " : [" <<
+ myHSMM->getForwardVariable()(0) << ", " <<
+ myHSMM->getForwardVariable()(1) << ", " <<
+ myHSMM->getForwardVariable()(2) << "]" << endl;
+ }
+ }
+
+
+ // Test Alpha Predictions
+ // (Let the model generate predictions (i.e. state of the model is not adjusted)
+ mat pred;
+
+ myHSMM->resetForwardVariable(); // Reset the forward variable
+
+ // Offline method (function allocates matrix itself)
+ pred = myHSMM->predictForwardVariable(200);
+
+ // Online method (we pre-allocate matrix ourselves)
+ pred.set_size(3,100);
+ myHSMM->predictForwardVariable(pred);
+
+ cout << "Alpha Predictions" << endl << pred.t() << endl;
+
+}
+
diff --git a/examples/test_lqr.cpp b/examples/test_lqr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..666d2bafb624100c720244df8397b3796b3aa21d
--- /dev/null
+++ b/examples/test_lqr.cpp
@@ -0,0 +1,89 @@
+
+/**
+Copyright (C) 2014, Danilo Bruno
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_lqr
+\brief Testing lqr
+
+\author Danilo Bruno
+\bug No known bugs.
+*/
+
+#include <iostream>
+#include <sstream>
+#include "pbdlib/lqr.h"
+#include "armadillo"
+#include <vector>
+
+
+using namespace pbdlib;
+using namespace arma;
+
+int main(int argc, char **argv)
+{
+ mat A,B;
+ float dt,rFactor;
+ int nbData = 400;
+ mat R,Target;
+ std::vector<float> DataIn;
+ std::vector<mat> Q;
+
+ A << 0 << 1 << endr << 0 << 0 << endr; //matrix for second order system
+ B << 0 << endr << 1 << endr; //matrix for force controller
+
+ dt = 0.01;
+ rFactor = 0.1;
+ R = rFactor;
+
+ Target = zeros(2,nbData);
+
+ Target.row(0) = 100*ones(1,nbData);
+
+ for (int t=0;t<nbData;t++){
+ DataIn.push_back(t*dt);
+ mat QTmp = zeros(2,2);
+ QTmp(0,0) = 1;
+ Q.push_back(QTmp);
+ }
+
+ LQR test_lqr(A,B,dt);
+
+ test_lqr.setProblem(R,Q,Target);
+
+ //Test infinite horizon
+ test_lqr.evaluate_gains_infiniteHorizon();
+
+ //Test finite horizon
+
+ //mat S;
+ //colvec d;
+
+ //S = zeros(2,2);
+ //d = zeros(2,1);
+
+ //test_lqr.evaluate_gains_finiteHorizon(S,d);
+
+ //Output data
+ for (int t=0;t<nbData;t++){
+ std::cout << t << " , " << test_lqr.getGains().at(t) << std::endl;
+ }
+ std::cout << test_lqr.getFF() << std::endl;
+
+ return 0;
+}
diff --git a/examples/test_mvn.cpp b/examples/test_mvn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0084179141d869124765e4735f2cf4ac6214246e
--- /dev/null
+++ b/examples/test_mvn.cpp
@@ -0,0 +1,66 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file learn_gmm.cpp
+\brief Learning GMM model
+Learning a GMM model from a demonstration saved in the file data_txyz.txt
+
+\author Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "pbdlib/mvn.h"
+#include <sstream>
+
+#define nbVar 4
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ // Make new distribution
+ //
+ colvec Mu;
+ mat SIGMA;
+
+ SIGMA << 0.0453 << 0.0021 << 0.0682 << endr
+ << 0.0021 << 0.4463 << 0.5093 << endr
+ << 0.0682 << 0.5093 << 0.8964;
+ SIGMA = SIGMA;
+ Mu << 0.4843 << 0.3222 << 0.5827;
+ GaussianDistribution G1(Mu,SIGMA);
+
+ cout << "Mu: " << endl << G1.getMU() << endl;
+ cout << "Sigma: " << endl << G1.getSIGMA() << endl;
+ cout << "Lambda: " << endl << G1.getLAMBDA() << endl;
+
+ cout << "getPDF(Mu): " << endl <<
+ G1.getPDFValue(Mu);
+
+ // Stochastic sampling
+ mat samps = G1.stochasticSampling(20);
+ cout << "Samples: " << endl;
+ for (int i = 0;i<20;i++)
+ cout << samps.col(i).t() << endl;
+
+ cout << "Probs of samples" << endl;
+ for (int i = 0;i<20;i++)
+ cout << G1.getPDFValue(samps.col(i)) << endl;
+}
+
diff --git a/examples/test_onlineDP.cpp b/examples/test_onlineDP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..91bc1baa09a4fa3203a1982defb86f56ed3c7f36
--- /dev/null
+++ b/examples/test_onlineDP.cpp
@@ -0,0 +1,93 @@
+
+/**
+Copyright (C) 2014, Danilo Bruno
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_onlineDP
+\brief Testing online Dirichlet Process clustering
+
+\author Danilo Bruno
+\bug No known bugs.
+*/
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include "pbdlib/gmm.h"
+#include "armadillo"
+#include <vector>
+#include <string>
+
+
+using namespace pbdlib;
+using namespace arma;
+
+int main(int argc, char **argv)
+{
+
+ //load data
+
+ std::ifstream dataFile("data.csv");
+
+ std::string line;
+
+ std::vector<colvec> Data;
+ colvec PTmp = zeros(2,1);
+
+
+ while(std::getline(dataFile,line)){
+ std::stringstream lineStream(line);
+ std::string charTmp;
+ std::getline(lineStream,charTmp,',');
+ PTmp(0) = std::atof(charTmp.c_str());
+ std::getline(lineStream,charTmp,',');
+ PTmp(1) = std::atof(charTmp.c_str());
+ Data.push_back(PTmp);
+ }
+
+ GMM_Model gmm(1,2);
+ double minSigma = 1E-5;
+ double lambda = 0.05;
+
+ mat minSIGMA = minSigma*eye(2,2);
+ std::vector<GaussianDistribution> comps;
+ GaussianDistribution componentTmp(Data[0],minSIGMA);
+
+ comps.push_back(componentTmp);
+ gmm.setCOMPONENTS(comps);
+
+ rowvec priorsTmp = zeros(1,1);
+ priorsTmp(0) = 1;
+ gmm.setPRIORS(priorsTmp);
+ int N = 1;
+ for (int i=1;i<Data.size();i++){
+ N++;
+ colvec P = Data.at(i);
+ gmm.onlineEMDP(N,P,lambda,minSigma);
+ }
+
+ for (int i=0;i<gmm.getNumSTATES();i++)
+ {
+ std::cout << gmm.getMU(i) << std::endl;
+ std::cout << gmm.getSIGMA(i) << std::endl;
+ std::cout << gmm.getPRIORS() << std::endl;
+ }
+
+
+ return 0;
+}
diff --git a/examples/test_rewardWeightedRefinement.cpp b/examples/test_rewardWeightedRefinement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e30fcd7cf38682d4a092cd9594e744d36b1172c5
--- /dev/null
+++ b/examples/test_rewardWeightedRefinement.cpp
@@ -0,0 +1,113 @@
+/**
+Copyright (C) 2014, Milad S. Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_rewardWeightedRefinement
+\brief Testing Expectation Maximization (EM) based reward-weighted refinement
+
+The "reward" function can be defined based on problem.
+
+\author Milad S. Malekzadeh, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#include "armadillo"
+#include "pbdlib/mvn.h"
+
+using namespace pbdlib;
+using namespace arma;
+
+// Reward function
+rowvec reward( const mat& sample )
+{
+ vec mu = 8 * ones<vec>(2,1);
+ mat sigma = 5 * eye<mat>(2,2);
+ GaussianDistribution *gd;
+ gd = new GaussianDistribution(mu, sigma);
+
+ return trans(gd->getPDFValue( sample )) / as_scalar(gd->getPDFValue(mu));
+}
+
+int main(int argc, char **argv)
+{
+ // Refinement parameters
+ double nEpisods = 150; // number of refinement iterations
+ double minSigma = 1e-1; // minimum exploration noise
+ uint nbImportanceSampling = 5; // number of importance sampling (choose 0 if there is no need)
+ uint nbS = 10; // number of samples used in stochastic sampling
+ uint nVars = 2; // number of refinement variables
+
+
+ // Set Initial policy and noise covariance
+ vec mu = ones<vec>(nVars,1);
+ mat sigma = eye<mat>(nVars,nVars);
+
+ GaussianDistribution *GD;
+ GD = new GaussianDistribution(mu, sigma);
+
+ mat minNoise, p, pNoisy;
+ rowvec r;
+
+ minNoise = zeros(nVars,nVars);
+ minNoise.diag() = minSigma * ones(nVars, 1);
+ //cout << endl << "minSigma:\n" << minNoise << endl;
+
+ // EM-based reward weighted refinement
+ for (uint i=0; i<=nEpisods; i++)
+ {
+ // Stochatic policy distribution sampling
+ pNoisy = GD->stochasticSampling( nbS );
+ //cout << endl << "pNoisy: " << endl << pNoisy; // DEBUG
+
+ // Reward evaluation of the policies
+ rowvec rNoisy = reward( pNoisy );
+ //cout << endl << "rr:\n" << rNoisy << endl; // DEBUG
+
+ // Add the new policy and reward to dataset
+ p = join_rows(p, pNoisy);
+ r = join_rows(r, rNoisy);
+
+ // after the first iteration nbS should be 1
+ nbS = 1;
+
+ // Reward-weighted update of policy distribution
+ //GD->setParamsFromData(p);
+ //GD->setParamsFromData(p, r);
+ GD->setParamsFromData(p, r, nbImportanceSampling);
+ GD->setSIGMA( GD->getSIGMA() + minNoise );
+ }
+
+ cout << "//////////////////////////////////////////////////" << endl;
+ cout << "/////////////// Before Refinement ////////////////" << endl;
+ cout << "//////////////////////////////////////////////////" << endl;
+
+ cout << endl << "Initial Mu: \n" << mu << endl;
+ cout << endl << "Initial Sigma: \n" << sigma << endl;
+
+ cout << "//////////////////////////////////////////////////" << endl;
+ cout << "/////////////// After Refinement /////////////////" << endl;
+ cout << "//////////////////////////////////////////////////" << endl;
+
+ cout << endl << "Refined Mu: \n" << GD->getMU() << endl;
+ cout << endl << "Refined Sigma: \n" << GD->getSIGMA() << endl;
+ cout << endl << "Returns: \n" << r << endl;
+ cout << endl << "Explored policies: \n" << p << endl;
+
+ return 0;
+}
+
diff --git a/examples/test_tpdemonstration.cpp b/examples/test_tpdemonstration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..23618ea510253e23d35982d9fed18947ac67d247
--- /dev/null
+++ b/examples/test_tpdemonstration.cpp
@@ -0,0 +1,128 @@
+/**
+Copyright (C) 2015,Martijn Zeestraten, Tohid Alizadeh, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file test_demonstration.cpp
+\brief testing Demonstration and Parameters classes
+Testing basic features of the Demonstration and Parameters classes
+
+\author Martijn Zeestraten
+\bug No known bugs.
+*/
+
+#include "pbdlib/tpdemonstration.h"
+#include "pbdlib/taskparameters.h"
+#include <sstream>
+
+using namespace pbdlib;
+using namespace arma;
+
+int main()
+{
+ uint nDemos = 4; // load from 4 files
+
+ // loading the demonstrations and task parameters from the txt files:
+ std::vector<TPDemonstration> demos; // Vector to hold demonstrations
+ TPDemonstration tmpDemo; // Temp variable to load individual demonstrations
+
+ char Datafilename[256];
+ char TPfilename[256];
+ cout << "Loading the demonstrations and the task parameters ..." << endl;
+ for (uint m=0; m<nDemos; m++){ //Loading demos in the loop
+ // Form filenames:
+ sprintf(Datafilename, "../../data/pgmm/Data0%d.txt",m+1);
+ sprintf(TPfilename, "../../data/pgmm/Param0%d.txt",m+1);
+
+ // Load files in demo:
+ // note we need to transpose the data (last argument)
+ // because the data in the file a row for each data point
+ // and a column for each variable. The agreement is
+ // the other way around
+ tmpDemo.loadFromFiles(Datafilename,TPfilename, true);
+
+ // push back demonstration:
+ demos.push_back(tmpDemo);
+ }
+
+ cout << "Demonstrations Loaded succesfully." << endl;
+
+ // Show Data:
+ for (uint m =0;m<nDemos ;m++)
+ {
+ cout << "----- DEMONSTRATION : " << m << endl;
+ cout << " NbVar: " << demos[m].getNumVARS() << endl;
+ cout << " NbPoints: " << demos[m].getNumPOINTS() << endl;
+ cout << "NbTask Parameters: " << demos[m].getNumTASKPARAMETERS() << endl;
+ cout << "----- Global Data.cols(0,10): " << endl;
+ cout << demos[m].getDataInGlobalSpace().getData().cols(0,10).t() << "..." << endl;
+ cout << "[Enter] To continue to Task Parameter Info" << endl;
+ getchar();
+ for (uint i=0;i<demos[m].getNumTASKPARAMETERS();i++)
+ {
+ cout << " Info Task Parameter " << i << endl;
+ // For datapoint 0 show task parameter i:
+ cout << "b^T: " <<
+ demos[m].getDataPointTPs(0).getTaskParameters(i).b.t()
+ << endl;
+ cout << "A: " << endl <<
+ demos[m].getDataPointTPs(0).getTaskParameters(i).A.t()
+ << endl;
+ cout << "Data.cols(0,10) in Task Parameter " << i << " space" << endl;
+ cout << demos[m].getDataInTaskParameters(i).cols(0,10).t() << "...." << endl;
+ cout << "[Enter] To continue to next Task Parameter" << endl;
+ getchar();
+ }
+ }
+
+ // Replacing Task Parameters:
+ cout << "Replacing Task variables for all Data...[Enter] " << endl;
+ getchar();
+
+ // Create new Idendity Parameters:
+ TaskParameters newTPs(demos[0].getNumVARS(), 1);
+ newTPs.getTaskParameters(0).A = eye<mat>(newTPs.getNumVARS(),newTPs.getNumVARS());
+ newTPs.getTaskParameters(0).b = zeros(newTPs.getNumVARS(),1);
+
+ // Apply parameters:
+
+ for (uint m=0;m<nDemos;m++)
+ {
+ // Set the same parameters for all datapoints:
+ demos[m].setDataPointTPs(newTPs);
+
+ // Display new info:
+ for (uint i=0;i<demos[m].getNumTASKPARAMETERS();i++)
+ {
+ cout << " Info Task Parameter " << i << endl;
+ // For datapoint 0 show task parameter i:
+ cout << "b^T: " <<
+ demos[m].getDataPointTPs(0).getTaskParameters(i).b.t()
+ << endl;
+ cout << "A: " << endl <<
+ demos[m].getDataPointTPs(0).getTaskParameters(i).A.t()
+ << endl;
+ cout << "Data.cols(0,10) in Task Parameter " << i << " space" << endl;
+ cout << demos[m].getDataInTaskParameters(i).cols(0,10).t() << "...." << endl;
+ cout << "[Enter] To continue to next Task Parameter" << endl;
+ getchar();
+ }
+
+ }
+ return 0;
+
+}
diff --git a/examples/test_tpgmm.cpp b/examples/test_tpgmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..07a345f8a1ade848ca0124045af85224874950f7
--- /dev/null
+++ b/examples/test_tpgmm.cpp
@@ -0,0 +1,184 @@
+/*
+* Copyright (c) 2014
+* - Davide De Tommaso @ dtmdvd[at]gmail[dot]com
+* - Milad Malekzadeh @ milad[dot]malekzadeh[at]gmail[dot]com
+* - Leonel Rozo @ leonel[dot]rozo[at]iit[dot]it
+* - Sylvain Calinon @ sylvain[dot]calinon[at]gmail[dot]com
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in the
+* documentation and/or other materials provided with the distribution.
+* * Neither the name of the <organization> nor the
+* names of its contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
+* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+*
+*
+* ----------------- NOTE ---------------------------
+* This example is a modified version of the 'old' example pgmm_test. This
+* new example uses the tpgmm class instead of pgmm. Changes that need to be
+* made when swithcing from pgmm to tpgmm are:
+* - Delaration PGMM_Model -> TPGMM
+* - Product of Gaussian: prodGMM -> transformed
+*
+*
+*
+*
+*/
+
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include "pbdlib/datapoints.h"
+#include "pbdlib/tpdemonstration.h"
+#include "pbdlib/taskparameters.h"
+#include <sstream>
+
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ // Model variables
+ uint nFrames = 2;
+ uint nStates = 3;
+ uint nVars = 3; //t,x,y
+ uint nDemos = 4; //Number of Demonstrations
+ uint nData = 200; //Number of datapoints in a demonstration
+ //
+ // time is input (t) and the attractor position (2D vector [x y]) is the output.
+ std::vector<std::string> varNames;
+ varNames.push_back("t");
+ varNames.push_back("x");
+ varNames.push_back("y");
+
+ //..................................................................................
+ //......... loading the demonstrations and task parameters from the txt files ......
+
+ std::vector<TPDemonstration> demos; // Vector to hold demonstrations
+ TPDemonstration tmpDemo; // Temp variable to load individual demonstrations
+
+ char Datafilename[256];
+ char TPfilename[256];
+ cout << "Loading the demonstrations and the task parameters ..." << endl;
+ for (uint m=0; m<nDemos; m++){ //Loading demos in the loop
+ // Form filenames:
+ sprintf(Datafilename, "../../data/pgmm/Data0%d.txt",m+1);
+ sprintf(TPfilename, "../../data/pgmm/Param0%d.txt",m+1);
+
+ // Load files in demo:
+ // note we need to transpose the data (last argument)
+ // because the data in the file a row for each data point
+ // and a column for each variable. The agreement is
+ // the other way around
+ tmpDemo.loadFromFiles(Datafilename,TPfilename, true);
+
+ // push back demonstration:
+ demos.push_back(tmpDemo);
+ }
+
+ cout << "Demonstrations Loaded succesfully." << endl;
+ cout<<"Press any key to continue..."<<endl; getchar();
+
+ //..................................................................................
+ //......... Learning the PGMM model parameters using the EM algorithm ......
+ cout<<"Learning model parameters using EM..." << endl;
+
+ TPGMM *tpgmm;
+ tpgmm = new TPGMM(demos, nStates);
+
+ cout << "\n Number of EM iterations: " << tpgmm->estimateTensorGMM() << endl;
+
+ // saving the learned model parameters
+ mat MuTmp = zeros(nVars, nStates*nFrames);
+ mat SigmaTmp = zeros(nVars, nVars*nStates*nFrames);
+ mat Priors = zeros(1,nStates);
+ std::vector<GMM_Model> GMMSresult;
+ GMMSresult = tpgmm->getGMMS();
+ for(uint m=0; m<nFrames; m++){
+ for (uint i=0;i<nStates; i++){
+ MuTmp.col(m*nStates+i) = GMMSresult[m].getMU(i);
+ SigmaTmp.cols(m*nVars*nStates+i*nVars, m*nVars*nStates+i*nVars+nVars-1) = GMMSresult[m].getSIGMA(i);
+ }
+ }
+ sprintf(Datafilename, "../../data/pgmm/Mu0%d.txt",2);
+ MuTmp.save(Datafilename, raw_ascii);
+ sprintf(Datafilename, "../../data/pgmm/Sigma0%d.txt",2);
+ SigmaTmp.save(Datafilename, raw_ascii);
+ sprintf(Datafilename, "../../data/pgmm/Priors0%d.txt",2);
+ Priors = tpgmm->getPRIORS();
+ Priors.save(Datafilename, raw_ascii);
+ cout<<"the learned model parameters are written into text files succesfully."<<endl; cout<<"Press any key to continue..."<<endl; getchar();
+
+
+ // Reproduction variables (GMR)
+ GMR *gmr = new GMR();
+ Datapoints *repro = new Datapoints(1,1); // Only one datapoint passed at each time step
+ mat reproData, auxYgmr;
+ std::vector<std::string> reproVarNames;
+ reproVarNames.push_back("t"); // time is the query point
+ repro->setVarNames(reproVarNames);
+
+ // loading new parameters for the reproduction from the text file
+ // In this case we have considered that the parameters are the same all along the reproduction. They can be varying.
+ TaskParameters ReproTPs(nVars,nFrames);
+ sprintf(TPfilename, "../../data/pgmm/ParamRepro0%d.txt",1);
+ ReproTPs.loadFromFile(TPfilename);
+
+ GMM_Model* gmm;
+ for(uint tn=1; tn<=nData; tn++){
+ // Computing the resulting GMM given the set of parameters {A,b}
+ gmm = tpgmm->getTransformedGMM(ReproTPs);
+ gmm->setVARSNames(varNames);
+
+ // Printing model components
+ cout << "Resulting GMM given the set of parameters 'A' and 'b'" << endl;
+ cout << "nbStates : " << gmm->getNumSTATES() << endl;
+ cout << "nbVar : " << gmm->getNumVARS() << endl;
+ cout << "VarNames " ;
+ for (uint i=0;i<gmm->getNumVARS();i++)
+ cout << varNames[i] ;
+ cout << endl;
+
+ cout << "Priors : " << gmm->getPRIORS() << endl;
+ for(uint i=0; i<nStates; i++){
+ cout << "State #" << i << ":"<< endl;
+ gmm->getMU(i).print("Mu = ");
+ gmm->getSIGMA(i).print("Sigma = ");
+ }
+
+ // Computing GMR
+ gmr->setGMMModel(gmm);
+ mat repDataPt;
+ repDataPt << (tn * 0.01); // time step as input
+ repDataPt.print("tn = ");
+ repro->setData(repDataPt);
+ GMM_Model* gmmOut = gmr->regression(repro);
+ gmmOut->getMU(0).print("MuOut = ");
+
+ cout << "Please press [ENTER] to continue or [CTRL+C] to finish" << endl;
+ char key;
+ std::cin.ignore(1);
+ }
+ return 0;
+}
+
+
+
+
diff --git a/examples/test_tpgmm_timeInvariant.cpp b/examples/test_tpgmm_timeInvariant.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce204451d3a22a0a38205316a3e09cb54f231aad
--- /dev/null
+++ b/examples/test_tpgmm_timeInvariant.cpp
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2014
+ * - João Silvério @ joao[dot]silverio[at]iit[dot]it
+ * - Davide De Tommaso @ dtmdvd[at]gmail[dot]com
+ * - Milad Malekzadeh @ milad[dot]malekzadeh[at]gmail[dot]com
+ * - Leonel Rozo @ leonel[dot]rozo[at]iit[dot]it
+ * - Sylvain Calinon @ sylvain[dot]calinon[at]gmail[dot]com
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the <organization> nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "pbdlib/tpgmm.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/gmr.h"
+#include "pbdlib/datapoints.h"
+#include "pbdlib/taskparameters.h"
+#include <sstream>
+#include <math.h>
+#include <iostream>
+
+using namespace std;
+using namespace pbdlib;
+
+int main(int argc, char **argv)
+{
+ // Filenames that contain model parameters
+ std::string fParamsTPGMM("../../data/pgmm/timeInvModel/pgmm_timeInv.txt");
+ std::string fVarsTPGMM ("../../data/pgmm/timeInvModel/vars_timeInv.txt");
+ std::string fPriorsTPGMM("../../data/pgmm/timeInvModel/priors_timeInv.txt");
+ std::string fMuTPGMM ("../../data/pgmm/timeInvModel/Zmu_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
+ std::string fSigmaTPGMM ("../../data/pgmm/timeInvModel/Zsigma_timeInv"); // Initialize without file extension -> "_P#" will be added depending on nParams
+
+ // Loading files and setting pgmm variables
+ vec ModelParams;// The model parameters are loaded from the PGMM model file
+ ModelParams.load(fParamsTPGMM);
+ uint nVars = ModelParams[0];
+ uint nFrames = ModelParams[1];
+ uint nStates = ModelParams[2];
+
+ // According to GMM class, the name of the variables should also come in a separate .txt file.
+ // I'll use a toy file for now, during object initialization, and keep it like below for the PGMM object construction.
+ std::vector<std::string> varNames;
+ varNames.push_back("x_in");
+ varNames.push_back("y_in");
+ varNames.push_back("z_in");
+ varNames.push_back("x_out");
+ varNames.push_back("y_out");
+ varNames.push_back("z_out");
+
+ TPGMM* tpgmm;
+ tpgmm = new TPGMM(nVars, nStates, nFrames);
+ tpgmm->loadTPGMMfromMATLAB(fPriorsTPGMM, fVarsTPGMM, fMuTPGMM, fSigmaTPGMM);
+
+ // Reproduction variables (GMR)
+ GMR *gmr = new GMR();
+ Datapoints *repro = new Datapoints(3,1); // Only one datapoint passed at each time step
+
+ std::vector<std::string> reproVarNames;
+ reproVarNames.push_back("x_in"); // time is the query point
+ reproVarNames.push_back("y_in");
+ reproVarNames.push_back("z_in");
+ repro->setVarNames(reproVarNames);
+
+
+ // Defining "artificial" parameters (New Implementation TPGMM)
+ TaskParameters TPs(nVars, nFrames);
+ TaskParameter auxTP;
+
+ // First Frame:
+ auxTP.A = eye<mat>(nVars,nVars);
+ auxTP.b = zeros(nVars,1);
+ TPs.setTaskParameters(0,auxTP);
+
+ // Second and third frame
+ for (uint i=0;i<2;i++)
+ {
+ auxTP.b.at(3) = tpgmm->getGMMS(i).getMU(0)[0]; // the object positions are set in the .txt file
+ auxTP.b.at(4) = tpgmm->getGMMS(i).getMU(0)[1];
+ auxTP.b.at(5) = tpgmm->getGMMS(i).getMU(0)[2];
+ TPs.setTaskParameters(i,auxTP);
+ }
+
+ /* OLD IMPLEMENTATION PGMM class, is left for comparison:
+ std::vector<mat> A_tmp;
+ mat auxA;
+ std::vector<colvec> b_tmp;
+ colvec auxb;
+
+ auxA = eye<mat>(nVars, nVars);
+ A_tmp.push_back(auxA);
+ A_tmp.push_back(auxA); // P=2 frames
+
+ auxb = zeros(nVars, 1);
+ tpgmm->getGMMS(0).getMU(0).print();
+
+ auxb.at(3) = tpgmm->getGMMS(0).getMU(0)[0]; // the object positions are set in the .txt file
+ auxb.at(4) = tpgmm->getGMMS(0).getMU(0)[1];
+ auxb.at(5) = tpgmm->getGMMS(0).getMU(0)[2];
+ b_tmp.push_back(auxb);
+ auxb.at(3) = tpgmm->getGMMS(1).getMU(0)[0];
+ auxb.at(4) = tpgmm->getGMMS(1).getMU(0)[1];
+ auxb.at(5) = tpgmm->getGMMS(1).getMU(0)[2];
+ b_tmp.push_back(auxb);
+ */
+
+ /* for tracking the time */
+ struct timeval tim;
+ double t0, t;
+
+ ofstream myfile;
+ myfile.open ("output_log.txt");
+
+ mat repDataPt;
+ repDataPt = zeros(3, 1);
+
+ for(int tn = -400 ; tn <= 400 ; tn++){
+
+ // Record time before computing GMM for new {A,b} and GMR
+ gettimeofday(&tim, NULL);
+ t0=tim.tv_sec*1000+(tim.tv_usec/1000.0); // tv_usec comes in u-sec -> divide by 1M for sec.; 1k for m-sec
+
+ // Computing the resulting gmm given the set of parameters {A,b}
+ GMM_Model* gmm;
+ gmm = tpgmm->getTransformedGMM(TPs, ORTHONORMAL); // New Implementation of TPGMM
+ //gmm = tpgmm->getTransformedProdGMM(A_tmp,b_tmp, ORTHONORMAL); // Old implementatin of pgmm
+ gmm->setVARSNames(varNames);
+ // Shaving off the "aux" frame - not needed when using a single frame
+ // A_tmp.erase(A_tmp.begin()+1);
+ // b_tmp.erase(b_tmp.begin()+1);
+
+
+ // Printing model components
+ cout << "Resulting GMM given the set of parameters 'A' and 'b'" << endl;
+ for(uint i = 0 ; i < nStates ; i++){
+ cout << "State #" << i << ":"<< endl;
+ gmm->getCOMPONENTS(i).getMU().print("Mu = ");
+ gmm->getCOMPONENTS(i).getSIGMA().print("Sigma = ");
+ }
+
+ // Computing GMR
+ gmr->setGMMModel(gmm); //<-problem
+ repDataPt[1] = tn*0.001;
+// repDataPt.at(0) = 0.005;
+// repDataPt.at(1) = 0.4950;
+// repDataPt.at(2) = 0.2;
+ repDataPt.print("x = ");
+ repro->setData(repDataPt);
+ GMM_Model* gmmOut = gmr->regression(repro);
+
+ gmmOut->getMU(0).print("MuOut = ");
+
+ // Calculating elapsed time from the beginning of GMM recomputation to GMR
+ gettimeofday(&tim, NULL);
+ t=tim.tv_sec*1000+(tim.tv_usec/1000.0);
+ cout << "GMR computational time: " << t-t0 << " ms" << endl;
+
+ cout << "Please press [ENTER] to continue or [CTRL+C] to finish" << endl;
+ char key;
+ std::cin.ignore(1);
+
+ }
+ myfile.close();
+ return 0;
+}
diff --git a/include/pbdlib/datapoints.h b/include/pbdlib/datapoints.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ee71f7a1fc7c5f102317398587e7fa86548ec58
--- /dev/null
+++ b/include/pbdlib/datapoints.h
@@ -0,0 +1,74 @@
+/**
+Copyright (C) 2014, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file datapoints.h
+\brief Datapoints class
+The class Datapoints allows to access samples of n-points and n-variables
+stored in a Armadillo matrix.
+
+\author Davide De Tommaso
+\bug No known bugs.
+*/
+
+
+#ifndef DATAPOINTS_H
+#define DATAPOINTS_H
+
+#include "armadillo"
+#include <fstream>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class Datapoints
+{
+ private:
+ uint nVARS, nPOINTS;
+ std::vector<std::string> vars_names;
+ mat data; // [nVARS]x[nPOINTS]
+
+ public:
+ Datapoints(){}
+ Datapoints(uint _nVARS, uint _nPOINTS);
+
+ /*!
+ getData() provides in output an Armadillo mat of size [nVARS]x[nPOINTS]
+
+ */
+ mat& getData();
+ colvec getData(uint i);
+ uint getNumVARS();
+ uint getNumPOINTS();
+ std::string& getVarName(uint varIndex);
+ uint getIndexVarname(std::string varname);
+ std::vector<std::string>& getVarNames();
+
+
+ void setData(mat &_data);
+ void setData(uint i, colvec& dataPoint);
+ void setVarNames(std::vector<std::string> &names);
+ void saveInFile(std::string path);
+ void loadFromFile(std::string path, bool is_transpose = false);
+};
+
+} //end of pbdlib namespace
+
+#endif
diff --git a/include/pbdlib/demonstration.h b/include/pbdlib/demonstration.h
new file mode 100644
index 0000000000000000000000000000000000000000..f81e52cc0b122f5a535a84506fdec54a6419f568
--- /dev/null
+++ b/include/pbdlib/demonstration.h
@@ -0,0 +1,58 @@
+/**
+Copyright (C) 2014, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file demonstration.h
+\brief Demonstration class
+The class Demonstration model a single demonstration in PbD framework
+
+\author Davide De Tommaso
+\bug No known bugs.
+*/
+
+
+#ifndef DEMONSTRATION_H
+#define DEMONSTRATION_H
+
+#include "armadillo"
+#include <fstream>
+
+#include "pbdlib/datapoints.h"
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class Demonstration
+{
+ private:
+ Datapoints data;
+
+ public:
+ Demonstration(std::string path);
+ Demonstration(uint _nVARS, uint _nPOINTS);
+
+ Datapoints& getDatapoints();
+ void saveInFile(std::string path);
+
+};
+
+} //end of pbdlib namespace
+
+#endif
diff --git a/include/pbdlib/gmm.h b/include/pbdlib/gmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb699686d0183761f8faf458ee5293fbd673f442
--- /dev/null
+++ b/include/pbdlib/gmm.h
@@ -0,0 +1,95 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file gmr.h
+\brief GMM_model class
+The class GMM_model allows to use a Gaussian Mixture Model and to learn the parameters from task demonstrations
+
+\author Davide De Tommaso, Milad Malekzadeh
+\bug No known bugs.
+*/
+
+#ifndef GMM_H
+#define GMM_H
+
+#include "pbdlib/datapoints.h"
+#include "pbdlib/demonstration.h"
+#include "pbdlib/mvn.h"
+#include "armadillo"
+#include <math.h>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class GMM_Model
+{
+ private:
+ uint nVARS, nSTATES;
+ std::vector<GaussianDistribution> COMPONENTS;
+ rowvec PRIORS;
+ std::vector<Demonstration> DEMONSTRATIONS;
+ std::vector<std::string> vars_names;
+
+ void learnKMEANS(double regularization = 0.);
+ bool EM_isfinished(double l_old, double l_new);
+ uint EM(double likelihood,double regularization);
+
+ public:
+ GMM_Model(std::vector<Demonstration> &demos, uint _nSTATES);
+ GMM_Model(uint _nSTATES, uint _nVARS);
+ GMM_Model(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path,const std::string &vars_path);
+ GMM_Model(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path);
+ ~GMM_Model(){}
+
+ void onlineEMDP(int N,colvec P,double lambda, double minSigma);
+ uint EM_learn(double regularization = 1E-5);
+ std::vector<std::string>& getVARSNames();
+ std::string getVARSNames(int);
+ uint getIndexOfVARName(const std::string& varname);
+ uint getNumVARS();
+ uint getNumSTATES();
+ rowvec& getPRIORS();
+ double getPRIORS(int);
+ std::vector<GaussianDistribution>& getCOMPONENTS();
+ GaussianDistribution& getCOMPONENTS(int);
+ colvec& getMU(int);
+ mat& getSIGMA(int);
+ mat& getLAMBDA(int);
+
+ double getProbability(const colvec& sample);
+ double getLikelihood(const mat& SAMPLES);
+ bool addDemo(Demonstration &demo);
+
+ void setPRIORS(const rowvec& priors);
+ void setCOMPONENTS(const std::vector<GaussianDistribution>& components);
+ void setMU(int, const colvec&);
+ void setSIGMA(int, const mat&);
+ void setLAMBDA(int,const mat&);
+ void setVARSNames(const std::vector<std::string>& vars);
+ void saveInFiles();
+ void clear();
+ };
+
+} //end of pbdlib namespace
+
+#endif
+
+
diff --git a/include/pbdlib/gmr.h b/include/pbdlib/gmr.h
new file mode 100644
index 0000000000000000000000000000000000000000..93f9a7834e7516f35f397c76fe19c786e0ce19d0
--- /dev/null
+++ b/include/pbdlib/gmr.h
@@ -0,0 +1,90 @@
+/**
+Copyright (C) 2014, Tohid Alizadeh, Milad Malekzadeh, Leonel Rozo, Davide De Tommaso, Sylvain Calinon
+
+This file is part of PbDLib (Programming-by-demonstration C++ Library).
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file gmr.h
+\brief gmr class
+This class is the implementation of the Gaussian mixture regression (GMR), as described in:
+
+@article{Calinon07SMC,
+ title="On Learning, Representing and Generalizing a Task in a Humanoid Robot",
+ author="S. Calinon and F. Guenter and A. Billard",
+ journal="IEEE Transactions on Systems, Man and Cybernetics, Part B.
+ Special issue on robot learning by observation, demonstration and imitation",
+ year="2007",
+ volume="37",
+ number="2",
+ pages="286--298"
+}
+
+\author Tohid Alizadeh, Milad Malekzadeh, Leonel Rozo, Davide De Tommaso, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#ifndef GMR_H
+#define GMR_H
+
+#include "pbdlib/gmm.h"
+#include "armadillo"
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class GMR
+{
+ private:
+ GMM_Model *gmm;
+ Datapoints *data_in;
+ /*!
+ COMPONENTS: A vector of Gaussian components, each component has PRIORS, MU and SIGMA
+ */
+ std::vector<GaussianDistribution> COMPONENTS;
+
+ // Regression Variables:
+ uint i,t;
+ colvec Mu, MuOut, MuOutTmp;
+ mat Sigma, SigmaOut, SigmaOutTmp, beta;
+ mat InvSigmaInIn;
+ mat Pxi;
+ GaussianDistribution* Gtmp;
+
+
+ public:
+ GMR(){}
+ // The GMM_Model should be already learnt. (SIGMA, MU, PRIORS)
+ GMR(GMM_Model* model);
+ /*!
+ setGMMModel(): Set the GMM model that will be used for regression
+ */
+ void setGMMModel(GMM_Model* gmmmodel);
+ /*!
+ regression(): Gaussian mixture regression
+ */
+ void regression(GMM_Model*, mat Data_in, urowvec Var_In, urowvec Var_Out);
+ GMM_Model* regression(Datapoints* data_in);
+ GMM_Model* regression(mat Data_in, urowvec Var_In, urowvec Var_Out);
+
+};
+
+} //end pbdlib namespace
+
+#endif
+
+
diff --git a/include/pbdlib/hmm.h b/include/pbdlib/hmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed3bd35e5f4b0c5e4d7348f07ff8e252ec568079
--- /dev/null
+++ b/include/pbdlib/hmm.h
@@ -0,0 +1,73 @@
+/**
+Copyright (C) 2015, Martijn Zeestraten
+
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file
+\brief HMM_model class
+The class GMM_model allows to use a Gaussian Mixture Model and to learn the parameters from task demonstrations
+
+*/
+
+#ifndef HMM_H
+#define HMM_H
+
+#include "pbdlib/datapoints.h"
+#include "pbdlib/demonstration.h"
+#include "pbdlib/gmm.h"
+#include "pbdlib/mvn.h"
+#include "armadillo"
+#include <math.h>
+
+using namespace arma;
+
+namespace pbdlib{
+class HMM: public GMM_Model
+{
+ private:
+ uint nVARS, nSTATES;
+ rowvec alpha; // Current value for alpha
+ // GMM Components:
+ GMM_Model* gmm;
+
+ // HMM Extension:
+ mat TransitionMatrix;
+
+ // EM Functions (overrides of GMM parts)
+ void learnKMEANS();
+ uint EM(double likelihood);
+ bool EM_isfinished(double _old, double l_new);
+
+ public:
+ HMM(uint _nSTATES, uint _nVARS);
+ HMM(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path,const std::string &transition_path);
+ ~HMM(){}
+
+ mat& getTRANSITION(){return TransitionMatrix;}
+
+
+ uint EM_Learn(); // Override function
+ double getProbability(const colvec& sample); // override
+ double getLikelihood(const mat& SAMPLES); // override
+ void setTRANSITION(const mat&);
+};
+
+
+#endif
+} // End pbdlib namespace
+
diff --git a/include/pbdlib/hsmm.h b/include/pbdlib/hsmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1b3a76dce8f696fc727ce11e8f31e03b16ad3db
--- /dev/null
+++ b/include/pbdlib/hsmm.h
@@ -0,0 +1,126 @@
+/**
+
+
+Copyright (C) 2015, Martijn Zeestraten
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file
+\brief HMM_model class
+The class HSMM_model allows to use a Gaussian Mixture Model and to learn the parameters from task demonstrations
+
+*/
+
+#ifndef HSMM_H
+#define HSMM_H
+
+#include "pbdlib/datapoints.h"
+#include "pbdlib/hmm.h"
+#include "pbdlib/demonstration.h"
+#include "pbdlib/mvn.h"
+#include "armadillo"
+#include <math.h>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class HSMM:public HMM
+{
+ private:
+ // HMM Components:
+ std::vector<GaussianDistribution> DurationCOMPONENTS;
+
+ // Variables for calculating Forward Variable:
+ GaussianDistribution* Gtmp;
+ mat Atmp1, Atmp2; // Help variables for the calculation of alpha variables
+ mat ALPHA; // Variable to keep track of past alphas
+ colvec bmx; // variable used to keep track of observation probability P(x|model)
+ colvec btmp; // Unnormalized observation probabilities
+ colvec S; //
+
+ mat Pd; // Matrix with precomputed probabilty values for the duration;
+ uint PdSize;
+ bool Initialized,tmpInit; // Variables used to check if alpha calculation is initialized
+
+ // For predictions:
+ colvec alphatmp; // Temp variable to hold in the loop alpha
+ mat ALPHAtmp; // to store ALPHA in the prediction loop
+ mat AlphaPred; // Matrix to hold predictions
+ colvec Stmp; // to store S in the prediction loop
+
+ void initializeFwdCalculation();
+ void updateBtmp(colvec& _Btmp,colvec& _obs);
+ void updateBtmp(colvec& _Btmp,colvec& _obs,urowvec& _ind);
+ void updateBmx(colvec& _bmx, mat& ALPHA, colvec& _btmp);
+ void updateALPHA(mat& ALPHA, colvec& S, colvec& _bmx);
+ void updateALPHA(mat& ALPHA, colvec& S);
+ void updateS(colvec& _S, mat& ALPHA, colvec& _bmx);
+ void updateS(colvec& _S, mat& ALPHA);
+ void updateAlpha(colvec& _alpha, mat& ALPHA, colvec& btmp);
+ void updateAlpha(colvec& _alpha, mat& ALPHA);
+
+
+ // Forward variable Functions
+ void initializeForwardVariable(); // initialization without observation
+ void initializeForwardVariable(colvec& obs); // initialization with observation
+ void initializeForwardVariable(colvec& obs, urowvec& ind); // initializaiton with observation
+
+ void lstepForwardVariable(); // Forward step without observation
+ void lstepForwardVariable(colvec& obs); // Forward step with observation
+ void lstepForwardVariable(colvec& obs,urowvec& ind); // Forward step with partial observation
+
+ colvec alpha; // Forward variable
+ public:
+ HSMM(uint _nSTATES, uint _nVARS);
+ HSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path);
+ ~HSMM(){}
+
+ // Properties:
+ std::vector<GaussianDistribution>& getDurationCOMPONENTS();
+ GaussianDistribution& getDurationCOMPONENTS(uint);
+ colvec& getDurMU(uint);
+ mat& getDurSIGMA(uint);
+
+ // Forward Variables:
+ void stepForwardVariable(); // Forward step without observation
+ void stepForwardVariable(colvec& obs); // Forward step with observation
+ void stepForwardVariable(colvec& obs,urowvec& ind); // Forward step with partial observation
+ colvec& getForwardVariable(){return alpha;} // Get current forward variable
+ void resetForwardVariable(){Initialized=false;}
+
+ mat& predictForwardVariable(uint N); // Predict forward variable without predicted observations
+ void predictForwardVariable(mat& _AlphaPred); // Predict forward variable without predicted observations (implementation for real-time)
+
+
+
+ void setDurationCOMPONENTS(const std::vector<GaussianDistribution>& components);
+ void setDurMU(int, const colvec&);
+ void setDurSIGMA(int, const mat&);
+
+ };
+
+} // PbdLib namespace
+
+#endif
+
diff --git a/include/pbdlib/lqr.h b/include/pbdlib/lqr.h
new file mode 100644
index 0000000000000000000000000000000000000000..fcf74a81dd04004d2e1dc761cedca0c6070885e5
--- /dev/null
+++ b/include/pbdlib/lqr.h
@@ -0,0 +1,101 @@
+/**
+Copyright (C) 2014, Danilo Bruno, Sylvain Calinon
+
+This file is part of PbDLib (Programming-by-demonstration C++ Library).
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file lqr.h
+\brief lwr class
+ This class is the implementation of a minimal intervention controller with
+ linear quadratic regulation, as described in:
+
+ @inproceedings{Calinon14ICRA,
+ author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+ title="A task-parameterized probabilistic model with minimal intervention control",
+ booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+ year="2014",
+ month="May-June",
+ address="Hong Kong, China",
+ pages="3339--3344"
+ }
+
+\author Danilo Bruno, Sylvain Calinon
+\bug No known bugs.
+*/
+
+#ifndef LQR_H
+#define LQR_H
+
+//#include "pbdlib/gmm.h"
+//#include "pbdlib/gmr.h"
+//#include "datapoints.h"
+#include "armadillo"
+#include <vector>
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class LQR
+{
+ private:
+ mat A; //Matrix defining linear system
+ mat B; //Matrix defining linear system
+ double dt; //time step
+ bool prob_set; //flag to 1 if problem matrices are set
+
+ std::vector<mat> Q; //Matrix defining variability
+ mat R; //Matrix defining command weights
+ mat Target; //Target trajectory
+
+ int nbData; //number of Data Points to evaluate
+ int nbVar; //Number of variables
+ int nbCtrl; //Number of controller variables;
+ std::vector<mat> S; //Riccati solution
+ mat d; //Linear DE solution
+
+ std::vector<mat> L; //LQR Gains
+ mat M; //Feedforward term
+
+ public:
+ LQR(mat,mat,double);
+ //LQR(GMM_Model* model);
+ //mat reproduction_finiteHorizon(mat,mat,colvec);
+ bool evaluate_gains_finiteHorizon(mat,colvec);
+ bool evaluate_gains_infiniteHorizon();
+ //Datapoints* reproduction_finiteHorizon(Datapoints*, Datapoints*, Datapoint);
+ //Datapoints* reproduction_infiniteHorizon(Datapoints*, Datapoints*, Datapoint);
+
+ void setQ(std::vector<mat> _Q){this->Q = _Q;};
+ void setR(mat _R){this->R = R;};
+ void setTarget(mat _Target){this->Target = _Target;};
+
+ std::vector<mat> getGains(){return this->L;};
+ mat getFF(){return this->M;};
+
+ bool setProblem(mat,std::vector<mat>,mat);
+
+ mat diff(mat);
+ mat solveAlgebraicRiccati(mat,mat,mat,mat);
+};
+
+} //end pbdlib namespace
+
+#endif
+
+
+
diff --git a/include/pbdlib/mvn.h b/include/pbdlib/mvn.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe3a1f295be80c9d63a8033ab2cb9e5c7f103269
--- /dev/null
+++ b/include/pbdlib/mvn.h
@@ -0,0 +1,81 @@
+/**
+Copyright (C) 2014, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file mvn.h
+\brief GaussianDistribution class
+The class GaussianDistribution model a multivariate Gaussian distribution.
+
+\author Davide De Tommaso, Milad Malekzadeh
+\bug No known bugs.
+*/
+
+
+#ifndef MVN_H
+#define MVN_H
+
+#define THRESHOLD_MIN std::numeric_limits<float>::epsilon()
+#define THRESHOLD_MAX 1.0e60
+
+#define PI 3.14159265359
+
+#include "armadillo"
+#include "pbdlib/datapoints.h"
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class GaussianDistribution
+{
+private:
+ uint nVARS;
+ mat SIGMA; // Covariance Matrix
+ mat LAMBDA; // Precision Matrix
+ colvec MU;
+
+public:
+ GaussianDistribution(uint _nVARS);
+ GaussianDistribution(colvec& _MU, mat& _SIGMA);
+
+ uint getNumVARS();
+ colvec& getMU();
+ mat& getSIGMA(); // Covariance Matrix
+ mat& getLAMBDA(); // Precision Matrix
+ // Get PDF Values
+ colvec getPDFValue(const mat& SAMPLES); // simple implementation
+ void getPDFValue(colvec& prob, mat SAMPLES);// Implementation for real time execution: doesnt require memory allocation:
+
+ mat stochasticSampling( uint nbS);
+ mat sqrtm(const mat SIGMA);
+
+ void setParamsFromData(const mat SAMPLES);
+ void setParamsFromData(const mat SAMPLES, const rowvec REWARD);
+ void setParamsFromData(const mat SAMPLES, const rowvec REWARD, const uint nbImportanceSampling);
+
+ void setMU(const colvec& _MU);
+ void setSIGMA(const mat& _SIGMA);
+ void setLAMBDA(const mat& _LAMBDA);
+ void setNumVARS(uint numvars);
+
+};
+
+} //end of namespace pbdlib
+
+#endif
diff --git a/include/pbdlib/taskparameters.h b/include/pbdlib/taskparameters.h
new file mode 100644
index 0000000000000000000000000000000000000000..337e05248ec4a2ec7b0cb64cf0b20f4f07bd2b4f
--- /dev/null
+++ b/include/pbdlib/taskparameters.h
@@ -0,0 +1,90 @@
+/**
+Copyright (C) 2014, Tohid Alizadeh, Milad Malekzadeh, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file taskparameters.h
+\brief TaskParameters class
+
+
+A Task parameter represents a linear transformations Ax+b. A Task parameter can be used in two ways:
+1) To transform demonstration data from a 'global' representation 'g'to the local representation 'l': l = A\(g-b)
+2) To transform local representation of data(e.g. GMMs defined in local data) into global representations : g = Al+b
+and vector b.
+
+The class TaskParameters represents a set of Task Parameters. By defining multiple task parameters global data can be
+viewd from different local perspectives.
+
+b: Position vector
+A: Projection matrix
+TaskParameter: struct of matrix A and vector b defining a linear transformation
+nPARAMS: Number of task parameters - according to the task this should be determined before
+nVARS: Number of variables
+taskparams: The set of task parameters
+
+
+\author Martijn Zeestraten, Tohid Alizadeh, Milad Malekzadeh, Davide De Tommaso
+\bug No known bugs.
+*/
+
+
+#ifndef TASKPARAMETERS_H
+#define TASKPARAMETERS_H
+
+#include <fstream>
+#include "armadillo"
+#include "pbdlib/datapoints.h"
+
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+
+struct TaskParameter
+{
+ vec b;
+ mat A;
+};
+
+class TaskParameters
+{
+ private:
+ std::vector<TaskParameter> MyTPs; // Vector of task parameters
+ uint nVARS; // Number of variables
+
+ public:
+ TaskParameters(){}
+ ~TaskParameters(){}
+ TaskParameters(uint nVARS,uint nTaskParameters);
+
+ std::vector<TaskParameter>& getTaskParameters();
+ TaskParameter& getTaskParameters(uint index);
+ uint getNumTASKPARAMETERS(){return MyTPs.size();}
+ uint getNumVARS(){return nVARS;}
+
+ void setTaskParameters(std::vector<TaskParameter> _TPs);
+ void setTaskParameters(uint index, TaskParameter _TP);
+ void loadFromFile(std::string path);
+ void saveToFile(std::string filename, std::vector<std::string> TPnames);
+
+};
+
+} // end of pbdlib namespace
+
+#endif // PARAMETERS_H
diff --git a/include/pbdlib/tpdemonstration.h b/include/pbdlib/tpdemonstration.h
new file mode 100644
index 0000000000000000000000000000000000000000..cef762c6d94548893259e92e578e5b580f020fcf
--- /dev/null
+++ b/include/pbdlib/tpdemonstration.h
@@ -0,0 +1,108 @@
+/**
+Copyright (C) 2014, Davide De Tommaso
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file demonstration.h
+\brief Demonstration class
+The class Demonstration model a single demonstration in PbD framework
+
+\author Davide De Tommaso
+\bug No known bugs.
+*/
+
+
+#ifndef TPDEMONSTRATION_H
+#define TPDEMONSTRATION_H
+
+#include "armadillo"
+#include <fstream>
+
+#include "pbdlib/datapoints.h"
+#include "pbdlib/taskparameters.h"
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+class TPDemonstration
+{
+ private:
+
+ // We store the 'global' demonstration data
+ Datapoints Data;
+
+ // We store unique task parameters for each datapoint:
+ std::vector<TaskParameters> PointTPs;
+ bool uniqueTPs; // flag that indicates if all TPs have unique TPs (used for saving)
+
+ public:
+ TPDemonstration(){}
+ TPDemonstration(mat& _data, std::vector<TaskParameters> VTPs);
+ TPDemonstration(mat& _data, TaskParameters TPs);
+ TPDemonstration(std::string data_path, std::string tp_path, bool DataTrans= false);
+
+
+
+
+ // Function used to set the data of the demonstration in the global reference
+ void setGlobalData(mat& Data);
+
+ // --------------- PARAMETER SET FUNCTIONS ---------------
+ // Internally each data point has a unique value Task parameters object:
+ // Each datapoint thus has nTaskParameters task parameters A and b.
+ // This is done since during demonstration the task parameters can change
+ // from timestep to timestep. By the following function we can
+ // set the unique task parameters corresponding to the datapoints:
+ void setDataPointTPs(std::vector<TaskParameters> _TPs);
+
+ // If you just want to set the same set of task parameters for each
+ // data point you can use the following function. Here you need to pass the
+ // Parameters
+ // Eeach data point will then have the same task parameters.
+ void setDataPointTPs(TaskParameters _TPs);
+ void setDataPointTPs(uint i, TaskParameters _Tps);
+ void setDataPointTPs(std::vector<TaskParameter> _TPs);
+ void setDataPointTPs(std::vector<mat> _A, std::vector<colvec> _b);
+
+ // Functions To Load data from Files:
+ void loadFromFiles(std::string data_path, std::string tp_path, bool Transpose =false);
+ void loadTaskParametersFromFile(std::string tp_path);
+ void loadDataFromFile(std::string data_path, bool DataTrans=false);
+
+ void saveInFiles(std::string prefix,std::vector<std::string> par_names,
+ std::vector<std::string> TPnames);
+ // ----------------------- DATA GET FUNCTIONS ---------------------------
+ // The data is stored in the global space. These functions are used to linear transform the data
+ // according to the specified task parameters of each frame
+ mat getDataInTaskParameters(uint i);
+ mat getDataInTaskParameters();
+ std::vector<TaskParameters>& getDataPointTPs(){return PointTPs;}
+ TaskParameters& getDataPointTPs(uint pIndex){return PointTPs[pIndex];}
+ uint getNumVARS(){return this->Data.getNumVARS();}
+ uint getNumPOINTS(){return this->Data.getNumPOINTS();}
+ uint getNumTASKPARAMETERS();
+ Datapoints getDataInGlobalSpace(){return this->Data;}
+
+
+
+};
+
+} //end of pbdlib namespace
+
+#endif
diff --git a/include/pbdlib/tpgmm.h b/include/pbdlib/tpgmm.h
new file mode 100644
index 0000000000000000000000000000000000000000..10caed89bb5b16828a50ff85d15e193e57a7d0e0
--- /dev/null
+++ b/include/pbdlib/tpgmm.h
@@ -0,0 +1,149 @@
+/*
+Copyright (C) 2014, Tohid Alizadeh, Milad Malekzadeh, Leonel Rozo, Davide De Tommaso, João Silvério, Sylvain Calinon
+
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*! \file pgmm.h
+\brief pgmm class
+This class is the implementation of the task parameterized Gaussian mixture models (TP-GMM) as described in:
+
+@inproceedings{Calinon13IROS,
+ author="Calinon, S. and Alizadeh, T. and Caldwell, D. G.",
+ title="On improving the extrapolation capability of task-parameterized movement models",
+ booktitle="Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
+ year="2013", month="November", address="Tokyo, Japan",
+ pages="610--616",
+}
+
+\author Tohid Alizadeh, Milad Malekzadeh, Leonel Rozo, Davide De Tommaso, João Silvério, Sylvain Calinon
+\bug No known bugs.
+*/
+#ifndef TPGMM_H
+#define TPGMM_H
+
+#include "armadillo"
+#include "pbdlib/gmm.h"
+#include "pbdlib/taskparameters.h"
+#include "pbdlib/tpdemonstration.h"
+
+using namespace arma;
+
+namespace pbdlib
+{
+
+#define REALMIN 2.2251e-200
+#define REALMAX 1.7977e200
+enum EMatrixType {INVERTIBLE, SYMMETRIC, ORTHONORMAL, OTHER};
+
+class TPGMM
+{
+ private:
+ /*!
+ nVARS: Number of variables
+ nSATAES: Number of Gaussian components of the model (This sould be set by the user)
+ nFRAMES: Number of frames (This sould be set by the user)
+ GMMS: A vector of GMM models containing for each task parameter, the center and covariance of the TPGMM
+ PRIORS: A vector containing the mixing coefficients of the Gaussian components
+ */
+ uint nVARS, nSTATES, nTaskParameters;
+ std::vector<GMM_Model> GMMS;
+ mat Data;
+ rowvec PRIORS;
+ std::vector<std::string> vars_names;
+
+ // Auxiliary variables for product of Gaussian:
+ GMM_Model* prodGMM;
+ colvec accMu, tmpMu;
+ mat accLambda, tmpLambda;
+
+ std::vector<mat> invA;
+ std::vector<mat> invAT;
+
+ void setAuxiliaryVarsProdGauss();
+ public:
+
+ /*!
+ initTensorGMM_timeBased(): Initialization of the centers and the covariances of GMMS using equally spaced time splits
+ */
+ void initTensorGMM_timeBased();
+
+ /*!
+ initTensorGMM_kmeans(): Initialization of the centers and the covariances of GMMS using kmeans clustering
+ */
+ void initTensorGMM_kmeans();
+
+ /*!
+ EM_tensorGMM(): The implementation of the Expectation-Maximization algorithm for pgmm
+ */
+ void EM_tensorGMM();
+
+ /*!
+ estimateTensorGMM(): Calls initTensorGMM_timeBased() & EM_tensorGMM()
+ */
+ uint estimateTensorGMM(); //EM algorithm with the last formulation (Tensor)
+
+ TPGMM(std::vector<TPDemonstration>& demos, uint nSTATES);
+ TPGMM(uint nVARS, uint nSTATES, uint _nTPs);
+ ~TPGMM(){}
+
+ /*!
+ loadPGMMfromMATLAB(): From a set of given .txt files reads the PRIORS, MU and SIGMA of the GMMS
+ Currently, the MU and SIGMA for every task parameter are saved in different files in the following format:
+ 'MuFileName_P' nbFrame '.txt'
+ 'SigmaFileName_P' nbFrame '.txt'
+ Note that in the MuFileName*.txt there is a matrix containing centers like this: [MU_1 MU_2 MU_3 ... MU_j]; j=1:nSTATES.
+ Similarly, in the SigmaFileName*.txt there is matrix containing the covariance matrices like this: [SIGMA_1 SIGMA_2 ... SIGMA_j] ; j=1:nSTATES
+ */
+ void loadTPGMMfromMATLAB(std::string PriorsFileName, std::string VarsFileName, std::string MuFileName, std::string SigmaFileName);
+
+ /*!
+ getNumVARS(): Gives nVARS
+ */
+ uint getNumVARS();
+ /*!
+ getNumSTATES(): Gives nSTATES
+ */
+ uint getNumSTATES();
+ /*!
+ getNumFRAMES(): Gives nFRAMES
+ */
+ uint getNumFRAMES();
+ /*!
+ getPRIORS(): Gives the PRIORS
+ */
+
+ rowvec& getPRIORS();
+ double getPRIORS(uint);
+
+ /*!
+ getGMMS(): Gives GMMS
+ */
+ std::vector<GMM_Model>& getGMMS();
+ GMM_Model& getGMMS(uint id);
+
+ GMM_Model* getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
+ GMM_Model* getTransformedGMM(TaskParameters _TPs, EMatrixType AType = OTHER );// [Calinon, 2012] Eq. (3) - Prod. of linearly transformed Gaussians
+
+ void setVARSNames(const std::vector<std::string>& vars);
+ std::vector<std::string>& getVARSNames();
+};
+
+} // end of namespace pbdlib
+
+#endif
+
diff --git a/src/datapoints.cpp b/src/datapoints.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b265534d4e9200b26f4175fdaa30bc3b90476b3
--- /dev/null
+++ b/src/datapoints.cpp
@@ -0,0 +1,113 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/datapoints.h"
+
+namespace pbdlib
+{
+
+Datapoints::Datapoints(uint _nVARS, uint _nPOINTS)
+{
+ nVARS = _nVARS;
+ nPOINTS = _nPOINTS;
+ data = zeros(_nVARS,_nPOINTS);
+}
+
+
+void Datapoints::setData(mat& _data)
+{
+ data = _data;
+ // Update dimensions:
+ nVARS= _data.n_rows;
+ nPOINTS = _data.n_cols;
+}
+
+void Datapoints::setData(uint i, colvec& _dataPoint)
+{
+ data.col(i) = _dataPoint;
+}
+
+mat& Datapoints::getData()
+{
+ return data;
+}
+colvec Datapoints::getData(uint i)
+{
+ return data.col(i);
+}
+
+uint Datapoints::getNumVARS()
+{
+ return nVARS;
+}
+
+uint Datapoints::getNumPOINTS()
+{
+ return nPOINTS;
+}
+
+std::string& Datapoints::getVarName(uint varIndex)
+{
+ if( varIndex < nVARS )
+ return vars_names.at(varIndex);
+ else
+ std::cout << "\n [ERROR]::Datapoints::getVarName if( varIndex < nVARS ) ... else .";
+
+ return vars_names.at(0);
+}
+
+void Datapoints::setVarNames(std::vector<std::string>& names)
+{
+ if(names.size() == nVARS)
+ vars_names = names;
+ else
+ std::cout << "\n [ERROR]::Datapoints::setVarNames if(names.size() == nVARS) ... else .";
+}
+
+
+std::vector<std::string>& Datapoints::getVarNames()
+{
+ return vars_names;
+}
+
+uint Datapoints::getIndexVarname(std::string varname)
+{
+ std::vector<std::string>::iterator it;
+ it = std::find (vars_names.begin(), vars_names.end(), varname);
+ return std::distance(vars_names.begin(), it);
+}
+
+
+void Datapoints::loadFromFile(std::string path, bool is_transpose)
+{
+ mat _data;
+ _data.load(path, raw_ascii);
+ if(is_transpose)
+ _data = _data.t();
+ setData(_data);
+}
+
+
+void Datapoints::saveInFile(std::string path)
+{
+ data.save(path, raw_ascii);
+}
+
+
+} // end of pbdlib namespace
diff --git a/src/demonstration.cpp b/src/demonstration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9df90d1cd9e6853e7096b25ff31ada1e97dcaff
--- /dev/null
+++ b/src/demonstration.cpp
@@ -0,0 +1,40 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/demonstration.h"
+
+namespace pbdlib
+{
+
+Demonstration::Demonstration(uint _nVARS, uint _nPOINTS)
+{
+ data = Datapoints(_nVARS,_nPOINTS);
+}
+
+Datapoints& Demonstration::getDatapoints()
+{
+ return data;
+}
+
+void Demonstration::saveInFile(std::string path)
+{
+ data.getData().save(path, raw_ascii);
+}
+
+} // end of pbdlib namespace
diff --git a/src/gmm.cpp b/src/gmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf59a98e94db4f01952d921eb33ec44672c899e3
--- /dev/null
+++ b/src/gmm.cpp
@@ -0,0 +1,514 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/gmm.h"
+
+namespace pbdlib
+{
+
+GMM_Model::GMM_Model(std::vector<Demonstration> &demos, uint _nSTATES)
+{
+ DEMONSTRATIONS = demos;
+ nVARS = demos[0].getDatapoints().getNumVARS();
+ nSTATES = _nSTATES;
+ PRIORS = rowvec(_nSTATES);
+ setVARSNames(demos[0].getDatapoints().getVarNames());
+
+ // Initialize Components
+ COMPONENTS.resize(nSTATES,GaussianDistribution(nVARS));
+}
+
+GMM_Model::GMM_Model(uint _nSTATES, uint _nVARS)
+{
+ nVARS = _nVARS;
+ nSTATES = _nSTATES;
+ PRIORS = rowvec(_nSTATES);
+
+ // Initialize Components
+ COMPONENTS.resize(nSTATES,GaussianDistribution(nVARS));
+}
+
+GMM_Model::GMM_Model(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path,const std::string &vars_path)
+{
+ mat priors, mu, sigma;
+
+ priors.load(priors_path, raw_ascii);
+ mu.load(mu_path, raw_ascii);
+ sigma.load(sigma_path, raw_ascii);
+
+ nVARS = mu.n_rows;
+ nSTATES = priors.n_elem;
+
+ std::ifstream varsfile(vars_path.c_str());
+ std::string varsnames;
+ std::getline (varsfile,varsnames);
+
+ std::vector<std::string> vars;
+
+ std::stringstream ss(varsnames); // Insert the string into a stream
+ std::string buf;
+
+ while (ss >> buf)
+ vars.push_back(buf);
+
+ setVARSNames(vars);
+
+ mat _SIGMA = zeros(nVARS, nVARS);
+ colvec _MU = zeros(nVARS,1);
+ rowvec _PRIORS(1, nSTATES);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<nSTATES; i++){
+ _PRIORS(0,i) = priors(0,i);
+ for(uint j=0; j<nVARS; j++){
+ _MU(j,0) = mu(j,i);
+ for(uint k=0; k<nVARS; k++)
+ _SIGMA(j,k) = sigma(j,k + i*(nVARS));
+ }
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+
+ setPRIORS(_PRIORS);
+ setCOMPONENTS(components);
+}
+
+GMM_Model::GMM_Model(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path)
+{
+ // Constructor which does not require to specify variable names
+ mat priors, mu, sigma;
+
+ priors.load(priors_path, raw_ascii);
+ mu.load(mu_path, raw_ascii);
+ sigma.load(sigma_path, raw_ascii);
+
+ nVARS = mu.n_rows;
+ nSTATES = priors.n_elem;
+
+ mat _SIGMA = zeros(nVARS, nVARS);
+ colvec _MU = zeros(nVARS,1);
+ rowvec _PRIORS(1, nSTATES);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<nSTATES; i++){
+ _PRIORS(0,i) = priors(0,i);
+ for(uint j=0; j<nVARS; j++){
+ _MU(j,0) = mu(j,i);
+ for(uint k=0; k<nVARS; k++)
+ _SIGMA(j,k) = sigma(j,k + i*(nVARS));
+ }
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+
+ setPRIORS(_PRIORS);
+ setCOMPONENTS(components);
+}
+
+void GMM_Model::saveInFiles()
+{
+ mat priors(1, nSTATES);
+ mat mu(nVARS, nSTATES);
+ mat sigma(nVARS, nVARS*nSTATES);
+ std::ofstream varsfile ("GMM_vars.txt");
+
+ for(uint i=0; i<nSTATES; i++){
+ priors(0,i) = getPRIORS()(i);
+ for(uint j=0; j<nVARS; j++){
+ mu(j,i) = getMU(i)(j);
+ for(uint k=0; k<nVARS; k++)
+ sigma(j,k + i*(nSTATES+1)) = getSIGMA(i)(j,k);
+ }
+ }
+
+ for(uint i=0; i<nVARS; i++){
+ varsfile << vars_names[i];
+ if(i<nVARS-1)
+ varsfile << " ";
+ }
+
+ priors.save("GMM_priors.txt", raw_ascii);
+ mu.save("GMM_mu.txt", raw_ascii);
+ sigma.save("GMM_sigma.txt", raw_ascii);
+
+ varsfile.close();
+}
+
+void GMM_Model::setPRIORS(const rowvec& priors)
+{
+ if(priors.n_elem == nSTATES)
+ PRIORS = priors;
+ else
+ std::cout << "\n [ERROR]::GMM_Model::setPRIORS if(priors.n_elem == nSTATES) ... else .";
+}
+
+
+void GMM_Model::setCOMPONENTS(const std::vector<GaussianDistribution>& components)
+{
+ if(components.size() == nSTATES)
+ COMPONENTS = components;
+ else
+ std::cout << "\n [ERROR]::GMM_Model::setCOMPONENTS if(components.size() == nSTATES) ... else .";
+}
+
+void GMM_Model::setMU(int id, const colvec& _MU)
+{
+ COMPONENTS[id].setMU(_MU);
+}
+
+void GMM_Model::setSIGMA(int id, const mat& _SIGMA)
+{
+ COMPONENTS[id].setSIGMA(_SIGMA);
+}
+
+void GMM_Model::setLAMBDA(int id, const mat& _LAMBDA)
+{
+ COMPONENTS[id].setLAMBDA(_LAMBDA);
+}
+
+uint GMM_Model::getIndexOfVARName(const std::string& varname)
+{
+ std::vector<std::string>::iterator it;
+ it = std::find (vars_names.begin(), vars_names.end(), varname);
+ return std::distance(vars_names.begin(), it);
+}
+
+uint GMM_Model::getNumVARS()
+{
+ return nVARS;
+}
+
+uint GMM_Model::getNumSTATES()
+{
+ return nSTATES;
+}
+
+rowvec& GMM_Model::getPRIORS()
+{
+ return PRIORS;
+}
+
+double GMM_Model::getPRIORS(int id)
+{
+ return PRIORS[id];
+}
+
+std::vector<GaussianDistribution>& GMM_Model::getCOMPONENTS()
+{
+ return COMPONENTS;
+}
+
+GaussianDistribution& GMM_Model::getCOMPONENTS(int id)
+{
+ return COMPONENTS[id];
+}
+
+colvec& GMM_Model::getMU(int id)
+{
+ return COMPONENTS[id].getMU();
+}
+
+mat& GMM_Model::getSIGMA(int id)
+{
+ return COMPONENTS[id].getSIGMA();
+}
+mat& GMM_Model::getLAMBDA(int id)
+{
+ return COMPONENTS[id].getLAMBDA();
+}
+
+double GMM_Model::getProbability(const colvec& sample)
+{
+ double P = 0.0;
+ // See Eq. (2.0.2) in doc/TechnicalReport.pdf
+ for(uint k=0; k<nSTATES; k++)
+ P += PRIORS[k] * as_scalar( COMPONENTS[k].getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ return P;
+}
+
+
+void GMM_Model::onlineEMDP(int N,colvec P,double lambda, double minSigma)
+{
+ /*
+ Performs online GMM clustering by using an updated DP-Means algorithms
+ Ref:
+ Kulis, B. and Jordan, M. I. (2012). Revisiting k-means: New algorithms via bayesian nonparametrics. In Proc. Intl Conf. on Machine Learning (ICML)
+
+ Input:
+ N: Number of points processed
+ P: current point being added to GMM
+ lambda: splitting distance
+ nimSigma: minimum covariance for regularization
+ */
+ double d = arma::norm(this->getMU(0)-P,2); //Initialized distance of P from first cluster
+ int minIndex = 0; //Index corresponding to current cluster
+ //Find cluster corresponding to minimum distance
+ for(int k=1;k<nSTATES;k++){
+ double Dist = arma::norm(this->getMU(k) - P,2);
+ if (Dist < d){
+ d = Dist;
+ minIndex = k; //Index corresponding to minimum distance
+ }
+ }
+ //Allocate new cluster if distance of each component higher than lambda
+ if (lambda < d){
+ minIndex = nSTATES;
+ mat SigmaTmp = minSigma*eye(nVARS,nVARS); //Sigma of new component is the minimum cov
+ GaussianDistribution newComponent(P,SigmaTmp); //Mean of new component is P
+ COMPONENTS.push_back(newComponent); //add new component
+ rowvec priorsTmp = zeros(1,nSTATES+1);
+ priorsTmp.cols(0,nSTATES-1) = this->getPRIORS();
+ priorsTmp(nSTATES) = 1./N; //prior for new component inversely proportional to #P
+ priorsTmp = priorsTmp/arma::norm(priorsTmp,1); //evaluate new priors
+ this->nSTATES = nSTATES +1; //update number of states
+ this->setPRIORS(priorsTmp);
+ }
+ else{
+ /*
+ *Update components belonging to P by using MAP estimate
+ Ref:
+ Gauvain, J.-L. and Lee, C.-H. (1994). Maximum a pos- teriori estimation for multivariate gaussian mixture observations of markov chians. IEE Transactions on Speech and Audio Processing, 2(2). */
+ double PriorsTmp = 1./N + PRIORS[minIndex];
+ colvec MuTmp = 1./PriorsTmp*(PRIORS[minIndex]*this->getMU(minIndex)+P/N);
+ mat SigmaTmp = PRIORS[minIndex]/PriorsTmp*(this->getSIGMA(minIndex)+(this->getMU(minIndex)-MuTmp)*(this->getMU(minIndex)-MuTmp).t()) + 1./(N*PriorsTmp)*(minSigma*eye(nVARS,nVARS)+(P-MuTmp)*(P-MuTmp).t());
+ this->setMU(minIndex,MuTmp);
+ this->setSIGMA(minIndex,SigmaTmp);
+ rowvec priors = this->getPRIORS();
+ priors[minIndex] = PriorsTmp;
+ priors = priors/arma::norm(priors,1);
+ this->setPRIORS(priors);
+ }
+}
+
+static urowvec randperm( int n )
+{
+ //return sort_index(randn<urowvec>(nbData));
+ return sort_index(randu<rowvec>(n)); // was randn, should work equally well?
+}
+
+void GMM_Model::learnKMEANS(double regularization)
+{
+
+ mat Data = DEMONSTRATIONS[0].getDatapoints().getData();
+
+ for(int i=1; i<DEMONSTRATIONS.size(); i++)
+ Data.insert_cols(DEMONSTRATIONS[0].getDatapoints().getNumPOINTS(), DEMONSTRATIONS[i].getDatapoints().getData());
+
+ //Criterion to stop the EM iterative update
+ double cumdist_threshold = 1e-10;
+ uint maxIter = 100;
+
+ //Initialization of the parameters
+ uint nbVar = Data.n_rows;
+ uint nbData = Data.n_cols;
+
+ double cumdist_old = -std::numeric_limits<double>::max();
+ uint nbStep = 0;
+
+ //srand (time(NULL));
+ // random permutation
+ urowvec idTmp = randperm(nbData);
+
+ uvec allrows = linspace<uvec>(0, nbVar-1, nbVar);
+
+ // nSTATES means
+ mat Mu = Data.submat(allrows, idTmp.cols(0,nSTATES-1));
+
+ //k-means iterations
+ while(true)
+ {
+ mat distTmp = zeros(nbData,nSTATES);
+
+ //E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ for (uint i=0; i<nSTATES; i++){
+ //Compute distances
+ distTmp.col(i) = trans( sum( pow(Data - repmat(Mu.col(i), 1, nbData), 2.0) ) );
+ }
+
+ vec vTmp = zeros<vec>(nbData,1);
+ uvec idList = zeros<uvec>(nbData,1);
+
+ for( uint i = 0; i < nbData; i++ )
+ {
+ // there is a chance that here two elements will be as close and will skip one?s
+ vTmp[i] = ((rowvec)distTmp.row(i)).min(idList[i]);
+ }
+
+ double cumdist = sum(vTmp);
+
+ //M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ uvec idTmp;
+
+ for (uint i=0; i<nSTATES; i++)
+ {
+ idTmp = find(idList == i);
+ Mu.col(i) = mean(Data.submat(allrows, idTmp), 1);
+ }
+
+ //Stopping criterion %%%%%%%%%%%%%%%%%%%%
+ if (fabs(cumdist-cumdist_old) < cumdist_threshold){
+ cout << "%%%%%%%%%%% KMEANS %%%%%%%%%%%%%%%%%%" << endl;
+ cout << nbStep << "Steps" << endl;
+ cout << endl << "Mu_kmeans :" << endl << Mu << endl;
+
+
+ GaussianDistribution *GD;
+ colvec mu;
+
+ for(int i=0; i<nSTATES; i++){
+ idTmp = find(idList == i);
+ COMPONENTS[i].setMU( Mu.col(i) );
+
+ PRIORS(i) = idTmp.n_elem;
+
+ COMPONENTS[i].setSIGMA(
+ cov( trans(join_rows(Data.submat(allrows, idTmp),Data.submat(allrows, idTmp))) ,0 )
+ + eye(nbVar, nbVar)*regularization
+ );
+ }
+
+ PRIORS = PRIORS / nbData;
+ cout << "PRIORS:" << endl << PRIORS << endl;
+
+ cout << "%%%%%%% KMEANS FINISHED %%%%%%%%%%%%%" << endl;
+ return;
+ }
+ cumdist_old = cumdist;
+ nbStep = nbStep + 1;
+
+ }
+}
+
+uint GMM_Model::EM_learn(double regularization)
+{
+ learnKMEANS(regularization);
+ return EM(std::numeric_limits<double>::min(),regularization);
+}
+
+double GMM_Model::getLikelihood(const mat &SAMPLES)
+{
+ double L=0.0;
+ colvec s(SAMPLES.n_rows);
+
+ // See Eq. (2.0.4) in doc/TechnicalReport.pdf
+ for(uint j=0; j<SAMPLES.n_cols; j++){
+ s = SAMPLES.col(j);
+ L += log( getProbability( s ) );
+ }
+ return L;
+}
+
+bool GMM_Model::EM_isfinished(double l_old, double l_new)
+{
+ //cout << "\n error=" << fabs((l_new/l_old) - 1.0);
+ if ( fabs((l_new/l_old) - 1.0) <= THRESHOLD_MIN)
+ return true;
+ return false;
+}
+
+
+uint GMM_Model::EM(double likelihood,double regularization)
+{
+ double likelihood_new;
+
+ uint k,i;
+ rowvec E;
+ colvec mu_tmp;
+ mat Pxi, Pix, Pix_tmp, DataTmp1, sigma_tmp;
+
+ mat demos = DEMONSTRATIONS[0].getDatapoints().getData();
+
+ for(int i=1; i<DEMONSTRATIONS.size(); i++)
+ demos.insert_cols(DEMONSTRATIONS[0].getDatapoints().getNumPOINTS(), DEMONSTRATIONS[i].getDatapoints().getData());
+
+ //E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // See Eq. (2.0.5) in doc/TechnicalReport.pdf
+ rowvec prior = PRIORS;
+
+ Pxi = zeros(demos.n_cols, nSTATES);
+ for(k=0; k<nSTATES; k++){
+ Pxi.col(k) = COMPONENTS[k].getPDFValue( demos );
+ }
+ // compute posterior probability p(i|x)
+ Pix_tmp = repmat(prior, demos.n_cols, 1) % Pxi;
+ Pix = Pix_tmp / repmat(sum(Pix_tmp, 1), 1, nSTATES);
+ // compute cumulated posterior probability
+ E = sum(Pix);
+
+ //M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ // See Eq. (2.0.6),(2.0.7) and (2.0.8) in doc/TechnicalReport.pdf
+ for (i=0; i<nSTATES; i++){
+ // update the priors
+ PRIORS(i) = E(i) / demos.n_cols;
+ // update the centers
+ mu_tmp = demos * Pix.col(i) / E(i);
+ COMPONENTS[i].setMU(mu_tmp);
+ // update the covariance matrices
+ DataTmp1 = demos - repmat( COMPONENTS[i].getMU(), 1, demos.n_cols);
+ sigma_tmp = (repmat(trans(Pix.col(i)), nVARS, 1) % DataTmp1 * trans(DataTmp1)) / E(i);
+ sigma_tmp = sigma_tmp + regularization * eye(nVARS,nVARS);
+ COMPONENTS[i].setSIGMA( sigma_tmp );
+ }
+
+
+ likelihood_new = getLikelihood( demos ); // See Eq. (2.0.4) in doc/TechnicalReport.pdf
+
+ if( EM_isfinished(likelihood, likelihood_new) )
+ return 1;
+
+ return EM(likelihood_new,regularization)+1;
+}
+
+void GMM_Model::setVARSNames(const std::vector<std::string>& vars)
+{
+ if(vars.size() == nVARS)
+ vars_names = vars;
+ //else
+ // std::cout << "\n [ERROR]::GMM_Model::setVARSNames if(vars.size() == nVARS) ... else .";
+}
+
+std::vector<std::string>& GMM_Model::getVARSNames()
+{
+ return vars_names;
+}
+
+std::string GMM_Model::getVARSNames(int id)
+{
+ return vars_names[id];
+}
+
+
+bool GMM_Model::addDemo(Demonstration& demo)
+{
+ if(demo.getDatapoints().getNumVARS() == nVARS)
+ DEMONSTRATIONS.push_back(demo);
+ else
+ return false;
+
+ return true;
+}
+
+void GMM_Model::clear(){
+ COMPONENTS.clear();
+ nSTATES=1;
+ setPRIORS(ones<vec>(1));
+}
+
+
+} //end of pbdlib namespace
+
diff --git a/src/gmr.cpp b/src/gmr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fad19c0e4c8311d183c6791844c0b28895373919
--- /dev/null
+++ b/src/gmr.cpp
@@ -0,0 +1,153 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/gmr.h"
+
+namespace pbdlib
+{
+
+GMR::GMR(GMM_Model* model)
+{
+ gmm = NULL;
+ setGMMModel(model);
+}
+GMM_Model* GMR::regression(Datapoints* data_in)
+{
+ // Change note 4/03/2015, Martijn Zeestraten
+ // This regression function is the initial version of the 'regression', it uses variable names
+ // from the data_in object to select the input and output variables. This function was quite cumbersome
+ // and now an overload is implemented which more closely resembles the Matlab version
+ //
+ // The actual regression is performed by regression(data_in, in, out) while this function only
+ // determines the input and output variables.
+ uint i,j,k;
+
+ // Check if variable names exist:
+ if(data_in->getVarNames().size() == 0){
+ std::cout << "\n [ERROR]::GMR::regression() if(data_in->getVarNames().size() == 0) ... else .";
+ //return 0;
+ }
+
+ // Extract the input and output variable indices
+ urowvec in,out; // row vectors to hold the input and output indices
+ bool found = false; //
+
+ in.zeros(1, data_in->getNumVARS()); // Size of In is the number of vars in the input
+ out.zeros(1, gmm->getNumVARS() - data_in->getNumVARS());// Size of Out is the diff between input and GMM size
+
+ // Determine the input variables
+ for(j=0; j<data_in->getNumVARS(); j++)
+ // Check which index in GMM has varname
+ for(i=0; i<gmm->getNumVARS(); i++)
+ if ( data_in->getVarName(j).compare( gmm->getVARSNames(i) ) == 0)
+ in(j) = i; // found, store variable index
+ // Remainder of the variables is output variable
+ for(i=0, k=0; i<gmm->getNumVARS(); i++){
+ for(j=0; j<data_in->getNumVARS(); j++)
+ if ( data_in->getVarName(j).compare( gmm->getVARSNames(i) ) == 0)
+ found = true;
+ if(!found)
+ out(k++) = i;
+ found = false;
+ }
+
+
+ // Extract the input data and put it into a mat shape
+ mat x = data_in->getData();
+ // Perform actual regression and return results
+ return regression(x,in,out);
+}
+
+GMM_Model* GMR::regression( mat data_in, urowvec in, urowvec out)
+{
+ // This regression performs regression of the model and returns
+ // a new GMM_Model object.
+
+ // Create the GMM_model
+ GMM_Model* gmmOut = new GMM_Model(data_in.n_cols, out.n_elem);
+ // Perform regression
+ regression(gmmOut,data_in,in,out);
+ // Return the model
+ return gmmOut;
+
+
+
+}
+
+
+void GMR::regression(GMM_Model* gmmOut, mat data_in, urowvec in, urowvec out)
+{
+ // Base regression function. Use this one to be as fast as possible.
+ // One need to supply a gmmOut object of appropriate size (nbStates = data_in.n_cols, nbVar = size(out))
+
+
+ Pxi = zeros(data_in.n_cols,gmm->getNumSTATES());
+ //Compute the influence of each GMM component, given input x
+ // See Eq. (3.0.5) in doc/TechnicalReport.pdf
+ for(i=0; i<gmm->getNumSTATES(); i++){
+ Mu = gmm->getMU(i)(in);
+ Sigma = gmm->getSIGMA(i)(in,in);
+ //Mu_tmp.print("Mu(i) = "); // DEBUGGING
+ //Sigma_tmp.print("Sigma(i) = "); // DEBUGGING
+ Gtmp->setMU(Mu);
+ Gtmp->setSIGMA(Sigma);
+ Pxi.col(i) = gmm->getPRIORS(i) * (Gtmp->getPDFValue(data_in));
+ }
+ //Priors.print("Priors = "); // DEBUGGING
+ //data_in->getData().print("t = "); // DEBUGGING
+ //Pxi.print("Pxi = "); // DEBUGGING
+ beta = Pxi / repmat(sum(Pxi,1),1,gmm->getNumSTATES()); // See Eq. (3.0.5) in doc/TechnicalReport.pdf
+
+ //beta.print("beta = "); // DEBUGGING
+ for(t=0; t<data_in.n_cols; t++){
+ MuOut.zeros(out.n_elem);
+ SigmaOut.zeros(out.n_elem,out.n_elem);
+ for(i=0; i<gmm->getNumSTATES(); i++){
+ Mu = gmm->getMU(i);
+ Sigma = gmm->getSIGMA(i);
+ // Pre compute inverse, use inv_sympd to improve performance
+ InvSigmaInIn = (Sigma(in,in).i());
+
+ MuOutTmp = Mu(out) + Sigma(out,in) * InvSigmaInIn * (data_in.col(t)-Mu(in)); // See Eq. (3.0.3) in doc/TechnicalReport.pdf
+ SigmaOutTmp = Sigma(out,out) - Sigma(out,in) * InvSigmaInIn * Sigma(in,out); // See Eq. (3.0.4) in doc/TechnicalReport.pdf
+ MuOut = MuOut + beta(t,i) * MuOutTmp; // See Eq. (3.0.2) in doc/TechnicalReport.pdf
+ SigmaOut = SigmaOut + beta(t,i) * SigmaOutTmp; // See Eq. (3.0.2) in doc/TechnicalReport.pdf
+ }
+// COMPONENTS.push_back( GaussianDistribution(MuOut, SigmaOut) );
+ gmmOut->getCOMPONENTS(t).setMU(MuOut);
+ gmmOut->getCOMPONENTS(t).setSIGMA(SigmaOut);
+
+ }
+
+// GMM_Model* gmmOut;
+// gmmOut->setCOMPONENTS(COMPONENTS);
+// return gmmOut;
+
+}
+
+void GMR::setGMMModel(GMM_Model* gmmmodel)
+{
+ gmm = gmmmodel;
+
+ // Initial settings for regression
+ Gtmp= new GaussianDistribution(gmm->getNumSTATES());
+}
+
+} //end of pbdlib namespace
+
diff --git a/src/hmm.cpp b/src/hmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a7abd7801857668bd8f5ce2014c60f3fd578494
--- /dev/null
+++ b/src/hmm.cpp
@@ -0,0 +1,185 @@
+/**
+ *
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/hmm.h"
+
+namespace pbdlib
+{
+HMM::HMM(uint _nSTATES, uint _nVARS):GMM_Model(_nSTATES,_nVARS)
+{
+}
+
+HMM::HMM(const std::string &priors_path, const std::string &mu_path, const std::string &sigma_path, const std::string &transition_path)
+ :GMM_Model(priors_path,mu_path,sigma_path)
+{
+// mat priors, mu, sigma; // GMM components
+ mat transition; // HMM addition
+
+ // Load from File
+// priors.load(priors_path, raw_ascii);
+// mu.load(mu_path, raw_ascii);
+// sigma.load(sigma_path, raw_ascii);
+ transition.load(transition_path,raw_ascii);
+
+ // Determine dimensions:
+ /*
+ nVARS = mu.n_rows;
+ nSTATES = priors.n_elem;
+
+
+ for (uint i = 0;i<nSTATES;i++)
+ {
+ this->setMU(i,mu.col(i));
+ this->setSIGMA(i,sigma.cols(i*nVARS,(i+1)*nVARS-1));
+ }
+ this->setPRIORS(priors);
+ */
+
+ // Load transition components:
+ this->setTRANSITION(transition);
+}
+
+void HMM::setTRANSITION(const mat& _Transition)
+{
+ TransitionMatrix = _Transition;
+}
+
+
+double HMM::getProbability(const colvec& sample)
+{
+ double P = 0.0;
+ // See Eq. (2.0.2) in doc/TechnicalReport.pdf
+ for(uint k=0; k<nSTATES; k++)
+ P += alpha[k] * as_scalar( this->gmm->getCOMPONENTS(k).getPDFValue(sample) ); // See Eq. (2.0.3) for "getPDFValue" in doc/TechnicalReport.pdf
+ return P;
+}
+
+
+/*
+void HMM::learnKMEANS()
+{
+ mat DemosTmp = DEMONSTRATIONS[0].getDatapoints().getData();
+
+ for(int i=1; i<DEMONSTRATIONS.size(); i++)
+ DemosTmp.insert_cols(DEMONSTRATIONS[0].getDatapoints().getNumPOINTS(), DEMONSTRATIONS[i].getDatapoints().getData());
+
+ //Criterion to stop the EM iterative update
+ double cumdist_threshold = 1e-10;
+ uint maxIter = 100;
+
+ //Initialization of the parameters
+ uint nbVar = DemosTmp.n_rows;
+ uint nbData = DemosTmp.n_cols;
+ double cumdist_old = -std::numeric_limits<double>::max();
+ uint nbStep = 0;
+
+ //srand (time(NULL));
+
+ urowvec idTmp = sort_index(randn<urowvec>(nbData));
+
+ uvec allrows = linspace<uvec>(0, nbVar-1, nbVar);
+
+ mat Mu = DemosTmp.submat(allrows, idTmp.cols(0,nSTATES-1));
+ cube Sigma = zeros(nbVar,nbVar,nSTATES);
+
+
+ //k-means iterations
+ while(true){
+ mat distTmp = zeros(nSTATES,nbData);
+
+ //E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ for (uint i=0; i<nSTATES; i++){
+ //Compute distances
+ distTmp.row(i) = sum((DemosTmp - repmat(Mu.col(i), 1, nbData)) % (DemosTmp - repmat(Mu.col(i), 1, nbData)));
+ }
+
+ vec vTmp = zeros<vec>(nbData,1);
+ vec idList = zeros<vec>(nbData,1);
+ uword index;
+ vec tempVec;
+ double tempId;
+ for (uint i=0; i<nbData; i++){
+ tempVec = distTmp.col(i);
+ vTmp.row(i) = min(tempVec);
+ tempId = tempVec.min(index);
+ idList.row(i) = index;
+ }
+
+ double cumdist = sum(vTmp);
+
+ //M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+ uvec idTmp1;
+ rowvec priors = zeros(1,nSTATES);
+ mat sigTmp = zeros(nbVar,nbVar);
+ for (uint i=0; i<nSTATES; i++){
+ idTmp1 = find(idList == i);
+ priors(i) = idTmp1.n_elem;
+ Mu.col(i) = mean(DemosTmp.submat(allrows, idTmp1), 1);
+ sigTmp = cov( trans(join_rows(DemosTmp.submat(allrows, idTmp1),DemosTmp.submat(allrows, idTmp1))) ,0 );
+ Sigma.slice(i) = sigTmp;
+ }
+ priors = priors/nbData;
+
+ //Stopping criterion %%%%%%%%%%%%%%%%%%%%
+ if (fabs(cumdist-cumdist_old) < cumdist_threshold){
+ cout << "%%%%%%%%%%% KMEANS %%%%%%%%%%%%%%%%%%" << endl;
+ cout << endl << "Priors_kmeans :" << endl << priors << endl;
+ cout << endl << "Mu_kmeans :" << endl << Mu << endl;
+ cout << endl << "Sigma_kmeans :" << endl << Sigma << endl;
+ cout << "%%%%%%% KMEANS FINISHED %%%%%%%%%%%%%" << endl;
+
+ GaussianDistribution *GD;
+ colvec mu;
+
+ for(int i=0; i<nSTATES; i++){
+ PRIORS(i) = priors(i);
+ mu = Mu.col(i);
+ COMPONENTS[i].setMU(mu);
+ COMPONENTS[i].setSIGMA(Sigma.slice(i));
+ }
+
+ return;
+ }
+ cumdist_old = cumdist;
+ nbStep = nbStep + 1;
+
+ //cout << endl << "nbStep " << nbStep << endl;
+ }
+}
+*/
+
+double HMM::getLikelihood(const mat &SAMPLES)
+{
+ double L=0.0;
+ colvec s(SAMPLES.n_rows);
+
+ // See Eq. (2.0.4) in doc/TechnicalReport.pdf
+ for(uint j=0; j<SAMPLES.n_cols; j++){
+ s = SAMPLES.col(j);
+ L += log( getProbability( s ) );
+ }
+ return L;
+}
+
+
+
+
+
+} // End pbdlib namespace
diff --git a/src/hsmm.cpp b/src/hsmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac3cb3f6c94439ac8e8d07fab8a674b8281e067f
--- /dev/null
+++ b/src/hsmm.cpp
@@ -0,0 +1,474 @@
+/**
+Copyright (C) 2015, Martijn Zeestraten
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/hsmm.h"
+
+namespace pbdlib
+{
+
+HSMM::HSMM(const std::string &priors_path,
+ const std::string &mu_path,
+ const std::string &sigma_path,
+ const std::string &transition_path,
+ const std::string &durMu_path,
+ const std::string &durSigma_path):
+ HMM(priors_path,mu_path,sigma_path,transition_path)
+{
+ mat durMu, durSigma;
+
+ // Load components
+ durMu.load(durMu_path, raw_ascii);
+ durSigma.load(durSigma_path, raw_ascii);
+
+ // Set components:
+ mat _SIGMA = zeros(1,1);
+ colvec _MU = zeros(1,1);
+ std::vector<GaussianDistribution> components;
+
+ for(uint i=0; i<this->getNumSTATES(); i++){
+ _MU(0,0) = durMu(0,i);
+ _SIGMA(0,0) = durSigma(0,i);
+ // for debugging
+ components.push_back(GaussianDistribution(_MU, _SIGMA));
+ }
+ setDurationCOMPONENTS(components);
+
+ // Forward variable calculation initialization:
+ initializeFwdCalculation();
+}
+
+
+void HSMM::setDurationCOMPONENTS(const std::vector<GaussianDistribution>& components)
+{
+ if(components.size() == this->getNumSTATES())
+ DurationCOMPONENTS = components;
+ else
+ std::cout << "\n [ERROR]::HMM::setCOMPONENTS if(components.size() == nSTATES) ... else .";
+}
+
+void HSMM::setDurMU(int id, const colvec& _MU)
+{
+ DurationCOMPONENTS[id].setMU(_MU);
+}
+
+void HSMM::setDurSIGMA(int id, const mat& _SIGMA)
+{
+ DurationCOMPONENTS[id].setSIGMA(_SIGMA);
+}
+
+
+std::vector<GaussianDistribution>& HSMM::getDurationCOMPONENTS()
+{
+ return DurationCOMPONENTS;
+}
+
+GaussianDistribution& HSMM::getDurationCOMPONENTS(uint id)
+{
+ return DurationCOMPONENTS[id];
+}
+
+colvec& HSMM::getDurMU(uint id)
+{
+ return DurationCOMPONENTS[id].getMU();
+}
+
+mat& HSMM::getDurSIGMA(uint id)
+{
+ return DurationCOMPONENTS[id].getSIGMA();
+}
+
+void HSMM::initializeFwdCalculation()
+{
+ // Calculate PD size:
+ cout << "Pd size : ";
+ PdSize=0;
+ for (uint i = 0;i<this->getNumSTATES();i++)
+ {
+ if (PdSize <accu(this->getDurMU(i)))
+ PdSize = accu(this->getDurMU(i));
+ }
+ PdSize *=1.5; // Enlarge the Size of Pd for 'accuracy'
+ //PdSize = 50;
+ cout << PdSize << endl;
+
+ // This function is used to set the forward variable matrices to the appropriate size:
+ ALPHA.set_size(this->getNumSTATES(), PdSize);
+ Atmp1.set_size(this->getNumSTATES(),PdSize-1);
+ Atmp2.set_size(this->getNumSTATES(),PdSize-1);
+ alpha.set_size(this->getNumSTATES());
+ bmx.set_size(this->getNumSTATES());
+ btmp.set_size(this->getNumSTATES());
+ S.set_size(this->getNumSTATES());
+ Pd.set_size(this->getNumSTATES(),PdSize);
+
+ // Duration input vector
+ mat dur = linspace(1,PdSize, PdSize);
+ // Pre-Calculate Pd
+ for (uint i = 0;i<this->getNumSTATES();i++)
+ {
+ // Note that we need to transpose twice....
+ // getPDFValue accepts a rowvec of values, but returns a colvec....
+ Pd.row(i) = getDurationCOMPONENTS(i).getPDFValue(dur.t()).t();
+
+ }
+
+ // For forward variable:
+ Gtmp = new GaussianDistribution(this->getNumVARS());
+
+ Initialized = false;
+}
+
+
+// -------------------------------------------------------------------
+/* Forward Variable Step calculations */
+// -------------------------------------------------------------------
+
+void HSMM::stepForwardVariable()
+{
+ if (Initialized==false)
+ {
+ initializeForwardVariable();
+ Initialized = true;
+ }
+ else
+ {
+ lstepForwardVariable();
+ }
+}
+
+void HSMM::stepForwardVariable(colvec& obs)
+{
+ if (Initialized==false)
+ {
+ initializeForwardVariable(obs);
+ Initialized = true;
+ }
+ else
+ {
+ lstepForwardVariable(obs);
+ }
+}
+
+void HSMM::stepForwardVariable(colvec& obs, urowvec& ind)
+{
+ if (Initialized==false)
+ {
+ initializeForwardVariable(obs, ind);
+ Initialized = true;
+ }
+ else
+ {
+ lstepForwardVariable(obs, ind);
+ }
+}
+// -------------------------------------------------------------------
+// ---------- INITIALIZATION of Forward Variable
+void HSMM::initializeForwardVariable()
+{
+ // ALPHA Variable
+ ALPHA = Pd;
+ ALPHA.each_col() %= this->getPRIORS().t(); // % is the element wise product
+
+ // Update S
+ updateS(S,ALPHA);
+
+ // Update Alpha
+ updateAlpha(alpha,ALPHA);
+}
+
+void HSMM::initializeForwardVariable(colvec& _obs)
+{
+ // Calculate the initial forward step
+ updateBtmp(btmp,_obs);
+
+ // ALPHA Variable
+ ALPHA = Pd;
+ ALPHA.each_col() %= this->getPRIORS().t(); // % is the element wise product
+
+ // Update bmx:
+ updateBmx(bmx, ALPHA, btmp);
+
+ // Update S
+ updateS(S,ALPHA,bmx);
+
+ // Update Alpha
+ updateAlpha(alpha,ALPHA,btmp);
+}
+
+
+void HSMM::initializeForwardVariable(colvec& _obs, urowvec& _ind)
+{
+ // Calculate the initial forward step
+ updateBtmp(btmp,_obs, _ind);
+
+ // ALPHA Variable
+ ALPHA = Pd;
+ ALPHA.each_col() %= this->getPRIORS().t(); // % is the element wise product
+
+ // Update bmx:
+ updateBmx(bmx, ALPHA, btmp);
+
+ // Update S
+ updateS(S,ALPHA,bmx);
+
+ // Update Alpha
+ updateAlpha(alpha,ALPHA,btmp);
+}
+
+
+// -------------------------------------------------------------------
+// ---------- Step functions of Forward Variable
+void HSMM::lstepForwardVariable()
+{
+ // No observation, assume all btmp is 1 (i.e. observation is equally likeli for all
+ // states
+
+ // ALPHA Variable
+// cout << "ALPHA: " << endl;
+ updateALPHA(ALPHA,S);
+// cout << ALPHA.t() << endl;
+
+ // Update S
+// cout << "S : ";
+ updateS(S,ALPHA);
+// cout << S << endl;
+
+ // Update Alpha
+// cout << "alpha : " ;
+ updateAlpha(alpha,ALPHA);
+// cout << alpha.t() << endl;
+}
+
+void HSMM::lstepForwardVariable(colvec& _obs)
+{
+ // update Btmp:
+ updateBtmp(btmp,_obs);
+ //cout << "Btmp: " << endl << btmp << endl;
+
+ // ALPHA Variable
+ updateALPHA(ALPHA,S);
+
+ // Update bmx:
+ updateBmx(bmx, ALPHA, btmp);
+ //cout << "Bmx: " << endl << bmx << endl;
+
+ // Update S
+ updateS(S,ALPHA,bmx);
+
+ // Update Alpha
+ updateAlpha(alpha,ALPHA,btmp);
+ //cout << "Alpha: " << endl << alpha << endl;
+}
+
+void HSMM::lstepForwardVariable(colvec& _obs, urowvec& _ind)
+{
+ // update Btmp:
+ updateBtmp(btmp,_obs,_ind);
+
+ // ALPHA Variable
+ updateALPHA(ALPHA,S);
+
+ // Update bmx:
+ updateBmx(bmx, ALPHA, btmp);
+
+ // Update S
+ updateS(S,ALPHA,bmx);
+
+ // Update Alpha
+ updateAlpha(alpha,ALPHA,btmp);
+}
+
+
+
+// -------------------------------------------------------------------
+// ---------- Prediction Functions for Forward Variable
+mat& HSMM::predictForwardVariable(uint _N)
+{
+ //cout << "Allocating Memory: ";
+ AlphaPred.set_size(this->getNumSTATES(), _N);
+ //cout << AlphaPred.n_rows << " x " << AlphaPred.n_cols << endl;
+
+ tmpInit = Initialized;
+ // Make copy of the current state of the system:
+ ALPHAtmp = ALPHA;
+ Stmp = S;
+
+
+ for (uint i = 0;i<_N;i++)
+ {
+ // Alpha variable
+ if (tmpInit==false)
+ {
+ // Initialize:
+ ALPHAtmp = Pd;
+ ALPHAtmp.each_col() %= this->getPRIORS().t(); // % is the element wise product
+ tmpInit = true;
+ }
+ else
+ {
+ updateALPHA(ALPHAtmp,Stmp);
+ }
+
+ // Update S
+ updateS(Stmp,ALPHAtmp);
+
+ updateAlpha(alphatmp,ALPHAtmp);
+
+ // Save alpha
+ AlphaPred.col(i) = alphatmp;
+ }
+ return AlphaPred;
+}
+
+// Implementation for real-time state prediction (no checks on sizes done)
+// We assume that _AlphaPred = nbStates x nbPred of size
+void HSMM::predictForwardVariable(mat& _AlphaPred)
+{
+
+ tmpInit = Initialized;
+ // Make copy of the current state of the system:
+ ALPHAtmp = ALPHA;
+ Stmp = S;
+
+
+ for (uint i = 0;i<_AlphaPred.n_cols;i++)
+ {
+ // Alpha variable
+ if (tmpInit==false)
+ {
+ // Initialize:
+ ALPHAtmp = Pd;
+ ALPHAtmp.each_col() %= this->getPRIORS().t(); // % is the element wise product
+ tmpInit = true;
+ }
+ else
+ {
+ updateALPHA(ALPHAtmp,Stmp);
+ }
+
+ // Update S
+ updateS(Stmp,ALPHAtmp);
+
+ // Update Alpha
+ updateAlpha(alphatmp,ALPHAtmp);
+
+ // Save alpha
+ _AlphaPred.col(i) = alphatmp;
+ // cout << "Done" << endl;
+ }
+}
+/* FUNCTIONS FACILITATING FORWARD VARIABLE CALCULATION
+ *
+ *
+ */
+void HSMM::updateBtmp(colvec& _btmp, colvec& _obs)
+{
+ // Calculate the initial forward step
+ for (uint i =0;i<this->getNumSTATES();i++)
+ {
+ _btmp(i) = this->getCOMPONENTS(i).getPDFValue(_obs)(0);
+ }
+ _btmp = _btmp/accu(_btmp);
+
+}
+void HSMM::updateBtmp(colvec& _btmp, colvec& _obs, urowvec& _ind)
+{
+ Gtmp->setNumVARS(_ind.n_elem);
+ // Calculate the initial forward step
+ for (uint i =0;i<this->getNumSTATES();i++)
+ {
+ // Create temporary Gaussian that uses only the specified indices:
+ Gtmp->setMU(this->getMU(i)(_ind));
+ Gtmp->setSIGMA(this->getSIGMA(i)(_ind,_ind));
+
+ // Evaluate the gaussian probability:
+ _btmp(i) = Gtmp->getPDFValue(_obs.rows(_ind))(0);
+ }
+ _btmp = _btmp/accu(_btmp);
+}
+
+
+// Equation (12): ALPHA MATRIX without observation:
+void HSMM::updateALPHA(mat& _ALPHA, colvec& _S)
+{
+ // Help variables for vector-wise multiplications:
+ Atmp1 = Pd.cols(0,PdSize-2);
+ Atmp1.each_col() %= _S;
+
+ Atmp2 = _ALPHA.cols(1,PdSize-1);
+
+ _ALPHA.cols(0,PdSize-2) = Atmp2 + Atmp1;
+ _ALPHA.col(PdSize-1) = _S % Pd.col(PdSize-1);
+}
+// Equation (12): ALPHA matrix update
+void HSMM::updateALPHA(mat& _ALPHA, colvec& _S, colvec& _bmx)
+{
+ // Help variables for vector-wise multiplications:
+ Atmp1 = Pd.cols(0,PdSize-2);
+ Atmp1.each_col() %= _S;
+
+ Atmp2 = _ALPHA.cols(1,PdSize-1);
+ Atmp2.each_col() %= _bmx;
+
+ _ALPHA.cols(0,PdSize-2) = Atmp2 + Atmp1;
+ _ALPHA.col(PdSize-1) = _S % Pd.col(PdSize-1);
+}
+
+
+void HSMM::updateBmx(colvec& _bmx, mat& _ALPHA, colvec& _Btmp)
+{
+ // Equation (2) & (3): bmx update
+ _bmx = _Btmp/accu(_Btmp.t()*sum(_ALPHA,1));
+}
+
+
+
+// Equation (6): Update S
+void HSMM::updateS(colvec& _S, mat& _ALPHA)
+{
+ // Equations (5) & (6) calculate S:
+ _S = this->getTRANSITION().t()*_ALPHA.col(0);
+}
+
+// Equation (6): Update S
+void HSMM::updateS(colvec& _S, mat& _ALPHA, colvec& _bmx)
+{
+ // Equations (5) & (6) calculate S:
+ _S = this->getTRANSITION().t()*(_bmx%_ALPHA.col(0));
+}
+
+
+
+void HSMM::updateAlpha(colvec& _alpha, mat& _ALPHA)
+{
+ // Calculate forward varaible
+ _alpha = sum(_ALPHA,1);
+ // Normalize
+// _alpha = _alpha/accu(_alpha);
+}
+
+void HSMM::updateAlpha(colvec& _alpha, mat& _ALPHA, colvec& _Btmp)
+{
+ // Calculate forward varaible
+ _alpha = _Btmp%sum(_ALPHA,1);
+ // Normalize
+// _alpha = _alpha/accu(_alpha);
+}
+
+} // End pbdlib namespace
diff --git a/src/lqr.cpp b/src/lqr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e94579e00fbd9c877c69a47a4edf8435aee4a40c
--- /dev/null
+++ b/src/lqr.cpp
@@ -0,0 +1,152 @@
+/**
+Copyright (C) 2014, Danilo Bruno, Sylvain Calinon
+
+This file is part of PbDLib (Programming-by-demonstration C++ Library).
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/lqr.h"
+#include <iostream>
+
+namespace pbdlib
+{
+
+LQR::LQR(mat _A, mat _B, double _dt)
+{
+ this->A = _A;
+ this->B = _B;
+ this->dt = _dt;
+
+ this->nbVar = A.n_rows;
+ this->nbCtrl = B.n_cols;
+
+ this->prob_set = 0;
+
+}
+
+bool LQR::evaluate_gains_finiteHorizon(mat SFinal,colvec dFinal){
+
+ if (this->prob_set == 0) {return 1;};
+
+ mat DTarget = LQR::diff(this->Target);
+
+ this->S[this->nbData-1] = SFinal;
+ this->d.col(this->nbData-1) = dFinal;
+
+ mat invR = this->R.i();
+
+ for (int t=this->nbData-2;t>=0;t--){
+ this->S[t] = this->S[t+1] + this->dt * (this->A.t()*this->S[t+1] + this->S[t+1]*this->A - this->S[t+1] * this->B * invR * this->B.t() * this->S[t+1] + this->Q[t+1]); //See Eq. (5.1.11) in doc/TechnicalReport.pdf
+ this->d.col(t) = this->d.col(t+1) - this->dt * (this->S[t+1]*this->B*invR * this->B.t() - this->A.t()) * this->d.col(t+1) + this->dt * this->S[t+1] * DTarget.col(t+1) - this->dt * (this->S[t+1] * this->A * this->Target.col(t+1)); //See Eq. (5.1.29) in doc/TechnicalReport.pdf
+ }
+ for (int t=0;t<nbData;t++){
+ this->L[t] = invR*this->B.t()*this->S[t]; //See Eq. (5.1.30) in doc/TechnicalReport.pdf
+ this->M.col(t) = invR*this->B.t()*this->d.col(t); //See Eq. (5.1.30) in doc/TechnicalReport.pdf
+ }
+ return 0;
+};
+
+bool LQR::evaluate_gains_infiniteHorizon(){
+
+ if (this->prob_set == 0) {return 1;};
+ mat DTarget = LQR::diff(this->Target);
+ mat invR = this->R.i();
+ for (int t=0;t < this->nbData;t++){
+ this->S[t] = this->solveAlgebraicRiccati(this->A,this->B,this->Q[t],this->R); //See Sec. (5.2) in doc/TechnicalReport.pdf
+ this->d.col(t) = (this->S[t]*this->B*invR*this->B.t() - this->A.t()).i() * (this->S[t]*DTarget.col(t) - this->S[t]*this->A*this->Target.col(t)); //See Eq. (5.1.29) in doc/TechnicalReport.pdf
+ this->L[t] = invR*this->B.t()*this->S[t]; //See Eq. (5.1.30) in doc/TechnicalReport.pdf
+ //std::cout << this->L[t] << std::endl;
+ this->M.col(t) = invR*this->B.t()*this->d.col(t); //See Eq. (5.1.30) in doc/TechnicalReport.pdf
+ }
+ return 0;
+};
+
+mat LQR::diff(mat V){
+
+ mat DV(V.n_rows,V.n_cols);
+
+ if (V.n_cols==1) { DV = zeros<mat>(V.n_rows,V.n_cols);}
+ else
+ {
+ DV.col(0) = (V.col(1) - V.col(0))/this->dt;
+ for (int i=1;i<V.n_cols-1;i++){
+ DV.col(i) = (V.col(i+1)-V.col(i-i))/(2*this->dt);
+ }
+ DV.col(V.n_cols-1) = (V.col(V.n_cols-1) - V.col(V.n_cols-2))/this->dt;
+ }
+ return DV;
+}
+
+mat LQR::solveAlgebraicRiccati(mat A, mat B, mat Q, mat R) //See Sec. (5.2) in doc/TechnicalReport.pdf
+{
+ int n = A.n_rows;
+
+ mat G = B*R.i()*B.t();
+ mat Z(2*n,2*n); //See Eq. (5.2.3) in doc/TechnicalReport.pdf
+ Z(span(0,n-1),span(0,n-1)) = A;
+ Z(span(n,2*n-1),span(0,n-1)) = -Q;
+ Z(span(0,n-1),span(n,2*n-1)) = -G;
+ Z(span(n,2*n-1),span(n,2*n-1)) = -A.t();
+
+ // Using schur decomposition
+ /*mat U(2*n,2*n);
+ mat T(2*n,2*n);
+ auxlib::schur_dec(U,T,Z); //Missing ordered schur...
+ //Ordered schur decomposition from lapack to add
+ mat S = U(span(0,n-1),span(0,n-1)).t().i()*U(span(n,2*n-1),span(0,n-1)).t();
+ */
+
+ //Using diagonalization (See Eq. (5.2.4) in doc/TechnicalReport.pdf)
+ cx_mat U(2*n,n);
+ cx_vec dd(2*n);
+ cx_mat V(2*n,2*n);
+
+ arma::eig_gen(dd,V,Z);
+ int i = 0;
+ for(int j=0;j<2*n;j++){
+ if (real(dd(j)) < 0) {
+ U.col(i) = V.col(j);
+ i++;
+ }
+ }
+ mat S1 = zeros(n,n);
+ cx_mat Sc = U(span(0,n-1),span(0,n-1)).t().i()*U(span(n,2*n-1),span(0,n-1)).t(); //See Eq. (5.2.5) in doc/TechnicalReport.pdf
+ mat S = real(Sc);
+ return S;
+};
+
+
+bool LQR::setProblem(mat _R, std::vector<mat> _Q, mat _Target)
+{
+ this->R = _R;
+ this->Q = _Q;
+ this->Target = _Target;
+
+ this->nbData = _Q.size();
+
+ for (int i=0;i<this->nbData;i++){
+ this->S.push_back(zeros(this->nbVar,this->nbVar));
+ this->L.push_back(zeros(this->nbCtrl,this->nbVar));
+ }
+ this->d = zeros<mat>(this->nbVar,this->nbData);
+ this->M = zeros<mat>(this->nbCtrl,this->nbData);
+
+ this->prob_set = 1;
+}
+
+
+} //end of pbdlib namespace
+
+
diff --git a/src/mvn.cpp b/src/mvn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a2073bc09bd4810dbb3e539756c4537cfa6021a
--- /dev/null
+++ b/src/mvn.cpp
@@ -0,0 +1,202 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/mvn.h"
+
+namespace pbdlib
+{
+
+GaussianDistribution::GaussianDistribution(colvec& _MU, mat& _SIGMA)
+{
+ nVARS = _MU.n_rows;
+ MU = _MU;
+ SIGMA = _SIGMA;
+ LAMBDA= _SIGMA.i();
+}
+
+GaussianDistribution::GaussianDistribution(uint _nVARS)
+{
+ nVARS = _nVARS;
+}
+
+uint GaussianDistribution::getNumVARS()
+{
+ return nVARS;
+}
+
+
+colvec &GaussianDistribution::getMU()
+{
+ return MU;
+}
+
+
+mat& GaussianDistribution::getSIGMA()
+{
+ return SIGMA;
+}
+
+mat& GaussianDistribution::getLAMBDA()
+{
+ return LAMBDA;
+}
+
+
+
+void GaussianDistribution::setMU(const colvec &_MU)
+{
+ MU = _MU;
+}
+
+
+void GaussianDistribution::setSIGMA(const mat &_SIGMA)
+{
+ SIGMA = _SIGMA;
+ LAMBDA = SIGMA.i();
+}
+
+void GaussianDistribution::setLAMBDA(const mat &_LAMBDA)
+{
+ LAMBDA = _LAMBDA;
+ SIGMA = _LAMBDA.i();
+}
+
+
+void GaussianDistribution::setNumVARS(uint numvars)
+{
+ nVARS = numvars;
+}
+
+
+colvec GaussianDistribution::getPDFValue(const mat &SAMPLES)
+{
+ // Allocate space for solution and temp variable:
+ colvec Probs(SAMPLES.n_cols);
+// mat D_Tmp;
+// D_Tmp.set_size(MU.n_rows,SAMPLES.n_cols);
+
+ // Calculate probabilities:
+ getPDFValue(Probs,SAMPLES);
+
+ return Probs;
+}
+void GaussianDistribution::getPDFValue(colvec& _probs, mat _SAMPLES)
+{
+ // Implementation for real time execution: doesnt require memory allocation:
+ // The user should supply:
+ // - _probs : colvec of length nbSamples
+ // - _SAMPLES: Matrix with samples (nbVar x nbSamples, also used as tmp variable)
+
+ // calculate difference (x-mu)
+ _SAMPLES= trans(_SAMPLES) - repmat(trans(MU),_SAMPLES.n_cols,1);
+
+ // calculate exponential (x-mu)^T*inv(sigma)*(x-mu):
+ _probs = sum((_SAMPLES* LAMBDA) % _SAMPLES, 1);
+
+ // calculate Exponential:
+ _probs = sqrt(fabs(det(LAMBDA))/pow(2*PI,LAMBDA.n_cols))*exp(-0.5*arma::abs(_probs));
+
+ // Implementation with determinant based on SIGMA instead of Lambda:
+ // Probs = exp(-0.5*arma::abs(Probs)) / sqrt(pow((2*PI),SIGMA.n_cols) * (fabs(det(SIGMA)) + THRESHOLD_MIN));
+
+}
+
+mat GaussianDistribution::stochasticSampling(uint nbS)
+{
+ mat noisySamples;
+
+ noisySamples = repmat(MU, 1, nbS) + sqrtm(SIGMA) * randn(nVARS, nbS);
+
+ return noisySamples;
+}
+
+mat GaussianDistribution::sqrtm(const mat SIGMA)
+{
+ mat sqrtmSigma=zeros(nVARS, nVARS);
+ vec eigval;
+ mat eigvec;
+ eig_sym(eigval,eigvec,SIGMA);
+ for(int i=0; i<eigval.n_elem;i++)
+ {
+ sqrtmSigma += eigvec.col(i)*trans(eigvec.col(i)) * sqrt(eigval(i));
+ }
+ return sqrtmSigma;
+}
+
+void GaussianDistribution::setParamsFromData(const mat SAMPLES, const rowvec REWARD, const uint nbImportanceSampling)
+{
+ mat pTmp, eTmp, rTmpDiag;
+ rowvec rTmp, rSrt;
+
+ // Keep the nbImportanceSampling points with highest rewards
+ urowvec idSrt = sort_index(REWARD, 1);
+ rSrt = sort(REWARD, 1);
+ //cout << endl << "rSrt:\n" << rSrt << endl;
+ //cout << endl << "indices:\n" << idSrt << endl;
+ uint nbP = idSrt.n_elem;
+
+ if ((idSrt.n_elem > nbImportanceSampling) && (!nbImportanceSampling == 0))
+ nbP = nbImportanceSampling;
+ //cout << endl << "nbImportanceSampling: \n" << nbP << endl;
+
+ pTmp = SAMPLES.cols(idSrt.cols(0,nbP-1));
+ //cout << endl << "pTmp: \n" << pTmp << endl;
+
+ rTmp = rSrt.cols(0,nbP-1);
+ //cout << endl << "rTmp: \n" << rTmp << endl;
+
+ //Compute error term
+ eTmp = pTmp - repmat(MU, 1, pTmp.n_cols);
+ //cout << endl << "eTmp: \n" << eTmp << endl;
+
+ //Udpate the current best SAMPLES
+ MU = MU + eTmp * trans(rTmp) /sum(rTmp);
+ //cout << endl << "Mu: \n" << MU << endl;
+
+ //Update the exploration noise (covariance matrix)
+ rTmpDiag = zeros(rTmp.n_cols, rTmp.n_cols);
+ rTmpDiag.diag() = rTmp;
+ SIGMA = (eTmp * rTmpDiag * trans(eTmp)) / sum(rTmp);
+
+ // Also calculate LAMBDA
+ LAMBDA = SIGMA.i();
+ //cout << endl << "SIGMA:\n" << SIGMA << endl;
+}
+
+void GaussianDistribution::setParamsFromData( const mat SAMPLES, const rowvec REWARD)
+{
+ // Reward weighted estimation without important sampling
+ uint nbImportanceSampling = SAMPLES.n_cols;
+ setParamsFromData(SAMPLES, REWARD, nbImportanceSampling);
+}
+
+void GaussianDistribution::setParamsFromData( const mat SAMPLES)
+{
+ // Calculation of MU and SIGMA (without weight and important sampling)
+ MU = mean(SAMPLES, 1);
+ SIGMA = cov(trans(SAMPLES), 1);
+ LAMBDA = SIGMA.i();
+ // OR
+ //rowvec REWARD = ones(1, SAMPLES.n_cols);
+ //uint nbImportanceSampling = SAMPLES.n_cols;
+ //setParamsFromData(SAMPLES, REWARD, nbImportanceSampling);
+}
+
+} //end of pbdlib namespace
+
diff --git a/src/taskparameters.cpp b/src/taskparameters.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf27345de96c3f9cb6678e5cf6f91a36fdf6b966
--- /dev/null
+++ b/src/taskparameters.cpp
@@ -0,0 +1,123 @@
+/**
+Copyright (C) 2014, Davide De Tommaso, Milad Malekzadeh
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "pbdlib/taskparameters.h"
+
+namespace pbdlib
+{
+
+TaskParameters::TaskParameters(uint _nVARS, uint nTaskParameters)
+{
+ nVARS = _nVARS;
+ MyTPs.resize(nTaskParameters);
+}
+
+std::vector<TaskParameter>& TaskParameters::getTaskParameters()
+{
+ return MyTPs;
+}
+
+TaskParameter& TaskParameters::getTaskParameters(uint _index)
+{
+ return MyTPs[_index];
+}
+
+void TaskParameters::setTaskParameters(std::vector<TaskParameter> _taskParameters)
+{
+ MyTPs= _taskParameters;
+}
+
+void TaskParameters::setTaskParameters(uint _index, TaskParameter _param)
+{
+ if (MyTPs.size()<=_index)
+ throw std::invalid_argument("TaskParameters::setTaskParameters(uint _index, TaskParameter _param)\n _index cannot be larger or equal than number of frames in TPGMM.");
+ else
+ MyTPs[_index] = _param;
+}
+
+void TaskParameters::loadFromFile(std::string path)
+{
+ mat rawTPs;
+ // Load Data
+ if( !rawTPs.load(path, raw_ascii) )
+ {
+ throw std::invalid_argument( "\n [ERROR]::TaskParameters::readParamsFromTxtFile(std::string path) if( paramTmp.load(path, raw_ascii) ) ... else .");
+ return;
+ }
+
+ // Check dimensions
+ if (rawTPs.n_rows % (nVARS+1)!=0|| rawTPs.n_cols!=nVARS)
+ {
+ throw std::invalid_argument("[Error]::TaskParameters::readParamsFromTxtFiel(std::string path) the size of the data in the file is not consistent with the specified parameter dimensions" );
+ }
+
+
+ // Allocate memory:
+ uint _nTPs = rawTPs.n_rows/(nVARS+1);
+ MyTPs.resize(_nTPs);
+
+ // Write task parameters:
+ uint m;
+ uint index;
+
+ for(m=0; m<MyTPs.size(); m++)
+ {
+ index = m*(nVARS+1);
+ MyTPs[m].b = trans(rawTPs.row(index));
+ MyTPs[m].A = rawTPs.rows(index+1 , index+nVARS);
+ }
+
+}
+
+void TaskParameters::saveToFile(std::string prefix, std::vector<std::string> TPnames)
+{
+
+ // Check size of TaskParameter name
+ if (TPnames.size()!=this->getNumTASKPARAMETERS())
+ throw std::invalid_argument("[ERROR]TaskParameters::saveToFile(...): \n The number of provided Task Parameter names does not correspond to the number Task Parameters.");
+
+ std::string data_fname = prefix + "_TaskParameters.txt";
+ std::string TPnames_fname = prefix + "_TPNames.txt";
+
+ mat tmpMat;
+ TaskParameter tmpTP;
+ for (uint m=0;m<this->getNumTASKPARAMETERS();m++)
+ {
+ tmpTP = this->getTaskParameters(m);
+ if (m==0)
+ tmpMat = join_cols(tmpTP.b.t(),tmpTP.A);
+ else
+ tmpMat = join_cols(tmpMat,join_cols(tmpTP.b.t(),tmpTP.A));
+ }
+
+ // Save Frames:
+ tmpMat.save(data_fname.c_str(),raw_ascii);
+
+ // Save Task Parameter names:
+ std::ofstream fTPnames(TPnames_fname.c_str());
+ for(unsigned int i=0; i<TPnames.size(); i++)
+ fTPnames<< TPnames[i] << endl;
+ fTPnames.close();
+
+
+}
+
+} //end of pbdlib namespace
+
diff --git a/src/tpdemonstration.cpp b/src/tpdemonstration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92e7db3da5216cbe46a22ec1fb9b7ba056697fe0
--- /dev/null
+++ b/src/tpdemonstration.cpp
@@ -0,0 +1,495 @@
+/**
+ *
+Copyright (C) 2015, Martijn Zeestraten
+
+This file is part of PbDLib.
+
+ PbDLib is free software: you can redistribute it and/or modify
+ it under the terms of the GNU Lesser General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PbDLib is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public License
+ along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/tpdemonstration.h"
+
+namespace pbdlib
+{
+
+// ----------------- CONSTRUCTORS -----------------------------------
+TPDemonstration::TPDemonstration(std::string data_path, std::string tp_path, bool DataTrans)
+{
+ // Load Data:
+ loadDataFromFile(data_path, DataTrans);
+
+ // Load taskparameters:
+ loadTaskParametersFromFile(tp_path);
+
+}
+TPDemonstration::TPDemonstration(mat& _data, std::vector<TaskParameters> _VTPs)
+{
+ // Set Data:
+ this->Data.setData(_data);
+ // Set Task Parameters:
+ this->setDataPointTPs(_VTPs);
+}
+TPDemonstration::TPDemonstration(mat& _data, TaskParameters _TPs)
+{
+ // Set Data:
+ this->Data.setData(_data);
+ // Set Task Parmaters:
+ this->setDataPointTPs(_TPs);
+}
+
+
+
+
+
+
+
+// ------------------ SET TASK PARAMETERS --------------------------------
+void TPDemonstration::setDataPointTPs(std::vector<TaskParameters> _PointTPs)
+{
+ // input
+ // _PointTPs: A vector containing [nPOINTS] objects TaskParameters each containing _nTaskParameters.
+ //
+ // Task-parameters are thus specified for each individual data point. This is usefull
+ // when the task-parameters were changing during the demonstration phase.
+
+ // Check Size:
+ if (_PointTPs.size()!=this->getNumPOINTS())
+ {
+ throw std::invalid_argument("[ERROR]TPDemonstration::setDataPointTPs( ... ): \nDimensions of _PointTPs are not consistent with class initialization.");
+ }
+
+ // Different task-parameters for each Data point are given, assign to PointTPs:
+ PointTPs = _PointTPs;
+
+ // Set flag for unique Task Parameters
+ uniqueTPs=true;
+
+}
+
+void TPDemonstration::setDataPointTPs(TaskParameters _TPs)
+{
+ // Input:
+ // _TPs: A object containing _nTaskParameters number of task-parameters
+ //
+ // Different frames for different task parameters are not given. Therefore
+ // we assume that the taskparameters are the same for each datapoint
+
+ // Assign the same task-parameters for each datapoint:
+ for (uint i = 0;i<this->getNumPOINTS();i++)
+ PointTPs[i] = _TPs;
+
+ // reset flag for unique TPs:
+ uniqueTPs = false;
+}
+
+void TPDemonstration::setDataPointTPs(uint pIndex, TaskParameters _TPs)
+{
+ // This function writes a set of task parameters to a specific data point
+ //
+ // Input:
+ // pIndex : Point index of the datapoint we are writing to
+ // _TPs: An object containing _nTaskParameters number of task-parameters
+
+
+ // Check index
+ if (pIndex>=this->getNumPOINTS())
+ throw std::invalid_argument("TPDemonstration::setDataPointTPs(...): \nIndex out of bounds.");
+
+ // Check Parameter dimensions:
+ if (_TPs.getNumTASKPARAMETERS() !=this->getNumTASKPARAMETERS()
+ || _TPs.getNumVARS()!= this->getNumVARS())
+ throw std::invalid_argument("TPDemonstration::setDataPointTPs(...):\nTask parameter dimensions are incompatible with the number of dimensions of the parameter object.");
+
+ // Write Parameters:
+ PointTPs[pIndex] = _TPs;
+
+ // Set flag for unique task parameters:
+ uniqueTPs = true;
+}
+
+void TPDemonstration::setDataPointTPs(std::vector<TaskParameter> _VTP)
+{
+ // Input _VTP : A vector of the object TaskParameter
+ // Each element in the vector represents a parameter set A, b
+ //
+ // No task parameters are specified for each datapoint individually. Therefore
+ // we assume that the taskparameters are the same for each datapoint
+
+ // Copy parameters into parameters object:
+ uint nVARS = this->getNumVARS();
+
+ TaskParameters _TPs(nVARS, nVARS);
+ for (uint i=0;i<_VTP.size();i++)
+ {
+ // Check Dimensions:
+ if (_VTP[i].A.n_elem !=nVARS*nVARS
+ || _VTP[i].b.n_elem != nVARS)
+ throw std::invalid_argument("[ERROR]TPDemonstration::setDataPointTPs(...) \n : Task parameter dimensions are incompatible with the number of variables in the data.");
+ else
+ _TPs.setTaskParameters(i,_VTP[i]);
+ }
+
+ // Set the same for all data points:
+ setDataPointTPs(_TPs);
+
+ // reset flag for unique task parameters:
+ uniqueTPs = false;
+
+}
+
+void TPDemonstration::setDataPointTPs(std::vector<mat> _A, std::vector<colvec> _b)
+{
+ // Function puts the Transformation Matrices containd in _A with
+ // corresponding translation matrices contained in _b into a TaskParameters
+ // object. The taskparameters will hold for all data points
+ // in the data of this class.
+ //
+ // Inputs:
+ // _A : Vector [nTPs] of matrices [nVARS x nVARS];
+ // _b : Vector [nTps] of colvecs [nVars x 1];
+
+
+ // Check dimensions of _A and _b
+ if (_A.size() != _b.size())
+ throw std::invalid_argument("[ERROR]TPDemonstration::setDataPointTPs(...): \nSize of Vectors _A and _b are not consistent.");
+
+ uint nVARS = this->getNumVARS();
+ uint nTPs = _A.size();
+
+ // Copy parameters into TaskParametere structure:
+ TaskParameters tmpTPs(this->getNumVARS(),nTPs);
+ for (uint m=0;m<nTPs;m++)
+ {
+ // Check dimensions
+ if (_A[m].n_elem !=nVARS*nVARS || _b[m].n_elem !=nVARS)
+ throw std::invalid_argument("[ERROR]TPDemonstration::setDataPointTPs(...): \nTask parameter dimensions are incompatible with the number of variables in the data.");
+ else
+ {
+ // Write A and b to temp parameters
+ tmpTPs.getTaskParameters(m).A = _A[m];
+ tmpTPs.getTaskParameters(m).b = _b[m];
+ }
+ }
+
+ // Set the same for all data points:
+ setDataPointTPs(tmpTPs);
+
+ // reset flag for unique task parameters:
+ uniqueTPs = false;
+}
+
+
+
+
+
+// ---------------------------- SET GLOBAL DATA -----------------------------------
+void TPDemonstration::setGlobalData(mat& _Data)
+{
+ // Check Size:
+ if (_Data.n_cols != this->getNumPOINTS()
+ && _Data.n_cols != this->getNumVARS())
+ {
+ throw std::invalid_argument("[ERROR] TPDemonstration::setGlobalData(...): \nDimensions of _Data are not consistent with class initialization.");
+ }
+ this->Data.setData(_Data);
+}
+
+
+
+
+
+// ---------------------- LOAD DATA FROM FILES -----------------------------------
+void TPDemonstration::loadFromFiles(std::string data_path, std::string tp_path, bool DataTrans)
+{
+ // Function loads the demonstration data and corresponding
+ // task parameters from the files specified.
+
+ // First load data
+ this->Data.loadFromFile(data_path, DataTrans);
+
+ // Set dimensions:
+ PointTPs.resize(this->getNumPOINTS());
+
+ // Load Parameters:
+ loadTaskParametersFromFile(tp_path);
+}
+
+void TPDemonstration::loadDataFromFile(std::string data_path, bool DataTrans)
+{
+
+ mat dataRaw;
+ if( !dataRaw.load(data_path, raw_ascii) )
+ {
+ throw std::invalid_argument( "[ERROR]::TaskParameters::readParamsFromTxtFile(...) if( paramTmp.load(path, raw_ascii) ) ... else .");
+ }
+ if (DataTrans)
+ dataRaw = dataRaw.t();
+
+ // Set global data:
+ setGlobalData(dataRaw);
+}
+
+
+
+
+
+
+
+// --------------------- LOAD TASK PARAMETERS -----------------------------------
+void TPDemonstration::loadTaskParametersFromFile(std::string tp_path)
+{
+ // Function loads Taskparameters from file.
+ // The file is assumed to have the following structure:
+ //
+ // <---- Data Points ---->
+ // ^ [b11^T, b12^T, ..., b1p^T]
+ // | [A11 , A12 , ..., A1p ]
+ // | [b21^T, b22^T, ..., b2p^T]
+ // TPs [A21 , A22 , ..., A2p ]
+ // | [... , ... , ..., ... ]
+ // | [bm1^T, bm2^T, ..., bmp^T]
+ // v [Am1 , Am2 , ..., Amp ]
+ //
+ // Where p represents the number of datapoints nPOINTS and
+ // m the number of taskparameters nTaskParameters. When the file only
+ // contains one column of task parmeters. It is assumed that all datapoints
+ // where recorded using the same task parameters.
+ //
+ // Input:
+ // Path to data file
+
+ // Load data from file:
+ mat TPraw;
+ uint nVARS = this->getNumVARS();
+
+ if( !TPraw.load(tp_path, raw_ascii) )
+ {
+ throw std::invalid_argument("[ERROR]TPDemonstration::loadTaskParametersFromFile(...): \n Failure loading parameter file.");
+ }
+
+
+ // check if the task parameters have the correct dimensions:
+ // Number of cols consistent|| Number of rows consistent
+ if ((TPraw.n_cols % nVARS)!=0 || (TPraw.n_rows % (nVARS+1)) !=0)
+ {
+ throw std::invalid_argument("[Error]TPDemonstration::loadTaskParametersFromFile(...):\n The size of the parameter file is inconsistent with the data dimensions." );
+ }
+ uint nTPs = TPraw.n_rows/(nVARS+1); // Number of TPs per column
+
+
+ // Check if the number TPs is equal to 1 (meaning the same set of TPs
+ // for each datapoint), or equal to NumPOINTS or equal to NumPOINTS
+ // (meaning unique TPs for each datapoint)
+ uint _nPoints = TPraw.n_cols/nVARS; // Number of columns with TPs
+ if (_nPoints!=this->getNumPOINTS()&& _nPoints!=1)
+ throw std::invalid_argument("[Error]TPDemonstration::loadTaskParametersFromFile(...): \n The size of the data in the file is inconsistent with the specified parameter dimensions." );
+
+ // Create a vector of Taskparameters object
+ std::vector<TaskParameters> tmpVTPs;
+
+ // Create a tmp TaskParameters Object
+ uint TPind;
+ TaskParameters tmpTPs(nVARS,nTPs);
+ // For each column of Task Parameters
+ for(uint p=0;p<_nPoints;p++)
+ {
+ // For each Task Parameter in the column
+ for(uint m=0; m<nTPs; m++)
+ {
+ TPind= m*(nVARS+1);
+ tmpTPs.getTaskParameters(m).A =
+ TPraw.submat(TPind+1 ,p*nVARS, TPind+nVARS,(p+1)*nVARS-1);
+ tmpTPs.getTaskParameters(m).b =
+ trans(TPraw.submat(TPind, p*nVARS,TPind,(p+1)*nVARS-1));
+ }
+ // Add to vector of TPs:
+ tmpVTPs.push_back(tmpTPs);
+ }
+
+ // Set task parameters to object:
+ if(_nPoints==1)
+ {
+ // Set the same task parameters for each data point
+ setDataPointTPs(tmpVTPs[0]);
+ }
+ else
+ {
+ // Set unique task parameters for each data point:
+ setDataPointTPs(tmpVTPs);
+ }
+}
+
+
+
+
+
+
+void TPDemonstration::saveInFiles(std::string prefix,
+ std::vector<std::string> varnames,
+ std::vector<std::string> TPnames)
+{
+ // Function Saves data and task parameters into files:
+ // - prefix_data.txt
+ // - prefix_TaskParameters.txt
+ // - prefix_VarNames.txt
+ // - prefix_TPNamex.txt
+ //
+ // The file of the task parameters will have the following
+ // structure:
+ //
+ // <---- Data Points ---->
+ // ^ [b11^T, b12^T, ..., b1p^T]
+ // | [A11 , A12 , ..., A1p ]
+ // | [b21^T, b22^T, ..., b2p^T]
+ // TPs [A21 , A22 , ..., A2p ]
+ // | [... , ... , ..., ... ]
+ // | [bm1^T, bm2^T, ..., bmp^T]
+ // v [Am1 , Am2 , ..., Amp ]
+ //
+ // Where p represents the number of datapoints nPOINTS and
+ // m the number of taskparameters nTaskParameters.
+ //
+ // When the file only contains unique task parameters then
+ // the task parameter file will have the following structure:
+ //
+ // ^ [b11^T]
+ // | [A11 ]
+ // | [b21^T]
+ // TPs [A21 ]
+ // | [... ]
+ // | [bm1^T]
+ // v [Am1 ]
+
+ // Check size of varnames:
+ if (varnames.size()!=this->getNumVARS())
+ throw std::invalid_argument("[ERROR]TPDemonstration::saveInFiles(...): \n The number of provided filenames does not correspond to the number of variables.");
+
+ // Check size of TaskParameter names
+ if (TPnames.size()!=this->getNumTASKPARAMETERS())
+ throw std::invalid_argument("[ERROR]TPDemonstration::saveInFiles(...): \n The number of provided Task Parameter names does not correspond to the number Task Parameters.");
+
+
+ std::string data_fname = prefix + "_data.txt";
+ std::string TPs_fname = prefix + "_TaskParameters.txt";
+ std::string varNames_fname = prefix + "_VarNames.txt";
+ std::string TPnames_fname = prefix + "_TPNames.txt";
+
+
+ // Write Data
+ this->Data.setVarNames(varnames);
+ this->Data.saveInFile(data_fname);
+
+ // Save Task Parameter names:
+ std::ofstream fVarNames(varNames_fname.c_str());
+ for(unsigned int i=0; i<varnames.size(); i++)
+ fVarNames<< varnames[i] << endl;
+ fVarNames.close();
+
+ // Put Task Parameters in Matrix:
+ if (uniqueTPs)
+ {
+
+ TaskParameters tmpTPs;
+ TaskParameter tmpTP;
+ mat tmpMat1;
+ mat tmpMat2;
+ for (uint p=0;p<this->getNumPOINTS();p++)
+ {
+ tmpTPs = this->getDataPointTPs(p);
+ for (uint m=0;m<this->getNumTASKPARAMETERS();m++)
+ {
+ // Concatenate TP in sets per data point:
+ tmpTP = tmpTPs.getTaskParameters(m);
+ if (m==0)
+ tmpMat1 = join_cols(tmpTP.b.t(),tmpTP.A);
+ else
+ tmpMat1 = join_cols(tmpMat1,join_cols(tmpTP.b.t(),tmpTP.A));
+ }
+
+ // Concatenate TPs of data points in columns:
+ if (p==0)
+ tmpMat2 = tmpMat1;
+ else
+ tmpMat2 = join_rows(tmpMat2,tmpMat1);
+ }
+ // Save to File:
+ tmpMat2.save(TPs_fname.c_str(),raw_ascii);
+
+ // Save Task Parameter names:
+ std::ofstream fTPnames(TPnames_fname.c_str());
+ for(unsigned int i=0; i<TPnames.size(); i++)
+ fTPnames<< TPnames[i] << endl;
+ fTPnames.close();
+ }
+ else
+ {
+ PointTPs[0].saveToFile(prefix, TPnames);
+ }
+
+
+
+
+}
+
+
+
+// ----------------------- GET INT TASKPARAMETERS FUNCTIONS ---------------------------
+
+// The data is stored in the global space. These functions are used to linear transform the data
+// according to the specified task parameters of each frame
+mat TPDemonstration::getDataInTaskParameters(uint _Pindex)
+{
+ // For each Data point
+ uint nPOINTS = this->getNumPOINTS();
+ mat dataOut = zeros(this->getNumVARS(),nPOINTS);
+ TaskParameter tmpParam;
+ for (uint i =0;i<nPOINTS;i++)
+ {
+ // Get parameters _Pindex corresponding to datapoint i:
+ tmpParam = PointTPs[i].getTaskParameters(_Pindex);
+
+ // Project data from global to local frame: g = Al+b => l =A\(g-b)
+ dataOut.col(i) = solve(tmpParam.A,this->Data.getData(i)-tmpParam.b);
+ }
+ return dataOut;
+}
+
+mat TPDemonstration::getDataInTaskParameters()
+{
+ // Create Matrix of appropriate size:
+ uint nVARS = this->getNumVARS();
+ uint nPOINTS = this->getNumPOINTS();
+ mat dataOut = zeros(nVARS*this->getNumTASKPARAMETERS(),nPOINTS);
+
+ for (uint m=0;m<this->getNumTASKPARAMETERS();m++)
+ {
+ // Stack data of each frame:
+ dataOut.rows(m*nVARS, (m+1)*nVARS-1) = getDataInTaskParameters(m);
+ }
+
+ return dataOut;
+
+
+}
+
+
+uint TPDemonstration::getNumTASKPARAMETERS()
+{
+ if (PointTPs.size()==0)
+ return 0;
+ else
+ return this->PointTPs[0].getNumTASKPARAMETERS();
+}
+
+} // end of pbdlib namespace
diff --git a/src/tpgmm.cpp b/src/tpgmm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0069bb9cd98536d64a3951fb57ea92ac8e6b1edc
--- /dev/null
+++ b/src/tpgmm.cpp
@@ -0,0 +1,485 @@
+/**
+Copyright (C) 2014, Tohid Alizadeh, Milad Malekzadeh, Leonel Rozo, João Silvério, Sylvain Calinon
+
+This file is part of PbDLib.
+
+PbDLib is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+PbDLib is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public License
+along with PbDLib. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pbdlib/tpgmm.h"
+
+namespace pbdlib
+{
+
+TPGMM::TPGMM(uint nVARS, uint nSTATES, uint _nTPs)
+{
+ // Assign object variables:
+ this->nVARS = nVARS;
+ this->nSTATES = nSTATES;
+ this->nTaskParameters = _nTPs;
+ this->PRIORS.resize(this->nSTATES);
+ this->GMMS.reserve(this->nTaskParameters);
+
+
+ // Prepare for calculating Gaussian Product:
+ setAuxiliaryVarsProdGauss();
+}
+
+
+TPGMM::TPGMM(std::vector<TPDemonstration>& demos,uint _nSTATES)
+{
+ // Extract information from the first demonstration:
+ this->nVARS = demos[0].getNumVARS();
+ this->nSTATES = _nSTATES;
+ this->PRIORS = rowvec(nSTATES);
+ this->nTaskParameters = demos[0].getNumTASKPARAMETERS();
+ this->GMMS.reserve(nTaskParameters);
+
+ // Copy all data from the demonstrations. The data matrix will be shaped as:
+ // [nVars*nParameters x nPOINTS]
+ mat tmpData;
+ for (uint i = 0;i<demos.size();i++)
+ {
+ if (i==0)
+ {
+ tmpData = demos[0].getDataInTaskParameters();
+ this->Data = tmpData;
+ }
+ else
+ {
+ tmpData = demos[i].getDataInTaskParameters();
+ this->Data = join_horiz(this->Data,tmpData );
+ }
+
+ /*
+ // For outputting the collected data:
+ cout << tmpData.t() << endl;
+ for (uint n=0;n<nTaskParameters;n++)
+ {
+ cout << "A["<< n << "] demo[" << i<< "] :" << endl;
+ cout << demos[i].getTaskParameters(0).getTaskParameters(n).A << endl;
+ cout << "b["<< n << "] demo[" << i<< "] :" << endl;
+ cout << demos[i].getTaskParameters(0).getTaskParameters(n).b.t() << endl;
+ }
+ cout<< "demo[" << i << "].data, Press Enter" << endl;
+ std::getchar();
+ */
+ }
+
+
+ // Prepare for performing Gaussian Product:
+ setAuxiliaryVarsProdGauss();
+}
+
+void TPGMM::loadTPGMMfromMATLAB(std::string PriorsFileName, std::string VarsFileName, std::string MuFilePrefix, std::string SigmaFilePrefix)
+{
+ std::stringstream index; //auxiliary variable to store the index of the taskParameter
+ std::stringstream MuFileName;
+ std::stringstream SigmaFileName;
+
+ cout << PriorsFileName << endl;
+ cout << VarsFileName << endl;
+
+ for(uint i=0; i<this->nTaskParameters; i++){
+
+ MuFileName.str(""); // Clears the string
+ SigmaFileName.str("");
+ index.str("");
+ index<<(i+1);
+
+ MuFileName << MuFilePrefix << "_P" << index.str() << ".txt";
+ SigmaFileName << SigmaFilePrefix << "_P" << index.str() << ".txt";
+
+ cout << MuFileName.str() << endl;
+ cout << SigmaFileName.str() << endl;
+
+ GMM_Model GMM_temp(PriorsFileName, MuFileName.str(), SigmaFileName.str(), VarsFileName);
+ this->PRIORS = GMM_temp.getPRIORS();
+ this->GMMS.push_back(GMM_temp);
+
+ // Setting variables' name using the last temporary GMM (For avoiding the re-implementation o$
+ if(this->vars_names.size() == 0)
+ this->setVARSNames(GMM_temp.getVARSNames());
+ }
+ cout << "PGMM loaded from text files!" << endl;
+
+
+ // Prepare for performing Gaussian Product:
+ setAuxiliaryVarsProdGauss();
+}
+
+void TPGMM::setAuxiliaryVarsProdGauss()
+{
+ // Function prepares the auxiliary variables for the product of Gaussian:
+ prodGMM= new GMM_Model(nSTATES, nVARS);
+ accMu.set_size(nVARS);
+ tmpMu.set_size(nVARS);
+ accLambda.set_size(nVARS,nVARS);
+ tmpLambda.set_size(nVARS,nVARS);
+
+ // Vectors to hold inv(A) and inv(A^T):
+ invA.resize(nTaskParameters);
+ invAT.resize(nTaskParameters);
+ for (uint m=0;m<nTaskParameters;m++)
+ {
+ invA[m] = zeros(nVARS,nVARS);
+ invAT[m] = zeros(nVARS,nVARS);
+ }
+}
+
+GMM_Model* TPGMM::getTransformedGMM(TaskParameters _TPs, EMatrixType AType)
+{
+ return getTransformedGMM(_TPs.getTaskParameters(),AType);
+}
+
+GMM_Model* TPGMM::getTransformedGMM(std::vector<TaskParameter> _TPs, EMatrixType AType)
+{
+ // Check size of Task taskparameters
+ if (_TPs.size()!=nTaskParameters)
+ throw std::invalid_argument("TPGMM::getTransformedProdGMM(std::vector<TaskParameter> _TPs, EMatrixType AType) Number of supplied task parameters is not consistent expected number of taskparameters");
+
+
+ //Projections in the different candidate taskParameters (see Eq. (3) Calinon et al, Humanoids'2012 paper)
+ if (AType==OTHER || nSTATES<2)
+ {
+ // Use implementation based on covariance matrices when type of matrix A is not specified or
+ // the number of states in the GMM is less than 2
+ for (uint i=0; i<nSTATES; i++){
+ accMu = zeros(nVARS, 1);
+ accLambda= zeros(nVARS, nVARS);
+ for (uint m = 0; m < nTaskParameters ; m++)
+ {
+ // Projecting Zmu of state "i" at taskParameter "m"
+ tmpMu = _TPs[m].A * GMMS[m].getMU(i) + _TPs[m].b;
+
+ // Projecting Zsigma of state "i" at taskParameter "m"
+ tmpLambda= inv(_TPs[m].A * GMMS[m].getSIGMA(i) * trans(_TPs[m].A));
+
+ // Accumulate:
+ accLambda+= tmpLambda;
+ accMu+= tmpLambda* tmpMu;
+ }
+ // Saving the Gaussian distribution for state "i"
+ prodGMM->setMU(i,accLambda.i()*accMu);
+ prodGMM->setLAMBDA(i,accLambda);
+ }
+ prodGMM->setPRIORS(this->PRIORS);
+ }
+ else
+ {
+ // Approach based on Precision matrices:
+ // Requires less inverses
+
+ // Pre-compute A^-1 and (A^T)^-1.
+ // First select method based on type of matrix A:
+ switch (AType)
+ {
+ case ORTHONORMAL:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.t();
+ invAT[m] = _TPs[m].A;
+ }
+ break;
+ case SYMMETRIC:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.i();
+ invAT[m] = invA[m];
+ }
+ break;
+ case INVERTIBLE:
+ for (uint m =0; m<nTaskParameters; m++)
+ {
+ invA[m] = _TPs[m].A.i();
+ invAT[m] = inv(_TPs[m].A.t());
+ }
+ break;
+ }
+
+ // Compute product of Gaussian
+ for (uint i=0; i<nSTATES; i++){
+ accMu = zeros(nVARS, 1);
+ accLambda= zeros(nVARS, nVARS);
+
+ for (uint m = 0; m < nTaskParameters ; m++)
+ {
+ // Projecting Zmu of state "i" at taskParameter "m"
+ tmpMu= _TPs[m].A * GMMS[m].getMU(i) + _TPs[m].b;
+
+ // Projecting Zsigma of state "i" at taskParameter "m"
+ tmpLambda= invAT[m] * GMMS[m].getLAMBDA(i) * invA[m];
+
+ // Accumulate
+ accLambda+= tmpLambda;
+ accMu+= tmpLambda*tmpMu;
+ }
+
+ // Saving the Gaussian distribution for state "i"
+ prodGMM->setMU(i,accLambda.i()*accMu);
+ prodGMM->setLAMBDA(i,accLambda);
+ }
+ prodGMM->setPRIORS(this->PRIORS);
+ }
+
+ /*
+ for (uint i=0;i<nSTATES;i++)
+ {
+ cout << prodGMM->getMU(i) << endl;
+ cout << prodGMM->getSIGMA(i) << endl;
+ }
+ */
+
+ return prodGMM;
+}
+
+/*----------------------------------------------------------------------------
+ * EM-ESTIMATION FUNCTIONS
+ *--------------------------------------------------------------------------- */
+
+
+/*----------------------------------------------------------------------------*/
+uint TPGMM::estimateTensorGMM()
+{
+// cout << "initTensorGMM ";
+ initTensorGMM_timeBased();
+// cout << "Done" << endl;
+
+// cout << "EMTensorGMM ";
+ EM_tensorGMM();
+// cout << "Done" << endl;
+ return true;
+}
+
+/*----------------------------------------------------------------------------*/
+void TPGMM::initTensorGMM_timeBased()
+{
+ // Auxiliary variable:
+ mat _tmpData;
+ mat _Mu;
+ cube _Sigma;
+ rowvec _TimingSep, _Priors;
+ uvec _idTmp;
+ double diagRegMat = 1e-4;
+
+ // Check if Data is of correct size
+ if (Data.n_rows != nTaskParameters*nVARS)
+ {
+ throw std::invalid_argument("[ERROR]::TPGMM::initTensorGMM_timeBased() Data dimension is not consistent with specified number of Parameters and number of Variables.");
+ }
+
+ // Linear Space Gaussian in time:
+ _TimingSep = linspace(min(Data.row(0)), max(Data.row(0)),nSTATES+1).t();
+
+ // Initialize
+ _Mu = zeros(nVARS*nTaskParameters,nSTATES);
+ _Sigma = zeros(nVARS*nTaskParameters,nVARS*nTaskParameters,nSTATES);
+ _Priors = zeros(1,nSTATES);
+
+ for (uint i=0; i<nSTATES; i++)
+ {
+ // Find All indices of data that
+ _idTmp = find(Data.row(0)>=_TimingSep(i));
+ _tmpData = Data.cols(_idTmp);
+ _idTmp = find(_tmpData.row(0) <=_TimingSep(i+1));
+ // Calculate Properties of GMM;
+ _Mu.col(i) = mean(_tmpData.cols(_idTmp),1);
+ _Sigma.slice(i) = cov(_tmpData.cols(_idTmp).t()) + eye(nVARS*nTaskParameters,nVARS*nTaskParameters)*diagRegMat;
+ _Priors(i) = _idTmp.n_elem;
+ }
+ this->PRIORS=_Priors/sum(_Priors);
+
+ // Copy result into a TPGMM structure.
+ // Here we use a vector of GMM in which each GMM represents a representation in one Task Parameter
+ mat _StateSigma, _TPSigma;
+ colvec _TPMu;
+
+ GMM_Model tmpGMM(nSTATES,nVARS);
+ for (uint m=0; m<nTaskParameters; m++){
+ GMMS.push_back(tmpGMM);
+ for (uint i=0; i<nSTATES; i++){
+ // Separate the GMMs for each Task Parameter:
+ _TPMu= _Mu.submat(m*nVARS,i,(m+1)*nVARS-1,i);
+ _StateSigma= _Sigma.slice(i);
+ _TPSigma= _StateSigma.submat(m*nVARS,m*nVARS, (m+1)*nVARS-1,(m+1)*nVARS-1);
+
+ // Saving the Gaussian distribution for state "i"
+ GMMS[m].setSIGMA(i,_TPSigma);
+ GMMS[m].setMU(i,_TPMu);
+ }
+ GMMS[m].setPRIORS(this->PRIORS);
+ }
+
+}
+
+
+/*----------------------------------------------------------------------------*/
+void TPGMM::initTensorGMM_kmeans()
+{
+ //Joao: you can implement this based on the Matlab code
+}
+
+/*----------------------------------------------------------------------------*/
+void TPGMM::EM_tensorGMM()
+{
+ //Parameters of the EM algorithm
+ uint nbMinSteps = 5; //Minimum number of iterations allowed
+ uint nbMaxSteps = 100; //Maximum number of iterations allowed
+ double maxDiffLL = 1E-4; //Likelihood increase threshold to stop the algorithm
+ double diagRegularizationFactor = 1E-4;
+
+ // Check if Data is of correct size
+ if (Data.n_rows != nTaskParameters*nVARS)
+ {
+ throw std::invalid_argument("[ERROR]::TPGMM::EM_tensorGMM() Data dimension is not consistent with specified number of Parameters and number of Variables.");
+ }
+
+ // Start EM algorithm
+ uint nDATA = Data.n_cols; //nDATA is the total number of data points including all the demonstrations
+ mat DataTmp,DataTmp2;
+ vec MuTmp;
+ mat SigmaTmp;
+ std::vector<mat> GAMMA0;
+ mat GAMMA = zeros(nVARS, nDATA);
+ mat GAMMA2 = zeros(nVARS, nDATA);
+ vec LL = zeros(nbMaxSteps);
+ GAMMA0.resize(nTaskParameters);
+ for (uint m=0;m<nTaskParameters; m++){
+ GAMMA0[m] = zeros(nVARS, nDATA);
+ }
+
+ cout<<"Start to learn TPGMM using tensor EM"<<endl;
+ for (uint nbIter=0; nbIter<nbMaxSteps; nbIter++){
+ // Expectation-Step, see Eq. (6.1) in doc/TechnicalReport.pdf
+ mat L = ones(nVARS,nDATA);
+ for (uint m=0; m<nTaskParameters; m++){
+ DataTmp = Data.rows(m*nVARS, (m+1)*nVARS-1);
+ for(uint i=0; i<nSTATES; i++){
+ GAMMA0[m].row(i) = GMMS[m].getPRIORS(i) * trans(GMMS[m].getCOMPONENTS(i).getPDFValue(DataTmp));
+ L.row(i) = L.row(i) % GAMMA0[m].row(i); //elementwise product
+ }
+ }
+ GAMMA = L/repmat(sum(L,0)+REALMIN,L.n_rows,1);
+ GAMMA2 = GAMMA/repmat(sum(GAMMA,1),1,GAMMA.n_cols);
+
+ // Maximization-Step, see Eq. (6.2),(6.3) and (6.4) in doc/TechnicalReport.pdf
+ MuTmp = zeros(nVARS);
+ SigmaTmp = zeros(nVARS, nVARS);
+ for (uint m=0; m<nTaskParameters; m++){
+ DataTmp = Data.rows(m*nVARS, (m+1)*nVARS-1);
+ for(uint i=0; i<nSTATES; i++){
+ //update PRIORS
+ PRIORS(i) = sum(sum(GAMMA.row(i)))/GAMMA.n_cols;
+ //update Mu
+ MuTmp = DataTmp * trans(GAMMA2.row(i));
+ GMMS[m].setMU(i, MuTmp);
+ //update Sigma
+ DataTmp2 = DataTmp - repmat(MuTmp, 1, DataTmp.n_cols);
+ SigmaTmp = DataTmp2 * diagmat(GAMMA2.row(i)) * trans(DataTmp2) + eye(nVARS,nVARS)*diagRegularizationFactor;
+ GMMS[m].setSIGMA(i, SigmaTmp);
+ }
+ GMMS[m].setPRIORS(PRIORS);
+ }
+
+ //Compute average log-likelihood
+ mat LL_Tmp;
+ LL(nbIter) = 0;
+ for(int n=0; n<nDATA; n++){
+ LL_Tmp = log(sum(L.col(n),0));
+ LL(nbIter) += as_scalar(sum(LL_Tmp,1));
+ }
+ LL(nbIter)/= nDATA;
+
+ //Stop the algorithm if EM converged (small change of LL)
+ if (nbIter>nbMinSteps){
+ if (LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1){
+ cout<<"EM converged after "<<nbIter<<" iterations"<<endl;
+ cout<<"The results of the model after the learning by EM:"<<endl;
+ cout<<"PRIORS"<<endl;
+ cout<<PRIORS<<endl;
+ for (int i=0; i<nSTATES; i++){
+ for(int m=0; m<nTaskParameters; m++){
+ cout<<"GMMS["<<m<<"].getMU("<<i<<"): "<<endl;
+ cout<<GMMS[m].getMU(i)<<endl;
+ cout<<"GMMS["<<m<<"].getSIGMA("<<i<<"): "<<endl;
+ cout<<GMMS[m].getSIGMA(i)<<endl;
+ }
+ }
+ break;
+ }
+ }
+ if (nbIter==nbMaxSteps)
+ cout<<"EM converged after "<<nbMaxSteps<<" iterations"<<endl;
+ }
+}
+
+
+/*----------------------------------------------------------------------------
+ * GET/SET FUNCTIONS
+ *--------------------------------------------------------------------------- */
+uint TPGMM::getNumVARS()
+{
+ return this->nVARS;
+}
+
+std::vector<GMM_Model>& TPGMM::getGMMS(){
+ return this->GMMS;
+}
+
+GMM_Model& TPGMM::getGMMS(uint id)
+{
+ // Check if GMM is available:
+ if (GMMS.size()<=id)
+ throw std::invalid_argument("[Error]TPGMM::getGMMS(int id), id cannot be larger than number of frames in TPGMM.");
+
+ return this->GMMS[id];
+}
+
+uint TPGMM::getNumSTATES()
+{
+ return this->nSTATES;
+}
+
+uint TPGMM::getNumFRAMES()
+{
+ return this->nTaskParameters;
+}
+
+rowvec& TPGMM::getPRIORS()
+{
+ return this->PRIORS;
+}
+
+double TPGMM::getPRIORS(uint id)
+{
+ if (nSTATES<=id)
+ throw std::invalid_argument("[Error]TPGMM::getPRIORS(int id), id cannot be larger than number of stats in TPGMM.");
+ return this->PRIORS(id);
+}
+
+std::vector<std::string>& TPGMM::getVARSNames()
+{
+ return this->vars_names;
+}
+
+void TPGMM::setVARSNames(const std::vector<std::string>& vars)
+{
+ if(vars.size() == nVARS)
+ this->vars_names = vars;
+ else
+ std::cout << "\n [ERROR]::TPGMM::setVARSNames if(vars.size() == nVARS) ... else .";
+}
+
+} //end of namespace pbdlib
+