SVD - Application to rigid-motion estimation

Dimensione: px

Iniziare la visualizzazioe della pagina:

Download "SVD - Application to rigid-motion estimation"

Luigi Evaristo Berardino
5 anni fa
Visualizzazioni

SVD - Application to rigid-motion estimation Now we use SVD to estimate the rigid motion of the observer (usually the robot) with respect to a collection of markers acquired in the environment which

1 SVD - Application to rigid-motion estimation Now we use SVD to estimate the rigid motion of the observer (usually the robot) with respect to a collection of markers acquired in the environment which must not be known a priori, i.e. which is unstructured Mariolino De Cecco Department of Structural Mechanical Engineering, University of rento mariolino.dececco@ing.unitn.it Let s suppose to acquire a laser scan of the environment and to identify as markers only edges and corners Let s consider to put the markers in the following points matrix: M = x x... x n $ ' ( ) xn # y y... y n & han the robot moves and collect the same markers in its new reference frame, those form a second points matrix: = x $ ' # y & y M = x x... x n $ ' # y y... y n & M P = x x... x n $ ' ( ) xn # y y... y n & P = x $ ' # y & x P 0 = x 0 $ ' # y 0 & If we dig-out the mean value only the rotation remains unknown: M P = R M Here the rotation matrix represents:. he coordinate transform from reference to. Rotation from frame back to

2 In literature the SVD based methods to compute rotation make use of the cross-correlation matrix instead of separately computing each SVD M M V # R =U C C = M M = M M # # R = # R =U C # C = # U U # R = R =U C C R =U C R =U C In this case, if we relax the knowledge to the markers not being correspondent, the matrix computed is not a cross-correlation matrix and therefore the computed rotation is not the right one! his is the classical weakness of many factorization methods used to estimate the rigid body motion both for the ego-motion case with respect to the unknown environment both in the case of motion capture or methods of image analysis for scene segmentation from motion analysis M # M * P = $ * p $... ' *' & ) & ) & p p... p n ) #&* p *) & ) &... () & ) * p n *( A simple example where the first two points are reversed by cross-correlation by cross-correlation In alternative we can use the SVD of each point matrix Let s see the differences: M P = R M M = M R herefore we know the following: R = V M M =U V M U =U # R =U V R = V Being the same distribution of markers, the singular values are the same U # = # In other words we know:. how to compute the rotation matrix (and easily also the translation by means of barycentre). How to verify the markers correspondence! R = V

3 What if we relax the knowledge to the markers not being correspondent (between the two point matrix rows)? he basic facts change to: R = V Let s come back to the example Legend: objects robot in the first position U #U $ = $ And therefore we still know:. how to compute the rotation matrix (and easily also the translation by means of barycentre). How to verify not strictly that the markers correspond but that the two sets are corresponding and therefore that the solution is correct. his last is very important! o + robot in the second position Markers seen in the first position Markers seen in the second position Esempi SVD per la soluzione della variazione di posa ricavata da matching di scansioni e camera in questo caso usiamo la SVD direttamente sulle matrici dei punti raccolti nelle due scansioni. simulazione dati sperimentali punti visti dalla prima posa: per ogni riga [xi yi], R nx (n righe, colonne) scan = [- 0; -. ; ; 0; - 0.3; -0.8 ; ; 0.5; 0.5 ; 0.] ; = scan - repmat(mean(scan),[size(scan,),]) compensazione baricentro 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 nel sistema alfa = 0 * pi / 80 ; rotazione del secondo sistema di riferimento rispetto al primo R_ = [cos(alfa) -sin(alfa); sin(alfa) cos(alfa)] ; R_ = R_' ; da sistema a sistema LI MESCOLIAMO! (supponiamo quindi di non conoscere la corrispondenza tra i markers) mescoliamo = 0 ; if mescoliamo n = 5 ; P = [P( n : end, : ); P( : n-, : ) ] ; end NOA: mescolarli non inficia la soluzione a meno di cambiamenti di segno!!!. aggiungiamo outlier!!! outlier = 0 ; if outlier = [ ; - 0.5] ; P = [P ; ] ; P = [P ; 0.5] ; end punti visti dalla seconda posa (x prima riga, y seconda): P = * R_' + normrnd(0, 0.00, ss(), ss()) ; 3

3. calcoliamo rotazione mediante le SVD calcolo SVD: [U, S, V] = svd( ) ; [U, S, V] = svd( P ) ; After the rotation estimation both set of markers are brought back to the same reference: verifica

4 3. calcoliamo rotazione mediante le SVD calcolo SVD: [U, S, V] = svd( ) ; [U, S, V] = svd( P ) ; After the rotation estimation both set of markers are brought back to the same reference: verifica uguaglianza matrici U si considerano solo gli autovettori significativi, ovvero quelli che moltiplicano gli elementi non-zero delle matrici S degli autovalori U(:,:) U(:,:) se si calcola la SVD della differenza entrambi i valori singolari devono avere valori comparabili con il rumore: Su = svd( U(:,:) - U(:,:) ) in particolare la seguente dovrebbe essere la matrice identità: U(:,:)' * U(:,:) si potrebbe verificare con la distanza da matrice unitaria con decomposizione polare! NOA: questa verifica non funziona se la corrispondenza tra i markers non è nota!!! stima della matrice R_ R_est = V * V' ; ricostruzione dei punti: P_est = P * R_est' ; his is the matrix U: first column the x, second column the y If we want to see numerically the results: (centre of mass compensated) =

5 U = U = Su = U(:,:)' * U(:,:) = Verifica Matrici di rotazione: R_est = R_ = Verifica Angolo di rotazione: angolovero = 0 angolostimato = What happens if the data set is corrupted from stronger noise: Mixing up the correspondences with the same noise: 5

6 If there are outliers (not corresponding features i.e. markers): he estimated angle is biased about.5 6

Documenti analoghi

Singular Value Decomposition Applications

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