Add unit dual quaternion

.travis.yml

@@ -188,6 +188,42 @@ matrix:
       - make
       - make test
 
+  #
+  # G++ 8
+  #
+  - os: linux
+    env:
+      - TEST="G++ 8"
+    addons:
+      apt:
+        sources:
+          - ubuntu-toolchain-r-test
+        packages:
+          - gcc-8
+          - g++-8
+    script:
+      - cmake -DCMAKE_CXX_COMPILER="g++-8" -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON ..
+      - make
+      - make test
+
+  #
+  # G++ 9
+  #
+  - os: linux
+    env:
+      - TEST="G++ 9"
+    addons:
+      apt:
+        sources:
+          - ubuntu-toolchain-r-test
+        packages:
+          - gcc-9
+          - g++-9
+    script:
+      - cmake -DCMAKE_CXX_COMPILER="g++-9" -DBUILD_EXAMPLES=ON -DBUILD_TESTING=ON ..
+      - make
+      - make test
+
   #
   # Clang 3.8
   #

examples/se2_localization.cpp

@@ -103,8 +103,6 @@
 
 #include "manif/SE2.h"
 
-#include <Eigen/Dense>
-
 #include <vector>
 
 #include <iostream>

examples/se2_sam.cpp

@@ -180,9 +180,6 @@
 // manif
 #include "manif/SE2.h"
 
-// Eigen
-#include <Eigen/Dense>
-
 // Std
 #include <vector>
 #include <map>

examples/se3_localization.cpp

@@ -103,8 +103,6 @@
 
 #include "manif/SE3.h"
 
-#include <Eigen/Dense>
-
 #include <vector>
 
 #include <iostream>

examples/se3_sam.cpp

@@ -180,9 +180,6 @@
 // manif
 #include "manif/SE3.h"
 
-// Eigen
-#include <Eigen/Dense>
-
 // Std
 #include <vector>
 #include <map>

examples/se3_sam_selfcalib.cpp

@@ -182,9 +182,6 @@
 // manif
 #include "manif/SE3.h"
 
-// Eigen
-#include <Eigen/Dense>
-
 // Std
 #include <vector>
 #include <map>

external/lt/lt/optional.hpp

@@ -391,7 +391,8 @@ struct optional_copy_base<T, false> : optional_operations_base<T> {
   using optional_operations_base<T>::optional_operations_base;
 
   optional_copy_base() = default;
-  optional_copy_base(const optional_copy_base &rhs) {
+  optional_copy_base(const optional_copy_base &rhs)
+    : optional_operations_base<T>() {
     if (rhs.has_value()) {
       this->construct(rhs.get());
     } else {

include/manif/DHu.h

@@ -0,0 +1,16 @@
+#ifndef _MANIF_DHU_H_
+#define _MANIF_DHU_H_
+
+#include "manif/impl/macro.h"
+#include "manif/impl/lie_group_base.h"
+#include "manif/impl/tangent_base.h"
+
+#include "manif/impl/dhu/DHu_properties.h"
+#include "manif/impl/dhu/DHu_base.h"
+#include "manif/impl/dhu/DHuTangent_base.h"
+#include "manif/impl/dhu/DHu.h"
+#include "manif/impl/dhu/DHuTangent.h"
+#include "manif/impl/dhu/DHu_map.h"
+#include "manif/impl/dhu/DHuTangent_map.h"
+
+#endif // _MANIF_DHU_H_

include/manif/impl/assignment_assert.h

@@ -0,0 +1,24 @@
+#ifndef _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_
+#define _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_
+
+namespace manif {
+namespace internal {
+
+template <typename Derived>
+struct AssignmentEvaluatorImpl
+{
+  template <typename T> static void run_impl(const T&) { }
+};
+
+template <typename Derived>
+struct AssignmentEvaluator : AssignmentEvaluatorImpl<Derived>
+{
+  using Base = AssignmentEvaluatorImpl<Derived>;
+
+  template <typename T> void run(T&& t) { Base::run_impl(std::forward<T>(t)); }
+};
+
+} // namespace internal
+} // namespace manif
+
+#endif // _MANIF_MANIF_IMPL_ASSIGNMENT_ASSERT_H_

include/manif/impl/dhu/DHu.h

@@ -0,0 +1,227 @@
+#ifndef _MANIF_MANIF_DHU_H_
+#define _MANIF_MANIF_DHU_H_
+
+#include "manif/impl/dhu/DHu_base.h"
+
+namespace manif {
+
+// Forward declare for type traits specialization
+
+template <typename _Scalar> struct DHu;
+template <typename _Scalar> struct DHuTangent;
+
+namespace internal {
+
+//! Traits specialization
+template <typename _Scalar>
+struct traits<DHu<_Scalar>>
+{
+  using Scalar = _Scalar;
+
+  using LieGroup = DHu<_Scalar>;
+  using Tangent  = DHuTangent<_Scalar>;
+
+  using Base = DHuBase<DHu<_Scalar>>;
+
+  static constexpr int Dim = LieGroupProperties<Base>::Dim;
+  static constexpr int DoF = LieGroupProperties<Base>::DoF;
+  static constexpr int RepSize = 8;
+
+  using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
+
+  using Jacobian       = Eigen::Matrix<Scalar, DoF, DoF>;
+  using Transformation = Eigen::Matrix<Scalar, 4, 4>;
+  using Rotation       = Eigen::Matrix<Scalar, Dim, Dim>;
+  using Translation    = Eigen::Matrix<Scalar, Dim, 1>;
+  using Vector         = Eigen::Matrix<Scalar, Dim, 1>;
+};
+
+} // namespace internal
+} // namespace manif
+
+namespace manif {
+
+//
+// LieGroup
+//
+
+/**
+ * @brief Represent an element of DHu.
+ */
+template <typename _Scalar>
+struct DHu : DHuBase<DHu<_Scalar>>
+{
+private:
+
+  using Base = DHuBase<DHu<_Scalar>>;
+  using Type = DHu<_Scalar>;
+
+protected:
+
+  using Base::derived;
+
+public:
+
+  MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
+
+  MANIF_COMPLETE_GROUP_TYPEDEF
+  using Translation = typename Base::Translation;
+  using Quaternion = Eigen::Quaternion<Scalar>;
+  using Real = Quaternion;
+  using Dual = Quaternion;
+
+  MANIF_INHERIT_GROUP_API
+  using Base::transform;
+  using Base::rotation;
+  using Base::normalize;
+
+  DHu()  = default;
+  ~DHu() = default;
+
+  MANIF_COPY_CONSTRUCTOR(DHu)
+
+  template <typename _DerivedOther>
+  DHu(const LieGroupBase<_DerivedOther>& o);
+
+  MANIF_GROUP_ASSIGN_OP(DHu)
+
+  /**
+   * @brief Constructor given a translation and a unit quaternion.
+   * @param[in] t A translation vector.
+   * @param[in] q A unit quaternion.
+   * @throws manif::invalid_argument on un-normalized complex number.
+   */
+  DHu(const Real& real, const Dual& dual);
+
+  /**
+   * @brief Constructor given a translation and a unit quaternion.
+   * @param[in] t A translation vector.
+   * @param[in] q A unit quaternion.
+   * @throws manif::invalid_argument on un-normalized complex number.
+   */
+  // DHu(const Translation& t,
+  //     const Eigen::Quaternion<Scalar>& q);
+
+  /**
+   * @brief Constructor given a translation and an angle axis.
+   * @param[in] t A translation vector.
+   * @param[in] angle_axis An angle-axis.
+   */
+  // DHu(const Translation& t,
+  //     const Eigen::AngleAxis<Scalar>& angle_axis);
+
+  /**
+   * @brief Constructor given a translation and SO3 element.
+   * @param[in] t A translation vector.
+   * @param[in] SO3 An element of SO3.
+   */
+  // DHu(const Translation& t,
+  //     const SO3<Scalar>& SO3);
+
+  /**
+   * @brief Constructor given translation components and
+   * roll-pitch-yaw angles.
+   * @param[in] x The x component of the translation.
+   * @param[in] y The y component of the translation.
+   * @param[in] z The z component of the translation.
+   * @param[in] roll The roll angle.
+   * @param[in] pitch The pitch angle.
+   * @param[in] yaw The yaw angle.
+   */
+  // DHu(const Scalar x, const Scalar y, const Scalar z,
+      // const Scalar roll, const Scalar pitch, const Scalar yaw);
+
+  /**
+   * @brief Constructor from a 3D Eigen::Isometry<Scalar>
+   * @param[in] h an isometry object from Eigen
+   *
+   * Isometry is a typedef from Eigen::Transform, in which the linear part is assumed a rotation matrix.
+   * This is used to speed up certain methods of Transform, especially inverse().
+   */
+  DHu(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h);
+
+  // LieGroup common API
+
+  DataType& coeffs();
+  const DataType& coeffs() const;
+
+  // DHu specific API
+
+protected:
+
+  DataType data_;
+};
+
+MANIF_EXTRA_GROUP_TYPEDEF(DHu)
+
+template <typename _Scalar>
+template <typename _DerivedOther>
+DHu<_Scalar>::DHu(const LieGroupBase<_DerivedOther>& o)
+  : DHu(o.coeffs())
+{
+  //
+}
+
+template <typename _Scalar>
+DHu<_Scalar>::DHu(const Real& real, const Dual& dual)
+  : DHu((DataType() << real.coeffs(), dual.coeffs() ).finished())
+{
+  //
+}
+
+// template <typename _Scalar>
+// DHu<_Scalar>::DHu(const Translation& t, const Eigen::Quaternion<Scalar>& q)
+//   : DHu((DataType() << t, q.coeffs() ).finished())
+// {
+//   //
+// }
+
+// template <typename _Scalar>
+// DHu<_Scalar>::DHu(const Translation& t, const Eigen::AngleAxis<Scalar>& a)
+//   : DHu(t, Quaternion(a))
+// {
+//   //
+// }
+
+// template <typename _Scalar>
+// DHu<_Scalar>::DHu(const Scalar x, const Scalar y, const Scalar z,
+//                   const Scalar roll, const Scalar pitch, const Scalar yaw)
+//   : DHu(Translation(x,y,z), Eigen::Quaternion<Scalar>(
+//       Eigen::AngleAxis<Scalar>(yaw,   Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
+//       Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
+//       Eigen::AngleAxis<Scalar>(roll,  Eigen::Matrix<Scalar, 3, 1>::UnitX())  ))
+// {
+//   //
+// }
+
+// template <typename _Scalar>
+// DHu<_Scalar>::DHu(const Translation& t, const SO3<Scalar>& so3)
+//   : DHu(t, so3.quat())
+// {
+//   //
+// }
+
+// template <typename _Scalar>
+// DHu<_Scalar>::DHu(const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h)
+//   : DHu(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()))
+// {
+//   //
+// }
+
+template <typename _Scalar>
+typename DHu<_Scalar>::DataType&
+DHu<_Scalar>::coeffs()
+{
+  return data_;
+}
+
+template <typename _Scalar>
+const typename DHu<_Scalar>::DataType&
+DHu<_Scalar>::coeffs() const
+{
+  return data_;
+}
+
+} // namespace manif
+
+#endif // _MANIF_MANIF_DHU_H_