Drawing lines for factors works

joansola · Aug 6, 2015 · f0e7ee5 · f0e7ee5
1 parent eaad9b4
commit f0e7ee5
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 18 deletions.
diff --git a/Graphics/gplot3.m → Graphics/factorLines.m b/Graphics/gplot3.m → 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
@@ -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
@@ -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
@@ -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,15 +141,15 @@
   '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
       'innType',      'ortDst',...    % innovation type for lines
       '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
@@ -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
@@ -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];