diff --git a/gtsam.h b/gtsam.h index 7ad901ae9..e1e3e0e93 100644 --- a/gtsam.h +++ b/gtsam.h @@ -674,6 +674,7 @@ class Values { void print(string s) const; static pose2SLAM::Values Circle(size_t n, double R); void insertPose(size_t key, const gtsam::Pose2& pose); + void updatePose(size_t key, const gtsam::Pose2& pose); gtsam::Pose2 pose(size_t i); Vector xs() const; Vector ys() const; @@ -723,6 +724,7 @@ class Values { void print(string s) const; static pose3SLAM::Values Circle(size_t n, double R); void insertPose(size_t key, const gtsam::Pose3& pose); + void updatePose(size_t key, const gtsam::Pose3& pose); gtsam::Pose3 pose(size_t i); Vector xs() const; Vector ys() const; @@ -770,6 +772,8 @@ class Values { void print(string s) const; void insertPose(size_t key, const gtsam::Pose2& pose); void insertPoint(size_t key, const gtsam::Point2& point); + void updatePose(size_t key, const gtsam::Pose2& pose); + void updatePoint(size_t key, const gtsam::Point2& point); gtsam::Pose2 pose(size_t key) const; gtsam::Point2 point(size_t key) const; }; @@ -871,6 +875,8 @@ class Values { Values(); void insertPose(size_t key, const gtsam::Pose3& pose); void insertPoint(size_t key, const gtsam::Point3& pose); + void updatePose(size_t key, const gtsam::Pose3& pose); + void updatePoint(size_t key, const gtsam::Point3& pose); size_t size() const; void print(string s) const; gtsam::Pose3 pose(size_t i); diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5d03cd646..01a9df763 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -78,7 +78,13 @@ namespace planarSLAM { /// insert a point void insertPoint(Key j, const Point2& point) { insert(j, point); } - }; + + /// update a pose + void updatePose(Key i, const Pose2& pose) { update(i, pose); } + + /// update a point + void updatePoint(Key j, const Point2& point) { update(j, point); } +}; /** * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 71710f823..778d844f2 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -58,6 +58,9 @@ namespace pose2SLAM { /// insert a pose void insertPose(Key i, const Pose2& pose) { insert(i, pose); } + /// update a pose + void updatePose(Key i, const Pose2& pose) { update(i, pose); } + /// get a pose Pose2 pose(Key i) const { return at(i); } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 448914f0d..9be699fd8 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -57,6 +57,9 @@ namespace pose3SLAM { /// insert a pose void insertPose(Key i, const Pose3& pose) { insert(i, pose); } + /// update a pose + void updatePose(Key i, const Pose3& pose) { update(i, pose); } + /// get a pose Pose3 pose(Key i) const { return at(i); } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 7674ed1e3..96a1e037c 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -66,6 +66,12 @@ namespace visualSLAM { /// insert a point void insertPoint(Key j, const Point3& point) { insert(j, point); } + /// update a pose + void updatePose(Key i, const Pose3& pose) { update(i, pose); } + + /// update a point + void updatePoint(Key j, const Point3& point) { update(j, point); } + /// get a pose Pose3 pose(Key i) const { return at(i); }