demo_Riemannian_S2_GMR04.m 8.15 KB
Newer Older
Sylvain CALINON's avatar
Sylvain CALINON committed
1 2
function demo_Riemannian_S2_GMR04
% GMR with input data on a sphere and output data in Euclidean space by relying on Riemannian manifold. 
3
%
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 40 41 42 43 44

addpath('./m_fcts/');


%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 100; %Number of datapoints
nbSamples = 5; %Number of demonstrations
nbIter = 10; %Number of iteration for the Gauss Newton algorithm
nbIterEM = 20; %Number of iteration for the EM algorithm
nbDrawingSeg = 30; %Number of segments used to draw ellipsoids

Sylvain CALINON's avatar
Sylvain CALINON committed
45
model.nbStates = 3; %Number of states in the GMM
46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 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
model.nbVar = 5; %Dimension of the tangent space (2D input + 3D output)
model.nbVarMan = 6; %Dimension of the manifold (3D input + 3D output)
model.params_diagRegFact = 1E-2; %Regularization of covariance


%% Generate input data on a sphere and Euclidean output data from handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Generate input data
demos=[];
load('data/2Dletters/U.mat');
uIn=[];
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
	uIn = [uIn [s(n).Data*1.3E-1]]; 
end
xIn = expmap(uIn, [0; -1; 0]);
%Generate output data
demos=[];
load('data/2Dletters/C.mat');
uOut=[];
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
	uOut = [uOut [s(n).Data([1:end,end],:)*1.3E-1]]; 
end
xOut = uOut;
x = [xIn; xOut];
u = [uIn; uOut];


%% GMM parameters estimation (joint distribution with sphere as input, Euclidean space as output)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = init_GMM_kbins(u, model, nbSamples);
model.MuMan = [expmap(model.Mu(1:2,:), [0; -1; 0]); model.Mu(3:5,:)]; %Center on the manifold
model.Mu = zeros(model.nbVar,model.nbStates); %Center in the tangent plane at point MuMan of the manifold

uTmp = zeros(model.nbVar,nbData*nbSamples,model.nbStates);
for nb=1:nbIterEM
	%E-step
	L = zeros(model.nbStates,size(x,2));
	for i=1:model.nbStates
		uTmp(:,:,i) = [logmap(xIn, model.MuMan(1:3,i)); xOut-repmat(model.MuMan(4:6,i),1,nbData*nbSamples)];
		L(i,:) = model.Priors(i) * gaussPDF(uTmp(:,:,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
			uTmp(:,:,i) = [logmap(xIn, model.MuMan(1:3,i)); xOut-repmat(model.MuMan(4:6,i),1,nbData*nbSamples)];
Sylvain CALINON's avatar
Sylvain CALINON committed
98
			model.MuMan(:,i) = [expmap(uTmp(1:2,:,i)*H(i,:)', model.MuMan(1:3,i)); (uTmp(3:5,:,i)+repmat(model.MuMan(4:6,i),1,nbData*nbSamples))*H(i,:)']; 
99 100 101 102 103 104 105 106 107 108 109 110 111 112
		end
		%Update Sigma
		model.Sigma(:,:,i) = uTmp(:,:,i) * diag(H(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact;
	end
end


%% GMR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
in=1:2; out=3:5; 
inMan=1:3; outMan=4:6;
nbVarOut = length(out);
nbVarOutMan = length(outMan);

Sylvain CALINON's avatar
Sylvain CALINON committed
113 114 115 116 117 118
% %Adding artificial noise on the inputs
% xIn(:,1:nbData) = xIn(:,1:nbData) + randn(3,nbData)*2E-1; 
% for t=1:nbData
% 	xIn(:,t) = xIn(:,t) / norm(xIn(:,t));
% end

119
xhat = zeros(nbVarOutMan,nbData);
Sylvain CALINON's avatar
Sylvain CALINON committed
120
uOut = zeros(nbVarOut,model.nbStates,nbData);
121 122 123
SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates);
expSigma = zeros(nbVarOut,nbVarOut,nbData);

Sylvain CALINON's avatar
Sylvain CALINON committed
124
%VERSION with single optimization loop
125 126 127 128 129 130 131 132 133 134 135
for t=1:nbData
	%Compute activation weight
	for i=1:model.nbStates
		H(i,t) = model.Priors(i) * gaussPDF(logmap(xIn(:,t), model.MuMan(inMan,i)), model.Mu(in,i), model.Sigma(in,in,i));
	end
	H(:,t) = H(:,t) / sum(H(:,t)+realmin); 

	%Compute conditional mean (with covariance transportation)
	for i=1:model.nbStates
		%Transportation of covariance (with both input and output components)
		AcIn = transp(model.MuMan(inMan,i), xIn(:,t));
Sylvain CALINON's avatar
Sylvain CALINON committed
136
		SigmaTmp(:,:,i) = blkdiag(AcIn,eye(3)) * model.Sigma(:,:,i) * blkdiag(AcIn,eye(3))';
137
		%Gaussian conditioning on the tangent space
Sylvain CALINON's avatar
Sylvain CALINON committed
138
		uOut(:,i,t) = model.MuMan(outMan,i) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * logmap(model.MuMan(inMan,i), xIn(:,t)); 
139
	end
Sylvain CALINON's avatar
Sylvain CALINON committed
140 141 142 143 144 145
	xhat(:,t) = uOut(:,:,t) * H(:,t);
	
% 	%Compute conditional covariances (by ignoring influence of centers uOut(:,i,t))
% 	for i=1:model.nbStates
% 		expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp(out,out,i) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * SigmaTmp(in,out,i));
% 	end
146
	
Sylvain CALINON's avatar
Sylvain CALINON committed
147
	%Compute conditional covariances (note that since uhat=0, the final part in the GMR computation is dropped)
148
	for i=1:model.nbStates
Sylvain CALINON's avatar
Sylvain CALINON committed
149 150
		SigmaOutTmp = SigmaTmp(out,out,i) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * SigmaTmp(in,out,i);
		expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaOutTmp + uOut(:,i,t) * uOut(:,i,t)');
151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
	end
end


%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clrmap = lines(model.nbStates);
[X,Y,Z] = sphere(20);

%Display of covariance contours on the sphere
tl = linspace(-pi, pi, nbDrawingSeg);
GdispIn = zeros(nbVarOutMan, nbDrawingSeg, model.nbStates);
for i=1:model.nbStates
	%in
	[V,D] = eig(model.Sigma(1:2,1:2,i));
	GdispIn(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(1:3,i));
end
	
%Manifold plot
Sylvain CALINON's avatar
Sylvain CALINON committed
170
figure('PaperPosition',[0 0 12 6],'position',[10,10,1350,650],'name','manifold data'); rotate3d on; 
171 172 173 174
colormap([.9 .9 .9]);
%in
subplot(1,2,1); hold on; axis off; grid off;
mesh(X,Y,Z);
Sylvain CALINON's avatar
Sylvain CALINON committed
175 176
plot3(x(1,:), x(2,:), x(3,:), '.','markersize',6,'color',[.4 .4 .4]);
plot3(xIn(1,1:nbData), xIn(2,1:nbData), xIn(3,1:nbData), '-','linewidth',3,'color',[0 0 0]);
177 178 179 180 181 182 183
for i=1:model.nbStates
	plot3(model.MuMan(1,i), model.MuMan(2,i), model.MuMan(3,i), '.','markersize',20,'color',clrmap(i,:));
	plot3(GdispIn(1,:,i), GdispIn(2,:,i), GdispIn(3,:,i), '-','linewidth',1,'color',clrmap(i,:));
end
view(-20,2); axis equal; axis vis3d; 
%out
subplot(1,2,2); hold on; axis off; grid off;
Sylvain CALINON's avatar
Sylvain CALINON committed
184
plot3(x(4,:), x(5,:), x(6,:), '.','markersize',6,'color',[.4 .4 .4]);
185 186 187
for i=1:model.nbStates
	plotGMM3D(model.MuMan(4:6,i), model.Sigma(3:5,3:5,i), clrmap(i,:), .1);
end
Sylvain CALINON's avatar
Sylvain CALINON committed
188
plot3(xhat(1,:), xhat(2,:), xhat(3,:), '-','linewidth',3,'color',[.8 0 0]);
189 190 191
view(-20,2); axis equal; axis vis3d;  
%print('-dpng','graphs/demo_Riemannian_sphere_GMR04.png');

Sylvain CALINON's avatar
Sylvain CALINON committed
192 193
pause;
close all;
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 221 222 223 224 225 226 227 228 229 230
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];	
end

function Ac = transp(g,h)
	E = [eye(2); zeros(1,2)];
	vm = rotM(g)' * [logmap(h,g); 0];
	mn = norm(vm);
	uv = vm / (mn+realmin);
	Rpar = eye(3) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv');	
	Ac = E' * rotM(h) * Rpar * rotM(g)' * E; %Transportation operator from g to h 
Sylvain CALINON's avatar
Sylvain CALINON committed
231
end