12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Check that P is non-empty before drawing covariance ellipses

release/4.3a0
Alex Cunningham 2012-09-18 17:48:20 +00:00
parent f272a07e29
commit c6f7fa8fb7
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/+gtsam/plotPoint2.m
View File

@ -5,6 +5,6 @@ if size(color,2)==1
else else
plot(p.x,p.y,color); plot(p.x,p.y,color);
end end
if exist('P', 'var') if exist('P', 'var') && (~isempty(P))
gtsam.covarianceEllipse([p.x;p.y],P,color(1)); gtsam.covarianceEllipse([p.x;p.y],P,color(1));
end end

2
matlab/+gtsam/plotPose2.m
View File

@ -6,7 +6,7 @@ plot(pose.x,pose.y,[color '*']);
c = cos(pose.theta); c = cos(pose.theta);
s = sin(pose.theta); s = sin(pose.theta);
quiver(pose.x,pose.y,c,s,axisLength,color); quiver(pose.x,pose.y,c,s,axisLength,color);
if nargin>2 if nargin>2 && (~isempty(P))
pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame
gRp = [c -s;s c]; % rotation from pose to global gRp = [c -s;s c]; % rotation from pose to global
gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color);