moved rtabmap-ros-pkg project in this project

git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@52 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-10-04 08:47:45 +08:00 · 2011-06-05 14:45:39 +00:00
commit 2d73090f3a
338 changed files with 56859 additions and 0 deletions
@@ -0,0 +1,121 @@
+%%%%%%%%%%%%%%%%%%%%%%%%
+%In this example, we show how the resursive Bayes filtering works in RTAB-Map
+%(Already Visited Place Detector). We load real data from an experiment on
+%the 090206-3 dataset (in bin/data/090206-3). This example doesn't
+%show the "forgotten" skill of the real algorithm implemented in RTAB-Map, so
+%it doesn't show how probabilities are managed when a signature is
+%forgotten. The links (parent, loop) between signatures are not shown.
+%
+%You can use the same script for your data. Just have the same format than
+%the one used in the data text files. An other way is to use the RTAB-Map Gui,
+%it provides an option to dump the working memory (Edit->Dump memory)
+%directly. 
+%%%%%%%%%%%%%%%%%%%%%%%%
+
+close all
+clear all
+
+%%%%%%%%%%%%
+%User inputs
+%%%%%%%%%%%%
+stepByStep = 0; %If we want a pause after each iteration (1) otherwise batch mode (0)
+plotPriorAfterEachIter = 1;
+loopThreshold = 0.3;
+predictionNP = 0.8; % prediction for "New place event"
+predictionLC = [0.1 0.175 0.1 0.275 0.05 0.15 0.025 0.025]; % prediction pattern for "Loop closure event"
+%%%%%%%%%%%%
+
+% load data
+signRef = dlmread('DumpMemorySign.txt', ' ', 1, 0);
+
+%ignore the virtual place, it will be generated afer each iteration
+signRef = signRef(2:end,:); %preallocation
+nIter = size(signRef,1);
+
+% Initialize the prior
+prior = cell(nIter,1); %preallocation
+% Initialize the memory, contains only a virtual place null
+memory = zeros(nIter+1,size(signRef,2)); %preallocation
+memory(1,1) = -1; %id virtual place
+dictionary = [];
+
+timeUpdateDictionary = zeros(nIter,1); %seconds
+timeUpdateCommonSignature = zeros(nIter,1); %seconds
+timeGetLikelihood = zeros(nIter,1); %seconds
+timeAdjustLikelihood = zeros(nIter,1); %seconds
+timeGeneratePrediction = zeros(nIter,1); %seconds
+timeUpdatePrior = zeros(nIter,1); %seconds
+
+% Recursive Bayes estimation
+for iter=1:nIter
+    % Signature creation
+    newSign = signRef(iter,:); %[id wordIds...]
+    t = cputime;
+    dictionary = updateDictionary(dictionary, newSign);
+    timeUpdateDictionary(iter) = cputime - t;
+    t = cputime;
+    
+    % add to memory
+    memory(iter+1, :) = newSign;
+    
+    % Update virtual place
+    [memory(1,:) dictionary] = updateCommonSignature(memory(1:iter+1,:), dictionary);
+    timeUpdateCommonSignature(iter) = cputime - t;
+    t = cputime;
+
+    % Compute the likelihood
+    likelihood = computeLikelihood(memory(iter+1,:), memory(1:iter+1,:), dictionary);
+    %ignore the last (current signature)
+    likelihood = likelihood(1:end-1);
+    timeGetLikelihood(iter) = cputime - t;
+    t = cputime;
+    
+    % normalize the likelihood with std dev and mean
+    likelihoodNormalized = adjustLikelihood(likelihood);
+    timeAdjustLikelihood(iter) = cputime - t;
+    t = cputime;
+
+    % generate prediction
+    prediction = generatePrediction(predictionNP, predictionLC, length(likelihood)-1);
+    timeGeneratePrediction(iter) = cputime - t;
+    t = cputime;
+
+    % update the prior (recursive Bayes estimation equation)
+    if iter == 1
+        prior{iter} = 1; % 100% probability to be in a new place
+    else
+        prior{iter} = [prior{iter-1};0];
+    end
+    prior{iter} = likelihoodNormalized .* (prediction' * prior{iter});
+    prior{iter} = prior{iter}/sum(prior{iter}); %Normalize
+    timeUpdatePrior(iter) = cputime - t;
+    t = cputime;
+      
+    if plotPriorAfterEachIter || iter == nIter
+        figure(1)
+        hold off
+        x = -1;
+        if length(prior{iter}) > 1
+            x = [x 1:length(prior{iter})-1];
+        end
+        plot(x, prior{iter});
+        axis([x(1) x(end)+6 0 1])
+        hold on
+        plot(x, ones(size(x))*loopThreshold, 'r')
+        title(['Prior pdf (iteration ' num2str(iter) '/' num2str(nIter) ')'])
+        legend('pdf', 'loop closure threshold')
+    end
+
+    clc
+    disp(['(iteration ' num2str(iter) '/' num2str(nIter) ')'])
+    
+    if stepByStep ~= 0 && iter ~= nIter
+        disp('Press any key to continue...')
+        pause
+    end
+end
+
+figure(2)
+plot([timeUpdateDictionary timeUpdateCommonSignature timeGetLikelihood timeAdjustLikelihood timeGeneratePrediction timeUpdatePrior])
+title('Timings (seconds)')
+legend('timeUpdateDictionary', 'timeUpdateCommonSignature', 'timeGetLikelihood', 'timeAdjustLikelihood', 'timeGeneratePrediction', 'timeUpdatePrior')