Cleanup some code, frank05
							parent
							
								
									3cb76ad95d
								
							
						
					
					
						commit
						a4f73ee04f
					
				|  | @ -0,0 +1,17 @@ | |||
| function addSimulatedConstraint(points,angles,sd,id1,id2,graph) | ||||
| % addSimulatedConstraint: create a simulated measurement with noise | ||||
| % standard deviations sd and add it to graph | ||||
| 
 | ||||
| key1 = sprintf('x%d', id1); | ||||
| key2 = sprintf('x%d', id2); | ||||
| 
 | ||||
| % ground truth | ||||
| delta_x = points(id1,:) - points(id2,:); | ||||
| delta_angle = angles(id1) - angles(id2); | ||||
| noisy = Pose2(delta_x(1) + sd(1)*randn, delta_x(2) + sd(2)*randn, delta_angle + sd(3)*randn); | ||||
| 
 | ||||
| % create factor | ||||
| factor=Pose2Factor(key1,key2,noisy,diag(sd.*sd)); | ||||
| 
 | ||||
| % add it to the graph | ||||
| graph.push_back(factor); | ||||
										
											Binary file not shown.
										
									
								
							|  | @ -0,0 +1,44 @@ | |||
| % frank03: create Beijing matrices in a cleaner way | ||||
| 
 | ||||
| load beijing.mat; | ||||
| load beijing_angles.mat; | ||||
| load beijing_graph.mat; | ||||
| n=size(points,1); | ||||
| 
 | ||||
| % create config or load it from file | ||||
| if 0 | ||||
|     load beijing_config.mat; | ||||
| else | ||||
|     config=Pose2Config(); | ||||
|     for j=1:n | ||||
|         if mod(j,1000) == 0, fprintf(1, 'adding node %d to config\n', j); end    | ||||
|         pose=Pose2(points(j,1),points(j,2),angles(j)); | ||||
|         key = sprintf('x%d', j); | ||||
|         config.insert(key,pose); | ||||
|     end | ||||
|     save('beijing_config.mat','config'); | ||||
| end | ||||
| 
 | ||||
| sd = [0.25;0.25;0.01]; | ||||
| 
 | ||||
| % Build factor graph for entire graph | ||||
| graph = Pose2Graph; | ||||
| 
 | ||||
| % First add tree constraints | ||||
| [I J] = find(tree); | ||||
| for k=length(edge_order):-1:1 | ||||
|     edge = edge_order(k); | ||||
|     if mod(k,1000) == 0, fprintf(1, 'simulating constraint %d\n', k); end    | ||||
|     addSimulatedConstraint(points,angles,sd,I(edge),J(edge),graph); | ||||
| end | ||||
| 
 | ||||
| % Then add remaining constraints C | ||||
| C=G-tree; | ||||
| [I J] = find(C); | ||||
| for k=1:length(I) | ||||
|     if mod(k,100) == 0, fprintf(1, 'simulating constraint %d\n', k); end    | ||||
|     addSimulatedConstraint(points,angles,sd,I(k),J(k),graph); | ||||
| end | ||||
| 
 | ||||
| % generate ordering | ||||
| ordering = bottom_up_ordering(pred); | ||||
|  | @ -0,0 +1,34 @@ | |||
| % frank04: show matrices associated with Beijing simulation | ||||
| 
 | ||||
| load beijing.mat; | ||||
| load beijing_angles.mat; | ||||
| load beijing_graph.mat; | ||||
| 
 | ||||
| % put spanning tree 'tree' in correct order | ||||
| T = tree(node_order,node_order); | ||||
| 
 | ||||
| % get loop closing constraints | ||||
| C=G(node_order,node_order)-T; | ||||
| 
 | ||||
| close all | ||||
| 
 | ||||
| % plot on map | ||||
| figure | ||||
| gplot(C,points(node_order,:),'r') | ||||
| hold on | ||||
| gplot(T,points(node_order,:),'k') | ||||
| axis equal | ||||
| 
 | ||||
| % show spanning tree, original graph, and loop closures | ||||
| figure | ||||
| spy(T,'k'); | ||||
| hold on | ||||
| spy(C,'r'); | ||||
| 
 | ||||
| figure | ||||
| spy(T); | ||||
| 
 | ||||
| figure | ||||
| spy(C); | ||||
| 
 | ||||
| 
 | ||||
|  | @ -0,0 +1,33 @@ | |||
| % frank05: show right pre-conditioning | ||||
| % first run frank03 or other script to generate graph, config, and ordering | ||||
| 
 | ||||
| % linearize the non-linear factor graph | ||||
| tic | ||||
| LFG = graph.linearize_(config); | ||||
| toc | ||||
| tic | ||||
| ijs = LFG.sparse(ordering); | ||||
| A = sparse(ijs(1,:),ijs(2,:),ijs(3,:));  | ||||
| toc | ||||
| figure(1) | ||||
| spy(A); | ||||
| 
 | ||||
| % isolate the spanning tree part | ||||
| A1=A(1:3*nnz(tree),:); | ||||
| figure(2) | ||||
| spy(A1) | ||||
| 
 | ||||
| % calculate | ||||
| tic | ||||
| R1 = qr(A1,0); | ||||
| toc | ||||
| figure(3) | ||||
| spy(R1) | ||||
| 
 | ||||
| % calculate the entire R factor (expensive) | ||||
| tic | ||||
| R = qr(A,0); | ||||
| toc | ||||
| figure(4) | ||||
| spy(R) | ||||
| 
 | ||||
|  | @ -5,7 +5,6 @@ load beijing_config.mat; | |||
| 
 | ||||
| cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01]; | ||||
| 
 | ||||
| 
 | ||||
| factors = Pose2Graph; | ||||
| factors2 = Pose2Graph; | ||||
| ord2 = Ordering(); | ||||
|  | @ -36,7 +35,7 @@ for i=length(edge_order):-1:1 | |||
| end | ||||
| ord2.unique(); | ||||
| 
 | ||||
| if 0 | ||||
| if 1 | ||||
|     config=Pose2Config(); | ||||
|     n=size(points,1); | ||||
|     for j=1:n | ||||
|  | @ -70,12 +69,6 @@ R = qr(A,0); | |||
| figure(3) | ||||
| spy(R) | ||||
| 
 | ||||
| % plot on map | ||||
| figure(7) | ||||
| gplot(tree,points) | ||||
| %gplot(tree,points) | ||||
| axis equal | ||||
| 
 | ||||
| % show re-ordered R factor | ||||
| % P = colamd(A); | ||||
| % figure(4) | ||||
|  | @ -83,3 +76,4 @@ axis equal | |||
| % R = qr(A(:,P),0); | ||||
| % figure(5) | ||||
| % spy(R) | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue