From 53b370566e26501c78df4ba44e72bd16d8d92824 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 29 Apr 2022 17:53:07 -0400
Subject: [PATCH] remove Pose2/3 casts from Sim2/3

---
 gtsam/geometry/Similarity2.cpp           |  2 --
 gtsam/geometry/Similarity2.h             |  3 ---
 gtsam/geometry/Similarity3.cpp           |  4 ----
 gtsam/geometry/Similarity3.h             |  5 -----
 gtsam/geometry/tests/testSimilarity3.cpp | 22 +++++++++++-----------
 5 files changed, 11 insertions(+), 25 deletions(-)

diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp
index e6fa73ef3..3c780e907 100644
--- a/gtsam/geometry/Similarity2.cpp
+++ b/gtsam/geometry/Similarity2.cpp
@@ -211,6 +211,4 @@ Matrix3 Similarity2::matrix() const {
   return T;
 }
 
-Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
-
 }  // namespace gtsam
diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h
index 74ebf9640..a09dc1858 100644
--- a/gtsam/geometry/Similarity2.h
+++ b/gtsam/geometry/Similarity2.h
@@ -156,9 +156,6 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// Return the scale
   double scale() const { return s_; }
 
-  /// Convert to a rigid body pose (R, s*t)
-  operator Pose2() const;
-
   /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
   inline static size_t Dim() { return 4; }
 
diff --git a/gtsam/geometry/Similarity3.cpp b/gtsam/geometry/Similarity3.cpp
index 43cfaaa96..7fde974c5 100644
--- a/gtsam/geometry/Similarity3.cpp
+++ b/gtsam/geometry/Similarity3.cpp
@@ -290,8 +290,4 @@ Matrix4 Similarity3::matrix() const {
   return T;
 }
 
-Similarity3::operator Pose3() const {
-  return Pose3(R_, s_ * t_);
-}
-
 } // namespace gtsam
diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h
index 8799ba244..845d4c810 100644
--- a/gtsam/geometry/Similarity3.h
+++ b/gtsam/geometry/Similarity3.h
@@ -190,11 +190,6 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// Return the scale
   double scale() const { return s_; }
 
-  /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a
-  /// cast.
-  operator Pose3() const;
-
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
   inline static size_t Dim() { return 7; }
 
diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp
index 428422072..7a134f6ef 100644
--- a/gtsam/geometry/tests/testSimilarity3.cpp
+++ b/gtsam/geometry/tests/testSimilarity3.cpp
@@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
   Values result;
   result = LevenbergMarquardtOptimizer(graph, initial).optimize();
   //result.print("Optimized Estimate\n");
-  Pose3 p1, p2, p3, p4, p5;
-  p1 = Pose3(result.at<Similarity3>(X(1)));
-  p2 = Pose3(result.at<Similarity3>(X(2)));
-  p3 = Pose3(result.at<Similarity3>(X(3)));
-  p4 = Pose3(result.at<Similarity3>(X(4)));
-  p5 = Pose3(result.at<Similarity3>(X(5)));
+  Similarity3 p1, p2, p3, p4, p5;
+  p1 = result.at<Similarity3>(X(1));
+  p2 = result.at<Similarity3>(X(2));
+  p3 = result.at<Similarity3>(X(3));
+  p4 = result.at<Similarity3>(X(4));
+  p5 = result.at<Similarity3>(X(5));
 
-  //p1.print("Pose1");
-  //p2.print("Pose2");
-  //p3.print("Pose3");
-  //p4.print("Pose4");
-  //p5.print("Pose5");
+  //p1.print("Similarity1");
+  //p2.print("Similarity2");
+  //p3.print("Similarity3");
+  //p4.print("Similarity4");
+  //p5.print("Similarity5");
 
   Similarity3 expected(0.7);
   EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));