Moved matlab to experimental
							parent
							
								
									078f07650f
								
							
						
					
					
						commit
						e9804afc07
					
				|  | @ -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*-'); | ||||
|  | @ -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; | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 (j<n)&&(measurements{j}.i==pose) | ||||
|         ord.push_back(sprintf('l%d',j)); | ||||
|         j=j+1; | ||||
|     end | ||||
|     pose=pose+1; | ||||
| end | ||||
|  | @ -1,16 +0,0 @@ | |||
| % Christian Potthast | ||||
| % Create a map with random landmarks | ||||
| 
 | ||||
| function map = create_random_landmarks(visibilityTh, mappingArea, steps) | ||||
| map=[]; | ||||
| points1=1:visibilityTh:mappingArea(1); | ||||
| points2=1:visibilityTh:mappingArea(2); | ||||
|  for i=1:size(points1,2) | ||||
|   map=[map,[points1(1,i)-steps;points2(1,i)],[points1(1,i);points2(1,i)-steps]]; | ||||
|  end | ||||
| 
 | ||||
| % for i=1:size(points1,2) | ||||
| %     for j=1:size(points2,2) | ||||
| %     map=[map,[points1(1,i);points2(1,j)]]; | ||||
| %     end | ||||
| % end | ||||
|  | @ -1,14 +0,0 @@ | |||
| % Christian Potthast | ||||
| % create an elimination ordering | ||||
| 
 | ||||
| function ord = create_ordering(n,m) | ||||
| 
 | ||||
| ord = Ordering(); | ||||
| 
 | ||||
| for j = 1:n | ||||
|     ord.push_back(sprintf('l%d',j)); | ||||
| end | ||||
| 
 | ||||
| for i = 1:m | ||||
|     ord.push_back(sprintf('x%d',i));  | ||||
| end | ||||
|  | @ -1,8 +0,0 @@ | |||
| % Christian Potthast | ||||
| % Create a map with random landmarks | ||||
| 
 | ||||
| function map = create_random_landmarks(numberOfLandmarks, mappingArea) | ||||
| 
 | ||||
| points = rand(2,numberOfLandmarks); | ||||
| 
 | ||||
| map=[points(1,:)*mappingArea(1);points(2,:)*mappingArea(2)]; | ||||
|  | @ -1,20 +0,0 @@ | |||
| % Christian Potthast | ||||
| % create visibility matrix | ||||
| 
 | ||||
| function V = create_visibility(map,pose,threshold) | ||||
| 
 | ||||
| n = size(map,2); | ||||
| m = size(pose,2); | ||||
| V = sparse([],[],[],n+m,n+m); | ||||
| 
 | ||||
| for t = 1:m | ||||
|     % find measurements within Manhattan range | ||||
|     js = find(2==sum(abs(map-pose(:,t)*ones(1,n))<[threshold;threshold]*ones(1,n))); | ||||
|     for j = js | ||||
|         V(j,t+n)=1; | ||||
|     end | ||||
|     % add in odometry links | ||||
|     if t>1 | ||||
|        V((t+n)-1,t+n)=1;  | ||||
|     end | ||||
| end | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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 | ||||
|  | @ -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)); | ||||
|  | @ -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) | ||||
|  | @ -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 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -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); | ||||
| 
 | ||||
|  | @ -1,5 +0,0 @@ | |||
| function CHECK(name,assertion) | ||||
| 
 | ||||
| if (assertion~=1) | ||||
|   error(['CHECK ' name ' fails']); | ||||
| end | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
|  | @ -1,4 +0,0 @@ | |||
| % run all matlab unit tests | ||||
| testGaussianFactor | ||||
| testConditionalGaussian | ||||
| testGaussianFactorGraph | ||||
|  | @ -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)); | ||||
|  | @ -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); | ||||
| 
 | ||||
|  | @ -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'); | ||||
|  | @ -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'); | ||||
| 
 | ||||
|  | @ -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; | ||||
|  | @ -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 | ||||
		Loading…
	
		Reference in New Issue