demo_Riemannian_S2_GMM01.m 7.95 KB
Newer Older
Sylvain CALINON's avatar
Sylvain CALINON committed
1
function demo_Riemannian_S2_GMM01
2 3
% GMM for data on a sphere by relying on Riemannian manifold. 
%
Sylvain CALINON's avatar
Sylvain CALINON committed
4
% If this code is useful for your research, please cite the related publication:
Sylvain CALINON's avatar
Sylvain CALINON committed
5
% @article{Calinon20RAM,
Sylvain CALINON's avatar
Sylvain CALINON committed
6 7
% 	author="Calinon, S. and Jaquier, N.",
% 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
Sylvain CALINON's avatar
Sylvain CALINON committed
8 9 10
% 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% 	year="2020",
% 	pages=""
11 12
% }
% 
Sylvain CALINON's avatar
Sylvain CALINON committed
13
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35
% 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/>.

addpath('./m_fcts/');


%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 50; %Number of datapoints
Sylvain CALINON's avatar
Sylvain CALINON committed
36
nbSamples = 8; %Number of demonstrations
37 38 39 40
nbIter = 10; %Number of iteration for the Gauss Newton algorithm
nbIterEM = 30; %Number of iteration for the EM algorithm
nbDrawingSeg = 30; %Number of segments used to draw ellipsoids

Sylvain CALINON's avatar
Sylvain CALINON committed
41
model.nbStates = 3; %Number of states in the GMM
42 43 44
model.nbVar = 2; %Dimension of the tangent space
model.nbVarMan = 3; %Dimension of the manifold
model.params_diagRegFact = 1E-4; %Regularization of covariance
Sylvain CALINON's avatar
Sylvain CALINON committed
45
x0 = [0; -1; 0]; %Point on the sphere to project handwriting data
46 47 48 49 50


%% Generate data on a sphere from handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
Sylvain CALINON's avatar
Sylvain CALINON committed
51
load('data/2Dletters/N.mat');
52 53 54
u=[];
for n=1:nbSamples
	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
55
	u = [u [s(n).Data*1.1E-1]]; 
56
end
Sylvain CALINON's avatar
Sylvain CALINON committed
57
x = expmap(u, x0);
58 59 60 61


%% GMM parameters estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sylvain CALINON's avatar
Sylvain CALINON committed
62 63 64 65 66 67 68
% %Random initialization
% model.Priors = ones(1,model.nbStates) / model.nbStates;
% model.MuMan = expmap(randn(model.nbVar,model.nbStates), randn(model.nbVarMan,1)); %Center on the manifold
% model.Mu = zeros(model.nbVar,model.nbStates); %Center in the tangent plane at point MuMan of the manifold
% model.Sigma = repmat(eye(model.nbVar)*5E-1,[1,1,model.nbStates]); %Covariance in the tangent plane at point MuMan

%Initialization based on k-bins/k-means
69
model = init_GMM_kbins(u, model, nbSamples);
Sylvain CALINON's avatar
Sylvain CALINON committed
70 71
% model = init_GMM_kmeans(u, model);
model.MuMan = expmap(model.Mu, x0); %Center on the manifold
72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
model.Mu = zeros(model.nbVar,model.nbStates); %Center in the tangent plane at point MuMan of the manifold

for nb=1:nbIterEM
	%E-step
	L = zeros(model.nbStates,size(x,2));
	for i=1:model.nbStates
		L(i,:) = model.Priors(i) * gaussPDF(logmap(x, model.MuMan(:,i)), model.Mu(:,i), model.Sigma(:,:,i));
	end
	GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1);
	H = GAMMA ./ repmat(sum(GAMMA,2),1,nbData*nbSamples);
	%M-step
	for i=1:model.nbStates
		%Update Priors
		model.Priors(i) = sum(GAMMA(i,:)) / (nbData*nbSamples);
		%Update MuMan
		for n=1:nbIter
			u(:,:,i) = logmap(x, model.MuMan(:,i));
			model.MuMan(:,i) = expmap(u(:,:,i)*H(i,:)', model.MuMan(:,i));
		end
		%Update Sigma
		model.Sigma(:,:,i) = u(:,:,i) * diag(H(i,:)) * u(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact;
	end
end


%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clrmap = lines(model.nbStates);

%Display of covariance contours on the sphere
tl = linspace(-pi, pi, nbDrawingSeg);
Gdisp = zeros(model.nbVarMan, nbDrawingSeg, model.nbStates);
for i=1:model.nbStates
	[V,D] = eig(model.Sigma(:,:,i));
	Gdisp(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(:,i));
end

Sylvain CALINON's avatar
Sylvain CALINON committed
109 110 111 112 113 114 115 116
%Display of covariance contours on the tangent space
Gdisp2 = zeros(model.nbVarMan, nbDrawingSeg, model.nbStates);
for i=1:model.nbStates
	[V,D] = eig(model.Sigma(:,:,i));
	S = blkdiag(real(V*D.^.5), 1);
	Gdisp2(:,:,i) = rotM(model.MuMan(:,i))' * S * [cos(tl); sin(tl); zeros(1,nbDrawingSeg)] + repmat(model.MuMan(:,i),1,nbDrawingSeg);
end

117
%Manifold plot
Sylvain CALINON's avatar
Sylvain CALINON committed
118 119
figure('position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; 
colormap([.8 .8 .8]);
120 121 122 123 124 125 126 127
[X,Y,Z] = sphere(20);
mesh(X,Y,Z);
%plot3(x(1,:), x(2,:), x(3,:), '--','markersize',12,'color',[0 0 0]);
for t=1:nbData*nbSamples
	plot3(x(1,t), x(2,t), x(3,t), '.','markersize',12,'color',GAMMA(:,t)'*clrmap);
end
for i=1:model.nbStates
	plot3(model.MuMan(1,i), model.MuMan(2,i), model.MuMan(3,i), '.','markersize',20,'color',clrmap(i,:));
Sylvain CALINON's avatar
Sylvain CALINON committed
128
	plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',2,'color',clrmap(i,:));
129 130 131
% 	%Draw tangent plane
% 	msh = repmat(model.MuMan(:,i),1,5) + rotM(model.MuMan(:,i))' * [1 1 -1 -1 1; 1 -1 -1 1 1; 0 0 0 0 0] * 1E0;
% 	patch(msh(1,:),msh(2,:),msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3);
Sylvain CALINON's avatar
Sylvain CALINON committed
132
% 	plot3(Gdisp2(1,:,i), Gdisp2(2,:,i), Gdisp2(3,:,i), '-','linewidth',1,'color',clrmap(i,:));
133
end
Sylvain CALINON's avatar
Sylvain CALINON committed
134 135 136
view(-20,15); axis equal; axis tight; axis vis3d;  
% print('-dpng','graphs/Riemannian_sphere_GMM01a.png');

137

Sylvain CALINON's avatar
Sylvain CALINON committed
138 139 140
%% Tangent space plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[670,10,1950,650]); 
141
for i=1:model.nbStates
Sylvain CALINON's avatar
Sylvain CALINON committed
142 143 144 145 146
	clrmap2 = ones(model.nbStates) .* .6;
	clrmap2(i,:) = clrmap(i,:);
	%subplot(ceil(model.nbStates/2),2,i); hold on; axis off; %title(['k=' num2str(i)]);
	subplot(1,model.nbStates,i); hold on; axis off; %title(['k=' num2str(i)]);
	plot(0,0,'+','markersize',40,'linewidth',2,'color',[.7 .7 .7]);
147 148
	%plot(u(1,:,i), u(2,:,i), '.','markersize',6,'color',[0 0 0]);
	for t=1:nbData*nbSamples
Sylvain CALINON's avatar
Sylvain CALINON committed
149
		plot(u(1,t,i), u(2,t,i), '.','markersize',12,'color',GAMMA(:,t)'*clrmap2); %w(1)*[1 0 0] + w(2)*[0 1 0]
150 151 152 153
	end
	plotGMM(model.Mu(:,i), model.Sigma(:,:,i)*3, clrmap(i,:), .3);
	axis equal; axis tight;
	
Sylvain CALINON's avatar
Sylvain CALINON committed
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178
% 	%Plot contours of Gaussian j in tangent plane of Gaussian i (version 1)
% 	for j=1:model.nbStates
% 		if j~=i
% 			udisp = logmap(Gdisp(:,:,j), model.MuMan(:,i));
% 			patch(udisp(1,:), udisp(2,:), clrmap(j,:),'lineWidth',1,'EdgeColor',clrmap(j,:)*0.5,'facealpha',.1,'edgealpha',.1);	
% 		end
% 	end

% 	%Plot contours of Gaussian j in tangent plane of Gaussian i (version 2)
% 	for j=1:model.nbStates
% 		if j~=i
% 			%Computing the eigenvectors of Gaussian j
% 			[V,D] = eig(model.Sigma(:,:,j));
% 			U0 = V * D.^.5;
% 			xU0 = expmap(U0, model.MuMan(:,j));
% 			%Transportation of Mu
% 			uMuTmp = logmap(model.MuMan(:,j), model.MuMan(:,i));
% 			%Transportation of eigenvectors and reconstruction of the corresponding Sigma
% 			U = logmap(xU0, model.MuMan(:,i));
% 			Uc = U - repmat(uMuTmp,1,model.nbVar);
% 			SigmaTmp = Uc * Uc';
% 			plotGMM(uMuTmp, SigmaTmp*3, clrmap(j,:), .1);
% 		end
% 	end
		
179 180
end

Sylvain CALINON's avatar
Sylvain CALINON committed
181 182 183 184 185 186 187 188
% subplot(2,2,4); hold on; axis off;
% u0 = logmap(x, [0; 0; 1]);
% plot(u0(1,:), u0(2,:), '.','markersize',6,'color',[.5 .5 .5]);

% print('-dpng','graphs/Riemannian_sphere_GMM01b.png');

pause;
close all;
189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216
end


%% Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x = expmap(u, mu)
	x = rotM(mu)' * expfct(u);
end

function u = logmap(x, mu)
	if norm(mu-[0;0;-1])<1e-6
		R = [1 0 0; 0 -1 0; 0 0 -1];
	else
		R = rotM(mu);
	end
	u = logfct(R*x);
end

function Exp = expfct(u)
	normv = sqrt(u(1,:).^2+u(2,:).^2);
	Exp = real([u(1,:).*sin(normv)./normv; u(2,:).*sin(normv)./normv; cos(normv)]);
	Exp(:,normv < 1e-16) = repmat([0;0;1],1,sum(normv < 1e-16));	
end

function Log = logfct(x)
	scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2);
	scale(isnan(scale)) = 1;
	Log = [x(1,:).*scale; x(2,:).*scale];	
Sylvain CALINON's avatar
Sylvain CALINON committed
217
end