add Sim(3)-based alignment to the wrapper

gtsam/geometry/Point3.h

@@ -35,9 +35,11 @@ namespace gtsam {
 typedef Vector3 Point3;
 
 // Convenience typedef
-typedef std::pair<Point3, Point3> Point3Pair;
+using Point3Pair = std::pair<Point3, Point3>;
 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
 
+using Point3Pairs = std::vector<Point3Pair>;
+
 /// distance between two points
 GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
 	                          OptionalJacobian<1, 3> H1 = boost::none,

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;

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

@@ -13,9 +13,10 @@
  * @file   Similarity3.cpp
  * @brief  Implementation of Similarity3 transform
  * @author Paul Drews
+ * @author John Lambert
  */
 
-#include <gtsam_unstable/geometry/Similarity3.h>
+#include <gtsam/geometry/Similarity3.h>
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Manifold.h>
@@ -24,13 +25,12 @@
 namespace gtsam {
 
 using std::vector;
-using PointPairs = vector<Point3Pair>;
 
 namespace {
 /// Subtract centroids from point pairs.
-static PointPairs subtractCentroids(const PointPairs &abPointPairs,
+static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
                                     const Point3Pair &centroids) {
-  PointPairs d_abPointPairs;
+  Point3Pairs d_abPointPairs;
   for (const Point3Pair& abPair : abPointPairs) {
     Point3 da = abPair.first - centroids.first;
     Point3 db = abPair.second - centroids.second;
@@ -40,7 +40,7 @@ static PointPairs subtractCentroids(const PointPairs &abPointPairs,
 }
 
 /// Form inner products x and y and calculate scale.
-static const double calculateScale(const PointPairs &d_abPointPairs,
+static const double calculateScale(const Point3Pairs &d_abPointPairs,
                                    const Rot3 &aRb) {
   double x = 0, y = 0;
   Point3 da, db;
@@ -55,7 +55,7 @@ static const double calculateScale(const PointPairs &d_abPointPairs,
 }
 
 /// Form outer product H.
-static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
+static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
   Matrix3 H = Z_3x3;
   for (const Point3Pair& d_abPair : d_abPointPairs) {
     H += d_abPair.first * d_abPair.second.transpose();
@@ -64,7 +64,7 @@ static Matrix3 calculateH(const PointPairs &d_abPointPairs) {
 }
 
 /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
-static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
+static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
                          const Point3Pair &centroids) {
   const double s = calculateScale(d_abPointPairs, aRb);
   const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
@@ -73,7 +73,7 @@ static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb,
 
 /// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
-static Similarity3 alignGivenR(const PointPairs &abPointPairs,
+static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
                                const Rot3 &aRb) {
   auto centroids = means(abPointPairs);
   auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
@@ -154,7 +154,7 @@ Point3 Similarity3::operator*(const Point3& p) const {
   return transformFrom(p);
 }
 
-Similarity3 Similarity3::Align(const PointPairs &abPointPairs) {
+Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
   // Refer to Chapter 3 of
   // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
   if (abPointPairs.size() < 3)
@@ -174,18 +174,19 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
 
   // calculate rotation
   vector<Rot3> rotations;
-  PointPairs abPointPairs;
+  Point3Pairs abPointPairs;
   rotations.reserve(n);
   abPointPairs.reserve(n);
-  Pose3 wTa, wTb;
+  Pose3 aTi, bTi;
   for (const Pose3Pair &abPair : abPosePairs) {
-    std::tie(wTa, wTb) = abPair;
-    rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
-    abPointPairs.emplace_back(wTa.translation(), wTb.translation());
+    std::tie(aTi, bTi) = abPair;
+    Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+    rotations.emplace_back(aRb);
+    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
   }
-  const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
+  const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
 
-  return alignGivenR(abPointPairs, aRb);
+  return alignGivenR(abPointPairs, aRb_estimate);
 }
 
 Matrix4 Similarity3::wedge(const Vector7 &xi) {

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

@@ -13,6 +13,7 @@
  * @file   Similarity3.h
  * @brief  Implementation of Similarity3 transform
  * @author Paul Drews
+ * @author John Lambert
  */
 
 #pragma once
@@ -125,7 +126,10 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
 
   /**
-   *  Create Similarity3 by aligning at least two pose pairs
+   * Create Similarity3 by aligning at least two pose pairs
+   * Given a list of pairs in world frame w1, and a list of pairs in another world
+   * frame w2, will compute the best-fit Similarity3 transformation to align them.
+   * `w2Sw1` will returned for pairs [ (w2Ti1,w1Ti1), (w2Ti2,w1Ti2), (w2Ti3,w1Ti3) ]
    */
   GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
 

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>
@@ -260,18 +260,22 @@ TEST(Similarity3, GroupAction) {
 //******************************************************************************
 // Group action on Pose3
 TEST(Similarity3, GroupActionPose3) {
-  Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
-
-  // Create source poses
-  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
-  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
-
-  // Create destination poses
-  Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
-  Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
-
-  EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
-  EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
+  // Suppose we know the pose of the egovehicle in the world frame
+  Similarity3 wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
+
+  // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
+  // Suppose they are 3d cuboids detected by onboard sensor, in the egovehicle frame
+  Pose3 eTo1(Rot3(), Point3(0, 0, 0));
+  Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
+
+  // Create destination poses (two objects now living in the world/city "w" frame)
+  // Desired to place the objects into the world frame for tracking
+  Pose3 expected_wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
+  Pose3 expected_wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
+
+  // objects now live in the world frame, instead of in the egovehicle frame
+  EXPECT(assert_equal(expected_wTo1, wSe.transformFrom(eTo1)));
+  EXPECT(assert_equal(expected_wTo2, wSe.transformFrom(eTo2)));
 }
 
 // Test left group action compatibility.
@@ -346,26 +350,28 @@ TEST(Similarity3, AlignPoint3_3) {
 //******************************************************************************
 // Align with Pose3 Pairs
 TEST(Similarity3, AlignPose3) {
-  Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
+  Similarity3 expected_wSe(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
 
-  // Create source poses
-  Pose3 Ta1(Rot3(), Point3(0, 0, 0));
-  Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
+  // Create source poses (two objects o1 and o2 living in the egovehicle "e" frame)
+  // Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+  Pose3 eTo1(Rot3(), Point3(0, 0, 0));
+  Pose3 eTo2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
 
-  // Create destination poses
-  Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
-  Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
+  // Create destination poses (same two objects, but instead living in the world/city "w" frame)
+  Pose3 wTo1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
+  Pose3 wTo2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
 
-  Pose3Pair bTa1(make_pair(Tb1, Ta1));
-  Pose3Pair bTa2(make_pair(Tb2, Ta2));
+  Pose3Pair wTe1(make_pair(wTo1, eTo1));
+  Pose3Pair wTe2(make_pair(wTo2, eTo2));
 
-  vector<Pose3Pair> correspondences{bTa1, bTa2};
+  vector<Pose3Pair> correspondences{wTe1, wTe2};
 
   // Cayley transform cannot accommodate 180 degree rotations,
   // hence we only test for Expmap
 #ifdef GTSAM_ROT3_EXPMAP
-  Similarity3 actual_aSb = Similarity3::Align(correspondences);
-  EXPECT(assert_equal(expected_aSb, actual_aSb));
+  // Recover the transformation wSe (i.e. world_S_egovehicle)
+  Similarity3 actual_wSe = Similarity3::Align(correspondences);
+  EXPECT(assert_equal(expected_wSe, actual_wSe));
 #endif
 }
 

gtsam/gtsam.i

@@ -448,6 +448,16 @@ class Point3 {
   void serialize() const;
 };
 
+#include <gtsam/geometry/Point3.h>
+class Point3Pairs
+{
+  Point3Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point3Pair at(size_t n) const;
+  void push_back(const gtsam::Point3Pair& point_pair);
+};
+
 #include <gtsam/geometry/Rot2.h>
 class Rot2 {
   // Standard Constructors and Named Constructors
@@ -766,6 +776,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
@@ -1058,6 +1078,29 @@ class PinholeCamera {
   void serialize() const;
 };
 
+
+#include <gtsam/geometry/Similarity3.h>
+class Similarity3 {
+  // Standard Constructors
+  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);
+
+  gtsam::Pose3 transformFrom(const gtsam::Pose3& T);
+  static Similarity3 Align(const gtsam::Point3Pairs & abPointPairs);
+  static Similarity3 Align(const gtsam::Pose3Pairs & abPosePairs);
+
+  // Standard Interface
+  const Matrix matrix() const;
+  const gtsam::Rot3& rotation();
+  const gtsam::Point3& translation();
+  double scale() const;
+};
+
+
+
 // 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,8 @@ set(ignore
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
+    gtsam::Point3Pairs
+    gtsam::Pose3Pairs
     gtsam::Pose3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3

python/gtsam/specializations.h

@@ -6,6 +6,8 @@ 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_, "Point3Pairs");
+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");