From a4f73ee04fd635ea77aa2ea9964396ab3fea2f5a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 13 Dec 2009 17:49:38 +0000
Subject: [PATCH] Cleanup some code, frank05

---
 matlab/addSimulatedConstraint.m       |  17 ++++++++++
 matlab/beijing_config.mat             | Bin 482 -> 0 bytes
 matlab/frank03_BejingPose2Graph.m     |  44 ++++++++++++++++++++++++++
 matlab/frank04_showMatrices.m         |  34 ++++++++++++++++++++
 matlab/frank05_rightPreconditioning.m |  33 +++++++++++++++++++
 matlab/kai01_BejingPose2Graph.m       |  10 ++----
 6 files changed, 130 insertions(+), 8 deletions(-)
 create mode 100644 matlab/addSimulatedConstraint.m
 delete mode 100644 matlab/beijing_config.mat
 create mode 100644 matlab/frank03_BejingPose2Graph.m
 create mode 100644 matlab/frank04_showMatrices.m
 create mode 100644 matlab/frank05_rightPreconditioning.m

diff --git a/matlab/addSimulatedConstraint.m b/matlab/addSimulatedConstraint.m
new file mode 100644
index 000000000..a53872b6d
--- /dev/null
+++ b/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);
diff --git a/matlab/beijing_config.mat b/matlab/beijing_config.mat
deleted file mode 100644
index 27eecb7cb24cc59ce215afa358a666aea9cd15e8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 482
zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2c0*X0%nwjV*I2WZRmZYXA<mXu_
z1Sgg#xTGd47#b-U8Cn@ySQ!Bs1_qW2#Dbd)P{8Qv%MWA*0&&Hh$H@r^GZ>EKIGkZT
z(!?WhfJ4;#+<EWQKI|VOC2HpWZFYQg$3bR7{G6;OJX2Yi>=eE^7`2OM@o7&v&t^JV
zP>b)8x5JF`nG>EQNHCm~;!*(_%*Y5c_{M=V2M(|;v22(znUN`CVU3$YUP9Xig##>4
zjSZ@PHT|e@_|w3-a6V^uokUy5=7y&d3$s9WCO$jkVB{{aYM!9q=7y_HKlV7hnKpB>
zRNRz^fC*6|^88oj8<)r@%D0?vdd4^5mpIRkzk8W|ZG}U6IJ;yUEEv1g89s8Yu;CF|
z{%naM%M<bCQkKu3FHK!${D9Hthswo6j5E)7wDzy#WBV~7gPW~o|IdYwc0B49*RnfU
z-^#8W@}HgU&cicD8YIu1_?GA`&%?9gC39>&GvnVMm&7gPHtcBbZLpjZQ26A->x2XI
e{gosFE_<`wNbab4<k)CG;WH-(14D?J(gXmOBC^5&

diff --git a/matlab/frank03_BejingPose2Graph.m b/matlab/frank03_BejingPose2Graph.m
new file mode 100644
index 000000000..4739ecf71
--- /dev/null
+++ b/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);
\ No newline at end of file
diff --git a/matlab/frank04_showMatrices.m b/matlab/frank04_showMatrices.m
new file mode 100644
index 000000000..542838cad
--- /dev/null
+++ b/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);
+
+
diff --git a/matlab/frank05_rightPreconditioning.m b/matlab/frank05_rightPreconditioning.m
new file mode 100644
index 000000000..36f38a71b
--- /dev/null
+++ b/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)
+
diff --git a/matlab/kai01_BejingPose2Graph.m b/matlab/kai01_BejingPose2Graph.m
index 4dc2c24a3..ab110b414 100644
--- a/matlab/kai01_BejingPose2Graph.m
+++ b/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)
+