demo_Riemannian_S2_GMM01.m 8.04 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
% 	author="Calinon, S.",
7
% 	title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
Sylvain CALINON's avatar
Sylvain CALINON committed
8 9
% 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% 	year="2020",
Sylvain CALINON's avatar
Sylvain CALINON committed
10 11 12 13 14
% 	month="June",
% 	volume="27",
% 	number="2",
% 	pages="33--45",
% 	doi="10.1109/MRA.2020.2980548"
15 16
% }
% 
Sylvain CALINON's avatar
Sylvain CALINON committed
17 18
% Copyright (c) 2019 Idiap Research Institute, https://idiap.ch/
% Written by Sylvain Calinon, https://calinon.ch/
19
% 
Sylvain CALINON's avatar
Sylvain CALINON committed
20
% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
21 22 23 24 25 26 27 28 29 30 31
% 
% 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
Sylvain CALINON's avatar
Sylvain CALINON committed
32
% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
33 34 35 36 37 38 39

addpath('./m_fcts/');


%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 50; %Number of datapoints
Sylvain CALINON's avatar
Sylvain CALINON committed
40
nbSamples = 8; %Number of demonstrations
41 42 43 44
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

45
model.nbStates = 1; %Number of states in the GMM
46 47 48
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
49
x0 = [0; -1; 0]; %Point on the sphere to project handwriting data
50 51 52 53 54


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


%% GMM parameters estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Sylvain CALINON's avatar
Sylvain CALINON committed
66 67 68 69 70 71 72
% %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
73
model = init_GMM_kbins(u, model, nbSamples);
Sylvain CALINON's avatar
Sylvain CALINON committed
74 75
% model = init_GMM_kmeans(u, model);
model.MuMan = expmap(model.Mu, x0); %Center on the manifold
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 109 110 111 112
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
113 114 115 116 117 118 119 120
%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

121
%Manifold plot
Sylvain CALINON's avatar
Sylvain CALINON committed
122 123
figure('position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; 
colormap([.8 .8 .8]);
124 125 126 127 128 129 130 131
[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
132
	plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',2,'color',clrmap(i,:));
133 134 135
% 	%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
136
% 	plot3(Gdisp2(1,:,i), Gdisp2(2,:,i), Gdisp2(3,:,i), '-','linewidth',1,'color',clrmap(i,:));
137
end
Sylvain CALINON's avatar
Sylvain CALINON committed
138 139 140
view(-20,15); axis equal; axis tight; axis vis3d;  
% print('-dpng','graphs/Riemannian_sphere_GMM01a.png');

141

Sylvain CALINON's avatar
Sylvain CALINON committed
142 143 144
%% Tangent space plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[670,10,1950,650]); 
145
for i=1:model.nbStates
Sylvain CALINON's avatar
Sylvain CALINON committed
146 147 148 149 150
	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]);
151 152
	%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
153
		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]
154 155 156 157
	end
	plotGMM(model.Mu(:,i), model.Sigma(:,:,i)*3, clrmap(i,:), .3);
	axis equal; axis tight;
	
Sylvain CALINON's avatar
Sylvain CALINON committed
158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182
% 	%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
		
183 184
end

Sylvain CALINON's avatar
Sylvain CALINON committed
185 186 187 188 189 190 191 192
% 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;
193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220
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
221
end