From b7dc6b36878bccec038b6bf0760ce7106668aa85 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Nov 2014 00:51:11 +0100 Subject: [PATCH] Fixed many utilities and examples --- matlab/+gtsam/VisualISAMPlot.m | 2 +- matlab/+gtsam/VisualISAMStep.m | 2 +- matlab/+gtsam/load3D.m | 4 +- matlab/+gtsam/plot2DPoints.m | 7 ++- matlab/+gtsam/plot2DTrajectory.m | 30 ++++++----- matlab/+gtsam/plot3DPoints.m | 6 ++- matlab/+gtsam/plot3DTrajectory.m | 53 +++++++++++-------- matlab/gtsam_examples/PlanarSLAMExample.m | 6 +-- .../gtsam_examples/PlanarSLAMExample_graph.m | 2 +- .../PlanarSLAMExample_sampling.m | 8 +-- matlab/gtsam_examples/Pose2SLAMExample.m | 2 +- .../gtsam_examples/Pose2SLAMExample_circle.m | 14 ++--- .../gtsam_examples/Pose2SLAMExample_graph.m | 2 +- matlab/gtsam_examples/Pose3SLAMExample.m | 14 ++--- .../gtsam_examples/Pose3SLAMExample_graph.m | 2 +- matlab/gtsam_examples/StereoVOExample_large.m | 4 +- wrap/StaticMethod.h | 2 +- 17 files changed, 88 insertions(+), 72 deletions(-) diff --git a/matlab/+gtsam/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m index 874dbf523..9b52f016f 100644 --- a/matlab/+gtsam/VisualISAMPlot.m +++ b/matlab/+gtsam/VisualISAMPlot.m @@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals); M = 1; while result.exists(symbol('x',M)) ii = symbol('x',M); - pose_i = result.at(ii); + pose_i = result.atPose3(ii); if options.hardConstraint && (M==1) gtsam.plotPose3(pose_i,[],10); else diff --git a/matlab/+gtsam/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m index 82b3754ef..6fa81e072 100644 --- a/matlab/+gtsam/VisualISAMStep.m +++ b/matlab/+gtsam/VisualISAMStep.m @@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex}) end %% Initial estimates for the new pose. -prevPose = result.at(symbol('x',nextPoseIndex-1)); +prevPose = result.atPose3(symbol('x',nextPoseIndex-1)); initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); %% Update ISAM diff --git a/matlab/+gtsam/load3D.m b/matlab/+gtsam/load3D.m index 94202e0c8..d536162e1 100644 --- a/matlab/+gtsam/load3D.m +++ b/matlab/+gtsam/load3D.m @@ -46,9 +46,9 @@ for i=1:n graph.add(BetweenFactorPose3(i1, i2, dpose, model)); if successive if i2>i1 - initial.insert(i2,initial.at(i1).compose(dpose)); + initial.insert(i2,initial.atPose3(i1).compose(dpose)); else - initial.insert(i1,initial.at(i2).compose(dpose.inverse)); + initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse)); end end end diff --git a/matlab/+gtsam/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m index d4703a5d7..8e91fa19d 100644 --- a/matlab/+gtsam/plot2DPoints.m +++ b/matlab/+gtsam/plot2DPoints.m @@ -18,15 +18,18 @@ 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') + try + p = values.atPoint2(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint2(p, linespec, P); else gtsam.plotPoint2(p, linespec); end + catch err + % I guess it's not a Point2 end + end if ~holdstate diff --git a/matlab/+gtsam/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m index 45e7fe467..1c29213cd 100644 --- a/matlab/+gtsam/plot2DTrajectory.m +++ b/matlab/+gtsam/plot2DTrajectory.m @@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'k*-'; + linespec = 'k*-'; end haveMarginals = exist('marginals', 'var'); @@ -25,24 +25,26 @@ plot(X,Y,linespec); % Quiver can also be vectorized if no marginals asked if ~haveMarginals - C = cos(theta); - S = sin(theta); - quiver(X,Y,C,S,0.1,linespec); + C = cos(theta); + S = sin(theta); + quiver(X,Y,C,S,0.1,linespec); else - % plotPose2 does both quiver and covariance matrix - keys = KeyVector(values.keys); - for i = 0:keys.size-1 - key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose2') - P = marginals.marginalCovariance(key); - gtsam.plotPose2(x,linespec(1), P); + % plotPose2 does both quiver and covariance matrix + keys = KeyVector(values.keys); + for i = 0:keys.size-1 + key = keys.at(i); + try + x = values.atPose2(key); + P = marginals.marginalCovariance(key); + gtsam.plotPose2(x,linespec(1), P); + catch err + % I guess it's not a Pose2 + end end - end end if ~holdstate - hold off + hold off end end diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 8feab1744..46e161807 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -18,14 +18,16 @@ 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.Point3') + try + p = values.atPoint3(key); if haveMarginals P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); else gtsam.plotPoint3(p, linespec); end + catch + % I guess it's not a Point3 end end diff --git a/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index d24e21297..05483e589 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -17,39 +17,48 @@ hold on lastIndex = []; for i = 0:keys.size-1 key = keys.at(i); - x = values.at(key); - if isa(x, 'gtsam.Pose3') + try + x = values.atPose3(key); 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); + try + lastPose = values.atPose3(lastKey); + plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec); + if haveMarginals + P = marginals.marginalCovariance(lastKey); + else + P = []; + end + gtsam.plotPose3(lastPose, P, scale); + catch err + warning(['no Pose3 at ' lastKey]); + end + lastIndex = i; + end + catch + warning(['no Pose3 at ' key]); + end + + % Draw final pose + if ~isempty(lastIndex) + lastKey = keys.at(lastIndex); + try + lastPose = values.atPose3(lastKey); if haveMarginals P = marginals.marginalCovariance(lastKey); else P = []; end gtsam.plotPose3(lastPose, P, scale); + catch + warning(['no Pose3 at ' lastIndex]); 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 = []; + + if ~holdstate + hold off end - gtsam.plotPose3(lastPose, P, scale); -end - -if ~holdstate - hold off -end - + end \ No newline at end of file diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index ef1516017..aec933d74 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -71,9 +71,9 @@ marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); plot2DPoints(result, 'b', marginals); -plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); -plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); -plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-'); +plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-'); +plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-'); axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/gtsam_examples/PlanarSLAMExample_graph.m b/matlab/gtsam_examples/PlanarSLAMExample_graph.m index 9ca88e49a..db6484c5c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_graph.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_graph.m @@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]); [graph,initial] = load2D(datafile, model); %% Add a Gaussian prior on a pose in the middle -priorMean = initial.at(40); +priorMean = initial.atPose2(40); priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]); graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 327c64d4d..375ed645c 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); - point{j} = sample.at(key); + point{j} = sample.atPoint2(key); Q{j}=marginals.marginalCovariance(key); S{j}=chol(Q{j}); % for sampling end -plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-'); -plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-'); -plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-'); +plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-'); +plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-'); view(2); axis auto; axis equal %% Do Sampling on point 2 diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m index 65ce28dbb..789d1c483 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses cla; hold on -plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-'); +plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-'); marginals = Marginals(graph, result); plot2DTrajectory(result, [], marginals); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m index 3d2265d76..d2676845c 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose2(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose2(0); +p1 = hexagon.atPose2(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config initial = Values; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); -initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); -initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); -initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); -initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); +initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index 83ec949cc..c479278c1 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -41,7 +41,7 @@ marginals = Marginals(graph, result); toc P={}; for i=1:result.size()-1 - pose_i = result.at(i); + pose_i = result.atPose2(i); P{i}=marginals.marginalCovariance(i); plotPose2(pose_i,'b',P{i}) end diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index 1d9c3b579..5a9c8bf03 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 39e48c204..de6f9e86d 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0. %% Plot Initial Estimate cla -first = initial.at(0); +first = initial.atPose3(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); drawnow; diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index b77733abd..464448335 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -45,7 +45,7 @@ for i=1:size(measurements,1) if ~initial.exists(symbol('l',sf(2))) % 3D landmarks are stored in camera coordinates: transform % to world coordinates using the respective initial pose - pose = initial.at(symbol('x', sf(1))); + pose = initial.atPose3(symbol('x', sf(1))); world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8))); initial.insert(symbol('l',sf(2)), world_point); end @@ -54,7 +54,7 @@ toc %% add a constraint on the starting pose key = symbol('x',1); -first_pose = initial.at(key); +first_pose = initial.atPose3(key); graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index af5cbce59..0aed83677 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -42,7 +42,7 @@ struct StaticMethod: public Function, public SignatureOverloads { // emit a list of comments, one for each overload void comment_fragment(FileWriter& proxyFile) const { - SignatureOverloads::comment_fragment(proxyFile, name_); + SignatureOverloads::comment_fragment(proxyFile, matlabName()); } void verifyArguments(const std::vector& validArgs) const {