From e9804afc07db5c936d0e01cb794c33dd97168f68 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 29 Oct 2010 14:53:49 +0000 Subject: [PATCH] Moved matlab to experimental --- matlab/example/alex01_simple1.m | 85 ------------- matlab/example/christian01.m | 59 --------- matlab/example/create_config.m | 15 --- matlab/example/create_gaussian_factor_graph.m | 46 ------- matlab/example/create_good_ordering.m | 17 --- matlab/example/create_landmarks.m | 16 --- matlab/example/create_ordering.m | 14 --- matlab/example/create_random_landmarks.m | 8 -- matlab/example/create_visibility.m | 20 ---- matlab/example/plot_config.m | 19 --- matlab/example/random_walk.m | 20 ---- matlab/example/simulate_measurements.m | 23 ---- matlab/example/solveCQP.m | 16 --- matlab/example/testCQP.m | 16 --- matlab/example/walk.m | 20 ---- matlab/startup.m | 14 --- matlab/tests/CHECK.m | 5 - matlab/tests/DOUBLES_EQUAL.m | 5 - matlab/tests/createGaussianFactorGraph.m | 30 ----- matlab/tests/createNoisyConfig.m | 10 -- matlab/tests/createZeroDelta.m | 10 -- matlab/tests/run_tests.m | 4 - matlab/tests/testGaussianFactor.m | 70 ----------- matlab/tests/testGaussianFactorGraph.m | 62 ---------- matlab/tests/testPose2Factor.m | 26 ---- matlab/timing/test_time_average.m | 76 ------------ matlab/timing/test_time_new.m | 112 ------------------ matlab/timing/time_spqr.m | 23 ---- 28 files changed, 841 deletions(-) delete mode 100644 matlab/example/alex01_simple1.m delete mode 100644 matlab/example/christian01.m delete mode 100644 matlab/example/create_config.m delete mode 100644 matlab/example/create_gaussian_factor_graph.m delete mode 100644 matlab/example/create_good_ordering.m delete mode 100644 matlab/example/create_landmarks.m delete mode 100644 matlab/example/create_ordering.m delete mode 100644 matlab/example/create_random_landmarks.m delete mode 100644 matlab/example/create_visibility.m delete mode 100644 matlab/example/plot_config.m delete mode 100644 matlab/example/random_walk.m delete mode 100644 matlab/example/simulate_measurements.m delete mode 100644 matlab/example/solveCQP.m delete mode 100644 matlab/example/testCQP.m delete mode 100644 matlab/example/walk.m delete mode 100644 matlab/startup.m delete mode 100644 matlab/tests/CHECK.m delete mode 100644 matlab/tests/DOUBLES_EQUAL.m delete mode 100644 matlab/tests/createGaussianFactorGraph.m delete mode 100644 matlab/tests/createNoisyConfig.m delete mode 100644 matlab/tests/createZeroDelta.m delete mode 100644 matlab/tests/run_tests.m delete mode 100644 matlab/tests/testGaussianFactor.m delete mode 100644 matlab/tests/testGaussianFactorGraph.m delete mode 100644 matlab/tests/testPose2Factor.m delete mode 100644 matlab/timing/test_time_average.m delete mode 100644 matlab/timing/test_time_new.m delete mode 100644 matlab/timing/time_spqr.m diff --git a/matlab/example/alex01_simple1.m b/matlab/example/alex01_simple1.m deleted file mode 100644 index 8f3eb5706..000000000 --- a/matlab/example/alex01_simple1.m +++ /dev/null @@ -1,85 +0,0 @@ -% Script to perform SQP on a simple example from the SQP tutorial -% This makes use of LDL solving of a full quadratic programming -% problem -% -% Problem: -% min(x) f(x) = (x2-2)^2 - x1^2 -% s.t. c(x) = 4x1^2 + x2^2 - 1 =0 -% state is x = [x1 x2]' , with dim(state) = 2 -% constraint has dim p = 1 - -n = 2; -p = 1; - -% initial conditions -x0 = [2; 4]; -lam0 = -0.5; -x = x0; lam = lam0; - -maxIt = 100; - -X = x0; Lam = lam0; -Bi = eye(2); - -for i=1:maxIt - i - x1 = x(1); x2 = x(2); - - % evaluate functions - fx = (x2-2)^2 + x1^2; - cx = 4*x1^2 + x2^2 - 1; - - % evaluate derivatives in x - dfx = [2*x1; 2*(x2-2)]; - dcx = [8*x1; 2*x2]; - dL = dfx - lam * dcx; - - % update the hessian (BFGS) - note: this does not use the full lagrangian - if (i>1) - Bis = Bi*s; - y = dfx - prev_dfx; - Bi = Bi + (y*y')/(y'*s) - (Bis*Bis')/(s'*Bis); - end - prev_dfx = dfx; - - % evaluate hessians in x - ddfx = diag([2, 2]); - ddcx = diag([8, 2]); - - % construct and solve CQP subproblem - Bgn0 = dfx * dfx'; - Bgn1 = dfx * dfx' - lam * dcx * dcx'; % GN approx 1 - Bgn2 = dL * dL'; % GN approx 2 - Ba = ddfx - lam * ddcx; % analytic hessians - - B = Bi; - g = dfx; - h = -cx; - [delta lambda] = solveCQP(B, -dcx, -dcx', g, h); - - % update - s = 0.1*delta; - x = x + s - lam = lambda - - - % save - X = [X x]; - Lam = [Lam lam]; - -end - -% verify -xstar = [0; 1]; -lamstar = -1; -display(fx) -display(cx) -final_error = norm(x-xstar) + norm(lam-lamstar) - -% draw -figure(1); -clf; -ezcontour('(x2-2)^2 + x1^2'); -hold on; -ezplot('4*x1^2 + x2^2 - 1'); -plot(X(1,:), X(2,:), 'r*-'); diff --git a/matlab/example/christian01.m b/matlab/example/christian01.m deleted file mode 100644 index 777a621c6..000000000 --- a/matlab/example/christian01.m +++ /dev/null @@ -1,59 +0,0 @@ -% Set up a small SLAM example -% Christian Potthast, Frank Dellaert - -clear; -close all; - -n = 100; -m = 20; - -% 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; - -% Set up the map -mappingArea=max(trajectory,[],2); -map = create_random_landmarks(n, mappingArea); -figure(1);clf; -plot(map(1,:), map(2,:),'g.'); hold on; -axis([0 mappingArea(1) 0 mappingArea(2)]);axis square; - -% Check visibility and plot this on the problem figure -visibility = create_visibility(map, trajectory,20); -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 -factorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n); - -% create an ordering -ord = create_ordering(n,m); - -% show the matrix -figure(2); clf; -ijs = factorGraph.sparse(ord); -A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); -spy(A); - -% optimizing a BayesNet is not possible from MATLAB as -% GaussianBayesNet is a typedef not a real class :-( -% BayesNet = factorGraph.eliminate_(ord); -% optimal = BayesNet.optimize; - -% However, we can call the optimize_ method of a GaussianFactorGraph -optimal = factorGraph.optimize_(ord); - -% plot the solution -figure(3);clf; -plot_config(optimal,n,m);hold on -plot(trajectory(1,:),trajectory(2,:),'b+'); -plot(map(1,:), map(2,:),'g.'); -axis([0 mappingArea(1) 0 mappingArea(2)]);axis square; diff --git a/matlab/example/create_config.m b/matlab/example/create_config.m deleted file mode 100644 index 9340a1c57..000000000 --- a/matlab/example/create_config.m +++ /dev/null @@ -1,15 +0,0 @@ -% Christian Potthast -% create a configuration - -function config = create_config(n,m) - -config = Simulated2DConfig(); -origin = Point2; - -for i = 1:m - config.insertPose(i, origin); -end - -for j = 1:n - config.insertPoint(j, origin); -end \ No newline at end of file diff --git a/matlab/example/create_gaussian_factor_graph.m b/matlab/example/create_gaussian_factor_graph.m deleted file mode 100644 index d4bc146d2..000000000 --- a/matlab/example/create_gaussian_factor_graph.m +++ /dev/null @@ -1,46 +0,0 @@ -% Christan Potthast -% create linear factor graph - -function lfg = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n) - -m = size(measurements,2); - -% create linear factor graph -lfg = GaussianFactorGraph(); - -% Point2 at origin -origin = Point2; - -% create prior for initial robot pose -model0_2 = SharedDiagonal([0.2;0.2]); -prior = Simulated2DPosePrior(origin,model0_2,1); -lf = prior.linearize(config); -lfg.push_back(lf); - -% add prior for landmarks -model1000 = SharedDiagonal([1000;1000]); -for j = 1:n - prior = Simulated2DPointPrior(origin,model1000,j); - lf = prior.linearize(config); - lfg.push_back(lf); -end - -% add odometry factors -odo_model = SharedDiagonal([odo_sigma;odo_sigma]); -for i = 2 : size(odometry,2) - odo = Point2(odometry{i}(1),odometry{i}(2)); - nlf = Simulated2DOdometry(odo, odo_model, i-1, i); - lf = nlf.linearize(config); - lfg.push_back(lf); -end - -% add measurement factors -measurement_model = SharedDiagonal([measurement_sigma;measurement_sigma]); -for k = 1 : size(measurements,2) - measurement = measurements{k}; - point = Point2(measurement.z(1),measurement.z(2)); - nlf = Simulated2DMeasurement(point, measurement_model, measurement.i, measurement.j); - lf = nlf.linearize(config); - lfg.push_back(lf); -end - diff --git a/matlab/example/create_good_ordering.m b/matlab/example/create_good_ordering.m deleted file mode 100644 index 36275f80e..000000000 --- a/matlab/example/create_good_ordering.m +++ /dev/null @@ -1,17 +0,0 @@ -% Christian Potthast -% create an elimination ordering - -function ord = create_good_ordering(n,m,measurements) - -ord = Ordering(); -j=1; -pose=1; -mes=size(measurements,2); -while (pose<=m)&&(j<=mes) - ord.push_back(sprintf('x%d',pose)); - while (j1 - V((t+n)-1,t+n)=1; - end -end \ No newline at end of file diff --git a/matlab/example/plot_config.m b/matlab/example/plot_config.m deleted file mode 100644 index 0554b9b06..000000000 --- a/matlab/example/plot_config.m +++ /dev/null @@ -1,19 +0,0 @@ -% Christian Potthast -% plot a configuration - -function plot_config(config,n,m) - -hold on; - -for j = 1:n - key = sprintf('l%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 - diff --git a/matlab/example/random_walk.m b/matlab/example/random_walk.m deleted file mode 100644 index a72a567e0..000000000 --- a/matlab/example/random_walk.m +++ /dev/null @@ -1,20 +0,0 @@ -% Christian Potthast -% random walk from a robot - -function pose = random_walk(initial, velocity, steps) - -pose(:,1) = initial; -bearing = 1; - - -for step = 2:steps - - bearing = bearing + 0.05*randn(); - - pose(1,step) = pose(1,step-1) + sin(bearing) * velocity; - pose(2,step) = pose(2,step-1) + cos(bearing) * velocity; - -end - - - diff --git a/matlab/example/simulate_measurements.m b/matlab/example/simulate_measurements.m deleted file mode 100644 index 31ef3555c..000000000 --- a/matlab/example/simulate_measurements.m +++ /dev/null @@ -1,23 +0,0 @@ -% Christian Potthast -% simulate measurements - -function [measurements,odometry] = simulate_measurements(map, pose, visibility, noise_sigma, odo_sigma) -m = size(pose,2); -n = size(map,2); -measurements = {}; -odometry = {}; -k =1; -for i = 1:m - js = find(visibility(1:n,i+n)); - if size(js ,1) > 0 - for j = js' - z = map(:,j)-pose(:,i)+randn(2,1)*noise_sigma; - measurement = struct('z',z,'i',i,'j',j); - measurements{k}=measurement; - k = k+1; - end - end - if i>1 - odometry{i}= pose(:,i)-pose(:,i-1)+randn(2,1)*odo_sigma; - end -end \ No newline at end of file diff --git a/matlab/example/solveCQP.m b/matlab/example/solveCQP.m deleted file mode 100644 index 2580f3854..000000000 --- a/matlab/example/solveCQP.m +++ /dev/null @@ -1,16 +0,0 @@ -function [delta lambda] = solveCQP(B, A, At, g, h) - -n = size(B,1); -p = size(A,2); - -% form the KKT matrix system -G = [B A; At zeros(p,p)]; -rhs = -[g; h]; - -% solve with LDL -[L D] = ldl(G); -approx_error = norm(G - L*D*L'); %% verify error -sol = L'\(D\(L\rhs)); - -delta = sol(1:n); -lambda = sol(n+1:size(sol)); \ No newline at end of file diff --git a/matlab/example/testCQP.m b/matlab/example/testCQP.m deleted file mode 100644 index 5e375bc70..000000000 --- a/matlab/example/testCQP.m +++ /dev/null @@ -1,16 +0,0 @@ -% Test example for simple Constrained Quadratic Programming problem - -A = [-1 -1; - -2 1; - 1 -1]; -At = A'; -B = 2*eye(3,3); - -b = [4; -2]; -g = zeros(3,1); - -[delta lambda] = solveCQP(B, A, At, g, b); - -expX = [2/7 10/7 -6/7]'; - -state_error = norm(expX-delta) \ No newline at end of file diff --git a/matlab/example/walk.m b/matlab/example/walk.m deleted file mode 100644 index 0fab6f3a3..000000000 --- a/matlab/example/walk.m +++ /dev/null @@ -1,20 +0,0 @@ -% Christian Potthast -% random walk from a robot - -function pose = walk(initial, velocity, steps) - -pose(:,1) = initial; -bearing = 0.7854; % 45? - - -for step = 2:steps - - %bearing = bearing + 0.05; - - pose(1,step) = pose(1,step-1) + sin(bearing) * velocity; - pose(2,step) = pose(2,step-1) + cos(bearing) * velocity; - -end - - - diff --git a/matlab/startup.m b/matlab/startup.m deleted file mode 100644 index c3dab1fb2..000000000 --- a/matlab/startup.m +++ /dev/null @@ -1,14 +0,0 @@ -% Matlab code to test gtsam toolbox -% -% -% -% -clc; -disp(' '); -disp(' Matlab scripts to test GTSAM toolbox '); -disp(' '); - -path(path,genpath(pwd)); -toolboxpath = '~/toolbox/gtsam' -addpath(toolboxpath); - diff --git a/matlab/tests/CHECK.m b/matlab/tests/CHECK.m deleted file mode 100644 index feb21b896..000000000 --- a/matlab/tests/CHECK.m +++ /dev/null @@ -1,5 +0,0 @@ -function CHECK(name,assertion) - -if (assertion~=1) - error(['CHECK ' name ' fails']); -end diff --git a/matlab/tests/DOUBLES_EQUAL.m b/matlab/tests/DOUBLES_EQUAL.m deleted file mode 100644 index 35b0d68fa..000000000 --- a/matlab/tests/DOUBLES_EQUAL.m +++ /dev/null @@ -1,5 +0,0 @@ -function DOUBLES_EQUAL(expected,actual,atol) - -if abs(expected-actual)>atol - error(sprintf('DOUBLES_EQUAL fails: expected %g but got %g',expected,actual)); -end diff --git a/matlab/tests/createGaussianFactorGraph.m b/matlab/tests/createGaussianFactorGraph.m deleted file mode 100644 index 5adf6cf46..000000000 --- a/matlab/tests/createGaussianFactorGraph.m +++ /dev/null @@ -1,30 +0,0 @@ -% create a linear factor graph -% The non-linear graph above evaluated at NoisyConfig -function fg = createGaussianFactorGraph() - -c = createNoisyConfig(); - -% Create -fg = GaussianFactorGraph; - -% Create shared Noise model -unit2 = SharedDiagonal([1;1]); - -% prior on x1 -I=eye(2); -f1 = GaussianFactor('x1', 10*I, [-1;-1], unit2); -fg.push_back(f1); - -% odometry between x1 and x2 -f2 = GaussianFactor('x1', -10*I, 'x2', 10*I, [2;-1], unit2); -fg.push_back(f2); - -% measurement between x1 and l1 -f3 = GaussianFactor('x1', -5*I, 'l1', 5*I, [0;1], unit2); -fg.push_back(f3); - -% measurement between x2 and l1 -f4 = GaussianFactor('x2', -5*I, 'l1', 5*I, [-1;1.5], unit2); -fg.push_back(f4); - -end \ No newline at end of file diff --git a/matlab/tests/createNoisyConfig.m b/matlab/tests/createNoisyConfig.m deleted file mode 100644 index 8da64f6da..000000000 --- a/matlab/tests/createNoisyConfig.m +++ /dev/null @@ -1,10 +0,0 @@ -% A noise configuration -function c = createNoisyConfig() -v_x1 = [0.1; 0.1]; -v_x2 = [1.4; 0.2]; -v_l1 = [0.1; -1.1]; -c = VectorConfig(); -c.insert('x1', v_x1); -c.insert('x2', v_x2); -c.insert('l1', v_l1); -end \ No newline at end of file diff --git a/matlab/tests/createZeroDelta.m b/matlab/tests/createZeroDelta.m deleted file mode 100644 index 4a03de54d..000000000 --- a/matlab/tests/createZeroDelta.m +++ /dev/null @@ -1,10 +0,0 @@ -% A zero delta configuration -function c = createZeroDelta() -v_x1 = [0; 0]; -v_x2 = [0; 0]; -v_l1 = [0; 0]; -c = VectorValues(); -c.insert('x1', v_x1); -c.insert('x2', v_x2); -c.insert('l1', v_l1); -end \ No newline at end of file diff --git a/matlab/tests/run_tests.m b/matlab/tests/run_tests.m deleted file mode 100644 index b1d5ec2f2..000000000 --- a/matlab/tests/run_tests.m +++ /dev/null @@ -1,4 +0,0 @@ -% run all matlab unit tests -testGaussianFactor -testConditionalGaussian -testGaussianFactorGraph diff --git a/matlab/tests/testGaussianFactor.m b/matlab/tests/testGaussianFactor.m deleted file mode 100644 index 0fc3f0a27..000000000 --- a/matlab/tests/testGaussianFactor.m +++ /dev/null @@ -1,70 +0,0 @@ -%----------------------------------------------------------------------- -% eliminate - -% the combined linear factor -Ax2 = [ --5., 0. -+0.,-5. -10., 0. -+0.,10. -]; - -Al1 = [ -5., 0. -0., 5. -0., 0. -0., 0. -]; - -Ax1 = [ -0.00, 0. % f4 -0.00, 0. % f4 --10., 0. % f2 -0.00,-10. % f2 -]; - -% the RHS -b2=[-1;1.5;2;-1]; -model4 = SharedDiagonal([1;1;1;1]); -combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, model4); - -% eliminate the combined factor -[actualCG,actualLF] = combined.eliminate('x2'); - -% create expected Conditional Gaussian -R11 = [ -11.1803, 0.00 -0.00, 11.1803 -]; -S12 = [ --2.23607, 0.00 -+0.00,-2.23607 -]; -S13 = [ --8.94427, 0.00 -+0.00,-8.94427 -]; -d=[2.23607;-1.56525]; -expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1;1]); - -% the expected linear factor -Bl1 = [ -4.47214, 0.00 -0.00, 4.47214 -]; - -Bx1 = [ -% x1 --4.47214, 0.00 -+0.00, -4.47214 -]; - -% the RHS -b1= [0.0;0.894427]; - -model2 = SharedDiagonal([1;1]); -expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, model2); - -% check if the result matches -CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4)); -CHECK('actualLF.equals(expectedLF,1e-5)',actualLF.equals(expectedLF,1e-4)); diff --git a/matlab/tests/testGaussianFactorGraph.m b/matlab/tests/testGaussianFactorGraph.m deleted file mode 100644 index 6769dc599..000000000 --- a/matlab/tests/testGaussianFactorGraph.m +++ /dev/null @@ -1,62 +0,0 @@ -%----------------------------------------------------------------------- -% equals -fg = createGaussianFactorGraph(); -fg2 = createGaussianFactorGraph(); -CHECK('equals',fg.equals(fg2,1e-9)); - -%----------------------------------------------------------------------- -% error -zero = createZeroDelta(); -actual = fg.error(zero); -DOUBLES_EQUAL( 5.625, actual, 1e-9 ); - -%----------------------------------------------------------------------- -% eliminate_x1 -fg = createGaussianFactorGraph(); -actual = fg.eliminateOne('x1'); - -%----------------------------------------------------------------------- -% eliminate_x2 -fg = createGaussianFactorGraph(); -actual = fg.eliminateOne('x2'); - -%----------------------------------------------------------------------- -% eliminateAll -I = eye(2); -cg1 = GaussianConditional('x1',[-1;-1], 10*I,[1;1]); - -sig1=0.149071; -d2=[0; .2]/sig1; -cg2 = GaussianConditional('l1', d2, I/sig1, 'x1', -I/sig1, [1;1]); - -sig2 = 0.0894427; -A21 = -0.2*I/sig2; -A22 = -0.8*I/sig2; -d3 =[.2; -.14]/sig2; -cg3 = GaussianConditional('x2',d3, I/sig2, 'l1', A21, 'x1', A22, [1;1]); - -expected = GaussianBayesNet; -expected.push_back(cg3); -expected.push_back(cg2); -expected.push_back(cg1); - -% Check one ordering -fg1 = createGaussianFactorGraph(); -ord1 = Ordering; -ord1.push_back('x2'); -ord1.push_back('l1'); -ord1.push_back('x1'); -actual1 = fg1.eliminate_(ord1); -CHECK('eliminateAll', actual1.equals(expected,1e-5)); - -%----------------------------------------------------------------------- -% matrix - -fg = createGaussianFactorGraph(); -ord = Ordering; -ord.push_back('x1'); -ord.push_back('x2'); -ord.push_back('l1'); - -[H,z] = fg.matrix(ord); - diff --git a/matlab/tests/testPose2Factor.m b/matlab/tests/testPose2Factor.m deleted file mode 100644 index b0c45b24b..000000000 --- a/matlab/tests/testPose2Factor.m +++ /dev/null @@ -1,26 +0,0 @@ -% test linearize Pose2Factor -cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01]; -key1='x1'; -key2='x2'; -measured=Pose2(2,2, pi/2); -measured.print('Pose'); -factor=Pose2Factor(key1,key2,measured, cov); -factor.print('Factor'); - - -% test linearize Pose2Graph - -p1=Pose2(1.1,2,pi/2); % robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) -p2=Pose2(-1,4.1,pi); % robot at (-1,4) looking at negative (ground truth is at 4.1,2) -config= Pose2Config() ; -config.insert('x1',p1); -config.insert('x2',p2); - -lf = factor.linearize(config); -lf.print('lf '); - -factors = Pose2Graph; -factors.push_back(factor); - -lfg=factors.linearize_(config); -lfg.print('lfg'); \ No newline at end of file diff --git a/matlab/timing/test_time_average.m b/matlab/timing/test_time_average.m deleted file mode 100644 index 990fa849e..000000000 --- a/matlab/timing/test_time_average.m +++ /dev/null @@ -1,76 +0,0 @@ -% Set up a small SLAM example in MATLAB to test the execution time - -clear; -clf; -%Parameters -noRuns=100; -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 - gaussianFactorGraph = create_gaussian_factor_graph(config, measurements, odometry, measurement_sigma, odo_sigma, n); - % - % create an ordering - ord = create_ordering(n,m); - - ijs = gaussianFactorGraph.sparse(ord); - A=sparse(ijs(1,:),ijs(2,:),ijs(3,:)); - - - runs=50; % for each graph run QR and elimination several times and average the time - - ck_qr=cputime; - for it=1:runs - R_qr = qr(A); - end - time_qr=[time_qr,(cputime-ck_qr)/runs]; - - % eliminate with that ordering - %time gt_sam - for it=1:runs+1 - if it==2 - ck_gt=cputime; - end - BayesNet = gaussianFactorGraph.eliminate_(ord); - end - time_gtsam=[time_gtsam,(cputime-ck_gt)/runs]; - - clear trajectory visibility gaussianFactorGraph measurements odometry; - m = m+5; - velocity=velocity+1; - steps=steps+1; -end -plot(time_qr,'r');hold on; -plot(time_gtsam,'b'); - diff --git a/matlab/timing/test_time_new.m b/matlab/timing/test_time_new.m deleted file mode 100644 index 835ced70b..000000000 --- a/matlab/timing/test_time_new.m +++ /dev/null @@ -1,112 +0,0 @@ -% 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 - gaussianFactorGraph = 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] = gaussianFactorGraph.matrix(ord); - %A=sparse(A_dense); - - %sparse matrix !!! - ijs = gaussianFactorGraph.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 = gaussianFactorGraph.eliminate(ord); -% end -ck_gt=cputime; -for i=1:runs+10 - BayesNet = gaussianFactorGraph.eliminate_(ord); -end - time_gtsam=(cputime-ck_gt)/runs - %time_gtsam=[time_gtsam,(cputime-ck)]; - -% clear trajectory visibility gaussianFactorGraph 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; diff --git a/matlab/timing/time_spqr.m b/matlab/timing/time_spqr.m deleted file mode 100644 index 2a30b9998..000000000 --- a/matlab/timing/time_spqr.m +++ /dev/null @@ -1,23 +0,0 @@ -dataSet = 'intel'; -saveFiles = 0; -maxID = 0; -dim = 3; - -[vertices,edges]=loadDataSet(dataSet,saveFiles); -[vert, ed]=cut2maxID(maxID, vertices, edges); - -p0 = vert(1,2:end)'; -s0 = diag([ed(1,6),ed(1,8),ed(1,9)]); - -fprintf('Initial config...'); -config=InitialConfig(ed,dim,p0); - -fprintf('graph2FactorGraph...'); -[A,b]=graph2FactorGraph(config,ed,dim,s0); - -tic -for trial=1:100 - fprintf('Trial %d\n', trial); - x = spqr_solve(A,b); -end -toc