add Sim(3)-based alignment to the wrapper

gtsam_unstable/geometry/Similarity3.cpp → gtsam/geometry/Similarity3.cpp

@@ -15,7 +15,7 @@
  * @author Paul Drews
  */
 
-#include <gtsam_unstable/geometry/Similarity3.h>
+#include <gtsam/geometry/Similarity3.h>
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Manifold.h>
@@ -24,7 +24,6 @@
 namespace gtsam {
 
 using std::vector;
-using PointPairs = vector<Point3Pair>;
 
 namespace {
 /// Subtract centroids from point pairs.

gtsam_unstable/geometry/Similarity3.h → gtsam/geometry/Similarity3.h

@@ -30,6 +30,8 @@ namespace gtsam {
 // Forward declarations
 class Pose3;
 
+typedef std::vector<Point3Pair> PointPairs;
+
 /**
  * 3D similarity transform
  */

gtsam_unstable/geometry/tests/testSimilarity3.cpp → gtsam/geometry/tests/testSimilarity3.cpp

@@ -16,7 +16,7 @@
  * @author Zhaoyang Lv
  */
 
-#include <gtsam_unstable/geometry/Similarity3.h>
+#include <gtsam/geometry/Similarity3.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>

gtsam/gtsam.i

@@ -1058,6 +1058,29 @@ class PinholeCamera {
   void serialize() const;
 };
 
+
+#include <gtsam/geometry/Similarity3.h>
+class PointPairs
+{
+  PointPairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point3Pair at(size_t n) const;
+  void push_back(const gtsam::Point3Pair& point_pair);
+};
+
+class Similarity3 {
+  Similarity3();
+  Similarity3(double s);
+  Similarity3(const gtsam::Rot3& R, const gtsam::Point3& t, double s);
+  Similarity3(const Matrix& R, const Vector& t, double s);
+  Similarity3(const Matrix& T);
+
+  static Similarity3 Align(const gtsam::PointPairs & abPointPairs);
+};
+
+
+
 // Forward declaration of PinholeCameraCalX is defined here.
 #include <gtsam/geometry/SimpleCamera.h>
 // Some typedefs for common camera types

gtsam_unstable/gtsam_unstable.i

@@ -163,6 +163,7 @@ class BearingS2 {
   void serializable() const; // enabling serialization functionality
 };
 
+
 #include <gtsam_unstable/geometry/SimWall2D.h>
 class SimWall2D {
   SimWall2D();

python/CMakeLists.txt

@@ -41,6 +41,7 @@ set(ignore
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
+    gtsam::PointPairs
     gtsam::Pose3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3

python/gtsam/specializations.h

@@ -6,6 +6,7 @@ py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "
 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::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");