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