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