12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

order w.r.t the edges

release/4.3a0
Kai Ni 2009-12-13 01:24:09 +00:00
parent c2ddf330e7
commit fe2dfe81d4
3 changed files with 34 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
matlab/beijing_graph.mat
View File

Binary file not shown.

9
matlab/bottom_up_ordering.m
View File

@ -10,13 +10,10 @@ while ~isempty(parents)
node_levels(parents) = current_level; node_levels(parents) = current_level;
current_level = current_level + 1; current_level = current_level + 1;
end end
max_level = current_level - 1; [~, node_order] = sort(node_levels, 'descend'); % the order of the nodes in leaves-to-root order
ordering = Ordering(); ordering = Ordering();
for level = max_level:-1:1 for i = 1:length(node_order)
ids = find(node_levels==level); ordering.push_back(sprintf('x%d', node_order(i)));
for i = 1:length(ids)
ordering.push_back(sprintf('x%d', ids(i)));
end
end end
end end

64
matlab/kai01_BejingPose2Graph.m
View File

@ -10,30 +10,28 @@ factors = Pose2Graph;
factors2 = Pose2Graph; factors2 = Pose2Graph;
ord2 = Ordering(); ord2 = Ordering();
[~, idx] = sort(way_levels, 'descend'); [rows cols] = find(tree);
for i=1:length(edge_order)
for i=1:length(idx)
if mod(i,500) == 0 if mod(i,500) == 0
fprintf(1, 'processing way %d\n', i); fprintf(1, 'processing edge %d\n', i);
end end
for j=1:length(ways{idx(i)})-1
id1 = ways{idx(i)}(j); id1 = rows(edge_order(i));
id2 = ways{idx(i)}(j+1); id2 = cols(edge_order(i));
key1 = sprintf('x%d', id1); key1 = sprintf('x%d', id1);
key2 = sprintf('x%d', id2); key2 = sprintf('x%d', id2);
delta_x = points(id1,:) - points(id2,:); delta_x = points(id1,:) - points(id2,:);
delta_angle = angles(id1) - angles(id2); delta_angle = angles(id1) - angles(id2);
measured = Pose2(delta_x(1), delta_x(2), delta_angle); measured = Pose2(delta_x(1), delta_x(2), delta_angle);
if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree
factor=Pose2Factor(key1,key2,measured, cov); factor=Pose2Factor(key1,key2,measured, cov);
factors.push_back(factor); factors.push_back(factor);
else %% not in the spanning tree else %% not in the spanning tree
factors2.push_back(Pose2Factor(key1,key2,measured, cov)); factors2.push_back(Pose2Factor(key1,key2,measured, cov));
ord2.push_back(key1); ord2.push_back(key1);
ord2.push_back(key2); ord2.push_back(key2);
end
end end
end end
ord2.unique(); ord2.unique();
@ -61,11 +59,11 @@ figure(1)
spy(A); spy(A);
%save('beijing_factors.mat', 'factors'); %save('beijing_factors.mat', 'factors');
LFG2=factors2.linearize_(config); % LFG2=factors2.linearize_(config);
ijs2=LFG2.sparse(ord2); % ijs2=LFG2.sparse(ord2);
A2=sparse(ijs2(1,:),ijs2(2,:),ijs2(3,:)); % A2=sparse(ijs2(1,:),ijs2(2,:),ijs2(3,:));
figure(2) % figure(2)
spy(A2); % spy(A2);
% show R factor % show R factor
R = qr(A,0); R = qr(A,0);
@ -73,9 +71,9 @@ figure(3)
spy(R) spy(R)
% show re-ordered R factor % show re-ordered R factor
P = colamd(A); % P = colamd(A);
figure(4) % figure(4)
spy(A(:,P)) % spy(A(:,P))
R = qr(A(:,P),0); % R = qr(A(:,P),0);
figure(5) % figure(5)
spy(R) % spy(R)