VIsualizzatore punti far/near per LiDAR velodyne

viewer.m

% 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.