From 080dd7d57ccc4890af3171e0a7d50cf2d99cbe17 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 23 Jul 2012 19:21:00 +0000 Subject: [PATCH] Updated some of the matlab utility functions --- matlab/covarianceEllipse3D.m | 2 +- matlab/examples/findExampleDataFile.m | 19 ++++++++ matlab/load2D.m | 10 +++-- matlab/plot2DPoints.m | 34 +++++++++++++++ matlab/plot2DTrajectory.m | 17 ++++++-- matlab/plot3DTrajectory.m | 63 +++++++++++++++++++++------ matlab/plotPose3.m | 26 ++++++----- 7 files changed, 137 insertions(+), 34 deletions(-) create mode 100644 matlab/examples/findExampleDataFile.m create mode 100644 matlab/plot2DPoints.m diff --git a/matlab/covarianceEllipse3D.m b/matlab/covarianceEllipse3D.m index 6d6ed9127..d3ef1f8a7 100644 --- a/matlab/covarianceEllipse3D.m +++ b/matlab/covarianceEllipse3D.m @@ -11,7 +11,7 @@ k = 11.82; radii = k*sqrt(diag(s)); % generate data for "unrotated" ellipsoid -[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3)); +[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8); % rotate data with orientation matrix U and center M data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); diff --git a/matlab/examples/findExampleDataFile.m b/matlab/examples/findExampleDataFile.m new file mode 100644 index 000000000..7719dfcfa --- /dev/null +++ b/matlab/examples/findExampleDataFile.m @@ -0,0 +1,19 @@ +function datafile = findExampleDataFile(datasetName) +%FINDEXAMPLEDATAFILE Find a dataset in the examples folder + +[ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); +searchPath = { ... + fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... + fullfile(myPath, [ 'Data/' datasetName ]) }; +datafile = []; +for path = searchPath + if exist(path{:}, 'file') + datafile = path{:}; + end +end +if isempty(datafile) + error([ 'Could not find example data file ' datasetName ]); +end + +end + diff --git a/matlab/load2D.m b/matlab/load2D.m index 31e96818f..c4b2e105d 100644 --- a/matlab/load2D.m +++ b/matlab/load2D.m @@ -2,6 +2,8 @@ function [graph,initial] = load2D(filename,model) % load2D: read TORO pose graph % cannot read noise model from file yet, uses specified model +import gtsam.* + fid = fopen(filename); if fid < 0 error(['load2D: Cannot open file ' filename]); @@ -13,17 +15,17 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose2SLAM.Graph; -initial = pose2SLAM.Values; +graph = NonlinearFactorGraph; +initial = Values; n=size(lines,1); for i=1:n line_i=lines{i}; if strcmp('VERTEX2',line_i(1:7)) v = textscan(line_i,'%s %d %f %f %f',1); - initial.insertPose(v{2}, gtsam.Pose2(v{3}, v{4}, v{5})); + initial.insert(v{2}, Pose2(v{3}, v{4}, v{5})); elseif strcmp('EDGE2',line_i(1:5)) e = textscan(line_i,'%s %d %d %f %f %f',1); - graph.addRelativePose(e{2}, e{3}, gtsam.Pose2(e{4}, e{5}, e{6}), model); + graph.add(BetweenFactorPose2(e{2}, e{3}, Pose2(e{4}, e{5}, e{6}), model)); end end diff --git a/matlab/plot2DPoints.m b/matlab/plot2DPoints.m new file mode 100644 index 000000000..0065b55c4 --- /dev/null +++ b/matlab/plot2DPoints.m @@ -0,0 +1,34 @@ +function plot2DPoints(values, marginals) +%PLOT2DPOINTS Plots the Point2's in a values, with optional covariances +% Finds all the Point2 objects in the given Values object and plots them. +% If a Marginals object is given, this function will also plot marginal +% covariance ellipses for each point. + +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot points and covariance matrices +for i = 0:keys.size-1 + key = keys.at(i); + p = values.at(key); + if isa(p, 'gtsam.Point2') + if haveMarginals + P = marginals.marginalCovariance(key); + plotPoint2(p, 'g', P); + else + plotPoint2(p, 'g'); + end + end +end + +if ~holdstate + hold off +end + +end + diff --git a/matlab/plot2DTrajectory.m b/matlab/plot2DTrajectory.m index 82c62bb40..fcbfe4b92 100644 --- a/matlab/plot2DTrajectory.m +++ b/matlab/plot2DTrajectory.m @@ -1,4 +1,4 @@ -function plot2DTrajectory(values, marginals) +function plot2DTrajectory(values, linespec, marginals) %PLOT2DTRAJECTORY Plots the Pose2's in a values, with optional covariances % Finds all the Pose2 objects in the given Values object and plots them % in increasing key order, connecting consecutive poses with a line. If @@ -7,10 +7,17 @@ function plot2DTrajectory(values, marginals) import gtsam.* +if ~exist('linespec', 'var') || isempty(linespec) + linespec = 'k*-'; +end + haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); -% Store poses and covariance matrices +holdstate = ishold; +hold on + +% Plot poses and covariance matrices lastIndex = []; for i = 0:keys.size-1 key = keys.at(i); @@ -21,7 +28,7 @@ for i = 0:keys.size-1 % last pose. lastKey = keys.at(lastIndex); lastPose = values.at(lastKey); - plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], 'k*-'); + plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); if haveMarginals P = marginals.marginalCovariance(lastKey); plotPose2(lastPose, 'g', P); @@ -39,5 +46,9 @@ if ~isempty(lastIndex) && haveMarginals plotPose2(lastPose, 'g', P); end +if ~holdstate + hold off +end + end diff --git a/matlab/plot3DTrajectory.m b/matlab/plot3DTrajectory.m index 83d3d76e2..771d567fa 100644 --- a/matlab/plot3DTrajectory.m +++ b/matlab/plot3DTrajectory.m @@ -1,18 +1,53 @@ -function plot3DTrajectory(values,style,frames,scale) +function plot3DTrajectory(values,linespec,frames,scale,marginals) % plot3DTrajectory -if nargin<3,frames=false;end -if nargin<4,scale=0;end +if ~exist('scale','var') || isempty(scale), scale=1; end +if ~exist('frames','var'), scale=[]; end -T=values.translations() -plot3(T(:,1),T(:,2),T(:,3),style); hold on -if frames - N=values.size; - for i=0:N-1 - pose = values.pose(i); - t = pose.translation; - R = pose.rotation.matrix; - quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),scale,'r'); - quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),scale,'g'); - quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b'); +import gtsam.* + +haveMarginals = exist('marginals', 'var'); +keys = KeyVector(values.keys); + +holdstate = ishold; +hold on + +% Plot poses and covariance matrices +lastIndex = []; +for i = 0:keys.size-1 + key = keys.at(i); + x = values.at(key); + if isa(x, 'gtsam.Pose3') + if ~isempty(lastIndex) + % Draw line from last pose then covariance ellipse on top of + % last pose. + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + plotPose3(lastPose, P, scale); + end + lastIndex = i; end +end + +% Draw final pose +if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + lastPose = values.at(lastKey); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + plotPose3(lastPose, P, scale); +end + +if ~holdstate + hold off +end + end \ No newline at end of file diff --git a/matlab/plotPose3.m b/matlab/plotPose3.m index 262d1dfc1..956b231be 100644 --- a/matlab/plotPose3.m +++ b/matlab/plotPose3.m @@ -6,18 +6,20 @@ if nargin<3,axisLength=0.1;end gRp = pose.rotation().matrix(); % rotation from pose to global C = pose.translation().vector(); -% draw the camera axes -xAxis = C+gRp(:,1)*axisLength; -L = [C xAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','r'); - -yAxis = C+gRp(:,2)*axisLength; -L = [C yAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','g'); - -zAxis = C+gRp(:,3)*axisLength; -L = [C zAxis]'; -line(L(:,1),L(:,2),L(:,3),'Color','b'); +if ~isempty(axisLength) + % draw the camera axes + xAxis = C+gRp(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+gRp(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+gRp(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); +end % plot the covariance if (nargin>2) && (~isempty(P))