From fcd00450d3eec745f734120d3660d95cbfcbb342 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 2 Mar 2015 20:09:44 -0800 Subject: [PATCH] Formatting, use Point3/Rot3, resolved link error of operator*(Point3) --- gtsam_unstable/geometry/Similarity3.cpp | 52 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 18 +++++---- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index cfc508ac7..77c54bd46 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -17,36 +17,34 @@ #include -#include -#include #include - #include namespace gtsam { Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : - R_(R), t_(t), s_(s) { + R_(R), t_(t), s_(s) { } Similarity3::Similarity3() : - R_(), t_(), s_(1){ + R_(), t_(), s_(1) { } Similarity3::Similarity3(double s) : - s_ (s) { + s_(s) { } -Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : - R_ (R), t_ (t), s_ (s) { +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_); + return Pose3(R_, s_ * t_); } Similarity3 Similarity3::identity() { - return Similarity3(); } + return Similarity3(); +} //Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { // return Vector7(); @@ -61,35 +59,36 @@ bool Similarity3::operator==(const Similarity3& other) const { } bool Similarity3::equals(const Similarity3& sim, double tol) const { - return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) - && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); + return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) && s_ < (sim.s_ + tol) + && s_ > (sim.s_ - tol); } -Similarity3::Translation Similarity3::transform_from(const Translation& p) const { +Point3 Similarity3::transform_from(const Point3& p) const { return R_ * (s_ * p) + t_; } -Matrix7 Similarity3::AdjointMap() const{ +Matrix7 Similarity3::AdjointMap() const { const Matrix3 R = R_.matrix(); const Vector3 t = t_.vector(); Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; - adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1; + adj << s_ * R, A, -s_ * t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix< + double, 1, 6>::Zero(), 1; return adj; } -inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { +Point3 Similarity3::operator*(const Point3& p) const { return transform_from(p); } Similarity3 Similarity3::inverse() const { - Rotation Rt = R_.inverse(); - Translation sRt = R_.inverse() * (-s_ * t_); - return Similarity3(Rt, sRt, 1.0/s_); + 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_); + return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); } void Similarity3::print(const std::string& s) const { @@ -100,18 +99,20 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } -Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) { +Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, + ChartJacobian H) { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. - Rotation r; //Create a zero rotation to do our retraction. + Rot3 r; //Create a zero rotation to do our retraction. return Similarity3( // r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Translation(v.segment<3>(3)), // Retract the translation + Point3(v.segment<3>(3)), // Retract the translation 1.0 + v[6]); //finally, update scale using v[6] } -Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rotation r; //Create a zero rotation to do the retraction +Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, + ChartJacobian H) { + Rot3 r; //Create a zero rotation to do the retraction Vector7 v; v.head<3>() = r.localCoordinates(other.R_); v.segment<3>(3) = other.t_.vector(); @@ -121,4 +122,3 @@ Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobi } } - diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 89f1010c4..2a03288dd 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -32,13 +32,15 @@ class Pose3; */ class Similarity3: public LieGroup { - /** Pose Concept requirements */ + /// @name Pose Concept + /// @{ typedef Rot3 Rotation; typedef Point3 Translation; + /// @} private: - Rotation R_; - Translation t_; + Rot3 R_; + Point3 t_; double s_; public: @@ -52,7 +54,7 @@ public: Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rotation& R, const Translation& t, double s); + Similarity3(const Rot3& R, const Point3& t, double s); /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); @@ -80,10 +82,10 @@ public: /// Return the inverse Similarity3 inverse() const; - Translation transform_from(const Translation& p) const; + Point3 transform_from(const Point3& p) const; /** syntactic sugar for transform_from */ - inline Translation operator*(const Translation& p) const; + Point3 operator*(const Point3& p) const; Similarity3 operator*(const Similarity3& T) const; @@ -92,12 +94,12 @@ public: /// @{ /// Return a GTSAM rotation - const Rotation& rotation() const { + const Rot3& rotation() const { return R_; }; /// Return a GTSAM translation - const Translation& translation() const { + const Point3& translation() const { return t_; };