From 950bd8fcad258bb3b8c147799b9d4f66c8632fa5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 3 Jun 2012 15:44:39 +0000 Subject: [PATCH] Added MATLAB utility functions to pose2SLAM --- examples/matlab/Pose2SLAMExample_graph.m | 36 +++++++++++------------- examples/matlab/plotTrajectory.m | 10 ------- gtsam.h | 5 +++- gtsam/slam/pose2SLAM.cpp | 30 ++++++++++++++++++++ gtsam/slam/pose2SLAM.h | 8 ++++-- gtsam/slam/tests/testPose2SLAM.cpp | 12 +++++++- 6 files changed, 67 insertions(+), 34 deletions(-) delete mode 100644 examples/matlab/plotTrajectory.m diff --git a/examples/matlab/Pose2SLAMExample_graph.m b/examples/matlab/Pose2SLAMExample_graph.m index d4c5f261a..af0b1f3fb 100644 --- a/examples/matlab/Pose2SLAMExample_graph.m +++ b/examples/matlab/Pose2SLAMExample_graph.m @@ -12,7 +12,7 @@ %% Create graph container and add factors to it graph = pose2SLAMGraph; -initialEstimate = pose2SLAMValues; +initial = pose2SLAMValues; odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); constraint=gtsamPose2; % identity @@ -27,35 +27,33 @@ if fid < 0 error('Cannot open the file '); end -disp('Reading data from file...') -lines=textscan(fid,'%s','delimiter','\n'); +columns=textscan(fid,'%s','delimiter','\n'); fclose(fid); -n=size(lines{1},1); +lines=columns{1}; +n=size(lines,1); for i=1:n - text=cell2mat(lines{1}(i)); - if strcmp('VERTEX2',text(1:7)) - v = textscan(text,'%s %d %f %f %f',1); - initialEstimate.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5})); - elseif strcmp('EDGE2',text(1:5)) - e = textscan(text,'%s %d %d %f %f %f',1); + 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}, gtsamPose2(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.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), odometryNoise); end end %% Plot Initial Estimate -figure(1) -clf -plotTrajectory(initialEstimate,'r-*') -hold on +figure(1);clf +plot(initial.xs(),initial.ys(),'r-*'); axis equal addEquivalences=0; if addEquivalences %% Add equivalence constraints for i=1:n - text=cell2mat(lines{1}(i)); - if strcmp('EQUIV',text(1:5)) - equiv = textscan(text,'%s %d %d',1); + line_i=cell2mat(lines(i)); + if strcmp('EQUIV',line_i(1:5)) + equiv = textscan(line_i,'%s %d %d',1); graph.addOdometry(e{2}, e{3}, constraint, odometryNoise); end end @@ -63,8 +61,8 @@ if addEquivalences end %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); -plotTrajectory(result,'g-*') +result = graph.optimize(initial); +hold on; plot(result.xs(),result.ys(),'g-*') %% Plot Covariance Ellipses % marginals = graph.marginals(result); diff --git a/examples/matlab/plotTrajectory.m b/examples/matlab/plotTrajectory.m deleted file mode 100644 index e67663a55..000000000 --- a/examples/matlab/plotTrajectory.m +++ /dev/null @@ -1,10 +0,0 @@ -function plotTrajectory(values,color) -% plotTrajectory: plot the poses in a values object -% assumes keys are 0 to N-1, where N is values.size() -X=[];Y=[]; -for i=0:values.size()-1 - pose_i = values.pose(i); - X=[X;pose_i.x]; - Y=[Y;pose_i.y]; -end -plot(X,Y,color); diff --git a/gtsam.h b/gtsam.h index 2c6b5a13e..57f2a6e23 100644 --- a/gtsam.h +++ b/gtsam.h @@ -448,9 +448,12 @@ namespace pose2SLAM { class Values { Values(); void insertPose(size_t key, const gtsam::Pose2& pose); - size_t size() const; + size_t size() const; void print(string s) const; gtsam::Pose2 pose(size_t i); + Vector xs() const; + Vector ys() const; + Vector thetas() const; }; class Graph { diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index b08f0c305..a1e5932d9 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -31,6 +31,36 @@ namespace pose2SLAM { return x; } + /* ************************************************************************* */ + Vector Values::xs() const { + size_t j=0; + Vector result(size()); + ConstFiltered poses = filter(); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) + result(j++) = keyValue.value.x(); + return result; + } + + /* ************************************************************************* */ + Vector Values::ys() const { + size_t j=0; + Vector result(size()); + ConstFiltered poses = filter(); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) + result(j++) = keyValue.value.y(); + return result; + } + + /* ************************************************************************* */ + Vector Values::thetas() const { + size_t j=0; + Vector result(size()); + ConstFiltered poses = filter(); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) + result(j++) = keyValue.value.theta (); + return result; + } + /* ************************************************************************* */ void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) { sharedFactor factor(new Prior(i, p, model)); diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 84fbc528f..f438ce24d 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -41,13 +41,15 @@ namespace pose2SLAM { gtsam::Values(values) { } - // Convenience for MATLAB wrapper, which does not allow for identically named methods + /// insert a pose + void insertPose(Key i, const Pose2& pose) { insert(i, pose); } /// get a pose Pose2 pose(Key i) const { return at(i); } - /// insert a pose - void insertPose(Key i, const Pose2& pose) { insert(i, pose); } + Vector xs() const; ///< get all x coordinates in a matrix + Vector ys() const; ///< get all y coordinates in a matrix + Vector thetas() const; ///< get all orientations in a matrix }; /** diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 580d40e8a..7f329f1cb 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -41,7 +41,17 @@ static Matrix cov(Matrix_(3, 3, 0.0, 0.0, st*st )); static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); -//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3)); + +/* ************************************************************************* */ +TEST_UNSAFE( Pose2SLAM, XYT ) +{ + pose2SLAM::Values values; + values.insertPose(1,Pose2(1,2,3)); + values.insertPose(2,Pose2(4,5,6)); + EXPECT(assert_equal(Vector_(2,1.0,4.0),values.xs())); + EXPECT(assert_equal(Vector_(2,2.0,5.0),values.ys())); + EXPECT(assert_equal(Vector_(2,3.0,6.0-2*M_PI),values.thetas())); +} /* ************************************************************************* */ // Test constraint, small displacement