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

show whole matrix and solve in MATLAB

release/4.3a0
Frank Dellaert 2009-12-12 06:19:30 +00:00
parent 2914d663de
commit 5140764c54
2 changed files with 89 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
matlab/BeijingPose2Graph.m
View File

@ -1,7 +1,7 @@
load beijing.mat;
load beijing_angles.mat;
load beijing_graph.mat;
%load beijing_config.mat;
load beijing_config.mat;
cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
@ -9,8 +9,9 @@ cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
factors = Pose2Graph;
factors2 = Pose2Graph;
ord = Ordering();
ord2 = Ordering();
for i=1:50%length(ways)
for i=1:length(ways)
if mod(i,50) == 0
fprintf(1, 'processing way %d\n', i);
end
@ -31,30 +32,46 @@ for i=1:50%length(ways)
ord.push_back(key2);
else %% not in the spanning tree
factors2.push_back(Pose2Factor(key1,key2,measured, cov));
ord2.push_back(key1);
ord2.push_back(key2);
end
end
end
ord.unique();
config=Pose2Config();
n=size(points,1);
for j=1:n
pose=Pose2(points(j,1),points(j,2),angles(j));
key = sprintf('x%d', j);
config.insert(key,pose);
if mod(j,50) == 0
key
end
end
save('beijing_config.mat','config');
ord2.unique();
ord2=factors.getOrdering_();
fprintf(1,'linearize...');
if 0
config=Pose2Config();
n=size(points,1);
for j=1:n
pose=Pose2(points(j,1),points(j,2),angles(j));
key = sprintf('x%d', j);
config.insert(key,pose);
if mod(j,1000) == 0
key
end
end
save('beijing_config.mat','config');
end
amd_ord=factors.getOrdering_(); % does not work
LFG=factors.linearize_(config);
fprintf(1,'linearize done!');
ijs=LFG.sparse(ord2);
fprintf(1,'sparse done');
ijs=LFG.sparse(ord);
A=sparse(ijs(1,:),ijs(2,:),ijs(3,:));
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);
% show R factor
P = COLAMD(A);
figure(3)
spy(A(:,P))
R = qr(A(:,P),0);
figure(4)
spy(R)

53
matlab/make_Pose2.m Normal file
View File

@ -0,0 +1,53 @@
% automatically generated by wrap on 2009-Dec-11
echo on
toolboxpath = '/Users/dellaert/toolbox/gtsam'
addpath(toolboxpath);
% cd(toolboxpath)
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2_.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2_P.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2_ddd.cpp
%
% cd @Pose2
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam print.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam equals.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam x.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam y.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam theta.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam dim.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam exmap.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam vector.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam rotate.cpp
cd(toolboxpath)
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2Config_.cpp
cd @Pose2Config
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam get.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam insert.cpp
% mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam print.cpp
cd(toolboxpath)
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2Factor_ssPM.cpp
cd @Pose2Factor
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam print.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam equals.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam error.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam size.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam linearize.cpp
cd(toolboxpath)
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam new_Pose2Graph_.cpp
cd @Pose2Graph
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam print.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam equals.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam linearize_.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam push_back.cpp
mex -I/opt/local/include/ -I/Users/dellaert/include -I/Users/dellaert/include/gtsam -L/Users/dellaert/lib -lgtsam getOrdering_.cpp
cd(toolboxpath)
echo off