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

use Kai's ordering

release/4.3a0
Frank Dellaert 2009-12-12 22:55:52 +00:00
parent f307ac4b7c
commit c2ddf330e7
2 changed files with 2 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
matlab/beijing_config.mat
View File

Binary file not shown.

9
matlab/kai01_BejingPose2Graph.m
View File

@ -8,7 +8,6 @@ cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
factors = Pose2Graph;
factors2 = Pose2Graph;
ord = Ordering();
ord2 = Ordering();
[~, idx] = sort(way_levels, 'descend');
@ -30,8 +29,6 @@ for i=1:length(idx)
if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree
factor=Pose2Factor(key1,key2,measured, cov);
factors.push_back(factor);
ord.push_back(key1);
ord.push_back(key2);
else %% not in the spanning tree
factors2.push_back(Pose2Factor(key1,key2,measured, cov));
ord2.push_back(key1);
@ -39,10 +36,7 @@ for i=1:length(idx)
end
end
end
ord.unique();
ord2.unique();
% ord.reverse();
% ord2.reverse();
if 1
config=Pose2Config();
@ -58,7 +52,8 @@ if 1
save('beijing_config.mat','config');
end
amd_ord=factors.getOrdering_(); % does not work
% Spanning tree with bottom-up ordering
ord = bottom_up_ordering(pred);
LFG=factors.linearize_(config);
ijs=LFG.sparse(ord);
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));