From 66c9a63919a8e12b3d8f5593e57ec4ffaa4f7eb0 Mon Sep 17 00:00:00 2001
From: alexma3312 <alexma3312@gmail.com>
Date: Mon, 14 Sep 2020 00:20:50 -0400
Subject: [PATCH] Fix double computation.

---
 gtsam_unstable/geometry/Similarity3.cpp | 25 ++++++++++++++++++++++---
 gtsam_unstable/geometry/Similarity3.h   |  6 +++++-
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
index 1d127f8cf..5b35b9978 100644
--- a/gtsam_unstable/geometry/Similarity3.cpp
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -103,8 +103,7 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs
   std::tie(aCentroid, bCentroid) = mean(abPointPairs);
 
   // calculate scale
-  double x = 0;
-  double y = 0;
+  double x = 0, y = 0;
   Point3 aPoint, bPoint;
   for (const Point3Pair& abPair : abPointPairs) {
     std::tie(aPoint, bPoint) = abPair;
@@ -121,6 +120,24 @@ Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& abPointPairs
   return Similarity3(aRb, aTb, s);
 }
 
+// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
+Similarity3 Similarity3::AlignGivenR(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) {
+  // calculate scale
+  double x = 0, y = 0;
+  Point3 da, db;
+  for (const Point3Pair& d_abPair : d_abPointPairs) {
+    std::tie(da, db) = d_abPair;
+    Vector3 Rdb = aRb * db;
+    y += da.transpose() * Rdb;
+    x += Rdb.transpose() * Rdb;
+  }
+  const double s = y / x;
+
+  // calculate translation
+  const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s;
+  return Similarity3(aRb, aTb, s);
+}
+
 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
 Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
   const size_t n = abPointPairs.size();
@@ -133,17 +150,19 @@ Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
   // Add to form H matrix
   Matrix3 H = Z_3x3;
   Point3 aPoint, bPoint;
+  std::vector<Point3Pair> d_abPointPairs;
   for (const Point3Pair& abPair : abPointPairs) {
     std::tie(aPoint, bPoint) = abPair;
     Point3 da = aPoint - aCentroid;
     Point3 db = bPoint - bCentroid;
+    d_abPointPairs.emplace_back(da, db);
     H += da * db.transpose();
   }
 
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
   Rot3 aRb = Rot3::ClosestTo(H);
 
-  return AlignGivenR(abPointPairs, aRb);
+  return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid);
 }
 
 Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
index b618705de..9b34f3095 100644
--- a/gtsam_unstable/geometry/Similarity3.h
+++ b/gtsam_unstable/geometry/Similarity3.h
@@ -211,9 +211,13 @@ private:
   /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 
-  /// This methods estimates the similarity transform from point pairs, given a known or estimated rotation.
+  /// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
   static Similarity3 AlignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb);
 
+  /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
+  static Similarity3 AlignGivenR(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid);
+
+
   /// @}
 };