feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,187 @@
|
||||
%%%%%%%%%%%%%%%%%%
|
||||
%Example of the recursive Bayes filtering
|
||||
%In this example, the memory is fixed (each new signatures aren't added to
|
||||
%the memory after each iteration). The likelihood is also predefined to see
|
||||
%the effect of the filter.
|
||||
%%%%%%%%%%%%%%%%%%
|
||||
close all
|
||||
clear all
|
||||
|
||||
%%%%%%%%%%%%
|
||||
%User inputs
|
||||
%%%%%%%%%%%%
|
||||
stepByStep = 0; %If we want a pause after each iteration (1) otherwise batch mode (0)
|
||||
usePrefinedGaussians = 0;
|
||||
loopThreshold = 0.3;
|
||||
%%%%%%%%%%%%
|
||||
|
||||
%Based on the very simple example on discrete recursive Bayes filter found
|
||||
%in the green book p. 543
|
||||
%(Russel and Norvig : Artificial Intelligence - A Modern Approach)
|
||||
|
||||
m = 11; %image counts in the working memory (virtual place included)
|
||||
nIter = 10; %iterations
|
||||
virtualPlacePrior = 0.8;
|
||||
|
||||
% likelihood = rand(nIter,m);
|
||||
likelihood = [0.537812527177546,0.973823826601782,0.679517492334616,0.511334385523834,0.133172784517682,0.616295123827850,0.333032553637148,0.0931269898117809,0.834956800914038,0.789846368465099,0.966949104425550,0.646502710583242;0.134060600345818,0.764640454269780,0.704121891203037,0.914142330394849,0.295008954572581,0.669364170554407,0.413416552284401,0.319027136248953,0.325143400597060,0.913520615552384,0.208804360594363,0.128169077499018;0.540947224212142,0.243683930209342,0.460884056746050,0.0919339388425310,0.166627082935154,0.0372015327510951,0.414348152562633,0.886961614706448,0.367639299837922,0.533254359985603,0.520476252773875,0.0813205028568441;0.857368192475905,1,2,8,2,1,0.414348152562633,0.413416552284401,0.794839466004007,0.804076444490502,0.225546129397670,0.659227054221110;0.198017363329941,0.137854510225166,1,2,8,2,1,0.414348152562633,0.0993087162421777,0.562660356611037,0.567197873830803,0.0273988510815916;0.155609101290637,0.629811667025549,0.0762044335388465,1,2,8,2,1,0.414348152562633,0.750876451961769,0.998163526074098,0.985179800828888;0.0613777678138677,0.857014692659853,0.444624971046257,0.785375111038118,1,2,8,2,1,0.414348152562633,0.131865336731199,0.539330947126751;0.661073988932246,0.899798333270360,0.165706296551124,0.602400257965727,0.218271272437479,1,2,8,2,1,0.414348152562633,0.373834916709097;0.0186026419313314,0.348368402231864,0.398748973477181,0.465908353449366,0.706077628393318,0.522221302087482,1,2,8,2,1,0.414348152562633;0.291102936564625,0.486310193286963,0.920584670910461,0.298131346189007,0.0390136924156214,0.567617714955188,0.901802906347484,1,2,8,2,1;];
|
||||
likelihoodNormalized = ones(nIter,m+1);
|
||||
|
||||
%Initialize the prediction matrix
|
||||
%first row is for the virtual place (loop closure)
|
||||
%next rows are for each already visited place
|
||||
prediction = zeros(m+1,m+1);
|
||||
%Gaussians...
|
||||
if(usePrefinedGaussians == 0)
|
||||
%A-Create prediction with two gaussians
|
||||
x = 1:1:m;
|
||||
for i=1:m
|
||||
y1 = gaussmf(x, [0.8 i-1]);
|
||||
y2 = gaussmf(x, [0.8 i+1]);
|
||||
y = y1+y2;
|
||||
y = (y / sum(y));
|
||||
prediction(i+1,:) = [0.1 y];
|
||||
%normalize
|
||||
prediction(i+1,:) = prediction(i+1,:) / sum(prediction(i+1,:));
|
||||
end
|
||||
prediction(1,:) = [virtualPlacePrior ones(1,m)*0.1/(m)];
|
||||
|
||||
%Example
|
||||
x=1:1:9;
|
||||
y=gaussmf(x,[0.8 4])+gaussmf(x,[0.8 6]);
|
||||
y=y/sum(y);
|
||||
y=[0.1 y];
|
||||
y=y/sum(y);
|
||||
figure(1);
|
||||
plot([-1 x],y)
|
||||
title(['Example of the prediction (two gaussians) for the loop closure event at 5' char(10) '(we suppose that it''s more probable to be in the next/previous place) after a loop closure.' char(10) 'Id -1 means a new place.'])
|
||||
|
||||
else
|
||||
%B-Create prediction using predefined values (representing approximativaly
|
||||
%a sum of gaussian curves)
|
||||
%(like implemented in c++, AvpdCore::BayesFilter::generatePrediction())
|
||||
%Format of predictionLC = {virtualPlace LoopCLosure neighbor-1 neighbor+1 neighbor-2 neighbor+2...}
|
||||
predictionLC = [0.1 0.175 0.1 0.275 0.05 0.15 0.025 0.025]; %Forward probabilities
|
||||
% predictionLC = [0.1 0.19 0.24 0.24 0.1 0.1 0.01 0.01]; %equal backward/forward
|
||||
prediction = generatePrediction(virtualPlacePrior, predictionLC, m);
|
||||
|
||||
%Example
|
||||
x=1:1:9;
|
||||
smallValue = (1-sum(predictionLC)) / length(x-5);
|
||||
y = [predictionLC(1) smallValue predictionLC(7) predictionLC(5) predictionLC(3) predictionLC(2) predictionLC(4) predictionLC(6) predictionLC(8) smallValue];
|
||||
y=y/sum(y);
|
||||
figure(1);
|
||||
plot([-1 x],y)
|
||||
title(['Example of the prediction (predefined pdf) for the loop closure event at 5' char(10) '(we suppose that it''s more probable to be in the next and/or previous place) after a loop closure.' char(10) 'Id -1 means a new place.'])
|
||||
end
|
||||
|
||||
%Initialize the prior with "no loop closure event"
|
||||
prior = zeros(nIter+1, m+1);
|
||||
prior(1,:) = prediction(1,:);
|
||||
|
||||
figure(2)
|
||||
subplot(211)
|
||||
imagesc(prediction)
|
||||
title('prediction matrix (m+1 x m+1 where m is the number of images + 1 virtual place)')
|
||||
subplot(212)
|
||||
imagesc(prediction')
|
||||
title('prediction^T')
|
||||
|
||||
statusLoop = zeros(1,nIter);
|
||||
|
||||
loopClosureId = 0;
|
||||
for iter=1:nIter
|
||||
%reset loopClosureId
|
||||
loopClosureId = 0;
|
||||
|
||||
%Adjust likelihood
|
||||
likelihoodNormalized(iter,:) = adjustLikelihood(likelihood(iter,:));
|
||||
|
||||
%posterior = n * likelihood * (prediction x prior)
|
||||
% Note the transposed prediction'
|
||||
prior(iter+1,:) = likelihoodNormalized(iter,:) .* (prediction' * prior(iter,:)')';
|
||||
|
||||
%normalize
|
||||
prior(iter+1,:) = prior(iter+1,:) / sum(prior(iter+1,:));
|
||||
|
||||
% Loop closure ?
|
||||
maxLoopProb = 0;
|
||||
for i=2:m+1
|
||||
if prior(iter+1,i) > maxLoopProb
|
||||
if i >= 3 && i <= m
|
||||
if prior(iter+1,i-1)+prior(iter+1,i)+prior(iter+1,i+1) > loopThreshold
|
||||
loopClosureId = i-1;
|
||||
maxLoopProb = prior(iter+1,i);
|
||||
end
|
||||
elseif i == 2
|
||||
if prior(iter+1,i) + prior(iter+1,i+1) > loopThreshold
|
||||
loopClosureId = i-1;
|
||||
maxLoopProb = prior(iter+1,i);
|
||||
end
|
||||
else
|
||||
if prior(iter+1,i-1) + prior(iter+1,i) > loopThreshold
|
||||
loopClosureId = i-1;
|
||||
maxLoopProb = prior(iter+1,i);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
statusLoop(iter) = loopClosureId;
|
||||
|
||||
figure(3)
|
||||
plot(statusLoop(1:iter), 'o');
|
||||
title(['Loop closure (image id)(iteration ' num2str(iter) ')'])
|
||||
xlabel('Iteration')
|
||||
ylabel('Loop closure id')
|
||||
|
||||
figure(4)
|
||||
subplot(411)
|
||||
plot(prior(iter,:)');
|
||||
title(['prior (iteration ' num2str(iter) ')'])
|
||||
subplot(412)
|
||||
plot((prediction' * prior(iter,:)')');
|
||||
title('prediction^T x prior')
|
||||
subplot(413)
|
||||
plot(likelihoodNormalized(iter,:));
|
||||
title('likelihoodNormalized')
|
||||
subplot(414)
|
||||
plot(prior(iter+1,:));
|
||||
title('posterior = n * likelihood * (prediction^T x prior)')
|
||||
|
||||
if(stepByStep ~=0)
|
||||
disp('Press any key to continue...')
|
||||
pause
|
||||
end
|
||||
end
|
||||
|
||||
figure(5)
|
||||
subplot(3,1,1)
|
||||
plot(likelihood');
|
||||
title('likelihood')
|
||||
subplot(3,1,2)
|
||||
plot(likelihoodNormalized');
|
||||
title('likelihoodNormalized')
|
||||
subplot(3,1,3)
|
||||
plot(prior');
|
||||
title('posterior')
|
||||
|
||||
%% We simulate here when the neighbors of id=4 don't exist...
|
||||
% On loop closure, the pdf is {1 ... 1 2 3 5 3 1...} where the
|
||||
% first 3 is the center... (its a forward probability)
|
||||
% No loop closure, the pdf is {10 1 1 1 1 1 1....}
|
||||
figure(6)
|
||||
tmp=[10,1,1,1,1,1,1,1,1,1,1;
|
||||
1,6,5,4,0,0,0,0,0,0,0;
|
||||
1,3,3,9,0,0,0,0,0,0,0;
|
||||
1,1,2,12,0,0,0,0,0,0,0;
|
||||
1,0,0,0,15,0,0,0,0,0,0;
|
||||
1,0,0,0,0,6,5,3,1,0,0;
|
||||
1,0,0,0,0,3,3,5,3,1,0;
|
||||
1,0,0,0,0,1,2,3,5,3,1;];
|
||||
subplot(211)
|
||||
imagesc(tmp)
|
||||
title('Example of the effect of forgotten neighbors (around 5) on a prediction matrix')
|
||||
subplot(212)
|
||||
imagesc(tmp')
|
||||
title('prediction^T')
|
||||
@@ -0,0 +1,17 @@
|
||||
function LN = adjustLikelihood(L)
|
||||
%ADJUSTLIKELIHOOD Adjust the likelihood with std dev and mean.
|
||||
% LN = adjustLikelihood(L)
|
||||
%
|
||||
% L the likelihood (m,1)
|
||||
% LN the likelihood normalized (m,1)
|
||||
|
||||
m = mean(L); % Calcul mean
|
||||
s = std(L); % Calcul std dev
|
||||
LN = zeros(size(L));
|
||||
for i=1:length(L)
|
||||
if L(i) > m + s
|
||||
LN(i) = (L(i)-s)/m;
|
||||
else
|
||||
LN(i) = 1;
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,46 @@
|
||||
function likelihood = computeLikelihood(sign, memory, dictionary)
|
||||
%GETLIKELIHOOD Compute the likelihood of the signature sign.
|
||||
% likelihood = getLikelihood(sign, memory, dictionary)
|
||||
%
|
||||
% Mem : the memory (Mem = [signId1 WordRefIds...; signId2 WordRefIds...; ...])
|
||||
% Dict : the visual words dictionary (Dict = [wordId1 SignRefIds...;
|
||||
% wordId2 SignRefIds...; ...])
|
||||
% sign : The signature reference which we want the likelihood with all
|
||||
% others in the memory (sign = [id wordsRef...])
|
||||
likelihood = zeros(size(memory,1),1);
|
||||
|
||||
nwi = 0; % nwi is the number of a specific word referenced by a place
|
||||
ni = 0; % ni is the total of words referenced by a place
|
||||
nw = 0; % nw is the number of places referenced by a specific word
|
||||
N = 0; % N is the total number of places
|
||||
|
||||
N = size(memory,1);
|
||||
|
||||
words = unique(sign(2:end));
|
||||
|
||||
for i=1:length(words)
|
||||
if words(i) ~= 0
|
||||
% "Inverted index" - For each places referenced by the word
|
||||
refs = unique(dictionary(find(dictionary(:,1) == words(i),1), 2:end));
|
||||
refs = refs(refs~=0);
|
||||
nw = 0;
|
||||
for j=1:length(refs)
|
||||
nw = nw + 1;
|
||||
end
|
||||
logNnw = log10(N/nw); % TODO : division by 0 (not supposed to occur)
|
||||
|
||||
if logNnw ~= 0
|
||||
for j=1:length(refs)
|
||||
pos = find(memory(:,1) == refs(j),1);
|
||||
sign = memory(pos, :);
|
||||
|
||||
nwi = sum(sign(2:end) == words(i));
|
||||
ni = sum(sign(2:end)>0);
|
||||
|
||||
if ni ~= 0
|
||||
likelihood(pos) = likelihood(pos) + ( nwi * logNnw ) / ni;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,85 @@
|
||||
function P = generatePrediction(NewPlacePrediction, LoopClosurePrediction, m)
|
||||
% GENERATEPREDICTION Generate a prediction matrix P(m+1,m+1) for the Bayes
|
||||
% filter
|
||||
% P = generatePrediction(NewPlacePrediction, LoopClosurePrediction, m)
|
||||
%
|
||||
% P(1,:) is the prediction for the "no loop closure event" using the
|
||||
% NoLoopClosurePrediction. NewPlacePrediction is a number between 0
|
||||
% and 1.
|
||||
%
|
||||
% P(2:end, :) is the predictions for each m "loop closure event" using the
|
||||
% LoopClosurePrediction pattern. Format LoopClosurePrediction:
|
||||
% [virtualPlace LoopCLosure neighbor-1 neighbor+1 neighbor-2
|
||||
% neighbor+2...]
|
||||
%
|
||||
% Example:
|
||||
% predictionLC = [0.1 0.19 0.24 0.24 0.1 0.1 0.01 0.01];
|
||||
% virtualPlacePrior = 0.8;
|
||||
% m = 10; %We have 10 places
|
||||
% P = generatePrediction(virtualPlacePrior, predictionLC, m);
|
||||
P = zeros(m+1,m+1);
|
||||
if NewPlacePrediction<0 || NewPlacePrediction>1
|
||||
error(['NoLoopClosurePrediction=' num2str(NewPlacePrediction) ' > 1 or < 0!']);
|
||||
end
|
||||
if sum(LoopClosurePrediction) > 1
|
||||
error(['sum(LoopClosurePrediction)=' num2str(sum(LoopClosurePrediction)) ' > 1 or < 0!']);
|
||||
elseif(sum(LoopClosurePrediction) == 1)
|
||||
warning(['sum(LoopClosurePrediction)=' num2str(sum(LoopClosurePrediction)) ' == 1, all probabilities will be zero for non-neighbors']);
|
||||
end
|
||||
P(1,:) = [NewPlacePrediction ones(1,m)*(1-NewPlacePrediction)/(m)];
|
||||
predictionLC = LoopClosurePrediction;
|
||||
for i=2:m+1
|
||||
y = zeros(1,m+1);
|
||||
|
||||
loopClosureId = i;
|
||||
|
||||
% The first must be the virtual place
|
||||
y(1) = predictionLC(1);
|
||||
|
||||
% Set all others to a small value
|
||||
y(2:length(y)) = (1-sum(predictionLC))/m;
|
||||
|
||||
% Set high values (gaussians curves) to loop closure neighbors
|
||||
probAdded = 0;
|
||||
% LoopID
|
||||
y(i) = y(i) + predictionLC(2); %0.175
|
||||
probAdded = probAdded + predictionLC(2);
|
||||
|
||||
% look up backward for each neighbors
|
||||
n = loopClosureId;
|
||||
for k=3:2:length(predictionLC)
|
||||
n = n-1;
|
||||
if n > 1
|
||||
y(n) = y(n) + predictionLC(k);
|
||||
probAdded = probAdded + predictionLC(k);
|
||||
else
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
% look up forward for each neighbors
|
||||
n = loopClosureId;
|
||||
for k=4:2:length(predictionLC)
|
||||
n = n+1;
|
||||
if n <= length(y)
|
||||
y(n) = y(n) + predictionLC(k);
|
||||
probAdded = probAdded + predictionLC(k);
|
||||
else
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
% add values not set (they are forgotten) to the loop id
|
||||
totalLCProb = sum(predictionLC(2:length(predictionLC)));
|
||||
if(probAdded >= 0 && probAdded < totalLCProb)
|
||||
y(i) = y(i) + totalLCProb - probAdded;
|
||||
elseif(probAdded > totalLCProb+0.001)
|
||||
error(['probAdded=' num2str(probAdded) ' > ' num2str(totalLCProb) ' ?']);
|
||||
end
|
||||
|
||||
P(i,:) = y;
|
||||
sum(P(i,:));
|
||||
if sum(P(i,:)) < 1-0.001 || sum(P(i,:)) > 1+0.001
|
||||
error(['sum of the resulting pdf is not one (' num2str(sum(P(i,:))) ')!']);
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,69 @@
|
||||
function [CS DU] = updateCommonSignature(Mem, Dict)
|
||||
%UPDATECOMMONSIGNATURE Update the common signature (virtual place)
|
||||
% [CS DU] = updateCommonSignature(Mem, Dict)
|
||||
%
|
||||
% CS = common signature
|
||||
% DU dictionary updated
|
||||
% Mem : the memory (Mem = [signId1 WordRefIds...; signId2 WordRefIds...; ...])
|
||||
% Dict : the visual words dictionary (Dict = [wordId1 SignRefIds...; wordId2 SignRefIds...; ...])
|
||||
|
||||
CS = Mem(1,:);
|
||||
CSid = Mem(1,1);
|
||||
|
||||
% Clear references to the virtual place
|
||||
for i=2:length(CS)
|
||||
index = find(Dict(:,1) == CS(i));
|
||||
if isempty(index) ~= 1
|
||||
indexes = find(Dict(index, :) == CSid);
|
||||
for j=1:length(indexes)
|
||||
Dict(index,indexes(j)) = 0;
|
||||
end
|
||||
end
|
||||
end
|
||||
%clear words in the common signature
|
||||
CS = CS(1);
|
||||
|
||||
%How many words we want... take the average of words by signature
|
||||
nbCommonWords = 0;
|
||||
memSize = size(Mem,1)-1; %Don't count the virtual place
|
||||
if(memSize > 0)
|
||||
totalActiveRef = sum(sum(Dict(:,2:end) ~= 0));
|
||||
nbCommonWords = floor(totalActiveRef / memSize);
|
||||
end
|
||||
if nbCommonWords>0
|
||||
%get common words
|
||||
list = [(sum(Dict(:,2:end)' ~= 0)') Dict(:,1)];
|
||||
list = sortrows(list)
|
||||
wordsAdded=0;
|
||||
for i=size(list,1):-1:1
|
||||
if i ~= size(list,1) && length(CS)>1
|
||||
ratio = floor(list(i+1,1)/list(i,1));
|
||||
len = length(CS);
|
||||
for j=2:ratio
|
||||
for k=2:len
|
||||
CS = [CS CS(k)];
|
||||
wordsAdded = wordsAdded + 1;
|
||||
if wordsAdded >= nbCommonWords
|
||||
break;
|
||||
end
|
||||
end
|
||||
if wordsAdded >= nbCommonWords
|
||||
break;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
if wordsAdded < nbCommonWords
|
||||
CS = [CS list(i,2)];
|
||||
wordsAdded = wordsAdded + 1;
|
||||
end
|
||||
if wordsAdded >= nbCommonWords
|
||||
break;
|
||||
end
|
||||
end
|
||||
CS = [CS zeros(1,size(Mem,2) - length(CS))];
|
||||
% add references
|
||||
DU = updateDictionary(Dict, CS);
|
||||
else
|
||||
DU = Dict;
|
||||
end
|
||||
@@ -0,0 +1,29 @@
|
||||
function DU = updateDictionary(Dict, Sign)
|
||||
%UPDATEDICTIONARY Update the dictionary with the new signature (place)
|
||||
% DU = updateDictionary(Dict, Sign)
|
||||
%
|
||||
% DU = dictionary updated
|
||||
% dictionary = [wordId1 SignRefIds...; wordId2 SignRefIds...; ...]
|
||||
% sign = [signId WordRefIds...]
|
||||
|
||||
signId = Sign(1);
|
||||
Sign = Sign(Sign~=0);
|
||||
for i=2:length(Sign)
|
||||
indexWord = [];
|
||||
if isempty(Dict) ~= 1
|
||||
indexWord = find(Dict(:,1) == Sign(i),1);
|
||||
end
|
||||
if isempty(indexWord) == 1
|
||||
Dict(size(Dict,1)+1,1) = Sign(i); %Word id
|
||||
Dict(size(Dict,1),2) = signId; %Signature id ref
|
||||
else
|
||||
ii = 1:length(Dict(indexWord,1:end));
|
||||
indexesZero = ii(Dict(indexWord,1:end) == 0);
|
||||
if isempty(indexesZero)
|
||||
Dict(indexWord, size(Dict,2)+1) = signId;
|
||||
else
|
||||
Dict(indexWord, indexesZero(1)) = signId;
|
||||
end
|
||||
end
|
||||
end
|
||||
DU = Dict;
|
||||
Reference in New Issue
Block a user