Singular Value Decomposition Applications Prof. Mariolino De Cecco, Ing. Luca Baglivo 1Department of Structural Mechanical Engineering, University of Trento Email: mariolino.dececco@ing.unitn.it

The environment can be represented in different ways Two common representations are: a collection of ‘features’: in this case only a collection of interesting points is estimated. Those can be simply laser scan points or more significative features like edges, corners, lines, etc probabilistic occupancy grids: the environment is divided in cells and for each a probability of being occupied is assigned by means of proper measures (commonly laser and camera) Environment representation

Probabilistic occupancy grid Laser scan points This local map can be incrementally built to estimate the global map while using it to localize the robot: SLAM significative features like edges, corners, lines Environment representation - collection of ‘features’

On the left the environment layout, on the right a POG estimation Environment representation - probabilistic occupancy grids

To follow this example lets refer to a simulated collection of laser scan data. Data available: two laser features collection collected in two distinct time instants The differential motion will be estimated. Iterations will lead to an incremental estimation of the whole trajectory Each differential motion of a rigid body can be divided in a translation component and a rotation. Translation can be estimated by the features centre of mass, rotation by means of the following algorithm A first application of SVD to motion estimation

Local view in the second pose A first application of SVD to motion estimation

Therefore, after a rotation R, each point shall satisfy the following: So we have a collection of points that, after a proper rotation, shall correspond: Therefore, after a rotation R, each point shall satisfy the following: Which can be expressed like a minimization problem to find R: A first application of SVD to motion estimation

To do so we can modify the cost function in the following way: Unfortunately the unknown is a matrix. This means the SVD to find the pseudoinverse solution cannot be directly applied. To do so we can modify the cost function in the following way: This leads to reformulate the function in a form similar to “Ax - b” A first application of SVD to motion estimation

The following will be minimized as a function of the vector x = [r1 r2 r3]T Than x can be again rearranged in a matrix form to give the ‘best’ estimate of the rotation matrix R’ A first application of SVD to motion estimation

… but this ‘best’ estimate of the rotation matrix R will be directly useful? A first application of SVD to motion estimation

Actually not as far as this will not be a rotation matrix!!! But we remember that the polar decomposition can be regarded as a generalization to matrices of the complex number Remembering that Q represents the phase that in matrix form is a rotation matrix (in this case A is square, i.e. n=m) We can use the fact that the nearest unitary matrix to A is Q which is the unitary factor of the polar decomposition A first application of SVD to motion estimation

These are the results after applying the inverse rotation to the points collected in the second time instant: Some noise was added to the points of the second scan A first application of SVD to motion estimation

Appendix - matlab code %% % Esempi SVD per la soluzione della variazione di posa ricavata da matching di scansioni % e camera %% simulazione dati sperimentali: % punti visti dalla prima posa: % x prima riga, y seconda: P1 = [-1 0; -1 1; 1 1; 1 0; -1 0.5; -0.5 1; 0 1; 0.5 1; 1 0.5]' ; ss = size(P1) ; % matrice di rotazione tra le due pose (si suppone traslazione nulla, ovvero % di aver riportato tutto rispetto ai baricentri dei marker) % trasforma i punti visti nel sistema 2 nel sistema 1 alfa = 25 * pi / 180 ; % rotazione del secondo sistema di riferimento rispetto al primo R1_2 = [cos(alfa) -sin(alfa); sin(alfa) cos(alfa)] ; R2_1 = R1_2^-1 ; % da sistema 1 a sistema 2 % punti visti dalla seconda posa: P2 = R2_1 * P1 + normrnd(0,0.005,ss(1),ss(2)) ; Appendix - matlab code

%% definizione dati problema di ottimizzazione lineare % trasformiamo il problema di determinare la matrice di rotazione R2_1 in un problema % di minimizzazione della relazione lineare |b - A*x| % dove: % in b vi sono i dati delle scansioni acquisite nella prima posa % in A vi sono i dati delle scansioni acquisite nella seconda posa % in x vi sono le componenti della matrice di rotazione % ricaviamo il vettore 'b': b = zeros(2*length(P1), 1) ; j = 1 ; for i = 2:2:2*length(P1) b(i-1) = P1(1,j) ; b(i) = P1(2,j) ; j = j + 1 ; end % ricaviamo la matrice 'A': A = zeros(2*length(P1), 3) ; j = 1; for i = 2:2:2*length(P2) A(i-1, 1) = P2(1,j) ; A(i-1, 3) = P2(2,j) ; A(i, 2) = P2(2,j) ; A(i, 3) = - P2(1,j) ; Appendix - matlab code

This is R’ estimated as a first trial: % determiniamo 'x' mediante SVD e pseudoinversa: [U,S,V] = svd(A) ; Sinv = S ; for i = 1:3 Sinv(i,i) = S(i,i)^-1 ; end % calcola inversa della matrice dei valori singlolari % stima della matrice R1_2 r = V*Sinv'*U' * b ; R1_2est = [r(1) r(3); -r(3) r(2)] ; This is R’ estimated as a first trial: Appendix - matlab code

% POLAR DECOMPOSITION per approssimare la matrice di rotazione ad una ortonormale: % R1_2est = Q * H , dove Q è la matrice ortonormale più vicina ad R1_2est [U,S,V] = svd(R1_2est) H = V * S(1:2,:) * V' Q = U(:,1:2)*V' % si prendono solo le prime due colonne in quanto il rango è 2 R1_2estPolar = Q ; % ricostruzione dei punti: P2_1est = R1_2estPolar * P2 ; We can use the fact that the nearest rotation matrix to R’ is Q which is the unitary factor of the polar decomposition: Appendix - matlab code

… Appendix - matlab code