diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d67b74812..b9ecf6f3b 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2816,7 +2816,19 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose2] +class BetweenFactorPose2s +{ + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); + // std::vector::shared_ptr> +// Ignored by pybind -> will be List[BetweenFactorPose3] class BetweenFactorPose3s { BetweenFactorPose3s(); @@ -2824,6 +2836,14 @@ class BetweenFactorPose3s gtsam::BetweenFactor* at(size_t i) const; void push_back(const gtsam::BetweenFactor* factor); }; +gtsam::BetweenFactorPose3s parse3DFactors(string filename); + +pair load3D(string filename); + +pair readG2o(string filename); +pair readG2o(string filename, bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); #include class InitializePose3 { @@ -2845,14 +2865,6 @@ class InitializePose3 { static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); }; -gtsam::BetweenFactorPose3s parse3DFactors(string filename); -pair load3D(string filename); - -pair readG2o(string filename); -pair readG2o(string filename, bool is3D); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - #include template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { @@ -3376,6 +3388,7 @@ namespace utilities { gtsam::KeySet createKeySet(string s, Vector I); Matrix extractPoint2(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values); + gtsam::Values allPose2s(gtsam::Values& values); Matrix extractPose2(const gtsam::Values& values); gtsam::Values allPose3s(gtsam::Values& values); Matrix extractPose3(const gtsam::Values& values); diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index a78f7a2d0..ffc279872 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -107,6 +107,11 @@ Matrix extractPoint3(const Values& values) { return result; } +/// Extract all Pose3 values +Values allPose2s(const Values& values) { + return values.filter(); +} + /// Extract all Pose2 values into a single matrix [x y theta] Matrix extractPose2(const Values& values) { Values::ConstFiltered poses = values.filter(); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index ba01e61f4..4bd7bc7e2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1284,7 +1284,13 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) { return initial; } -// To be deprecated when pybind wrapper is merged: +// Wrapper-friendly versions of parseFactors and parseFactors +BetweenFactorPose2s +parse2DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) { + return parseFactors(filename, model, maxIndex); +} + BetweenFactorPose3s parse3DFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index f6752eb34..7ceca00f4 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -341,7 +341,13 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db); */ GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db); -// To be deprecated when pybind wrapper is merged: +// Wrapper-friendly versions of parseFactors and parseFactors +using BetweenFactorPose2s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose2s +parse2DFactors(const std::string &filename, + const noiseModel::Diagonal::shared_ptr &model = nullptr, + size_t maxIndex = 0); + using BetweenFactorPose3s = std::vector::shared_ptr>; GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string &filename, diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 315f81f4e..23898f61d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -26,6 +26,7 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector gtsam::Pose3Vector diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index e23534c85..2dd4276a7 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -3,4 +3,5 @@ py::bind_vector >(m_, "KeyVector"); py::bind_vector > >(m_, "Point2Vector"); py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); \ No newline at end of file +py::bind_vector > > >(m_, "BetweenFactorPose3s"); +py::bind_vector > > >(m_, "BetweenFactorPose2s"); \ No newline at end of file