Rotationally align clouds of points (generalized Procrustes problem) function X = generalized_procrustes(A_measure) The input is a 3D matrix A_measure of size nxmxN. Each of the N slices A_measure(:, :, i) is a cloud of m points in R^n. These clouds are assumed to be (noisy) rotated versions of a reference cloud Atrue. This algorithm tries to find the optimal rotations to apply to the individual clouds such that they will match each other as much as possible following a least-squares cost. The output A is an estimate of the cloud Atrue (up to rotation). The output R is a 3D matrix of size nxnxN containing the rotation matrices such that R(:, :, i) * A is approximately equal to A_measure(:, :, i).
0001 function [A, R] = generalized_procrustes(A_measure) 0002 % Rotationally align clouds of points (generalized Procrustes problem) 0003 % 0004 % function X = generalized_procrustes(A_measure) 0005 % 0006 % The input is a 3D matrix A_measure of size nxmxN. Each of the N slices 0007 % A_measure(:, :, i) is a cloud of m points in R^n. These clouds are 0008 % assumed to be (noisy) rotated versions of a reference cloud Atrue. 0009 % This algorithm tries to find the optimal rotations to apply to the 0010 % individual clouds such that they will match each other as much as 0011 % possible following a least-squares cost. 0012 % 0013 % The output A is an estimate of the cloud Atrue (up to rotation). The 0014 % output R is a 3D matrix of size nxnxN containing the rotation matrices 0015 % such that R(:, :, i) * A is approximately equal to A_measure(:, :, i). 0016 0017 % This file is part of Manopt and is copyrighted. See the license file. 0018 % 0019 % Main author: Nicolas Boumal, July 8, 2013 0020 % Contributors: 0021 % 0022 % Change log: 0023 % 0024 0025 if ~exist('A_measure', 'var') 0026 % Generate random data to test the method. 0027 % There are N clouds of m points in R^n. Each of them is a noisy, 0028 % rotated version of a reference cloud A. Rotations are uniformly 0029 % random and noise on each rotated cloud is iid normal with 0030 % standard deviation sigma. 0031 n = 3; 0032 m = 10; 0033 N = 50; 0034 % The reference cloud 0035 Atrue = randn(n, m); 0036 % A 3D matrix containing the N measured clouds 0037 sigma = .3; 0038 A_measure = multiprod(randrot(n, N), Atrue) + sigma*randn(n, m, N); 0039 else 0040 [n, m, N] = size(A_measure); 0041 end 0042 0043 % Construct a manifold structure representing the product of groups of 0044 % rotations with the Euclidean space for A. We optimize simultaneously 0045 % for the reference cloud and for the rotations that affect each of the 0046 % measured clouds. Notice that there is a group invariance because 0047 % there is no way of telling which orientation the reference cloud 0048 % should be in. 0049 tuple.R = rotationsfactory(n, N); 0050 tuple.A = euclideanfactory(n, m); 0051 M = productmanifold(tuple); 0052 0053 % Define the cost function here. Points on the manifold M are 0054 % structures with fields X.A and X.R, containing matrices of sizes 0055 % respectively nxm and nxnxN. The store structure (the caching system) 0056 % is used to keep the residue matrix E in memory, as it is also used in 0057 % the computation of the gradient and of the Hessian. This way, we 0058 % prevent redundant computations. 0059 function [f, store] = cost(X, store) 0060 if ~isfield(store, 'E') 0061 R = X.R; 0062 A = X.A; 0063 store.E = multiprod(R, A) - A_measure; 0064 end 0065 E = store.E; 0066 f = (E(:)'*E(:))/(2*N); 0067 end 0068 0069 % Riemannian gradient of the cost function. 0070 function [g, store] = grad(X, store) 0071 R = X.R; 0072 A = X.A; 0073 if ~isfield(store, 'E') 0074 [~, store] = cost(X, store); 0075 end 0076 E = store.E; 0077 % Compute the Euclidean gradient of the cost wrt the rotations R 0078 % and wrt the cloud A, 0079 egrad.R = multiprod(E, A'/N); 0080 egrad.A = A - mean(multiprod(multitransp(R), A_measure), 3); 0081 % then transform this Euclidean gradient into the Riemannian 0082 % gradient. 0083 g = M.egrad2rgrad(X, egrad); 0084 store.egrad = egrad; 0085 end 0086 0087 % It is not necessary to define the Hessian of the cost. We do it 0088 % mostly to illustrate how to do it and to study the spectrum of the 0089 % Hessian at the solution (see further down). 0090 function [h, store] = hess(X, Xdot, store) 0091 R = X.R; 0092 A = X.A; 0093 % Careful: tangent vectors on the rotation group are represented as 0094 % skew symmetric matrices. To obtain the corresponding vectors in 0095 % the ambient space, we need a little transformation. This 0096 % transformation is typically not needed when we compute the 0097 % formulas for the gradient and the Hessian directly in Riemannian 0098 % form instead of resorting the egrad2rgrad and ehess2rhess. These 0099 % latter tools are convenient for prototyping but are not always 0100 % the most efficient form to execute the computations. 0101 Rdot = tuple.R.tangent2ambient(R, Xdot.R); 0102 Adot = Xdot.A; 0103 if ~isfield(store, 'egrad') 0104 [~, store] = grad(X, store); 0105 end 0106 E = store.E; 0107 egrad = store.egrad; 0108 0109 ehess.R = multiprod(multiprod(Rdot, A) + multiprod(R, Adot), A') + ... 0110 multiprod(E, Adot'); 0111 ehess.R = ehess.R / N; 0112 ehess.A = Adot-mean(multiprod(multitransp(Rdot), A_measure), 3); 0113 0114 h = M.ehess2rhess(X, egrad, ehess, Xdot); 0115 end 0116 0117 % Setup the problem structure with manifold M and cost+grad functions. 0118 problem.M = M; 0119 problem.cost = @cost; 0120 problem.grad = @grad; 0121 problem.hess = @hess; 0122 0123 % For debugging, it's always nice to check the gradient a few times. 0124 % checkgradient(problem); 0125 % pause; 0126 % checkhessian(problem); 0127 % pause; 0128 0129 % Call a solver on our problem. This can probably be much improved if a 0130 % clever initial guess is used instead of a random one. 0131 X = trustregions(problem); 0132 A = X.A; 0133 R = X.R; 0134 0135 % To evaluate the performance of the algorithm, see how well Atrue (the 0136 % reference cloud) matches A (the found cloud). Since the recovery is 0137 % up to rotation, apply Kabsch algorithm (or standard Procrustes), 0138 % i.e., compute the polar factorization to best align Atrue and A. 0139 if exist('Atrue', 'var') 0140 [U, ~, V] = svd(Atrue*A'); 0141 Ahat = (U*V')*A; 0142 fprintf('Registration error: %g.\n', norm(Atrue-Ahat, 'fro')); 0143 end 0144 0145 % Plot the spectrum of the Hessian at the solution found. 0146 % Notice that the invariance of f under a rotation yields dim SO(n), 0147 % that is, n*(n-1)/2 zero eigenvalues in the Hessian spectrum at the 0148 % solution. This indicates that critical points are not isolated and 0149 % can theoretically prevent quadratic convergence. One solution to 0150 % circumvent this would be to fix one rotation arbitrarily. Another 0151 % solution would be to work on a quotient manifold. Both can be 0152 % achieved in Manopt: they simply require a little more work on the 0153 % manifold description side. 0154 if M.dim() <= 512 0155 stairs(sort(hessianspectrum(problem, X))); 0156 title('Spectrum of the Hessian at the solution found.'); 0157 xlabel('Eigenvalue number (sorted)'); 0158 ylabel('Value of the eigenvalue'); 0159 end 0160 0161 end