ΤΕΧΝΟΛΟΓΙΚΟ ΕΚΠΑΙΔΕΥΤΙΚΟ ΙΔΡΥΜΑ ΚΡΗΤΗΣ ΔΠΜΣ ΠΡΟΗΓΜΕΝΑ ΣΥΣΤΗΜΑΤΑ ΠΑΡΑΓΩΓΗΣ ΑΥΤΟΜΑΤΙΣΜΟΥ ΚΑΙ ΡΟΜΠΟΤΙΚΗΣ ΕΡΓΑΣΙΑ 4. Drone Localization ΣΠΟΥΔΑΣΤΕΣ:

Transcript

1 ΤΕΧΝΟΛΟΓΙΚΟ ΕΚΠΑΙΔΕΥΤΙΚΟ ΙΔΡΥΜΑ ΚΡΗΤΗΣ ΔΠΜΣ ΠΡΟΗΓΜΕΝΑ ΣΥΣΤΗΜΑΤΑ ΠΑΡΑΓΩΓΗΣ ΑΥΤΟΜΑΤΙΣΜΟΥ ΚΑΙ ΡΟΜΠΟΤΙΚΗΣ 01 10/02/2017 Unknown ΕΡΓΑΣΙΑ 4 Drone Localization ΣΠΟΥΔΑΣΤΕΣ: ΓΕΡΜΕΝΗΣ ΕΥΑΓΓΕΛΟΣ (Α.Μ.: ΜΗ77) ΠΑΠΑΔΟΠΟΥΛΟΣ ΓΕΩΡΓΙΟΣ (Α.Μ.:ΜΗ79) ΕΠΙΒΛΕΠΩΝ ΚΑΘΗΓΗΤΗΣ: Δρ.ΑΛΕΞΑΝΔΡΟΣ ΜΑΚΡΗΣ ΗΡΑΚΛΕΙΟ 2017

2 Εισαγωγή Σκοπός Στόχος της παρούσας εργασίας είναι να εκτιμηθεί η τροχιά ενός UAV (Unmanned Air Vehicle, Drone) χρησιμοποιώντας οπτικές πληροφορίες. Υποθέτοντας ότι στο κέλυφος του Drone έχουμε προσαρτήσει σταθερή κάμερα το πρόβλημα ανάγεται στην εκτίμηση της κίνησης της κάμερας μεταξύ δύο διαδοχικών καρέ(frame).για την επαλήθευση της μεθόδου παρέχεται dataset από 80rgb/depth εικόνες και μία groundtruth διαδρομή. Υλοποίηση Για την επίλυση του προβλήματος δημιουργήθηκαν τρείς ενότητες που περιγράφουν τα βήματα που ακολουθήθηκαν αναλυτικά για την επίλυση του προβλήματος. Chapter 1 Chapter 2 Chapter 3 Preperation Intialization Step Analysis Step 0 : Load Frames Step 1: Feature Correspondences Step 2: Depth insert and Camera Calibration a) Load Depth b) Depth Insert c) Camera Calibration d) Prepare 3D Points for Calculation Step 3: SVD of Least Squares Step 4: Fit problem to Ransac Algorithm Step 5: Trajectory plot Trajectory Compare Ransac Svd of Least Squares Conclusion References 2

3 Chapter 1 Preparation Για την ασφαλή εισαγωγή δεδομένων σαυτό το βήμα αποφασίστηκε να αλλαχθούν τα filenames από τις depth εικόνες λόγω του ότι όλες rgb,depth βρίσκονται στο ίδιο path της Matlab με τα ίδια file name. Για την αποφυγή του σφάλματος αυτού αλλάχτηκαν όλες οι depth εικόνες κατά ένα χαρακτήρα από 0000.png σε 000.png Initialization Σαυτό το στάδιο παρουσιάζεται η πρώτη όψη του προγράμματος Ask4.m που περιέχει όλες τις αρχικοποιήσεις και καλούνται όλες οι κύριες συναρτήσεις. clc;clear all;close all; %% Rinitial Rtinit =[ ]; %% Frames Nframes=80; %% LEAST SQUARES: Trajectory Calculation [Rt_cellarray] = CalcTrajectory(Rtinit,Nframes); %% LEAST SQUARES: Trajectory Projection id='lsquare' groundtruth=dlmread('groundtruth.txt'); TrajectoryPlot(groundtruth,Rt_cellarray,id); %% RANSAC Calculation Niter=100; %Number of iterations dmax=0.03; %Maximum inlier distance Npoints=4; %Number of correspondences to use [maxr,track_points] = CalcRigidMotionRansac( Niter,dmax,Npoints,Nframes,Rtinit ); %% RANSAC Projection id='ransac' TrajectoryPlot(groundtruth,track_points,id); Section Rtinitial: Αρχικοποιείται ο ομογενής μετασχηματισμός Rtinit που περιέχει την αρχή της διαδρομής του Drone. Section Frames : Στην μεταβλητή Νframes εισάγεται ο αριθμός τον frames που πρέπει να χρησιμοποιούμαι κάθε φορά για τον υπολογισμό της διαδρομής. Καλύπτει Depth και RGB. Section Least Squares Trajectory Calculation : Καλείται η συνάρτηση [Rt_cellarray] = CalcTrajectory(Rtinit,Nframes) στην οποία εμπεριέχεται υπολογισμός της διαδρομής με βάση την αριθμητική επίλυση SVD Of LeastSquares [1]. 3

4 Section Least Squares Trajectory Projection : Καλείται η συνάρτηση TrajectoryPlot(groundtruth,Rt_cellarray,id); η οποία οπτικοποιεί κάθε φορά τα αποτελέσματα βάση του αλγορίθμου που υπολογίστηκαν(id= Ransac or Lsquare ). Section Ransac Calculation : Δημιουργήθηκε η συνάρτηση CalcRigidMotionRansac( Niter,dmax,Npoints,Nframes,Rtinit ); με τις κατάλληλες παραμέτρους για τον υπολογισμό της διαδρομής κατά Ransac. Section Ransac Projection : Καλείται η ίδια συνάρτηση όπως και προηγουμένως TrajectoryPlot(groundtruth,Rt_cellarray,id); για την γραφική απεικόνιση της διαδρομής αλλά αυτή την φόρα με id= Ransac. 4

5 Chapter 2 Step Analysis Σ αυτό το στάδιο δημιουργήθηκαν 5 αναλυτικά steps με υποενότητες.η ακολουθία τον παρακάτω βημάτων μας οδηγούν στην επίτευξη του σκοπού. Παρακάτω βλέπουμε ανοιγμένη την function [Rt_cellarray] = CalcTrajectory(Rtinit,Nframes) για να κατανοήσουμε καλύτερα το ρόλο των επόμενων step. function [track_points] = CalcTrajectory( Rtinit,Nframes) %% Drone Trajectory Computation with SVD of Least Squares(per frame) RealRt=Rtinit; %Initial Rt tra=[ ]; %Point of view track_points{1}=realrt*tra';% Initial point for n=0:nframes-1; %Frame Counter [I1,I2]=LoadFrames(n); %RGB frames (X,Y) [P1,P2] = match_features_surf(i1,i2); %Feature Match [I1z,I2z]=LoadDepth(n); %Depth frames (Z) dimension [ P1,P2 ] = DepthInsert( P1,P2,I1z,I2z ); %Insert z Dimension in 2D points [ P1,P2 ] = Camera( P1,P2 ); %Camera calibration [ Rt ] = CalcRigidMotionSVD(P1,P2); %Homogenous Matrix (Rotation&Translation) %% Drone Trajectory 3D points Computation (per frame) RealRt=RealRt*Rt; %Multiply Transform per frame track_points{n+2}=double(realrt*tra');% Trajectory points (for all Frames) Step 0 : Load Frames Function : [ I1,I2 ] = LoadFrames(n) Input: (n) αριθμός του τρέχων frame. Output : Δημιουργία ζεύγους πινάκων για 2 διαδοχικά frames [Ι1, Ι2]. function [ I1,I2 ] = LoadFrames(n) 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); Στην συνάρτηση χρησιμοποιήθηκε η εντολή num2string από matlab για την κατασκευή του filename της εικόνας. 5

6 Step 1: Feature Correspondences Function : [ P1, P2 ] = match_features_surf( Ι1,Ι2 ) Input: (I1,I2) Εικόνες από δύο διαδοχικά frames Output : Ζευγάρια όμοιων χαρακτηριστικών σε δυο διαδοχικά frames [P1, P2]. function [ P1, P2 ] = match_features_surf( li,ri ) lpoints = detectsurffeatures(li); rpoints = detectsurffeatures(ri); [lfeatures, lpoints] = extractfeatures(li, lpoints); [rfeatures, rpoints] = extractfeatures(ri, rpoints); Pairs = matchfeatures(lfeatures, rfeatures); matchedlpoints = lpoints(pairs(:, 1), :); matchedrpoints = rpoints(pairs(:, 2), :); P1 = (matchedlpoints.location)'; P2 = (matchedrpoints.location)'; % viz_features_surf( li, ri, matchedlpoints, matchedrpoints ); Σ αυτό το στάδιο υλοποιείται το Feature Matching με τη βοήθεια του αλγορίθμου Surf και της έτοιμης συνάρτηση απ το path match_features_surf.m. Η συνάρτηση μας επιστρέφει τους πίνακες με τα κοινά χαρακτηριστικά σε 2 διαστάσεις(x,y). P1=[u1;v1] και P2=[u2;v2] Step 2: Depth insert and Camera Calibration a) Load Depth Function : [I1z,I2z] = LoadDepth(n) Input: (n) αριθμός του τρέχων frame. Output : Φορτώνονται 2 διαδοχικές εικόνες depth απ το path στους πίνακες [I1z,I2z]. function [I1z,I2z] = LoadDepth(n) % [Z] Depth I1z=num2str(n,'%03d'); I2z=num2str(n+1,'%03d'); I1z = strcat(i1z,'.png'); I1z=imread(I1z); I2z = strcat(i2z,'.png'); I2z=imread(I2z); 6

7 b) Depth Insert Function : [ P1,P2 ] = DepthInsert( P1,P2,I1z,I2z ) Input: Ζευγάρια όμοιων χαρακτηριστικών σε δυο διαδοχικά frames [P1, P2] ως δείκτες και πίνακες [I1z,I2z] από (a). Output : [P1,P2] 3D. Τα σημεία μετατράπηκα σε P1=[x1,y1,z1] και P2=[x2,y2,z2] function [ P1,P2 ] = DepthInsert( P1,P2,I1z,I2z ) %insert depth Z dimension for i=1:length(p1); % P1(3,i)=I1z(round(P1(2,i)),round(P1(1,i))) P1(3,i)=I1z(floor(P1(2,i)),floor(P1(1,i))); P2(3,i)=I2z(floor(P2(2,i)),floor(P2(1,i))); c) Camera Calibration Function : [ P1,P2 ] = Camera( P1,P2 ) Input: [P1,P2] 3D σημεία από (b). Output : [P1,P2] 3D calibrated στις παραμέτρους τις κάμερας function [ P1,P2 ] = Camera( P1,P2 ) %% Camera Parameters: f=525.0; %focal length u0 = 319.5; %principal point v0 = 239.5; %principal point s = 5000; %depth scale %% Get 3d points from point/depth: %P1 P1(3,:) = P1(3,:)./s; P1(1,:) = (P1(1,:)-u0).*P1(3,:)./f; P1(2,:) = (P1(2,:)-v0).*P1(3,:)./f; %P2 P2(3,:) = P2(3,:)./s; P2(1,:) = (P2(1,:)-u0).*P2(3,:)./f; P2(2,:) = (P2(2,:)-v0).*P2(3,:)./f; % P1(3,:) is the measured depth. % P1(3,:) is the measured depth. Στο step c όλα τα σημεία προβάλλονται απ το κέντρο της κάμερας. 7

8 d) Prepare 3D Points for Calculation Function : [ P1,P2 ] = Camera( P1,P2 ) Input: [P1,P2] 3D σημεία από (c). Output : [P1,P2] πίνακες σε ομογενής συντεταγμένες. function [ P1,P2 ] = Hm( P1,P2) P2=[P2;ones(1,size(P2,2))]; P1=[P1;ones(1,size(P1,2))]; Στο βήμα αυτό τα σημεία μετατρέπονται σε ομογενή για να χρησιμοποιηθούν στη συνέχεια για τους απαιτούμενους υπολογισμούς. 8

9 Step 3: SVD of Least Squares Function : [ Rt ] = CalcRigidMotionSVD(P1,P2) Input: [P1,P2] 3D σημεία από (d). Output : [Rt] ομογενής μετασχηματισμός που σε πηγαίνει από την Ι2 εικόνα στην Ι1. function [ Rt ] = CalcRigidMotionSVD(P1,P2) %% Paper nn=length(p1);% n feature correspondences %Average Pmeso Avx=mean(P2(1,:)); Avy=mean(P2(2,:)); Avz=mean(P2(3,:)); Pmeso=[Avx;Avy;Avz]; %Average qmeso Avqx=mean(P1(1,:)); Avqy=mean(P1(2,:)); Avqz=mean(P1(3,:)); qmeso=[avqx;avqy;avqz]; %Define new xi & yi for i=1:nn-1; xi=p2(1:3,i)-pmeso; yi=p1(1:3,i)-qmeso; X(1:3,i)=xi; Y(1:3,i)=yi; %Singular Value Decomposition S1=X*Y'; %least square [U S V]=svd(S1); %svd of least square matr=[eye(size(u,1),size(u,2))]; matr(length(matr),length(matr))=det(v*u'); %% Rotation Rtn=V*matr*U'; %% translation t=qmeso-(rtn*pmeso); %% Homogenous Trans&rotation Matrix Rt=[Rtn t ]; Η συνάρτηση CalcRigidMotionSVD.m μας επιστρέφει τον ομογενή μετασχηματισμό που περιέχει την περιστροφή και την μετατόπιση που μας πάει από το Ι2 σύστημα συντεταγμένων στο Ι1. Τα βήματα για τον υπολογισμό του Rt βασίστηκαν στη δημοσίευση Least-Squares Rigid Motion Using SVD [1]. Αυτό το step είναι και ένα από τα βασικότερα γι αυτό στο σημείο αυτό έγινε έλεγχος αν η συνάρτηση δουλεύει όπως θα περιμέναμε πάνω σε τυχαία σημεία στο αρχείο SVD_Check.m. 9

10 Step 4: Fit problem to Ransac Algorithm Function : [maxr,track_points] = CalcRigidMotionRansac( Niter,dmax,Npoints,Nframes,Rtinit ) Input: Ransac Parameters: Niter: Number of iterations Dmax: Maximum inlier distance Npoints: Maximum number of correspondences to use Other Parameters: Nframes: Αριθμός συνολικών frame προς χρήση. Rtinit: Αρχή της διαδρομής του Drone (Μετασχηματιμός) Output : [maxr,track_points]. Ο πίνακας track_points περιέχει όλα τα 3D σημεία τα οποία έχουν προέλθει απ τον βέλτιστο μετασχηματισμό Rt κατά Ransac (max inlier Ratio) που προκύπτει μετά από Νiterations(Niter) ανά frame(nframes).ο πίνακας maxr περιέχει το current max inlier ratio. Παρακάτω παρουσιάζεται ανοιγμένη η συνάρτηση CalcRigidMotionRansac.m για να κατανοηθεί καλύτερα ο αλγόριθμος Ransac και τα βήματα επίλυσης,προσαρμοσμένος για το συγκεκριμένο πρόβλημα. 1) Αρχικά τρέχουν οι συναρτήσεις από τα step 1,2,3 2) Επιλέγονται από τα feature correspondences Npoints τυχαία(rp1,rp2) σημεία 3) Τα Rp1,Rp2 τυχαία σημεία γίνονται input της CalcRigidMotionSVD(Rp1,Rp2) 4) Από την CalcRigidMotionSVD παίρνουμε τυχαίο μετασχηματισμό Rt* 5) Μετασχηματίζουμε όλο το P2 βάση του Rt* δημιουργώντας τον πίνακα Pmet=Rt*P2. 6) Υπολογίζουμε την ευκλείδεια απόσταση του μετασχηματισμένου από το P1 και περιμένουμε να πέσουμε κοντά στον P1 με την σχέση d=sqrt(sum((pmet-p1).^2));. 7) Υπολογίζεται η παράμετρος του Ransac inliers βάση της σχέσης D=(d<=dmax);, inliers=sum(d);. Inlier θεωρείται ένα σημείο αν η απόσταση d είναι μικρότερη απ την μέγιστη που έχω αυθαίρετα θέση (dmax σε m). 8) Υπολογίζω τον inliers Ratio το οποίο θα μου καθορίσει και τον μετασχηματισμό Rt που θα κρατήσω. Ψάχνω πάντα το υψηλότερο Ratio. 9) Γίνεται έλεγχος ώστε σε κάθε iteration να κρατώ τον μετασχηματισμό με το max Ratio. if r>maxr; maxr=r; Rtmax=Rt; 10) Σε περίπτωση που κατά την εκτέλεση του Αλγορίθμου έχω λιγότερα Feature correspondences απ αυτά που έχω θέση ως όριο(npoints) τότε αυτόματα το πρόγραμμα σταματά στο συγκεκριμένο frame και η διαδρομή υπολογίζεται με βάση τα δεδομένα που έχει κρατήσει έως εκείνο το σημείο. elseif size(p1,2)<npoints break % In case of not sufficient Match Correspondences 10

11 function [maxr,track_points] = CalcRigidMotionRansac( Niter,dmax,Npoints,Nframes,Rtinit ) RealRt=Rtinit; %Initial tra=[ ]; %momment translation track_points{1}=realrt*tra';% Initial point for n=0:nframes-1 [I1,I2]=LoadFrames(n); %RGB frames (X,Y) [P1,P2] = match_features_surf(i1,i2); %Feature Match [I1z,I2z]=LoadDepth(n); %Depth frames (Z) dimension [ P1,P2 ] = DepthInsert( P1,P2,I1z,I2z ); %Insert z Dimension in 2D points [ P1,P2 ] = Camera( P1,P2 ); %Camera calibration [ P1,P2 ] = Hm(P1,P2);% zeros in last row %% Ransac %Random Points if size(p1,2)>=npoints; %minimum pair of correspondeces check %Max Ration Initialization maxr=0; for i=1:niter; %!!!!!Niter!!!!!!! Randcol=randperm(size(P1,2),Npoints) ; %!!!!!!!!!Npoints!!!!!!!!! for c=1:npoints; RandP1=P1(:,Randcol(c)); RandP2=P2(:,Randcol(c)); RandomP1{c}=RandP1; RandomP2{c}=RandP2; Rp1=cell2mat(RandomP1); % pairs of Random P1 points Rp2=cell2mat(RandomP2); % pairs of Random P2 points [ Rt ] = CalcRigidMotionSVD(Rp1,Rp2); %Rt* Least squares [Random Points] Pmet=Rt*P2;% P2 with Rt* Transformed %EFKLIDIA APOSTASH d=sqrt(sum((pmet-p1).^2)); %Inliers D=(d<=dmax); %!!!!!!dmax!!!!!!!!!! inliers=sum(d); %Ratio r=inliers/size(p1,2); % Ratio Test (Keep Max) if r>maxr; maxr=r; Rtmax=Rt; elseif size(p1,2)<npoints break % In case of not sufficient Match Correspondences %3D Points with optimal Rt RealRt=RealRt*Rtmax;% Multiply Transform per frame track_points{n+1}=double(realrt*tra');% Trajectory points (for all Frames) 11

12 Step 5: Trajectory plot Function : TrajectoryPlot(groundtruth,track_points,id); Input: [groundtruth,track_points,id] groundtruth: Το groundtrudth είναι το αρχείο με την πραγματική διαδρομή του Drone. track_points: O πίνακας track_points έχει πάντα τα σημεία της υπολογισμένης διαδρομής κατά Ransac ή Lsquare. id: Επιλογέας (switch) για την επιλογή κάθε φορά του κατάλληλου plot. Ransac ή Lsquare. Output : Γράφημα σε figure με την groundtruth διαδρομή σε σχέση με την υπολογισμένη κάθε φορά. function [ ] = TrajectoryPlot(groundtruth,Rt_cellarray,id) x=groundtruth(:,2); y=groundtruth(:,3); z=groundtruth(:,4); M=cell2mat(Rt_cellarray); x1=m(1,:); y1=m(2,:); z1=m(3,:); % scatter3(x,y,z); figure;plot3(x,y,z);grid on; hold on plot3(x1,y1,z1,'-r','markeredgecolor','blue','markerfacecolor','red'); switch id case 'Lsquare' title('svd Least Squares Approximation') case 'Ransac' title('svd Ransac Approximation') 12

13 Chapter 3 Trajectory Compare Ransac Με την SVD of Least Square σε συνδυασμό με τον αλγόριθμο Ransac,παρατηρούμε ότι ο Ransac έπεσε πολύ κοντά στην διαδρομή διορθώνοντας το μεγάλο σφάλμα που δημιουργείται συνήθως από τις λάθος μετρήσεις ή τον θόρυβο στα Least Squares. Εν τέλει ο Ransac με την σωστή ρύθμιση των παραμέτρων του επέλεξε τον βέλτιστο μετασχηματισμό Rt. Η απόκλιση από την διαδρομή που φαίνεται στην συνέχεια είναι θεμιτή και οφείλεται στο dataset δηλαδή στον αριθμό των frames που επιλέξαμε. Προς το τέλος τα frames της διαδρομής που επιλέχθηκαν δεν είχανε καλό sequence διότι έγινε μικρή δειγματοληψία από το σύνολο των frame για την επαλήθευση της μεθόδου. 13

14 SVD of Least Squares Στα Least Square δημιουργείται μεγάλο σφάλμα συνήθως από τις λάθος μετρήσεις ή τον θόρυβο. Αυτοί οι δύο παράγοντες δημιουργούν μεγάλες διαταραχές στην επιλογή της βέλτιστης ευθείας. Με αποτέλεσμα έστω και μια λάθος μέτρηση με μεγάλη βαρύτητα θα επηρεάσει όλο το μοντέλο προς διαφορετική κλίση. Όπως είναι φανερό και στο παρακάτω Figure τα Least Square αποκλίνουν πολύ απ την πραγματικότητα. 14

15 Conclusions Από την εφαρμογή των παραπάνω αλγορίθμων καταλήγουμε στα εξής συμπεράσματα. Αλγόριθμοι όπως ο Ransac χρησιμοποιούνται και προσαρμόζονται κάθε φορά στη φύση του προβλήματος. Ανεξάρτητα απ την απλή λογική του αλγορίθμου Ransac φαίνεται ότι έχει πολύ καλή απόκριση σε δύσκολα προβλήματα όπως το Tracking. Ο Ransac παρουσιάζει δυσκολίες λόγο των πολλών παραμέτρων που πρέπει να ρυθμιστούν. Συνήθως για την επίλυση προβλημάτων όπως το Tracking χρησιμοποιούνται αλγόριθμοι συνδυαστικά όπως SVD Least Squares με Ransac μεθοδολογία. Η Μηχανική όραση μπορεί να αντικαταστήσει ακριβούς και βαρέως τύπου εξοπλισμούς που χρησιμοποιούνται για Tracking μέχρι και σήμερα. 15

16 References [1] Olga Sorkine-Hornung and Michael Rabinovic, Least-Squares Rigid Motion Using SVD, Department of Computer Science, ETH Zurich (2016). 16

17 Annotations Drone Localization 01 Unknown Unknown Page 1 10/2/ :39 B:10/10!