sim3/rxso3: replace quaternion mutator by setter to ensure class inva…

…riant (strasdat#54)
dengchengcheng · Jan 17, 2017 · b333581 · b333581
1 parent 13e9be0
commit b333581
Show file tree

Hide file tree

Showing 3 changed files with 52 additions and 36 deletions.
diff --git a/sophus/rxso3.hpp b/sophus/rxso3.hpp
@@ -134,7 +134,7 @@ class RxSO3GroupBase {
   // Note: The first three Scalars represent the imaginary parts, while the
   // forth Scalar represent the real part.
   //
-  SOPHUS_FUNC Scalar* data() { return quaternion().coeffs().data(); }
+  SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); }
 
   // Const version of data() above.
   //
@@ -245,23 +245,25 @@ class RxSO3GroupBase {
       const RxSO3Group<Scalar>& other) {
     using std::sqrt;
 
-    quaternion() *= other.quaternion();
+    quaternion_nonconst() *= other.quaternion();
     Scalar scale = this->scale();
     if (scale < Constants<Scalar>::epsilon()) {
       SOPHUS_ENSURE(scale > 0, "Scale must be greater zero.");
       // Saturation to ensure class invariant.
-      quaternion().normalize();
-      quaternion().coeffs() *= sqrt(Constants<Scalar>::epsilon());
+      quaternion_nonconst().normalize();
+      quaternion_nonconst().coeffs() *= sqrt(Constants<Scalar>::epsilon());
     }
     return *this;
   }
 
-  // Mutator of quaternion.
+  // Sets non-zero quaternion
   //
-  // TODO: Replace this be a setter to ensure class invariant.
-  //
-  SOPHUS_FUNC QuaternionReference quaternion() {
-    return static_cast<Derived*>(this)->quaternion();
+  // Precondition: ``quat`` must not be close to zero.
+  SOPHUS_FUNC void setQuaternion(ConstQuaternionReference quat) {
+    SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
+                                           Constants<Scalar>::epsilon(),
+                  "Scale factor must be greater-equal epsilon.");
+    static_cast<Derived*>(this)->quaternion_nonconst() = quat;
   }
 
   // Accessor of quaternion.
@@ -299,8 +301,8 @@ class RxSO3GroupBase {
   SOPHUS_FUNC
   void setScale(const Scalar& scale) {
     using std::sqrt;
-    quaternion().normalize();
-    quaternion().coeffs() *= sqrt(scale);
+    quaternion_nonconst().normalize();
+    quaternion_nonconst().coeffs() *= sqrt(scale);
   }
 
   // Setter of quaternion using scaled rotation matrix ``sR``.
@@ -317,8 +319,8 @@ class RxSO3GroupBase {
                                       Constants<Scalar>::epsilon(),
                   "Scale factor must be greater-equal epsilon.");
     Scalar scale = sqrt(squared_scale);
-    quaternion() = sR / scale;
-    quaternion().coeffs() *= sqrt(scale);
+    quaternion_nonconst() = sR / scale;
+    quaternion_nonconst().coeffs() *= sqrt(scale);
   }
 
   ////////////////////////////////////////////////////////////////////////////
@@ -490,6 +492,13 @@ class RxSO3GroupBase {
                    static_cast<Scalar>(1. / 3.) *
                        (Omega(0, 0) + Omega(1, 1) + Omega(2, 2)));
   }
+
+ protected:
+  // Mutator of quaternion is private to ensure class invariant.
+  //
+  SOPHUS_FUNC QuaternionReference quaternion_nonconst() {
+    return static_cast<Derived*>(this)->quaternion_nonconst();
+  }
 };
 
 // RxSO3 default type - Constructors and default storage for RxSO3 Type.
@@ -498,18 +507,22 @@ class RxSO3Group : public RxSO3GroupBase<RxSO3Group<_Scalar, _Options>> {
   typedef RxSO3GroupBase<RxSO3Group<_Scalar, _Options>> Base;
 
  public:
-  typedef typename Eigen::internal::traits<SO3Group<_Scalar, _Options>>::Scalar
-      Scalar;
+  typedef
+      typename Eigen::internal::traits<RxSO3Group<_Scalar, _Options>>::Scalar
+          Scalar;
   typedef typename Eigen::internal::traits<
-      SO3Group<_Scalar, _Options>>::QuaternionType& QuaternionReference;
+      RxSO3Group<_Scalar, _Options>>::QuaternionType& QuaternionReference;
   typedef const typename Eigen::internal::traits<
-      SO3Group<_Scalar, _Options>>::QuaternionType& ConstQuaternionReference;
+      RxSO3Group<_Scalar, _Options>>::QuaternionType& ConstQuaternionReference;
 
   typedef typename Base::Transformation Transformation;
   typedef typename Base::Point Point;
   typedef typename Base::Tangent Tangent;
   typedef typename Base::Adjoint Adjoint;
 
+  // ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
+  friend class RxSO3GroupBase<RxSO3Group<_Scalar, _Options>>;
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   // Default constructor initialize quaternion to identity rotation and scale.
@@ -568,17 +581,15 @@ class RxSO3Group : public RxSO3GroupBase<RxSO3Group<_Scalar, _Options>> {
                   "Scale factor must be greater-equal epsilon.");
   }
 
-  // Mutator of quaternion
-  //
-  SOPHUS_FUNC
-  QuaternionReference quaternion() { return quaternion_; }
-
   // Accessor of quaternion.
   //
-  SOPHUS_FUNC
-  ConstQuaternionReference quaternion() const { return quaternion_; }
+  SOPHUS_FUNC ConstQuaternionReference quaternion() const {
+    return quaternion_;
+  }
 
  protected:
+  SOPHUS_FUNC QuaternionReference quaternion_nonconst() { return quaternion_; }
+
   Eigen::Quaternion<Scalar> quaternion_;
 };
 
@@ -609,22 +620,23 @@ class Map<Sophus::RxSO3Group<_Scalar>, _Options>
   typedef typename Base::Tangent Tangent;
   typedef typename Base::Adjoint Adjoint;
 
+  // ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
+  friend class Sophus::SO3GroupBase<Map<Sophus::SO3Group<_Scalar>, _Options>>;
+
   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
   using Base::operator*=;
   using Base::operator*;
 
   SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
 
-  // Mutator of quaternion
-  //
-  SOPHUS_FUNC QuaternionReference quaternion() { return quaternion_; }
-
   // Accessor of quaternion.
   //
   SOPHUS_FUNC
   ConstQuaternionReference quaternion() const { return quaternion_; }
 
  protected:
+  SOPHUS_FUNC QuaternionReference quaternion_nonconst() { return quaternion_; }
+
   Map<Eigen::Quaternion<Scalar>, _Options> quaternion_;
 };
 

diff --git a/sophus/sim3.hpp b/sophus/sim3.hpp
@@ -219,19 +219,17 @@ class Sim3GroupBase {
     return *this;
   }
 
-  // Mutator of quaternion.
+  // Setter of non-zero quaternion.
   //
-  SOPHUS_FUNC
-  typename Eigen::internal::traits<Derived>::RxSO3Type::QuaternionReference
-  quaternion() {
-    return rxso3().quaternion();
+  // Precondition: ``quat`` must not be close to zero.
+  //
+  SOPHUS_FUNC void setQuaternion(const Eigen::Quaternion<Scalar>& quat) {
+    rxso3().setQuaternion(quat);
   }
 
   // Accessor of quaternion.
   //
-  SOPHUS_FUNC typename Eigen::internal::traits<
-      Derived>::RxSO3Type::ConstQuaternionReference
-  quaternion() const {
+  SOPHUS_FUNC const Eigen::Quaternion<Scalar>& quaternion() const {
     return rxso3().quaternion();
   }
 

diff --git a/test/core/test_sim3.cpp b/test/core/test_sim3.cpp
@@ -108,6 +108,12 @@ void tests() {
   sim3.setScale(scale);
   SOPHUS_TEST_APPROX(passed, scale, sim3.scale(), Constants<Scalar>::epsilon(),
                      "setScale");
+
+  sim3.setQuaternion(sim3_vec[0].rxso3().quaternion());
+  SOPHUS_TEST_APPROX(passed, sim3_vec[0].rxso3().quaternion().coeffs(),
+                     sim3_vec[0].rxso3().quaternion().coeffs(),
+                     Constants<Scalar>::epsilon(), "setQuaternion");
+
   processTestResult(passed);
 }