Make products & casts compatible with ceres::Jet

strasdat · Feb 15, 2022 · 5c97c7d · 5c97c7d
1 parent e10eb6e
commit 5c97c7d
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 8 deletions.
diff --git a/sophus/rxso2.hpp b/sophus/rxso2.hpp
@@ -134,7 +134,9 @@ class RxSO2Base {
   ///
   template <class NewScalarType>
   SOPHUS_FUNC RxSO2<NewScalarType> cast() const {
-    return RxSO2<NewScalarType>(complex().template cast<NewScalarType>());
+    typename RxSO2<NewScalarType>::ComplexType c =
+        complex().template cast<NewScalarType>();
+    return RxSO2<NewScalarType>(c);
   }
 
   /// This provides unsafe read/write access to internal data. RxSO(2) is

diff --git a/sophus/rxso3.hpp b/sophus/rxso3.hpp
@@ -241,8 +241,12 @@ class RxSO3Base {
       RxSO3Base<OtherDerived> const& other) const {
     using std::sqrt;
     using ResultT = ReturnScalar<OtherDerived>;
-    typename RxSO3Product<OtherDerived>::QuaternionType result_quaternion(
-        quaternion() * other.quaternion());
+    using QuaternionProductType =
+        typename RxSO3Product<OtherDerived>::QuaternionType;
+
+    QuaternionProductType result_quaternion(
+        Sophus::SO3<double>::QuaternionProduct<QuaternionProductType>(
+            quaternion(), other.quaternion()));
 
     ResultT scale = result_quaternion.squaredNorm();
     if (scale < Constants<ResultT>::epsilon()) {

diff --git a/sophus/so3.hpp b/sophus/so3.hpp
@@ -323,6 +323,17 @@ class SO3Base {
     return *this;
   }
 
+  template <typename QuaternionProductType, typename QuaternionTypeA,
+            typename QuaternionTypeB>
+  static QuaternionProductType QuaternionProduct(const QuaternionTypeA& a,
+                                                 const QuaternionTypeB& b) {
+    return QuaternionProductType(
+        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
+        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
+        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
+        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x());
+  }
+
   /// Group multiplication, which is rotation concatenation.
   ///
   template <typename OtherDerived>
@@ -335,11 +346,8 @@ class SO3Base {
     /// NOTE: We cannot use Eigen's Quaternion multiplication because it always
     /// returns a Quaternion of the same Scalar as this object, so it is not
     /// able to multiple Jets and doubles correctly.
-    return SO3Product<OtherDerived>(QuaternionProductType(
-        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
-        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
-        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
-        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()));
+    return SO3Product<OtherDerived>(
+        QuaternionProduct<QuaternionProductType>(a, b));
   }
 
   /// Group action on 3-points.