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 27eecb7cb..000000000 Binary files a/matlab/beijing_config.mat and /dev/null differ 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) +