diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 56acab747..50de5cdf5 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -255,6 +255,12 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } +/* ************************************************************************* */ +Point3 Rot3::unrotate(const Point3& p) const { + // Eigen expression + return Point3(transpose_*p.vector()); // q = Rt*p +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index e0d3086e9..3f2f13057 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -313,12 +313,6 @@ Quaternion Rot3::toQuaternion() const { return Quaternion(rot_); } -/* ************************************************************************* */ -Point3 Rot3::unrotate(const Point3& p) const { - // Eigen expression - return Point3(transpose()*p.vector()); // q = Rt*p -} - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index b52acd017..4c79a6957 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -173,11 +173,6 @@ namespace gtsam { /* ************************************************************************* */ Quaternion Rot3::toQuaternion() const { return quaternion_; } - /* ************************************************************************* */ - Point3 Rot3::unrotate(const Point3& p) const { - return Point3(transpose()*p.vector()); // q = Rt*p - } - /* ************************************************************************* */ } // namespace gtsam