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;
current_level = current_level + 1;
end
max_level = current_level - 1;
[~, node_order] = sort(node_levels, 'descend'); % the order of the nodes in leaves-to-root order
ordering = Ordering();
for level = max_level:-1:1
ids = find(node_levels==level);
for i = 1:length(ids)
ordering.push_back(sprintf('x%d', ids(i)));
end
for i = 1:length(node_order)
ordering.push_back(sprintf('x%d', node_order(i)));
end
end

64
matlab/kai01_BejingPose2Graph.m
View File

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