Added more support for Pose2 data

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<gtsam::BetweenFactor<Pose2>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose2]
+class BetweenFactorPose2s
+{
+  BetweenFactorPose2s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
+};
+gtsam::BetweenFactorPose2s parse2DFactors(string filename);
+
 // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose3]
 class BetweenFactorPose3s
 {
   BetweenFactorPose3s();
@@ -2824,6 +2836,14 @@ class BetweenFactorPose3s
   gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
   void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
 };
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& estimate, string filename);
 
 #include <gtsam/slam/InitializePose3.h>
 class InitializePose3 {
@@ -2845,14 +2865,6 @@ class InitializePose3 {
   static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
 };
 
-gtsam::BetweenFactorPose3s parse3DFactors(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
-void writeG2o(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& estimate, string filename);
-
 #include <gtsam/slam/KarcherMeanFactor-inl.h>
 template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
 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);

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<Pose2>();
+}
+
 /// Extract all Pose2 values into a single matrix [x y theta]
 Matrix extractPose2(const Values& values) {
   Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();

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<Pose2> and parseFactors<Pose2>
+BetweenFactorPose2s
+parse2DFactors(const std::string &filename,
+               const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
+  return parseFactors<Pose2>(filename, model, maxIndex);
+}
+
 BetweenFactorPose3s
 parse3DFactors(const std::string &filename,
                const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {

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<Pose2> and parseFactors<Pose2>
+using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::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<BetweenFactor<Pose3>::shared_ptr>;
 GTSAM_EXPORT BetweenFactorPose3s
 parse3DFactors(const std::string &filename,

python/CMakeLists.txt

@@ -26,6 +26,7 @@ set(ignore
     gtsam::ISAM2ThresholdMapValue
     gtsam::FactorIndices
     gtsam::FactorIndexSet
+    gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::Pose3Vector

python/gtsam/specializations.h

@@ -3,4 +3,5 @@
 py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
 py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
-py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
+py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
+py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");