12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed Matlab examples

release/4.3a0
Viorela Ila 2009-12-01 16:04:15 +00:00
parent c00252a8f6
commit fb7beb4494
13 changed files with 180 additions and 280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
matlab/createGaussianFactorGraph.m Normal file
View File

@ -0,0 +1,61 @@
% create a linear factor graph
% The non-linear graph above evaluated at NoisyConfig
function fg = createGaussianFactorGraph()
c = createNoisyConfig();
% Create
fg = GaussianFactorGraph;
sigma1=.1;
% prior on x1
A11=eye(2);
b = - c.get('x1');
f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry
fg.push_back(f1);
% odometry between x1 and x2
sigma2=.1;
A21=-eye(2);
A22=eye(2);
b = [.2;-.1];
f2 = GaussianFactor('x1', A21, 'x2', A22, b,sigma2);
fg.push_back(f2);
% measurement between x1 and l1
sigma3=.2;
A31=-eye(2);
A33=eye(2);
b = [0;.2];
f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3);
fg.push_back(f3);
% measurement between x2 and l1
sigma4=.2;
A42=-eye(2);
A43=eye(2);
b = [-.2;.3];
f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4);
fg.push_back(f4);
% Optimization
n=1;
m=2;
ord = create_ordering(n,m);
%BayesNet = GaussianFactorGraph.eliminate_(ord);
end

62
matlab/createLinearFactorGraph.m
View File

@ -1,62 +0,0 @@
% create a linear factor graph
% The non-linear graph above evaluated at NoisyConfig
function fg = createGaussianFactorGraph()
c = createNoisyConfig();
% Create
fg = GaussianFactorGraph;
sigma1=.1;
% prior on x1
A11=eye(2);
b = - c.get('x1');
<<<<<<< .mine
f1 = LinearFactor('x1', A11, b, sigma1);
=======
f1 = GaussianFactor('x1', A11, b);
>>>>>>> .r1017
fg.push_back(f1);
% odometry between x1 and x2
sigma2=.1;
<<<<<<< .mine
A21=-eye(2);
A22=eye(2);
b = [.2;-.1];
f2 = LinearFactor('x1', A21, 'x2', A22, b,sigma2);
=======
f2 = GaussianFactor('x1', A21, 'x2', A22, b);
>>>>>>> .r1017
fg.push_back(f2);
% measurement between x1 and l1
sigma3=.2;
A31=-eye(2);
A33=eye(2);
b = [0;.2];
<<<<<<< .mine
f3 = LinearFactor('x1', A31, 'l1', A33, b,sigma3);
=======
f3 = GaussianFactor('x1', A31, 'l1', A32, b);
>>>>>>> .r1017
fg.push_back(f3);
% measurement between x2 and l1
sigma4=.2;
A42=-eye(2);
A43=eye(2);
b = [-.2;.3];
<<<<<<< .mine
f4 = LinearFactor('x2', A42, 'l1', A43, b,sigma4);
=======
f4 = GaussianFactor('x2', A41, 'l1', A42, b);
>>>>>>> .r1017
fg.push_back(f4);
end

2
matlab/create_config.m
View File

@ -6,7 +6,7 @@ function config = create_config(n,m)
config = VectorConfig();
for j = 1:n
config.insert(sprintf('m%d',j), [0;0]);
config.insert(sprintf('l%d',j), [0;0]);
end
for i = 1:m

2
matlab/create_good_ordering.m
View File

@ -10,7 +10,7 @@ mes=size(measurements,2);
while (pose<=m)&&(j<=mes)
ord.push_back(sprintf('x%d',pose));
while (j<n)&&(measurements{j}.i==pose)
ord.push_back(sprintf('m%d',j));
ord.push_back(sprintf('l%d',j));
j=j+1;
end
pose=pose+1;

4
matlab/create_linear_factor_graph.m
View File

@ -15,7 +15,7 @@ lfg.push_back(lf);
% add prior for landmarks
for j = 1:n
key = sprintf('m%d',j);
key = sprintf('l%d',j);
prior = Point2Prior([0;0],1000,key);
lf = prior.linearize(config);
lfg.push_back(lf);
@ -25,7 +25,7 @@ end
for k = 1 : size(measurements,2)
measurement = measurements{k};
i = sprintf('x%d',measurement.i);
j = sprintf('m%d',measurement.j);
j = sprintf('l%d',measurement.j);
nlf = Simulated2DMeasurement(measurement.z, measurement_sigma, i, j);
lf = nlf.linearize(config);
lfg.push_back(lf);

2
matlab/create_ordering.m
View File

@ -6,7 +6,7 @@ function ord = create_ordering(n,m)
ord = Ordering();
for j = 1:n
ord.push_back(sprintf('m%d',j));
ord.push_back(sprintf('l%d',j));
end
for i = 1:m

68
matlab/example.m
View File

@ -1,68 +0,0 @@
% Set up a small SLAM example in MATLAB
% Authors: Christian Potthast, Frank Dellaert
clear;
n = 1000;
m = 200;
% Set up the map
map = create_random_landmarks(n,[1000,1000]);
figure(1);clf;
plot(map(1,:), map(2,:),'g.'); hold on;
% have the robot move in this world
trajectory = random_walk([0.1,0.1],5,m);
plot(trajectory(1,:),trajectory(2,:),'b+');
axis([0 1000 0 1000]);axis square;
% Check visibility and plot this on the problem figure
visibility = create_visibility(map, trajectory,50);
gplot(visibility,[map trajectory]');
figure(2);clf;
spy(visibility)
% simulate the measurements
measurement_sigma = 1;
odo_sigma = 0.1;
[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma);
% create a configuration of all zeroes
config = create_config(n,m);
% create the factor graph
linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
% create an ordering
ord = create_ordering(n,m);
% show the matrix
figure(3); clf;
[A_dense,b] = linearFactorGraph.matrix(ord);
%spy(A);
A=sparse(A_dense);
% eliminate with that ordering
ck = cputime;
BayesNet = linearFactorGraph.eliminate(ord);
time_gtsam = cputime - ck
ckqr = cputime;
R = qr(A);
time_qr = cputime - ckqr
%time_gtsam=[time_gtsam,(cputime-ck)]
% show the eliminated matrix
figure(4); clf;
[R,d] = BayesNet.matrix();
spy(R);
% optimize in the BayesNet
optimal = BayesNet.optimize;
% plot the solution
figure(5);clf;
plot_config(optimal,n,m);hold on
plot(trajectory(1,:),trajectory(2,:),'b+');
plot(map(1,:), map(2,:),'g.');
axis([0 1000 0 1000]);axis square;

19
matlab/plot_config.m
View File

@ -1,19 +0,0 @@
% Christian Potthast
% plot a configuration
function plot_config(config,n,m)
hold on;
for j = 1:n
key = sprintf('m%d',j);
mj = config.get(key);
plot(mj(1), mj(2),'r*');
end
for i = 1:m
key = sprintf('x%d',i);
xi = config.get(key);
plot(xi(1), xi(2),'rx');
end

28
matlab/testConditionalGaussian.m
View File

@ -1,28 +0,0 @@
%-----------------------------------------------------------------------
% solve
expected = [15.0471 ; -18.8824];
% create a conditional gaussion node
A1 =[1 2; 3 4];
A2 = [6 0.2;8 0.4];
R = [0.1 0.3; 0.0 0.34];
d=[0.2;0.5];
tau=[1;.34];
cg = ConditionalGaussian('x',d, R, 'x1', A1, 'l1', A2, tau);
sx1 = [0.2;0.5];
sl1 = [0.5;0.8];
%solution = FGConfig;
solution.insert('x1', sx1);
solution.insert('l1', sl1);
result = cg.solve(solution);
if(~all( abs(expected - result) < 0.0001 )) warning('solve failed'); end
%-----------------------------------------------------------------------

2
matlab/testLinearFactor.m → matlab/testGaussianFactor.m
View File

@ -48,7 +48,7 @@ S13 = [
+0.00,-8.94427
];
d=[2.23607;-1.56525];
expectedCG = ConditionalGaussian('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]');
% the expected linear factor
Bl1 = [

112
matlab/testGaussianFactorGraph.m Normal file
View File

@ -0,0 +1,112 @@
%-----------------------------------------------------------------------
% equals
fg = createGaussianFactorGraph();
fg2 = createGaussianFactorGraph();
CHECK('equals',fg.equals(fg,fg2));
%-----------------------------------------------------------------------
% error
cfg = createZeroDelta();
actual = fg.error(cfg);
DOUBLES_EQUAL( 5.625, actual, 1e-9 );
%-----------------------------------------------------------------------
% combine_factors_x1
fg = createGaussianFactorGraph();
%actual = fg.combine_factors('x1');
actual = fg.combined('x1');
Al1 = [
0., 0.
0., 0.
0., 0.
0., 0.
5., 0.
0., 5.
];
Ax1 = [
10., 0.
0.00, 10.
-10., 0.
0.00,-10.
-5., 0.
00., -5.
];
Ax2 = [
0., 0.
0., 0.
10., 0.
+0.,10.
0., 0.
0., 0.
];
b=[-1;-1;2;-1;0;1];
expected = GaussianFactor('l1',Al1,'x1',Ax1,'x2',Ax2,b);
CHECK('combine_factors_x1', actual.equals(expected,1e-9));
%-----------------------------------------------------------------------
% combine_factors_x2
fg = createGaussianFactorGraph();
actual = fg.combine_factors('x2');
%-----------------------------------------------------------------------
% eliminate_x1
fg = createGaussianFactorGraph();
actual = fg.eliminateOne('x1');
%-----------------------------------------------------------------------
% eliminate_x2
fg = createGaussianFactorGraph();
actual = fg.eliminateOne('x2');
%-----------------------------------------------------------------------
% eliminateAll
sigma1=.1;
R1 = eye(2);
d1=[-.1;-.1];
cg1 = ConditionalGaussian('x1',d1, R1,sigma1);
sigma2=0.149071;
R2 = eye(2);
A1= -eye(2);
d2=[0; .2];
cg2 = ConditionalGaussian('l1',d2, R2, 'x1', A1,sigma2);
sigma3=0.0894427;
R3 = eye(2);
A21 = [ -.2, 0.0
0.0, -.2];
A22 = [-.8, 0.0
0.0, -.8];
d3 =[.2; -.14];
cg3 = ConditionalGaussian('x2',d3, R3, 'l1', A21, 'x1', A22, sigma3);
expected = GaussianBayesNet;
expected.push_back(cg1);
expected.push_back(cg2);
expected.push_back(cg3);
expected.print_();
% Check one ordering
fg1 = createGaussianFactorGraph();
ord1 = Ordering;
ord1.push_back('x2');
ord1.push_back('l1');
ord1.push_back('x1');
actual1 = fg1.eliminate_(ord1);
actual1.print();
%CHECK('eliminateAll', actual1.equals(expected));
%-----------------------------------------------------------------------
% matrix
fg = createGaussianFactorGraph();
ord = Ordering;
ord.push_back('x2');
ord.push_back('l1');
ord.push_back('x1');
A = fg.matrix(ord);

96
matlab/test_time.m
View File

@ -1,96 +0,0 @@
% Set up a small SLAM example in MATLAB to test the execution time
clear;
%Parameters
noRuns=5;
steps=1;
m = 5;
velocity=1;
time_qr=[];
time_gtsam=[];
for steps=1:noRuns
%figure(1);clf;
% robot moves in the world
trajectory = walk([0.1,0.1],velocity,m);
mappingArea=max(trajectory,[],2);
%plot(trajectory(1,:),trajectory(2,:),'b+'); hold on;
visibilityTh=sqrt(mappingArea(1)^2+mappingArea(2)^2)/m; %distance between poses
% Set up the map
map = create_landmarks(visibilityTh, mappingArea,steps);
%plot(map(1,:), map(2,:),'g.');
%axis([0 mappingArea(1) 0 mappingArea(2)]); axis square;
n=size(map,1)*size(map,2);
% Check visibility and plot this on the problem figure
visibilityTh=visibilityTh+steps;
visibility = create_visibility(map, trajectory,visibilityTh);
%gplot(visibility,[map trajectory]');
steps
% simulate the measurements
measurement_sigma = 1;
odo_sigma = 0.1;
[measurements, odometry] = simulate_measurements(map, trajectory, visibility, measurement_sigma, odo_sigma);
% % create a configuration of all zeroes
config = create_config(n,m);
% create the factor graph
linearFactorGraph = create_linear_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n);
%
% create an ordering
ord = create_ordering(n,m);
% show the matrix
% figure(3); clf;
%[A_dense,b] = linearFactorGraph.matrix(ord);
%A=sparse(A_dense);
ijs = linearFactorGraph.sparse(ord);
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
%spy(A);
%time qr
ck=cputime;
R_qr = qr(A);
time_qr=[time_qr,(cputime-ck)];
%figure(2)
%clf
%spy(R_qr);
% eliminate with that ordering
%time gt_sam
ck=cputime;
BayesNet = linearFactorGraph.eliminate_(ord);
time_gtsam=[time_gtsam,(cputime-ck)];
clear trajectory visibility linearFactorGraph measurements odometry;
m = m+5;
velocity=velocity+1;
steps=steps+1;
end
plot(time_qr,'r');hold on;
plot(time_gtsam,'b');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % show the eliminated matrix
% figure(4); clf;
% [R,d] = BayesNet.matrix();
% spy(R);
%
% % optimize in the BayesNet
% optimal = BayesNet.optimize;
%
% % plot the solution
% figure(5);clf;
% plot_config(optimal,n,m);hold on
% plot(trajectory(1,:),trajectory(2,:),'b+');
% plot(map(1,:), map(2,:),'g.');
% axis([0 10 0 10]);axis square;

2
matlab/test_time_new.m
View File

@ -78,7 +78,7 @@ time_gtsam=[];
% end
ck_gt=cputime;
for i=1:runs+10
BayesNet = linearFactorGraph.eliminate(ord);
BayesNet = linearFactorGraph.eliminate_(ord);
end
time_gtsam=(cputime-ck_gt)/runs
%time_gtsam=[time_gtsam,(cputime-ck)];