show whole matrix and solve in MATLAB

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();
+ord2.unique();
-n=size(points,1);
+
-for j=1:n
+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,50) == 0
+        if mod(j,1000) == 0
             key
         end
+    end
+    save('beijing_config.mat','config');
 end
-save('beijing_config.mat','config');
 
-ord2=factors.getOrdering_();
+amd_ord=factors.getOrdering_(); % does not work
-fprintf(1,'linearize...');
 LFG=factors.linearize_(config);
-fprintf(1,'linearize done!');
+ijs=LFG.sparse(ord);
-ijs=LFG.sparse(ord2);
-fprintf(1,'sparse done');
 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)

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