i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactoring of the cameraStereoImages and cameraRGBDImages classes (now inheriting from CameraImages) for easy setting of laser scan path, timestamps path and ground truth path.

Browse Source 
Added graph::importPoses().
Can now have a ground truth published with SensorData (filled optionally by CameraImages classes).
Increased database closing time performance when the database is not saved.
Added ParametersToolBox widget in DatabaseViewer for core parameters (refactoring done to make easy access to all rtabmap parameters in DatabaseViewer).
Added Parameters::getType(key).
Added Transform::interpolate() to interpolate between two transforms (SLERP)
Updated pf_filter.m and added test_pf_filter.m MATLAB scripts (making easier to compare with a ground truth)
UPlot: can now save all curve data of a figure in one action (see right-click on legend area->"Copy all curve data to clipboard")
This commit is contained in:
matlabbe
2015-12-17 14:13:19 -05:00
parent f5c062448d
commit 3292fb1146
49 changed files with 2135 additions and 2010 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Matlab/ParticleFilter/pf_filter.m
View File

@@ -18,7 +18,7 @@ for i = 1:length(x);
if sum(weights(:)) > 0
weights = weights ./sum(weights(:));
end
filtered(i) = weights'*particles;
particles = pf_resample(particles, weights);
end

18
Matlab/ParticleFilter/pf_filter.m~
View File

@@ -1,18 +0,0 @@
function filtered = pf_filter(x, nParticles, noise, lambda)
particles = zeros(nParticles,1) ;
weights = zeros(nParticles,1);
filtered=zeros(1,length(x));
for i = 1:length(x);
for j = 1:nParticles
rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++
bruit= noise*rn;
particles(j) = particles(j) + bruit ;
dist = abs(particles(j) - x(i));
weights(j) = exp(-lambda*dist);
end
weights = weights ./(sum(weights(:)));
filtered(i) = weights'*particles;
particles = Rresample2(particles,weights);
end

47
Matlab/ParticleFilter/test_pf_filter.m Normal file
View File

@@ -0,0 +1,47 @@
clc
%close all
particles = 300;
noiseXYZ = 0.02;
lambdaXYZ = 1;
noiseRPY = 0.005;
lambdaRPY = 250;
noise = [noiseXYZ noiseXYZ noiseXYZ noiseRPY noiseRPY noiseRPY];
lambda = [lambdaXYZ lambdaXYZ lambdaXYZ lambdaRPY lambdaRPY lambdaRPY];
axeNames = ['x' 'y' 'z' 'R' 'P' 'Y'];
% P = [13 x t] (id, incremental odom, incremental ground truth)
T = P(2:7,:);
G = P(8:end,:);
T(4:end,:) = T(4:end,:)*pi/180;
G(4:end,:) = G(4:end,:)*pi/180;
x_filtered = zeros(6, size(T, 2));
for i=1:6
x=T(i,:);
x=x';
x_filtered(i,:) = pf_filter([x(2:end); 0], particles, noise(i), lambda(i));
end
figure
for i=1:6
subplot(2,3,i)
x=T(i,:);
x_gt = G(i,:);
plot(1:length(x),cumsum(x),'b', 1:length(x),cumsum(x_filtered(i,:)),'r', 1:length(x),cumsum(x_gt),'g');
legend(axeNames(i), [axeNames(i) ' filtered'], [axeNames(i) ' gt'])
end
figure
for i=1:6
subplot(2,3,i)
x=T(i,:);
x_gt = G(i,:);
plot(1:length(x),x,'b', 1:length(x),x_filtered(i,:),'r', 1:length(x),x_gt,'g');
legend(axeNames(i), [axeNames(i) ' filtered'], [axeNames(i) ' gt'])
end