69 lines
1.6 KiB
Matlab
69 lines
1.6 KiB
Matlab
% 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;
|