Re-organized MATLAB code

matlab/addSimulatedConstraint.m

@@ -1,17 +0,0 @@
-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/bottom_up_ordering.m

@@ -1,19 +0,0 @@
-% find a bottom to up ordering given the tree structure {pred} returned by matlab's graphminspantree
-function [ordering] = bottom_up_ordering(pred)
-
-%% compute the levels of the nodes
-parents = [0];
-node_levels = zeros(length(pred), 1);
-current_level = 1;
-while ~isempty(parents)
-    parents = find(ismember(pred, parents));
-    node_levels(parents) = current_level;
-    current_level = current_level + 1;
-end
-[~, node_order] = sort(node_levels, 'descend');  % the order of the nodes in leaves-to-root order
-
-ordering = Ordering();
-for i = 1:length(node_order)
-    ordering.push_back(sprintf('x%d', node_order(i)));
-end
-end

matlab/frank01_CGD.m

@@ -1,39 +0,0 @@
-%-----------------------------------------------------------------------
-% frank01.m: try conjugate gradient on our example graph
-%-----------------------------------------------------------------------
-
-% get matrix form H and z
-fg = createGaussianFactorGraph();
-ord = Ordering;
-ord.push_back('x1');
-ord.push_back('x2');
-ord.push_back('l1');
-
-[H,z] = fg.matrix(ord);
-
-% form system of normal equations
-A=H'*H
-b=H'*z
-
-% k=0
-x = zeros(6,1)
-g = A*x-b
-d = -g
-
-for k=1:5
-    alpha = - (d'*g)/(d'*A*d)
-    x = x + alpha*d
-    g = A*x-b
-    beta = (d'*A*g)/(d'*A*d)
-    d = -g + beta*d
-end
-
-% Do gradient descent
-% fg2 = createGaussianFactorGraph();
-% zero = createZeroDelta();
-% actual = fg2.gradientDescent(zero);
-% CHECK(assert_equal(expected,actual,1e-2));
-
-% Do conjugate gradient descent
-% actual2 = fg2.conjugateGradientDescent(zero);
-% CHECK(assert_equal(expected,actual2,1e-2));

matlab/frank02_ToroPose2Graph.m

@@ -1 +0,0 @@
-% load the Toro 2D dataset and build pose graph

matlab/frank03_BejingPose2Graph.m

@@ -1,44 +0,0 @@
-% 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

@@ -1,34 +0,0 @@
-% 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

@@ -1,41 +0,0 @@
-% 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),:);
-% add prior
-figure(2)
-spy(A1)
-
-% calculate R1
-tic
-R1 = qr(A1,0);
-toc
-figure(3)
-spy(R1)
-
-% calculate R1
-tic
-R1 = chol(A1'*A1);
-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

@@ -1,79 +0,0 @@
-load beijing.mat;
-load beijing_angles.mat;
-load beijing_graph.mat;
-load beijing_config.mat;
-
-cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
-
-factors = Pose2Graph;
-factors2 = Pose2Graph;
-ord2 = Ordering();
-
-[rows cols] = find(tree);
-for i=length(edge_order):-1:1
-    if mod(i,500) == 0
-        fprintf(1, 'processing edge %d\n', i);
-    end
-    
-    id1 = rows(edge_order(i));
-    id2 = cols(edge_order(i));
-    key1 = sprintf('x%d', id1);
-    key2 = sprintf('x%d', id2);
-    
-    delta_x = points(id1,:) - points(id2,:);
-    delta_angle = angles(id1) - angles(id2);
-    measured = Pose2(delta_x(1), delta_x(2), delta_angle);
-    
-    if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree
-        factor=Pose2Factor(key1,key2,measured, cov);
-        factors.push_back(factor);
-    else %% not in the spanning tree
-        factors2.push_back(Pose2Factor(key1,key2,measured, cov));
-        ord2.push_back(key1);
-        ord2.push_back(key2);
-    end
-end
-ord2.unique();
-
-if 1
-    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
-
-% Spanning tree with bottom-up ordering
-ord = bottom_up_ordering(pred);
-LFG=factors.linearize_(config);
-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
-R = qr(A,0);
-figure(3)
-spy(R)
-
-% show re-ordered R factor
-% P = colamd(A);
-% figure(4)
-% spy(A(:,P))
-% R = qr(A(:,P),0);
-% figure(5)
-% spy(R)
-

matlab/kai02_bottomUp.m

@@ -1,4 +0,0 @@
-load beijing_graph.mat;
-
-ordering = bottom_up_ordering(pred);
-ordering.print('ordering')

matlab/vila01_BejingPose2Graph.m

@@ -1,84 +0,0 @@
-load beijing.mat;
-load beijing_angles.mat;
-load beijing_graph.mat;
-%load beijing_config.mat;
-
-cov = [ 0.25, 0, 0; 0, 0.25, 0; 0, 0, 0.01];
-
-
-factors = Pose2Graph;
-factors2 = Pose2Graph;
-ord = Ordering();
-ord2 = Ordering();
-
-for i=1:length(ways)
-    if mod(i,500) == 0
-        fprintf(1, 'processing way %d\n', i);
-    end
-    for j=1:length(ways{i})-1
-        id1 = ways{i}(j);
-        id2 = ways{i}(j+1);
-        key1 = sprintf('x%d', id1);
-        key2 = sprintf('x%d', id2);
-
-        delta_x = points(id1,:) - points(id2,:);
-        delta_angle = angles(id1) - angles(id2);
-        measured = Pose2(delta_x(1), delta_x(2), delta_angle);
-
-        if pred(id1) == id2 || pred(id2) == id1 %% in the spanning tree
-            factor=Pose2Factor(key1,key2,measured, cov);
-            factors.push_back(factor);
-            ord.push_back(key1);
-            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();
-ord2.unique();
-% ord.reverse();
-% ord2.reverse();
-
-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);
-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
-R = qr(A,0);
-figure(3)
-spy(R)
-
-% show re-ordered R factor
-P = colamd(A);
-figure(4)
-spy(A(:,P))
-R = qr(A(:,P),0);
-figure(5)
-spy(R)