Easier indexing for ellipse drawing.

joansola · Oct 19, 2018 · 73c9db1 · 73c9db1
1 parent 70916f2
commit 73c9db1
Show file tree

Hide file tree

Showing 4 changed files with 3 additions and 4 deletions.
diff --git a/Graphics/drawEucPnt.m b/Graphics/drawEucPnt.m
@@ -18,7 +18,7 @@ function drawEucPnt(MapFig, Lmk, color, MapOpt)
         x = Lmk.state.x;
         % Covariance
         if ~isempty(Map.pr)
-            [~,r] = ismember(Lmk.state.r, Map.pr);
+            r = Lmk.state.r;
             P = Map.P(r,r);
         else
             P = zeros(3);

diff --git a/HighLevel/slamtb_graph.m b/HighLevel/slamtb_graph.m
@@ -61,7 +61,6 @@
 % Clear user data - not needed anymore
 clear Robot Sensor World Time   % clear all user data
 
-
 %% III. Initialize data logging
 % TODO: Create source and/or destination files and paths for data input and
 % logs.

diff --git a/InterfaceLevel/drawMapFig.m b/InterfaceLevel/drawMapFig.m
@@ -38,7 +38,7 @@
     MapFig.Rob(rob).patch = drawObject(MapFig.Rob(rob).patch, Rob(rob));
 
     if FigOpt.map.showEllip && ~isempty(Map.pr)
-        [~, r] = ismember(Rob(rob).state.r(1:3), Map.pr);
+        r = Rob(rob).state.r(1:3);
         P = Map.P(r,r);
         drawEllipse(MapFig.Rob(rob).ellipse,Rob(rob).frame.x(1:3),P);
     end

diff --git a/InterfaceLevel/solveGraphQR.m b/InterfaceLevel/solveGraphQR.m
@@ -84,7 +84,7 @@
 end
 
 % Compute full covariance matrix.
-Map.P = inv(full(Map.R))*inv(full(Map.R))';
+Map.P(pr,pr) = inv(full(Map.R))*inv(full(Map.R))';
 
 end
-Original file line number
+Diff line change
@@ Expand Up / @@ -84,7 +84,7 @@ @@
     end
     % Compute full covariance matrix.
-    Map.P = inv(full(Map.R))*inv(full(Map.R))';
+    Map.P(pr,pr) = inv(full(Map.R))*inv(full(Map.R))';
     end
@@ Expand Down @@