Assignment 4 ΟΜΑΔΑ 2 ΕΠΙΒΛΕΠΩΝ ΚΑΘΘΓΘΣΘ ΔΠΜ-ΠΡΟΗΓΜΕΝΑ ΤΣΗΜΑΣΑ ΠΑΡΑΓΩΓΗ ΑΤΣΟΜΑΣΙΜΟΤ ΚΑΙ ΡΟΜΠΟΣΙΚΗ

Transcript

1 Assignment /02/2017 Unknown ΟΜΑΔΑ 2 Δθμιτρθσ Βοςκάκθσ (mth76@edu.teicrete.gr) Νικόλαοσ Βαρδάκθσ (mth75@edu.teicrete.gr) ΕΠΙΒΛΕΠΩΝ ΚΑΘΘΓΘΣΘ Κ.Αλζξανδροσ Μακρισ Page1

2 Contents Drone Localization... 3 Θεωρθτικι Ανάλυςθ... 4 Ανάλυςθ του LEAST SQUARE SVD... 4 Ανάλυςθ του RANSAC... 5 Ανάπτυξθ του Μακθματικοφ Μοντζλου... 6 Ερμθνεία του προγράμματοσ και των ςυναρτιςεων... 7 Εφαρμογζσ Αποτελζςματα Βιβλιογραφικι Αναφορά: Page2

3 Drone Localization The aim of the project is to estimate the trajectory of a UAV (unmanned aerial vehicle, drone) using visual information. Assuming that the drone has a rigidly attached camera the problem reduces to estimating the camera motion between two consecutive frames. To test your method we provide a dataset named freiburg_floor, containing 80 rgb/depth frames and groundtruth. For more datasets visit: Implement a function: [Rt_cellarray] = CalcTrajectory(Rtinit, n_frames,rgb_tmpl, depth_tmpl, camera); that takes as input the initial transformation Rtinit (at t=1), the image template filenames, and the camera parameters and outputs the drone trajectory. To that the function should perform the following steps for each pair of consecutive frames: 1. Local feature detection (sift, surf) and matching with the features of the other image using an appropriate distance metric (e.g. euclidean distance). You can use one of the provided functions match_features_sift and match_features_surf that take as input the two images and calculate the matching feature pairs. 2. Given the feature correspondences find the rigid motion transformation from I2 to I1 i.e. the 4x4 camera rotation and translation matrix Rt. Follow the algorithm described on the paper 'Least-Squares Rigid Motion Using Svd' [e-class: svd_rot.pdf]. Try two different approaches: 1. Least squares: [ Rt ] = CalcRigidMotionSVD( P1, P2 ) For this approach you can use the matlab function svd. To debug it try it first with a set of manually defined point correspondences. 2. RANSAC: [ Rt, inliers_ratio ] = CalcRigidMotionRansac(P1,P2,Niter,dmax,Npoints ) Implement RANSAC that returns the rigid transformation with the highest inliers ratio. The RANSAC parameters are the number of iterations (Niter), the max inlier distance (dmax), and the number of correspondences to use in order to calculate the transform at each iteration (Npoints). Try different values for these parameters and comment on the results. Combine the transformation to get the trajectory for the whole sequence. Apply the method to the provided freiburg_floor dataset. Visually compare your trajectory with the provided groundtruth trajectory (file: groundtruth.txt) e.g. using matlab s scatter3 function. The Rtinit transformation and the camera parameters for this dataset are in info.txt. Page3

4 Θεωρητική Ανάλυςη τθν ςυγκεκριμζνθ άςκθςθ μασ ηθτικθκε να επιλφςουμε το πρόβλθμα τθσ τροχιάσ του τετρακόπτερου μζςω ανάλυςθσ RANSAC και LEAST SQUARE.Παρακάτω αναλφουμε τον κάκε αλγόρικμο πιο αναλυτικά. Ανάλυςη του LEAST SQUARE SVD Σο κυριότερο πρόβλθμα τθσ άςκθςθσ ιταν θ εφρεςθ του ομογενοφσ μεταςχθματιςμοφ δφο ςθμείων που βρίςκονται ςτον πραγματκό κόςμο.πιο ςυγκεκριμζνα κεωριςαμε το W ωσ μοναδιαίο.).(να επιςυμάνουμε ότι ςτθν άςκθςθ Τπολογίζοντασ τα βαρυτικά κέντρα Αρχικά υποκζτουμε ότι ο πίνακασ ςτροφισ είναι δεδομζνοσ,ζχοντασ τον παραπάνω τφπο. Τπολογίηοντασ τθν παράγωγο τθσ παραπάνω ςυνάρτθςθσ καταλιγουμε ότι ο μζςοσ όροσ των δφο ςθμείων δίνονται από τουσ τφπουσ ζτςι ο τφποσ τθσ μετατόπιςθσ καταλιγει ςτθν εξισ μορφι. Τπoλογίζοντασ τα centered vectors Αν αντικαταςτιςουμε τα δεδομζνα που βρικαμε ςτον αρχικό τφπο βρίςκουμε ότι,όπου χ και y είναι τα ςθμεία ςτο καρτεςιανό επίπεδο. Τπολογίζοντασ τον πίνακα ςτροφήσ Για τον υπολογιςμό του πίνακα ςτροφισ πρζπει να υποκζςουμε ότι θ μετατόπιςθ είναι μθδενικι.ζτςι αν επιλφςουμε τον ςυγκεκριμζνο τφπο Καταλιγουμε ότι και υπολογίηοντασ τθν SVD βρίςκουμε ότι ο πίανκασ ςτροφισ είναι Τπολογίζοντασ την μετατόπιςη Αφοφ υπολογίςαμε όλα τα απαραίτθτα δεδομζνα αντικακιςτοφμε ςτον παρακάτω τφπο:. Page4

5 Ανάλυςη του RANSAC Ο αλγόρικμοσ Random Sample Consensus ι αλλιϊσ RANSAC εκδόκθκε για πρϊτθ φορά από Fischler και Bolles ςτο SRI International το 1981.Είναι μια επαναλθπτικι μζκοδοσ για τθν εκτίμθςθ των παραμζτρων του μακθματικοφ μοντζλου από ζνα ςφνολο παρατθροφμενων δεδομζνων.ουςιαςτικά είναι μια τεχνικι μάκθςθσ για τθν εκτίμθςθ των δεδομζνων με τυχαία δειγματολθψία.όπωσ μπορείτε να δείτε ςτο παρακάτω παράδειγμα βρικαμε τθν βζλτιςτθ γραμμι βάςει των παραμζτρων που μασ δόκθκαν. Σα μπλζ ςθμεία λζγοντα inliers και τα κόκκινα outliers.θ γραμμι δθμιουργικθκε βάςει των inlier και κάποιων ςυνκθκϊν που δόκθκαν μζςα ςτο πρόγραμμα (Σο ςυγκεκριμζνο παράδειγμα δεν ζχει καμία ςχζςθ με τθν εν λόγω άςκθςθ).σζλοσ κα μποροφςαμε να αναφζρουμε περιλθπτικά τα βιματα τθσ μεκόδου RANSAC: 1. Επιλζγουμε μια τυχαία παράμετρο όπου όςα ςτοιχεία τθν πλθροφν τα ονομάηουμε inliers. 2. Διαλζγουμε κάκε φορά τυχαία ςθμεία για ζνα ςυγκεκριμζνο αρικμό επαναλιψεων.σα ςθμεία που δεν ταιριάηουν με τθν παραπάνω παράμετρο ονομάηονται outliers. 3. ε κάκε επανάλθψθ που κάνει ψθφίηει πόςοσ ιταν ο μεγαλφτεροσ αρικμόσ ςθμείων που πιρε πλθρϊντασ τισ παραμζτρουσ που του ζχουμε δϊςει. Page5

6 Ανάπτυξη του Μαθηματικού Μοντέλου Ζςτω t το διάνυςμα των τιμϊν του χρόνου, που τραβιχτθκαν οι φωτογραφίεσ, X0t το ςθμείο * και St το ςφςτθμα ςυντεταγμζνων τθ χρονικι ςτιγμι t, όπωσ δείχνει το παρακάτω ςχιμα. Ιςχφουν οι παρακάτω ςχζςεισ, όπου τα ςθμεία Χ είναι ςε Ομογενείσ ςυντεταγμζνεσ: X 1 St-1 = Rt* X 1 St X 0 St-1 = Rt* X 0 St (1) X 0 St = Rt * X 1 St+1 (2) Από τισ (1) και (2) ζπεται ότι: X 0 St-1 = Rt* Rt * X 1 St+1 (3). Θεωρϊντασ ότι Rt_init είναι ο Μεταςχθματιςμόσ του 1 ου ςθμείου ωσ προσ το φςτθμα ςυντεταγμζνων θ (3) γίνεται: X 0 St-1 = Rt_init *Rt* Rt * X 1 St+1 (4). Ζτςι: RtTotal = Rt_init *Rt* Rt (5) Εκτελϊντασ τθ ίδια διαδικαςία από εικόνα ςε εικόνα ο ςυνολικόσ μεταςχθματιςμόσ γίνεται: RtTotal = Rt_init *Rt* Rt * Rt * Rt. Ο πολλαπλαςιαςμόσ του Rt γίνεται από δεξιά, διότι γίνεται ωσ προσ το τρζχον πλαίςιο. Page6

7 Ερμηνεία του προγράμματοσ και των ςυναρτήςεων Σα προγράμματα που χρθςιμοποιικθκαν για τον προςδιοριςμό τθσ τροχιάσ εείναι το «Drone_Loc.m», και οι ςυναρτιςεισ «CalcRigidMotionSVD», «CalcRigidMotionRansac» και «calctrajectrory». Αρχικά μετονομάηω τισ εικόνεσ (0000.png, 0001.png κλπ) που αντιςτοιχοφν ςτο βάκοσ ςε (D0000.png, D0001.png κλπ), ϊςτε να μποροφν να είναι ςτον ίδιο φάκελο με τισ rgb. A) Drone_Loc Σο κυρίωσ πρόγραμμα απεικόνιςθσ τθσ τροχιάσ του Drone είναι το «Drone_Loc.m», όπου: Ορίηει τθν τροχιά του groundtrouth για να ςυγκρικεί με τθν υπολογιςμζνθ με τον παρακάτω αλγόρικμο: data=importdata('groundtruth.txt'); xd=data(:,2); yd=data(:,3); zd=data(:,4); Ειςάγει τισ παραμζτρουσ τθσ κάμερασ και τον αρικμό των εικόνων f=525; u0=319.5; v0=239.5; s=5000; Rtinit = [ ; ; ; ]; N=40; %Number of frames Καλεί τθν Rt_cellArray=calcTrajectrory( Rtinit, N, u0,v0,s,f ); Που υπολογίηει τον Ομογενι Μεταςχθματιςμό Rt από εικόνα ςε εικόνα. ε κάκε ηεφγοσ εικόνων ο Rt τοποκετείται ςε μια δομι. Με τον παρακάτω αλγόρικμο προςδιορίηονται τα διανφςματα των ςυντεταγμζνων τθσ τροχιάσ: RtTotal=Rtinit; P=Rtinit*[0;0;0;1]; x(1)=p(1); y(1)=p(2); z(1)=p(3); for i=1:n RtTotal=RtTotal*Rt_cellArray(i).pin; P=RtTotal*[0;0;0;1]; z(i+1)=p(3); x(i+1)=p(1); y(i+1)=p(2); B) CalcRigidMotionSVD a. Τπολογίηει τουσ Μζςουσ Όρουσ pav, qav των ςυντεταγμζνων του Ρ1 και Ρ2. sp1=size(p1); pav=[mean(p1(1,:)); mean(p1(2,:)); mean(p1(3,:))]; qav=[mean(p2(1,:)); mean(p2(2,:)); mean(p2(3,:))]; b. Τπολογίηει τα διανφςματα: Χ=Ρ1- pav και Τ=Ρ2- qav. for i=1:sp1(2) X(1,i)=P1(1,i)-pav(1); X(2,i)=P1(2,i)-pav(2); X(3,i)=P1(3,i)-pav(3); Y(1,i)=P2(1,i)-qav(1); Page7

8 Y(2,i)=P2(2,i)-qav(2); Y(3,i)=P2(3,i)-qav(3); c. Τπολογίηει τθν SVD, R, t και τον Ομογενι Μεταςχθματιςμό Rt S=X*Y'; [U, SS, V]=svd(S); R=V*U'; t=qav-r*pav; Rt=[R,t; ]; C) CalcRigidMotionRansac ε κάκε επανάλθψι του: 1. Διαλζγει από τα Ρ1, Ρ2 nrp ςθμεία τυχαία με το παρακάτω βρόχο. j=randperm(np,nrp); %Choose nrp points randomly for i=1:length(j) %Create Random points matrix p1r(:,i)=p1(:,j(i)); p2r(:,i)=p2(:,j(i)); 2. Καλεί τθν SVD θ οποία επιςτρζφει ζνα Rt1. Rt1 = CalcRigidMotionSVD(p1r,p2r); 3. Μετράει και προςδιορίηει τουσ Inliers, δθλαδι τα ςθμεία των Ρ1 και Ρ2 που ζχουν τθν ιδιότθτα: D<Dth. D είναι θ ευκλείδεια απόςταςθ μεταξφ των Ρ2 και των Ρ2ret, όπου Ρ2ret=Rt*P1. Σα ςθμεία αυτά τοποκετοφνται ςτουσ πίνακεσ P1rIN και P2rIN και προςδιορίηονται από τον παρακάτω αλγόρικμο: cnt=0; %Count Inliers InliersNo=0; for i=1:np P2ret(:,i)=Rt1*P1(:,i); %p2r in return a=p2(:,i); b=p2ret(:,i); D=norm(a(1:3)-b(1:3)); if D<Dth cnt=cnt+1; P1rIN(:,cnt)=P1(:,i); P2rIN(:,cnt)=P2(:,i); 4.Προςδιοριςμόσ του Inliers_ratio και του Rt που αντιςτοιχεί ςτο μζγιςτο Inliers_ratio. if InliersNo<cnt InliersNo=cnt; inliers_ratio=cnt/np; Rt = CalcRigidMotionSVD(P2rIN,P1rIN); clear P1rIN, clear P2rIN; D) calctrajectrory Ηθτάει από το χριςτθ να επιλζξει αν ο υπολογιςμόσ τθσ τροχιάσ κα είναι με τθ μζκοδο «Least Squares» ι με «RANSAC». mtd=input('1 for Least Square or 2 for RANSAC method\n'); Και ςτισ δφο περιπτϊςεισ διαβάηει τισ rgb εικόνεσ και βρίςκει τα κοινά τουσ ςθμεία με τον αλγόρικμο: I1=num2str(n,'%04d'); %Read RGB images I2=num2str(n+1,'%04d'); I1 = strcat(i1,'.png'); I1=imread(I1); I1=rgb2gray(I1); I2 = strcat(i2,'.png'); I2=imread(I2); I2=rgb2gray(I2); si1=size(i1); [P1, P2] = match_features_surf(i1,i2); Page8

9 Ο παρακάτω αλγόρικμοσ αντιςτοιχεί τα κοινά ςθμεία των rgb εικόνων με το βάκοσ τουσ. Επίςθσ, αν ο αρικμόσ των κοινϊν ςθμείων είναι μικρότεροσ του 3, διακόπτει τθ ρουτίνα διότι οι τρζχουςεσ φωτογραφίεσ δεν είναι ικανζσ να δϊςουν τθν τροχιά. P1=round(P1); P2=round(P2); Iz1=num2str(n,'D%04d'); %Read Depth images Iz1 = strcat(iz1,'.png'); Iz1=imread(Iz1); Az1=Iz1; Iz2=num2str(n+1,'D%04d'); Iz2 = strcat(iz2,'.png'); Iz2=imread(Iz2); Az2=Iz2; np=length(p1); %Corresponds RGB with Depth if np<=3 break for k=1:np x1=p1(1,k); y1=p1(2,k); P1(3,k)=double(Az1(y1,x1)); P1(4,k)=1; x2=p2(1,k); y2=p2(2,k); P2(3,k)=double(Az2(y2,x2)); %Depth Calculation P2(4,k)=1; Ζπειτα μετατρζπουμε τα Ρ1, Ρ2 ςε καρτεςιάνεσ ςυντεταγμζνεσ και ζπειτα ομογενείσ. P1(3,:)=P1(3,:)/s; P1(1,:)=(P1(1,:)-u0).*P1(3,:)/f; P1(2,:)=(P1(2,:)-v0).*P1(3,:)/f; P2(3,:)=P2(3,:)/s; P2(1,:)=(P2(1,:)-u0).*P2(3,:)/f; P2(2,:)=(P2(2,:)-v0).*P2(3,:)/f; Σζλοσ φτιάχνει μια δομι cell_array για κάκε Rt που υπολογίηει: Α. Για SVD [Rt] = CalcRigidMotionSVD(P2,P1); Rt_cellArray(cn).pin=Rt; n cn=cn+1; Β. Για RANSAC nrp=4; %No of random points. Niter=200; Dth=0.02; [ Rt, inliers_ratio ] = CalcRigidMotionRansac( P1,P2,Niter,Dth,nrp ); Rt_cellArray(cn).pin=Rt; n cn=cn+1; Page9

10 z ΔΠΜ-ΠΡΟΗΓΜΕΝΑ ΤΣΗΜΑΣΑ ΠΑΡΑΓΩΓΗ ΑΤΣΟΜΑΣΙΜΟΤ ΚΑΙ Εφαρμογέσ Αποτελέςματα Εκτελϊ το «Drone_Loc» για Ν=40 frames. Με κόκκινο ςθμειϊνεται θ πραγματικι τροχιά και με μπλε θ υπολογιςμζνθ. Θ τροχιά με τθν απλι μζκοδο SVD είναι: y x Page10

11 z ΔΠΜ-ΠΡΟΗΓΜΕΝΑ ΤΣΗΜΑΣΑ ΠΑΡΑΓΩΓΗ ΑΤΣΟΜΑΣΙΜΟΤ ΚΑΙ Με τθ μζκοδο RANSAC είναι: y x Page11

12 z ΔΠΜ-ΠΡΟΗΓΜΕΝΑ ΤΣΗΜΑΣΑ ΠΑΡΑΓΩΓΗ ΑΤΣΟΜΑΣΙΜΟΤ ΚΑΙ Για 20 frames: y x Παρατθριςεισ: 1. Θ μζκοδο SVD αποκλίνει από τθν πραγματικι τροχιά ςχεδόν από τθν εκκίνθςθ του Drone. Αυτό οφείλεται ςτο γεγονόσ ότι τα κοινά ςθμεία των γειτονικϊν φωτογραφιϊν δεν είναι όλα πραγματικά. 2. Θ μζκοδοσ RANSAC φαίνεται ότι ακολουκάει με ικανοποιθτικι ακρίβεια τθν πραγματικι τροχιά για περίπου 17 frames και ζπειτα αποκλίνει. Θ υπολογιςμζνθ τροχιά διαφζρει λίγο κάκε φορά που εκτελείται το πρόγραμμα, διότι λαμβάνει τυχαία ςθμεία, τα οποία διαφζρουν. Page12

13 Βιβλιογραφική Αναφορά: 1. Forsyth 22.1, Szeliski A.2, 4.3.2, Lectures Alexandros Makris 4. A. Horn, Doubly stochastic matrices and the diagonal of a rotation matrix. Amer. J. Math. Page13

14 Annotations ΔΙΠΛΩΜΑ ΕΙΔΙΚΕΥΣΗΣ Assignment 4 01 Unknown Unknown Page 1 10/2/ :37 B:10/10