Formatting, use Point3/Rot3, resolved link error of operator*(Point3)

gtsam_unstable/geometry/Similarity3.cpp

@@ -17,36 +17,34 @@
 
 #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>
 
 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) :
+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 {
-  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)
+  return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol) && s_ < (sim.s_ + tol)
-      && s_ < (sim.s_+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<double, 3, 1>::Zero(), Eigen::Matrix<double, 1, 6>::Zero(), 1;
+  adj << s_ * R, A, -s_ * t, Z_3x3, R, Eigen::Matrix<double, 3, 1>::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();
+  Rot3 Rt = R_.inverse();
-  Translation sRt = R_.inverse() * (-s_ * t_);
+  Point3 sRt = R_.inverse() * (-s_ * t_);
-  return Similarity3(Rt, sRt, 1.0/s_);
+  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) {
+Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other,
-  Rotation r;  //Create a zero rotation to do the retraction
+    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
 }
 }
 
-

gtsam_unstable/geometry/Similarity3.h

@@ -32,13 +32,15 @@ class Pose3;
  */
 class Similarity3: public LieGroup<Similarity3, 7> {
 
-  /** Pose Concept requirements */
+  /// @name Pose Concept
+  /// @{
   typedef Rot3 Rotation;
   typedef Point3 Translation;
+  /// @}
 
 private:
-  Rotation R_;
+  Rot3 R_;
-  Translation t_;
+  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_;
   };