From 7d90e5040b9c6cb4d9a4557348012c17564559c3 Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Thu, 25 Feb 2021 20:51:33 -0500
Subject: [PATCH] add Align() for pose3pairs

---
 gtsam/geometry/Pose3.h         |  1 +
 gtsam/gtsam.i                  | 13 +++++++++++++
 python/CMakeLists.txt          |  1 +
 python/gtsam/specializations.h |  1 +
 4 files changed, 16 insertions(+)

diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 4c8973996..318baab3d 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -389,6 +389,7 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
 
 // Convenience typedef
 using Pose3Pair = std::pair<Pose3, Pose3>;
+using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
 
 // For MATLAB wrapper
 typedef std::vector<Pose3> Pose3Vector;
diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index b6b3ecac4..9bdbe298d 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -766,6 +766,16 @@ class Pose3 {
   void serialize() const;
 };
 
+#include <gtsam/geometry/Pose3.h>
+class Pose3Pairs
+{
+  Pose3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Pose3Pair at(size_t n) const;
+  void push_back(const gtsam::Pose3Pair& pose_pair);
+};
+
 // std::vector<gtsam::Pose3>
 #include <gtsam/geometry/Pose3.h>
 class Pose3Vector
@@ -1077,6 +1087,9 @@ class Similarity3 {
   Similarity3(const Matrix& T);
 
   static Similarity3 Align(const gtsam::PointPairs & abPointPairs);
+  static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
+
+
 };
 
 
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 3d6bf1702..e87fb70e7 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -42,6 +42,7 @@ set(ignore
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::PointPairs
+    gtsam::Pose3Pairs
     gtsam::Pose3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
index 04c1aa82c..2ed105ef2 100644
--- a/python/gtsam/specializations.h
+++ b/python/gtsam/specializations.h
@@ -7,6 +7,7 @@ py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
 #endif
 py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Point3Pair> >(m_, "PointPairs");
+py::bind_vector<std::vector<gtsam::Pose3Pair> >(m_, "Pose3Pairs");
 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::Pose2> > > >(m_, "BetweenFactorPose2s");