diff --git a/CMakeLists.txt b/CMakeLists.txt
index 14cea49b702abb8715aeb8e8938ab01d6771d175..f7942870da7ecf50291e955e4aba9d23b02597e1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -105,6 +105,17 @@ set(GFX3_SUPPORT_SOURCES
# Executables
+add_executable(demo_GMR01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_GMR01.cpp
+)
+target_link_libraries(demo_GMR01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+)
+
add_executable(demo_infHorLQR01_glsl
${GFX3_SUPPORT_SOURCES}
${PROJECT_SOURCE_DIR}/src/demo_infHorLQR01_glsl.cpp
@@ -165,21 +176,21 @@ target_link_libraries(demo_MPC_velocity01
${GLFW_LIB}
)
-add_executable(demo_online_gmm01
+add_executable(demo_online_GMM01
${GL2_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_online_gmm01.cpp
+ ${PROJECT_SOURCE_DIR}/src/demo_online_GMM01.cpp
)
-target_link_libraries(demo_online_gmm01
+target_link_libraries(demo_online_GMM01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
)
-add_executable(demo_online_hsmm01
+add_executable(demo_online_HSMM01
${GL2_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_online_hsmm01.cpp
+ ${PROJECT_SOURCE_DIR}/src/demo_online_HSMM01.cpp
)
-target_link_libraries(demo_online_hsmm01
+target_link_libraries(demo_online_HSMM01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLFW_LIB}
@@ -239,10 +250,10 @@ target_link_libraries(demo_Riemannian_pose_batchLQR01
${GLFW_LIB}
)
-add_executable(demo_Riemannian_pose_gmm01
- ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_gmm01.cpp
+add_executable(demo_Riemannian_pose_GMM01
+ ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_GMM01.cpp
)
-target_link_libraries(demo_Riemannian_pose_gmm01
+target_link_libraries(demo_Riemannian_pose_GMM01
${ARMADILLO_LIBRARIES}
)
@@ -256,10 +267,10 @@ target_link_libraries(demo_Riemannian_pose_infHorLQR01
${GLFW_LIB}
)
-add_executable(demo_Riemannian_pose_tpgmm01
- ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_tpgmm01.cpp
+add_executable(demo_Riemannian_pose_TPGMM01
+ ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_pose_TPGMM01.cpp
)
-target_link_libraries(demo_Riemannian_pose_tpgmm01
+target_link_libraries(demo_Riemannian_pose_TPGMM01
${ARMADILLO_LIBRARIES}
)
@@ -273,21 +284,21 @@ target_link_libraries(demo_Riemannian_quat_infHorLQR01
${GLFW_LIB}
)
-add_executable(demo_Riemannian_quat_tpgmm01
+add_executable(demo_Riemannian_quat_TPGMM01
${GL2_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_quat_tpgmm01.cpp
+ ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_quat_TPGMM01.cpp
)
-target_link_libraries(demo_Riemannian_quat_tpgmm01
+target_link_libraries(demo_Riemannian_quat_TPGMM01
${ARMADILLO_LIBRARIES}
${OPENGL_LIBRARIES}
${GLFW_LIB}
)
-add_executable(demo_Riemannian_sphere_gmm01
+add_executable(demo_Riemannian_sphere_GMM01
${GFX2_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_gmm01.cpp
+ ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_GMM01.cpp
)
-target_link_libraries(demo_Riemannian_sphere_gmm01
+target_link_libraries(demo_Riemannian_sphere_GMM01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLEW_LIBRARIES}
@@ -316,11 +327,11 @@ target_link_libraries(demo_Riemannian_sphere_product01
${GLFW_LIB}
)
-add_executable(demo_Riemannian_sphere_tpgmm01
+add_executable(demo_Riemannian_sphere_TPGMM01
${GFX2_SUPPORT_SOURCES}
- ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_tpgmm01.cpp
+ ${PROJECT_SOURCE_DIR}/src/demo_Riemannian_sphere_TPGMM01.cpp
)
-target_link_libraries(demo_Riemannian_sphere_tpgmm01
+target_link_libraries(demo_Riemannian_sphere_TPGMM01
${OPENGL_LIBRARIES}
${ARMADILLO_LIBRARIES}
${GLEW_LIBRARIES}
@@ -337,3 +348,14 @@ target_link_libraries(demo_TPbatchLQR01
${GLFW_LIB}
${ARMADILLO_LIBRARIES}
)
+
+add_executable(demo_TPGMMProduct01
+ ${GFX2_SUPPORT_SOURCES}
+ ${PROJECT_SOURCE_DIR}/src/demo_TPGMMProduct01.cpp
+ )
+target_link_libraries(demo_TPGMMProduct01
+ ${OPENGL_LIBRARIES}
+ ${GLEW_LIBRARIES}
+ ${GLFW_LIB}
+ ${ARMADILLO_LIBRARIES}
+ )
diff --git a/README.md b/README.md
index c37e84e15da6857cdad5ed6ef96c8db4f920828c..8a9f9d8203b17dec6b1ae6ef4611ee5c6b1e58bb 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,6 @@
-# PbDlib-cpp
+# PbDlib-cpp-sandbox
-PbDlib-cpp is a set of tools in C++ combining statistical learning, optimal control and differential geometry for programming-by-demonstration applications.
+PbDlib-cpp-sandbox is a set of tools in C++ combining statistical learning, optimal control and differential geometry for programming-by-demonstration applications.
Other versions of the library are available at http://www.idiap.ch/software/pbdlib/ (in Matlab, C++ and Python).
@@ -63,6 +63,12 @@ See [https://gitlab.idiap.ch/rli/pbdlib-cpp/blob/master/examples.md](https://git
### Gallery
+demo\_GMR01
+
+
+
+***
+
demo\_MPC\_batch01
@@ -87,9 +93,9 @@ demo\_MPC\_velocity01
***
-demo\_online\_gmm01
+demo\_online\_GMM01
-
+
***
@@ -129,15 +135,15 @@ demo\_Riemannian\_quat\_infHorLQR01
***
-demo\_Riemannian\_quat\_tpgmm01
+demo\_Riemannian\_quat\_TPGMM01
-
+
***
-demo\_Riemannian\_sphere\_gmm01
+demo\_Riemannian\_sphere\_GMM01
-
+
***
@@ -153,12 +159,18 @@ demo\_Riemannian\_sphere\_product01
***
-demo\_Riemannian\_sphere\_tpgmm01
+demo\_Riemannian\_sphere\_TPGMM01
-
+
***
demo\_TPbatchLQR01
+
+***
+
+demo\_TPGMMProduct01
+
+
diff --git a/examples.md b/examples.md
index 426067a769093220708f1bf7f1567d4f035038b1..7d0ac36ea7e64b0c61a69e939539d5574cb141cb 100644
--- a/examples.md
+++ b/examples.md
@@ -153,23 +153,25 @@ This project will build a number of executables, as listed in the table below.
| Filename | ref. | Description |
|----------|------|-------------|
| demo\_infHorLQR01\_glsl | [1] | Discrete infinite horizon linear quadratic regulation |
+| demo\_GMR01 | [1] | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction |
| demo\_MPC\_batch01 | [1,8] | Model predictive control (MPC) with batch linear quadratic tracking (LQT) formulation |
| demo\_MPC\_iterative01 | [1,8] | Model predictive control (MPC) with iterative linear quadratic tracking (LQT) formulation |
| demo\_MPC\_semitied01 | [5,8] | MPC with semi-tied covariances |
| demo\_MPC\_velocity01 | [1,8] | MPC with an objective including velocity tracking |
-| demo\_online\_gmm01 | [6] | Online GMM learning and LQR-based trajectory generation |
-| demo\_online\_hsmm01 | [2,7] | Online HSMM learning and sampling with LQR-based trajectory generation |
+| demo\_online\_GMM01 | [6] | Online GMM learning and LQR-based trajectory generation |
+| demo\_online\_HSMM01 | [2,7] | Online HSMM learning and sampling with LQR-based trajectory generation |
| demo\_proMP01 | [] | Conditioning on trajectory distributions with ProMP |
| demo\_Riemannian\_cov\_GMR01 | [10] | GMR with time as input and covariance data as output by relying on Riemannian manifold |
| demo\_Riemannian\_cov\_interp02 | [4] | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
| demo\_Riemannian\_pose\_batchLQR01 | [] | Linear quadratic regulation of pose by relying on Riemannian manifold and batch LQR (MPC without constraints) |
-| demo\_Riemannian\_pose\_gmm01 | [] | GMM for 3D position and unit quaternion data by relying on Riemannian manifold |
+| demo\_Riemannian\_pose\_GMM01 | [] | GMM for 3D position and unit quaternion data by relying on Riemannian manifold |
| demo\_Riemannian\_pose\_infHorLQR01 | [] | Linear quadratic regulation of pose by relying on Riemannian manifold and infinite-horizon LQR |
-| demo\_Riemannian\_pose\_tpgmm01 | [] | TP-GMM of pose (R3 x S3) with two frames |
+| demo\_Riemannian\_pose\_TPGMM01 | [] | TP-GMM of pose (R3 x S3) with two frames |
| demo\_Riemannian\_quat\_infHorLQR01 | [3] | Linear quadratic regulation on hypersphere (orientation as unit quaternions) by relying on Riemannian manifold and infinite-horizon LQR |
-| demo\_Riemannian\_quat\_tpgmm01 | [] | TP-GMM on S3 (unit quaternion) with two frames |
-| demo\_Riemannian\_sphere\_gmm01 | [3] | GMM for data on a sphere by relying on Riemannian manifold |
+| demo\_Riemannian\_quat\_TPGMM01 | [] | TP-GMM on S3 (unit quaternion) with two frames |
+| demo\_Riemannian\_sphere\_GMM01 | [3] | GMM for data on a sphere by relying on Riemannian manifold |
| demo\_Riemannian\_sphere\_infHorLQR01 | [3] | Linear quadratic regulation on a sphere by relying on Riemannian manifold and infinite-horizon LQR |
| demo\_Riemannian\_sphere\_product01 | [] | Gaussian product on sphere |
-| demo\_Riemannian\_sphere\_tpgmm01 | [3] | TP-GMM for data on a sphere by relying on Riemannian manifold (with single frame) |
+| demo\_Riemannian\_sphere\_TPGMM01 | [3] | TP-GMM for data on a sphere by relying on Riemannian manifold (with single frame) |
| demo\_TPbatchLQR01 | [1] | Linear quadratic control (unconstrained linear MPC) acting in multiple frames, which is equivalent to the fusion of Gaussian controllers |
+| demo\_TPGMMProduct01 | [] | Product of Gaussians for a Task-Parametrized GMM |
diff --git a/images/demo_GMR01.gif b/images/demo_GMR01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..04a140c5fa7cf06c8b24337cfe36d30229bd0375
Binary files /dev/null and b/images/demo_GMR01.gif differ
diff --git a/images/demo_Riemannian_quat_tpgmm01.gif b/images/demo_Riemannian_quat_TPGMM01.gif
similarity index 100%
rename from images/demo_Riemannian_quat_tpgmm01.gif
rename to images/demo_Riemannian_quat_TPGMM01.gif
diff --git a/images/demo_Riemannian_sphere_gmm01.gif b/images/demo_Riemannian_sphere_GMM01.gif
similarity index 100%
rename from images/demo_Riemannian_sphere_gmm01.gif
rename to images/demo_Riemannian_sphere_GMM01.gif
diff --git a/images/demo_Riemannian_sphere_tpgmm01.gif b/images/demo_Riemannian_sphere_TPGMM01.gif
similarity index 100%
rename from images/demo_Riemannian_sphere_tpgmm01.gif
rename to images/demo_Riemannian_sphere_TPGMM01.gif
diff --git a/images/demo_TPGMMProduct01.gif b/images/demo_TPGMMProduct01.gif
new file mode 100644
index 0000000000000000000000000000000000000000..163e82c4db08d73b9953c947693e5ceb0a07312c
Binary files /dev/null and b/images/demo_TPGMMProduct01.gif differ
diff --git a/images/demo_online_gmm01.gif b/images/demo_online_GMM01.gif
similarity index 100%
rename from images/demo_online_gmm01.gif
rename to images/demo_online_GMM01.gif
diff --git a/include/gfx2.h b/include/gfx2.h
index 8f72291dca00e61df1f160d49038f32ef6e4b995..c4960423d749700967cade511b6318a2878070aa 100644
--- a/include/gfx2.h
+++ b/include/gfx2.h
@@ -265,6 +265,7 @@ namespace gfx2
// Other
bool lightning_enabled;
+ bool use_one_minus_src_alpha_blending;
};
//-------------------------------------------------------------------------
@@ -298,7 +299,8 @@ namespace gfx2
model_t create_line(const arma::fvec& color, const arma::mat& points,
const arma::fvec& position = arma::zeros<arma::fvec>(3),
const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
+ transforms_t* parent_transforms = 0,
+ bool line_strip = true);
//-------------------------------------------------------------------------
// Create a line mesh, colored (no lightning), from an array containing the
@@ -307,7 +309,8 @@ namespace gfx2
model_t create_line(const arma::fvec& color, const std::vector<arma::vec>& points,
const arma::fvec& position = arma::zeros<arma::fvec>(3),
const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
- transforms_t* parent_transforms = 0);
+ transforms_t* parent_transforms = 0,
+ bool line_strip = true);
//-------------------------------------------------------------------------
// Create a mesh, colored (no lightning), from a matrix containing the
@@ -318,6 +321,24 @@ namespace gfx2
const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
transforms_t* parent_transforms = 0);
+ //-------------------------------------------------------------------------
+ // Create a mesh representing a gaussian, colored (no lightning)
+ //-------------------------------------------------------------------------
+ model_t create_gaussian_background(const arma::fvec& color, const arma::vec& mu,
+ const arma::mat& sigma,
+ const arma::fvec& position = arma::zeros<arma::fvec>(3),
+ const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+ transforms_t* parent_transforms = 0);
+
+ //-------------------------------------------------------------------------
+ // Create a line mesh representing a gaussian border, colored (no lightning)
+ //-------------------------------------------------------------------------
+ model_t create_gaussian_border(const arma::fvec& color, const arma::vec& mu,
+ const arma::mat& sigma,
+ const arma::fvec& position = arma::zeros<arma::fvec>(3),
+ const arma::fmat& rotation = arma::eye<arma::fmat>(4, 4),
+ transforms_t* parent_transforms = 0);
+
//-------------------------------------------------------------------------
// Release the OpenGL resources used by the model
//-------------------------------------------------------------------------
@@ -414,4 +435,31 @@ namespace gfx2
bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
arma::fvec &result);
+
+ /******************************** OTHERS *********************************/
+
+ //-------------------------------------------------------------------------
+ // Returns the vertices needed to create a mesh representing the background
+ // a gaussian
+ //
+ // The result is a matrix of shape (2, nb_points * 3)
+ //-------------------------------------------------------------------------
+ arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
+ int nb_points = 60);
+
+ //-------------------------------------------------------------------------
+ // Returns the vertices needed to create a line representing the border of
+ // a gaussian
+ //
+ // If line_strip is true:
+ // - The result is a matrix of shape (2, nb_points)
+ // - The rendering mode must be GL_LINE_STRIP
+ //
+ // If line_strip is false:
+ // - The result is a matrix of shape (2, nb_points * 2)
+ // - The rendering mode must be GL_LINES
+ //-------------------------------------------------------------------------
+ arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
+ int nb_points = 60, bool line_strip = true);
+
};
diff --git a/src/demo_GMR01.cpp b/src/demo_GMR01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1dda2827899402d0090c94548a6052030b8f4518
--- /dev/null
+++ b/src/demo_GMR01.cpp
@@ -0,0 +1,1026 @@
+/*
+ * demo_GMR01.cpp
+ *
+ * Gaussian mixture model (GMM) with time-based Gaussian mixture regression
+ * (GMR) used for reproduction.
+ *
+ * @article{Calinon16JIST,
+ * author="Calinon, S.",
+ * title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
+ * journal="Intelligent Service Robotics",
+ * publisher="Springer Berlin Heidelberg",
+ * doi="10.1007/s11370-015-0187-9",
+ * year="2016",
+ * volume="9",
+ * number="1",
+ * pages="1--29"
+ * }
+ *
+ * Authors: Sylvain Calinon, Philip Abbet
+ */
+
+
+#include <stdio.h>
+#include <armadillo>
+#include <mvn.h>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <imgui_internal.h>
+#include <window_utils.h>
+
+using namespace arma;
+
+
+/***************************** ALGORITHM SECTION *****************************/
+
+typedef std::vector<vec> vector_list_t;
+typedef std::vector<mat> matrix_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the parameters used by the algorithm. Some of them are
+// modifiable through the UI, others are hard-coded.
+//-----------------------------------------------------------------------------
+struct parameters_t {
+ int nb_states; // Number of components in the GMM
+ int nb_data; // Number of datapoints in a trajectory
+ float dt; // Time step (without rescaling, large values such
+ // as 1 has the advantage of creating clusers based
+ // on position information)
+};
+
+
+//-----------------------------------------------------------------------------
+// Model trained using the algorithm
+//-----------------------------------------------------------------------------
+struct model_t {
+ parameters_t parameters; // Parameters used to train the model
+
+ int nb_var; // Number of variables [t,x1,x2]
+ vector_list_t mu;
+ matrix_list_t sigma;
+ vec priors;
+};
+
+
+//-----------------------------------------------
+
+arma::vec gaussPDF(mat Data, colvec Mu, mat Sigma) {
+
+ int nbVar = Data.n_rows;
+ int nbData = Data.n_cols;
+ Data = Data.t() - repmat(Mu.t(), nbData, 1);
+
+ vec prob = sum((Data * inv(Sigma)) % Data, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nbVar) * det(Sigma) + 2.2251E-308);
+
+ return prob;
+}
+
+//-----------------------------------------------------------------------------
+// Training of the model
+//-----------------------------------------------------------------------------
+void learn(const matrix_list_t& demos, model_t &model) {
+
+ model.nb_var = 3;
+
+ // Initialization of Gaussian Mixture Model (GMM) parameters by clustering
+ // the data into equal bins based on the first variable (time steps).
+ const float diag_reg_fact = 1e-4f;
+
+ vec timing_sep = linspace<vec>(
+ demos[0](0, 0), demos[0](0, demos[0].n_cols - 1), model.parameters.nb_states + 1
+ );
+
+ model.mu.clear();
+ model.sigma.clear();
+ model.priors = vec(model.parameters.nb_states);
+
+ mat data(model.nb_var, model.parameters.nb_data * demos.size());
+ for (unsigned int m = 0; m < demos.size(); ++m) {
+ data(span::all, span(m * model.parameters.nb_data,
+ (m + 1) * model.parameters.nb_data - 1)) =
+ demos[m];
+ }
+
+ for (unsigned int i = 0; i < model.parameters.nb_states; ++i) {
+ uvec idtmp = find( (data(0, span::all) >= timing_sep(i)) &&
+ (data(0, span::all) < timing_sep(i + 1)) );
+
+ model.priors(i) = idtmp.size();
+ model.mu.push_back(mean(data.cols(idtmp), 1));
+
+ mat sigma = cov(data.cols(idtmp).t());
+
+ // Optional regularization term to avoid numerical instability
+ sigma = sigma + eye(model.nb_var, model.nb_var) * diag_reg_fact;
+
+ model.sigma.push_back(sigma);
+ }
+
+ model.priors = model.priors / sum(model.priors);
+
+
+ // Training of a Gaussian mixture model (GMM) with an expectation-maximization
+ // (EM) algorithm
+ const int nb_max_steps = 100; // Maximum number of iterations allowed
+ const int nb_min_steps = 5; // Minimum number of iterations allowed
+ const double max_diff_log_likelihood = 1e-4; // Likelihood increase threshold
+ // to stop the algorithm
+
+ std::vector<double> log_likelihoods;
+
+ for (int iter = 0; iter < nb_max_steps; ++iter) {
+
+ // E-step
+ mat L = ones(model.parameters.nb_states, data.n_cols);
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ L(i, span::all) = model.priors(i) * mat(gaussPDF(data, model.mu[i], model.sigma[i])).t();
+ }
+
+ mat gamma = L / repmat(sum(L, 0) + DBL_MIN, model.parameters.nb_states, 1);
+ mat gamma2 = gamma / repmat(sum(gamma, 1), 1, data.n_cols);
+
+
+ // M-step
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+
+ // Update priors
+ model.priors(i) = sum(gamma(i, span::all)) / data.n_cols;
+
+ // Update mu
+ model.mu[i] = data * gamma2(i, span::all).t();
+
+ // Update sigma
+ mat data_tmp = data - repmat(model.mu[i], 1, data.n_cols);
+ model.sigma[i] = data * diagmat(gamma2(i, span::all)) * data_tmp.t() +
+ eye(data.n_rows, data.n_rows) * diag_reg_fact;
+ }
+
+ // Compute average log-likelihood
+ log_likelihoods.push_back(vec(sum(log(sum(L, 0)), 1))[0] / data.n_cols);
+
+ // Stop the algorithm if EM converged (small change of log-likelihood)
+ if (iter >= nb_min_steps) {
+ if (log_likelihoods[iter] - log_likelihoods[iter - 1] < max_diff_log_likelihood)
+ break;
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Gaussian mixture regression (GMR)
+//-----------------------------------------------------------------------------
+void compute_GMR(const model_t& model, const vec& time_steps, mat &points,
+ matrix_list_t &sigma_out) {
+
+ const int in = 0;
+ const span out(1, model.nb_var - 1);
+
+ const int nb_data = time_steps.n_elem;
+ const int nb_var_out = out.b - out.a + 1;
+
+ const float diag_reg_fact = 1e-8f;
+
+ mat mu_tmp = zeros(nb_var_out, model.parameters.nb_states);
+ points = zeros(nb_var_out, nb_data);
+ sigma_out.clear();
+
+ mat H(model.parameters.nb_states, nb_data);
+
+ for (int t = 0; t < nb_data; ++t) {
+
+ // Compute activation weight
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ mat time_step(1, 1);
+ time_step(0, 0) = time_steps(t);
+
+ vec mu(1);
+ mu(0) = model.mu[i](in);
+
+ mat sigma(1, 1);
+ sigma(0, 0) = model.sigma[i](in, in);
+
+ H(i, t) = model.priors(i) * gaussPDF(time_step, mu, sigma)[0];
+ }
+
+ H(span::all, t) = H(span::all, t) / sum(H(span::all, t) + DBL_MIN);
+
+ // Compute conditional means
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ mu_tmp(span::all, i) = model.mu[i](out) +
+ model.sigma[i](out, in) / model.sigma[i](in, in) *
+ (time_steps(t) * model.mu[i](in));
+
+ points(span::all, t) = points(span::all, t) + H(i, t) * mu_tmp(span::all, i);
+ }
+
+ // Compute conditional covariances
+ mat sigma = zeros(nb_var_out, nb_var_out);
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ mat sigma_tmp = model.sigma[i](out, out) -
+ model.sigma[i](out, in) / model.sigma[i](in, in) *
+ model.sigma[i](in, out);
+
+ sigma = sigma + H(i, t) * (sigma_tmp + mu_tmp(span::all, i) * mu_tmp(span::all, i).t());
+ }
+
+ sigma = sigma - points(span::all, t) * points(span::all, t).t() +
+ eye(nb_var_out, nb_var_out) * diag_reg_fact;
+
+ sigma_out.push_back(sigma);
+ }
+}
+
+
+/****************************** HELPER FUNCTIONS *****************************/
+
+static void error_callback(int error, const char* description) {
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+
+//-----------------------------------------------------------------------------
+// Contains all the informations about a viewport
+//-----------------------------------------------------------------------------
+struct viewport_t {
+ int x;
+ int y;
+ int width;
+ int height;
+
+ // Projection matrix parameters
+ arma::vec projection_top_left;
+ arma::vec projection_bottom_right;
+ double projection_near;
+ double projection_far;
+};
+
+
+//-----------------------------------------------------------------------------
+// Helper function to setup a viewport
+//-----------------------------------------------------------------------------
+void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
+ double near = -1.0, double far = 1.0) {
+
+ viewport->x = x;
+ viewport->y = y;
+ viewport->width = width;
+ viewport->height = height;
+ viewport->projection_top_left = vec({ (double) -width / 2,
+ (double) height / 2 });
+ viewport->projection_bottom_right = vec({ (double) width / 2,
+ (double) -height / 2 });
+ viewport->projection_near = near;
+ viewport->projection_far = far;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from UI-space to OpenGL-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec ui2fb(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // ui -> viewport
+ result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width - viewport.x;
+ result(1) = (window_size.win_height - coords(1)) *
+ (float) window_size.fb_height / (float) window_size.win_height - viewport.y;
+
+ // viewport -> fb
+ result(0) = result(0) - (float) viewport.width * 0.5f;
+ result(1) = result(1) - (float) viewport.height * 0.5f;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from OpenGL-space to UI-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // fb -> viewport
+ result(0) = coords(0) + (float) viewport.width * 0.5f;
+ result(1) = coords(1) + (float) viewport.height * 0.5f;
+
+ // viewport -> ui
+ result(0) = (result(0) + viewport.x) * (float) window_size.win_width / (float) window_size.fb_width;
+
+ result(1) = window_size.win_height - (result(1) + viewport.y) * (float) window_size.win_height / (float) window_size.fb_height;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Colors of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const mat COLORS({
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ { 0.0, 0.75, 0.75 },
+ { 0.75, 0.0, 0.75 },
+ { 0.75, 0.75, 0.0 },
+ { 0.25, 0.25, 0.25 },
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+});
+
+
+//-----------------------------------------------------------------------------
+// Create a demonstration (with a length of 'timestamps.size()') from a
+// trajectory (of any length)
+//-----------------------------------------------------------------------------
+mat sample_trajectory(const vector_list_t& trajectory, const vec& time_steps) {
+
+ // Resampling of the trajectory
+ vec x(trajectory.size());
+ vec y(trajectory.size());
+ vec x2(trajectory.size());
+ vec y2(trajectory.size());
+
+ for (size_t i = 0; i < trajectory.size(); ++i) {
+ x(i) = trajectory[i](0);
+ y(i) = trajectory[i](1);
+ }
+
+ vec from_indices = linspace<vec>(0, trajectory.size() - 1, trajectory.size());
+ vec to_indices = linspace<vec>(0, trajectory.size() - 1, time_steps.size());
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ // Create the demonstration
+ mat demo(3, time_steps.size());
+ for (int i = 0; i < time_steps.size(); ++i) {
+ demo(0, i) = time_steps[i];
+ demo(1, i) = x2[i];
+ demo(2, i) = y2[i];
+ }
+
+ return demo;
+}
+
+
+//-----------------------------------------------------------------------------
+// Contains all the needed infos about the state of the application (values of
+// the parameters modifiable via the UI, which action the user is currently
+// doing, ...)
+//-----------------------------------------------------------------------------
+struct gui_state_t {
+ // Indicates if the user is currently drawing a new demonstration
+ bool is_drawing_demonstration;
+
+ // Indicates if the parameters dialog is displayed
+ bool is_parameters_dialog_displayed;
+
+ // Indicates if the parameters were modified through the UI
+ bool are_parameters_modified;
+
+ // Indicates if the reproductions must be recomputed
+ bool must_recompute_GMR;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ // in parameters_t)
+ int parameter_nb_states;
+ int parameter_nb_data;
+};
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations & model" viewport
+//-----------------------------------------------------------------------------
+void draw_demos_viewport(const viewport_t& viewport,
+ const vector_list_t& current_trajectory,
+ const matrix_list_t& demonstrations,
+ const model_t& model) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.7f, 0.7f, 0.7f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ // Draw the GMM states
+ if (!model.mu.empty()) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % 10).t()),
+ model.mu[i](span(1, model.nb_var - 1), span::all),
+ model.sigma[i](span(1, model.nb_var - 1),
+ span(1, model.nb_var - 1))
+ );
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ }
+
+ // Draw the currently created demonstration (if any)
+ if (current_trajectory.size() > 1)
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
+
+ // Draw the demonstrations
+ int color_index = 0;
+ for (size_t i = 0; i < demonstrations.size(); ++i) {
+ arma::mat datapoints = demonstrations[i](span(1, 2), span::all);
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ ++color_index;
+ if (color_index >= demonstrations.size())
+ color_index = 0;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_GMR_viewport(const viewport_t& viewport, const mat& points,
+ const std::vector<gfx2::model_t>& models) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ if (!models.empty()) {
+ for (int i = 0; i < models.size(); ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw(models[i]);
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ gfx2::draw_line(arma::fvec({0.0f, 0.0f, 0.0f}), points);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Returns the dimensions that a plot should have inside the provided viewport
+//-----------------------------------------------------------------------------
+ivec get_plot_dimensions(const viewport_t& viewport) {
+
+ const int MARGIN = 50;
+
+ ivec result(2);
+ result(0) = viewport.width - 2 * MARGIN;
+ result(1) = viewport.height - 2 * MARGIN;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "timeline" viewport
+//-----------------------------------------------------------------------------
+void draw_timeline_viewport(const gfx2::window_size_t& window_size,
+ const viewport_t& viewport,
+ const matrix_list_t& demonstrations,
+ const model_t& model,
+ const mat& GMR_points, matrix_list_t GMR_sigma,
+ unsigned int dimension) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ ivec plot_dimensions = get_plot_dimensions(viewport);
+
+ ivec plot_top_left({ -plot_dimensions(0) / 2, plot_dimensions(1) / 2 });
+ ivec plot_bottom_right({ plot_dimensions(0) / 2, -plot_dimensions(1) / 2 });
+
+ // Axis labels
+ ui::begin("Text");
+
+ vec coords = fb2ui(vec({ -20.0, double(-viewport.height / 2 + 45) }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), "t", ImVec4(0,0,0,1));
+
+ std::stringstream label;
+ label << "x" << dimension;
+
+ coords = fb2ui(vec({ double(-viewport.width / 2) + 10, -20.0 }),
+ window_size, viewport);
+ ui::text(ImVec2(coords(0), coords(1)), label.str(), ImVec4(0,0,0,1));
+
+ ui::end();
+
+ // Draw the axes
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_bottom_right(0)) },
+ { double(plot_bottom_right(1)), double(plot_bottom_right(1)) }
+ })
+ );
+
+ gfx2::draw_line(fvec({0.0f, 0.0f, 0.0f}),
+ mat({ { double(plot_top_left(0)), double(plot_top_left(0)) },
+ { double(plot_bottom_right(1)), double(plot_top_left(1)) }
+ })
+ );
+
+ // Check if there is something to display
+ if (demonstrations.empty())
+ return;
+
+ // Draw the GMR
+ double scale_x = (double) plot_dimensions(0) / demonstrations[0](0, demonstrations[0].n_cols - 1);
+ double scale_y = (double) plot_dimensions(1) / viewport.height;
+
+ mat top_vertices(2, GMR_points.n_cols);
+ mat bottom_vertices(2, GMR_points.n_cols);
+
+ for (int j = 0; j < GMR_points.n_cols; ++j) {
+ top_vertices(0, j) = demonstrations[0](0, j) * scale_x - plot_dimensions(0) / 2;
+ top_vertices(1, j) = (GMR_points(dimension - 1, j) +
+ sqrt(GMR_sigma[j](dimension - 1, dimension - 1))) * scale_y;
+
+ bottom_vertices(0, j) = top_vertices(0, j);
+ bottom_vertices(1, j) = (GMR_points(dimension - 1, j) -
+ sqrt(GMR_sigma[j](dimension - 1, dimension - 1))) * scale_y;
+ }
+
+ mat gmr_points(2, (GMR_points.n_cols - 1) * 6);
+
+ for (int j = 0; j < GMR_points.n_cols - 1; ++j) {
+ gmr_points(span::all, j * 6 + 0) = top_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 1) = bottom_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 2) = top_vertices(span::all, j + 1);
+
+ gmr_points(span::all, j * 6 + 3) = top_vertices(span::all, j + 1);
+ gmr_points(span::all, j * 6 + 4) = bottom_vertices(span::all, j);
+ gmr_points(span::all, j * 6 + 5) = bottom_vertices(span::all, j + 1);
+ }
+
+ gfx2::model_t gmr_model = gfx2::create_mesh(fvec({ 0.0f, 0.8f, 0.0f, 0.05f }), gmr_points);
+ gmr_model.use_one_minus_src_alpha_blending = true;
+ gfx2::draw(gmr_model);
+
+ // Draw the GMM states
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ vec mu = model.mu[i].elem(uvec({ 0, dimension }));
+ mu(0) = mu(0) * scale_x - plot_dimensions(0) / 2;
+ mu(1) *= scale_y;
+
+ mat sigma = model.sigma[i].submat(uvec({ 0, dimension }), uvec({ 0, dimension }));
+ sigma(0, 0) = sigma(0, 0) * (scale_x * scale_x);
+ sigma(0, 1) = sigma(0, 1) * scale_x;
+ sigma(1, 0) = sigma(1, 0) * scale_x;
+
+ gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(i % 10).t()), mu, sigma);
+ }
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ // Draw the demonstrations
+ int color_index = 0;
+ for (size_t i = 0; i < demonstrations.size(); ++i) {
+ arma::mat datapoints = demonstrations[i].rows(uvec({ 0, dimension }));
+
+ datapoints(0, span::all) = datapoints(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ datapoints(1, span::all) *= scale_y;
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ ++color_index;
+ if (color_index >= demonstrations.size())
+ color_index = 0;
+ }
+
+ // Draw the GRM result
+ mat points(2, GMR_points.n_cols);
+ points(0, span::all) = demonstrations[0](0, span::all);
+ points(1, span::all) = GMR_points(dimension - 1, span::all);
+
+ points(0, span::all) = points(0, span::all) * scale_x - plot_dimensions(0) / 2;
+ points(1, span::all) *= scale_y;
+
+ gfx2::draw_line(arma::fvec({0.0f, 0.0f, 0.0f}), points);
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+ arma_rng::set_seed_random();
+
+ // Model
+ model_t model;
+
+ // Parameters
+ model.parameters.nb_states = 6;
+ model.parameters.nb_data = 200;
+ model.parameters.dt = 0.001f;
+
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 800;
+ window_size.win_height = 800;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+ int viewport_width = 0;
+ int viewport_height = 0;
+
+
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
+
+ if (!glfwInit())
+ return -1;
+
+ glfwWindowHint(GLFW_SAMPLES, 4);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
+
+ // Open a window and create its OpenGL context
+ GLFWwindow* window = create_window_at_optimal_size(
+ "Demo Conditioning on trajectory distributions",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+
+ // Setup GLSL
+ gfx2::init();
+ glEnable(GL_SCISSOR_TEST);
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_LINE_SMOOTH);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ // Setup ImGui
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+
+
+ // Viewports
+ viewport_t viewport_demos;
+ viewport_t viewport_GMR;
+ viewport_t viewport_x1;
+ viewport_t viewport_x2;
+
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ gui_state.must_recompute_GMR = false;
+ gui_state.parameter_nb_states = model.parameters.nb_states;
+ gui_state.parameter_nb_data = model.parameters.nb_data;
+
+
+ // List of demonstrations and GMr results
+ vec time_steps;
+ matrix_list_t demos;
+ mat GMR_points;
+ matrix_list_t GMR_sigma;
+ std::vector<gfx2::model_t> GMR_models;
+
+
+ // Main loop
+ vector_list_t current_trajectory;
+ std::vector<vector_list_t> original_trajectories;
+
+ while (!glfwWindowShouldClose(window)) {
+ glfwPollEvents();
+
+ // Detect when the window was resized
+ if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
+ (ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+
+ window_size.win_width = ImGui::GetIO().DisplaySize.x;
+ window_size.win_height = ImGui::GetIO().DisplaySize.y;
+
+ glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+
+ viewport_width = window_size.fb_width / 2 - 1;
+ viewport_height = window_size.fb_height / 2 - 1;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demos, 0, window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_GMR, window_size.fb_width - viewport_width,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_x1, 0, 0, viewport_width, viewport_height);
+
+ setup_viewport(&viewport_x2, window_size.fb_width - viewport_width, 0,
+ viewport_width, viewport_height);
+ }
+
+
+ // If the parameters changed, learn the model again
+ if (gui_state.are_parameters_modified) {
+
+ if (time_steps.size() != gui_state.parameter_nb_data) {
+ demos.clear();
+
+ time_steps = linspace<vec>(
+ 0, gui_state.parameter_nb_data - 1, gui_state.parameter_nb_data
+ ) * model.parameters.dt;
+
+ for (size_t i = 0; i < original_trajectories.size(); ++i) {
+ demos.push_back(sample_trajectory(original_trajectories[i],
+ time_steps)
+ );
+ }
+ }
+
+ model.parameters.nb_data = gui_state.parameter_nb_data;
+ model.parameters.nb_states = gui_state.parameter_nb_states;
+
+ if (!demos.empty()) {
+ learn(demos, model);
+ gui_state.must_recompute_GMR = true;
+ }
+
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Recompute the GMR (if necessary)
+ if (gui_state.must_recompute_GMR) {
+
+ compute_GMR(model, time_steps, GMR_points, GMR_sigma);
+
+ // Create one big mesh for the GMR viewport (for performance reasons)
+ for (int i = 0; i < GMR_models.size(); ++i)
+ gfx2::destroy(GMR_models[i]);
+
+ GMR_models.clear();
+
+ const int NB_POINTS = 60;
+
+ mat vertices(2, NB_POINTS * 3 * GMR_sigma.size());
+ mat lines(2, NB_POINTS * 2 * GMR_sigma.size());
+
+ for (int j = 0; j < GMR_sigma.size(); ++j) {
+
+ mat v = gfx2::get_gaussian_background_vertices(GMR_points(span::all, j),
+ GMR_sigma[j], NB_POINTS);
+
+ vertices(span::all, span(j * NB_POINTS * 3, (j + 1) * NB_POINTS * 3 - 1)) = v;
+
+ mat p = gfx2::get_gaussian_border_vertices(GMR_points(span::all, j),
+ GMR_sigma[j], NB_POINTS, false);
+
+ lines(span::all, span(j * NB_POINTS * 2, (j + 1) * NB_POINTS * 2 - 1)) = p;
+ }
+
+ GMR_models.push_back(
+ gfx2::create_mesh(fvec({ 0.0f, 0.8f, 0.0f, 0.1f }), vertices)
+ );
+
+ GMR_models[0].use_one_minus_src_alpha_blending = true;
+
+ GMR_models.push_back(
+ gfx2::create_line(fvec({ 0.0f, 0.4f, 0.0f, 0.1f }), lines,
+ arma::zeros<arma::fvec>(3),
+ arma::eye<arma::fmat>(4, 4), 0, false)
+ );
+
+ gui_state.must_recompute_GMR = false;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glScissor(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT);
+
+ draw_demos_viewport(viewport_demos, current_trajectory, demos, model);
+
+ draw_GMR_viewport(viewport_GMR, GMR_points, GMR_models);
+
+ draw_timeline_viewport(window_size, viewport_x1, demos, model, GMR_points, GMR_sigma, 1);
+
+ draw_timeline_viewport(window_size, viewport_x2, demos, model, GMR_points, GMR_sigma, 2);
+
+
+ // Window: Demonstrations & model
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Demonstrations & model", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Demonstrations & model ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Clear")) {
+ original_trajectories.clear();
+ demos.clear();
+ GMR_points = mat();
+ GMR_sigma.clear();
+ GMR_models.clear();
+ model.mu.clear();
+ model.sigma.clear();
+ }
+
+ ImGui::SameLine();
+ ImGui::Text(" ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ ImGui::End();
+
+
+ // Window: GMR
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - window_size.win_width / 2, 0));
+ ImGui::Begin("GMR", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("GMR");
+
+ ImGui::End();
+
+
+ // Window: Timeline x1
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, window_size.win_height / 2));
+ ImGui::Begin("Timeline: x1", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline: x1");
+
+ ImGui::End();
+
+
+ // Window: Timeline x2
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 2, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width - window_size.win_width / 2,
+ window_size.win_height / 2));
+ ImGui::Begin("Timeline: x2", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Timeline: x2");
+
+ ImGui::End();
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(440, 106));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 440) / 2, (window_size.win_height - 106) / 2));
+ ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
+
+ if (gui_state.is_parameters_dialog_displayed)
+ ImGui::OpenPopup("Parameters");
+
+ if (ImGui::BeginPopupModal("Parameters", NULL,
+ ImGuiWindowFlags_NoResize |
+ ImGuiWindowFlags_NoSavedSettings)) {
+
+ ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 2, 20);
+ ImGui::SliderInt("Nb data", &gui_state.parameter_nb_data, 100, 300);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ if (!gui_state.is_drawing_demonstration) {
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1)) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if ((mouse_x <= window_size.win_width / 2) &&
+ (mouse_y > 36) && (mouse_y <= window_size.win_height / 2))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+
+ vec last_point = current_trajectory[current_trajectory.size() - 1];
+ vec diff = abs(coords - last_point);
+
+ if ((diff(0) > 1e-6) && (diff(1) > 1e-6))
+ current_trajectory.push_back(coords);
+
+ // Left mouse button release: end the demonstration creation
+ if (!ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ gui_state.is_drawing_demonstration = false;
+
+ if (current_trajectory.size() > 1) {
+ demos.push_back(sample_trajectory(current_trajectory, time_steps));
+
+ original_trajectories.push_back(current_trajectory);
+
+ learn(demos, model);
+
+ gui_state.must_recompute_GMR = true;
+ }
+
+ current_trajectory.clear();
+ }
+ }
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_Riemannian_pose_gmm01.cpp b/src/demo_Riemannian_pose_GMM01.cpp
similarity index 100%
rename from src/demo_Riemannian_pose_gmm01.cpp
rename to src/demo_Riemannian_pose_GMM01.cpp
diff --git a/src/demo_Riemannian_pose_tpgmm01.cpp b/src/demo_Riemannian_pose_TPGMM01.cpp
similarity index 100%
rename from src/demo_Riemannian_pose_tpgmm01.cpp
rename to src/demo_Riemannian_pose_TPGMM01.cpp
diff --git a/src/demo_Riemannian_quat_tpgmm01.cpp b/src/demo_Riemannian_quat_TPGMM01.cpp
similarity index 100%
rename from src/demo_Riemannian_quat_tpgmm01.cpp
rename to src/demo_Riemannian_quat_TPGMM01.cpp
diff --git a/src/demo_Riemannian_sphere_gmm01.cpp b/src/demo_Riemannian_sphere_GMM01.cpp
similarity index 100%
rename from src/demo_Riemannian_sphere_gmm01.cpp
rename to src/demo_Riemannian_sphere_GMM01.cpp
diff --git a/src/demo_Riemannian_sphere_tpgmm01.cpp b/src/demo_Riemannian_sphere_TPGMM01.cpp
similarity index 100%
rename from src/demo_Riemannian_sphere_tpgmm01.cpp
rename to src/demo_Riemannian_sphere_TPGMM01.cpp
diff --git a/src/demo_TPGMMProduct01.cpp b/src/demo_TPGMMProduct01.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49274e7e7a0eb005de40d009118ff51cfab7feda
--- /dev/null
+++ b/src/demo_TPGMMProduct01.cpp
@@ -0,0 +1,1543 @@
+/*
+ * demo_TPGMMProduct01.cpp
+ *
+ * Product of Gaussians for a Task-Parametrized GMM
+ *
+ * @article{Calinon16JIST,
+ * author="Calinon, S.",
+ * title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
+ * journal="Intelligent Service Robotics",
+ * publisher="Springer Berlin Heidelberg",
+ * doi="10.1007/s11370-015-0187-9",
+ * year="2016",
+ * volume="9",
+ * number="1",
+ * pages="1--29"
+ * }
+ *
+ * Authors: Sylvain Calinon, Philip Abbet, Andras Kupcsik
+ */
+
+
+#include <stdio.h>
+#include <float.h>
+#include <armadillo>
+
+#include <gfx2.h>
+#include <gfx_ui.h>
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <imgui_impl_glfw_gl2.h>
+#include <window_utils.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui_internal.h>
+
+using namespace arma;
+
+
+/***************************** ALGORITHM SECTION *****************************/
+
+typedef std::vector<vec> vector_list_t;
+typedef std::vector<mat> matrix_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the parameters used by the algorithm. Some of them are
+// modifiable through the UI, others are hard-coded.
+//-----------------------------------------------------------------------------
+struct parameters_t {
+ int nb_states; // Number of components in the GMM
+ int nb_frames; // Number of candidate frames of reference
+ int nb_deriv; // Number of static and dynamic features
+ int nb_data; // Number of datapoints in a trajectory <==== not used for product of Gaussians!!!
+ float dt; // Time step duration <==== not used for product of Gaussians!!!
+};
+
+//-----------------------------------------------------------------------------
+// Model trained using the algorithm
+//-----------------------------------------------------------------------------
+struct model_t {
+ parameters_t parameters; // Parameters used to train the model
+
+ // These lists contain one element per GMM state and per frame (access them
+ // by doing: mu[state][frame])
+ std::vector<vector_list_t> mu;
+ std::vector<matrix_list_t> sigma;
+
+ int nb_var;
+ mat pix;
+ vec priors;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a coordinate system, aka a reference frame
+//-----------------------------------------------------------------------------
+struct coordinate_system_t {
+
+ coordinate_system_t(const arma::vec& position, const arma::mat& orientation,
+ const parameters_t& parameters) {
+
+ this->position = zeros(2 + (parameters.nb_deriv - 1) * 2);
+ this->position(span(0, 1)) = position(span(0, 1));
+
+ this->orientation = kron(eye(parameters.nb_deriv, parameters.nb_deriv),
+ orientation(span(0, 1), span(0, 1)));
+ }
+
+ vec position;
+ mat orientation;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of coordinate systems
+//-----------------------------------------------------------------------------
+typedef std::vector<coordinate_system_t> coordinate_system_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the needed informations about a demonstration, like:
+// - its reference frames
+// - the trajectory originally drawn by the user
+// - the resampled trajectory
+// - the trajectory expressed in each reference frame
+//-----------------------------------------------------------------------------
+class Demonstration
+{
+public:
+ Demonstration(coordinate_system_list_t coordinate_systems,
+ const std::vector<arma::vec>& points,
+ const parameters_t& parameters)
+ : coordinate_systems(coordinate_systems)
+ {
+ points_original = mat(2, points.size());
+
+ for (size_t i = 0; i < points.size(); ++i) {
+ points_original(0, i) = points[i](0);
+ points_original(1, i) = points[i](1);
+ }
+
+ update(parameters);
+ }
+
+
+ //-------------------------------------------------------------------------
+ // Resample the trajectory and recompute it in each reference frame
+ // according to the provided parameters
+ //-------------------------------------------------------------------------
+ void update(const parameters_t& parameters)
+ {
+ // Resampling of the trajectory
+ arma::vec x = points_original.row(0).t();
+ arma::vec y = points_original.row(1).t();
+ arma::vec x2(parameters.nb_data);
+ arma::vec y2(parameters.nb_data);
+
+ arma::vec from_indices = arma::linspace<arma::vec>(0, points_original.n_cols - 1, points_original.n_cols);
+ arma::vec to_indices = arma::linspace<arma::vec>(0, points_original.n_cols - 1, parameters.nb_data);
+
+ interp1(from_indices, x, to_indices, x2, "*linear");
+ interp1(from_indices, y, to_indices, y2, "*linear");
+
+ points = mat(2 * parameters.nb_deriv, parameters.nb_data);
+ points(span(0), span::all) = x2.t();
+ points(span(1), span::all) = y2.t();
+
+ // Compute the derivatives
+ mat D = (diagmat(ones(1, parameters.nb_data - 1), -1) -
+ eye(parameters.nb_data, parameters.nb_data)) / parameters.dt;
+
+ D(parameters.nb_data - 1, parameters.nb_data - 1) = 0.0;
+
+ points(span(2, 3), span::all) = points(span(0, 1), span::all) * pow(D, 1);
+
+ // Compute the points in each coordinate system
+ points_in_coordinate_systems.clear();
+
+ for (int m = 0; m < coordinate_systems.size(); ++m) {
+ points_in_coordinate_systems.push_back(
+ pinv(coordinate_systems[m].orientation) *
+ (points - repmat(coordinate_systems[m].position, 1, parameters.nb_data))
+ );
+ }
+ }
+
+
+ //-------------------------------------------------------------------------
+ // Returns the coordinates of a point in a specific reference frame of
+ // the demonstration
+ //-------------------------------------------------------------------------
+ arma::vec convert_to_coordinate_system(const arma::vec& point, int frame) const {
+ vec original_point = zeros(points.n_rows);
+ original_point(span(0, 1)) = point(span(0, 1));
+
+ vec result = pinv(coordinate_systems[frame].orientation) *
+ (original_point - coordinate_systems[frame].position);
+
+ return result(span(0, 1));
+ }
+
+
+public:
+ coordinate_system_list_t coordinate_systems;
+ arma::mat points_original;
+ arma::mat points;
+ matrix_list_t points_in_coordinate_systems;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of demonstrations
+//-----------------------------------------------------------------------------
+typedef std::vector<Demonstration> demonstration_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Computes the factorial of a number
+//-----------------------------------------------------------------------------
+int factorial(int n) {
+ return (n == 1 || n == 0) ? 1 : factorial(n - 1) * n;
+}
+
+
+//-----------------------------------------------------------------------------
+// Return the likelihood of datapoint(s) to be generated by a Gaussian
+// parameterized by center and covariance
+//-----------------------------------------------------------------------------
+arma::vec gaussPDF(const mat& data, colvec mu, mat sigma) {
+
+ int nb_var = data.n_rows;
+ int nb_data = data.n_cols;
+
+ mat data2 = data.t() - repmat(mu.t(), nb_data, 1);
+
+ vec prob = sum((data2 * inv(sigma)) % data2, 1);
+
+ prob = exp(-0.5 * prob) / sqrt(pow((2 * datum::pi), nb_var) * det(sigma) + DBL_MIN);
+
+ return prob;
+}
+
+
+//-----------------------------------------------------------------------------
+// Initialization of Gaussian Mixture Model (GMM) parameters by clustering an
+// ordered dataset into equal bins
+//-----------------------------------------------------------------------------
+void init_tensorGMM_kbins(const demonstration_list_t& demos,
+ model_t &model) {
+
+ model.priors.resize(model.parameters.nb_states);
+ model.mu.clear();
+ model.sigma.clear();
+
+ model.nb_var = demos[0].points_in_coordinate_systems[0].n_rows;
+
+
+ // Initialize bins
+ uvec t_sep = linspace<uvec>(0, model.parameters.nb_data - 1,
+ model.parameters.nb_states + 1);
+
+ struct bin_t {
+ mat data;
+ vec mu;
+ mat sigma;
+ };
+
+ std::vector<bin_t> bins;
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ bin_t bin;
+ bin.data = zeros(model.nb_var * model.parameters.nb_frames,
+ demos.size() * (t_sep(i + 1) - t_sep(i) + 1));
+
+ bins.push_back(bin);
+ }
+
+
+ // Split each demonstration in K equal bins
+ for (int n = 0; n < demos.size(); ++n) {
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ int bin_size = t_sep(i + 1) - t_sep(i) + 1;
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ bins[i].data(span(m * model.nb_var, (m + 1) * model.nb_var - 1),
+ span(n * bin_size, (n + 1) * bin_size - 1)) =
+ demos[n].points_in_coordinate_systems[m](span::all, span(t_sep(i), t_sep(i + 1)));
+ }
+ }
+ }
+
+
+ // Calculate statistics on bin data
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ bins[i].mu = mean(bins[i].data, 1);
+ bins[i].sigma = cov(bins[i].data.t());
+ model.priors(i) = bins[i].data.n_elem;
+ }
+
+
+ // Reshape GMM into a tensor
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ model.mu.push_back(vector_list_t());
+ model.sigma.push_back(matrix_list_t());
+ }
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ model.mu[i].push_back(bins[i].mu(span(m * model.nb_var, (m + 1) * model.nb_var - 1)));
+
+ model.sigma[i].push_back(bins[i].sigma(span(m * model.nb_var, (m + 1) * model.nb_var - 1),
+ span(m * model.nb_var, (m + 1) * model.nb_var - 1)));
+ }
+ }
+
+ model.priors /= sum(model.priors);
+}
+
+
+//-----------------------------------------------------------------------------
+// Training of a task-parameterized Gaussian mixture model (GMM) with an
+// expectation-maximization (EM) algorithm.
+//
+// The approach allows the modulation of the centers and covariance matrices of
+// the Gaussians with respect to external parameters represented in the form of
+// candidate coordinate systems.
+//-----------------------------------------------------------------------------
+void train_EM_tensorGMM(const demonstration_list_t& demos,
+ model_t &model) {
+
+ const int nb_max_steps = 100; // Maximum number of iterations allowed
+ const int nb_min_steps = 5; // Minimum number of iterations allowed
+ const double max_diff_log_likelihood = 1e-5; // Likelihood increase threshold
+ // to stop the algorithm
+ const double diag_reg_fact = 1e-2; // Regularization term is optional
+
+
+ cube data(model.nb_var, model.parameters.nb_frames,
+ demos.size() * model.parameters.nb_data);
+
+ for (int n = 0; n < demos.size(); ++n) {
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+ data(span::all, span(m), span(n * model.parameters.nb_data,
+ (n + 1) * model.parameters.nb_data - 1)) =
+ demos[n].points_in_coordinate_systems[m];
+ }
+ }
+
+
+ std::vector<double> log_likelihoods;
+
+ for (int iter = 0; iter < nb_max_steps; ++iter) {
+
+ // E-step
+ mat L = ones(model.parameters.nb_states, data.n_slices);
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+
+ // Matricization/flattening of tensor
+ mat data_mat(data(span::all, span(m), span::all));
+
+ vec gamma0 = gaussPDF(data_mat, model.mu[i][m], model.sigma[i][m]);
+
+ L(i, span::all) = L(i, span::all) % gamma0.t();
+ }
+
+ L(i, span::all) = L(i, span::all) * model.priors(i);
+ }
+
+ mat gamma = L / repmat(sum(L, 0) + DBL_MIN, L.n_rows, 1);
+ mat gamma2 = gamma / repmat(sum(gamma, 1), 1, data.n_slices);
+
+ model.pix = gamma2;
+
+
+ // M-step
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+
+ // Update priors
+ model.priors(i) = sum(gamma(i, span::all)) / data.n_slices;
+
+ for (int m = 0; m < model.parameters.nb_frames; ++m) {
+
+ // Matricization/flattening of tensor
+ mat data_mat(data(span::all, span(m), span::all));
+
+ // Update mu
+ model.mu[i][m] = data_mat * gamma2(i, span::all).t();
+
+ // Update sigma
+ mat data_tmp = data_mat - repmat(model.mu[i][m], 1, data.n_slices);
+ model.sigma[i][m] = data_tmp * diagmat(gamma2(i, span::all)) * data_tmp.t() +
+ eye(data_tmp.n_rows, data_tmp.n_rows) * diag_reg_fact;
+ }
+ }
+
+ // Compute average log-likelihood
+ log_likelihoods.push_back(vec(sum(log(sum(L, 0)), 1))[0] / data.n_slices);
+
+ // Stop the algorithm if EM converged (small change of log-likelihood)
+ if (iter >= nb_min_steps) {
+ if (log_likelihoods[iter] - log_likelihoods[iter - 1] < max_diff_log_likelihood)
+ break;
+ }
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Training of the model
+//-----------------------------------------------------------------------------
+void learn(const demonstration_list_t& demos, model_t &model) {
+
+ init_tensorGMM_kbins(demos, model);
+ train_EM_tensorGMM(demos, model);
+
+}
+
+
+/*************************** DEMONSTRATION SECTION ***************************/
+
+static void error_callback(int error, const char* description) {
+ fprintf(stderr, "Error %d: %s\n", error, description);
+}
+
+
+//-----------------------------------------------------------------------------
+// Contains all the informations about a viewport
+//-----------------------------------------------------------------------------
+struct viewport_t {
+ int x;
+ int y;
+ int width;
+ int height;
+
+ // Projection matrix parameters
+ arma::vec projection_top_left;
+ arma::vec projection_bottom_right;
+ double projection_near;
+ double projection_far;
+
+ // View matrix parameters
+ arma::fvec view;
+};
+
+
+//-----------------------------------------------------------------------------
+// Helper function to setup a viewport
+//-----------------------------------------------------------------------------
+void setup_viewport(viewport_t* viewport, int x, int y, int width, int height,
+ double near = -1.0, double far = 1.0,
+ const fvec& view_transforms = zeros<fvec>(3)) {
+
+ viewport->x = x;
+ viewport->y = y;
+ viewport->width = width;
+ viewport->height = height;
+ viewport->projection_top_left = vec({ (double) -width / 2,
+ (double) height / 2 });
+ viewport->projection_bottom_right = vec({ (double) width / 2,
+ (double) -height / 2 });
+ viewport->projection_near = near;
+ viewport->projection_far = far;
+ viewport->view = view_transforms;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from UI-space to OpenGL-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec ui2fb(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // ui -> viewport
+ result(0) = coords(0) * (float) window_size.fb_width / (float) window_size.win_width - viewport.x;
+ result(1) = (window_size.win_height - coords(1)) *
+ (float) window_size.fb_height / (float) window_size.win_height - viewport.y;
+
+ // viewport -> fb
+ result(0) = result(0) - (float) viewport.width * 0.5f;
+ result(1) = result(1) - (float) viewport.height * 0.5f;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Converts some coordinates from OpenGL-space to UI-space, taking the
+// coordinates of a viewport into account
+//-----------------------------------------------------------------------------
+arma::vec fb2ui(const arma::vec& coords, const gfx2::window_size_t& window_size,
+ const viewport_t& viewport) {
+ arma::vec result = coords;
+
+ // fb -> viewport
+ result(0) = result(0) + (float) viewport.width * 0.5f;
+ result(1) = result(1) + (float) viewport.height * 0.5f;
+
+ // viewport -> ui
+ result(0) = (result(0) + viewport.x) * (float) window_size.win_width /
+ (float) window_size.fb_width;
+
+ result(1) = window_size.win_height - (result(1) + viewport.y) *
+ (float) window_size.win_height / (float) window_size.fb_height;
+
+ return result;
+}
+
+
+//-----------------------------------------------------------------------------
+// Colors used by the movable handlers
+//-----------------------------------------------------------------------------
+const arma::fmat HANDLER_COLORS({
+ { 0.0, 0.0, 0.0 },
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ { 0.0, 0.75, 0.75 },
+});
+
+
+//-----------------------------------------------------------------------------
+// Colors used by the fixed handlers
+//-----------------------------------------------------------------------------
+const arma::fmat HANDLER_FIXED_COLORS({
+ { 0.4, 0.4, 0.4 },
+ { 0.4, 0.4, 1.0 },
+ { 0.2, 0.5, 0.2 },
+ { 1.0, 0.4, 0.4 },
+ { 0.3, 0.75, 0.75 },
+});
+
+
+//-----------------------------------------------------------------------------
+// Colors of the displayed lines and gaussians
+//-----------------------------------------------------------------------------
+const arma::mat COLORS({
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+ { 0.0, 0.75, 0.75 },
+ { 0.75, 0.0, 0.75 },
+ { 0.75, 0.75, 0.0 },
+ { 0.25, 0.25, 0.25 },
+ { 0.0, 0.0, 1.0 },
+ { 0.0, 0.5, 0.0 },
+ { 1.0, 0.0, 0.0 },
+});
+
+
+//-----------------------------------------------------------------------------
+// An user-movable UI widget representing a coordinate system
+//
+// Can be "fixed", so the user can't move it anymore
+//-----------------------------------------------------------------------------
+class Handler
+{
+public:
+ Handler(const viewport_t* viewport, const ImVec2& position, const ImVec2& y,
+ int index)
+ : viewport(viewport), hovered(false), fixed(false), moved(false), index(index)
+ {
+ ui_position = position;
+ ui_y = y;
+
+ fvec color = HANDLER_COLORS.row(index).t();
+
+ models[0] = gfx2::create_rectangle(color, 25.0f, 5.0f);
+ models[0].transforms.parent = &transforms;
+ models[0].transforms.rotation = gfx2::rotate(arma::fvec({0.0f, 0.0f, 1.0f}),
+ gfx2::deg2rad(90.0f));
+
+ models[1] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[1].transforms.parent = &transforms;
+ models[1].transforms.position(0) = 30.0f;
+ models[1].transforms.position(1) = -10.0f;
+
+ models[2] = gfx2::create_rectangle(color, 60.0f, 5.0f);
+ models[2].transforms.parent = &transforms;
+ models[2].transforms.position(0) = 30.0f;
+ models[2].transforms.position(1) = 10.0f;
+ }
+
+
+ void update(const gfx2::window_size_t& window_size)
+ {
+ transforms.position.rows(0, 1) = arma::conv_to<arma::fvec>::from(
+ ui2fb(ui_position, window_size, *viewport)
+ );
+
+ arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
+
+ transforms.rotation(0, 0) = -delta(0);
+ transforms.rotation(1, 0) = delta(1);
+ transforms.rotation(0, 1) = -delta(1);
+ transforms.rotation(1, 1) = -delta(0);
+ }
+
+
+ void update()
+ {
+ transforms.position(0) = ui_position.x;
+ transforms.position(1) = ui_position.y;
+
+ arma::vec delta = ui_y / arma::norm(arma::vec(ui_y));
+
+ transforms.rotation(0, 0) = -delta(0);
+ transforms.rotation(1, 0) = delta(1);
+ transforms.rotation(0, 1) = -delta(1);
+ transforms.rotation(1, 1) = -delta(0);
+ }
+
+
+ void viewport_resized(const gfx2::window_size_t& window_size)
+ {
+ ui_position = fb2ui(arma::conv_to<arma::vec>::from(transforms.position.rows(0, 1)),
+ window_size, *viewport);
+ }
+
+
+ void fix()
+ {
+ fixed = true;
+
+ for (int i = 0; i < 3; ++i)
+ models[i].diffuse_color = HANDLER_FIXED_COLORS.row(index).t();
+ }
+
+
+ bool draw(const gfx2::window_size_t& window_size)
+ {
+ if (!fixed)
+ {
+ std::stringstream id;
+ id << "handler" << (uint64_t) this;
+
+ ImGuiWindow* window = ImGui::GetCurrentWindow();
+ if (window->SkipItems)
+ return false;
+
+ ImGui::PushID(id.str().c_str());
+
+ // Position
+ ImVec2 current_position = ui_position;
+
+ ui_position = ui::dragger(0, ui_position, false, ui::config.draggerSize );
+
+ moved = moved || (ui_position.x != current_position.x) || (ui_position.y != current_position.y);
+
+ hovered = ImGui::IsItemHovered();
+
+ // y-axis
+ ImVec2 py = ui_position + ui_y;
+ current_position = ui_y;
+
+ py = ui::dragger(1, py, false, ui::config.draggerSize * 0.7);
+ ui_y = py - ui_position;
+
+ moved = moved || ((ui_y.x != current_position.x) || (ui_y.y != current_position.y));
+
+ hovered = ImGui::IsItemHovered() || hovered;
+
+ window->DrawList->AddLine(ui_position, py, ui::config.lineColor);
+
+ ImGui::PopID();
+
+ update(window_size);
+ }
+
+ return hovered;
+ }
+
+
+ void draw_anchor() const
+ {
+ for (int i = 0; i < 3; ++i)
+ gfx2::draw(models[i]);
+ }
+
+
+public:
+ const viewport_t* viewport;
+ int index;
+
+ ImVec2 ui_position;
+ ImVec2 ui_y;
+
+ gfx2::transforms_t transforms;
+ gfx2::model_t models[3];
+
+ bool hovered;
+ bool fixed;
+ bool moved;
+};
+
+
+//-----------------------------------------------------------------------------
+// Represents a list of handlers
+//-----------------------------------------------------------------------------
+typedef std::vector<Handler*> handler_list_t;
+
+
+//-----------------------------------------------------------------------------
+// Contains all the needed infos about the state of the application (values of
+// the parameters modifiable via the UI, which action the user is currently
+// doing, ...)
+//-----------------------------------------------------------------------------
+struct gui_state_t {
+ // Indicates if the user can draw a new demonstration
+ bool can_draw_demonstration;
+
+ // Indicates if the user is currently drawing a new demonstration
+ bool is_drawing_demonstration;
+
+ // Indicates if the parameters dialog is displayed
+ bool is_parameters_dialog_displayed;
+
+ // Indicates if the parameters were modified through the UI
+ bool are_parameters_modified;
+
+ // Parameters modifiable via the UI (they correspond to the ones declared
+ //in parameters_t)
+ int parameter_nb_states;
+ int parameter_nb_frames;
+ int parameter_nb_data;
+};
+
+
+//-----------------------------------------------------------------------------
+// Create a handler at a random position (within the given boundaries)
+//-----------------------------------------------------------------------------
+Handler* create_random_handler(const viewport_t* viewport, int index,
+ int min_x, int min_y, int max_x, int max_y) {
+ return new Handler(viewport,
+ ImVec2((randu() * 0.8f + 0.1f) * (max_x - min_x) + min_x,
+ (randu() * 0.5f + 0.1f) * (max_y - min_y) + min_y),
+ ImVec2((randu() - 0.5) * 10, randu() * -10 - 10),
+ index);
+}
+
+
+//-----------------------------------------------------------------------------
+// Create handlers to be used for a new demonstration at random positions
+//-----------------------------------------------------------------------------
+void create_new_demonstration_handlers(const viewport_t& viewport,
+ const gfx2::window_size_t& window_size,
+ int nb_frames,
+ handler_list_t &handlers) {
+
+ for (size_t i = 0; i < handlers.size(); ++i)
+ delete handlers[i];
+
+ handlers.clear();
+
+ handlers.push_back(
+ new Handler(&viewport, ImVec2(window_size.win_width / 6,
+ window_size.win_height - 50),
+ ImVec2(0, 30), 0)
+ );
+
+ for (int n = 1; n < nb_frames; ++n) {
+ handlers.push_back(
+ create_random_handler(&viewport, n, 10, 20,
+ window_size.win_width / 3 - 10,
+ window_size.win_height / 2 - 20)
+ );
+ };
+}
+
+
+//-----------------------------------------------------------------------------
+// Create handlers to be used for a new reproduction at random positions
+//-----------------------------------------------------------------------------
+void create_reproduction_handlers(const viewport_t& viewport,
+ const gfx2::window_size_t& window_size,
+ int nb_frames,
+ handler_list_t &handlers) {
+
+ for (size_t i = 0; i < handlers.size(); ++i)
+ delete handlers[i];
+
+ handlers.clear();
+
+ handlers.push_back(
+ new Handler(&viewport, ImVec2(window_size.win_width / 2,
+ window_size.win_height - 50),
+ ImVec2(0, 30), 0)
+ );
+
+ for (int n = 1; n < nb_frames; ++n) {
+ handlers.push_back(
+ create_random_handler(&viewport, n,
+ window_size.win_width / 3 + 20,
+ window_size.win_height / 2 + 20,
+ window_size.win_width * 2 / 3 - 20,
+ window_size.win_height - 20)
+ );
+ };
+}
+
+
+//-----------------------------------------------------------------------------
+// Extract a list of coordinate systems from a list of handlers
+//-----------------------------------------------------------------------------
+void convert(const handler_list_t& from, coordinate_system_list_t &to,
+ const parameters_t& parameters) {
+
+ to.clear();
+
+ for (int n = 0; n < from.size(); ++n) {
+ to.push_back(
+ coordinate_system_t(arma::conv_to<arma::vec>::from(from[n]->transforms.position),
+ arma::conv_to<arma::mat>::from(from[n]->transforms.rotation),
+ parameters)
+ );
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Extract a list of coordinate systems from a list of demonstrations
+//-----------------------------------------------------------------------------
+void convert(const demonstration_list_t& from, std::vector<coordinate_system_list_t> &to) {
+
+ to.clear();
+
+ for (int n = 0; n < from.size(); ++n) {
+ to.push_back(from[n].coordinate_systems);
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Compute a view matrix centered on the given reference frame across all the
+// demonstrations
+//-----------------------------------------------------------------------------
+arma::fvec compute_centered_view_matrix(const demonstration_list_t& demonstrations,
+ int frame) {
+ vec top_left({-30, 0});
+ vec bottom_right({0, 0});
+
+ for (auto iter = demonstrations.begin(); iter != demonstrations.end(); ++iter) {
+ top_left(0) = fmin(top_left(0), iter->points_in_coordinate_systems[frame](0, span::all).min());
+ top_left(1) = fmax(top_left(1), iter->points_in_coordinate_systems[frame](1, span::all).max());
+
+ bottom_right(0) = fmax(bottom_right(0), iter->points_in_coordinate_systems[frame](0, span::all).max());
+ bottom_right(1) = fmin(bottom_right(1), iter->points_in_coordinate_systems[frame](1, span::all).min());
+ }
+
+ vec center = (bottom_right - top_left) / 2 + top_left;
+
+ return fvec({ (float) -center(0), (float) -center(1), 0.0f });
+}
+
+
+//-----------------------------------------------------------------------------
+// Render the "demonstrations" viewport
+//-----------------------------------------------------------------------------
+void draw_demos_viewport(const viewport_t& viewport,
+ const std::vector<arma::vec>& current_trajectory,
+ const demonstration_list_t& demonstrations,
+ const std::vector<handler_list_t> fixed_demonstration_handlers,
+ const handler_list_t current_demonstration_handlers) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.7f, 0.7f, 0.7f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+ // Draw the currently created demonstration (if any)
+ if (current_trajectory.size() > 1)
+ gfx2::draw_line(arma::fvec({0.33f, 0.97f, 0.33f}), current_trajectory);
+
+ // Draw the demonstrations
+ int color_index = 0;
+ for (auto iter = demonstrations.begin(); iter != demonstrations.end(); ++iter) {
+ arma::mat datapoints = iter->points(span(0, 1), span::all);
+
+ arma::fvec color = arma::conv_to<arma::fvec>::from(COLORS.row(color_index));
+
+ gfx2::draw_line(color, datapoints);
+
+ ++color_index;
+ if (color_index >= demonstrations.size())
+ color_index = 0;
+ }
+
+ // Draw the handlers
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ fixed_demonstration_handlers[n][i]->draw_anchor();
+ }
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->draw_anchor();
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_reproductions_viewport(const viewport_t& viewport,
+ const std::vector<handler_list_t>& handlers,
+ const matrix_list_t& reproductions) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+ // Draw the handlers
+ for (size_t n = 0; n < handlers.size(); ++n) {
+ for (size_t i = 0; i < handlers[n].size(); ++i)
+ handlers[n][i]->draw_anchor();
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "reproduction" viewport
+//-----------------------------------------------------------------------------
+void draw_reproductions_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers,
+ const matrix_list_t& reproductions) {
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ draw_reproductions_viewport(viewport, handler_list, reproductions);
+}
+
+
+//-----------------------------------------------------------------------------
+// Render a "GMMs" viewport
+//-----------------------------------------------------------------------------
+void draw_GMMs_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers, model_t model) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ // Draw the handlers
+ for (size_t n = 0; n < handler_list.size(); ++n) {
+ for (size_t i = 0; i < handler_list[n].size(); ++i)
+ handler_list[n][i]->draw_anchor();
+ }
+
+ // Draw the GMM states
+ if (model.mu.size() > 0 ) {
+ for (int f = 0; f < model.parameters.nb_frames; f++) {
+
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(COLORS.row(f).t()),
+ handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.mu[i][f].subvec(0, 1) + handler_list[0][f]->transforms.position.subvec(0, 1),
+ handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.sigma[i][f].submat(0, 0, 1, 1) * handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t());
+ }
+ }
+ }
+
+
+}
+
+//-----------------------------------------------------------------------------
+// Render a "GMMs" viewport
+//-----------------------------------------------------------------------------
+void draw_product_viewport(const viewport_t& viewport,
+ const handler_list_t& handlers, model_t model) {
+
+ glViewport(viewport.x, viewport.y, viewport.width, viewport.height);
+ glScissor(viewport.x, viewport.y, viewport.width, viewport.height);
+ glClearColor(0.9f, 0.9f, 0.9f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(viewport.projection_top_left(0), viewport.projection_bottom_right(0),
+ viewport.projection_bottom_right(1), viewport.projection_top_left(1),
+ viewport.projection_near, viewport.projection_far);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(viewport.view(0), viewport.view(1), viewport.view(2));
+
+
+ std::vector<handler_list_t> handler_list;
+ handler_list.push_back(handlers);
+
+ // Draw the handlers
+ for (size_t n = 0; n < handler_list.size(); ++n) {
+ for (size_t i = 0; i < handler_list[n].size(); ++i)
+ handler_list[n][i]->draw_anchor();
+ }
+
+ mat black = {0.0, 0.0, 0.0};
+
+ // Draw the GMM states
+ if (model.mu.size() > 0 ) {
+ for (int i = 0; i < model.parameters.nb_states; ++i) {
+ vec mu = zeros(2, 1);
+ mat cov = zeros(2, 2);
+ for (int f = 0; f < model.parameters.nb_frames; f++) {
+ vec mutmp = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.mu[i][f].subvec(0, 1) + handler_list[0][f]->transforms.position.subvec(0, 1);
+ mat covtmp = handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1) * model.sigma[i][f].submat(0, 0, 1, 1) * handler_list[0][f]->transforms.rotation.submat(0, 0, 1, 1).t();
+ mat icovtmp = arma::inv(covtmp);
+ cov = cov + icovtmp;
+ mu = mu + icovtmp * mutmp;
+
+ }
+
+ cov = arma::inv(cov);
+ mu = cov * mu;
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+ gfx2::draw_gaussian(conv_to<fvec>::from(black.row(0).t()), mu, cov);
+ }
+ }
+
+
+}
+
+
+/******************************* MAIN FUNCTION *******************************/
+
+int main(int argc, char **argv) {
+ arma_rng::set_seed_random();
+
+ // Model
+ model_t model;
+
+ // Parameters
+ model.parameters.nb_states = 4;
+ model.parameters.nb_frames = 2;
+ model.parameters.nb_deriv = 2;
+ model.parameters.nb_data = 200;
+ model.parameters.dt = 0.1f;
+
+ // Take 4k screens into account (framebuffer size != window size)
+ gfx2::window_size_t window_size;
+ window_size.win_width = 1200;
+ window_size.win_height = 400;
+ window_size.fb_width = -1; // Will be known later
+ window_size.fb_height = -1;
+ int viewport_width = 0;
+ int viewport_height = 0;
+
+
+ // Initialise GLFW
+ glfwSetErrorCallback(error_callback);
+
+ if (!glfwInit())
+ return -1;
+
+ glfwWindowHint(GLFW_SAMPLES, 4);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
+ glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
+
+ // Open a window and create its OpenGL context
+ GLFWwindow* window = create_window_at_optimal_size(
+ "Demo - Gaussian Product for TPGMM",
+ window_size.win_width, window_size.win_height
+ );
+
+ glfwMakeContextCurrent(window);
+
+ // Setup GLSL
+ gfx2::init();
+ glEnable(GL_SCISSOR_TEST);
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+ glEnable(GL_LINE_SMOOTH);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ // Setup ImGui
+ ImGui::CreateContext();
+ ImGui_ImplGlfwGL2_Init(window, true);
+
+
+ // Viewports
+ viewport_t viewport_demos;
+ viewport_t viewport_product;
+ viewport_t viewport_GMMs;
+
+ // GUI state
+ gui_state_t gui_state;
+ gui_state.can_draw_demonstration = true;
+ gui_state.is_drawing_demonstration = false;
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = false;
+ gui_state.parameter_nb_data = model.parameters.nb_data;
+ gui_state.parameter_nb_states = model.parameters.nb_states;
+ gui_state.parameter_nb_frames = model.parameters.nb_frames;
+
+
+ // Initial handlers
+ std::vector<handler_list_t> fixed_demonstration_handlers;
+ handler_list_t current_demonstration_handlers;
+ handler_list_t reproduction_handlers;
+
+ // create_new_demonstration_handlers(viewport_demos, window_size,
+ // model.parameters.nb_frames,
+ // current_demonstration_handlers
+ // );
+ //
+ // create_reproduction_handlers(viewport_product, window_size,
+ // model.parameters.nb_frames,
+ // reproduction_handlers);
+
+
+ // List of demonstrations and reproductions
+ demonstration_list_t demos;
+
+ // Main loop
+ std::vector<arma::vec> current_trajectory;
+
+ while (!glfwWindowShouldClose(window)) {
+ glfwPollEvents();
+
+ // Detect when the window was resized
+ if ((ImGui::GetIO().DisplaySize.x != window_size.win_width) ||
+ (ImGui::GetIO().DisplaySize.y != window_size.win_height)) {
+
+ bool first = (window_size.win_width == -1) || (window_size.fb_width == -1);
+
+ window_size.win_width = ImGui::GetIO().DisplaySize.x;
+ window_size.win_height = ImGui::GetIO().DisplaySize.y;
+
+ glfwGetFramebufferSize(window, &window_size.fb_width, &window_size.fb_height);
+
+ viewport_width = window_size.fb_width / 3 - 1;
+ viewport_height = window_size.fb_height;
+
+ // Update all the viewports
+ setup_viewport(&viewport_demos, 0, window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_product, viewport_width + 2,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+ setup_viewport(&viewport_GMMs, window_size.fb_width - viewport_width,
+ window_size.fb_height - viewport_height,
+ viewport_width, viewport_height);
+
+
+ // Update all the handlers
+ if (!first) {
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->viewport_resized(window_size);
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i)
+ reproduction_handlers[i]->viewport_resized(window_size);
+ }
+
+ // At the very first frame: load initial data from files (can't be done
+ // outside the loop because we need to know the size of the OpenGL front
+ // buffer)
+ //
+ // Note: The loaded data was recorded in another demo with 3x2 viewports,
+ // so appropriate scaling must be applied
+ else if ((window_size.win_width != -1) && (window_size.fb_width != -1)) {
+ cube loaded_trajectories;
+ mat loaded_frames;
+ loaded_trajectories.load("data/data_tpbatch_lqr_trajectories.txt");
+ loaded_frames.load("data/data_tpbatch_lqr_frames.txt");
+
+ for (int n = 0; n < loaded_frames.n_cols / 2; ++n) {
+ Handler* handler1 = new Handler(
+ &viewport_demos,
+ ImVec2(loaded_frames(0, n * 2) * window_size.win_width,
+ loaded_frames(1, n * 2) * window_size.win_height * 2),
+ ImVec2(loaded_frames(2, n * 2) * window_size.win_width,
+ loaded_frames(3, n * 2) * window_size.win_height * 2),
+ 0
+ );
+ handler1->update(window_size);
+ handler1->fix();
+
+ Handler* handler2 = new Handler(
+ &viewport_demos,
+ ImVec2(loaded_frames(0, n * 2 + 1) * window_size.win_width,
+ loaded_frames(1, n * 2 + 1) * window_size.win_height * 2),
+ ImVec2(loaded_frames(2, n * 2 + 1) * window_size.win_width,
+ loaded_frames(3, n * 2 + 1) * window_size.win_height * 2),
+ 1
+ );
+ handler2->update(window_size);
+ handler2->fix();
+
+ handler_list_t handlers;
+ handlers.push_back(handler1);
+ handlers.push_back(handler2);
+
+ fixed_demonstration_handlers.push_back(handlers);
+ }
+
+ Handler* handler_reproduction_1 = new Handler(
+ &viewport_product,
+ fixed_demonstration_handlers[0][0]->ui_position +
+ ImVec2(window_size.win_width / 3, 0),
+ fixed_demonstration_handlers[0][0]->ui_y,
+ 0
+ );
+ handler_reproduction_1->update(window_size);
+ reproduction_handlers.push_back(handler_reproduction_1);
+
+ Handler* handler_reproduction_2 = new Handler(
+ &viewport_product,
+ fixed_demonstration_handlers[0][1]->ui_position +
+ ImVec2(window_size.win_width / 3, 0),
+ fixed_demonstration_handlers[0][1]->ui_y,
+ 1
+ );
+ handler_reproduction_2->update(window_size);
+ reproduction_handlers.push_back(handler_reproduction_2);
+
+ for (int n = 0; n < loaded_trajectories.n_slices; ++n) {
+ vector_list_t trajectory;
+ for (int i = 0; i < loaded_trajectories.n_cols; ++i) {
+ mat t = loaded_trajectories(span::all, span(i), span(n));
+ t(0, span::all) *= window_size.fb_width;
+ t(1, span::all) *= window_size.fb_height * 2;
+ trajectory.push_back(t);
+ }
+
+ coordinate_system_list_t coordinate_systems;
+ convert(fixed_demonstration_handlers[n], coordinate_systems, model.parameters);
+
+ Demonstration demonstration(coordinate_systems, trajectory, model.parameters);
+ demos.push_back(demonstration);
+ }
+
+ // Initial learning from the loaded data
+ learn(demos, model);
+ }
+ }
+
+
+ // If the parameters changed, recompute
+ if (gui_state.are_parameters_modified) {
+
+ // If the number of frames changed, clear everything
+ if (model.parameters.nb_frames != gui_state.parameter_nb_frames) {
+ demos.clear();
+ model.mu.clear();
+ model.sigma.clear();
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ delete current_demonstration_handlers[i];
+
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ delete fixed_demonstration_handlers[n][i];
+ }
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i)
+ delete reproduction_handlers[i];
+
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
+ reproduction_handlers.clear();
+
+
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ gui_state.parameter_nb_frames,
+ current_demonstration_handlers
+ );
+
+ create_reproduction_handlers(viewport_product, window_size,
+ gui_state.parameter_nb_frames,
+ reproduction_handlers);
+
+ model.parameters.nb_frames = gui_state.parameter_nb_frames;
+
+ gui_state.can_draw_demonstration = true;
+ }
+
+ // If the number of states changed, recompute the model
+ if (model.parameters.nb_states != gui_state.parameter_nb_states) {
+
+ model.parameters.nb_states = gui_state.parameter_nb_states;
+
+ for (auto iter = demos.begin(); iter != demos.end(); ++iter)
+ iter->update(model.parameters);
+
+ if (!demos.empty()) {
+ learn(demos, model);
+ }
+ }
+
+ gui_state.are_parameters_modified = false;
+ }
+
+
+ // Start the rendering
+ ImGui_ImplGlfwGL2_NewFrame();
+
+ glViewport(0, 0, window_size.fb_width, window_size.fb_height);
+ glScissor(0, 0, window_size.fb_width, window_size.fb_height);
+ glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT);
+
+ draw_demos_viewport(viewport_demos, current_trajectory, demos,
+ fixed_demonstration_handlers,
+ current_demonstration_handlers);
+
+ draw_product_viewport(viewport_product, reproduction_handlers, model);
+
+ draw_GMMs_viewport(viewport_GMMs, reproduction_handlers, model);
+
+
+ // Draw the UI controls of the handlers
+ ui::begin("handlers");
+
+ bool hovering_ui = false;
+
+ for (size_t i = 0; i < reproduction_handlers.size(); ++i) {
+ hovering_ui = reproduction_handlers[i]->draw(window_size) || hovering_ui;
+ }
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ hovering_ui = current_demonstration_handlers[i]->draw(window_size) || hovering_ui;
+
+ ui::end();
+
+
+ // Window: Demonstrations
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(0, 0));
+ ImGui::Begin("Demonstrations", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Demonstrations ");
+ ImGui::SameLine();
+
+ if (!gui_state.can_draw_demonstration) {
+ if (ImGui::Button("Add")) {
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ gui_state.parameter_nb_frames,
+ current_demonstration_handlers
+ );
+
+ gui_state.can_draw_demonstration = true;
+ }
+ }
+
+ ImGui::SameLine();
+
+ if (ImGui::Button("Clear")) {
+ demos.clear();
+ model.mu.clear();
+ model.sigma.clear();
+
+ for (size_t i = 0; i < current_demonstration_handlers.size(); ++i)
+ delete current_demonstration_handlers[i];
+
+ for (size_t n = 0; n < fixed_demonstration_handlers.size(); ++n) {
+ for (size_t i = 0; i < fixed_demonstration_handlers[n].size(); ++i)
+ delete fixed_demonstration_handlers[n][i];
+ }
+
+ current_demonstration_handlers.clear();
+ fixed_demonstration_handlers.clear();
+
+ create_new_demonstration_handlers(viewport_demos, window_size,
+ model.parameters.nb_frames,
+ current_demonstration_handlers
+ );
+
+ gui_state.can_draw_demonstration = true;
+ }
+
+ ImGui::SameLine();
+ ImGui::Text(" ");
+ ImGui::SameLine();
+
+ if (ImGui::Button("Parameters"))
+ gui_state.is_parameters_dialog_displayed = true;
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+ ImGui::End();
+
+ // Window: Product
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width / 3, 0));
+ ImGui::Begin("Product", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("Product");
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+ ImGui::End();
+
+ // Window: GMMs in global coordinates
+ ImGui::SetNextWindowSize(ImVec2(window_size.win_width / 3, 36));
+ ImGui::SetNextWindowPos(ImVec2(window_size.win_width * 2 / 3, 0));
+ ImGui::Begin("GMMs", NULL,
+ ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoSavedSettings |
+ ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse |
+ ImGuiWindowFlags_NoTitleBar
+ );
+
+ ImGui::Text("GMMs");
+
+ hovering_ui = ImGui::IsWindowHovered() || hovering_ui;
+
+ ImGui::End();
+
+
+ // Window: Parameters
+ ImGui::SetNextWindowSize(ImVec2(600, 100));
+ ImGui::SetNextWindowPos(ImVec2((window_size.win_width - 600) / 2, (window_size.win_height - 100) / 2));
+ ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0, 0, 0, 255));
+
+ if (gui_state.is_parameters_dialog_displayed)
+ ImGui::OpenPopup("Parameters");
+
+ if (ImGui::BeginPopupModal("Parameters", NULL,
+ ImGuiWindowFlags_NoResize |
+ ImGuiWindowFlags_NoSavedSettings)) {
+
+ ImGui::SliderInt("Nb states", &gui_state.parameter_nb_states, 1, 10);
+ ImGui::SliderInt("Nb frames", &gui_state.parameter_nb_frames, 2, 5);
+
+ if (ImGui::Button("Close")) {
+ ImGui::CloseCurrentPopup();
+ gui_state.is_parameters_dialog_displayed = false;
+ gui_state.are_parameters_modified = true;
+ }
+
+ ImGui::EndPopup();
+
+ hovering_ui = true;
+ }
+
+ ImGui::PopStyleColor();
+
+
+ // GUI rendering
+ ImGui::Render();
+ ImGui_ImplGlfwGL2_RenderDrawData(ImGui::GetDrawData());
+
+ // Swap buffers
+ glfwSwapBuffers(window);
+
+ // Keyboard input
+ if (ImGui::IsKeyPressed(GLFW_KEY_ESCAPE))
+ break;
+
+
+ // Left click: start a new demonstration (only if not on the UI and in the
+ // demonstrations viewport)
+ if (!gui_state.is_drawing_demonstration) {
+ if (ImGui::IsMouseClicked(GLFW_MOUSE_BUTTON_1) && gui_state.can_draw_demonstration) {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ if (!hovering_ui && (mouse_x <= window_size.win_width / 3))
+ {
+ gui_state.is_drawing_demonstration = true;
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+ current_trajectory.push_back(coords);
+ }
+ }
+ } else {
+ double mouse_x, mouse_y;
+ glfwGetCursorPos(window, &mouse_x, &mouse_y);
+
+ vec coords = ui2fb({ mouse_x, mouse_y }, window_size, viewport_demos);
+
+ vec last_point = current_trajectory[current_trajectory.size() - 1];
+ vec diff = abs(coords - last_point);
+
+ if ((diff(0) > 1e-6) && (diff(1) > 1e-6))
+ current_trajectory.push_back(coords);
+
+ // Left mouse button release: end the demonstration creation
+ if (!ImGui::IsMouseDown(GLFW_MOUSE_BUTTON_1)) {
+ gui_state.is_drawing_demonstration = false;
+
+ if (current_trajectory.size() > 1) {
+
+ coordinate_system_list_t coordinate_systems;
+ convert(current_demonstration_handlers, coordinate_systems, model.parameters);
+
+ Demonstration demonstration(coordinate_systems, current_trajectory,
+ model.parameters);
+
+ demos.push_back(demonstration);
+
+ for (int i = 0; i < current_demonstration_handlers.size(); ++i)
+ current_demonstration_handlers[i]->fix();
+
+ fixed_demonstration_handlers.push_back(current_demonstration_handlers);
+ current_demonstration_handlers.clear();
+
+ learn(demos, model);
+
+ gui_state.can_draw_demonstration = false;
+ }
+
+ current_trajectory.clear();
+ }
+ }
+
+ ImGui::CaptureMouseFromApp();
+ }
+
+
+ // Cleanup
+ ImGui_ImplGlfwGL2_Shutdown();
+ glfwTerminate();
+
+ return 0;
+}
diff --git a/src/demo_online_gmm01.cpp b/src/demo_online_GMM01.cpp
similarity index 100%
rename from src/demo_online_gmm01.cpp
rename to src/demo_online_GMM01.cpp
diff --git a/src/demo_online_hsmm01.cpp b/src/demo_online_HSMM01.cpp
similarity index 100%
rename from src/demo_online_hsmm01.cpp
rename to src/demo_online_HSMM01.cpp
diff --git a/src/utils/gfx2.cpp b/src/utils/gfx2.cpp
index f9a1ee086a6c0dad55ada251dc7ef078a4895d64..b8cfbc302c0f7a6136c853aea16ed47e432e093b 100644
--- a/src/utils/gfx2.cpp
+++ b/src/utils/gfx2.cpp
@@ -284,6 +284,7 @@ model_t create_rectangle(const arma::fvec& color, float width, float height,
model.mode = GL_TRIANGLES;
model.lightning_enabled = false;
+ model.use_one_minus_src_alpha_blending = false;
// Position & rotation
model.transforms.position = position;
@@ -338,6 +339,7 @@ model_t create_square(const arma::fvec& color, float size, const arma::fvec& pos
model.mode = GL_TRIANGLES;
model.lightning_enabled = false;
+ model.use_one_minus_src_alpha_blending = false;
// Position & rotation
model.transforms.position = position;
@@ -391,6 +393,7 @@ model_t create_sphere(float radius, const arma::fvec& position,
model.mode = GL_TRIANGLES;
model.lightning_enabled = true;
+ model.use_one_minus_src_alpha_blending = false;
// Position & rotation
model.transforms.position = position;
@@ -475,12 +478,13 @@ model_t create_sphere(float radius, const arma::fvec& position,
model_t create_line(const arma::fvec& color, const arma::mat& points,
const arma::fvec& position, const arma::fmat& rotation,
- transforms_t* parent_transforms)
+ transforms_t* parent_transforms, bool line_strip)
{
model_t model = { 0 };
- model.mode = GL_LINE_STRIP;
+ model.mode = (line_strip ? GL_LINE_STRIP : GL_LINES);
model.lightning_enabled = false;
+ model.use_one_minus_src_alpha_blending = false;
// Position & rotation
model.transforms.position = position;
@@ -516,14 +520,14 @@ model_t create_line(const arma::fvec& color, const arma::mat& points,
model_t create_line(const arma::fvec& color, const std::vector<arma::vec>& points,
const arma::fvec& position, const arma::fmat& rotation,
- transforms_t* parent_transforms)
+ transforms_t* parent_transforms, bool line_strip)
{
arma::mat points_mat(points[0].n_rows, points.size());
for (size_t i = 0; i < points.size(); ++i)
points_mat.col(i) = points[i];
- return create_line(color, points_mat, position, rotation, parent_transforms);
+ return create_line(color, points_mat, position, rotation, parent_transforms, line_strip);
}
//-----------------------------------------------
@@ -536,6 +540,7 @@ model_t create_mesh(const arma::fvec& color, const arma::mat& vertices,
model.mode = GL_TRIANGLES;
model.lightning_enabled = false;
+ model.use_one_minus_src_alpha_blending = false;
// Position & rotation
model.transforms.position = position;
@@ -569,6 +574,71 @@ model_t create_mesh(const arma::fvec& color, const arma::mat& vertices,
//-----------------------------------------------
+model_t create_gaussian_background(const arma::fvec& color, const arma::vec& mu,
+ const arma::mat& sigma,
+ const arma::fvec& position, const arma::fmat& rotation,
+ transforms_t* parent_transforms)
+{
+ const int NB_POINTS = 60;
+
+ arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
+ arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
+ );
+
+ arma::vec eigval(2);
+ arma::mat eigvec(2, 2);
+ eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+
+ arma::mat R = eigvec * diagmat(sqrt(eigval));
+
+ arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
+
+ arma::mat vertices(2, NB_POINTS * 3);
+
+ for (int i = 0; i < NB_POINTS - 1; ++i)
+ {
+ vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
+ vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
+ }
+
+ vertices(arma::span::all, (NB_POINTS - 1) * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, (NB_POINTS - 1) * 3 + 1) = pts(arma::span::all, 0);
+ vertices(arma::span::all, (NB_POINTS - 1) * 3 + 2) = pts(arma::span::all, NB_POINTS - 1);
+
+ model_t model = create_mesh(color, vertices, position, rotation, parent_transforms);
+
+ model.use_one_minus_src_alpha_blending = true;
+
+ return model;
+}
+
+//-----------------------------------------------
+
+model_t create_gaussian_border(const arma::fvec& color, const arma::vec& mu,
+ const arma::mat& sigma,
+ const arma::fvec& position, const arma::fmat& rotation,
+ transforms_t* parent_transforms)
+{
+ const int NB_POINTS = 60;
+
+ arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS)),
+ arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, NB_POINTS))
+ );
+
+ arma::vec eigval(2);
+ arma::mat eigvec(2, 2);
+ eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+
+ arma::mat R = eigvec * diagmat(sqrt(eigval));
+
+ arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, NB_POINTS);
+
+ return create_line(color, pts, position, rotation, parent_transforms);
+}
+
+//-----------------------------------------------
+
void destroy(const model_t& model)
{
if (model.vertex_buffer)
@@ -636,9 +706,44 @@ bool draw(const model_t& model, const light_list_t& lights)
glPushMatrix();
glMultMatrixf(model_matrix.memptr());
+ GLboolean is_blending_enabled = glIsEnabled(GL_BLEND);
+ GLint blend_src_rgb, blend_src_alpha, blend_dst_rgb, blend_dst_alpha;
+
+ if (model.use_one_minus_src_alpha_blending)
+ {
+ if (is_blending_enabled)
+ {
+ glGetIntegerv(GL_BLEND_SRC_RGB, &blend_src_rgb);
+ glGetIntegerv(GL_BLEND_SRC_ALPHA, &blend_src_alpha);
+ glGetIntegerv(GL_BLEND_DST_RGB, &blend_dst_rgb);
+ glGetIntegerv(GL_BLEND_DST_ALPHA, &blend_dst_alpha);
+ }
+ else
+ {
+ glEnable(GL_BLEND);
+ }
+
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ }
+
// Draw the mesh
glDrawArrays(model.mode, 0, model.nb_vertices);
+ if (model.use_one_minus_src_alpha_blending)
+ {
+ if (is_blending_enabled)
+ {
+ glBlendFunc(GL_SRC_COLOR, blend_src_rgb);
+ glBlendFunc(GL_SRC_ALPHA, blend_src_alpha);
+ glBlendFunc(GL_DST_COLOR, blend_dst_rgb);
+ glBlendFunc(GL_DST_ALPHA, blend_dst_alpha);
+ }
+ else
+ {
+ glDisable(GL_BLEND);
+ }
+ }
+
glPopMatrix();
// Cleanup
@@ -828,4 +933,72 @@ bool intersects(const ray_t& ray, const arma::fvec& center, float radius,
return true;
}
+
+/********************************** OTHERS ***********************************/
+
+arma::mat get_gaussian_background_vertices(const arma::vec& mu, const arma::mat& sigma,
+ int nb_points)
+{
+ arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points)),
+ arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points))
+ );
+
+ arma::vec eigval(2);
+ arma::mat eigvec(2, 2);
+ eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+
+ arma::mat R = eigvec * diagmat(sqrt(eigval));
+
+ arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, nb_points);
+
+ arma::mat vertices(2, nb_points * 3);
+
+ for (int i = 0; i < nb_points - 1; ++i)
+ {
+ vertices(arma::span::all, i * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, i * 3 + 1) = pts(arma::span::all, i + 1);
+ vertices(arma::span::all, i * 3 + 2) = pts(arma::span::all, i);
+ }
+
+ vertices(arma::span::all, (nb_points - 1) * 3) = mu(arma::span(0, 1));
+ vertices(arma::span::all, (nb_points - 1) * 3 + 1) = pts(arma::span::all, 0);
+ vertices(arma::span::all, (nb_points - 1) * 3 + 2) = pts(arma::span::all, nb_points - 1);
+
+ return vertices;
+}
+
+//-----------------------------------------------
+
+arma::mat get_gaussian_border_vertices(const arma::vec& mu, const arma::mat& sigma,
+ int nb_points, bool line_strip)
+{
+ arma::mat pts0 = arma::join_cols(arma::cos(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points)),
+ arma::sin(arma::linspace<arma::rowvec>(0, 2 * arma::datum::pi, nb_points))
+ );
+
+ arma::vec eigval(2);
+ arma::mat eigvec(2, 2);
+ eig_sym(eigval, eigvec, sigma(arma::span(0, 1), arma::span(0, 1)));
+
+ arma::mat R = eigvec * diagmat(sqrt(eigval));
+
+ arma::mat pts = R * pts0 + arma::repmat(mu(arma::span(0, 1)), 1, nb_points);
+
+ if (line_strip)
+ return pts;
+
+ arma::mat vertices(2, nb_points * 2);
+
+ for (int i = 0; i < nb_points - 1; ++i)
+ {
+ vertices(arma::span::all, i * 2) = pts(arma::span::all, i);;
+ vertices(arma::span::all, i * 2 + 1) = pts(arma::span::all, i + 1);
+ }
+
+ vertices(arma::span::all, (nb_points - 1) * 2) = pts(arma::span::all, 0);;
+ vertices(arma::span::all, (nb_points - 1) * 2 + 1) = pts(arma::span::all, nb_points - 1);
+
+ return vertices;
+}
+
}