Cleanup some code, frank05

matlab/addSimulatedConstraint.m

@@ -0,0 +1,17 @@
+function addSimulatedConstraint(points,angles,sd,id1,id2,graph)
+% addSimulatedConstraint: create a simulated measurement with noise
+% standard deviations sd and add it to graph
+
+key1 = sprintf('x%d', id1);
+key2 = sprintf('x%d', id2);
+
+% ground truth
+delta_x = points(id1,:) - points(id2,:);
+delta_angle = angles(id1) - angles(id2);
+noisy = Pose2(delta_x(1) + sd(1)*randn, delta_x(2) + sd(2)*randn, delta_angle + sd(3)*randn);
+
+% create factor
+factor=Pose2Factor(key1,key2,noisy,diag(sd.*sd));
+
+% add it to the graph
+graph.push_back(factor);

matlab/frank03_BejingPose2Graph.m

@@ -0,0 +1,44 @@
+% frank03: create Beijing matrices in a cleaner way
+
+load beijing.mat;
+load beijing_angles.mat;
+load beijing_graph.mat;
+n=size(points,1);
+
+% create config or load it from file
+if 0
+    load beijing_config.mat;
+else
+    config=Pose2Config();
+    for j=1:n
+        if mod(j,1000) == 0, fprintf(1, 'adding node %d to config\n', j); end   
+        pose=Pose2(points(j,1),points(j,2),angles(j));
+        key = sprintf('x%d', j);
+        config.insert(key,pose);
+    end
+    save('beijing_config.mat','config');
+end
+
+sd = [0.25;0.25;0.01];
+
+% Build factor graph for entire graph
+graph = Pose2Graph;
+
+% First add tree constraints
+[I J] = find(tree);
+for k=length(edge_order):-1:1
+    edge = edge_order(k);
+    if mod(k,1000) == 0, fprintf(1, 'simulating constraint %d\n', k); end   
+    addSimulatedConstraint(points,angles,sd,I(edge),J(edge),graph);
+end
+
+% Then add remaining constraints C
+C=G-tree;
+[I J] = find(C);
+for k=1:length(I)
+    if mod(k,100) == 0, fprintf(1, 'simulating constraint %d\n', k); end   
+    addSimulatedConstraint(points,angles,sd,I(k),J(k),graph);
+end
+
+% generate ordering
+ordering = bottom_up_ordering(pred);

matlab/frank04_showMatrices.m

@@ -0,0 +1,34 @@
+% frank04: show matrices associated with Beijing simulation
+
+load beijing.mat;
+load beijing_angles.mat;
+load beijing_graph.mat;
+
+% put spanning tree 'tree' in correct order
+T = tree(node_order,node_order);
+
+% get loop closing constraints
+C=G(node_order,node_order)-T;
+
+close all
+
+% plot on map
+figure
+gplot(C,points(node_order,:),'r')
+hold on
+gplot(T,points(node_order,:),'k')
+axis equal
+
+% show spanning tree, original graph, and loop closures
+figure
+spy(T,'k');
+hold on
+spy(C,'r');
+
+figure
+spy(T);
+
+figure
+spy(C);
+
+

matlab/frank05_rightPreconditioning.m

@@ -0,0 +1,33 @@
+% frank05: show right pre-conditioning
+% first run frank03 or other script to generate graph, config, and ordering
+
+% linearize the non-linear factor graph
+tic
+LFG = graph.linearize_(config);
+toc
+tic
+ijs = LFG.sparse(ordering);
+A = sparse(ijs(1,:),ijs(2,:),ijs(3,:)); 
+toc
+figure(1)
+spy(A);
+
+% isolate the spanning tree part
+A1=A(1:3*nnz(tree),:);
+figure(2)
+spy(A1)
+
+% calculate
+tic
+R1 = qr(A1,0);
+toc
+figure(3)
+spy(R1)
+
+% calculate the entire R factor (expensive)
+tic
+R = qr(A,0);
+toc
+figure(4)
+spy(R)
+

matlab/kai01_BejingPose2Graph.m

@@ -5,7 +5,6 @@ load beijing_config.mat;
 
 cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
 
-
 factors = Pose2Graph;
 factors2 = Pose2Graph;
 ord2 = Ordering();
@@ -36,7 +35,7 @@ for i=length(edge_order):-1:1
 end
 ord2.unique();
 
-if 0
+if 1
     config=Pose2Config();
     n=size(points,1);
     for j=1:n
@@ -70,12 +69,6 @@ R = qr(A,0);
 figure(3)
 spy(R)
 
-% plot on map
-figure(7)
-gplot(tree,points)
-%gplot(tree,points)
-axis equal
-
 % show re-ordered R factor
 % P = colamd(A);
 % figure(4)
@@ -83,3 +76,4 @@ axis equal
 % R = qr(A(:,P),0);
 % figure(5)
 % spy(R)
+