From 89dfdf082f58cbe0b2c45da42787d26bbc7fa7b0 Mon Sep 17 00:00:00 2001
From: John Lambert <johnwl@stanford.edu>
Date: Wed, 3 Mar 2021 00:16:08 -0500
Subject: [PATCH] PointPairs to Point3Pairs, and move to Point3.h

---
 gtsam/geometry/Point3.h        |  2 ++
 gtsam/geometry/Similarity3.cpp | 16 ++++++++--------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 001218ff7..3ff78442b 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -38,6 +38,8 @@ typedef Vector3 Point3;
 typedef std::pair<Point3, Point3> Point3Pair;
 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,
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index b086a5b5c..ea6ed2563 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -27,9 +27,9 @@ using std::vector;
 
 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;
@@ -39,7 +39,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;
@@ -54,7 +54,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();
@@ -63,7 +63,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;
@@ -72,7 +72,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);
@@ -153,7 +153,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)
@@ -173,7 +173,7 @@ 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;