Commit cf8c8e33 authored by Sylvain CALINON's avatar Sylvain CALINON

IROS'17 paper examples added with demo_Riemannian_cov_* filenames

parent 29a98549
......@@ -39,22 +39,35 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[3] Riemannian manifolds:
[3] Riemannian manifolds (S2,S3):
```
@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",
title="An Approach for Imitation Learning on {R}iemannian Manifolds",
journal="{IEEE} Robotics and Automation Letters ({RA-L})",
doi="",
doi="10.1109/LRA.2017.2657001",
year="2017",
month="",
volume="",
number="",
month="June",
volume="2",
number="3",
pages="1240--1247"
}
```
[4] Riemannian manifolds (S+):
```
@inproceedings{Jaquier17IROS,
author="Jaquier, N. and Calinon, S.",
title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with {sEMG}",
booktitle="Proc. {IEEE/RSJ} Intl Conf. on Intelligent Robots and Systems ({IROS})",
year="2017",
month="September",
address="Vancouver, Canada",
pages=""
}
```
[4] Semi-tied GMM:
[5] Semi-tied GMM:
```
@article{Tanwani16RAL,
author="Tanwani, A. K. and Calinon, S.",
......@@ -69,7 +82,7 @@ Did you find PbDLib useful for your research? Please acknowledge the authors in
}
```
[5] DP-means:
[6] DP-means:
```
@article{Bruno16AURO,
author="Bruno, D. and Calinon, S. and Caldwell, D. G.",
......@@ -110,7 +123,7 @@ All the examples are located in the main folder, and the functions are located i
| demo_DMP_GMR04 | [1] | Same as demo_DMP_GMR03 by using the task-parameterized model formalism |
| demo_DMP_GMR_LQR01 | [1] | Same as demo_DMP_GMR04 but with LQR used to refine the parameters of the spring-damper system |
| demo_DMP_GMR_LQR02 | [1] | Same as demo_DMP_GMR_LQR01 with perturbations added to show the benefit of full covariance to coordinate disturbance rejection |
| demo_DPMeans_Online01 | [5] | Online clustering with DP-means algorithm |
| demo_DPMeans_Online01 | [6] | Online clustering with DP-means algorithm |
| demo_DSGMR01 | [1] | Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, with decay variable used as input (as in DMP) |
| demo_DTW01 | [1] | Trajectory realignment through dynamic time warping (DTW) |
| demo_GMM01 | [1] | Gaussian mixture model (GMM) parameters estimation |
......@@ -139,6 +152,15 @@ All the examples are located in the main folder, and the functions are located i
| demo_MPPCA01 | [1] | Mixture of probabilistic principal component analyzers (MPPCA) |
| demo_regularization01 | [1] | Regularization of GMM parameters with minimum admissible eigenvalue |
| demo_regularization02 | [1] | Regularization of GMM parameters with the addition of a small circular covariance |
| demo_Riemannian_cov_GMM01 | [4] | GMM for covariance data by relying on Riemannian manifold |
| demo_Riemannian_cov_GMR01 | [4] | GMR with time as input and covariance data as output by relying on Riemannian manifold |
| demo_Riemannian_cov_GMR02 | [4] | GMR with time as input and position vector as output with comparison between computation in vector and matrix forms |
| demo_Riemannian_cov_GMR03 | [4] | GMR with vector as input and covariance data as output by relying on Riemannian manifold |
| demo_Riemannian_cov_interp01 | [4] | Covariance interpolation on Riemannian manifold |
| demo_Riemannian_cov_interp02 | [4] | Covariance interpolation on Riemannian manifold from a GMM with augmented covariances |
| demo_Riemannian_cov_interp03 | [4] | Trajectory morphing through covariance interpolation on Riemannian manifold (with augmented Gaussian trajectory distribution) |
| demo_Riemannian_cov_search01 | [4] | EM-based stochastic optimization of covariance on Riemannian manifold |
| demo_Riemannian_cov_vecTransp01 | [4] | Verification of angle conservation in parallel transport on the symmetric positive definite |
| demo_Riemannian_sphere_GaussProd01 | [3] | Product of Gaussians on a sphere by relying on Riemannian manifold |
| demo_Riemannian_sphere_GMM01 | [3] | GMM for data on a sphere by relying on Riemannian manifold |
| demo_Riemannian_sphere_GMR01 | [3] | GMR with input and output data on a sphere by relying on Riemannian manifold |
......@@ -154,7 +176,7 @@ All the examples are located in the main folder, and the functions are located i
| demo_Riemannian_quat_vecTransp01 | [3] | Parallel transport for unit quaternions |
| demo_SEDS01 | [1] | Continuous autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
| demo_SEDS_discrete01 | [1] | Discrete autonomous dynamical system with state-space encoding using GMM, with GMR used for reproduction by using a constrained optimization similar to the SEDS approach |
| demo_semitiedGMM01 | [4] | Semi-tied Gaussian Mixture Model by tying the covariance matrices of a GMM with a set of common basis vectors |
| demo_semitiedGMM01 | [5] | Semi-tied Gaussian Mixture Model by tying the covariance matrices of a GMM with a set of common basis vectors |
| demo_stdPGMM01 | [1] | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
| demo_testDampingRatio01 | [1] | Test with critically damped system and ideal underdamped system |
| demo_testLQR01 | [1] | Test of linear quadratic regulation (LQR) with different variance in the data |
......@@ -179,7 +201,7 @@ All the examples are located in the main folder, and the functions are located i
| demo_TPtrajGMM01 | [1] | Task-parameterized model with trajectory-GMM encoding |
| demo_trajDistrib01 | [1] | Stochastic sampling with Gaussian trajectory distribution |
| demo_trajGMM01 | [1] | Reproduction of trajectory with a GMM with dynamic features (trajectory-GMM) |
| demo_trajHSMM01 | [1] | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
| demo_trajHSMM01 | [2] | Trajectory synthesis with an HSMM with dynamic features (trajectory-HSMM) |
| demo_trajMFA01 | [1] | Trajectory model with either a mixture of factor analysers (MFA), a mixture of probabilistic principal component analyzers (MPPCA), or a high-dimensional data clustering approach (HD-GMM) |
| demoIK_nullspace_TPGMM01 | [1] | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
| demoIK_pointing_TPGMM01 | [1] | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) |
......
function demo_Riemannian_cov_GMM01
% GMM for covariance data by relying on Riemannian manifold computation between the
% concatenation of the data in two different vector forms and matrix data
%
% 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{Jaquier17IROS,
% author="Jaquier, N. and Calinon, S.",
% title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds:
% Application to Wrist Motion Estimation with s{EMG}",
% year="2017",
% booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
% address = "Vancouver, Canada"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon and Noémie 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 <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 30; % Number of datapoints
nbSamples = 1; % Number of demonstrations
nbIter = 5; % Number of iteration for the Gauss Newton algorithm
nbIterEM = 5; % Number of iteration for the EM algorithm
model.nbStates = 3; % Number of states in the GMM
model.nbVar = 2; % Dimension of the manifold and tangent space (2^2 data)
model.params_diagRegFact = 1E-4; % Regularization of covariance
e0 = eye(model.nbVar);
% Initialisation of the covariance transformation
[covOrder4to2, covOrder2to4] = set_covOrder4to2(model.nbVar);
% Tensor regularization term
tensor_diagRegFact_mat = eye(model.nbVar + model.nbVar * (model.nbVar-1)/2);
tensor_diagRegFact = covOrder2to4(tensor_diagRegFact_mat);
%% Generate covariance data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:model.nbStates
xn = randn(model.nbVar,3);
S(:,:,i) = cov(xn');
end
x = zeros(model.nbVar, model.nbVar, nbData*nbSamples);
idList = repmat(kron(1:model.nbStates,ones(1,ceil(nbData/model.nbStates))),1,nbSamples);
for t=1:nbData*nbSamples
xn = randn(model.nbVar,5) * 3E-1;
x(:,:,t) = S(:,:,idList(t)) + cov(xn');
end
xvec = reshape(x, [model.nbVar^2, nbData*nbSamples]); % Data on the manifold
uvec = logmap_vec(xvec, reshape(e0,[model.nbVar^2,1])); % Data on the tangent space
%% GMM parameters estimation (computation in vector form v = [x(1,1),x(2,2),sqrt(2)*x(1,2)])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
modelv2 = model;
d = 2;
modelv2.nbVar = d + d*(d-1)/2; %Dimension of the manifold and tangent space
xcvec = symMat2Vec(x); % Data on the manifold
ucvec = symMat2Vec(logmap(x,e0)); % Data on the tangent space
modelv2 = init_GMM_kbins(ucvec, modelv2, nbSamples);
modelv2.MuMan = expmap_vec2(modelv2.Mu, symMat2Vec(e0)); % Center on the manifold
modelv2.Mu = zeros(size(modelv2.MuMan)); % Center in the tangent plane at point MuMan of the manifold
L = zeros(modelv2.nbStates, nbData*nbSamples);
xts = zeros(modelv2.nbVar, nbData*nbSamples, modelv2.nbStates);
for nb=1:nbIterEM
% E-step
for i=1:modelv2.nbStates
xts(:,:,i) = logmap_vec2(xcvec, modelv2.MuMan(:,i));
L(i,:) = modelv2.Priors(i) * gaussPDF(xts(:,:,i), modelv2.Mu(:,i), modelv2.Sigma(:,:,i));
end
GAMMA = L ./ repmat(sum(L,1)+realmin, modelv2.nbStates, 1);
H = GAMMA ./ repmat(sum(GAMMA,2)+realmin, 1, nbData*nbSamples);
% M-step
for i=1:modelv2.nbStates
% Update Priors
modelv2.Priors(i) = sum(GAMMA(i,:)) / (nbData*nbSamples);
% Update MuMan
for n=1:nbIter
uTmp = logmap_vec2(xcvec, modelv2.MuMan(:,i));
modelv2.MuMan(:,i) = expmap_vec2(uTmp * H(i,:)', modelv2.MuMan(:,i));
end
% Update Sigma
modelv2.Sigma(:,:,i) = uTmp * diag(H(i,:)) * uTmp' + eye(modelv2.nbVar) * modelv2.params_diagRegFact;
end
end
%% GMM parameters estimation (computation in matrix form)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
modelm = model;
ucvec = symMat2Vec(logmap(x,e0));
% Initialisation in vector form
modelm = init_GMM_kbins(ucvec, modelm, nbSamples);
% Conversion to matrix form
modelm.MuMan = expmap(vec2symMat(modelm.Mu), e0);
modelm.Mu = zeros(size(modelm.MuMan));
sigma = modelm.Sigma;
modelm.Sigma = zeros(modelm.nbVar,modelm.nbVar,modelm.nbVar,modelm.nbVar,modelm.nbStates);
for i = 1:modelm.nbStates
modelm.Sigma(:,:,:,:,i) = covOrder2to4(sigma(:,:,i));
end
L = zeros(modelm.nbStates, nbData*nbSamples);
L2 = zeros(modelm.nbStates, nbData*nbSamples);
Xts = zeros(modelm.nbVar, modelm.nbVar, nbData*nbSamples, modelm.nbStates);
for nb=1:nbIterEM
% E-step
for i=1:modelm.nbStates
Xts(:,:,:,i) = logmap(x, modelm.MuMan(:,:,i));
% L2 and L are equivalent
L2(i,:) = modelm.Priors(i) * gaussPDF_tensor(Xts(:,:,:,i), modelm.Mu(:,:,i), modelm.Sigma(:,:,:,:,i), covOrder4to2);
xts = symMat2Vec(Xts(:,:,:,i));
MuVec = symMat2Vec(modelm.Mu(:,:,i));
SigmaVec = covOrder4to2(modelm.Sigma(:,:,:,:,i));
L(i,:) = modelm.Priors(i) * gaussPDF(xts, MuVec, SigmaVec);
end
GAMMA = L ./ repmat(sum(L,1)+realmin, modelm.nbStates, 1);
H = GAMMA ./ repmat(sum(GAMMA,2)+realmin, 1, nbData*nbSamples);
% M-step
for i=1:modelm.nbStates
% Update Priors
modelm.Priors(i) = sum(GAMMA(i,:)) / (nbData*nbSamples);
% Update MuMan
for n=1:nbIter
uTmpTot = zeros(modelm.nbVar,modelm.nbVar);
uTmp = logmap(x, modelm.MuMan(:,:,i));
for k = 1:nbData*nbSamples
uTmpTot = uTmpTot + uTmp(:,:,k) .* H(i,k);
end
modelm.MuMan(:,:,i) = expmap(uTmpTot, modelm.MuMan(:,:,i));
end
% Update Sigma
modelm.Sigma(:,:,:,:,i) = zeros(modelm.nbVar,modelm.nbVar,modelm.nbVar,modelm.nbVar);
for k = 1:nbData*nbSamples
modelm.Sigma(:,:,:,:,i) = modelm.Sigma(:,:,:,:,i) + H(i,k) .* outerprod(uTmp(:,:,k),uTmp(:,:,k));
end
modelm.Sigma(:,:,:,:,i) = modelm.Sigma(:,:,:,:,i) + tensor_diagRegFact.*modelm.params_diagRegFact;
end
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Vector form v = [x(1,1),x(2,2),sqrt(2)*x(1,2)])
figure('PaperPosition',[0 0 8 8],'position',[10,10,1650,1250], 'Name', 'Computation in vector form');
hold on; axis off;
clrmap = lines(modelv2.nbStates);
for t=1:nbData
plotGMM(zeros(2,1), x(:,:,t), [.6 .6 .6], .05);
end
for i=1:modelv2.nbStates
plotGMM(zeros(2,1), vec2symMat(modelv2.MuMan(:,i))*.8, clrmap(i,:), .3);
end
axis equal;
% Matrix form
figure('PaperPosition',[0 0 8 8],'position',[10,10,1650,1250],'Name', 'Computation in matrix form');
hold on; axis off;
clrmap = lines(modelm.nbStates);
for t=1:nbData
plotGMM(zeros(2,1), x(:,:,t), [.6 .6 .6], .05);
end
axis equal;
%print('-dpng','graphs/demo_Riemannian_cov_GMM01a.png');
for i=1:modelm.nbStates
plotGMM(zeros(2,1), modelm.MuMan(:,:,i)*.8, clrmap(i,:), .3);
end
%print('-dpng','graphs/demo_Riemannian_cov_GMM01b.png');
% %Plot activation functions
% figure; hold on;
% clrmap = lines(model.nbStates);
% for i=1:model.nbStates
% plot(1:nbData, GAMMA(i,:),'linewidth',2,'color',clrmap(i,:));
% end
% axis([1, nbData, 0, 1.02]);
% set(gca,'xtick',[],'ytick',[]);
% xlabel('t'); ylabel('h_i');
% pause;
% close all;
end
%% Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function X = expmap(U,S)
% Exponential map (SPD manifold)
N = size(U,3);
for n = 1:N
X(:,:,n) = S^.5 * expm(S^-.5 * U(:,:,n) * S^-.5) * S^.5;
end
end
function U = logmap(X,S)
% Logarithm map (SPD manifold)
N = size(X,3);
for n = 1:N
U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
end
end
function x = expmap_vec(u,s)
% Exponential map for the first vector form (SPD manifold)
nbData = size(u,2);
d = size(u,1)^.5;
U = reshape(u, [d, d, nbData]);
S = reshape(s, [d, d]);
x = zeros(d^2, nbData);
for t=1:nbData
x(:,t) = reshape(expmap(U(:,:,t),S), [d^2, 1]);
end
end
function u = logmap_vec(x,s)
% Logarithm map for the first vector form (SPD manifold)
nbData = size(x,2);
d = size(x,1)^.5;
X = reshape(x, [d, d, nbData]);
S = reshape(s, [d, d]);
u = zeros(d^2, nbData);
for t=1:nbData
u(:,t) = reshape(logmap(X(:,:,t),S), [d^2, 1]);
end
end
function x = expmap_vec2(u,s)
% Exponential map for the second vector form (SPD manifold)
U = vec2symMat(u);
S = vec2symMat(s);
x = symMat2Vec(expmap(U,S));
end
function u = logmap_vec2(x,s)
% Logarithm map for the second vector form (SPD manifold)
X = vec2symMat(x);
S = vec2symMat(s);
u = symMat2Vec(logmap(X,S));
end
function prob = gaussPDF_tensor(Data, Mu, Sigma, covOrder4to2)
% Likelihood of matrix datapoint(s) to be generated by a Gaussian
% parameterized by center and 4th-order covariance tensor.
[nbVar, ~, nbData] = size(Data);
% Substract mean
Data = Data - repmat(Mu,1,1,nbData);
% Compute inverse and determinant of covariance
[~, V, D] = covOrder4to2(Sigma);
detSigmaInv = sum(diag(D).^-1);
Sinv = zeros(size(Sigma));
for j = 1:size(V,3)
Sinv = Sinv + D(j,j)^-1 .* outerprod(V(:,:,j),V(:,:,j));
end
% Compute Gaussian PDF
prob = zeros(nbData,1);
for n = 1:nbData
prob(n,1) = tensor4o_mult(permute(Data(:,:,n),[3,4,1,2]),tensor4o_mult(Sinv,Data(:,:,n)));
end
prob = exp(-0.5*prob) / sqrt((2*pi)^(nbVar+nbVar*(nbVar-1)/2) * abs(detSigmaInv) + realmin);
end
function v = symMat2Vec(S)
% Reduced vectorisation of a symmetric matrix.
[d,~,N] = size(S);
v = zeros(d+d*(d-1)/2,N);
for n = 1:N
v(1:d,n) = diag(S(:,:,n));
row = d+1;
for i = 1:d-1
v(row:row + d-1-i,n) = sqrt(2).*S(i+1:end,i,n);
row = row + d-i;
end
end
end
function S = vec2symMat(v)
% Matricisation of a vector to a symmetric matrix.
[t, N] = size(v);
d = (-1 + sqrt(1+8*t))/2;
S = zeros(d,d,N);
for n= 1:N
% Side elements
i = d+1;
for row = 1:d-1
S(row,row+1:d,n) = v(i:i+d-1-row,n)./sqrt(2);
i = i+d-row;
end
S(:,:,n) = S(:,:,n) + S(:,:,n)';
% Diagonal elements
S(:,:,n) = S(:,:,n) + diag(v(1:d,n));
end
end
function M = tensor2mat(T, rows, cols)
% Matricisation of a tensor (he rows, respectively columns of the matrix
% are 'rows', respectively 'cols' of the tensor)
if nargin <=2
cols = [];
end
sizeT = size(T);
N = ndims(T);
if isempty(rows)
rows = 1:N;
rows(cols) = [];
end
if isempty(cols)
cols = 1:N;
cols(rows) = [];
end
if any(rows(:)' ~= 1:length(rows)) || any(cols(:)' ~= length(rows)+(1:length(cols)))
T = permute(T,[rows(:)' cols(:)']);
end
M = reshape(T,prod(sizeT(rows)), prod(sizeT(cols)));
end
function T = tensor4o_mult(A,B)
% Multiplication of two 4th-order tensors A and B
if ndims(A) == 4 || ndims(B) == 4
sizeA = size(A);
sizeB = size(B);
if ismatrix(A)
sizeA(3:4) = [1,1];
end
if ismatrix(B)
sizeB(3:4) = [1,1];
end
if sizeA(3) ~= sizeB(1) || sizeA(4) ~= sizeB(2)
error('Dimensions mismatch: two last dim of A should be the same than two first dim of B.');
end
T = reshape(tensor2mat(A,[1,2]) * tensor2mat(B,[1,2]), [sizeA(1),sizeA(2),sizeB(3),sizeB(4)]);
else
if ismatrix(A) && isscalar(B)
T = A*B;
else
error('Dimensions mismatch.');
end
end
end
function [covOrder4to2, covOrder2to4] = set_covOrder4to2(dim)
% Set the factors necessary to simplify a 4th-order covariance of symmetric
% matrix to a 2nd-order covariance. The dimension ofthe 4th-order covariance is
% dim x dim x dim x dim. Return the functions covOrder4to2 and covOrder2to4.
% This function must be called one time for each covariance's size.
newDim = dim+dim*(dim-1)/2;
% Computation of the indices and coefficients to transform 4th-order
% covariances to 2nd-order covariances
sizeS = [dim,dim,dim,dim];
sizeSred = [newDim,newDim];
id = [];
idred = [];
coeffs = [];
% left-up part
for k = 1:dim
for m = 1:dim
id = [id,sub2ind(sizeS,k,k,m,m)];
idred = [idred,sub2ind(sizeSred,k,m)];
coeffs = [coeffs,1];
end
end
% right-down part
row = dim+1; col = dim+1;
for k = 1:dim-1
for m = k+1:dim
for p = 1:dim-1
for q = p+1:dim
id = [id,sub2ind(sizeS,k,m,p,q)];
idred = [idred,sub2ind(sizeSred,row,col)];
coeffs = [coeffs,2];
col = col+1;
end
end
row = row + 1;
col = dim+1;
end
end
% side-parts
for k = 1:dim
col = dim+1;
for p = 1:dim-1
for q = p+1:dim
id = [id,sub2ind(sizeS,k,k,p,q)];
idred = [idred,sub2ind(sizeSred,k,col)];
id = [id,sub2ind(sizeS,k,k,p,q)];
idred = [idred,sub2ind(sizeSred,col,k)];
coeffs = [coeffs,sqrt(2),sqrt(2)];
col = col+1;
end
end
end
% Computation of the indices and coefficients to transform eigenvectors to
% eigentensors
sizeV = [dim,dim,newDim];
sizeVred = [newDim,newDim];
idEig = [];
idredEig = [];
coeffsEig = [];
for n = 1:newDim
% diagonal part
for j = 1:dim
idEig = [idEig,sub2ind(sizeV,j,j,n)];
idredEig = [idredEig,sub2ind(sizeVred,j,n)];
coeffsEig = [coeffsEig,1];
end
% side parts
j = dim+1;
for k = 1:dim-1
for m = k+1:dim
idEig = [idEig,sub2ind(sizeV,k,m,n)];
idredEig = [idredEig,sub2ind(sizeVred,j,n)];
idEig = [idEig,sub2ind(sizeV,m,k,n)];
idredEig = [idredEig,sub2ind(sizeVred,j,n)];
coeffsEig = [coeffsEig,1/sqrt(2),1/sqrt(2)];
j = j+1;
end
end
end
function [Sred, V, D] = def_covOrder4to2(S)
Sred = zeros(newDim,newDim);
Sred(idred) = bsxfun(@times,S(id),coeffs);
[v,D] = eig(Sred);
V = zeros(dim,dim,newDim);
V(idEig) = bsxfun(@times,v(idredEig),coeffsEig);
end
function [S, V, D] = def_covOrder2to4(Sred)
[v,D] = eig(Sred);
V = zeros(dim,dim,newDim);
V(idEig) = bsxfun(@times,v(idredEig),coeffsEig);
S = zeros(dim,dim,dim,dim);
for i = 1:size(V,3)
S = S + D(i,i).*outerprod(V(:,:,i),V(:,:,i));
end
end
covOrder4to2 = @def_covOrder4to2;
covOrder2to4 = @def_covOrder2to4;
end
function demo_Riemannian_cov_GMR01
% GMR with time as input and covariance 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{Jaquier17IROS,
% author="Jaquier, N. and Calinon, S.",
% title="Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds:
% Application to Wrist Motion Estimation with s{EMG}",
% year="2017",
% booktitle = "{IEEE/RSJ} Intl. Conf. on Intelligent Robots and Systems ({IROS})",
% address = "Vancouver, Canada"
% }
%
% Copyright (c) 2017 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon and Noémie 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 <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 30; % Number of datapoints
nbSamples = 1; % Number of demonstrations
nbIter = 5; % Number of iteration for the Gauss Newton algorithm
nbIterEM = 5; % Number of iteration for the EM algorithm
model.nbStates = 3; % Number of states in the GMM
model.nbVar = 3; % Dimension of the manifold and tangent space (1D input + 2^2 output)
model.nbVarCovOut = model.nbVar + model.nbVar*(model.nbVar-1)/2; %Dimension of the output covariance
model.dt = 1E-1; % Time step duration
model.params_diagRegFact = 1E-4; % Regularization of covariance
% Initialisation of the covariance transformation
[covOrder4to2, covOrder2to4] = set_covOrder4to2(model.nbVar);
% Tensor regularization term
tensor_diagRegFact_mat = eye(model.nbVar + model.nbVar * (model.nbVar-1)/2);
tensor_diagRegFact = covOrder2to4(tensor_diagRegFact_mat);
%% Generate covariance data from handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbSamples0 = 5;
demos=[];
load('./data/2Dletters/C.mat');
xIn(1,:) = [1:nbData] * model.dt;
Data=[];
for n=1:nbSamples0
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Data = [Data [xIn; s(n).Data]];
end
mtmp = model;
mtmp.nbStates = 3;
mtmp.params_diagRegFact = 1E-4;
mtmp = init_GMM_kbins(Data, mtmp, nbSamples0);
%mtmp = init_GMM_timeBased(Data, mtmp);
%mtmp = EM_GMM(Data, mtmp);
[~, SigmaOut] = GMR(mtmp, xIn, 1, 2:model.nbVar);
X = zeros(model.nbVar, model.nbVar, nbData*nbSamples);
X(1,1,:) = reshape(repmat(xIn,1,nbSamples),1,1,nbData*nbSamples);
X(2:end,2:end,:) = SigmaOut;
% %% Generate covariance data from rotating covariance
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% xIn(1,:) = [1:nbData] * model.dt;
%
% Vdata = eye(model.nbVar-1);
% Ddata = eye(model.nbVar-1);
%
% X = zeros(3,3,nbData*nbSamples);
% X(1,1,:) = reshape(repmat(xIn,1,nbSamples),1,1,nbData*nbSamples);
%
% for t = 1:nbData
% Ddata(1,1) = t * 1E-1;
% a = pi/2 * t * 1E-1;
% R = [cos(a) -sin(a); sin(a) cos(a)];
% V2 = R * Vdata;
% X(2:3,2:3,t) = V2 * Ddata * V2';
% end
%% GMM parameters estimation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('Learning...');
% Initialisation on the manifold
in=1; out=2:model.nbVar;
id = cov_findNonZeroID(in, out, 0, 0);