From 74605df6417f8febe1562482c3c4a539a70fff84 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 3 Mar 2015 07:42:31 -0800 Subject: [PATCH] Re-ordered methods in .h and .cpp to match them --- gtsam_unstable/geometry/Similarity3.cpp | 89 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 61 +++++++++-------- 2 files changed, 77 insertions(+), 73 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 662a39916..aafbf4c5f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -22,10 +22,6 @@ namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(s) { -} - Similarity3::Similarity3() : R_(), t_(), s_(1) { } @@ -38,24 +34,8 @@ Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : R_(R), t_(t), s_(s) { } -Similarity3::operator Pose3() const { - return Pose3(R_, s_ * t_); -} - -Similarity3 Similarity3::identity() { - return Similarity3(); -} - -//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { -// return Vector7(); -//} -// -//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { -// return Similarity3(); -//} - -bool Similarity3::operator==(const Similarity3& other) const { - return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } bool Similarity3::equals(const Similarity3& sim, double tol) const { @@ -63,6 +43,31 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const { && s_ > (sim.s_ - tol); } +bool Similarity3::operator==(const Similarity3& other) const { + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); +} + +void Similarity3::print(const std::string& s) const { + std::cout << std::endl; + std::cout << s; + rotation().print("R:\n"); + translation().print("t: "); + std::cout << "s: " << scale() << std::endl; +} + +Similarity3 Similarity3::identity() { + return Similarity3(); +} +Similarity3 Similarity3::operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +} + +Similarity3 Similarity3::inverse() const { + Rot3 Rt = R_.inverse(); + Point3 sRt = R_.inverse() * (-s_ * t_); + return Similarity3(Rt, sRt, 1.0 / s_); +} + Point3 Similarity3::transform_from(const Point3& p, // OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { if (H1) { @@ -78,11 +83,8 @@ Point3 Similarity3::transform_from(const Point3& p, // // 0001 000 1 1 000 1 1 } -const Matrix4 Similarity3::matrix() const { - Matrix4 T; - T.topRows<3>() << s_ * R_.matrix(), t_.vector(); - T.bottomRows<1>() << 0, 0, 0, 1; - return T; +Point3 Similarity3::operator*(const Point3& p) const { + return transform_from(p); } Matrix7 Similarity3::AdjointMap() const { @@ -96,26 +98,12 @@ Matrix7 Similarity3::AdjointMap() const { return adj; } -Point3 Similarity3::operator*(const Point3& p) const { - return transform_from(p); +Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { + return Vector7(); } -Similarity3 Similarity3::inverse() const { - Rot3 Rt = R_.inverse(); - Point3 sRt = R_.inverse() * (-s_ * t_); - return Similarity3(Rt, sRt, 1.0 / s_); -} - -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); -} - -void Similarity3::print(const std::string& s) const { - std::cout << std::endl; - std::cout << s; - rotation().print("R:\n"); - translation().print("t: "); - std::cout << "s: " << scale() << std::endl; +Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { + return Similarity3(); } Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, @@ -139,5 +127,16 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, v[6] = other.s_ - 1.0; return v; } + +const Matrix4 Similarity3::matrix() const { + Matrix4 T; + T.topRows<3>() << s_ * R_.matrix(), t_.vector(); + T.bottomRows<1>() << 0, 0, 0, 1; + return T; } +Similarity3::operator Pose3() const { + return Pose3(R_, s_ * t_); +} + +} diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 85a773498..82a3d8362 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -48,6 +48,7 @@ public: /// @name Constructors /// @{ + /// Default constructor Similarity3(); /// Construct pure scaling @@ -79,9 +80,16 @@ public: /// Return an identity transform static Similarity3 identity(); + /// Composition + Similarity3 operator*(const Similarity3& T) const; + /// Return the inverse Similarity3 inverse() const; + /// @} + /// @name Group action on Point3 + /// @{ + Point3 transform_from(const Point3& p, // OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; @@ -89,7 +97,31 @@ public: /** syntactic sugar for transform_from */ Point3 operator*(const Point3& p) const; - Similarity3 operator*(const Similarity3& T) const; + /// @} + /// @name Lie Group + /// @{ + + /// Not currently implemented, required because this is a lie group + static Vector7 Logmap(const Similarity3& s, // + OptionalJacobian<7, 7> Hm = boost::none); + + /// Not currently implemented, required because this is a lie group + static Similarity3 Expmap(const Vector7& v, // + OptionalJacobian<7, 7> Hm = boost::none); + + /// Update Similarity transform via 7-dim vector in tangent space + struct ChartAtOrigin { + static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); + + /// 7-dimensional vector v in tangent space that makes other = this->retract(v) + static Vector7 Local(const Similarity3& other, + ChartJacobian H = boost::none); + }; + + /// Project from one tangent space to another + Matrix7 AdjointMap() const; + + using LieGroup::inverse; /// @} /// @name Standard interface @@ -127,33 +159,6 @@ public: } /// @} - /// @name Chart - /// @{ - - /// Update Similarity transform via 7-dim vector in tangent space - struct ChartAtOrigin { - static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none); - - /// 7-dimensional vector v in tangent space that makes other = this->retract(v) - static Vector7 Local(const Similarity3& other, - ChartJacobian H = boost::none); - }; - - /// Project from one tangent space to another - Matrix7 AdjointMap() const; - - /// @} - /// @name Stubs - /// @{ - - /// Not currently implemented, required because this is a lie group - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = - boost::none); - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = - boost::none); - - using LieGroup::inverse; - // version with derivative }; template<>