Commit cf8c8e33 by Sylvain CALINON

IROS'17 paper examples added with demo_Riemannian_cov_* filenames

parent 29a98549
......@@ -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) |
......
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
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
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
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;
end
end
% side-parts
for k = 1:d
col = d+1;
for p = 1:d-1
for q = p+1:d