Singular Value Decomposition Applications · Q = U(:,1:2)*V' % si prendono solo le prime due...

1

M. De Cecco - Robotics and Sensor Fusion

Singular Value DecompositionApplications

Prof. Mariolino De Cecco, Ing. Luca Baglivo1Department of Structural Mechanical Engineering, University of Trento

Email: [email protected]


The environment can be represented in different ways

Two common representations are:

- a collection of ‘features’: in this case only a collection ofinteresting points is estimated. Those can be simply laserscan points or more significative features like edges,corners, lines, etc

- probabilistic occupancy grids: the environment is dividedin cells and for each a probability of being occupied isassigned by means of proper measures (commonly laserand camera)

Environment representation

2


-1 0 1 2 3 4 5 6

1

2

3

4

5

6

X [m]

Y [m]

Punti Scansionati

Laser scan pointsProbabilistic

occupancy grid

Environment representation - collection of ‘features’

-1 0 1 2 3 4 5 6

1

2

3

4

5

6

X [m]

Y [m]

Punti Scansionati

This local mapcan beincrementallybuilt to estimatethe global mapwhile using it tolocalize the robot:SLAM

significative features likeedges, corners, lines


On the left the environment layout, on the right a POG estimation

Environment representation - probabilistic occupancy grids

3


To follow this example lets refer to a simulated collection of laserscan data.

Data available: two laser features collection collected in two distincttime instants

The differential motion will be estimated. Iterations will lead to anincremental estimation of the whole trajectory

Each differential motion of a rigid body can be divided in a translationcomponent and a rotation. Translation can be estimated by thefeatures centre of mass, rotation by means of the following algorithm

A first application of SVD to motion estimation



Local view in the second pose

4


So we have a collection of points that, after a proper rotation, shallcorrespond:


!

scan1= Pi

1,i =1...n{ }

scan2 = Pi

2,i =1...n{ }

Therefore, after a rotation R, each point shall satisfy the following:

!

"i =1...n Pi

1 # R $Pi

2

Which can be expressed like a minimization problem to find R:

!

R =minR

R "Pi

2#P

i

1

2( )


Unfortunately the unknown is a matrix. This means the SVD to findthe preusoinverse solution cannot be directly applied.

To do so we can modify the cost function in the following way:


!

R "r1

r3

#r3

r2

$

% &

'

( )

r1

r3

#r3

r2

$

% &

'

( ) *x

y

$

% & '

( ) =

x 0 y

0 y #x

$

% &

'

( ) *

r1

r2

r3

$

%

& & &

'

(

) ) )

This leads to reformulate the function in a form similar to Ax-b

5


The following will be minimized as a function of the vector x = [r1 r2 r3]T


!

r1

r3

"r3

r2

#

$ %

&

' ( )x

y

#

$ % &

' ( =

x 0 y

0 y "x

#

$ %

&

' ( )

r1

r2

r3

#

$

% % %

&

'

( ( (

x1

20 y

1

2

0 y1

2x1

2

... ... ...

xn2

0 yn2

0 yn2 "xn

2

#

$

% % % % % %

&

'

( ( ( ( ( (

)

r1

r2

r3

#

$

% % %

&

'

( ( (

"

x1

1

y1

1

...

xn1

yn1

#

$

% % % % % %

&

'

( ( ( ( ( (

* Ar "b

!

r = A"#b

Than x can be again rearranged in amatrix form to give the ‘best’ estimateof the rotation matrix R’

!

R| "

r1

r3

#r3

r2

$

% &

'

( )



… but this ‘best’ estimate of the rotation matrix Rwill be directly useful?

6


Actually not as far as this will not be a rotation matrix!!!


But we remember that the polar decomposition can be regarded as ageneralization to matrices of the complex number

!

A =Q "H

Q =UEVE

T

H =VE#EVE

T

Remembering that Q represents the phase thatin matrix form is a rotation matrix (in this case Ais square, i.e. n=m)

We can use the fact that the nearest unitarymatrix to A is Q which is the unitary factor ofthe polar decomposition

!

R'=UE"EVE

T=Q #H

Rest$Q =U

EVE

T


These are the results after applying the inverse rotation to the pointscollected in the second time instant:


Some noise wasadded to the points ofthe second scan

7


%%% 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 1alfa = 25 * pi / 180 ; % rotazione del secondo sistema di riferimento rispetto al primoR1_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:% x prima riga, y seconda:

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) ; j = j + 1 ;end


8


% 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_2r = V*Sinv'*U' * b ;

R1_2est = [r(1) r(3); -r(3) r(2)] ;


This is R’ estimated as a first trial:

!

R| "

r1

r3

#r3

r2

$

% &

'

( )


% 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 è 2R1_2estPolar = Q ;

% ricostruzione dei punti:P2_1est = R1_2estPolar * P2 ;


We can use the fact that the nearest rotationmatrix to R’ is Q which is the unitary factor ofthe polar decomposition:

!

R'=UE"EVE

T=Q #H

Rest$Q =U

EVE

T

9


…


Singular Value Decomposition Applications · Q = U(:,1:2)*V' % si prendono solo le prime due...

Documents

Transcript of Singular Value Decomposition Applications · Q = U(:,1:2)*V' % si prendono solo le prime due...