diff --git a/demo_Riemannian_quat_GMM01.m b/demo_Riemannian_quat_GMM01.m new file mode 100644 index 0000000000000000000000000000000000000000..46c0a56769c4fd8266352b087a743ef04a7a9aee --- /dev/null +++ b/demo_Riemannian_quat_GMM01.m @@ -0,0 +1,200 @@ +function demo_Riemannian_quat_GMM01 +% GMM for unit quaternion data by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/ +% Written by Sylvain Calinon and Joao Silverio +% +% 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 5; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm +nbDrawingSeg = 20; %Number of segments used to draw ellipsoids + +model.nbStates = 4; %Number of states in the GMM +model.nbVar = 3; %Dimension of the tangent space +model.nbVarMan = 4; %Dimension of the manifold +model.params_diagRegFact = 1E-4; %Regularization of covariance + + +%% Generate artificial unit quaternion data from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/2Dletters/S.mat'); +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 + u = [u, s(n).Data([1:end,end],:)*2E-2]; +end +x = expmap(u, [0; 1; 0; 0]); + + +%% GMM parameters estimation +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = expmap(model.Mu, [0; 1; 0; 0]); %Center on the manifold +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); + GAMMA2 = 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)*GAMMA2(i,:)', model.MuMan(:,i)); + end + %Update Sigma + model.Sigma(:,:,i) = u(:,:,i) * diag(GAMMA2(i,:)) * u(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Display of covariance contours on the sphere +t = 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(t); sin(t); zeros(1,nbDrawingSeg)], model.MuMan(:,i)); +end + +clrmap = lines(model.nbStates); +%Tangent space plot +figure('PaperPosition',[0 0 6 8],'position',[10,10,1300,650]); +for i=1:model.nbStates + subplot(ceil(model.nbStates/2),2,i); hold on; axis off; title(['k=' num2str(i)]); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + %plot(u(1,:,i), u(2,:,i), '.','markersize',6,'color',[0 0 0]); + for t=1:nbData*nbSamples + plot(u(1,t,i), u(2,t,i), '.','markersize',6,'color',GAMMA(:,t)'*clrmap); %w(1)*[1 0 0] + w(2)*[0 1 0] + end + plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i)*3, clrmap(i,:), .3); + axis equal; axis tight; + + %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 + +end +%print('-dpng','graphs/demo_Riemannian_quat_GMM01.png'); + +% pause; +% close all; +end + + + +%% Functions +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function x = expmap(u, mu) + x = QuatMatrix(mu) * expfct(u); +end + +function u = logmap(x, mu) + if norm(mu-[1;0;0;0])<1e-6 + Q = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; + else + Q = QuatMatrix(mu); + end + u = logfct(Q'*x); +end + +function Exp = expfct(u) + normv = sqrt(u(1,:).^2+u(2,:).^2+u(3,:).^2); + Exp = real([cos(normv) ; u(1,:).*sin(normv)./normv ; u(2,:).*sin(normv)./normv ; u(3,:).*sin(normv)./normv]); + Exp(:,normv < 1e-16) = repmat([1;0;0;0],1,sum(normv < 1e-16)); +end + +function Log = logfct(x) +% scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2); + scale = acoslog(x(1,:)) ./ sqrt(1-x(1,:).^2); + scale(isnan(scale)) = 1; + Log = [x(2,:).*scale; x(3,:).*scale; x(4,:).*scale]; +end + +function Q = QuatMatrix(q) + Q = [q(1) -q(2) -q(3) -q(4); + q(2) q(1) -q(4) q(3); + q(3) q(4) q(1) -q(2); + q(4) -q(3) q(2) q(1)]; +end + +% Arcosine re-defitinion to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis) +function acosx = acoslog(x) + for n=1:size(x,2) + % sometimes abs(x) is not exactly 1.0 + if(x(n)>=1.0) + x(n) = 1.0; + end + if(x(n)<=-1.0) + x(n) = -1.0; + end + if(x(n)>=-1.0 && x(n)<0) + acosx(n) = acos(x(n))-pi; + else + acosx(n) = acos(x(n)); + end + end +end + +function Ac = transp(g,h) + E = [zeros(1,3); eye(3)]; + vm = QuatMatrix(g) * [0; logmap(h,g)]; + mn = norm(vm); + if mn < 1e-10 + disp('Angle of rotation too small (<1e-10)'); + Ac = eye(3); + return; + end + uv = vm / mn; + Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv'); + Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h +end diff --git a/demo_Riemannian_quat_GMR01.m b/demo_Riemannian_quat_GMR01.m new file mode 100644 index 0000000000000000000000000000000000000000..78e8f669ce3cdbc9f1191069f912dc218a285a42 --- /dev/null +++ b/demo_Riemannian_quat_GMR01.m @@ -0,0 +1,265 @@ +function demo_Riemannian_quat_GMR01 +% GMR with unit quaternions as input and output data by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 5; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm + +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 6; %Dimension of the tangent space (input+output) +model.nbVarMan = 8; %Dimension of the manifold (input+output) +model.params_diagRegFact = 1E-4; %Regularization of covariance + + +%% Generate input and output data as unit quaternions 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:end,end],:)*2E-2]]; +end +xIn = expmap(uIn, [0; 1; 0; 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],:)*2E-2]]; +end +xOut = expmap(uOut, [0; 1; 0; 0]); + +x = [xIn; xOut]; +u = [uIn; uOut]; + + +%% GMM parameters estimation (joint distribution with sphere as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [expmap(model.Mu(1:3,:), [0; 1; 0; 0]); expmap(model.Mu(4:6,:), [0; 1; 0; 0])]; %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:4,i)); logmap(xOut, model.MuMan(5:8,i))]; + 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); + GAMMA2 = 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:4,i)); logmap(xOut, model.MuMan(5:8,i))]; + model.MuMan(:,i) = [expmap(uTmp(1:3,:,i)*GAMMA2(i,:)', model.MuMan(1:4,i)); expmap(uTmp(4:6,:,i)*GAMMA2(i,:)', model.MuMan(5:8,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(GAMMA2(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1:3; out=4:6; +inMan=1:4; outMan=5:8; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +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); %eq.(12) + + %Compute conditional mean (with covariance transportation) + [~,id] = max(H(:,t)); + if t==1 + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from g to h (with both input and output compoments) + AcIn = transp(model.MuMan(inMan,i), xIn(:,t)); + AcOut = transp(model.MuMan(outMan,i), xhat(:,t)); + U1 = blkdiag(AcIn,AcOut) * U0(:,:,i); + SigmaTmp(:,:,i) = U1 * U1'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * logmap(model.MuMan(inMan,i), xIn(:,t)); + end + xhat(:,t) = expmap(uOutTmp(:,:,t)*H(:,t), xhat(:,t)); + end + + %Compute conditional covariances + 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 +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); + +%Timeline plot +figure('PaperPosition',[0 0 6 8],'position',[10,10,650,650],'name','timeline data'); +for k=1:4 + subplot(2,2,k); hold on; + for n=1:nbSamples + plot(1:nbData, x(4+k,(n-1)*nbData+1:n*nbData), '-','color',[.6 .6 .6]); + end + plot(1:nbData, xhat(k,:), '-','linewidth',2,'color',[.8 0 0]); + xlabel('t'); ylabel(['q_' num2str(k)]); +end + +%Tangent space plot +figure('PaperPosition',[0 0 12 4],'position',[670,10,650,650],'name','tangent space data'); +for i=1:model.nbStates + %in + subplot(2,model.nbStates,i); hold on; axis off; title(['k=' num2str(i) ', input space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(1,:,i), uTmp(2,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i)*3, clrmap(i,:), .3); + axis equal; axis tight; + %out + subplot(2,model.nbStates,model.nbStates+i); hold on; axis off; title(['k=' num2str(i) ', output space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(3,:,i), uTmp(4,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(3:4,i), model.Sigma(3:4,3:4,i), clrmap(i,:), .3); + axis equal; axis tight; +end +%print('-dpng','graphs/demo_Riemannian_quat_GMR01.png'); + +% pause; +% close all; +end + + +%% Functions +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function x = expmap(u, mu) + x = QuatMatrix(mu) * expfct(u); +end + +function u = logmap(x, mu) + if norm(mu-[1;0;0;0])<1e-6 + Q = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; + else + Q = QuatMatrix(mu); + end + u = logfct(Q'*x); +end + +function Exp = expfct(u) + normv = sqrt(u(1,:).^2+u(2,:).^2+u(3,:).^2); + Exp = real([cos(normv) ; u(1,:).*sin(normv)./normv ; u(2,:).*sin(normv)./normv ; u(3,:).*sin(normv)./normv]); + Exp(:,normv < 1e-16) = repmat([1;0;0;0],1,sum(normv < 1e-16)); +end + +function Log = logfct(x) +% scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2); + scale = acoslog(x(1,:)) ./ sqrt(1-x(1,:).^2); + scale(isnan(scale)) = 1; + Log = [x(2,:).*scale; x(3,:).*scale; x(4,:).*scale]; +end + +function Q = QuatMatrix(q) + Q = [q(1) -q(2) -q(3) -q(4); + q(2) q(1) -q(4) q(3); + q(3) q(4) q(1) -q(2); + q(4) -q(3) q(2) q(1)]; +end + +% Arcosine re-defitinion to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis) +function acosx = acoslog(x) + for n=1:size(x,2) + % sometimes abs(x) is not exactly 1.0 + if(x(n)>=1.0) + x(n) = 1.0; + end + if(x(n)<=-1.0) + x(n) = -1.0; + end + if(x(n)>=-1.0 && x(n)<0) + acosx(n) = acos(x(n))-pi; + else + acosx(n) = acos(x(n)); + end + end +end + +function Ac = transp(g,h) + E = [zeros(1,3); eye(3)]; + vm = QuatMatrix(g) * [0; logmap(h,g)]; + mn = norm(vm); + if mn < 1e-10 + disp('Angle of rotation too small (<1e-10)'); + Ac = eye(3); + return; + end + uv = vm / mn; + Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv'); + Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h +end diff --git a/demo_Riemannian_quat_GMR02.m b/demo_Riemannian_quat_GMR02.m new file mode 100644 index 0000000000000000000000000000000000000000..dc9b40c3fdb848f23935655bb136675cae000643 --- /dev/null +++ b/demo_Riemannian_quat_GMR02.m @@ -0,0 +1,252 @@ +function demo_Riemannian_quat_GMR02 +% GMR with time as input and unit quaternion as output by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 4; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm + +model.nbStates = 5; %Number of states in the GMM +model.nbVar = 4; %Dimension of the tangent space (incl. time) +model.nbVarMan = 5; %Dimension of the manifold (incl. time) +model.dt = 0.01; %Time step duration +model.params_diagRegFact = 1E-4; %Regularization of covariance + + +%% Generate artificial unit quaternion as output data from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/2Dletters/S.mat'); +uIn=[]; 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],:)*2E-2]; + uIn = [uIn, [1:nbData]*model.dt]; +end +xOut = expmap(uOut, [0; 1; 0; 0]); +%xOut = expmap(uOut, e0); +xIn = uIn; +u = [uIn; uOut]; +x = [xIn; xOut]; + + +%% GMM parameters estimation (joint distribution with time as input, unit quaternion as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [model.Mu(1,:); expmap(model.Mu(2:end,:), [0; 1; 0; 0])]; %Center on the manifold %Data(1,nbData/2) +%model.MuMan = [model.Mu(1,:); expmap(model.Mu(2:end,:), e0)]; %Center on the manifold %Data(1,nbData/2) +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 + L(i,:) = model.Priors(i) * gaussPDF([xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:end,i))], model.Mu(:,i), model.Sigma(:,:,i)); + end + GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1); + GAMMA2 = 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) = [xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:end,i))]; + model.MuMan(:,i) = [(model.MuMan(1,i)+uTmp(1,:,i))*GAMMA2(i,:)'; expmap(uTmp(2:end,:,i)*GAMMA2(i,:)', model.MuMan(2:end,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(GAMMA2(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1; out=2:4; outMan=2:5; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +uhat = zeros(nbVarOut,nbData); +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +for t=1:nbData + %Compute activation weight + for i=1:model.nbStates + H(i,t) = model.Priors(i) * gaussPDF(xIn(:,t)-model.MuMan(in,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) + if t==1 + [~,id] = max(H(:,t)); + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(outMan,i) to xhat(:,t) + Ac = transp(model.MuMan(outMan,i), xhat(:,t)); + U = blkdiag(1, Ac) * U0(:,:,i); %First variable in Euclidean space + SigmaTmp(:,:,i) = U * U'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) + SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * (xIn(:,t)-model.MuMan(in,i)); + end + uhat(:,t) = uOutTmp(:,:,t) * H(:,t); + xhat(:,t) = expmap(uhat(:,t), xhat(:,t)); + end + + %Compute conditional covariances + 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 +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); + +%Timeline plot +figure('PaperPosition',[0 0 6 8],'position',[10,10,650,650],'name','timeline data'); +for k=1:4 + subplot(2,2,k); hold on; + for n=1:nbSamples + plot(x(1,(n-1)*nbData+1:n*nbData), x(1+k,(n-1)*nbData+1:n*nbData), '-','color',[.6 .6 .6]); + end + plot(x(1,1:nbData), xhat(k,:), '-','linewidth',2,'color',[.8 0 0]); + xlabel('t'); ylabel(['q_' num2str(k)]); +end + +%Tangent space plot +figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650],'name','tangent space data'); +for i=1:model.nbStates + subplot(ceil(model.nbStates/2),2,i); hold on; axis off; title(['k=' num2str(i) ', output space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(2,:,i), uTmp(3,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(2:3,i), model.Sigma(2:3,2:3,i)*3, clrmap(i,:), .3); + axis equal; +end +%print('-dpng','graphs/demo_Riemannian_quat_GMR02.png'); + +% pause; +% close all; +end + + +%% Functions +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function x = expmap(u, mu) + x = QuatMatrix(mu) * expfct(u); +end + +function u = logmap(x, mu) + if norm(mu-[1;0;0;0])<1e-6 + Q = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; + else + Q = QuatMatrix(mu); + end + u = logfct(Q'*x); +end + +function Exp = expfct(u) + normv = sqrt(u(1,:).^2+u(2,:).^2+u(3,:).^2); + Exp = real([cos(normv) ; u(1,:).*sin(normv)./normv ; u(2,:).*sin(normv)./normv ; u(3,:).*sin(normv)./normv]); + Exp(:,normv < 1e-16) = repmat([1;0;0;0],1,sum(normv < 1e-16)); +end + +function Log = logfct(x) +% scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2); + scale = acoslog(x(1,:)) ./ sqrt(1-x(1,:).^2); + scale(isnan(scale)) = 1; + Log = [x(2,:).*scale; x(3,:).*scale; x(4,:).*scale]; +end + +function Q = QuatMatrix(q) + Q = [q(1) -q(2) -q(3) -q(4); + q(2) q(1) -q(4) q(3); + q(3) q(4) q(1) -q(2); + q(4) -q(3) q(2) q(1)]; +end + +% Arcosine re-defitinion to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis) +function acosx = acoslog(x) + for n=1:size(x,2) + % sometimes abs(x) is not exactly 1.0 + if(x(n)>=1.0) + x(n) = 1.0; + end + if(x(n)<=-1.0) + x(n) = -1.0; + end + if(x(n)>=-1.0 && x(n)<0) + acosx(n) = acos(x(n))-pi; + else + acosx(n) = acos(x(n)); + end + end +end + +function Ac = transp(g,h) + E = [zeros(1,3); eye(3)]; + vm = QuatMatrix(g) * [0; logmap(h,g)]; + mn = norm(vm); + if mn < 1e-10 + disp('Angle of rotation too small (<1e-10)'); + Ac = eye(3); + return; + end + uv = vm / mn; + Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv'); + Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h +end + diff --git a/demo_Riemannian_quat_vecTransp01.m b/demo_Riemannian_quat_vecTransp01.m new file mode 100644 index 0000000000000000000000000000000000000000..0fb3540b5864e669d0e64a3c65822eaada8f129c --- /dev/null +++ b/demo_Riemannian_quat_vecTransp01.m @@ -0,0 +1,182 @@ +function demo_Riemannian_quat_vecTransp01 +% Parallel transport for unit quaternions. +% +% 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 3; %Dimension of the tangent space +model.nbVarMan = 4; %Dimension of the manifold + + +%% Setting GMM parameters manually +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.Priors = ones(1,model.nbStates) / model.nbStates; + +%model.MuMan = randn(model.nbVarMan,model.nbStates); +model.MuMan(:,1) = [1; -.5; 0; .1]; +model.MuMan(:,2) = [0; -1; .5; .2]; +for i=1:model.nbStates + model.MuMan(:,i) = model.MuMan(:,i) / norm(model.MuMan(:,i)); +end + +model.Mu = zeros(model.nbVar,model.nbStates); + +model.Sigma(:,:,1) = diag([2,4,3]) * 5E-2; +model.Sigma(:,:,2) = diag([2,4,3]) * 5E-2; + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% Transportation of covariance +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +g = model.MuMan(:,1); +h = model.MuMan(:,2); + +tl = linspace(0,1,20); + +for n=1:20 + t = tl(n); + + hist(n).MuMan = expmap(logmap(h,g)*t, g); + + Ac = transp(g, hist(n).MuMan); + hist(n).U = Ac * U0(:,:,1); + hist(n).Sigma = hist(n).U * hist(n).U'; + + % Direction of the geodesic + hist(n).dirG = logmap(h, hist(n).MuMan); + if norm(hist(n).dirG) > 1E-5 + % Normalise the direction + hist(n).dirG = hist(n).dirG ./ norm(hist(n).dirG); + % Compute the inner product with the first eigenvector + inprod(n) = hist(n).dirG' * hist(n).U(:,1); + end + +end + +% %Check that the two vectors below are the same +% p1 = -logmap(g,h); +% p2 = E' * rotM(h) * Rpar * rotM(g)' * E * logmap(h,g); +% norm(p1-p2) +% +% %Check that the transported eigenvectors remain orthogonal +% hist(end).U(:,1)' * hist(end).U(:,2) + +% Check that the inner product between the direction of the geodesic and +% parallel transported vectors (e.g. eigenvectors) is conserved. +inprod + +end + + +%% Functions +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function x = expmap(u, mu) + x = QuatMatrix(mu) * expfct(u); +end + +function u = logmap(x, mu) + if norm(mu-[1;0;0;0])<1e-6 + Q = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; + else + Q = QuatMatrix(mu); + end + u = logfct(Q'*x); +end + +function Exp = expfct(u) + normv = sqrt(u(1,:).^2+u(2,:).^2+u(3,:).^2); + Exp = real([cos(normv) ; u(1,:).*sin(normv)./normv ; u(2,:).*sin(normv)./normv ; u(3,:).*sin(normv)./normv]); + Exp(:,normv < 1e-16) = repmat([1;0;0;0],1,sum(normv < 1e-16)); +end +% v1 = cos(norm(u)/2); +% v2 = sin(norm(u)/2) * u/norm(u); +% q = Quaternion([v1 v2]); + +function Log = logfct(x) +% scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2); + scale = acoslog(x(1,:)) ./ sqrt(1-x(1,:).^2); + scale(isnan(scale)) = 1; + Log = [x(2,:).*scale; x(3,:).*scale; x(4,:).*scale]; +end +% u = 2 * acos(q.s) * q.v/norm(q.v); + + +function Q = QuatMatrix(q) + Q = [q(1) -q(2) -q(3) -q(4); + q(2) q(1) -q(4) q(3); + q(3) q(4) q(1) -q(2); + q(4) -q(3) q(2) q(1)]; +end + +% Arcosine re-defitinion to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis) +function acosx = acoslog(x) + for n=1:size(x,2) + % sometimes abs(x) is not exactly 1.0 + if(x(n)>=1.0) + x(n) = 1.0; + end + if(x(n)<=-1.0) + x(n) = -1.0; + end + if(x(n)>=-1.0 && x(n)<0) + acosx(n) = acos(x(n))-pi; + else + acosx(n) = acos(x(n)); + end + end +end + +function Ac = transp(g,h) + E = [zeros(1,3); eye(3)]; + vm = QuatMatrix(g) * [0; logmap(h,g)]; + mn = norm(vm); + if mn < 1e-10 + disp('Angle of rotation too small (<1e-10)'); + Ac = eye(3); + return; + end + uv = vm / mn; + Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv'); + Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h +end diff --git a/demo_Riemannian_sphere_GMM01.m b/demo_Riemannian_sphere_GMM01.m new file mode 100644 index 0000000000000000000000000000000000000000..3e79c8bfc9758b4f83aad74ae7ebe95de7f0a5a9 --- /dev/null +++ b/demo_Riemannian_sphere_GMM01.m @@ -0,0 +1,178 @@ +function demo_Riemannian_sphere_GMM01 +% GMM for data on a sphere by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 5; %Number of demonstrations +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 + +model.nbStates = 5; %Number of states in the GMM +model.nbVar = 2; %Dimension of the tangent space +model.nbVarMan = 3; %Dimension of the manifold +model.params_diagRegFact = 1E-4; %Regularization of covariance + + +%% Generate data on a sphere from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/2Dletters/S.mat'); +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 + u = [u [s(n).Data*1.3E-1]]; +end +x = expmap(u, [0; -1; 0]); + + +%% GMM parameters estimation +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = expmap(model.Mu, [0; -1; 0]); %Center on the manifold +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 + +%Manifold plot +figure('PaperPosition',[0 0 8 8],'position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; +colormap([.9 .9 .9]); +[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,:)); + plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',1,'color',clrmap(i,:)); +% %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); +end +view(-20,2); axis equal; axis tight; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GMM01a.png'); + +%Tangent space plot +figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650]); +for i=1:model.nbStates + subplot(ceil(model.nbStates/2),2,i); hold on; axis off; title(['k=' num2str(i)]); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + %plot(u(1,:,i), u(2,:,i), '.','markersize',6,'color',[0 0 0]); + for t=1:nbData*nbSamples + plot(u(1,t,i), u(2,t,i), '.','markersize',6,'color',GAMMA(:,t)'*clrmap); %w(1)*[1 0 0] + w(2)*[0 1 0] + end + plotGMM(model.Mu(:,i), model.Sigma(:,:,i)*3, clrmap(i,:), .3); + axis equal; axis tight; + + %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 +end +%print('-dpng','graphs/demo_Riemannian_sphere_GMM01b.png'); + +% pause; +% close all; +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 diff --git a/demo_Riemannian_sphere_GMR01.m b/demo_Riemannian_sphere_GMR01.m new file mode 100644 index 0000000000000000000000000000000000000000..aaefde66ab6e855a64a2512db96df01a2bde79b0 --- /dev/null +++ b/demo_Riemannian_sphere_GMR01.m @@ -0,0 +1,272 @@ +function demo_Riemannian_sphere_GMR01 +% GMR with input and output data on a sphere by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +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 + +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 4; %Dimension of the tangent space (input+output) +model.nbVarMan = 6; %Dimension of the manifold (input+output) +model.params_diagRegFact = 1E-2; %Regularization of covariance + + +%% Generate input and output data on a sphere 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.3E-1]]; +end +xOut = expmap(uOut, [0; -1; 0]); + +x = [xIn; xOut]; +u = [uIn; uOut]; + + +%% GMM parameters estimation (joint distribution with sphere as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [expmap(model.Mu(1:2,:), [0; -1; 0]); expmap(model.Mu(3:4,:), [0; -1; 0])]; %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)); logmap(xOut, model.MuMan(4:6,i))]; + 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)); logmap(xOut, model.MuMan(4:6,i))]; + model.MuMan(:,i) = [expmap(uTmp(1:2,:,i)*H(i,:)', model.MuMan(1:3,i)); expmap(uTmp(3:4,:,i)*H(i,:)', model.MuMan(4:6,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(H(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1:2; out=3:4; +inMan=1:3; outMan=4:6; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +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); %eq.(12) + + %Compute conditional mean (with covariance transportation) + [~,id] = max(H(:,t)); + if t==1 + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance (with both input and output components) + AcIn = transp(model.MuMan(inMan,i), xIn(:,t)); + AcOut = transp(model.MuMan(outMan,i), xhat(:,t)); + Ac = blkdiag(AcIn,AcOut); + SigmaTmp(:,:,i) = Ac * model.Sigma(:,:,i) * Ac'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * logmap(model.MuMan(inMan,i), xIn(:,t)); + end + xhat(:,t) = expmap(uOutTmp(:,:,t)*H(:,t), xhat(:,t)); + end + + %Compute conditional covariances + 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 +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); +GdispOut = 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)); + %out + [V,D] = eig(model.Sigma(3:4,3:4,i)); + GdispOut(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(4:6,i)); +end +Gregr = zeros(nbVarOutMan, nbDrawingSeg, nbData); +for t=1:nbData + [V,D] = eig(expSigma(:,:,t)); + Gregr(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], xhat(:,t)); +end + +%Manifold plot +figure('PaperPosition',[0 0 12 6],'position',[10,10,650,350],'name','manifold data'); rotate3d on; +colormap([.9 .9 .9]); +%in +subplot(1,2,1); hold on; axis off; grid off; +mesh(X,Y,Z); +plot3(x(1,:), x(2,:), x(3,:), '.','markersize',8,'color',[0 0 0]); +plot3(xIn(1,1:nbData), xIn(2,1:nbData), xIn(3,1:nbData), '-','linewidth',3,'color',[0 .8 0]); +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; +mesh(X,Y,Z); +plot3(x(4,:), x(5,:), x(6,:), '.','markersize',8,'color',[0 0 0]); +for i=1:model.nbStates + plot3(model.MuMan(4,i), model.MuMan(5,i), model.MuMan(6,i), '.','markersize',20,'color',clrmap(i,:)); + plot3(GdispOut(1,:,i), GdispOut(2,:,i), GdispOut(3,:,i), '-','linewidth',1,'color',clrmap(i,:)); +end +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '.','markersize',12,'color',[.8 0 0]); +for t=1:nbData + plot3(Gregr(1,:,t), Gregr(2,:,t), Gregr(3,:,t), '-','linewidth',1,'color',[.8 0 0]); +end +view(-20,2); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GMR01a.png'); + +%Tangent plane plot +figure('PaperPosition',[0 0 12 4],'position',[670,10,650,650],'name','tangent space data'); +for i=1:model.nbStates + %in + subplot(2,model.nbStates,i); hold on; axis off; title(['k=' num2str(i) ', input space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(1,:,i), uTmp(2,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(1:2,i), model.Sigma(1:2,1:2,i)*3, clrmap(i,:), .3); + axis equal; axis tight; + %out + subplot(2,model.nbStates,model.nbStates+i); hold on; axis off; title(['k=' num2str(i) ', output space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(3,:,i), uTmp(4,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(3:4,i), model.Sigma(3:4,3:4,i), clrmap(i,:), .3); + axis equal; axis tight; +end +%print('-dpng','graphs/demo_Riemannian_sphere_GMR01b.png'); + +% pause; +% close all; +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 +end + diff --git a/demo_Riemannian_sphere_GMR02.m b/demo_Riemannian_sphere_GMR02.m new file mode 100644 index 0000000000000000000000000000000000000000..0c737b70c38a3d41519c1476cd00fa387339a346 --- /dev/null +++ b/demo_Riemannian_sphere_GMR02.m @@ -0,0 +1,241 @@ +function demo_Riemannian_sphere_GMR02 +% GMR with time as input and spherical data as output by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 4; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm +nbDrawingSeg = 20; %Number of segments used to draw ellipsoids + +model.nbStates = 3; %Number of states in the GMM +model.nbVar = 3; %Dimension of the tangent space (incl. time) +model.nbVarMan = 4; %Dimension of the manifold (incl. time) +model.dt = 0.01; %Time step duration +model.params_diagRegFact = 1E-3; %Regularization of covariance + + +%% Generate output data on a sphere from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/2Dletters/S.mat'); +uIn=[]; 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.3E-1]; + uIn = [uIn, [1:nbData]*model.dt]; +end +xOut = expmap(uOut, [0; -1; 0]); +xIn = uIn; +u = [uIn; uOut]; +x = [xIn; xOut]; + + +%% GMM parameters estimation (joint distribution with time as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [model.Mu(1,:); expmap(model.Mu(2:3,:), [0; -1; 0])]; %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 + L(i,:) = model.Priors(i) * gaussPDF([xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:4,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) = [xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:4,i))]; + model.MuMan(:,i) = [(model.MuMan(1,i)+uTmp(1,:,i))*H(i,:)'; expmap(uTmp(2:3,:,i)*H(i,:)', model.MuMan(2:4,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(H(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1; out=2:3; outMan=2:4; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +uhat = zeros(nbVarOut,nbData); +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +for t=1:nbData + %Compute activation weight + for i=1:model.nbStates + H(i,t) = model.Priors(i) * gaussPDF(xIn(:,t)-model.MuMan(in,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) + if t==1 + [~,id] = max(H(:,t)); + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(outMan,i) to xhat(:,t) + Ac = transp(model.MuMan(outMan,i), xhat(:,t)); + U = blkdiag(1, Ac) * U0(:,:,i); %First variable in Euclidean space + SigmaTmp(:,:,i) = U * U'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) + SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * (xIn(:,t)-model.MuMan(in,i)); + end + uhat(:,t) = uOutTmp(:,:,t) * H(:,t); + xhat(:,t) = expmap(uhat(:,t), xhat(:,t)); + end + + %Compute conditional covariances + 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 +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); +[X,Y,Z] = sphere(20); + +%Display of covariance contours on the sphere +tl = linspace(-pi, pi, nbDrawingSeg); +Gdisp = zeros(nbVarOutMan, nbDrawingSeg, model.nbStates); +for i=1:model.nbStates + [V,D] = eig(model.Sigma(2:3,2:3,i)); + Gdisp(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(2:4,i)); +end +Gregr = zeros(nbVarOutMan, nbDrawingSeg, nbData); +for t=1:nbData + [V,D] = eig(expSigma(:,:,t)); + Gregr(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], xhat(:,t)); +end + +%Manifold plot +figure('PaperPosition',[0 0 8 8],'position',[10,10,650,650],'name','manifold data'); hold on; axis off; grid off; rotate3d on; +colormap([.9 .9 .9]); +mesh(X,Y,Z); +plot3(x(2,:), x(3,:), x(4,:), '.','markersize',8,'color',[0 0 0]); +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '-','linewidth',2,'color',[0 0 0]); +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '.','markersize',12,'color',[0 0 0]); +for t=1:nbData + plot3(Gregr(1,:,t), Gregr(2,:,t), Gregr(3,:,t), '-','linewidth',1,'color',[.5 .5 .5]); +end +for i=1:model.nbStates + plot3(model.MuMan(2,i), model.MuMan(3,i), model.MuMan(4,i), '.','markersize',20,'color',clrmap(i,:)); + plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',2,'color',clrmap(i,:)); +end +view(-20,2); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GMR02a.png'); + +%Tangent plane plot +figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650],'name','tangent space data'); +for i=1:model.nbStates + subplot(ceil(model.nbStates/2),2,i); hold on; axis off; title(['k=' num2str(i) ', output space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(2,:,i), uTmp(3,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(2:3,i), model.Sigma(2:3,2:3,i)*3, clrmap(i,:), .3); + axis equal; +end +%print('-dpng','graphs/demo_Riemannian_sphere_GMR02b.png'); + +% pause; +% close all; +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 +end + + diff --git a/demo_Riemannian_sphere_GMR03.m b/demo_Riemannian_sphere_GMR03.m new file mode 100644 index 0000000000000000000000000000000000000000..9f5b4df4660eea16dc63fc7202f9af66775077b1 --- /dev/null +++ b/demo_Riemannian_sphere_GMR03.m @@ -0,0 +1,260 @@ +function demo_Riemannian_sphere_GMR03 +% GMR with 3D Euclidean data as input and spherical data as output by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 4; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm +nbDrawingSeg = 20; %Number of segments used to draw ellipsoids + +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 5; %Dimension of the tangent space (3D input + 2D output) +model.nbVarMan = 6; %Dimension of the manifold (3D input + 3D output) +model.dt = 0.01; %Time step duration +model.params_diagRegFact = 1E-3; %Regularization of covariance + + +%% Generate input and output data from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Generate 3D Euclidean 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:end,end],:)*1.3E-1]; +end +xIn = uIn; +%Generate spherical 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.3E-1]]; +end +xOut = expmap(uOut, [0; -1; 0]); +u = [uIn; uOut]; +x = [xIn; xOut]; + + +%% GMM parameters estimation (joint distribution with time as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [model.Mu(1:3,:); expmap(model.Mu(4:5,:), [0; -1; 0])]; %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 + L(i,:) = model.Priors(i) * gaussPDF([xIn-repmat(model.MuMan(1:3,i),1,nbData*nbSamples); logmap(xOut, model.MuMan(4:end,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) = [xIn-repmat(model.MuMan(1:3,i),1,nbData*nbSamples); logmap(xOut, model.MuMan(4:end,i))]; + model.MuMan(:,i) = [(uTmp(1:3,:,i)+repmat(model.MuMan(1:3,i),1,nbData*nbSamples))*H(i,:)'; ... + expmap(uTmp(4:end,:,i)*H(i,:)', model.MuMan(4:end,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(H(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1:3; out=4:5; outMan=4:6; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +uhat = zeros(nbVarOut,nbData); +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +for t=1:nbData + %Compute activation weight + for i=1:model.nbStates + H(i,t) = model.Priors(i) * gaussPDF(xIn(:,t)-model.MuMan(in,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) + if t==1 + [~,id] = max(H(:,t)); + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(outMan,i) to xhat(:,t) + Ac = transp(model.MuMan(outMan,i), xhat(:,t)); + U = blkdiag(eye(3), Ac) * U0(:,:,i); %First variable in Euclidean space + SigmaTmp(:,:,i) = U * U'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) + SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * (xIn(:,t)-model.MuMan(in,i)); + end + uhat(:,t) = uOutTmp(:,:,t) * H(:,t); + xhat(:,t) = expmap(uhat(:,t), xhat(:,t)); + end + + %Compute conditional covariances + 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 +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); +[X,Y,Z] = sphere(20); + +%Display of covariance contours on the sphere +tl = linspace(-pi, pi, nbDrawingSeg); +Gdisp = zeros(nbVarOutMan, nbDrawingSeg, model.nbStates); +for i=1:model.nbStates + [V,D] = eig(model.Sigma(4:5,4:5,i)); + Gdisp(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(4:6,i)); +end +Gregr = zeros(nbVarOutMan, nbDrawingSeg, nbData); +for t=1:nbData + [V,D] = eig(expSigma(:,:,t)); + Gregr(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], xhat(:,t)); +end + +%Manifold plot +figure('PaperPosition',[0 0 8 8],'position',[10,10,650,350],'name','manifold data'); rotate3d on; +colormap([.9 .9 .9]); +%in +subplot(1,2,1); hold on; axis off; grid off; +plot3(x(1,:), x(2,:), x(3,:), '.','markersize',8,'color',[0 0 0]); +plot3(xIn(1,1:nbData), xIn(2,1:nbData), xIn(3,1:nbData), '-','linewidth',3,'color',[0 .8 0]); +for i=1:model.nbStates + plotGMM3D(model.MuMan(1:3,i), model.Sigma(1:3,1:3,i), clrmap(i,:), .1); +end +view(-20,2); axis equal; axis vis3d; +%out +subplot(1,2,2); hold on; axis off; grid off; +mesh(X,Y,Z); +plot3(x(4,:), x(5,:), x(6,:), '.','markersize',8,'color',[0 0 0]); +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '-','linewidth',2,'color',[.8 0 0]); +for i=1:model.nbStates + plot3(model.MuMan(4,i), model.MuMan(5,i), model.MuMan(6,i), '.','markersize',20,'color',clrmap(i,:)); + plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',1,'color',clrmap(i,:)); +end +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '.','markersize',12,'color',[.8 0 0]); +for t=1:nbData + plot3(Gregr(1,:,t), Gregr(2,:,t), Gregr(3,:,t), '-','linewidth',1,'color',[.8 0 0]); +end +view(-20,2); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GMR03a.png'); + +%Tangent plane plot +figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650],'name','tangent space data'); +for i=1:model.nbStates + subplot(ceil(model.nbStates/2),2,i); hold on; axis off; title(['k=' num2str(i) ', output space']); + plot(0,0,'+','markersize',40,'linewidth',1,'color',[.7 .7 .7]); + plot(uTmp(4,:,i), uTmp(5,:,i), '.','markersize',4,'color',[0 0 0]); + plotGMM(model.Mu(4:5,i), model.Sigma(4:5,4:5,i)*3, clrmap(i,:), .3); + axis equal; +end +%print('-dpng','graphs/demo_Riemannian_sphere_GMR03b.png'); + +% pause; +% close all; +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 +end + + diff --git a/demo_Riemannian_sphere_GMR04.m b/demo_Riemannian_sphere_GMR04.m new file mode 100644 index 0000000000000000000000000000000000000000..5499ef9fb03d45159faf3844cb7035e9ae9985d7 --- /dev/null +++ b/demo_Riemannian_sphere_GMR04.m @@ -0,0 +1,232 @@ +function demo_Riemannian_sphere_GMR04 +% GMR with input data on a sphere and output data in Eudlidean space by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +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 + +model.nbStates = 1; %Number of states in the GMM +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)]; + 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,:)']; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(H(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1:2; out=3:5; +inMan=1:3; outMan=4:6; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +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)); + U1 = blkdiag(AcIn,eye(3)) * U0(:,:,i); + SigmaTmp(:,:,i) = U1 * U1'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = model.MuMan(outMan,i) - SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * logmap(model.MuMan(inMan,i), xIn(:,t)); + end + xhat(:,t) = uOutTmp(:,:,t) * H(:,t); + + %Compute conditional covariances + 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 +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 +figure('PaperPosition',[0 0 12 6],'position',[10,10,1200,700],'name','manifold data'); rotate3d on; +colormap([.9 .9 .9]); +%in +subplot(1,2,1); hold on; axis off; grid off; +mesh(X,Y,Z); +plot3(x(1,:), x(2,:), x(3,:), '.','markersize',8,'color',[0 0 0]); +plot3(xIn(1,1:nbData), xIn(2,1:nbData), xIn(3,1:nbData), '-','linewidth',3,'color',[0 .8 0]); +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; +plot3(x(4,:), x(5,:), x(6,:), '.','markersize',8,'color',[0 0 0]); +for i=1:model.nbStates + plotGMM3D(model.MuMan(4:6,i), model.Sigma(3:5,3:5,i), clrmap(i,:), .1); +end +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '.','markersize',12,'color',[.8 0 0]); +view(-20,2); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GMR04.png'); + +% pause; +% close all; +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 +end + diff --git a/demo_Riemannian_sphere_GaussProd01.m b/demo_Riemannian_sphere_GaussProd01.m new file mode 100644 index 0000000000000000000000000000000000000000..e3ed41f44aebebb3de57daf4c22ceae3b7924131 --- /dev/null +++ b/demo_Riemannian_sphere_GaussProd01.m @@ -0,0 +1,220 @@ +function demo_Riemannian_sphere_GaussProd01 +% Product of Gaussians on a sphere by relying 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 5; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbDrawingSeg = 30; %Number of segments used to draw ellipsoids + +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 2; %Dimension of the tangent space +model.nbVarMan = 3; %Dimension of the manifold +model.params_diagRegFact = 1E-3; %Regularization of covariance + + +%% Setting GMM parameters explicitly +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.Priors = ones(1,model.nbStates) / model.nbStates; + +model.MuMan(:,1) = [-1; .2; .6]; +model.MuMan(:,2) = [.5; .5; .8]; + +for i=1:model.nbStates + model.MuMan(:,i) = model.MuMan(:,i) / norm(model.MuMan(:,i)); +end + +model.Mu = zeros(model.nbVar,model.nbStates); + +model.Sigma(:,:,1) = diag([.1,15]) * 1E-1; +model.Sigma(:,:,2) = diag([.1,15]) * 1E-1; + + +%% Sampling from GMM +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nb = round(nbData*nbSamples/model.nbStates); +x = []; +for i=1:model.nbStates + if i==model.nbStates + nb = nbData*nbSamples - (model.nbStates-1)*nb; + end + [V,D] = eig(model.Sigma(:,:,i)); + utmp = V*D.^.5 * randn(model.nbVar,nb); + x = [x, expmap(utmp, model.MuMan(:,i))]; +end +%Compute points on tangent spaces +for i=1:model.nbStates + u(:,:,i) = logmap(x, model.MuMan(:,i)); +end +%Compute likelihoods +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); + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% Product of Gaussians (version transporting the covariances) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +MuMan = [-1; -1; 0]; +MuMan = MuMan / norm(MuMan); + +%Compute MuMan incrementally +MuTmp = zeros(model.nbVar,model.nbStates); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +for n=1:nbIter + Mu = zeros(model.nbVar,1); + SigmaSum = zeros(model.nbVar); + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(:,i) to MuMan + Ac = transp(model.MuMan(:,i), MuMan); + U1 = Ac * U0(:,:,i); + SigmaTmp(:,:,i) = U1 * U1'; + %Tracking component for Gaussian i + SigmaSum = SigmaSum + inv(SigmaTmp(:,:,i)); + MuTmp(:,i) = logmap(model.MuMan(:,i), MuMan); + Mu = Mu + SigmaTmp(:,:,i) \ MuTmp(:,i); + end + Sigma = inv(SigmaSum); + %Gradient computation + Mu = Sigma * Mu; + %Keep an history for plotting + hist(n).MuMan = MuMan; + hist(n).Sigma = Sigma; + %Update MuMan + MuMan = expmap(Mu, MuMan); +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 +for n=1:nbIter + [V,D] = eig(hist(n).Sigma); + hist(n).Gdisp2 = expmap(V*D.^.5*[cos(tl); sin(tl)], hist(n).MuMan); +end + +%Manifold plot +figure('position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; +colormap(repmat(linspace(1,.2,64),3,1)'); + +%colored sphere +nbp = 40; +[X,Y,Z] = sphere(nbp-1); +p = [reshape(X,1,nbp^2); reshape(Y,1,nbp^2); reshape(Z,1,nbp^2)]; +c = zeros(nbp^2,1); +for i=1:model.nbStates + dtmp = logmap(p,model.MuMan(:,i))'; + c = c + sum((dtmp/model.Sigma(:,:,i)).*dtmp, 2); +end +surf(X,Y,Z,reshape(c,nbp,nbp),'linestyle','none'); + +for n=1:nbData*nbSamples + plot3(x(1,n), x(2,n), x(3,n), '.','markersize',12,'color',GAMMA(:,n)'*clrmap); +end +for i=1:model.nbStates + plot3(model.MuMan(1,i), model.MuMan(2,i), model.MuMan(3,i), '.','markersize',12,'color',clrmap(i,:)); + plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',3,'color',clrmap(i,:)); +end + +%Plot history +for n=1:nbIter + coltmp = [.3 1 .3] * (nbIter-n)/nbIter; + plot3(hist(n).MuMan(1), hist(n).MuMan(2), hist(n).MuMan(3), '.','markersize',12,'color',coltmp); + plot3(hist(n).Gdisp2(1,:), hist(n).Gdisp2(2,:), hist(n).Gdisp2(3,:), '-','linewidth',1,'color',coltmp); +end +view(-150,20); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_GaussProd01.png'); + +% pause; +% close all; +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 +end + diff --git a/demo_Riemannian_sphere_TPGMM01.m b/demo_Riemannian_sphere_TPGMM01.m new file mode 100644 index 0000000000000000000000000000000000000000..717ee7095639832d59fe1f7a1d53a89669d855f2 --- /dev/null +++ b/demo_Riemannian_sphere_TPGMM01.m @@ -0,0 +1,264 @@ +function demo_Riemannian_sphere_TPGMM01 +% TP-GMM for data on a sphere by relying on Riemannian manifold (with single frame). +% +% 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 4; %Number of demonstrations +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm +nbDrawingSeg = 15; %Number of segments used to draw ellipsoids + +model.nbStates = 5; %Number of states in the GMM +model.nbFrames = 1; %Number of candidate frames of reference +model.nbVar = 3; %Dimension of the tangent space (incl. time) +model.nbVarMan = 4; %Dimension of the manifold (incl. time) +model.dt = 0.01; %Time step duration +model.params_diagRegFact = 1E-3; %Regularization of covariance +e0 = [0; 0; 1]; %Origin on the manifold + + +%% Generate dataset on a sphere from handwriting data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +demos=[]; +load('data/2Dletters/S.mat'); +uIn=[]; 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*0.5E-1]; + uIn = [uIn, [1:nbData]*model.dt]; + for m=1:model.nbFrames + s(n).p(m).b = (rand(model.nbVarMan-1,1)-0.5)*2; + s(n).p(m).b = s(n).p(m).b / norm(s(n).p(m).b); + [s(n).p(m).A,~] = qr(randn(model.nbVar-1)); + end +end +xOut = expmap(uOut, e0); +xIn = uIn; +u = [uIn; uOut]; +x = [xIn; xOut]; + + +%% GMM parameters estimation (joint distribution with time as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [model.Mu(1,:); expmap(model.Mu(2:3,:), e0)]; %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 + L(i,:) = model.Priors(i) * gaussPDF([xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:4,i))], model.Mu(:,i), model.Sigma(:,:,i)); + end + GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1); + GAMMA2 = 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) = [xIn-model.MuMan(1,i); logmap(xOut, model.MuMan(2:4,i))]; + model.MuMan(:,i) = [(model.MuMan(1,i)+uTmp(1,:,i))*GAMMA2(i,:)'; expmap(uTmp(2:3,:,i)*GAMMA2(i,:)', model.MuMan(2:4,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(GAMMA2(i,:)) * uTmp(:,:,i)' + eye(size(u,1)) * model.params_diagRegFact; + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% GMR in each frame +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +in=1; out=2:3; outMan=2:4; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +uhat = zeros(nbVarOut,nbData); +xhat = zeros(nbVarOutMan,nbData); +uOutTmp = zeros(nbVarOut,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,nbData); + +%Version with single optimization loop +for t=1:nbData + %Compute activation weight + for i=1:model.nbStates + H(i,t) = model.Priors(i) * gaussPDF(xIn(:,t)-model.MuMan(in,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) + if t==1 + [~,id] = max(H(:,t)); + xhat(:,t) = model.MuMan(outMan,id); %Initial point + else + xhat(:,t) = xhat(:,t-1); + end + for n=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(outMan,i) to xhat(:,t) + Ac = transp(model.MuMan(outMan,i), xhat(:,t)); + U1 = blkdiag(1, Ac) * U0(:,:,i); %First variable in Euclidean space + SigmaTmp(:,:,i) = U1 * U1'; + %Gaussian conditioning on the tangent space + uOutTmp(:,i,t) = logmap(model.MuMan(outMan,i), xhat(:,t)) + SigmaTmp(out,in,i)/SigmaTmp(in,in,i) * (xIn(:,t)-model.MuMan(in,i)); + end + uhat(:,t) = uOutTmp(:,:,t) * H(:,t); + xhat(:,t) = expmap(uhat(:,t), xhat(:,t)); + end + + %Compute conditional covariances + for i=1:model.nbStates + 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 + uOutTmp(:,i,t)*uOutTmp(:,i,t)'); + end + expSigma(:,:,t) = expSigma(:,:,t) - uhat(:,t)*uhat(:,t)' + eye(nbVarOut) * model.params_diagRegFact; +end + + +%% Transportation of GMR results from e0 to s(n).p(1).b +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +for n=1:nbSamples + s(n).x = expmap(s(n).p(1).A * uOut, s(n).p(1).b); + uTmp = s(n).p(1).A * logmap(xhat, e0); + s(n).xhat = expmap(uTmp, s(n).p(1).b); + for t=1:nbData + s(n).expSigma(:,:,t) = s(n).p(1).A * expSigma(:,:,t) * s(n).p(1).A'; + end +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); + +%Display of covariance contours on the sphere +tl = linspace(-pi, pi, nbDrawingSeg); +Gdisp = zeros(nbVarOutMan, nbDrawingSeg, model.nbStates); +for i=1:model.nbStates + [V,D] = eig(model.Sigma(2:3,2:3,i)); + Gdisp(:,:,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(2:4,i)); +end +Gregr = zeros(nbVarOutMan, nbDrawingSeg, nbData); +for t=1:nbData + [V,D] = eig(expSigma(:,:,t)); + Gregr(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], xhat(:,t)); +end +for n=1:nbSamples + for t=1:nbData + [V,D] = eig(s(n).expSigma(:,:,t)); + utmp = logmap(s(n).xhat(:,t), s(n).p(1).b); + s(n).Gdisp(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)] + repmat(utmp,1,nbDrawingSeg), s(n).p(1).b); + end +end + +%Manifold plot +figure('PaperPosition',[0 0 8 8],'position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; +colormap([.9 .9 .9]); +[X,Y,Z] = sphere(20); +mesh(X,Y,Z); +plot3(xOut(1,:), xOut(2,:), xOut(3,:), '.','markersize',6,'color',[.6 .6 .6]); +plot3(xhat(1,:), xhat(2,:), xhat(3,:), '.','markersize',12,'color',[.8 0 0]); +for t=1:nbData + plot3(Gregr(1,:,t), Gregr(2,:,t), Gregr(3,:,t), '-','linewidth',1,'color',[.8 0 0]); +end + +for n=1:nbSamples + plot3(s(n).p(1).b(1), s(n).p(1).b(2), s(n).p(1).b(3), '+','markersize',12,'color',[0 0 0]); + plot3(s(n).x(1,:), s(n).x(2,:), s(n).x(3,:), '.','markersize',6,'color',[.6 .6 .6]); + plot3(s(n).xhat(1,:), s(n).xhat(2,:), s(n).xhat(3,:), '.','markersize',12,'color',[0 0 0]); + for t=1:nbData + plot3(s(n).Gdisp(1,:,t), s(n).Gdisp(2,:,t), s(n).Gdisp(3,:,t), '-','linewidth',1,'color',[0 0 0]); + end +end +for i=1:model.nbStates + plot3(model.MuMan(1,i), model.MuMan(2,i), model.MuMan(3,i), '.','markersize',12,'color',clrmap(i,:)); + plot3(Gdisp(1,:,i), Gdisp(2,:,i), Gdisp(3,:,i), '-','linewidth',1,'color',clrmap(i,:)); +end +view(-20,70); axis equal; axis tight; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_TPGMM01.png'); + +% pause; +% close all; +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 +end diff --git a/demo_Riemannian_sphere_TPGMM02.m b/demo_Riemannian_sphere_TPGMM02.m new file mode 100644 index 0000000000000000000000000000000000000000..26e0315083668f35e749af339ae45a7a5450e635 --- /dev/null +++ b/demo_Riemannian_sphere_TPGMM02.m @@ -0,0 +1,357 @@ +function demo_Riemannian_sphere_TPGMM02 +% TP-GMM for data on a sphere by relying on Riemannian manifold (with two frames). +% +% 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 50; %Number of datapoints +nbSamples = 4; %Number of demonstrations +nbRepros = 3; %Number of reproductions +nbIter = 10; %Number of iteration for the Gauss Newton algorithm +nbIterEM = 10; %Number of iteration for the EM algorithm +nbDrawingSeg = 15; %Number of segments used to draw ellipsoids + +model.nbStates = 5; %Number of states in the GMM +model.nbFrames = 2; %Number of candidate frames of reference +model.nbVar = 3; %Dimension of the tangent space (incl. time) +model.nbVarMan = 4; %Dimension of the manifold (incl. time) +model.dt = 0.01; %Time step duration +model.params_diagRegFact = 1E-4; %Regularization of covariance +e0 = [0; 0; 1]; %Origin on the manifold + + +%% Generate data from TP-GMM example +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for +% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and +% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations +% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=2 the dimension of a +% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200) +% multiplied by the number of demonstrations (M=5). +load('data/Data01.mat'); +sc = 1.5E-1; +nbD = 200; +uIn=[]; uOut=[]; +for n=1:nbSamples + Dtmp = []; + for m=1:model.nbFrames + Dtmp = [Dtmp; squeeze(Data(:,m,(n-1)*nbD+1:n*nbD))]; + end + s(n).Data = spline(1:nbD, Dtmp, linspace(1,nbD,nbData)); + uOut = [uOut, s(n).Data*sc]; + uIn = [uIn, [1:nbData]*model.dt]; + for m=1:model.nbFrames + s(n).p(m).b = expmap(s(n).p(m).b, e0); + for v=1:2 + s(n).p(m).A(:,v) = s(n).p(m).A(:,v) / norm(s(n).p(m).A(:,v)); + end + end +end +xOut = [expmap(uOut(1:2,:), e0); expmap(uOut(3:4,:), e0)]; +xIn = uIn; +u = [uIn; uOut]; +x = [xIn; xOut]; + + +%% GMM parameters estimation (joint distribution with time as input, sphere as output) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Learning...'); +model = init_GMM_kbins(u, model, nbSamples); +model.MuMan = [model.Mu(1,:); expmap(model.Mu(2:3,:), e0); expmap(model.Mu(4:5,:), e0)]; %Center on the manifold +model.Mu = zeros(1+(model.nbVar-1)*2,model.nbStates); %Center in the tangent plane at point MuMan of the manifold + +%Learning of task-parameterized model as a meta Gaussian mixture model (see also demo_TPmetaGMM02.m) +uTmp = zeros(1+(model.nbVar-1)*2,nbData*nbSamples,model.nbStates); +for nb=1:nbIterEM + %E-step + L = zeros(model.nbStates,size(x,2)); + for i=1:model.nbStates + xcTmp = [xIn-model.MuMan(1,i); logmap(xOut(1:3,:), model.MuMan(2:4,i)); logmap(xOut(4:6,:), model.MuMan(5:7,i))]; + L(i,:) = model.Priors(i) * gaussPDF(xcTmp, model.Mu(:,i), model.Sigma(:,:,i)); + end + GAMMA = L ./ repmat(sum(L,1)+realmin, model.nbStates, 1); + GAMMA2 = 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) = [xIn-model.MuMan(1,i); logmap(xOut(1:3,:), model.MuMan(2:4,i)); logmap(xOut(4:6,:), model.MuMan(5:7,i))]; + model.MuMan(:,i) = [(model.MuMan(1,i)+uTmp(1,:,i))*GAMMA2(i,:)'; expmap(uTmp(2:3,:,i)*GAMMA2(i,:)', model.MuMan(2:4,i)); ... + expmap(uTmp(4:5,:,i)*GAMMA2(i,:)', model.MuMan(5:7,i))]; + end + %Update Sigma + model.Sigma(:,:,i) = uTmp(:,:,i) * diag(GAMMA2(i,:)) * uTmp(:,:,i)' + eye(size(uTmp,1)) * model.params_diagRegFact; + end +end + +%Restructuration as a tensor GMM +MuManOld = model.MuMan; +SigmaOld = model.Sigma; +model.Mu = zeros(model.nbVar,model.nbFrames,model.nbStates); %Center in the tangent plane at point MuMan of the manifold +model.MuMan = []; +model.Sigma = []; +for i=1:model.nbStates + for m=1:model.nbFrames + id = [1,[1:2]+(m-1)*2+1]; + idMan = [1,[1:3]+(m-1)*3+1]; + model.MuMan(:,m,i) = MuManOld(idMan,i); + model.Sigma(:,:,m,i) = SigmaOld(id,id,i); + end +end + +%Eigendecomposition of Sigma +for i=1:model.nbStates + for m=1:model.nbFrames + [V,D] = eig(model.Sigma(:,:,m,i)); + U0(:,:,m,i) = V * D.^.5; + end +end + + +%% GMR in each frame +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('GMR in each frame...'); +in=1; out=2:3; outMan=2:4; +nbVarOut = length(out); +nbVarOutMan = length(outMan); + +uhat = zeros(nbVarOut,model.nbFrames,nbData); +xhat = zeros(nbVarOutMan,model.nbFrames,nbData); +uOutTmp = zeros(nbVarOut,model.nbFrames,model.nbStates,nbData); +SigmaTmp = zeros(model.nbVar,model.nbVar,model.nbFrames,model.nbStates); +expSigma = zeros(nbVarOut,nbVarOut,model.nbFrames,nbData); + +%Version with single optimization loop +for t=1:nbData + %Compute activation weight (common input shared by all the frames) + for i=1:model.nbStates + H(i,t) = model.Priors(i) * gaussPDF(xIn(:,t)-model.MuMan(in,1,i), model.Mu(in,1,i), model.Sigma(in,in,1,i)); + end + H(:,t) = H(:,t) / sum(H(:,t)+realmin); + + %Compute conditional mean (with covariance transportation) + for m=1:model.nbFrames + if t==1 + [~,id] = max(H(:,t)); + xhat(:,m,t) = model.MuMan(outMan,m,id); %Initial point + else + xhat(:,m,t) = xhat(:,m,t-1); + end + for k=1:nbIter + for i=1:model.nbStates + %Transportation of covariance from model.MuMan(outMan,m,i) to xhat(:,m,t) + Ac = transp(model.MuMan(outMan,m,i), xhat(:,m,t)); + U1 = blkdiag(1, Ac) * U0(:,:,m,i); %First variable in Euclidean space + SigmaTmp(:,:,m,i) = U1 * U1'; + %Gaussian conditioning on the tangent space + uOutTmp(:,m,i,t) = logmap(model.MuMan(outMan,m,i), xhat(:,m,t)) + SigmaTmp(out,in,m,i)/SigmaTmp(in,in,m,i) * (xIn(:,t)-model.MuMan(in,m,i)); + end %i + uhat(:,m,t) = squeeze(uOutTmp(:,m,:,t)) * H(:,t); + xhat(:,m,t) = expmap(uhat(:,m,t), xhat(:,m,t)); + end %k + + %Compute conditional covariances + for i=1:model.nbStates + expSigma(:,:,m,t) = expSigma(:,:,m,t) + H(i,t) * (SigmaTmp(out,out,m,i) - SigmaTmp(out,in,m,i)/SigmaTmp(in,in,m,i) * SigmaTmp(in,out,m,i)); + end %i + end %m +end %t + + +%% Transportation of GMR results from e0 to s(n).p(m).b +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Transportation...'); +for n=1:nbRepros + s(n).x = zeros(nbVarOutMan,model.nbFrames,nbData*nbSamples); + s(n).xhat = zeros(nbVarOutMan,model.nbFrames,nbData); + s(n).expSigma = zeros(nbVarOut,nbVarOut,model.nbFrames,nbData); + for m=1:model.nbFrames + s(n).x(:,m,:) = expmap(s(n).p(m).A * uOut([1:2]+(m-1)*2,:), s(n).p(m).b); + uTmp = s(n).p(m).A * logmap(squeeze(xhat(:,m,:)), e0); + s(n).xhat(:,m,:) = expmap(uTmp, s(n).p(m).b); + for t=1:nbData + s(n).expSigma(:,:,m,t) = s(n).p(m).A * expSigma(:,:,m,t) * s(n).p(m).A'; + end + end +end + +%Eigendecomposition of s(n).expSigma +for n=1:nbRepros + for t=1:nbData + for m=1:model.nbFrames + [V,D] = eig(s(n).expSigma(:,:,m,t)); + s(n).U0(:,:,m,t) = V * D.^.5; + end + end +end + + +%% Products of linearly transformed Gaussians +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Products of linearly transformed Gaussians...'); +for n=1:nbRepros + for t=1:nbData + MuTmp = zeros(nbVarOut,model.nbStates); + SigmaTmp = zeros(nbVarOut,model.nbVar-1,model.nbStates); + s(n).xh(:,t) = s(n).xhat(:,1,t); + for k=1:nbIter + Mu = zeros(nbVarOut,1); + SigmaSum = zeros(model.nbVar-1); + for m=1:model.nbFrames + %Transportation of covariance from s(n).xhat(:,m,t) to s(n).xh(:,t) + Ac = transp(s(n).xhat(:,m,t), s(n).xh(:,t)); + U1 = Ac * s(n).U0(:,:,m,t); + SigmaTmp(:,:,i) = U1 * U1'; + %Tracking component for Gaussian i + SigmaSum = SigmaSum + inv(SigmaTmp(:,:,i)); + MuTmp(:,i) = logmap(s(n).xhat(:,m,t), s(n).xh(:,t)); + Mu = Mu + SigmaTmp(:,:,i) \ MuTmp(:,i); + end + Sigma = inv(SigmaSum); % + eye(nbVarOut)* model.params_diagRegFact; + Mu = Sigma * Mu; %Gradient computation + s(n).xh(:,t) = expmap(Mu, s(n).xh(:,t)); %Update MuMan + s(n).xhSigma(:,:,t) = Sigma; + end + end +end + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('Plotting...'); +clrmap = lines(model.nbFrames); + +%Display of covariance contours on the sphere +tl = linspace(-pi, pi, nbDrawingSeg); +Gdisp = zeros(model.nbVarMan-1, nbDrawingSeg, model.nbFrames, model.nbStates); +for i=1:model.nbStates + for m=1:model.nbFrames + [V,D] = eig(model.Sigma(2:3,2:3,m,i)); + Gdisp(:,:,m,i) = expmap(V*D.^.5*[cos(tl); sin(tl)], model.MuMan(2:4,m,i)); + end +end +Gregr = zeros(nbVarOutMan, nbDrawingSeg, model.nbFrames, nbData); +for m=1:model.nbFrames + for t=1:nbData + [V,D] = eig(expSigma(:,:,m,t)); + Gregr(:,:,m,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], xhat(:,m,t)); + end +end +for n=1:nbRepros + for m=1:model.nbFrames + for t=1:nbData + [V,D] = eig(s(n).expSigma(:,:,m,t)); + utmp = logmap(s(n).xhat(:,m,t), s(n).p(m).b); + s(n).Gdisp(:,:,m,t) = expmap(V*D.^.5*[cos(tl); sin(tl)] + repmat(utmp,1,nbDrawingSeg), s(n).p(m).b); + end + end +end +for n=1:nbRepros + for t=1:nbData + [V,D] = eig(s(n).xhSigma(:,:,t)); + s(n).Gh(:,:,t) = expmap(V*D.^.5*[cos(tl); sin(tl)], s(n).xh(:,t)); + end +end + + +%Manifold plot +figure('PaperPosition',[0 0 12 4],'position',[10,10,1300,450]); +for n=1:nbRepros +subplot(1,3,n); hold on; axis off; grid off; rotate3d on; +colormap([.9 .9 .9]); +[X,Y,Z] = sphere(20); +mesh(X,Y,Z); +plot3(s(n).p(1).b(1), s(n).p(1).b(2), s(n).p(1).b(3), '+','markersize',12,'color',[0 0 0]); +for m=1:model.nbFrames + plot3(squeeze(s(n).x(1,m,:)), squeeze(s(n).x(2,m,:)), squeeze(s(n).x(3,m,:)), '.','markersize',6,'color',clrmap(m,:)); + plot3(squeeze(s(n).xhat(1,m,:)), squeeze(s(n).xhat(2,m,:)), squeeze(s(n).xhat(3,m,:)), '.','markersize',12,'color',clrmap(m,:)); + for t=1:nbData + plot3(s(n).Gdisp(1,:,m,t), s(n).Gdisp(2,:,m,t), s(n).Gdisp(3,:,m,t), '-','linewidth',1,'color',clrmap(m,:)); + end +end +plot3(s(n).xh(1,:), s(n).xh(2,:), s(n).xh(3,:), '.','markersize',12,'color',[0 .7 0]); +for t=1:nbData + plot3(s(n).Gh(1,:,t), s(n).Gh(2,:,t), s(n).Gh(3,:,t), '-','linewidth',1,'color',[0 .7 0]); +end +view(-20,70); axis equal; axis tight; axis vis3d; +end %n +%print('-dpng','graphs/demo_Riemannian_sphere_TPGMM02.png'); + +% pause; +% close all; +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 +end + diff --git a/demo_Riemannian_sphere_vecTransp01.m b/demo_Riemannian_sphere_vecTransp01.m new file mode 100644 index 0000000000000000000000000000000000000000..8f0be859a8c522f51c27017903a117cdeeecd84e --- /dev/null +++ b/demo_Riemannian_sphere_vecTransp01.m @@ -0,0 +1,190 @@ +function demo_Riemannian_sphere_vecTransp01 +% Parallel transport on a sphere. +% +% 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{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", +% journal="{IEEE} Robotics and Automation Letters ({RA-L})", +% doi="", +% year="2017", +% month="", +% volume="", +% number="", +% pages="" +% } +% +% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/ +% Written by Sylvain Calinon, Martijn Zeestraten and Noemie Jaquier +% +% 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 . + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.nbStates = 2; %Number of states in the GMM +model.nbVar = 2; %Dimension of the tangent space +model.nbVarMan = 3; %Dimension of the manifold + + +%% Setting GMM parameters manually +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +model.Priors = ones(1,model.nbStates) / model.nbStates; + +%model.MuMan = randn(model.nbVarMan,model.nbStates); +model.MuMan(:,1) = [1; -.5; 0]; +model.MuMan(:,2) = [0; -1; .5]; +for i=1:model.nbStates + model.MuMan(:,i) = model.MuMan(:,i) / norm(model.MuMan(:,i)); +end + +model.Mu = zeros(model.nbVar,model.nbStates); + +model.Sigma(:,:,1) = diag([2,4]) * 5E-2; +model.Sigma(:,:,2) = diag([2,4]) * 5E-2; + +%Eigendecomposition of Sigma +for i=1:model.nbStates + [V,D] = eig(model.Sigma(:,:,i)); + U0(:,:,i) = V * D.^.5; +end + + +%% Transportation of covariance +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +g = model.MuMan(:,1); +h = model.MuMan(:,2); + +tl = linspace(0,1,20); +for n=1:20 + t = tl(n); + + hist(n).MuMan = expmap(logmap(h,g)*t, g); + + Ac = transp(g, hist(n).MuMan); + hist(n).U = Ac * U0(:,:,1); + hist(n).Sigma = hist(n).U * hist(n).U'; + + % Direction of the geodesic + hist(n).dirG = logmap(h, hist(n).MuMan); + if norm(hist(n).dirG) > 1E-5 + % Normalise the direction + hist(n).dirG = hist(n).dirG ./ norm(hist(n).dirG); + % Compute the inner product with the first eigenvector + inprod(n) = hist(n).dirG' * hist(n).U(:,1); + end + +end + +% %Check that the two vectors below are the same +% p1 = -logmap(g,h); +% p2 = E' * rotM(h) * Rpar * rotM(g)' * E * logmap(h,g); +% norm(p1-p2) +% +% %Check that the transported eigenvectors remain orthogonal +% hist(end).U(:,1)' * hist(end).U(:,2) + +% Check that the inner product between the direction of the geodesic and +% parallel transported vectors (e.g. eigenvectors) is conserved. +inprod + + +%% Plots +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clrmap = lines(model.nbStates); +MuMan0 = [0; 0; 1]; +nbDrawingSeg = 30; %Number of segments used to draw ellipsoids + +%Display of covariance contours on the sphere +t = 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(t); sin(t)], model.MuMan(:,i)); +end +for n=1:20 + [V,D] = eig(hist(n).Sigma); + hist(n).Gdisp = expmap(V*D.^.5*[cos(t); sin(t)], hist(n).MuMan); +end + +%Manifold plot +figure('position',[10,10,650,650]); hold on; axis off; grid off; rotate3d on; +colormap([.9 .9 .9]); +nbp = 40; +[X,Y,Z] = sphere(nbp-1); +mesh(X,Y,Z); +for i=1:model.nbStates + plot3(model.MuMan(1,i), model.MuMan(2,i), model.MuMan(3,i), '.','markersize',12,'color',clrmap(i,:)); + plot3Dframe(rotM(model.MuMan(:,i))'*0.08, model.MuMan(:,i)); +end +%Plot transported covariance +for n=1:20 + plot3(hist(n).MuMan(1), hist(n).MuMan(2), hist(n).MuMan(3), '.','markersize',12,'color',clrmap(1,:)*n/20); + plot3(hist(n).Gdisp(1,:), hist(n).Gdisp(2,:), hist(n).Gdisp(3,:), '-','linewidth',.2,'color',clrmap(1,:)*n/20); + for j=1:model.nbVar + msh = expmap(squeeze(hist(n).U(:,j))*linspace(0,1,nbDrawingSeg), hist(n).MuMan); + plot3(msh(1,:), msh(2,:), msh(3,:), '-','linewidth',1,'color',clrmap(1,:)*n/20); + end +end +plot3Dframe(rotM(MuMan0)'*0.08, MuMan0); +view(30,12); axis equal; axis vis3d; +%print('-dpng','graphs/demo_Riemannian_sphere_vecTransp01.png'); + +% pause; +% close all; +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 +end + diff --git a/m_fcts/init_GMM_kbins.m b/m_fcts/init_GMM_kbins.m index 0d51913ad4209c4b5c37303e4feba20c6b142bab..8cac76b1a0641aa93fa50457c967f93942e60a43 100644 --- a/m_fcts/init_GMM_kbins.m +++ b/m_fcts/init_GMM_kbins.m @@ -11,12 +11,12 @@ function model = init_GMM_kbins(Data, model, nbSamples) % author="Calinon, S.", % title="A Tutorial on Task-Parameterized Movement Learning and Retrieval", % journal="Intelligent Service Robotics", -% publisher="Springer Berlin Heidelberg", -% doi="10.1007/s11370-015-0187-9", -% year="2016", -% volume="9", -% number="1", -% pages="1--29" +% publisher="Springer Berlin Heidelberg", +% doi="10.1007/s11370-015-0187-9", +% year="2016", +% volume="9", +% number="1", +% pages="1--29" % } % % Copyright (c) 2015 Idiap Research Institute, http://idiap.ch/ @@ -37,20 +37,25 @@ function model = init_GMM_kbins(Data, model, nbSamples) % along with PbDlib. If not, see . -diagRegularizationFactor = 1E-8; %Optional regularization term to avoid numerical instability +%Parameters nbData = size(Data,2) / nbSamples; +if ~isfield(model,'params_diagRegFact') + model.params_diagRegFact = 1E-4; %Optional regularization term to avoid numerical instability +end %Delimit the cluster bins for the first demonstration -tSep = round(linspace(1, nbData, model.nbStates+1)); +tSep = round(linspace(0, nbData, model.nbStates+1)); %Compute statistics for each bin for i=1:model.nbStates id=[]; for n=1:nbSamples - id = [id (n-1)*nbData+[tSep(i):tSep(i+1)]]; + id = [id (n-1)*nbData+[tSep(i)+1:tSep(i+1)]]; end model.Priors(i) = length(id); model.Mu(:,i) = mean(Data(:,id),2); - model.Sigma(:,:,i) = cov(Data(:,id)') + eye(size(Data,1)) * diagRegularizationFactor; + model.Sigma(:,:,i) = cov(Data(:,id)') + eye(size(Data,1)) * model.params_diagRegFact; end model.Priors = model.Priors / sum(model.Priors); + + diff --git a/m_fcts/plot3Dframe.m b/m_fcts/plot3Dframe.m new file mode 100644 index 0000000000000000000000000000000000000000..0daae121433f600afa024bc02aa70720e1d4f991 --- /dev/null +++ b/m_fcts/plot3Dframe.m @@ -0,0 +1,24 @@ +function h = plot3Dframe(rotAxis, orgPt, colMat) +% Leonel Rozo, Sylvain Calinon, 2015 +% +% This function plots a 3D frame of reference using rgb colors for each axis. +% rotAxis: The rotation matrix representing the 3D frame +% orgPt: The origin of the frame + +if nargin<3 + colMat = eye(3); +end + +if (~ishold) + hold on; + axis equal; +end + +% h(1) = quiver3( orgPt(1), orgPt(2), orgPt(3), rotAxis(1,1), rotAxis(2,1), rotAxis(3,1), 0.2, 'linewidth', 2, 'color', colMat(:,1)); +% h(2) = quiver3( orgPt(1), orgPt(2), orgPt(3), rotAxis(1,2), rotAxis(2,2), rotAxis(3,2), 0.2, 'linewidth', 2, 'color', colMat(:,2)); +% h(3) = quiver3( orgPt(1), orgPt(2), orgPt(3), rotAxis(1,3), rotAxis(2,3), rotAxis(3,3), 0.2, 'linewidth', 2, 'color', colMat(:,3)); + +%for faster plot +h(1) = plot3( [orgPt(1) orgPt(1)+rotAxis(1,1)], [orgPt(2) orgPt(2)+rotAxis(2,1)], [orgPt(3) orgPt(3)+rotAxis(3,1)], 'linewidth', 2, 'color', colMat(:,1)); +h(2) = plot3( [orgPt(1) orgPt(1)+rotAxis(1,2)], [orgPt(2) orgPt(2)+rotAxis(2,2)], [orgPt(3) orgPt(3)+rotAxis(3,2)], 'linewidth', 2, 'color', colMat(:,2)); +h(3) = plot3( [orgPt(1) orgPt(1)+rotAxis(1,3)], [orgPt(2) orgPt(2)+rotAxis(2,3)], [orgPt(3) orgPt(3)+rotAxis(3,3)], 'linewidth', 2, 'color', colMat(:,3)); diff --git a/m_fcts/rotM.m b/m_fcts/rotM.m new file mode 100644 index 0000000000000000000000000000000000000000..3cdcf100883ec8c1b6166428f5fd20d1bb3aaa89 --- /dev/null +++ b/m_fcts/rotM.m @@ -0,0 +1,55 @@ +function rot = rotM(v,angle) +% ROTM : 3 x 3 rotation matrix +% [rot]= rotM(v) gives the rotation that moves v to the north +% pole +% [rot]= rotM(v, angle) gives the rotation with axis 'v' and +% angle 'angle' +% +% Use: 3x3 matrix rot can be pre-multiplied to 3 x n matrix 'data' +% to have a rotated data set. +% +% % Example: generate data on a unit sphere +% n = 50; +% theta = linspace(0,pi*1.5,n); +% data = 5*[cos(theta); sin(theta)] + randn(2,n); +% data = data/10; +% data = ExpNP(data); +% % calculate extrinsic mean +% c0 = mean(data,2); +% c0 = c0/norm(c0); +% % rotate whole data in a way that the EM moves to the north +% % pole. +% rotM(c0)*data +% +% +% See also ExpNP, LogNP, geodmeanS2. + +% Last updated Aug 10, 2009 +% Sungkyu Jung + + +if nargin == 1 % then perform rotation that moves 'v' to the 'north pole'. + st = v / norm(v); + acangle = st(3); + cosa = acangle; + sina = sqrt(1-acangle^2); + if (1-acangle)>1e-16 + v = [st(2);-st(1);0]/sina; + else + v = [0;0;0]; + end +else % then perform rotation with axis 'v' and angle 'angle' + v = v/norm(v); + cosa = cos(angle); + sina = sin(angle); +end + +vera = 1 - cosa; + +x = v(1); +y = v(2); +z = v(3); + +rot = [cosa+x^2*vera x*y*vera-z*sina x*z*vera+y*sina; ... + x*y*vera+z*sina cosa+y^2*vera y*z*vera-x*sina; ... + x*z*vera-y*sina y*z*vera+x*sina cosa+z^2*vera];