From 5140764c54398dd034ef66272fba554a1f6221bb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Dec 2009 06:19:30 +0000 Subject: [PATCH] show whole matrix and solve in MATLAB --- matlab/BeijingPose2Graph.m | 55 +++++++++++++++++++++++++------------- matlab/make_Pose2.m | 53 ++++++++++++++++++++++++++++++++++++ 2 files changed, 89 insertions(+), 19 deletions(-) create mode 100644 matlab/make_Pose2.m diff --git a/matlab/BeijingPose2Graph.m b/matlab/BeijingPose2Graph.m index 3f093494e..b3cfe2e55 100644 --- a/matlab/BeijingPose2Graph.m +++ b/matlab/BeijingPose2Graph.m @@ -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) diff --git a/matlab/make_Pose2.m b/matlab/make_Pose2.m new file mode 100644 index 000000000..57680068f --- /dev/null +++ b/matlab/make_Pose2.m @@ -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