feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,106 @@
|
||||
|
||||
## Multi-Session Visual SLAM for Illumination Invariant Re-Localization in Indoor Environments
|
||||
|
||||
* Paper: https://doi.org/10.3389/frobt.2022.801886
|
||||
|
||||
* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
|
||||
|
||||
|
||||
|
||||
## Description
|
||||
|
||||
This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using multi-session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
|
||||
|
||||
|
||||
|
||||
The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
## Dataset
|
||||
|
||||
We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.
|
||||
|
||||
* [Images](https://drive.google.com/file/d/1fUm1m8oW6q8qlThx7BjrBH2vrVbNQ9bQ/view?usp=drive_link):
|
||||
* `rgb`: folder containing *.jpg color camera images
|
||||
* `depth`: folder containing *.png 16bits mm depth images
|
||||
* `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`.
|
||||
* `device_poses.txt`: VIO poses of each image in `device` frame
|
||||
* `camera_poses.txt`: VIO poses of each image in `camera` frame
|
||||
* [RTAB-Map Databases](https://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link)
|
||||
* Dataset now also available on [Federated Research Data Repository (FRDR)](https://doi.org/10.20383/103.0931) (if links above don't work)
|
||||
|
||||
|
||||
## How reproduce results shown in the paper
|
||||
|
||||
1. RTAB-Map should be built from source with those dependencies (don't need to "install" it, we will launch it from build directory in the scripts below to avoid conflicting with another rtabmap already installed):
|
||||
* Use Ubuntu 20.04+ to avoid any python2/python3 conflicts.
|
||||
* OpenCV built with **xfeatures2d** and **nonfree** modules
|
||||
* [torchlib c++](https://pytorch.org/get-started/locally/) (tested on v1.10.2) to enable [SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork)
|
||||
* Git clone [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) into scripts directory.
|
||||
* Generate `superpoint_v1.pt` in the scripts directory (can also be downloaded from [here](https://github.com/KinglittleQ/SuperPoint_SLAM/blob/master/superpoint.pt) but may not be compatible with more recent pytorch versions):
|
||||
```bash
|
||||
cd rtabmap/archive/2022-IlluminationInvariant/scripts
|
||||
wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth
|
||||
wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py
|
||||
python trace.py
|
||||
```
|
||||
|
||||
2. Download databases of the dataset and extract them.
|
||||
3. Adjust the path inside `rtabmap_latest.sh` script to match where you just built rtabmap with right dependencies.
|
||||
4. Run `run_all.sh DATABASES_PATH OUTPUT_PATH`, this script will do the following steps (warning, this could take hours to do...):
|
||||
* Recreate the map databases for each feature type
|
||||
* Create the merged databases
|
||||
* Run localization databases over all map/merged databases
|
||||
* Run consecutive localization experiment
|
||||
|
||||
5. Export statistics with `export_stats.sh` script.
|
||||
|
||||
6. Use the MatLab/Octave scripts in this folder to show results you want. Set `dataDir` to directory containing the exported statistics.
|
||||
```
|
||||
sudo apt install octave liboctave-dev
|
||||
|
||||
# In octave:
|
||||
pkg install -forge control signal
|
||||
```
|
||||
|
||||
### Docker
|
||||
|
||||
1. Create the docker image:
|
||||
```
|
||||
cd rtabmap
|
||||
docker build -t rtabmap_frontiers -f docker/frontiers2022/Dockerfile .
|
||||
```
|
||||
|
||||
2. Assuming you extracted the databases of the dataset in `~/Downloads/Illumination_invariant_databases`, create an output directory for results:
|
||||
```
|
||||
mkdir ~/Downloads/Illumination_invariant_databases/results
|
||||
```
|
||||
|
||||
3. Run script:
|
||||
```
|
||||
docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
|
||||
--user $(id -u):$(id -g) \
|
||||
-w=/workspace/scripts \
|
||||
-v ~/Downloads/Illumination_invariant_databases:/workspace/databases \
|
||||
-v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
|
||||
rtabmap_frontiers /workspace/scripts/run_all.sh /workspace/databases /workspace/results
|
||||
```
|
||||
|
||||
4. Export statistics:
|
||||
```
|
||||
docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
|
||||
--env="DISPLAY=$DISPLAY" \
|
||||
--env="QT_X11_NO_MITSHM=1" \
|
||||
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
|
||||
--env="XAUTHORITY=$XAUTH" \
|
||||
--volume="$XAUTH:$XAUTH" \
|
||||
--user $(id -u):$(id -g) \
|
||||
-w=/workspace/results \
|
||||
-v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
|
||||
rtabmap_frontiers /workspace/scripts/export_stats.sh /workspace/results
|
||||
```
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 224 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 266 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 253 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 282 KiB |
@@ -0,0 +1,169 @@
|
||||
|
||||
clear all
|
||||
close all
|
||||
|
||||
# sudo apt install octave-signal
|
||||
pkg load signal
|
||||
|
||||
# Use with files generated by export_stats.sh
|
||||
|
||||
dataDir = 'SET_PATH_TO_RESULTS_DIR';
|
||||
resultsToShow = 1; % 1=single 2=Consecutive
|
||||
|
||||
prefix = 'Stat';
|
||||
RAMaddOverhead = 0;
|
||||
% Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
|
||||
% Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
|
||||
|
||||
statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
|
||||
|
||||
|
||||
datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
|
||||
sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
|
||||
sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
|
||||
|
||||
if resultsToShow == 2
|
||||
sep = [0, 1000, 3000, 5000, 7000, 9000];
|
||||
sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
|
||||
prefix = 'Consecutive';
|
||||
statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
|
||||
endif
|
||||
|
||||
MapsN = length(sepName);
|
||||
|
||||
allCumResults = {};
|
||||
allMaxResults = {};
|
||||
|
||||
for s=1:length(statNames)
|
||||
|
||||
avgResults = {};
|
||||
maxResults = {};
|
||||
totalResults = {};
|
||||
absResults = {};
|
||||
|
||||
statName = strrep(statNames{s},'/','-');
|
||||
|
||||
for d=1:length(datasets)
|
||||
|
||||
if strcmp(statName,'Inliers_ratio_%')
|
||||
data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
dataWords = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
data(:, 2:end) = data(:, 2:end) ./ dataWords(:, 2:end) * 100;
|
||||
elseif strcmp(statName, 'Odometry_average')
|
||||
data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
|
||||
data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
else
|
||||
data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
endif
|
||||
sessions = size(data,2)-1;
|
||||
|
||||
avgResultsTmp = zeros(sessions, length(sep)-1);
|
||||
maxResultsTmp = zeros(sessions, length(sep)-1);
|
||||
totalResultsTmp = zeros(sessions, length(sep)-1);
|
||||
absResultsTmp = zeros(sessions, length(sep)-1);
|
||||
|
||||
for i = 1:sessions
|
||||
for j = 1:length(sep)-1
|
||||
x = data(:,1);
|
||||
y = data(:,i+1);
|
||||
y = y(x>=sep(j) & x<=sep(j+1), :);
|
||||
x = x(x>=sep(j) & x<=sep(j+1), :);
|
||||
if strcmp(statName, 'Odometry_average')
|
||||
y(2:end) = y(2:end) - y(1:end-1);
|
||||
y(y < 0.05) = 0;
|
||||
elseif strcmp(statName, 'Loop-Map_id-')
|
||||
y = y+1;
|
||||
y(y>0) = 1;
|
||||
end
|
||||
if strcmp(statName, 'Memory-RAM_estimated-MB') && RAMaddOverhead == 1
|
||||
% Valgrind estimated around 90 MB constant overhead
|
||||
y = y + 90;
|
||||
if datasets(d) == 7
|
||||
%% 135 MB overhead for BRISK kernel
|
||||
y = y + 135;
|
||||
elseif datasets(d) == 11 || datasets(d) == 111
|
||||
%% 645 MB (library cuda) + 800 MB (network) for SuperPoint
|
||||
y = y + 645+800;
|
||||
elseif datasets(d) == 13 || datasets(d) == 14
|
||||
%% 64 MB overhead for DAISY
|
||||
y = y + 64;
|
||||
endif
|
||||
endif
|
||||
nonzeros = y(y>0);
|
||||
if strcmp(statName, 'Loop-Map_id-')
|
||||
nonzeros = y;
|
||||
end
|
||||
if strcmp(statName, 'Distance_since_last_loc_under_50cm')
|
||||
y(y>0.55) = 0;
|
||||
y(isnan(y)) = 0;
|
||||
y(y>0) = 1;
|
||||
nonzeros = y;
|
||||
end
|
||||
if length(nonzeros) > 0
|
||||
avgValue = sum(nonzeros)/length(nonzeros);
|
||||
avgResultsTmp(i,j) = avgValue;
|
||||
maxResultsTmp(i,j) = max(nonzeros);
|
||||
totalResultsTmp(i,j) = length(nonzeros);
|
||||
absResultsTmp(i,j) = sum(nonzeros);
|
||||
endif
|
||||
endfor
|
||||
endfor
|
||||
avgResults{1,d} = avgResultsTmp;
|
||||
maxResults{1,d} = maxResultsTmp;
|
||||
totalResults{1,d} = totalResultsTmp;
|
||||
absResults{1,d} = absResultsTmp;
|
||||
endfor
|
||||
|
||||
% compute cumulative results
|
||||
cumResults = zeros(sessions+2, length(datasets)+1);
|
||||
for d=1:length(datasets)
|
||||
cumResults(1,d+1) = datasets(d);
|
||||
if sum(totalResults{1,d}, 2)
|
||||
cumResults(2:end-1,d+1) = sum(absResults{1,d}, 2) ./ sum(totalResults{1,d}, 2);
|
||||
endif
|
||||
cumResults(end,d+1) = sum(sum(absResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN))) / sum(sum(totalResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
|
||||
end
|
||||
cumResults(2:end-1,1) = 1:sessions;
|
||||
|
||||
allCumResults{1,s} = statNames{s};
|
||||
if strcmp(statNames{s}, 'Loop/Odom_correction_norm/m')
|
||||
cumResults(2:end,2:end) = cumResults(2:end,2:end) * 1000;
|
||||
allCumResults{1,s} = 'Loop/Odom_correction_norm/mm';
|
||||
elseif strcmp(statNames{s}, 'Loop/Map_id/')
|
||||
cumResults(2:end,2:end) = cumResults(2:end,2:end) * 100;
|
||||
endif
|
||||
allCumResults{2,s} = round(cumResults);
|
||||
|
||||
% compute max results
|
||||
cumMaxResults = zeros(sessions+2, length(datasets)+1);
|
||||
for d=1:length(datasets)
|
||||
cumMaxResults(1,d+1) = datasets(d);
|
||||
if sum(totalResults{1,d}, 2)
|
||||
cumMaxResults(2:end-1,d+1) = max(maxResults{1,d}, [], 2);
|
||||
endif
|
||||
cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
|
||||
end
|
||||
cumMaxResults(2:end-1,1) = 1:sessions;
|
||||
|
||||
allMaxResults{1,s} = statNames{s};
|
||||
allMaxResults{2,s} = cumMaxResults;
|
||||
|
||||
endfor % statNames
|
||||
|
||||
|
||||
if resultsToShow == 2
|
||||
disp('30min')
|
||||
for d=1:length(datasets)
|
||||
round(sum(absResults{1,d} .* eye(5,5)) / sum(totalResults{1,d} .*eye(5,5)) * 100)
|
||||
endfor
|
||||
disp('60min')
|
||||
for d=1:length(datasets)
|
||||
round(sum(absResults{1,d} .* [[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) / sum(totalResults{1,d} .*[[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) * 100)
|
||||
endfor
|
||||
disp('120min')
|
||||
for d=1:length(datasets)
|
||||
round(avgResults{1,d}(1,5) * 100)
|
||||
endfor
|
||||
endif
|
||||
|
||||
@@ -0,0 +1,253 @@
|
||||
|
||||
close all
|
||||
clear all
|
||||
|
||||
pkg load signal
|
||||
|
||||
# Use with files generated by export_stats.sh
|
||||
|
||||
dataDir = 'SET_PATH_TO_RESULTS_DIR';
|
||||
resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive
|
||||
|
||||
datasetPrefix = 'Stat';
|
||||
datasets = [0 1 6 7 9 14 11 111]; % 0 1 6 7 8 9 11 12
|
||||
datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
|
||||
datasetsName{112} = 'SuperGlue'
|
||||
sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
|
||||
sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
|
||||
|
||||
if resultsToShow == 3
|
||||
sep = [0, 1000, 3000, 5000, 7000, 9000];
|
||||
sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
|
||||
datasetPrefix = 'Consecutive'
|
||||
endif
|
||||
|
||||
percentResults = {};
|
||||
totalResults = {};
|
||||
locResults = {};
|
||||
|
||||
figure
|
||||
|
||||
colors = get(gca, 'ColorOrder');
|
||||
tmp=colors(3,:);
|
||||
colors(3,:) = colors(5,:);
|
||||
colors(5,:) = tmp;
|
||||
|
||||
globalSeparators = [];
|
||||
globalx = [];
|
||||
globaly = [];
|
||||
globalc = [];
|
||||
|
||||
for d=1:length(datasets)
|
||||
|
||||
data = dlmread([dataDir '/' datasetPrefix num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
|
||||
|
||||
curvesBeg = 2;
|
||||
curvesEnd = size(data,2)-5; % -4 for '0', -5 for '1'
|
||||
|
||||
if resultsToShow == 2
|
||||
curvesBeg = 8; % 2 if only 4 merged_reduced in stats, 8 to skip first 6
|
||||
curvesEnd = size(data,2);
|
||||
elseif resultsToShow == 3
|
||||
curvesEnd = size(data,2);
|
||||
endif
|
||||
curves = curvesEnd - curvesBeg + 1;
|
||||
|
||||
percentResultsTmp = zeros(curves, length(sep)-1);
|
||||
totalResultsTmp = zeros(curves, length(sep)-1);
|
||||
locResultsTmp = zeros(curves, length(sep)-1);
|
||||
|
||||
offset = 1;
|
||||
|
||||
for i = 1:curves
|
||||
index = i + curvesBeg - 1;
|
||||
separators = [];
|
||||
x_all = [];
|
||||
y_all = [];
|
||||
m_all = [];
|
||||
previousMax = 0;
|
||||
for j = 1:length(sep)-1
|
||||
x = data(:,1);
|
||||
y = data(:,index);
|
||||
y = y(x>=sep(j) & x<=sep(j+1), :);
|
||||
x = x(x>=sep(j) & x<=sep(j+1), :);
|
||||
minimum = x(1,1);
|
||||
separators = [separators previousMax];
|
||||
x = x - (minimum-previousMax);
|
||||
previousMax = x(end,1);
|
||||
y = y + 1;
|
||||
m = y;
|
||||
y(y>0) = 1;
|
||||
y(isnan(y)) = 0;
|
||||
percent = sum(y)/length(y);
|
||||
percentResultsTmp(i,j) = percent;
|
||||
locResultsTmp(i,j) = sum(y);
|
||||
totalResultsTmp(i,j) = length(y);
|
||||
y(y>0) = -(d-1)*curves -i - (d-1)*offset;
|
||||
%x(y==0) = nan;
|
||||
m(y==0) = nan;
|
||||
y(y==0) = nan;
|
||||
if resultsToShow == 2
|
||||
if i==1 %% Merged 1, 6
|
||||
m(m==1) = 1;
|
||||
m(m==2) = 6;
|
||||
elseif i==2 %% Merged 1,3(2 sessions),5
|
||||
m(m==1) = 1;
|
||||
m(m==2) = 3;
|
||||
m(m==3) = 3;
|
||||
m(m==4) = 5;
|
||||
elseif i==3 %% Merged 2(2 sessions),4,6
|
||||
m(m==1) = 2;
|
||||
m(m==2) = 2;
|
||||
m(m==4) = 6;
|
||||
m(m==3) = 4;
|
||||
elseif i>=4 %% Merged 1, 2(2 sessions), 3(2 sessions),4,5,6
|
||||
m(m==1) = 1;
|
||||
m(m==2) = 2;
|
||||
m(m==3) = 2;
|
||||
m(m==4) = 3;
|
||||
m(m==5) = 3;
|
||||
m(m==6) = 4;
|
||||
m(m==7) = 5;
|
||||
m(m==8) = 6;
|
||||
endif
|
||||
endif
|
||||
x = upsample(x, 2);
|
||||
y = upsample(y, 2);
|
||||
m = upsample(m, 2);
|
||||
x(2:2:end-1) = x(3:2:end);
|
||||
y(2:2:end-1) = y(3:2:end);
|
||||
m(2:2:end) = m(1:2:end);
|
||||
x = x(1:end-1);
|
||||
y = y(1:end-1);
|
||||
m = m(1:end-1);
|
||||
|
||||
x_all = [x_all nan x'];
|
||||
y_all = [y_all nan y'];
|
||||
m_all = [m_all nan m'];
|
||||
endfor
|
||||
if resultsToShow == 2
|
||||
globalx = [globalx x_all];
|
||||
globaly = [globaly y_all];
|
||||
globalc = [globalc m_all];
|
||||
else
|
||||
plot(x_all,y_all, 'linewidth', 3, 'color', colors(i,:))
|
||||
hold on
|
||||
endif
|
||||
separators = [separators previousMax];
|
||||
globalSeparators = separators;
|
||||
endfor
|
||||
percentResults{1,d} = percentResultsTmp;
|
||||
totalResults{1,d} = totalResultsTmp;
|
||||
locResults{1,d} = locResultsTmp;
|
||||
endfor
|
||||
|
||||
if resultsToShow == 2
|
||||
indColors = ones(length(globalc), 3);
|
||||
for j=1:length(globalc)
|
||||
if ~isnan(globalc(j))
|
||||
indColors(j,:) = colors(globalc(j),:);
|
||||
endif
|
||||
endfor
|
||||
for i=1:6
|
||||
tmpx = globalx;
|
||||
tmpy = globaly;
|
||||
tmpx(globalc~=i) = nan;
|
||||
tmpy(globalc~=i) = nan;
|
||||
plot(tmpx, tmpy, 'linewidth', 3, 'color', colors(i,:));
|
||||
if i==1
|
||||
hold on
|
||||
endif
|
||||
endfor
|
||||
endif
|
||||
|
||||
for j=1:length(globalSeparators)
|
||||
x = globalSeparators(j);
|
||||
plot([x,x],[(-length(datasets)*(curves+1)) ,0], 'k','linewidth', 2);
|
||||
endfor
|
||||
|
||||
for d=1:length(datasets)
|
||||
annotation ("textbox", [0, 0.96-((d-0.5)/length(datasets))*0.95, 0,0], 'string', datasetsName{datasets(d)+1})
|
||||
endfor
|
||||
for s=1:length(sep)-1
|
||||
annotation ("textbox", [0.1 + ((separators(s+1)-separators(s))/2+separators(s))/separators(end)*0.75, 0.98, 0,0], 'string', sepName{s})
|
||||
endfor
|
||||
axis('tight')
|
||||
|
||||
|
||||
|
||||
set(gca, 'units', 'normalized');
|
||||
Tight = get(gca, 'Position');
|
||||
NewPos = [Tight(1) 0.01 0.77 0.95]; %New plot position [X Y W H]
|
||||
set(gca, 'Position', NewPos);
|
||||
if length(sep) == 7
|
||||
legend('16:46', '17:27', '17:54', '18:27', '18:56', '19:35', "location", 'northeastoutside' )
|
||||
else
|
||||
legend('16:46', '17:27', '17:54', '18:27', '18:56', "location", 'northeastoutside' )
|
||||
endif
|
||||
box off
|
||||
axis off
|
||||
|
||||
#disp(percentResults);
|
||||
#disp(totalResults);
|
||||
|
||||
figure;
|
||||
for d=1:length(datasets)
|
||||
subplot(4,2,d)
|
||||
data=percentResults{1,d}*100;
|
||||
data(isnan(data)) = 0;
|
||||
hAxes = gca;
|
||||
% Upscaling the image to reduce anti-aliasing effect in pfd viewers
|
||||
scale = 50;
|
||||
tickXStep = zeros(1, size(data, 2));
|
||||
tickYStep = zeros(1, size(data, 1));
|
||||
dataUp = upsample(upsample(data',scale)',scale);
|
||||
for x=0:size(data, 2)-1
|
||||
for y=1:scale-1
|
||||
dataUp(:,(x*scale+1)+y) = dataUp(:,x*scale+1);
|
||||
endfor
|
||||
tickXStep(1,x+1) = scale/2 + scale*x;
|
||||
endfor
|
||||
for x=0:size(data, 1)-1
|
||||
for y=1:scale-1
|
||||
dataUp((x*scale+1)+y,:) = dataUp(x*scale+1,:);
|
||||
endfor
|
||||
tickYStep(1,x+1) = scale/2 + scale*x;
|
||||
endfor
|
||||
imagesc( hAxes, dataUp, [0, 100])
|
||||
%title({"",datasetsName{datasets(d)+1}})
|
||||
colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
|
||||
colors(1,:) = 1;
|
||||
colormap( hAxes , colors)
|
||||
c = colorbar;
|
||||
labels = {};
|
||||
for v=get(c,'ytick'), labels{end+1} = sprintf('%d%%',v); end
|
||||
set(c,'yticklabel',labels);
|
||||
if mod(d,2) == 1
|
||||
ylabel("Map")
|
||||
endif
|
||||
xlabel([datasetsName{datasets(d)+1} " Localization"])
|
||||
set(gca, "xaxislocation", "top");
|
||||
set(gca, 'XTick', tickXStep)
|
||||
set(gca, 'XTickLabel', sepName, 'fontsize',7)
|
||||
set(gca, 'YTick', tickYStep)
|
||||
if resultsToShow == 3
|
||||
set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
|
||||
elseif resultsToShow == 2
|
||||
set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+5+6', '1-2-3-4-5-6', 'not set'}, 'fontsize',7)
|
||||
else
|
||||
set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
|
||||
endif
|
||||
endfor
|
||||
|
||||
% compute cumulative localizations
|
||||
cumResults = zeros(curves+2, length(datasets)+1);
|
||||
for d=1:length(datasets)
|
||||
cumResults(1,d+1) = datasets(d);
|
||||
cumResults(2:end-1,d+1) = round(sum(locResults{1,d}, 2) ./ sum(totalResults{1,d}, 2) * 100);
|
||||
if resultsToShow == 1
|
||||
cumResults(end,d+1) = round(sum(sum(locResults{1,d}.*eye(curves,curves))) / sum(totalResults{1,d},2)(1,1) * 100);
|
||||
endif
|
||||
end
|
||||
cumResults(2:end-1,1) = 1:curves;
|
||||
cumResults
|
||||
@@ -0,0 +1,109 @@
|
||||
|
||||
clear all
|
||||
close all
|
||||
|
||||
# sudo apt install octave-signal
|
||||
pkg load signal
|
||||
|
||||
# Use with files generated by export_stats.sh
|
||||
|
||||
dataDir = 'SET_PATH_TO_RESULTS_DIR';
|
||||
prefix = 'Stat';
|
||||
|
||||
dataset = 6
|
||||
sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
|
||||
sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
|
||||
|
||||
statName = strrep('Loop/Distance_since_last_loc/','/','-');
|
||||
|
||||
data = dlmread([dataDir '/' prefix num2str(dataset) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
|
||||
|
||||
sessions = size(data,2)-1
|
||||
|
||||
j = 1 % session #
|
||||
x3 = data(:,1);
|
||||
y3 = data(:,11);
|
||||
y3 = y3(x3>=sep(j) & x3<=sep(j+1), :);
|
||||
x3 = x3(x3>=sep(j) & x3<=sep(j+1), :);
|
||||
|
||||
max = 0;
|
||||
for i=2:length(y3)
|
||||
if isfinite(y3(i)) && isfinite(y3(i-1)) && (x3(i) - x3(i-1) < 1.5)
|
||||
v = y3(i);
|
||||
if v>max
|
||||
max = v
|
||||
endif
|
||||
endif
|
||||
endfor
|
||||
xb = max/2:max:5;
|
||||
[nn3, xx3] = hist (y3, xb);
|
||||
nn3 = nn3/length(x3);
|
||||
nn3(nn3==0) = NaN;
|
||||
|
||||
for j=1:6
|
||||
|
||||
x{j} = data(:,1);
|
||||
y{j} = data(:,2);
|
||||
y{j} = y{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
|
||||
x{j} = x{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
|
||||
[nn{j}, xx{j}] = hist (y{j}, xb);
|
||||
%nn{j} = nn{j}/length(x{j});
|
||||
%nn{j} = nn{j}/sum(nn{j});
|
||||
|
||||
for k=length(nn{j}):-1:1
|
||||
if nn{j}(k) != 0
|
||||
break;
|
||||
else
|
||||
nn{j}(k) = NaN;
|
||||
endif
|
||||
endfor
|
||||
|
||||
endfor
|
||||
|
||||
j = 6 % session #
|
||||
x4 = data(:,1);
|
||||
y4 = data(:,12);
|
||||
y4 = y4(x4>=sep(j) & x4<=sep(j+1), :);
|
||||
x4 = x4(x4>=sep(j) & x4<=sep(j+1), :);
|
||||
[nn4, xx4] = hist (y4, xb);
|
||||
nn4 = nn4/length(x4);
|
||||
nn4(nn4==0) = NaN;
|
||||
|
||||
figure
|
||||
hold on
|
||||
%plot(x1,y1, '.-');
|
||||
%plot(x2-x2(1),y2, '.-');
|
||||
plot(x3-x3(1),y3, '.-');
|
||||
%plot(x4-x4(1),y4, '.-');
|
||||
%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
|
||||
legend('Map1+2+3+4+5+6 -> LocA')
|
||||
xlabel('Time')
|
||||
ylabel('m')
|
||||
title('Distance since last loc')
|
||||
|
||||
figure
|
||||
hold on
|
||||
%plot(x1,y1, '.-');
|
||||
%plot(x2-x2(1),y2, '.-');
|
||||
plot(x{6}-x{6}(1),y{6}, '.-');
|
||||
%plot(x4-x4(1),y4, '.-');
|
||||
%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
|
||||
legend('Map1 -> LocF')
|
||||
xlabel('Time')
|
||||
ylabel('m')
|
||||
title('Distance since last loc')
|
||||
|
||||
figure
|
||||
hold on
|
||||
for j=1:6
|
||||
plot(xx{j},nn{j}, '-', 'linewidth', 3)
|
||||
endfor
|
||||
%plot(xx3,nn3, '.-', 'linewidth', 3)
|
||||
%plot(xx4,nn4, '.-', 'linewidth', 3)
|
||||
%legend('Loc-F', 'Loc-E', 'Loc-D', 'Loc-C', 'Loc-B', 'Loc-A')
|
||||
legend('A-16:51', 'B-17:31', 'C-17:58', 'D-18:30', 'E-18:59', 'F-19:42', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
|
||||
%h = get(gca,'Children');
|
||||
%set(gca,'Children',[h(6) h(5) h(4) h(3) h(2) h(1)])
|
||||
%set(gca, 'YScale', 'log')
|
||||
xlabel(['Distance not localized (m) Step ' num2str(max) ' m'])
|
||||
ylabel('Re-Localization Probability on Map 1 (16:46)')
|
||||
@@ -0,0 +1,19 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 1 ]
|
||||
then
|
||||
echo "No arguments supplied. It should be the data directory (where the reprocessed map databases are saved)."
|
||||
exit
|
||||
fi
|
||||
DATA=$1
|
||||
|
||||
DETECTOR=(0 1 6 7 9 14 11 111)
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
for d in "${DETECTOR[@]}"
|
||||
do
|
||||
rtabmap-report --export --export_prefix "Stat$d" --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
|
||||
rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
|
||||
done
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 3 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
TYPE=$1
|
||||
INPUT=$2
|
||||
OUTPUT=$3
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
[ ! -d "$OUTPUT" ] && mkdir $OUTPUT
|
||||
[ ! -d "$OUTPUT/$TYPE" ] && mkdir $OUTPUT/$TYPE
|
||||
# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
|
||||
DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
|
||||
|
||||
PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
|
||||
|
||||
if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
|
||||
then
|
||||
# binary descriptors
|
||||
PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
|
||||
else
|
||||
# float descriptors
|
||||
PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
|
||||
fi
|
||||
|
||||
if [ $TYPE -eq 111 ]
|
||||
then
|
||||
PARAMS="--Vis/CorNNType 6 --SuperGlue/Path SuperGluePretrainedNetwork/rtabmap_superglue.py --Reg/RepeatOnce false --Vis/CorGuessWinSize 0 $PARAMS --Kp/DetectorStrategy 11 --Vis/FeatureType 11"
|
||||
fi
|
||||
|
||||
echo $PARAMS
|
||||
for db in "${DATABASES[@]}"
|
||||
do
|
||||
rtabmap-reprocess --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 0 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint_v1.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --Db/TargetVersion "" --Icp/CorrespondenceRatio 0.1 --RGBD/MaxOdomCacheSize 0 --uwarn $PARAMS $INPUT/$db $OUTPUT/$TYPE/$db
|
||||
rtabmap-detectMoreLoopClosures --uwarn $OUTPUT/$TYPE/$db
|
||||
done
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
INPUT=$1
|
||||
OUTPUT=$2
|
||||
|
||||
DETECTOR=(0 1 6 7 9 14 11 111)
|
||||
|
||||
for d in "${DETECTOR[@]}"
|
||||
do
|
||||
./reprocess_maps.sh $d $INPUT $OUTPUT
|
||||
./run_merge.sh $d $OUTPUT
|
||||
done
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
#!/bin/bash
|
||||
|
||||
export PATH=~/workspace/rtabmap/build/bin:$PATH
|
||||
export LD_LIBRARY_PATH=~/workspace/rtabmap/build/lib:$LD_LIBRARY_PATH
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
INPUT=$1
|
||||
OUTPUT=$2
|
||||
|
||||
./reprocess_maps_all.sh $INPUT $OUTPUT
|
||||
./run_localization_single_all.sh $INPUT $OUTPUT
|
||||
./run_consecutive_localization_all.sh $OUTPUT
|
||||
|
||||
+25
@@ -0,0 +1,25 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
|
||||
exit
|
||||
fi
|
||||
TYPE=$1
|
||||
DATA=$2
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
|
||||
TARGETS=($DATA/$TYPE'/map_190321-172717.db;'$DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-182709.db;' $DATA/$TYPE'/map_190321-182709.db;'$DATA/$TYPE'/map_190321-185608.db' $DATA/$TYPE'/map_190321-185608.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-193556.db' )
|
||||
|
||||
|
||||
[ ! -d "$DATA/$TYPE/consecutive_loc" ] && mkdir $DATA/$TYPE/consecutive_loc
|
||||
|
||||
for i in ${!SOURCE[@]}
|
||||
do
|
||||
db=${SOURCE[$i]}
|
||||
loc_dbs=${TARGETS[$i]}
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$DATA/$TYPE/$db;$loc_dbs" $DATA/$TYPE/consecutive_loc/loc_$db
|
||||
done
|
||||
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 1 ]
|
||||
then
|
||||
echo "No arguments supplied. It should be the data directory (where the reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
DATA=$1
|
||||
|
||||
DETECTOR=(0 1 6 7 9 14 11 111)
|
||||
|
||||
for d in "${DETECTOR[@]}"
|
||||
do
|
||||
./run_consecutive_localization.sh $d $DATA
|
||||
done
|
||||
|
||||
+33
@@ -0,0 +1,33 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 3 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
TYPE=$1
|
||||
INPUT=$2
|
||||
OUTPUT=$3
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
|
||||
LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
|
||||
|
||||
[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/accuracy
|
||||
|
||||
db=merged_123456.db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
|
||||
|
||||
rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
|
||||
|
||||
|
||||
+28
@@ -0,0 +1,28 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 3 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
TYPE=$1
|
||||
INPUT=$2
|
||||
OUTPUT=$3
|
||||
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db'
|
||||
DATABASES=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db')
|
||||
# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
|
||||
LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
|
||||
|
||||
[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/loc
|
||||
|
||||
echo $PARAMS
|
||||
for db in "${DATABASES[@]}"
|
||||
do
|
||||
rtabmap-reprocess -loc_null --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/loc/loc_$db
|
||||
done
|
||||
|
||||
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
INPUT=$1
|
||||
OUTPUT=$2
|
||||
|
||||
DETECTOR=(0 1 6 7 9 14 11 111)
|
||||
|
||||
for d in "${DETECTOR[@]}"
|
||||
do
|
||||
./run_localization_single.sh $d $INPUT $OUTPUT
|
||||
done
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
|
||||
exit
|
||||
fi
|
||||
TYPE=$1
|
||||
DATA=$2
|
||||
|
||||
MIN_INLIERS=20 #20 40 60 80
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
DATABASES="$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-185608.db;$DATA/$TYPE/map_190321-193556.db"
|
||||
|
||||
# To compute "Ground truth"
|
||||
rtabmap-reprocess --uwarn "$DATABASES" $DATA/$TYPE/merged_123456.db
|
||||
|
||||
cp $DATA/$TYPE/merged_123456.db $DATA/$TYPE/merged_123456_gt.db
|
||||
rtabmap-detectMoreLoopClosures -r 0.5 -i 5 $DATA/$TYPE/merged_123456_gt.db
|
||||
|
||||
rtabmap-reprocess --uwarn -gt $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456.db
|
||||
|
||||
rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_16.db
|
||||
|
||||
rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-185608.db" $DATA/$TYPE/merged_135.db
|
||||
|
||||
rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_246.db
|
||||
|
||||
# Reduced graph
|
||||
rtabmap-reprocess --uwarn -gt --Mem/ReduceGraph true --Vis/MinInliers $MIN_INLIERS $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456_reduced.db
|
||||
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
import torch
|
||||
import torchvision
|
||||
from demo_superpoint import SuperPointNet
|
||||
model = SuperPointNet()
|
||||
model.load_state_dict(torch.load("superpoint_v1.pth"))
|
||||
model.eval()
|
||||
example = torch.rand(1, 1, 640, 480)
|
||||
traced_script_module = torch.jit.trace(model, example)
|
||||
traced_script_module.save("superpoint_v1.pt")
|
||||
@@ -0,0 +1,20 @@
|
||||
#!/bin/bash
|
||||
|
||||
if [ $# -lt 2 ]
|
||||
then
|
||||
echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
|
||||
exit
|
||||
fi
|
||||
INPUT=$1
|
||||
OUTPUT=$2
|
||||
|
||||
DETECTOR=(0 1 6 7 9 14 11 111)
|
||||
|
||||
source rtabmap_latest.bash
|
||||
|
||||
for d in "${DETECTOR[@]}"
|
||||
do
|
||||
valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false "${OUTPUT}/${d}/map_190321-164651.db;${INPUT}/loc_190321-165128.db" output.db
|
||||
rm output.db
|
||||
done
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
##close all
|
||||
##clear all
|
||||
|
||||
%% Use Export Poses in TORO format, then copy columns
|
||||
load vertexes.txt;
|
||||
load edges.txt;
|
||||
|
||||
set(0,'defaultAxesFontName', 'Times')
|
||||
set(0,'defaultTextFontName', 'Times')
|
||||
|
||||
%matlab indexes % rtabmap indexes
|
||||
endMap1 = 201; % ID=206
|
||||
endMap2 = 401; % ID=411
|
||||
endMap3 = 604; % ID=621
|
||||
endMap4 = 794; % ID=814
|
||||
endMap5 = 968; % ID=990
|
||||
endMap6 = 1201; % ID=1230
|
||||
|
||||
%% 3D
|
||||
|
||||
t = vertexes(:,1);
|
||||
|
||||
##figure
|
||||
##plot3(vertexes(1:endMap1,2), vertexes(1:endMap1,3), vertexes(1:endMap1,1))
|
||||
##hold on
|
||||
##plot3(vertexes(endMap1+1:endMap2,2), vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1))
|
||||
##plot3(vertexes(endMap2+1:endMap3,2), vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1))
|
||||
##plot3(vertexes(endMap3+1:endMap4,2), vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1))
|
||||
##plot3(vertexes(endMap4+1:endMap5,2), vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1))
|
||||
##plot3(vertexes(endMap5+1:end,2), vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1))
|
||||
|
||||
mapIds = zeros(vertexes(end,1), 2); % matlab index to vertexes, map id
|
||||
for i=1:size(vertexes,1)
|
||||
mapIds(vertexes(i,1),1) = i;
|
||||
if i <= endMap1
|
||||
mapIds(vertexes(i,1),2) = 1;
|
||||
elseif i<=endMap2
|
||||
mapIds(vertexes(i,1),2) = 2;
|
||||
elseif i<=endMap3
|
||||
mapIds(vertexes(i,1),2) = 3;
|
||||
elseif i<=endMap4
|
||||
mapIds(vertexes(i,1),2) = 4;
|
||||
elseif i<=endMap5
|
||||
mapIds(vertexes(i,1),2) = 5;
|
||||
else
|
||||
mapIds(vertexes(i,1),2) = 6;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
##interLoopClosures = 0;
|
||||
##intraLoopClosures = 0;
|
||||
##
|
||||
##for i=1:size(edges, 1)
|
||||
## if edges(i,2) > edges(i,1)+1
|
||||
## x = [vertexes(mapIds(edges(i,1),1), 2) vertexes(mapIds(edges(i,2),1), 2)];
|
||||
## y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
|
||||
## t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
|
||||
## if mapIds(edges(i,1),2) ~= mapIds(edges(i,2),2)
|
||||
## plot3(x,y,t, 'g')
|
||||
## interLoopClosures = interLoopClosures+1;
|
||||
## else
|
||||
## plot3(x,y,t, 'r')
|
||||
## intraLoopClosures = intraLoopClosures + 1;
|
||||
## end
|
||||
## end
|
||||
##end
|
||||
##xlabel('x')
|
||||
##ylabel('y')
|
||||
##zlabel('Node indexes')
|
||||
##
|
||||
##interLoopClosures
|
||||
##intraLoopClosures
|
||||
|
||||
%% 2D
|
||||
figure
|
||||
hold on
|
||||
plot([-8 6], [vertexes(endMap1,1) vertexes(endMap1,1)], 'k:')
|
||||
plot([-8 6], [vertexes(endMap2,1) vertexes(endMap2,1)], 'k:')
|
||||
plot([-8 6], [vertexes(endMap3,1) vertexes(endMap3,1)], 'k:')
|
||||
plot([-8 6], [vertexes(endMap4,1) vertexes(endMap4,1)], 'k:')
|
||||
plot([-8 6], [vertexes(endMap5,1) vertexes(endMap5,1)], 'k:')
|
||||
|
||||
colors = {'r:', 'g:', 'c:', 'y:', 'm:', 'c'};
|
||||
|
||||
for i=1:size(edges, 1)
|
||||
if edges(i,2) > edges(i,1)+1
|
||||
y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
|
||||
t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
|
||||
mapId = mapIds(edges(i,1),2);
|
||||
if mapId ~= mapIds(edges(i,2),2) && (mapId == 1 || mapIds(edges(i,2),2) == 1)
|
||||
plot(y,t, 'r')
|
||||
else
|
||||
%plot(y,t, 'r')
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
curveColor = 'b'
|
||||
plot(vertexes(1:endMap1,3), vertexes(1:endMap1,1), curveColor)
|
||||
plot(vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1), curveColor)
|
||||
plot(vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1), curveColor)
|
||||
plot(vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1), curveColor)
|
||||
plot(vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1), curveColor)
|
||||
plot(vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1), 'k')
|
||||
|
||||
xlabel('y')
|
||||
ylabel('Node indexes')
|
||||
Reference in New Issue
Block a user