demo_trajMFA01.m 5.88 KB
Newer Older
Sylvain Calinon's avatar
Sylvain Calinon committed
1
function demo_trajMFA01
2
% Trajectory model with either (see lines 79-81): 
Sylvain Calinon's avatar
Sylvain Calinon committed
3 4
%  -a mixture of factor analysers (MFA)
%  -a mixture of probabilistic principal component analyzers (MPPCA)
5
%  -a high-dimensional data clustering approach proposed by Bouveyron (HD-GMM)
Sylvain Calinon's avatar
Sylvain Calinon committed
6
%
7 8 9 10
% 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 reward the authors by citing the related publications, 
% and consider making your own research available in this way.
Sylvain Calinon's avatar
Sylvain Calinon committed
11
%
12
% @article{Calinon16JIST,
Sylvain Calinon's avatar
Sylvain Calinon committed
13
%   author="Calinon, S.",
14 15
%   title="A Tutorial on Task-Parameterized Movement Learning and Retrieval",
%   journal="Intelligent Service Robotics",
16 17 18 19 20 21
%		publisher="Springer Berlin Heidelberg",
%		doi="10.1007/s11370-015-0187-9",
%		year="2016",
%		volume="9",
%		number="1",
%		pages="1--29"
Sylvain Calinon's avatar
Sylvain Calinon committed
22
% }
23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
% 
% Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.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/>.
Sylvain Calinon's avatar
Sylvain Calinon committed
40 41 42

addpath('./m_fcts/');

43

Sylvain Calinon's avatar
Sylvain Calinon committed
44 45
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
46
model.nbStates = 5; %Number of components in the mixture model
Sylvain Calinon's avatar
Sylvain Calinon committed
47 48 49 50 51
model.nbFA = 1; %Dimension of the subspace in each cluster
model.nbVarPos = 4; %Dimension of the position datapoint
model.nbDeriv = 3; %Nb derivatives+1 (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
model.nbVar = model.nbVarPos * model.nbDeriv;
model.dt = 1; %Time step (large values such as 1 will tend to create clusers by following position information)
52
nbData = 100; %Number of datapoints in a trajectory
53
nbSamples = 5; %Number of trajectory samples
Sylvain Calinon's avatar
Sylvain Calinon committed
54

55
[PHI,PHI1] = constructPHI(model,nbData,nbSamples); %Construct PHI operator (big sparse matrix)
Sylvain Calinon's avatar
Sylvain Calinon committed
56 57


58
%% Load handwriting data
Sylvain Calinon's avatar
Sylvain Calinon committed
59
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60
demos=[];
61
load('data/2Dletters/G.mat'); %Load x1,x2 variables
Sylvain Calinon's avatar
Sylvain Calinon committed
62
for n=1:nbSamples
63
	s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Sylvain Calinon's avatar
Sylvain Calinon committed
64
end
65
demos=[];
66
load('data/2Dletters/C.mat'); %Load x3,x4 variables
67 68 69 70
Data=[];
for n=1:nbSamples
	s(n).Data = [s(n).Data; spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData))]; %Resampling
	Data = [Data s(n).Data]; 
Sylvain Calinon's avatar
Sylvain Calinon committed
71 72 73
end

%Re-arrange data in vector form
74 75 76
x = reshape(Data, model.nbVarPos*nbData*nbSamples, 1) * 1E2; %Scale data to avoid numerical computation problem
zeta = PHI*x; %y is for example [x1(1), x2(1), x1d(1), x2d(1), x1(2), x2(2), x1d(2), x2d(2), ...], see Eq. (2.4.5) in doc/TechnicalReport.pdf
Data = reshape(zeta, model.nbVarPos*model.nbDeriv, nbData*nbSamples); %Include derivatives in Data
Sylvain Calinon's avatar
Sylvain Calinon committed
77 78 79 80 81 82


%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_kmeans(Data, model);

83 84
%[model, GAMMA2] = EM_GMM(Data, model);
[model, GAMMA2] = EM_MFA(Data, model);
Sylvain Calinon's avatar
Sylvain Calinon committed
85 86 87 88
%[model, GAMMA2] = EM_MPPCA(Data, model);
%[model, GAMMA2] = EM_HDGMM(Data, model);


89
%% Reconstruction
Sylvain Calinon's avatar
Sylvain Calinon committed
90
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 92 93 94 95 96 97 98 99 100 101
%Compute best path for the n-th demonstration
[~,r(1).q] = max(GAMMA2(:,1:nbData),[],1); %works also for nbStates=1

%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (2.4.8) in doc/TechnicalReport.pdf
MuQ = reshape(model.Mu(:,r(1).q), model.nbVar*nbData, 1); 
%MuQ = zeros(model.nbVar*nbData,1);
SigmaQ = zeros(model.nbVar*nbData);
for t=1:nbData
	id = (t-1)*model.nbVar+1:t*model.nbVar;
	%MuQ(id) = model.Mu(:,r(n).q(t)); 
	SigmaQ(id,id) = model.Sigma(:,:,r(1).q(t));
Sylvain Calinon's avatar
Sylvain Calinon committed
102
end
103 104
%SigmaQ can alternatively be computed with:
%SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,r(1).q), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
Sylvain Calinon's avatar
Sylvain Calinon committed
105

106 107
%Least squares computation
[zeta,~,mse,S] = lscov(PHI1, MuQ, SigmaQ, 'chol'); %Retrieval of data with weighted least squares solution
108
r(1).Data = reshape(zeta, model.nbVarPos, nbData); %Reshape data for plotting
Sylvain Calinon's avatar
Sylvain Calinon committed
109

110 111 112 113 114
%Rebuild covariance by reshaping S
for t=1:nbData
	id = (t-1)*model.nbVarPos+1:t*model.nbVarPos;
	r(1).expSigma(:,:,t) = S(id,id) * nbData;
end
Sylvain Calinon's avatar
Sylvain Calinon committed
115 116 117 118 119 120 121


%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,1200,600]);

subplot(1,2,1); hold on;
122 123 124 125 126 127
plotGMM(r(1).Data([1,2],:), r(1).expSigma([1,2],[1,2],:), [.5 .8 1], .2);
plotGMM(model.Mu(1:2,:),model.Sigma(1:2,1:2,:), [.8 0 0], .2);
plotGMM(model.Mu(1:2,:),model.P(1:2,1:2,:), [1 .4 .4], .2);
for i=1:model.nbStates
	plotGMM(model.Mu(1:2,i), model.L(1:2,:,i)*model.L(1:2,:,i)'+eye(2)*1E-1, [1 .4 .4]);
end
Sylvain Calinon's avatar
Sylvain Calinon committed
128 129 130 131 132 133 134
plot(Data(1,:), Data(2,:),'.','linewidth',2,'color',[0,0,0]);
plot(r(1).Data(1,:), r(1).Data(2,:),'-','linewidth',2,'color',[.8,0,0]);
axis equal;
xlabel('x_1','fontsize',16); ylabel('x_2','fontsize',16);

subplot(1,2,2); hold on;
plotGMM(r(1).Data([3,4],:), r(1).expSigma([3,4],[3,4],:), [.5 .8 1]);
135 136 137 138 139
plotGMM(model.Mu(3:4,:),model.Sigma(3:4,3:4,:), [.8 0 0], .2);
plotGMM(model.Mu(3:4,:),model.P(3:4,3:4,:), [1 .4 .4], .2);
for i=1:model.nbStates
	plotGMM(model.Mu(3:4,i), model.L(3:4,:,i)*model.L(3:4,:,i)'+eye(2)*1E-1, [1 .4 .4]);
end
Sylvain Calinon's avatar
Sylvain Calinon committed
140 141 142 143 144 145
plot(Data(3,:), Data(4,:),'.','linewidth',2,'color',[0,0,0]);
plot(r(1).Data(3,:), r(1).Data(4,:),'-','linewidth',2,'color',[.8,0,0]);
axis equal;
xlabel('x_3','fontsize',16); ylabel('x_4','fontsize',16);

%print('-dpng','graphs/demo_trajMFA01.png');
146 147
%pause;
%close all;