Last active
January 7, 2020 18:25
-
-
Save Mm7/6200b755ede216e34d2c562b56cb5091 to your computer and use it in GitHub Desktop.
VIsualizzatore punti far/near per LiDAR velodyne
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
% Apri il dataset e estrai il primo frame. | |
reader = velodyneFileReader('../Statico.vibrato.pcap','VLP16'); | |
ptcloud = readFrame(reader); | |
% Ottieni un tensore [16, colonne, 3] in cui 16 sono i raggi (angolo | |
% pitch) e 3 sono i valori di XYZ. | |
% Colonne dovrebbero essere gli angoli di yaw (azimuth) ma in realtà ci | |
% sono parecchi Nan e il numero varia (di poco) di frame in frame. | |
location = ptcloud.Location; | |
% Osservazione: X è Nan sse lo sono anche Y e Z: | |
X_nan = isnan(location(:,:,1)); | |
Y_nan = isnan(location(:,:,2)); | |
Z_nan = isnan(location(:,:,3)); | |
assert(isequal(X_nan, Y_nan, Z_nan)); | |
% D'ora in poi, controlliamo solo se X è Nan, perchè automaticamente lo | |
% sono anche le altre dimensioni. | |
nans = X_nan; | |
% Una quadrupletta è uno slice del tipo: | |
% location(:,i:i+4,:); | |
% con i=1:colonne/4:4 (colonne sembra essere sempre multiplo di 4). | |
colonne = size(location, 2); | |
% Come prima osservazione, circa il 90% delle quadruplette sono della | |
% forma: | |
% quadr(:,1,:) != nan | |
% quadr(:,2,:) != nan | |
% quadr(:,3,:) == nan | |
% quadr(:,4,:) == nan | |
c = 0; | |
for i = 1:4:colonne | |
for u = 1:16 | |
if ~nans(u,i) && ~nans(u,i+1) && nans(u,i+2) && nans(u,i+3) | |
c = c + 1; | |
end | |
end | |
end | |
% Scommenta per visualizzare la percetuale. | |
%c / (colonne * 4) * 100 | |
% Questo mi porta a pensare che ogni quadrupletta memorizzi due raggi. Se | |
% entrambi hanno ricevuto un solo eco (plausibilmente nella maggioranza dei | |
% casi), matlab memorizza la posizione dei due punti in 1,2 e lascia 3,4 a | |
% Nan. Viceversa se entrambi i raggi ricevono due echi allora vengono | |
% utilizzati tutti i valori della quadruplette: 1,2 far/near del primo | |
% raggio, 3,4 far/near del secondo raggio. | |
% A conferma di questo, ecco alcuni plot della nuvola di punti per i tre | |
% casi. | |
% 1: Solo i punti con 1,2 non nan e 3,4 nan. Questi dovrebbero essere i | |
% punti "standard" cioè quelli con un solo eco. | |
new_loc = zeros(size(location)); | |
for i = 1:4:colonne | |
for u = 1:16 | |
if ~nans(u,i) && ~nans(u,i+1) && nans(u,i+2) && nans(u,i+3) | |
new_loc(u,i,:) = location(u,i,:); | |
new_loc(u,i+1,:) = location(u,i+1,:); | |
end | |
end | |
end | |
% Scommenta per visualizzare. | |
%pcshow(new_loc) | |
% 2: Solo le quadruplette senza nan e mostra solo i punti far, che vengono | |
% memorizzati nelle positioni 1,2 della quadrupletta. | |
new_loc = zeros(size(location)); | |
for i = 1:4:colonne | |
for u = 1:16 | |
if ~nans(u,i) && ~nans(u,i+1) && ~nans(u,i+2) && ~nans(u,i+3) | |
new_loc(u,i,:) = location(u,i,:); | |
new_loc(u,i+1,:) = location(u,i+1,:); | |
end | |
end | |
end | |
% Scommenta per visualizzare. | |
%pcshow(new_loc) | |
% 3: Solo le quadruplette senza nan e mostra solo i punti near, che vengono | |
% memorizzati nelle positioni 3,4 della quadrupletta. | |
new_loc = zeros(size(location)); | |
for i = 1:4:colonne | |
for u = 1:16 | |
if ~nans(u,i) && ~nans(u,i+1) && ~nans(u,i+2) && ~nans(u,i+3) | |
new_loc(u,i,:) = location(u,i+2,:); | |
new_loc(u,i+1,:) = location(u,i+3,:); | |
end | |
end | |
end | |
% Scommenta per visualizzare. | |
%pcshow(new_loc) | |
% C'è dell'altro! Calcoliamo l'angolo di yaw (azimuth) dei punti (per il | |
% primo raggio per semplicità, non cambia niente se usiamo altri raggi). | |
yaw = atan2(location(1,:,2), location(1,:,1)); | |
% E visualizziamo i primi 40 elementi (scommenta). Ovviamente, ogni 4 ne | |
% mancano due (perchè sono a Nan). Ora supponendo che quelli a Nan | |
% rappresentino comunque dei raggi che per qualche motivo hanno avuto | |
% dei problemi e i LiDAR non ha ricevuto echi, dovrei vedere che i restanti | |
% punti sono allineati lungo una retta. Non è cosi. | |
%scatter(1:40, yaw(1:40)); | |
% Se l'ipotesi è vera invece, rimuovendo i due punti a nan. I punti sono | |
% allineati! (scommenta). | |
%scatter(1:20, yaw(1:2:40)); | |
% Questo fatto è importante per il "compressore polare" perchè esso stima | |
% lo yaw dall'indice di colonna. Se i punti sono allineati, la retta | |
% interpolante sarà una buona stima, se hanno quell'andamento | |
% "seghettatto", la stima sarà peggiore. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment