diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index b8fcd8ce0..cfc508ac7 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -12,45 +12,33 @@ /** * @file Similarity3.cpp * @brief Implementation of Similarity3 transform + * @author Paul Drews */ +#include + #include #include +#include + #include -#include namespace gtsam { -Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) { - R_ = R; - t_ = t; - s_ = s; -} - -/// Return the translation -const Vector3 Similarity3::t() const { - return t_.vector(); -} - -/// Return the rotation matrix -const Matrix3 Similarity3::R() const { - return R_.matrix(); +Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) : + R_(R), t_(t), s_(s) { } Similarity3::Similarity3() : - R_(), t_(), s_(1){ + R_(), t_(), s_(1){ } -/// Construct pure scaling -Similarity3::Similarity3(double s) { - s_ = s; +Similarity3::Similarity3(double s) : + s_ (s) { } -/// Construct from GTSAM types -Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) { - R_ = R; - t_ = t; - s_ = s; +Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) : + R_ (R), t_ (t), s_ (s) { } Similarity3::operator Pose3() const { @@ -58,30 +46,26 @@ Similarity3::operator Pose3() const { } Similarity3 Similarity3::identity() { - //std::cout << "Identity!" << std::endl; return Similarity3(); } -Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) { - std::cout << "Logmap!" << std::endl; - return Vector7(); -} - -Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { - std::cout << "Expmap!" << std::endl; - 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_); } -/// Compare with tolerance bool Similarity3::equals(const Similarity3& sim, double tol) const { - return rotation().equals(sim.rotation(), tol) && translation().equals(sim.translation(), tol) - && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); + return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) + && s_ < (sim.s_+tol) && s_ > (sim.s_-tol); } -Point3 Similarity3::transform_from(const Point3& p) const { +Similarity3::Translation Similarity3::transform_from(const Translation& p) const { return R_ * (s_ * p) + t_; } @@ -94,14 +78,13 @@ Matrix7 Similarity3::AdjointMap() const{ return adj; } -/** syntactic sugar for transform_from */ -inline Point3 Similarity3::operator*(const Point3& p) const { +inline Similarity3::Translation Similarity3::operator*(const Translation& p) const { return transform_from(p); } Similarity3 Similarity3::inverse() const { - Rot3 Rt = R_.inverse(); - Point3 sRt = R_.inverse() * (-s_ * t_); + Rotation Rt = R_.inverse(); + Translation sRt = R_.inverse() * (-s_ * t_); return Similarity3(Rt, sRt, 1.0/s_); } @@ -117,35 +100,18 @@ void Similarity3::print(const std::string& s) const { std::cout << "s: " << scale() << std::endl; } -/// Return the rotation matrix -Rot3 Similarity3::rotation() const { - return R_; -} - -/// Return the translation -Point3 Similarity3::translation() const { - return t_; -} - -/// Return the scale -double Similarity3::scale() const { - return s_; -} - -/// Update Similarity transform via 7-dim vector in tangent space 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. - Rot3 r; //Create a zero rotation to do our retraction. + Rotation r; //Create a zero rotation to do our retraction. return Similarity3( // r.retract(v.head<3>()), // retract rotation using v[0,1,2] - Point3(v.segment<3>(3)), // Retract the translation + Translation(v.segment<3>(3)), // Retract the translation 1.0 + v[6]); //finally, update scale using v[6] } -/// 7-dimensional vector v in tangent space that makes other = this->retract(v) Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) { - Rot3 r; //Create a zero rotation to do the retraction + Rotation 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(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 59425bb72..89f1010c4 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -12,19 +12,21 @@ /** * @file Similarity3.h * @brief Implementation of Similarity3 transform + * @author Paul Drews */ -#ifndef GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ -#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ - +#pragma once #include #include -#include +#include #include namespace gtsam { +// Forward declarations +class Pose3; + /** * 3D similarity transform */ @@ -35,55 +37,77 @@ class Similarity3: public LieGroup { typedef Point3 Translation; private: - Rot3 R_; - Point3 t_; + Rotation R_; + Translation t_; double s_; public: + /// @name Constructors + /// @{ + Similarity3(); /// Construct pure scaling Similarity3(double s); /// Construct from GTSAM types - Similarity3(const Rot3& R, const Point3& t, double s); + Similarity3(const Rotation& R, const Translation& t, double s); /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s); - /// Convert to a rigid body pose - operator Pose3() const; - - /// Return the translation - const Vector3 t() const; - - /// Return the rotation matrix - const Matrix3 R() const; - - static Similarity3 identity(); - - static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none); - - static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none); - - bool operator==(const Similarity3& other) const; + /// @} + /// @name Testable + /// @{ /// Compare with tolerance bool equals(const Similarity3& sim, double tol) const; - Point3 transform_from(const Point3& p) const; + /// Compare with standard tolerance + bool operator==(const Similarity3& other) const; - Matrix7 AdjointMap() const; + /// Print with optional string + void print(const std::string& s) const; + + /// @} + /// @name Group + /// @{ + + /// Return an identity transform + static Similarity3 identity(); + + /// Return the inverse + Similarity3 inverse() const; + + Translation transform_from(const Translation& p) const; /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const; - - Similarity3 inverse() const; + inline Translation operator*(const Translation& p) const; Similarity3 operator*(const Similarity3& T) const; - void print(const std::string& s) const; + /// @} + /// @name Standard interface + /// @{ + + /// Return a GTSAM rotation + const Rotation& rotation() const { + return R_; + }; + + /// Return a GTSAM translation + const Translation& translation() const { + return t_; + }; + + /// Return the scale + double scale() const { + return s_; + }; + + /// Convert to a rigid body pose + operator Pose3() const; /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes inline static size_t Dim() { @@ -95,14 +119,9 @@ public: return 7; }; - /// Return the rotation matrix - Rot3 rotation() const; - - /// Return the translation - Point3 translation() const; - - /// Return the scale - double scale() const; + /// @} + /// @name Chart + /// @{ /// Update Similarity transform via 7-dim vector in tangent space struct ChartAtOrigin { @@ -112,11 +131,20 @@ public: 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<> struct traits : public internal::LieGroupTraits {}; } - -#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 6741b7481..b2b5d5702 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -12,9 +12,13 @@ /** * @file testSimilarity3.cpp * @brief Unit tests for Similarity3 class + * @author Paul Drews */ #include +#include +#include +#include #include #include #include