Merge branch 'master' into main

Datasets/readme.txt

+This folder is the default folder to store the datasets used for training the GATA shallow neural network
+using the provided matlab scripts. However, the path can be changed in the param_loader.m script.

GATANeuralNetTraining.m

+function GATANeuralNetTraining()
+    % GATA T-IV classification of traversable lidar points using a shallow neural
+    % network
+
+    %% First, we load constants (labels and so on)
+    label_loader
+
+    %% Now, we load the user's parameters
+    param_loader    
+
+    %% Loading the datasets
+    [training_dataset, testing_dataset] = loadDataset(training_set_filename, testing_set_filename, training_testing_ratio, downsampling_ratio);
+
+    %% Now we fuse the classes to match the desired classification task
+    % NOTE: training and testing datasets are different, because they were
+    % from different files OR we have already splitted them, so we need to fuse
+    % classes in both datasets!
+    disp('Fusing KITTI ground truth classes in training set...')
+    [training_dataset] = fuseClasses(training_dataset, sink_classes_cell_array, ... 
+                                     source_classes_cell_array);
+    disp('Done!')
+
+    disp('Fusing KITTI ground truth classes in testing set...')
+    [testing_dataset] = fuseClasses(testing_dataset, sink_classes_cell_array, ... 
+                                    source_classes_cell_array);
+    disp('Done!')
+
+%%
+% Now we train the classifier
+disp('Training a Shallow Neural Network model for classification...')
+[nn_model, tr_x, tr_y, training_Pct_Err] = neural_network_classificator_training(training_dataset, ...
+                                                    desired_features_indices, classes_names, weight_classes, ...
+                                                    hiddenLayerSize, trainFcn, performFcn);
+disp('Done!')
+
+%%
+disp('Using NN for predicting classes...')
+[nn_classificated_dataset] = use_nn_classificator(nn_model, ... 
+    testing_dataset, desired_features_indices);  
+ 
+disp('Done!')
+
+%% Now we evaluate the system performance
+disp('Computing performance statistics...')
+[precission, recall, f1_score, overall_precission, ...
+    overall_recall, overall_f1_score, IoU] = ... 
+    evaluating_segmentation_results(nn_classificated_dataset);
+disp('Done!')
+
+%% Finally we export the model weights to csv file to use with GATA in ROS
+save_model_to_csv(nn_model, neural_net_filename);
+end

Neural_Nets_Weights/readme.txt

+This folder is the default folder to store the shallow neural networks weights to be used in the GATA ROS node. However, the path can be changed in the param_loader.m script.

evaluating_segmentation_results.m

+function [precission, recall, f1_score, overall_precission, overall_recall, ...
+    overall_f1_score, IoU] = evaluating_segmentation_results(dataset)
+    disp('Evaluating segmentation results...')
+    
+    %% First we load constants (labels and so on)
+    label_loader
+    
+    %%
+    % We extract the predictions
+    predicted_classes = dataset.c(:);
+    
+    % And the labels
+    gt_classes = dataset.GTC(:);
+
+    figure 
+   
+    confusion_matrix = confusionchart(gt_classes, predicted_classes);
+
+    confusion_matrix.ColumnSummary = 'column-normalized';
+    confusion_matrix.RowSummary = 'row-normalized';
+    confusion_matrix.Title = ' Confusion Matrix';
+    
+    [m,order] = confusionmat(gt_classes,predicted_classes);
+    Diagonal  = diag(m);
+    
+    sum_rows       = sum(m,2);
+    recall         = Diagonal./sum_rows;
+    overall_recall = mean(recall);
+    
+    sum_col           = sum(m,1);
+    precission        = Diagonal./sum_col';
+    overall_precission = mean(precission);
+
+    f1_score = 2 * ((precission .* recall) ./ (precission + recall));
+    overall_f1_score = mean (f1_score);
+     
+%   IoU = (TP) / (TP + FP + FN);
+    IoU = Diagonal ./ (sum_rows + sum_col' - Diagonal);
+end
\ No newline at end of file

fuseClasses.m

+function [dataset_fused_labels] = ... 
+    fuseClasses(dataset, sink_classes_cell_array, ... 
+    source_classes_cell_array) 
+  
+  label_loader
+
+  dataset_fused_labels = dataset;
+
+  dataset_fused_labels.GTC(:) = NON_TRAVERSABLE;
+      
+  number_of_sink_classes = height(sink_classes_cell_array{1});
+  for i = 1:number_of_sink_classes
+    current_sink_class = sink_classes_cell_array{1}(i);
+    number_of_source_classes_for_current_sink = length(source_classes_cell_array{i});
+    for j = 1:number_of_source_classes_for_current_sink
+        class_to_fuse = source_classes_cell_array{i}(j);
+        source_indices = find(dataset.GTC == class_to_fuse);
+        dataset_fused_labels.GTC(source_indices) = current_sink_class;
+    end
+  end
+
+end
\ No newline at end of file

label_loader.m

+% Load of constants
+
+% GATA T-IV
+% GATA labels
+GROUND = 46;
+OBSTACLE = 100;
+OVERHANGING_OBSTACLE =  15;
+
+% SemanticKITTI labels
+% Ground classes
+ROAD = 40; 
+SIDEWALK = 48;
+PARKING = 44;
+OTHER_GROUND = 49;
+LANE_MARKING = 60;
+TERRAIN = 72;
+VEGETATION = 70; 
+
+% For binary classification tasks we can use the following labels
+TRAVERSABLE = 1;
+NON_TRAVERSABLE = 0;
+
+% to use with NN one hot encoding
+NN_ROAD = 1;
+NN_SIDEWALK = 2;
+NN_TERRAIN = 3;
+NN_VEGETATION = 4;

loadDataset.m

+function [training_dataset, testing_dataset] = loadDataset(training_set_filename, testing_set_filename, training_testing_ratio, downsampling_ratio)
+disp('Loading dataset from file...')
+
+use_same_seq_for_training_and_testing = true;
+if(training_set_filename ~= testing_set_filename)
+  use_same_seq_for_training_and_testing = false;
+end
+
+if(use_same_seq_for_training_and_testing)
+  disp('Loading dataset from file... using the same dataset for training and testing')
+
+  training_and_testing_dataset_filename = training_set_filename;
+  dataset = parseDataset(training_and_testing_dataset_filename);
+  disp('Done!')
+    
+  % once you have the data loaded in a table format, we can continue with the
+  % process
+  disp('Removing outliers and other non evaluable points...')
+  filtered_dataset = remove_non_evaluable_points(dataset);
+  clear dataset
+  disp('Done!')
+
+  if ~exist('training_testing_ratio','var')
+    disp('WARNING training / test ratio not specified, using default value of 0.7...')
+    training_testing_ratio = 0.7;
+  end
+    
+  % Now we split the dataset between train and test
+  disp('Splitting the dataset into training and testing sets')
+  dataset_length = height(filtered_dataset);
+  training_dataset_length = floor(training_testing_ratio * dataset_length);
+  training_filtered_dataset_not_down = filtered_dataset(1:training_dataset_length, :);
+  testing_filtered_dataset_not_down = filtered_dataset(training_dataset_length + 1:dataset_length, :);
+  clear filtered_dataset
+
+  disp('Done!')
+else
+  disp('Loading dataset from file... using different datasets for training and testing!')
+  training_raw_dataset = parseDataset(training_set_filename);
+  testing_raw_dataset = parseDataset(testing_set_filename);
+  disp('Done!')
+  % once you have the data loaded in a table format, we can continue with the
+  % process
+  disp('Removing outliers and other non evaluable points...')
+  training_filtered_dataset_not_down = remove_non_evaluable_points(training_raw_dataset);
+  testing_filtered_dataset_not_down = remove_non_evaluable_points(testing_raw_dataset);
+  clear training_raw_dataset
+  clear testing_raw_dataset
+  disp('Done!')
+end
+
+use_downsampling = true;
+if (~exist('downsampling_ratio', 'var') || downsampling_ratio < 2)
+  use_downsampling = false;
+end
+
+if(use_downsampling)
+  training_dataset = training_filtered_dataset_not_down(1:downsampling_ratio:end, :);
+  testing_dataset = testing_filtered_dataset_not_down(1:downsampling_ratio:end, :);
+else
+  training_dataset = training_filtered_dataset_not_down;
+  testing_dataset = testing_filtered_dataset_not_down;
+end
+
+clear training_filtered_dataset_not_down
+clear testing_filtered_dataset_not_down
+
+end
\ No newline at end of file

neural_network_classificator_training.m

+function [Mdl, tr_x, tr_y, training_Pct_Err] = neural_network_classificator_training(dataset, ... 
+    desired_features_indices, classes_names, weight_classes, ...
+    hiddenLayerSize, trainFcn, performFcn)
+    %% First we load constants (labels and so on)
+    label_loader
+
+    %%
+    dataset_for_classification = dataset;
+    training_X = [table2array(dataset_for_classification(:,desired_features_indices))];
+    training_Y = dataset_for_classification.GTC;
+    
+    rng('default')
+    tr_x = training_X';
+    tr_y = training_Y';
+
+    if(weight_classes)
+        for i=0:length(classes_names)-1
+            w(i+1) = length(find(tr_y == i))/length(tr_y);
+        end
+        w = 1 ./ w;
+        w = w / sum(w);
+    end
+    
+    for i=0:length(classes_names)-1
+        classes_values(i+1) = i;
+    end
+
+    tr_y = categorical(tr_y, classes_values, classes_names, "Ordinal",true);
+    tr_y = onehotencode(tr_y, 1, "ClassNames",classes_names);
+    
+    net = patternnet(hiddenLayerSize, trainFcn, performFcn);
+
+    net = configure(net,tr_x,tr_y);
+    
+    if(weight_classes)
+        [Mdl, tr, output] = train(net, tr_x, tr_y, [],[],w');
+    else
+        [Mdl, tr, output] = train(net, tr_x, tr_y);
+    end
+    
+    trueclass = vec2ind(tr_y);
+    N = length(trueclass);
+    assignedclass = vec2ind(output);
+    Nerr = sum(assignedclass~=trueclass);
+ 
+    training_Pct_Err = 100*Nerr/N
+end

param_loader.m

+% Load of config parameteres
+neural_net_filename = './Neural_Nets_Weights/vegetation_and_terrain_are_non_traversable.csv';
+
+% If both files are the same, we will split it into two parts
+training_set_filename = './Datasets/probando_repo_dataset.csv';
+testing_set_filename  = './Datasets/probando_repo_dataset.csv';
+
+% This value is only used if we need to split the training dataset
+% (not used if there are two separate files for training and testing)
+training_testing_ratio = 0.7;
+
+% Taking into account that the neural net is small, we can decide 
+% to not use the whole training dataset. With the downsample ratio
+% we read the dataset in order and take one out of downsampling_ratio value
+% It is not random sampling
+%
+% Pointwise downsampling is only applied if its ratio is >= 2
+downsampling_ratio = 10;
+
+weight_classes = true;  % To give weight inversely to the class frequency
+                        % In T-IV paper it was not use, so the default value 
+                        % is false
+
+hiddenLayerSize = [39]; % Number of neurons in hidden layer. The length of
+                        % this vector is the number of hidden layers
+
+trainFcn = 'trainbr'; % See patternnet documentation if you want to use 
+performFcn = 'sse';   % other functions
+
+% Now we specify the features that we want to use (this sets the number of
+% neurons in the input layer. Our GATA algorithm extracts 13 features and
+% the best results are obtained using them all. However if faster
+% performance is required one can try to eliminate some of them (We
+% recommend to use the MATLAB's Classification Learner App to find the less 
+% informative features and then change this vector accordingly 
+desired_features_indices = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13];
+
+
+% Classification configuration: Please comment / uncomment / modify the code
+% as required by your application. 
+% The idea is that we will fuse labels using the information provided here. 
+% All the classes not especified will be labeled as NON_TRAVERSABLE.
+% The number of neurons in the output layer is dictated by the length of 
+% the sink_classes_cell_array, or in other words, the number of classes is
+% equal to the its length. 
+
+% % Multiclass example
+% % In this example, we consider ROAD, PARKING, LANE_MARKING and
+% % OTHER_GROUND to belong to the same class (with label 1), SIDEWALK is
+% % will have label 2, TERRAIN is 3 and VEGETATION is 4. So the neural net
+% % will have 4 outputs
+% sink_classes_cell_array = {};
+% sink_classes_cell_array{1} = [1;2;3;4]; % Always use consecutive numbers 
+%                                         % starting in 1 
+% source_classes_cell_array = {};
+% source_classes_cell_array{1} = [ROAD, PARKING, LANE_MARKING, OTHER_GROUND];
+% source_classes_cell_array{2} = [SIDEWALK];
+% source_classes_cell_array{3} = [TERRAIN];
+% source_classes_cell_array{4} = [VEGETATION];
+% % Now we put the categorical names for the Patternnet (class 0 is always
+% % 'obstacle', choose the names for the rest of the classes
+% classes_names = {'obstacle' 'road' 'sidewalk' 'terrain' 'vegetation'};
+
+% Binary classification examples
+% We want VEGETATION and TERRAIN to be considered as NON_TRAVERSABLE
+% So we put the traversable label to all the other ground classes  
+sink_classes_cell_array = {};
+sink_classes_cell_array{1} = [TRAVERSABLE]; % TRAVERSABLE is label 1
+source_classes_cell_array = {};
+source_classes_cell_array{1} = [ROAD, SIDEWALK, PARKING, LANE_MARKING, OTHER_GROUND];
+classes_names = {'obstacle' 'traversable'};
+
+% % Other binary example: Only ROAD is traversable
+% % For this example we just need to put label TRAVERSABLE for ROAD points 
+% % and leave the rest as NON_TRAVERSABLE
+% sink_classes_cell_array = {};
+% sink_classes_cell_array{1} = [TRAVERSABLE]; % TRAVERSABLE is label 1
+% source_classes_cell_array = {};
+% source_classes_cell_array{1} = [ROAD];
+% classes_names = {'obstacle' 'traversable'};
+
+

parseDataset.m

+function dataset = parseDataset(filename)
+  dataset = readtable(filename);
+  dataset = renamevars(dataset,["Var1","Var2","Var3","Var4","Var5","Var6" ...
+      ,"Var7","Var8","Var9","Var10","Var11","Var12","Var13", "Var14"], ...
+                 ["squared_dist_point_sensor", "incidence_angle", "intensity", ... 
+                 "squared_dist_point_ref", "pred_error", "score", "ratio", ... 
+                 "mean_intensity", "var_intensity", "mean_pred_error", ... 
+                 "var_pred_error", "mean_score", "var_score", "GTC"]);
+end
+

remove_non_evaluable_points.m

+function [filtered_dataset] = remove_non_evaluable_points(dataset)
+    % we first filter out the non evaluable points
+    UNLABELED = 0;
+    OUTLIER = 1;
+    OTHER_STRUCTURE = 52;
+    OTHER_OBJECT = 99;
+    
+    filtered_dataset = dataset(dataset.GTC ~= UNLABELED, :);
+    filtered_dataset = filtered_dataset(filtered_dataset.GTC ~= OUTLIER, :);
+    filtered_dataset = filtered_dataset(filtered_dataset.GTC ~= OTHER_STRUCTURE, :);
+    filtered_dataset = filtered_dataset(filtered_dataset.GTC ~= OTHER_OBJECT, :);
+    
+    % We filter some extrange rows that are all filled with zeros
+    %filtered_dataset = filtered_dataset(filtered_dataset.c ~= UNLABELED, :); 
+    
+    %TOTAL_POINTS = height(filtered_dataset)
+end
\ No newline at end of file

save_model_to_csv.m

+function save_model_to_csv(nn_classificator_model, nn_csv_name)
+  xoffset=nn_classificator_model.inputs{1}.processSettings{1}.xoffset;
+  gain=nn_classificator_model.inputs{1}.processSettings{1}.gain;
+  ymin=nn_classificator_model.inputs{1}.processSettings{1}.ymin;
+  w1 = nn_classificator_model.IW{1};   
+  w2 = nn_classificator_model.LW{2};   
+  b1 = nn_classificator_model.b{1};    
+  b2 = nn_classificator_model.b{2};
+
+  writematrix(b1, nn_csv_name);
+  writematrix(b2, nn_csv_name, 'WriteMode','append');
+  writematrix(gain, nn_csv_name, 'WriteMode','append');
+  writematrix(w1, nn_csv_name, 'WriteMode','append');
+  writematrix(w2, nn_csv_name, 'WriteMode','append');
+  writematrix(xoffset, nn_csv_name, 'WriteMode','append');
+  writematrix(ymin, nn_csv_name, 'WriteMode','append');
+end
\ No newline at end of file

use_nn_classificator.m

+function [classified_dataset] = use_nn_classificator ...
+    (nn_model, classified_dataset, desired_features_indices)
+    %% First we load constants (labels and so on)
+    label_loader
+    
+    %%
+    X = [table2array(classified_dataset(:,desired_features_indices))];
+
+    X = X';
+    Ymod = nn_model(X);
+
+    %% This is a manual computation to check that we can do the prediction
+    %  exactly as the Matlab model does. This is important because we will
+    %  export the weights to a .csv file and do these computations in C++
+    %  in the ROS node.
+    xoffset=nn_model.inputs{1}.processSettings{1}.xoffset;
+    gain=nn_model.inputs{1}.processSettings{1}.gain;
+    ymin=nn_model.inputs{1}.processSettings{1}.ymin;
+    w1 = nn_model.IW{1};   
+    w2 = nn_model.LW{2};   
+    b1 = nn_model.b{1};    
+    b2 = nn_model.b{2};
+    % Input 1
+    y1 = bsxfun(@times,bsxfun(@minus,X,xoffset),gain);
+    y1 = bsxfun(@plus,y1,ymin);
+    % Layer 1
+    a1 = 2 ./ (1 + exp(-2*(repmat(b1,1,size(X,2)) + w1*y1))) - 1;
+    % output
+    n=repmat(b2,1,size(X,2)) + w2*a1;
+    nmax = max(n,[],1);
+    n = bsxfun(@minus,n,nmax);
+    num = exp(n);
+    den = sum(num,1);
+    den(den == 0) = 1;
+    Y = bsxfun(@rdivide,num,den);
+
+    %%%% just a check!
+    first_obs = X(:,1);
+    x_offset_applied = first_obs - xoffset;
+    x_gain_applied = x_offset_applied .* gain;
+    x_final = x_gain_applied + ymin;
+
+    a1_first = (2 ./ (1 + exp((-2 * (b1 + w1 * x_final))))) - 1;
+    n_first = b2 + w2 * a1_first;
+    nmax_first = max(n_first);
+    n_first = n_first - nmax_first;
+    num_first = exp(n_first);
+    den_first = sum(num_first);
+    y_first = num_first / den_first;
+
+    error = sum(y_first - Y(:,1));
+    whole_error = Y - Ymod;
+    %%%
+
+    % Now we need to decode the network output
+    max_outputs = max(Y);
+    idx = (Y == max_outputs);
+
+    labels = [];
+    for i = 0:height(Y)-1
+      labels(i+1) = i;
+    end
+
+    idx_decoded = (idx' * labels')';
+  
+    classified_dataset(:, "c") = num2cell(idx_decoded');
+end
\ No newline at end of file