IROS'17 paper examples added with demo_Riemannian_cov_* filenames

README.md

@@ -39,22 +39,35 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
 }
 ```
 
-[3] Riemannian manifolds:
+[3] Riemannian manifolds (S2,S3):
 ```
 @article{Zeestraten17RAL,
 	author="Zeestraten, M. J. A. and Havoutis, I. and Silv\'erio, J. and Calinon, S. and Caldwell, D. G.",
-	title="An Approach for Imitation Learning on Riemannian Manifolds",
+	title="An Approach for Imitation Learning on {R}iemannian Manifolds",
 	journal="{IEEE} Robotics and Automation Letters ({RA-L})",
-	doi="",
+	doi="10.1109/LRA.2017.2657001",
 	year="2017",
-	month="",
-	volume="",
-	number="",
+	month="June",
+	volume="2",
+	number="3",
+	pages="1240--1247"
+}
+```
+
+[4] Riemannian manifolds (S+):
+```
+@inproceedings{Jaquier17IROS,
+	author="Jaquier, N. and Calinon, S.", 
+	title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with {sEMG}",
+	booktitle="Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
+	year="2017",
+	month="September",
+	address="Vancouver, Canada",
 	pages=""
 }
 ```
 
-[4] Semi-tied GMM:
+[5] Semi-tied GMM:
 ```
 @article{Tanwani16RAL,
 	author="Tanwani, A. K. and Calinon, S.",
@@ -69,7 +82,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
 }
 ```
 
-[5] DP-means:
+[6] DP-means:
 ```
 @article{Bruno16AURO,
 	author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
@@ -110,7 +123,7 @@ All the examples are located in the main folder, and the functions are located i
 | demo_DMP_GMR04 | [1] | Same as demo_DMP_GMR03 by using the task-parameterized model formalism | 
 | demo_DMP_GMR_LQR01 | [1] | Same as demo_DMP_GMR04 but with LQR used to refine the parameters of the spring-damper system | 
 | demo_DMP_GMR_LQR02 | [1] | Same as demo_DMP_GMR_LQR01 with perturbations added to show the benefit of full covariance to coordinate disturbance rejection | 
-| demo_DPMeans_Online01 | [5] | Online clustering with DP-means algorithm |
+| demo_DPMeans_Online01 | [6] | Online clustering with DP-means algorithm |
 | demo_DSGMR01 | [1] | Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, with decay variable used as input (as in DMP) |
 | demo_DTW01 | [1] | Trajectory realignment through dynamic time warping (DTW) |
 | demo_GMM01 | [1] | Gaussian mixture model (GMM) parameters estimation |
@@ -139,6 +152,15 @@ All the examples are located in the main folder, and the functions are located i
 | demo_MPPCA01 | [1] | Mixture of probabilistic principal component analyzers (MPPCA) |
 | demo_regularization01 | [1] | Regularization of GMM parameters with minimum admissible eigenvalue |
 | demo_regularization02 | [1] | Regularization of GMM parameters with the addition of a small circular covariance |
+| demo_Riemannian_cov_GMM01 | [4] | GMM for covariance data by relying on Riemannian manifold |
+| demo_Riemannian_cov_GMR01 | [4] | GMR with time as input and covariance data as output by relying on Riemannian manifold |
+| demo_Riemannian_cov_GMR02 | [4] | GMR with time as input and position vector as output with comparison between computation in vector and matrix forms |
+| demo_Riemannian_cov_GMR03 | [4] | GMR with vector as input and covariance data as output by relying on Riemannian manifold |
+| demo_Riemannian_cov_interp01 | [4] | Covariance interpolation on Riemannian manifold |
+| demo_Riemannian_cov_interp02 | [4] | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
+| demo_Riemannian_cov_interp03 | [4] | Trajectory morphing through covariance interpolation on Riemannian manifold (with augmented Gaussian trajectory distribution) |
+| demo_Riemannian_cov_search01 | [4] | EM-based stochastic optimization of covariance on Riemannian manifold |
+| demo_Riemannian_cov_vecTransp01 | [4] | Verification of angle conservation in parallel transport on the symmetric positive definite |
 | demo_Riemannian_sphere_GaussProd01 | [3] | Product of Gaussians on a sphere by relying on Riemannian manifold |
 | demo_Riemannian_sphere_GMM01 | [3] | GMM for data on a sphere by relying on Riemannian manifold |
 | demo_Riemannian_sphere_GMR01 | [3] | GMR with input and output data on a sphere by relying on Riemannian manifold |
@@ -154,7 +176,7 @@ All the examples are located in the main folder, and the functions are located i
 | demo_Riemannian_quat_vecTransp01 | [3] | Parallel transport for unit quaternions |
 | demo_SEDS01 | [1] | Continuous autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
 | demo_SEDS_discrete01 | [1] | Discrete autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
-| demo_semitiedGMM01 | [4] | Semi-tied Gaussian Mixture Model by tying the covariance matrices of a GMM with a set of common basis vectors |
+| demo_semitiedGMM01 | [5] | Semi-tied Gaussian Mixture Model by tying the covariance matrices of a GMM with a set of common basis vectors |
 | demo_stdPGMM01 | [1] | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
 | demo_testDampingRatio01 | [1] | Test with critically damped system and ideal underdamped system |
 | demo_testLQR01 | [1] | Test of linear quadratic regulation (LQR) with different variance in the data |
@@ -179,7 +201,7 @@ All the examples are located in the main folder, and the functions are located i
 | demo_TPtrajGMM01 | [1] | Task-parameterized model with trajectory-GMM encoding |
 | demo_trajDistrib01 | [1] | Stochastic sampling with Gaussian trajectory distribution |
 | demo_trajGMM01 | [1] | Reproduction of trajectory with a GMM with dynamic features (trajectory-GMM) |
-| demo_trajHSMM01 | [1] | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
+| demo_trajHSMM01 | [2] | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
 | demo_trajMFA01 | [1] | Trajectory model with either a mixture of factor analysers (MFA), a mixture of probabilistic principal component analyzers (MPPCA), or a high-dimensional data clustering approach (HD-GMM) |
 | demoIK_nullspace_TPGMM01 | [1] | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
 | demoIK_pointing_TPGMM01 | [1] | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) |

demo_Riemannian_cov_interp01.m

+function demo_Riemannian_cov_interp01
+% Covariance interpolation on Riemannian manifold
+%
+% Writing code takes time. Polishing it and making it available to others takes longer! 
+% If some parts of the code were useful for your research of for a better understanding 
+% of the algorithms, please cite the related publications.
+%
+% @article{Jaquier17IROS,
+%   author="Jaquier, N. and Calinon, S.",
+%   title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: 
+%	    Application to Wrist Motion Estimation with s{EMG}",
+%   year="2017",
+%	  booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
+%	  address = "Vancouver, Canada"
+% }
+% 
+% Copyright (c) 2017 Sylvain Calinon, Idiap Research Institute, http://idiap.ch/
+% 
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Number of variables
+model.nbStates = 2; %Number of states
+nbData = 20; %Number of interpolations
+% nbIter = 5; %Number of iteration for the Gauss Newton algorithm
+
+
+%% Gaussians parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.Mu(:,1) = [0; 0];
+d1 = [2; 3];
+model.Sigma(:,:,1) = d1*d1' + eye(model.nbVar)*1;
+
+model.Mu(:,2) = [10; 0];
+% d2 = [4; 1];
+% model.Sigma(:,:,2) = d2*d2' + eye(model.nbVar)*1E-1;
+[R,~] = qr(randn(model.nbVar));
+model.Sigma(:,:,2) = R * model.Sigma(:,:,1) * R';
+
+
+%% Linear interpolation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w = [linspace(1,0,nbData); linspace(0,1,nbData)];
+Mu0 = interp1([1,0], model.Mu', w(1,:))';
+Sigma0 = zeros(model.nbVar,model.nbVar,nbData);
+for t=1:nbData
+	for i=1:model.nbStates
+		Sigma0(:,:,t) = Sigma0(:,:,t) + w(i,t) * model.Sigma(:,:,i);
+	end
+	[V0(:,:,t), D0(:,:,t)] = eigs(Sigma0(:,:,t));
+	S0det(t) = det(Sigma0(:,:,t)); 
+	S0tra(t) = trace(Sigma0(:,:,t));
+end
+
+
+%% Geodesic interpolation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w = [linspace(1,0,nbData); linspace(0,1,nbData)];
+Mu = interp1([1,0], model.Mu', w(1,:))';
+Sigma = zeros(model.nbVar,model.nbVar,nbData);
+
+% S = model.Sigma(:,:,1);
+for t=1:nbData
+	
+% 	%Interpolation between more than 2 covariances can be computed in an iterative form
+% 	for n=1:nbIter
+% 		W = zeros(model.nbVar);
+% 		for i=1:model.nbStates
+% 			W = W + w(i,t) * logmap(model.Sigma(:,:,i), S);
+% 		end
+% 		S = expmap(W,S);
+% 	end
+% 	Sigma(:,:,t) = S;
+
+	%Interpolation between two covariances can be computed in closed form
+	Sigma(:,:,t) = expmap(w(2,t)*logmap(model.Sigma(:,:,2), model.Sigma(:,:,1)), model.Sigma(:,:,1));
+
+	[V(:,:,t), D(:,:,t)] = eigs(Sigma(:,:,t));
+	Sdet(t) = det(Sigma(:,:,t));
+	Stra(t) = trace(Sigma(:,:,t));
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1300,800]); 
+
+%Plot determinant for linear interpolation
+subplot(2,2,1); hold on; title('Linear interpolation');
+plot(S0det,'k-');
+axis([1, nbData, min(S0det)-1, max(S0det)+1]);
+xlabel('t'); ylabel('det(S)');
+%Plot determinant for geodesic interpolation
+subplot(2,2,2); hold on; title('Geodesic interpolation');
+plot(Sdet,'k-');
+axis([1, nbData, min(Sdet)-1, max(Sdet)+1]);
+xlabel('t'); ylabel('det(S)');
+
+%Plot linear interpolation
+subplot(2,2,3); hold on; axis off; 
+plotGMM(model.Mu, model.Sigma, [0 0 0]);
+plotGMM(Mu0, Sigma0, [0 .8 0], .1);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%Plot geodesic interpolation
+subplot(2,2,4); hold on; axis off; 
+plotGMM(model.Mu, model.Sigma, [0 0 0]);
+plotGMM(Mu, Sigma, [.8 0 0], .1);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+
+% pause;
+% close all;
+end
+
+
+%% Functions
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function S = expmap(W,S)
+	S = S^.5 * expm(S^-.5 * W * S^-.5) * S^.5;
+end
+
+function S = logmap(W,S)
+	S = S^.5 * logm(S^-.5 * W * S^-.5) * S^.5;
+end
+
+
+
+

demo_Riemannian_cov_interp02.m

+function demo_Riemannian_cov_interp02
+% Covariance interpolation on Riemannian manifold from a GMM with augmented covariances
+%
+% Writing code takes time. Polishing it and making it available to others takes longer! 
+% If some parts of the code were useful for your research of for a better understanding 
+% of the algorithms, please cite the related publications.
+%
+% @article{Jaquier17IROS,
+%   author="Jaquier, N. and Calinon, S.",
+%   title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: 
+%	    Application to Wrist Motion Estimation with s{EMG}",
+%   year="2017",
+%	  booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
+%	  address = "Vancouver, Canada"
+% }
+% 
+% Copyright (c) 2017 Sylvain Calinon, Idiap Research Institute, http://idiap.ch/
+% 
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 2; %Number of states in the GMM
+model.nbVar = 2; %Number of variables [x1,x2]
+nbData = 20; %Length of each trajectory
+
+
+%% Generate random Gaussians
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.Mu = rand(model.nbVar, model.nbStates) * 1E1;
+for i=1:model.nbStates
+	xtmp = randn(model.nbVar,5) * 1E0;
+	model.Sigma(:,:,i) = xtmp*xtmp' + eye(model.nbVar) * 1E-4;
+end
+
+
+%% Transformation to Gaussians with augmented covariances centered on zero 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model2 = model;
+model2.nbVar = model.nbVar+1;
+model2.Mu = zeros(model2.nbVar, model2.nbStates);
+model2.Sigma = zeros(model2.nbVar, model2.nbVar, model2.nbStates);
+for i=1:model.nbStates
+	model2.Sigma(:,:,i) = [model.Sigma(:,:,i)+model.Mu(:,i)*model.Mu(:,i)', model.Mu(:,i); model.Mu(:,i)', 1];
+end
+
+
+%% Geodesic interpolation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w = linspace(0,1,nbData); %standard interpolation
+% w = linspace(-.5,1.5,nbData); %extrapolation (exaggeration)
+
+wi = [linspace(1,0,nbData); linspace(0,1,nbData)]; %standard interpolation
+% wi = [linspace(1.5,-.5,nbData); linspace(-.5,1.5,nbData)]; %extrapolation (exaggeration)
+S = model2.Sigma(:,:,1);
+
+Sigma2 = zeros(model2.nbVar, model2.nbVar, nbData*(model.nbStates-1));
+for i=2:model.nbStates
+	for t=1:nbData		
+% 		%Interpolation between more than 2 covariances can be computed in an iterative form
+% 		nbIter = 10; %Number of iterations for the convergence of Riemannian estimate
+% 		for n=1:nbIter
+% 			W = zeros(model2.nbVar);
+% 			for j=1:model.nbStates
+% 				W = W + wi(j,t) * logmap(model2.Sigma(:,:,j), S);
+% 			end
+% 			S = expmap(W,S);
+% 		end
+% 		Sigma2(:,:,(i-2)*nbData+t) = S;
+		
+		%Interpolation between two covariances can be computed in closed form
+		Sigma2(:,:,(i-2)*nbData+t) = expmap(w(t)*logmap(model2.Sigma(:,:,i), model2.Sigma(:,:,i-1)), model2.Sigma(:,:,i-1));
+	end
+end
+
+Mu = zeros(model.nbVar, nbData*(model.nbStates-1));
+Sigma = zeros(model.nbVar, model.nbVar, nbData*(model.nbStates-1));
+for t=1:nbData*(model.nbStates-1)
+	beta = Sigma2(end,end,t);
+	Mu(:,t) = Sigma2(end,1:end-1,t) ./ beta;
+	Sigma(:,:,t) = Sigma2(1:end-1,1:end-1,t) - beta * Mu(:,t)*Mu(:,t)';
+end
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,650,650]); hold on; axis off;
+plot(Mu(1,:), Mu(2,:), '-','linewidth',2,'color',[.8 0 0]);
+plotGMM(model.Mu, model.Sigma, [.5 .5 .5], .5);
+plotGMM(Mu, Sigma, [.8 0 0], .02);
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%print('-dpng','graphs/demo_Riemannian_cov_interp02.png');
+
+% pause;
+% close all;
+end
+
+
+%% Functions
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function X = expmap(U,S)
+	X = S^.5 * expm(S^-.5 * U * S^-.5) * S^.5;
+end
+
+function U = logmap(X,S)
+	U = S^.5 * logm(S^-.5 * X * S^-.5) * S^.5;
+end
+

demo_Riemannian_cov_interp03.m

+function demo_Riemannian_cov_interp03
+% Trajectory morphing through covariance interpolation on Riemannian manifold (with augmented Gaussian trajectory distribution) 
+%
+% Writing code takes time. Polishing it and making it available to others takes longer! 
+% If some parts of the code were useful for your research of for a better understanding 
+% of the algorithms, please cite the related publications.
+%
+% @article{Jaquier17IROS,
+%   author="Jaquier, N. and Calinon, S.",
+%   title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: 
+%	    Application to Wrist Motion Estimation with s{EMG}",
+%   year="2017",
+%	  booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
+%	  address = "Vancouver, Canada"
+% }
+% 
+% Copyright (c) 2017 Sylvain Calinon, Idiap Research Institute, http://idiap.ch/
+% 
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 2; %Number of states in the GMM
+model.nbVar = 2; %Number of variables [t,x1,x2]
+nbData = 20; %Length of each trajectory
+nbInterp = 50; %Length of each trajectory
+nbSamples = 5; %Number of demonstrations
+
+
+%% Load handwriting data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/2Dletters/S.mat');
+for n=1:nbSamples
+	DataTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+	Data(:,n,1) = reshape(DataTmp, model.nbVar*nbData, 1); 
+end
+demos=[];
+load('data/2Dletters/N.mat');
+for n=1:nbSamples
+	DataTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+	Data(:,n,2) = reshape(DataTmp+20, model.nbVar*nbData, 1); 
+end
+
+
+%% Compute normal trajectory distribution
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = model.nbVar*nbData;
+for i=1:model.nbStates
+	model.Mu(:,i) = mean(Data(:,:,i),2); 
+	model.Sigma(:,:,i) = cov(Data(:,:,i)') * 1E0 + eye(size(Data,1)) * 1E-5;
+end
+
+%Initialisation of model2 (Gaussians with augmented covariances centered on zero)
+model2 = model;
+model2.nbVar = model.nbVar+1;
+model2.Mu = zeros(model2.nbVar, model2.nbStates);
+model2.Sigma = zeros(model2.nbVar, model2.nbVar, model2.nbStates);
+for i=1:model.nbStates
+	model2.Sigma(:,:,i) = [model.Sigma(:,:,i)+model.Mu(:,i)*model.Mu(:,i)', model.Mu(:,i); model.Mu(:,i)', 1];
+end
+
+
+%% Geodesic interpolation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+w = linspace(0,1,nbInterp); %standard interpolation
+
+% wi = [linspace(1,0,nbInterp); linspace(0,1,nbInterp)];
+% S = model2.Sigma(:,:,1);
+
+Sigma2 = zeros(model2.nbVar, model2.nbVar, nbInterp*(model.nbStates-1));
+for i=2:model.nbStates
+	for t=1:nbInterp
+% 		%Interpolation between more than 2 covariances can be computed in an iterative form
+% 		nbIter = 10; %Number of iterations for the convergence of Riemannian estimate
+% 		for n=1:nbIter
+% 			W = zeros(model2.nbVar);
+% 			for j=1:model.nbStates
+% 				W = W + wi(j,t) * logmap(model2.Sigma(:,:,j), S);
+% 			end
+% 			S = expmap(W,S);
+% 		end
+% 		Sigma2(:,:,(i-2)*nbInterp+t) = S;
+
+		%Interpolation between two covariances can be computed in closed form
+		Sigma2(:,:,(i-2)*nbInterp+t) = expmap(w(t)*logmap(model2.Sigma(:,:,i), model2.Sigma(:,:,i-1)), model2.Sigma(:,:,i-1));
+	end	
+end
+
+for t=1:size(Sigma2,3)
+	beta = Sigma2(end,end,t);
+	Mu(:,t) = Sigma2(end,1:end-1,t) ./ beta;
+	Sigma(:,:,t) = Sigma2(1:end-1,1:end-1,t) - beta * Mu(:,t)*Mu(:,t)';
+end
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,650,650]); hold on; axis off;
+for i=1:model.nbStates
+	for n=1:nbSamples
+		plot(Data(1:2:end,n,i), Data(2:2:end,n,i), '-','linewidth',1,'color',[.5 .5 .5]);
+	end
+	plot(model.Mu(1:2:end,i), model.Mu(2:2:end,i), '-','linewidth',1,'color',[0 0 0]);
+end
+for t=1:nbInterp
+	plot(Mu(1:2:end,t), Mu(2:2:end,t), '-','linewidth',1,'color',[.8 0 0]);
+end
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%print('-dpng','graphs/demo_Riemannian_cov_interp03.png');
+
+% pause;
+% close all;
+end
+
+
+%% Functions
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function X = expmap(U,S)
+	X = S^.5 * expm(S^-.5 * U * S^-.5) * S^.5;
+end
+
+function U = logmap(X,S)
+	U = S^.5 * logm(S^-.5 * X * S^-.5) * S^.5;
+end
+

demo_Riemannian_cov_search01.m

+function demo_Riemannian_cov_search01
+% EM-based stochastic optimization of covariance on Riemannian manifold 
+%
+% Writing code takes time. Polishing it and making it available to others takes longer! 
+% If some parts of the code were useful for your research of for a better understanding 
+% of the algorithms, please cite the related publications.
+%
+% @article{Jaquier17IROS,
+%   author="Jaquier, N. and Calinon, S.",
+%   title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: 
+%	    Application to Wrist Motion Estimation with s{EMG}",
+%   year="2017",
+%	  booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
+%	  address = "Vancouver, Canada"
+% }
+% 
+% Copyright (c) 2017 Sylvain Calinon, Idiap Research Institute, http://idiap.ch/
+% 
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbVar = 2; %Number of variables [t,x1,x2]
+nbVarCov = nbVar + nbVar * (nbVar-1)/2; %Dimension of the vecotrized form of covariance
+nbData = 10; %Number of initial datapoints
+nbEpisods = 20; %Number of iterations for the stochastic search
+nbIter = 5; %Number of iteration for the Gauss Newton algorithm
+minSigma = eye(nbVarCov) * 1E-4; %Minimal exploration noise 
+
+
+%% Generate random covariances
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Desired point 
+xtmp = randn(nbVar,5) * 1E-1;
+xvecHat = symMat2vec(xtmp*xtmp' + eye(nbVar)*1E-2); 
+
+%Initial guess and exploration noise
+MuMan = symMat2vec(eye(nbVar) * 1E-1);
+Sigma = eye(nbVarCov) * 1E-2;
+
+xvec = [];
+for n=1:nbEpisods
+	%Sampling of new point(s)
+	[V,D] = eig(Sigma);
+	if n==1
+		uvec = V*D.^.5 * randn(nbVarCov,nbData);
+	else
+		uvec = V*D.^.5 * randn(nbVarCov,1);
+	end
+	xvec = [xvec, expmap_vec(uvec,MuMan)];
+	
+	%Evaluate return
+	%utmp = logmap_vec(xvec, xvecHat) + realmin;
+	%H = sum(utmp.*utmp)'.^-1;
+	%H = gaussPDF(utmp, zeros(nbVarCov,1), eye(nbVarCov)*1E-5);
+	H = gaussPDF(xvec, xvecHat, eye(nbVarCov)*1E-5);
+	H = (H+realmin) ./ sum(H+realmin); 	
+
+	%Update MuMan
+	for t=1:nbIter
+		uvec = logmap_vec(xvec, MuMan);
+		MuMan = expmap_vec(uvec * H, MuMan);
+	end
+	%Update Sigma
+	Sigma = uvec * diag(H) * uvec' + minSigma;
+	
+	%Log data
+	r(n).MuMan = MuMan;
+end
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,650,650]); hold on; axis off;
+% for t=1:size(xvec,2)
+% 	plotGMM(zeros(2,1), reshape(xvec(:,t),2,2), [.5 .5 .5], .1);
+% end
+plotGMM(zeros(2,1), vec2symMat(xvecHat), [.7 .7 1], 1);
+for t=1:nbData
+	plotGMM(zeros(2,1), vec2symMat(xvec(:,t)), [.7 .7 .7], .1);
+end
+axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+%pause
+for n=1:nbEpisods
+	plotGMM(zeros(2,1), vec2symMat(r(n).MuMan), [.8*n/nbEpisods .8*(1-n/nbEpisods) 0], .05);
+	pause(0.01);
+end
+%print('-dpng','graphs/demo_Riemannian_cov_search01.png');
+
+% pause;
+% close all;
+end
+
+
+%% Functions
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function X = expmap(U,S)
+% Exponential map (SPD manifold)
+N = size(U,3);
+for n=1:N
+	X(:,:,n) = S^.5 * expm(S^-.5 * U(:,:,n) * S^-.5) * S^.5;
+end
+end
+
+function U = logmap(X,S)
+% Logarithm map (SPD manifold)
+N = size(X,3);
+for n=1:N
+	U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
+end
+end
+
+function x = expmap_vec(u,s)
+% Exponential map for the vector form (SPD manifold)
+	U = vec2symMat(u);
+	S = vec2symMat(s);
+	x = symMat2vec(expmap(U,S));
+end
+
+function u = logmap_vec(x,s)
+% Logarithm map for the vector form (SPD manifold)
+	X = vec2symMat(x);
+	S = vec2symMat(s);
+	u = symMat2vec(logmap(X,S));
+end
+
+function v = symMat2vec(S)
+% Reduced vectorisation of a symmetric matrix
+[d,~,N] = size(S);
+v = zeros(d+d*(d-1)/2,N);
+for n = 1:N
+	v(1:d,n) = diag(S(:,:,n));
+	row = d+1;
+	for i = 1:d-1
+		v(row:row + d-1-i,n) = sqrt(2).*S(i+1:end,i,n);
+		row = row + d-i;
+	end
+end
+end
+
+function S = vec2symMat(v)
+% Matricisation of a vector to a symmetric matrix
+[t, N] = size(v);
+d = (-1 + sqrt(1+8*t))/2;
+S = zeros(d,d,N);
+for n= 1:N
+	% Side elements
+	i = d+1;
+	for row = 1:d-1
+		S(row,row+1:d,n) = v(i:i+d-1-row,n)./sqrt(2);
+		i = i+d-row;
+	end
+	S(:,:,n) = S(:,:,n) + S(:,:,n)';
+	% Diagonal elements
+	S(:,:,n) = S(:,:,n) + diag(v(1:d,n));
+end
+end
+
+function [Sred, V, D] = covOrder4to2(S)
+% Reduction of a 4th-order covariance to a 2nd-order covariance. Return the
+% reduced covariance, the eigentensors and eigenvalues.
+d = size(S,1);
+% Reduction to 2nd-order
+Sred = zeros(d+d*(d-1)/2);
+% left-up part
+for k = 1:d
+	for m = 1:d
+		Sred(k,m) = S(k,k,m,m);
+	end
+end
+% right-down part
+row = d+1; col = d+1;
+for k = 1:d-1
+	for m = k+1:d
+		for p = 1:d-1
+			for q = p+1:d
+				Sred(row,col) = 2*S(k,m,p,q);
+				col = col+1;
+			end
+		end
+		row = row + 1;
+		col = d+1;