Skip to content

pbdlib-matlab
pbdlib-matlab
Commit 9a397f8c authored by Sylvain Calinon's avatar Sylvain Calinon
Browse files
Options

Examples from R-AL 2017 paper added

parent 4c15df45
Hide whitespace changes
Inline Side-by-side
Showing with 3207 additions and 10 deletions
demo_Riemannian_quat_GMM01.m 0 → 100644
View file @ 9a397f8c
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 <http://www.gnu.org/licenses/>.
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
demo_Riemannian_quat_GMR01.m 0 → 100644
View file @ 9a397f8c
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 <http://www.gnu.org/licenses/>.
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
demo_Riemannian_quat_GMR02.m 0 → 100644
View file @ 9a397f8c
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 <http://www.gnu.org/licenses/>.
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