fixed Matlab examples
							parent
							
								
									c00252a8f6
								
							
						
					
					
						commit
						fb7beb4494
					
				|  | @ -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 | ||||
|  | @ -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 | ||||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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; | ||||
|  | @ -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 | ||||
| 
 | ||||
|  | @ -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 | ||||
|      | ||||
| %----------------------------------------------------------------------- | ||||
| 
 | ||||
|  | @ -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 = [ | ||||
|  | @ -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); | ||||
| 
 | ||||
|  | @ -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; | ||||
|  | @ -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)]; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue