update Matlab scripts to work after changes in gtsam
parent
6252ff2497
commit
b43304aff8
|
@ -1,4 +1,4 @@
|
|||
function CEHCK(name,assertion)
|
||||
function CHECK(name,assertion)
|
||||
|
||||
if (assertion~=1)
|
||||
error(['CHECK ' name ' fails']);
|
||||
|
|
|
@ -6,36 +6,57 @@ c = createNoisyConfig();
|
|||
|
||||
% Create
|
||||
fg = GaussianFactorGraph;
|
||||
sigma1=.1;
|
||||
|
||||
% prior on x1
|
||||
A11=[10 0; 0 10];
|
||||
b = - c.get('x1')/0.1;
|
||||
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
|
||||
A21=[-10 0; 0 -10];
|
||||
A22=[ 10 0; 0 10];
|
||||
b = [2;-1];
|
||||
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
|
||||
A31=[-5 0; 0 -5];
|
||||
A32=[ 5 0; 0 5];
|
||||
b = [0;1];
|
||||
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
|
||||
A41=[-5 0; 0 -5];
|
||||
A42=[ 5 0; 0 5];
|
||||
b = [-1;1.5];
|
||||
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
|
|
@ -3,7 +3,7 @@ function c = createZeroDelta()
|
|||
v_x1 = [0; 0];
|
||||
v_x2 = [0; 0];
|
||||
v_l1 = [0; 0];
|
||||
c = FGConfig();
|
||||
c = VectorConfig();
|
||||
c.insert('x1', v_x1);
|
||||
c.insert('x2', v_x2);
|
||||
c.insert('l1', v_l1);
|
||||
|
|
|
@ -1,6 +1,8 @@
|
|||
% Christian Potthast
|
||||
% Create a map with random landmarks
|
||||
|
||||
function map = create_random_landmarks(numberOfLandmarks)
|
||||
function map = create_random_landmarks(numberOfLandmarks, mappingArea)
|
||||
|
||||
map = rand(2,numberOfLandmarks)*100;
|
||||
points = rand(2,numberOfLandmarks);
|
||||
|
||||
map=[points(1,:)*mappingArea(1);points(2,:)*mappingArea(2)];
|
||||
|
|
|
@ -3,21 +3,21 @@
|
|||
|
||||
clear;
|
||||
|
||||
n = 100;
|
||||
m = 20;
|
||||
n = 1000;
|
||||
m = 200;
|
||||
|
||||
% Set up the map
|
||||
map = create_random_landmarks(n);
|
||||
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 100 0 100]);axis square;
|
||||
axis([0 1000 0 1000]);axis square;
|
||||
|
||||
% Check visibility and plot this on the problem figure
|
||||
visibility = create_visibility(map, trajectory,10);
|
||||
visibility = create_visibility(map, trajectory,50);
|
||||
gplot(visibility,[map trajectory]');
|
||||
figure(2);clf;
|
||||
spy(visibility)
|
||||
|
@ -38,12 +38,20 @@ ord = create_ordering(n,m);
|
|||
|
||||
% show the matrix
|
||||
figure(3); clf;
|
||||
[A,b] = linearFactorGraph.matrix(ord);
|
||||
spy(A);
|
||||
|
||||
[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();
|
||||
|
@ -57,4 +65,4 @@ figure(5);clf;
|
|||
plot_config(optimal,n,m);hold on
|
||||
plot(trajectory(1,:),trajectory(2,:),'b+');
|
||||
plot(map(1,:), map(2,:),'g.');
|
||||
axis([0 100 0 100]);axis square;
|
||||
axis([0 1000 0 1000]);axis square;
|
||||
|
|
|
@ -8,13 +8,15 @@ 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(d, R, 'x1', A1, 'l1', A2);
|
||||
|
||||
cg = ConditionalGaussian('x',d, R, 'x1', A1, 'l1', A2, tau);
|
||||
|
||||
sx1 = [0.2;0.5];
|
||||
sl1 = [0.5;0.8];
|
||||
|
||||
solution = FGConfig;
|
||||
%solution = FGConfig;
|
||||
solution.insert('x1', sx1);
|
||||
solution.insert('l1', sl1);
|
||||
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
clear;
|
||||
|
||||
%Parameters
|
||||
noRuns=100;
|
||||
noRuns=5;
|
||||
steps=1;
|
||||
m = 5;
|
||||
velocity=1;
|
||||
|
@ -28,7 +28,7 @@ for steps=1:noRuns
|
|||
visibility = create_visibility(map, trajectory,visibilityTh);
|
||||
%gplot(visibility,[map trajectory]');
|
||||
|
||||
|
||||
steps
|
||||
% simulate the measurements
|
||||
measurement_sigma = 1;
|
||||
odo_sigma = 0.1;
|
||||
|
@ -46,9 +46,12 @@ for steps=1:noRuns
|
|||
|
||||
% show the matrix
|
||||
% figure(3); clf;
|
||||
[A_dense,b] = linearFactorGraph.matrix(ord);
|
||||
A=sparse(A_dense);
|
||||
size(A)
|
||||
%[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;
|
||||
|
|
|
@ -0,0 +1,112 @@
|
|||
% Set up a small SLAM example in MATLAB to test the execution time
|
||||
|
||||
clear;
|
||||
|
||||
%Parameters
|
||||
|
||||
noRuns=5;
|
||||
steps=50;
|
||||
m = 5*steps;
|
||||
velocity=1*steps;
|
||||
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,2);
|
||||
% Check visibility and plot this on the problem figure
|
||||
visibilityTh=visibilityTh+steps;
|
||||
visibility = create_visibility(map, trajectory,visibilityTh);
|
||||
gplot(visibility,[map trajectory]');
|
||||
|
||||
|
||||
% 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_good_ordering(n,m,measurements);
|
||||
ord = create_ordering(n,m);
|
||||
% show the matrix
|
||||
% figure(3); clf;
|
||||
%[A_dense,b] = linearFactorGraph.matrix(ord);
|
||||
%A=sparse(A_dense);
|
||||
|
||||
%sparse matrix !!!
|
||||
ijs = linearFactorGraph.sparse(ord);
|
||||
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
|
||||
%spy(A);
|
||||
%time qr
|
||||
runs=1;
|
||||
|
||||
ck_qr=cputime;
|
||||
for i=1:runs
|
||||
R_qr = qr(A);
|
||||
end
|
||||
time_qr=(cputime-ck_qr)/runs
|
||||
%time_qr=[time_qr,(cputime-ck)];
|
||||
|
||||
%figure(2)
|
||||
%clf
|
||||
%spy(R_qr);
|
||||
|
||||
% eliminate with that ordering
|
||||
%time gt_sam
|
||||
% for i=1:runs+10
|
||||
% if i==11
|
||||
% ck_gt=cputime;
|
||||
% end
|
||||
% BayesNet = linearFactorGraph.eliminate(ord);
|
||||
% end
|
||||
ck_gt=cputime;
|
||||
for i=1:runs+10
|
||||
BayesNet = linearFactorGraph.eliminate(ord);
|
||||
end
|
||||
time_gtsam=(cputime-ck_gt)/runs
|
||||
%time_gtsam=[time_gtsam,(cputime-ck)];
|
||||
|
||||
% clear trajectory visibility linearFactorGraph measurements odometry;
|
||||
% m = m+5;
|
||||
% velocity=velocity+1;
|
||||
|
||||
% end
|
||||
% %time_qr=time_qr/noRuns
|
||||
% 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;
|
Loading…
Reference in New Issue