Make the code prettier.

gtsam_unstable/geometry/Similarity3.cpp

@@ -12,45 +12,33 @@
 /**
  * @file   Similarity3.cpp
  * @brief  Implementation of Similarity3 transform
+ * @author Paul Drews
  */
 
+#include <gtsam_unstable/geometry/Similarity3.h>
+
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+
 #include <gtsam/base/Manifold.h>
-#include <gtsam_unstable/geometry/Similarity3.h>
 
 namespace gtsam {
 
-Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) {
+Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
-  R_ = R;
+      R_(R), t_(t), s_(s) {
-  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() :
-        R_(), t_(), s_(1){
+            R_(), t_(), s_(1){
 }
 
-/// Construct pure scaling
+Similarity3::Similarity3(double s) :
-Similarity3::Similarity3(double s) {
+  s_ (s) {
-  s_ = s;
 }
 
-/// Construct from GTSAM types
+Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
-Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) {
+      R_ (R), t_ (t), s_ (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) {
+//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
-  std::cout << "Logmap!" << std::endl;
+//  return Vector7();
-  return Vector7();
+//}
-}
+//
-
+//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
-Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
+//  return Similarity3();
-  std::cout << "Expmap!" << std::endl;
+//}
-  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)
+  return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
-      && scale() < (sim.scale()+tol) && scale() > (sim.scale()-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 Similarity3::Translation Similarity3::operator*(const Translation& p) const {
-inline Point3 Similarity3::operator*(const Point3& p) const {
   return transform_from(p);
 }
 
 Similarity3 Similarity3::inverse() const {
-  Rot3 Rt = R_.inverse();
+  Rotation Rt = R_.inverse();
-  Point3 sRt = R_.inverse() * (-s_ * t_);
+  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();

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_
+#pragma once
-#define GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_
-
 
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Point3.h>
-#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
 
 namespace gtsam {
 
+// Forward declarations
+class Pose3;
+
 /**
  * 3D similarity transform
  */
@@ -35,55 +37,77 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   typedef Point3 Translation;
 
 private:
-  Rot3 R_;
+  Rotation R_;
-  Point3 t_;
+  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;
+  /// @name Testable
-
+  /// @{
-  /// 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;
 
   /// 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;
+  inline Translation operator*(const Translation& p) const;
-
-  Similarity3 inverse() 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;
+  /// @name Chart
-
+  /// @{
-  /// Return the translation
-  Point3 translation() const;
-
-  /// Return the scale
-  double scale() const;
 
   /// 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<Similarity3, 7>::inverse; // version with derivative
 };
 
 template<>
 struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
 }
-
-#endif /* GTSAM_UNSTABLE_GEOMETRY_SIMILARITY3_H_ */

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -12,9 +12,13 @@
 /**
  * @file   testSimilarity3.cpp
  * @brief  Unit tests for Similarity3 class
+ * @author Paul Drews
  */
 
 #include <gtsam_unstable/geometry/Similarity3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>