From f0e7ee56ef5dbe851871d481fa23f30f5e5d7e31 Mon Sep 17 00:00:00 2001 From: Joan Sola Date: Thu, 9 Jul 2015 22:38:04 +0200 Subject: [PATCH] Drawing lines for factors works --- Graphics/{gplot3.m => factorLines.m} | 5 +++-- HighLevel/createGraphicsStructures.m | 3 ++- HighLevel/slamtb_graph.m | 1 + HighLevel/userDataGraph.m | 12 +++++++----- InterfaceLevel/createMapFig.m | 20 +++++++++++++------- InterfaceLevel/drawMapFig.m | 27 ++++++++++++++++++++++++--- 6 files changed, 50 insertions(+), 18 deletions(-) rename Graphics/{gplot3.m => factorLines.m} (94%) diff --git a/Graphics/gplot3.m b/Graphics/factorLines.m similarity index 94% rename from Graphics/gplot3.m rename to Graphics/factorLines.m index e6ac679..cb7700c 100644 --- a/Graphics/gplot3.m +++ b/Graphics/factorLines.m @@ -1,4 +1,5 @@ -function h=gplot3(h,A,xyz) +function [X,Y,Z] = factorLines(A,xyz) + %GPLOT Plot graph, as in "graph theory". % GPLOT(A,xy) plots the graph specified by A and xy. A graph, G, is % a set of nodes numbered from 1 to n, and a set of connections, or @@ -39,5 +40,5 @@ Z=[]; end % line(X,Y,Z,'LineStyle',lsty,'Color',csty,'Marker',msty); -set(h,'xdata',X,'ydata',Y,'zdata',Z); +% set(h,'xdata',X,'ydata',Y,'zdata',Z); diff --git a/HighLevel/createGraphicsStructures.m b/HighLevel/createGraphicsStructures.m index bd3d211..c1167c1 100644 --- a/HighLevel/createGraphicsStructures.m +++ b/HighLevel/createGraphicsStructures.m @@ -1,5 +1,6 @@ function [MapFig,SenFig] = createGraphicsStructures(... Rob, Sen, Lmk, Obs,... + Trj, Frm, Fac, ... SimRob, SimSen, SimLmk,... FigOpt) @@ -27,7 +28,7 @@ % Init map figure -MapFig = createMapFig(Rob,Sen,Lmk,SimRob,SimSen,SimLmk,FigOpt); +MapFig = createMapFig(Rob,Sen,Lmk, Trj, Frm, Fac,SimRob,SimSen,SimLmk,FigOpt); % Init sensor's measurement space figures SenFig = createSenFig(Sen,Obs,SimLmk,FigOpt); diff --git a/HighLevel/slamtb_graph.m b/HighLevel/slamtb_graph.m index 06d8579..fe677ac 100644 --- a/HighLevel/slamtb_graph.m +++ b/HighLevel/slamtb_graph.m @@ -54,6 +54,7 @@ % Graphics handles [MapFig,SenFig] = createGraphicsStructures(... Rob, Sen, Lmk, Obs,... % SLAM data + Trj, Frm, Fac, ... SimRob, SimSen, SimLmk,... % Simulator data FigOpt); % User-defined graphic options diff --git a/HighLevel/userDataGraph.m b/HighLevel/userDataGraph.m index 96e79bc..c9a17b1 100644 --- a/HighLevel/userDataGraph.m +++ b/HighLevel/userDataGraph.m @@ -60,8 +60,8 @@ 'orientationStd', [0;0;0],... % orient. error, std, in degrees 'dx', [.08;0;0],... % position increment 'daDegrees', [0;0;.9],... % angle increment, degrees - 'dxStd', 0.01*[1;1;1],... % odo linear error std - 'daStd', 0.1*[1;1;1]); % odo ang error std, degrees + 'dxStd', 0.03*[1;1;1],... % odo linear error std + 'daStd', 0.3*[1;1;1]); % odo ang error std, degrees % Robot{2} = struct(... % ODOMETRY EXAMPLE % 'id', 2,... % robot identifier @@ -141,7 +141,7 @@ 'correct', struct(... % options for lmk correction 'reprojectLmks', false,... % reproject lmks after active search? 'reparametrize', true,... % reparametrize lmk? - 'nUpdates', 20,... % max simultaneus updates + 'nUpdates', 10,... % max simultaneus updates 'MD2th', 9,... % Threshold on Mahalanobis distance squared 'linTestIdp', 0.1,... % threshold on IDP linearity test 'lines', struct(... % options for line corrections @@ -149,7 +149,7 @@ 'extPolicy', false,... % line extending policy ? 'extSwitch', 10)),... % extension policy switch point in pixels 'init', struct(... % Options for initialization - 'nbrInits', [5 5],... % number of inits [firstFrame, otherFrames] + 'nbrInits', [5 3],... % number of inits [firstFrame, otherFrames] 'initType', 'eucPnt',... % Type of lmk to use for init 'idpPnt', struct(... % options for lmk initialization 'nonObsMean', .1,... % mean of non obs @@ -207,6 +207,8 @@ 'orbit', [0 0],... % Azimuth and Elevation orbit angle increments - use to animate figure 'showSimLmk', false,... % show simulated landmarks? 'showEllip', false,... % show ellipsoids? + 'showMotFac', true,... % show motion factors? + 'showMeaFac', true,... % show measurement factors? 'colors', struct(... % map figure colors 'border', [1 1 1],... % [r g b] 'axes', [0 0 0],... % with: @@ -227,7 +229,7 @@ 'graph', struct(... % graph 'frame', 'b',... % frame 'motion', 'b',... % motion factors - 'meas', 'g'),... % measurement factors + 'meas', 'y'),... % measurement factors 'simu', 'b',... % or 'r', 'b', etc. 'est', 'g',... % estimated robots and sensors 'ground', [.8 .8 .8],... % simulated robots and sensors diff --git a/InterfaceLevel/createMapFig.m b/InterfaceLevel/createMapFig.m index 0caeba1..c04c6cb 100644 --- a/InterfaceLevel/createMapFig.m +++ b/InterfaceLevel/createMapFig.m @@ -1,4 +1,4 @@ -function MapFig = createMapFig(Rob,Sen,Lmk,SimRob,SimSen,SimLmk,FigOpt) +function MapFig = createMapFig(Rob,Sen,Lmk, Trj, Frm, Fac,SimRob,SimSen,SimLmk,FigOpt) % CREATEMAPFIG Create 3D map figure and handles. % MAPFIG = CREATEMAPFIG(Rob,Sen,Lmk,SimRob,SimSen,SimLmk,MapFigure) @@ -148,20 +148,26 @@ end % trajectory + n = size(Frm,2); + Z = zeros(2,n); MapFig.Rob(rob).trj = line(... + Z, ... + Z, ... + Z, ... 'parent', MapFig.axes,... - 'xdata', [], ... - 'ydata', [], ... - 'zdata', [], ... + 'visible','off', ... 'color', FigOpt.map.colors.graph.motion, ... 'marker', 'o'); % measurement factors + n = numel(Fac); + Z = zeros(2,n); MapFig.Rob(rob).factors = line(... + Z, ... + Z, ... + Z, ... 'parent', MapFig.axes,... - 'xdata', [], ... - 'ydata', [], ... - 'zdata', [], ... + 'visible','off', ... 'color', FigOpt.map.colors.graph.meas, ... 'marker', 'none'); diff --git a/InterfaceLevel/drawMapFig.m b/InterfaceLevel/drawMapFig.m index b1606bb..85641e9 100644 --- a/InterfaceLevel/drawMapFig.m +++ b/InterfaceLevel/drawMapFig.m @@ -51,9 +51,30 @@ end % Factors -[A, a, B, b] = buildAdjacencyMatrix(Lmk,Frm,Fac); -MapFig.Rob(rob).trj = gplot3(MapFig.Rob(rob).trj,A,a); -MapFig.Rob(rob).factors = gplot3(MapFig.Rob(rob).factors,B,b); +if FigOpt.map.showMotFac || FigOpt.map.showMeaFac + [A, a, B, b] = buildAdjacencyMatrix(Lmk,Frm,Fac); +end + +% Motion factors +if FigOpt.map.showMotFac + [X,Y,Z] = factorLines(A,a); + n = size(X,2); + for i=1:n + set(MapFig.Rob(rob).trj(i), 'visible', 'on', 'xdata', X(:,i), 'ydata', Y(:,i), 'zdata', Z(:,i)); + end + set(MapFig.Rob(rob).trj(n+1:end), 'visible', 'off'); +end + +% Measurement factors +if FigOpt.map.showMeaFac + [X,Y,Z] = factorLines(B,b); + n = size(X,2); + for i=1:n + set(MapFig.Rob(rob).factors(i), 'visible', 'on', 'xdata', X(:,i), 'ydata', Y(:,i), 'zdata', Z(:,i)); + end + set(MapFig.Rob(rob).factors(n+1:end), 'visible', 'off'); +end + % erase non used landmarks used = [Lmk.used];