--- /dev/null
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2020, Huawei Technologies Co., Ltd. All rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Liangqian Kong <kongliangqian@huawei.com>
+// Longbu Wang <wanglongbu@huawei.com>
+#ifndef OPENCV_CORE_DUALQUATERNION_HPP
+#define OPENCV_CORE_DUALQUATERNION_HPP
+
+#include <opencv2/core/quaternion.hpp>
+#include <opencv2/core/affine.hpp>
+
+namespace cv{
+//! @addtogroup core
+//! @{
+
+template <typename _Tp> class DualQuat;
+template <typename _Tp> std::ostream& operator<<(std::ostream&, const DualQuat<_Tp>&);
+
+/**
+ * Dual quaternions were introduced to describe rotation together with translation while ordinary
+ * quaternions can only describe rotation. It can be used for shortest path pose interpolation,
+ * local pose optimization or volumetric deformation. More details can be found
+ * - https://en.wikipedia.org/wiki/Dual_quaternion
+ * - ["A beginners guide to dual-quaternions: what they are, how they work, and how to use them for 3D character hierarchies", Ben Kenwright, 2012](https://borodust.org/public/shared/beginner_dual_quats.pdf)
+ * - ["Dual Quaternions", Yan-Bin Jia, 2013](http://web.cs.iastate.edu/~cs577/handouts/dual-quaternion.pdf)
+ * - ["Geometric Skinning with Approximate Dual Quaternion Blending", Kavan, 2008](https://www.cs.utah.edu/~ladislav/kavan08geometric/kavan08geometric)
+ * - http://rodolphe-vaillant.fr/?e=29
+ *
+ * A unit dual quaternion can be classically represented as:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * \sigma &= \left(r+\frac{\epsilon}{2}tr\right)\\
+ * &= [w, x, y, z, w\_, x\_, y\_, z\_]
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * where \f$r, t\f$ represents the rotation (ordinary unit quaternion) and translation (pure ordinary quaternion) respectively.
+ *
+ * A general dual quaternions which consist of two quaternions is usually represented in form of:
+ * \f[
+ * \sigma = p + \epsilon q
+ * \f]
+ * where the introduced dual unit \f$\epsilon\f$ satisfies \f$\epsilon^2 = \epsilon^3 =...=0\f$, and \f$p, q\f$ are quaternions.
+ *
+ * Alternatively, dual quaternions can also be interpreted as four components which are all [dual numbers](https://www.cs.utah.edu/~ladislav/kavan08geometric/kavan08geometric):
+ * \f[
+ * \sigma = \hat{q}_w + \hat{q}_xi + \hat{q}_yj + \hat{q}_zk
+ * \f]
+ * If we set \f$\hat{q}_x, \hat{q}_y\f$ and \f$\hat{q}_z\f$ equal to 0, a dual quaternion is transformed to a dual number. see normalize().
+ *
+ * If you want to create a dual quaternion, you can use:
+ *
+ * ```
+ * using namespace cv;
+ * double angle = CV_PI;
+ *
+ * // create from eight number
+ * DualQuatd dq1(1, 2, 3, 4, 5, 6, 7, 8); //p = [1,2,3,4]. q=[5,6,7,8]
+ *
+ * // create from Vec
+ * Vec<double, 8> v{1,2,3,4,5,6,7,8};
+ * DualQuatd dq_v{v};
+ *
+ * // create from two quaternion
+ * Quatd p(1, 2, 3, 4);
+ * Quatd q(5, 6, 7, 8);
+ * DualQuatd dq2 = DualQuatd::createFromQuat(p, q);
+ *
+ * // create from an angle, an axis and a translation
+ * Vec3d axis{0, 0, 1};
+ * Vec3d trans{3, 4, 5};
+ * DualQuatd dq3 = DualQuatd::createFromAngleAxisTrans(angle, axis, trans);
+ *
+ * // If you already have an instance of class Affine3, then you can use
+ * Affine3d R = dq3.toAffine3();
+ * DualQuatd dq4 = DualQuatd::createFromAffine3(R);
+ *
+ * // or create directly by affine transformation matrix Rt
+ * // see createFromMat() in detail for the form of Rt
+ * Matx44d Rt = dq3.toMat();
+ * DualQuatd dq5 = DualQuatd::createFromMat(Rt);
+ *
+ * // Any rotation + translation movement can
+ * // be expressed as a rotation + translation around the same line in space (expressed by Plucker
+ * // coords), and here's a way to represent it this way.
+ * Vec3d axis{1, 1, 1}; // axis will be normalized in createFromPitch
+ * Vec3d trans{3, 4 ,5};
+ * axis = axis / std::sqrt(axis.dot(axis));// The formula for computing moment that I use below requires a normalized axis
+ * Vec3d moment = 1.0 / 2 * (trans.cross(axis) + axis.cross(trans.cross(axis)) *
+ * std::cos(rotation_angle / 2) / std::sin(rotation_angle / 2));
+ * double d = trans.dot(qaxis);
+ * DualQuatd dq6 = DualQuatd::createFromPitch(angle, d, axis, moment);
+ * ```
+ *
+ * A point \f$v=(x, y, z)\f$ in form of dual quaternion is \f$[1+\epsilon v]=[1,0,0,0,0,x,y,z]\f$.
+ * The transformation of a point \f$v_1\f$ to another point \f$v_2\f$ under the dual quaternion \f$\sigma\f$ is
+ * \f[
+ * 1 + \epsilon v_2 = \sigma * (1 + \epsilon v_1) * \sigma^{\star}
+ * \f]
+ * where \f$\sigma^{\star}=p^*-\epsilon q^*.\f$
+ *
+ * A line in the \f$Pl\ddot{u}cker\f$ coordinates \f$(\hat{l}, m)\f$ defined by the dual quaternion \f$l=\hat{l}+\epsilon m\f$.
+ * To transform a line, \f[l_2 = \sigma * l_1 * \sigma^*,\f] where \f$\sigma=r+\frac{\epsilon}{2}rt\f$ and
+ * \f$\sigma^*=p^*+\epsilon q^*\f$.
+ *
+ * To extract the Vec<double, 8> or Vec<float, 8>, see toVec();
+ *
+ * To extract the affine transformation matrix, see toMat();
+ *
+ * To extract the instance of Affine3, see toAffine3();
+ *
+ * If two quaternions \f$q_0, q_1\f$ are needed to be interpolated, you can use sclerp()
+ * ```
+ * DualQuatd::sclerp(q0, q1, t)
+ * ```
+ * or dqblend().
+ * ```
+ * DualQuatd::dqblend(q0, q1, t)
+ * ```
+ * With more than two dual quaternions to be blended, you can use generalize linear dual quaternion blending
+ * with the corresponding weights, i.e. gdqblend().
+ *
+ */
+template <typename _Tp>
+class CV_EXPORTS DualQuat{
+ static_assert(std::is_floating_point<_Tp>::value, "Dual quaternion only make sense with type of float or double");
+ using value_type = _Tp;
+
+public:
+ static constexpr _Tp CV_DUAL_QUAT_EPS = (_Tp)1.e-6;
+
+ DualQuat();
+
+ /**
+ * @brief create from eight same type numbers.
+ */
+ DualQuat(const _Tp w, const _Tp x, const _Tp y, const _Tp z, const _Tp w_, const _Tp x_, const _Tp y_, const _Tp z_);
+
+ /**
+ * @brief create from a double or float vector.
+ */
+ DualQuat(const Vec<_Tp, 8> &q);
+
+ _Tp w, x, y, z, w_, x_, y_, z_;
+
+ /**
+ * @brief create Dual Quaternion from two same type quaternions p and q.
+ * A Dual Quaternion \f$\sigma\f$ has the form:
+ * \f[\sigma = p + \epsilon q\f]
+ * where p and q are defined as follows:
+ * \f[\begin{equation}
+ * \begin{split}
+ * p &= w + x\boldsymbol{i} + y\boldsymbol{j} + z\boldsymbol{k}\\
+ * q &= w\_ + x\_\boldsymbol{i} + y\_\boldsymbol{j} + z\_\boldsymbol{k}.
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * The p and q are the real part and dual part respectively.
+ * @param realPart a quaternion, real part of dual quaternion.
+ * @param dualPart a quaternion, dual part of dual quaternion.
+ * @sa Quat
+ */
+ static DualQuat<_Tp> createFromQuat(const Quat<_Tp> &realPart, const Quat<_Tp> &dualPart);
+
+ /**
+ * @brief create a dual quaternion from a rotation angle \f$\theta\f$, a rotation axis
+ * \f$\boldsymbol{u}\f$ and a translation \f$\boldsymbol{t}\f$.
+ * It generates a dual quaternion \f$\sigma\f$ in the form of
+ * \f[\begin{equation}
+ * \begin{split}
+ * \sigma &= r + \frac{\epsilon}{2}\boldsymbol{t}r \\
+ * &= [\cos(\frac{\theta}{2}), \boldsymbol{u}\sin(\frac{\theta}{2})]
+ * + \frac{\epsilon}{2}[0, \boldsymbol{t}][[\cos(\frac{\theta}{2}),
+ * \boldsymbol{u}\sin(\frac{\theta}{2})]]\\
+ * &= \cos(\frac{\theta}{2}) + \boldsymbol{u}\sin(\frac{\theta}{2})
+ * + \frac{\epsilon}{2}(-(\boldsymbol{t} \cdot \boldsymbol{u})\sin(\frac{\theta}{2})
+ * + \boldsymbol{t}\cos(\frac{\theta}{2}) + \boldsymbol{u} \times \boldsymbol{t} \sin(\frac{\theta}{2})).
+ * \end{split}
+ * \end{equation}\f]
+ * @param angle rotation angle.
+ * @param axis rotation axis.
+ * @param translation a vector of length 3.
+ * @note Axis will be normalized in this function. And translation is applied
+ * after the rotation. Use @ref createFromQuat(r, r * t / 2) to create a dual quaternion
+ * which translation is applied before rotation.
+ * @sa Quat
+ */
+ static DualQuat<_Tp> createFromAngleAxisTrans(const _Tp angle, const Vec<_Tp, 3> &axis, const Vec<_Tp, 3> &translation);
+
+ /**
+ * @brief Transform this dual quaternion to an affine transformation matrix \f$M\f$.
+ * Dual quaternion consists of a rotation \f$r=[a,b,c,d]\f$ and a translation \f$t=[\Delta x,\Delta y,\Delta z]\f$. The
+ * affine transformation matrix \f$M\f$ has the form
+ * \f[
+ * \begin{bmatrix}
+ * 1-2(e_2^2 +e_3^2) &2(e_1e_2-e_0e_3) &2(e_0e_2+e_1e_3) &\Delta x\\
+ * 2(e_0e_3+e_1e_2) &1-2(e_1^2+e_3^2) &2(e_2e_3-e_0e_1) &\Delta y\\
+ * 2(e_1e_3-e_0e_2) &2(e_0e_1+e_2e_3) &1-2(e_1^2-e_2^2) &\Delta z\\
+ * 0&0&0&1
+ * \end{bmatrix}
+ * \f]
+ * if A is a matrix consisting of n points to be transformed, this could be achieved by
+ * \f[
+ * new\_A = M * A
+ * \f]
+ * where A has the form
+ * \f[
+ * \begin{bmatrix}
+ * x_0& x_1& x_2&...&x_n\\
+ * y_0& y_1& y_2&...&y_n\\
+ * z_0& z_1& z_2&...&z_n\\
+ * 1&1&1&...&1
+ * \end{bmatrix}
+ * \f]
+ * where the same subscript represent the same point. The size of A should be \f$[4,n]\f$.
+ * and the same size for matrix new_A.
+ * @param _R 4x4 matrix that represents rotations and translation.
+ * @note Translation is applied after the rotation. Use createFromQuat(r, r * t / 2) to create
+ * a dual quaternion which translation is applied before rotation.
+ */
+ static DualQuat<_Tp> createFromMat(InputArray _R);
+
+ /**
+ * @brief create dual quaternion from an affine matrix. The definition of affine matrix can refer to createFromMat()
+ */
+ static DualQuat<_Tp> createFromAffine3(const Affine3<_Tp> &R);
+
+ /**
+ * @brief A dual quaternion is a vector in form of
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * \sigma &=\boldsymbol{p} + \epsilon \boldsymbol{q}\\
+ * &= \cos\hat{\frac{\theta}{2}}+\overline{\hat{l}}\sin\frac{\hat{\theta}}{2}
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * where \f$\hat{\theta}\f$ is dual angle and \f$\overline{\hat{l}}\f$ is dual axis:
+ * \f[
+ * \hat{\theta}=\theta + \epsilon d,\\
+ * \overline{\hat{l}}= \hat{l} +\epsilon m.
+ * \f]
+ * In this representation, \f$\theta\f$ is rotation angle and \f$(\hat{l},m)\f$ is the screw axis, d is the translation distance along the axis.
+ *
+ * @param angle rotation angle.
+ * @param d translation along the rotation axis.
+ * @param axis rotation axis represented by quaternion with w = 0.
+ * @param moment the moment of line, and it should be orthogonal to axis.
+ * @note Translation is applied after the rotation. Use createFromQuat(r, r * t / 2) to create
+ * a dual quaternion which translation is applied before rotation.
+ */
+ static DualQuat<_Tp> createFromPitch(const _Tp angle, const _Tp d, const Vec<_Tp, 3> &axis, const Vec<_Tp, 3> &moment);
+
+ /**
+ * @brief return a quaternion which represent the real part of dual quaternion.
+ * The definition of real part is in createFromQuat().
+ * @sa createFromQuat, getDualPart
+ */
+ Quat<_Tp> getRealPart() const;
+
+ /**
+ * @brief return a quaternion which represent the dual part of dual quaternion.
+ * The definition of dual part is in createFromQuat().
+ * @sa createFromQuat, getRealPart
+ */
+ Quat<_Tp> getDualPart() const;
+
+ /**
+ * @brief return the conjugate of a dual quaternion.
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * \sigma^* &= (p + \epsilon q)^*
+ * &= (p^* + \epsilon q^*)
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * @param dq a dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> conjugate(const DualQuat<T> &dq);
+
+ /**
+ * @brief return the conjugate of a dual quaternion.
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * \sigma^* &= (p + \epsilon q)^*
+ * &= (p^* + \epsilon q^*)
+ * \end{split}
+ * \end{equation}
+ * \f]
+ */
+ DualQuat<_Tp> conjugate() const;
+
+ /**
+ * @brief return the rotation in quaternion form.
+ */
+ Quat<_Tp> getRotation(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief return the translation vector.
+ * The rotation \f$r\f$ in this dual quaternion \f$\sigma\f$ is applied before translation \f$t\f$.
+ * The dual quaternion \f$\sigma\f$ is defined as
+ * \f[\begin{equation}
+ * \begin{split}
+ * \sigma &= p + \epsilon q \\
+ * &= r + \frac{\epsilon}{2}{t}r.
+ * \end{split}
+ * \end{equation}\f]
+ * Thus, the translation can be obtained as follows
+ * \f[t = 2qp^*.\f]
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ * @note This dual quaternion's translation is applied after the rotation.
+ */
+ Vec<_Tp, 3> getTranslation(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief return the norm \f$||\sigma||\f$ of dual quaternion \f$\sigma = p + \epsilon q\f$.
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * ||\sigma|| &= \sqrt{\sigma * \sigma^*} \\
+ * &= ||p|| + \epsilon \frac{p \cdot q}{||p||}.
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * Generally speaking, the norm of a not unit dual
+ * quaternion is a dual number. For convenience, we return it in the form of a dual quaternion
+ * , i.e.
+ * \f[ ||\sigma|| = [||p||, 0, 0, 0, \frac{p \cdot q}{||p||}, 0, 0, 0].\f]
+ *
+ * @note The data type of dual number is dual quaternion.
+ */
+ DualQuat<_Tp> norm() const;
+
+ /**
+ * @brief return a normalized dual quaternion.
+ * A dual quaternion can be expressed as
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * \sigma &= p + \epsilon q\\
+ * &=||\sigma||\left(r+\frac{1}{2}tr\right)
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * where \f$r, t\f$ represents the rotation (ordinary quaternion) and translation (pure ordinary quaternion) respectively,
+ * and \f$||\sigma||\f$ is the norm of dual quaternion(a dual number).
+ * A dual quaternion is unit if and only if
+ * \f[
+ * ||p||=1, p \cdot q=0
+ * \f]
+ * where \f$\cdot\f$ means dot product.
+ * The process of normalization is
+ * \f[
+ * \sigma_{u}=\frac{\sigma}{||\sigma||}
+ * \f]
+ * Next, we simply proof \f$\sigma_u\f$ is a unit dual quaternion:
+ * \f[
+ * \renewcommand{\Im}{\operatorname{Im}}
+ * \begin{equation}
+ * \begin{split}
+ * \sigma_{u}=\frac{\sigma}{||\sigma||}&=\frac{p + \epsilon q}{||p||+\epsilon\frac{p\cdot q}{||p||}}\\
+ * &=\frac{p}{||p||}+\epsilon\left(\frac{q}{||p||}-p\frac{p\cdot q}{||p||^3}\right)\\
+ * &=\frac{p}{||p||}+\epsilon\frac{1}{||p||^2}\left(qp^{*}-p\cdot q\right)\frac{p}{||p||}\\
+ * &=\frac{p}{||p||}+\epsilon\frac{1}{||p||^2}\Im(qp^*)\frac{p}{||p||}.\\
+ * \end{split}
+ * \end{equation}
+ * \f]
+ * As expected, the real part is a rotation and dual part is a pure quaternion.
+ */
+ DualQuat<_Tp> normalize() const;
+
+ /**
+ * @brief if \f$\sigma = p + \epsilon q\f$ is a dual quaternion, p is not zero,
+ * the inverse dual quaternion is
+ * \f[\sigma^{-1} = \frac{\sigma^*}{||\sigma||^2}, \f]
+ * or equivalentlly,
+ * \f[\sigma^{-1} = p^{-1} - \epsilon p^{-1}qp^{-1}.\f]
+ * @param dq a dual quaternion.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, dual quaternion dq assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ template <typename T>
+ friend DualQuat<T> inv(const DualQuat<T> &dq, QuatAssumeType assumeUnit);
+
+ /**
+ * @brief if \f$\sigma = p + \epsilon q\f$ is a dual quaternion, p is not zero,
+ * the inverse dual quaternion is
+ * \f[\sigma^{-1} = \frac{\sigma^*}{||\sigma||^2}, \f]
+ * or equivalentlly,
+ * \f[\sigma^{-1} = p^{-1} - \epsilon p^{-1}qp^{-1}.\f]
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ DualQuat<_Tp> inv(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief return the dot product of two dual quaternion.
+ * @param p other dual quaternion.
+ */
+ _Tp dot(DualQuat<_Tp> p) const;
+
+ /**
+ ** @brief return the value of \f$p^t\f$ where p is a dual quaternion.
+ * This could be calculated as:
+ * \f[
+ * p^t = \exp(t\ln p)
+ * \f]
+ * @param dq a dual quaternion.
+ * @param t index of power function.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, dual quaternion dq assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ template <typename T>
+ friend DualQuat<T> power(const DualQuat<T> &dq, const T t, QuatAssumeType assumeUnit);
+
+ /**
+ ** @brief return the value of \f$p^t\f$ where p is a dual quaternion.
+ * This could be calculated as:
+ * \f[
+ * p^t = \exp(t\ln p)
+ * \f]
+ *
+ * @param t index of power function.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ DualQuat<_Tp> power(const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief return the value of \f$p^q\f$ where p and q are dual quaternions.
+ * This could be calculated as:
+ * \f[
+ * p^q = \exp(q\ln p)
+ * \f]
+ * @param p a dual quaternion.
+ * @param q a dual quaternion.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, dual quaternion p assume to be a dual unit quaternion
+ * and this function will save some computations.
+ */
+ template <typename T>
+ friend DualQuat<T> power(const DualQuat<T>& p, const DualQuat<T>& q, QuatAssumeType assumeUnit);
+
+ /**
+ * @brief return the value of \f$p^q\f$ where p and q are dual quaternions.
+ * This could be calculated as:
+ * \f[
+ * p^q = \exp(q\ln p)
+ * \f]
+ *
+ * @param q a dual quaternion
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a dual unit quaternion
+ * and this function will save some computations.
+ */
+ DualQuat<_Tp> power(const DualQuat<_Tp>& q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief return the value of exponential function value
+ * @param dq a dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> exp(const DualQuat<T> &dq);
+
+ /**
+ * @brief return the value of exponential function value
+ */
+ DualQuat<_Tp> exp() const;
+
+ /**
+ * @brief return the value of logarithm function value
+ *
+ * @param dq a dual quaternion.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, dual quaternion dq assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ template <typename T>
+ friend DualQuat<T> log(const DualQuat<T> &dq, QuatAssumeType assumeUnit);
+
+ /**
+ * @brief return the value of logarithm function value
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ */
+ DualQuat<_Tp> log(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief Transform this dual quaternion to a vector.
+ */
+ Vec<_Tp, 8> toVec() const;
+
+ /**
+ * @brief Transform this dual quaternion to a affine transformation matrix
+ * the form of matrix, see createFromMat().
+ */
+ Matx<_Tp, 4, 4> toMat(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief Transform this dual quaternion to a instance of Affine3.
+ */
+ Affine3<_Tp> toAffine3(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const;
+
+ /**
+ * @brief The screw linear interpolation(ScLERP) is an extension of spherical linear interpolation of dual quaternion.
+ * If \f$\sigma_1\f$ and \f$\sigma_2\f$ are two dual quaternions representing the initial and final pose.
+ * The interpolation of ScLERP function can be defined as:
+ * \f[
+ * ScLERP(t;\sigma_1,\sigma_2) = \sigma_1 * (\sigma_1^{-1} * \sigma_2)^t, t\in[0,1]
+ * \f]
+ *
+ * @param q1 a dual quaternion represents a initial pose.
+ * @param q2 a dual quaternion represents a final pose.
+ * @param t interpolation parameter
+ * @param directChange if true, it always return the shortest path.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ *
+ * For example
+ * ```
+ * double angle1 = CV_PI / 2;
+ * Vec3d axis{0, 0, 1};
+ * Vec3d t(0, 0, 3);
+ * DualQuatd initial = DualQuatd::createFromAngleAxisTrans(angle1, axis, t);
+ * double angle2 = CV_PI;
+ * DualQuatd final = DualQuatd::createFromAngleAxisTrans(angle2, axis, t);
+ * DualQuatd inter = DualQuatd::sclerp(initial, final, 0.5);
+ * ```
+ */
+ static DualQuat<_Tp> sclerp(const DualQuat<_Tp> &q1, const DualQuat<_Tp> &q2, const _Tp t,
+ bool directChange=true, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
+ /**
+ * @brief The method of Dual Quaternion linear Blending(DQB) is to compute a transformation between dual quaternion
+ * \f$q_1\f$ and \f$q_2\f$ and can be defined as:
+ * \f[
+ * DQB(t;{\boldsymbol{q}}_1,{\boldsymbol{q}}_2)=
+ * \frac{(1-t){\boldsymbol{q}}_1+t{\boldsymbol{q}}_2}{||(1-t){\boldsymbol{q}}_1+t{\boldsymbol{q}}_2||}.
+ * \f]
+ * where \f$q_1\f$ and \f$q_2\f$ are unit dual quaternions representing the input transformations.
+ * If you want to use DQB that works for more than two rigid transformations, see @ref gdqblend
+ *
+ * @param q1 a unit dual quaternion representing the input transformations.
+ * @param q2 a unit dual quaternion representing the input transformations.
+ * @param t parameter \f$t\in[0,1]\f$.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, this dual quaternion assume to be a unit dual quaternion
+ * and this function will save some computations.
+ *
+ * @sa gdqblend
+ */
+ static DualQuat<_Tp> dqblend(const DualQuat<_Tp> &q1, const DualQuat<_Tp> &q2, const _Tp t,
+ QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
+
+ /**
+ * @brief The generalized Dual Quaternion linear Blending works for more than two rigid transformations.
+ * If these transformations are expressed as unit dual quaternions \f$q_1,...,q_n\f$ with convex weights
+ * \f$w = (w_1,...,w_n)\f$, the generalized DQB is simply
+ * \f[
+ * gDQB(\boldsymbol{w};{\boldsymbol{q}}_1,...,{\boldsymbol{q}}_n)=\frac{w_1{\boldsymbol{q}}_1+...+w_n{\boldsymbol{q}}_n}
+ * {||w_1{\boldsymbol{q}}_1+...+w_n{\boldsymbol{q}}_n||}.
+ * \f]
+ * @param dualquat vector of dual quaternions
+ * @param weights vector of weights, the size of weights should be the same as dualquat, and the weights should
+ * satisfy \f$\sum_0^n w_{i} = 1\f$ and \f$w_i>0\f$.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, these dual quaternions assume to be unit quaternions
+ * and this function will save some computations.
+ * @note the type of weights' element should be the same as the date type of dual quaternion inside the dualquat.
+ */
+ template <int cn>
+ static DualQuat<_Tp> gdqblend(const Vec<DualQuat<_Tp>, cn> &dualquat, InputArray weights,
+ QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
+
+ /**
+ * @brief The generalized Dual Quaternion linear Blending works for more than two rigid transformations.
+ * If these transformations are expressed as unit dual quaternions \f$q_1,...,q_n\f$ with convex weights
+ * \f$w = (w_1,...,w_n)\f$, the generalized DQB is simply
+ * \f[
+ * gDQB(\boldsymbol{w};{\boldsymbol{q}}_1,...,{\boldsymbol{q}}_n)=\frac{w_1{\boldsymbol{q}}_1+...+w_n{\boldsymbol{q}}_n}
+ * {||w_1{\boldsymbol{q}}_1+...+w_n{\boldsymbol{q}}_n||}.
+ * \f]
+ * @param dualquat The dual quaternions which have 8 channels and 1 row or 1 col.
+ * @param weights vector of weights, the size of weights should be the same as dualquat, and the weights should
+ * satisfy \f$\sum_0^n w_{i} = 1\f$ and \f$w_i>0\f$.
+ * @param assumeUnit if @ref QUAT_ASSUME_UNIT, these dual quaternions assume to be unit quaternions
+ * and this function will save some computations.
+ * @note the type of weights' element should be the same as the date type of dual quaternion inside the dualquat.
+ */
+ static DualQuat<_Tp> gdqblend(InputArray dualquat, InputArray weights,
+ QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT);
+
+ /**
+ * @brief Return opposite dual quaternion \f$-p\f$
+ * which satisfies \f$p + (-p) = 0.\f$
+ *
+ * For example
+ * ```
+ * DualQuatd q{1, 2, 3, 4, 5, 6, 7, 8};
+ * std::cout << -q << std::endl; // [-1, -2, -3, -4, -5, -6, -7, -8]
+ * ```
+ */
+ DualQuat<_Tp> operator-() const;
+
+ /**
+ * @brief return true if two dual quaternions p and q are nearly equal, i.e. when the absolute
+ * value of each \f$p_i\f$ and \f$q_i\f$ is less than CV_DUAL_QUAT_EPS.
+ */
+ bool operator==(const DualQuat<_Tp>&) const;
+
+ /**
+ * @brief Subtraction operator of two dual quaternions p and q.
+ * It returns a new dual quaternion that each value is the sum of \f$p_i\f$ and \f$-q_i\f$.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * std::cout << p - q << std::endl; //[-4, -4, -4, -4, 4, -4, -4, -4]
+ * ```
+ */
+ DualQuat<_Tp> operator-(const DualQuat<_Tp>&) const;
+
+ /**
+ * @brief Subtraction assignment operator of two dual quaternions p and q.
+ * It subtracts right operand from the left operand and assign the result to left operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * p -= q; // equivalent to p = p - q
+ * std::cout << p << std::endl; //[-4, -4, -4, -4, 4, -4, -4, -4]
+ *
+ * ```
+ */
+ DualQuat<_Tp>& operator-=(const DualQuat<_Tp>&);
+
+ /**
+ * @brief Addition operator of two dual quaternions p and q.
+ * It returns a new dual quaternion that each value is the sum of \f$p_i\f$ and \f$q_i\f$.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * std::cout << p + q << std::endl; //[6, 8, 10, 12, 14, 16, 18, 20]
+ * ```
+ */
+ DualQuat<_Tp> operator+(const DualQuat<_Tp>&) const;
+
+ /**
+ * @brief Addition assignment operator of two dual quaternions p and q.
+ * It adds right operand to the left operand and assign the result to left operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * p += q; // equivalent to p = p + q
+ * std::cout << p << std::endl; //[6, 8, 10, 12, 14, 16, 18, 20]
+ *
+ * ```
+ */
+ DualQuat<_Tp>& operator+=(const DualQuat<_Tp>&);
+
+ /**
+ * @brief Multiplication assignment operator of two quaternions.
+ * It multiplies right operand with the left operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion multiplication:
+ * The dual quaternion can be written as an ordered pair of quaternions [A, B]. Thus
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p * q &= [A, B][C, D]\\
+ * &=[AC, AD + BC]
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * p *= q;
+ * std::cout << p << std::endl; //[-60, 12, 30, 24, -216, 80, 124, 120]
+ * ```
+ */
+ DualQuat<_Tp>& operator*=(const DualQuat<_Tp>&);
+
+ /**
+ * @brief Multiplication assignment operator of a quaternions and a scalar.
+ * It multiplies right operand with the left operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion multiplication with a scalar:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p * s &= [w, x, y, z, w\_, x\_, y\_, z\_] * s\\
+ * &=[w s, x s, y s, z s, w\_ \space s, x\_ \space s, y\_ \space s, z\_ \space s].
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double s = 2.0;
+ * p *= s;
+ * std::cout << p << std::endl; //[2, 4, 6, 8, 10, 12, 14, 16]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ DualQuat<_Tp> operator*=(const _Tp s);
+
+
+ /**
+ * @brief Multiplication operator of two dual quaternions q and p.
+ * Multiplies values on either side of the operator.
+ *
+ * Rule of dual quaternion multiplication:
+ * The dual quaternion can be written as an ordered pair of quaternions [A, B]. Thus
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p * q &= [A, B][C, D]\\
+ * &=[AC, AD + BC]
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * std::cout << p * q << std::endl; //[-60, 12, 30, 24, -216, 80, 124, 120]
+ * ```
+ */
+ DualQuat<_Tp> operator*(const DualQuat<_Tp>&) const;
+
+ /**
+ * @brief Division operator of a dual quaternions and a scalar.
+ * It divides left operand with the right operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion division with a scalar:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p / s &= [w, x, y, z, w\_, x\_, y\_, z\_] / s\\
+ * &=[w/s, x/s, y/s, z/s, w\_/s, x\_/s, y\_/s, z\_/s].
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double s = 2.0;
+ * p /= s; // equivalent to p = p / s
+ * std::cout << p << std::endl; //[0.5, 1, 1.5, 2, 2.5, 3, 3.5, 4]
+ * ```
+ * @note the type of scalar should be equal to this dual quaternion.
+ */
+ DualQuat<_Tp> operator/(const _Tp s) const;
+
+ /**
+ * @brief Division operator of two dual quaternions p and q.
+ * Divides left hand operand by right hand operand.
+ *
+ * Rule of dual quaternion division with a dual quaternion:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p / q &= p * q.inv()\\
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * std::cout << p / q << std::endl; // equivalent to p * q.inv()
+ * ```
+ */
+ DualQuat<_Tp> operator/(const DualQuat<_Tp>&) const;
+
+ /**
+ * @brief Division assignment operator of two dual quaternions p and q;
+ * It divides left operand with the right operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion division with a quaternion:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p / q&= p * q.inv()\\
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * DualQuatd q{5, 6, 7, 8, 9, 10, 11, 12};
+ * p /= q; // equivalent to p = p * q.inv()
+ * std::cout << p << std::endl;
+ * ```
+ */
+ DualQuat<_Tp>& operator/=(const DualQuat<_Tp>&);
+
+ /**
+ * @brief Division assignment operator of a dual quaternions and a scalar.
+ * It divides left operand with the right operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion division with a scalar:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p / s &= [w, x, y, z, w\_, x\_, y\_ ,z\_] / s\\
+ * &=[w / s, x / s, y / s, z / s, w\_ / \space s, x\_ / \space s, y\_ / \space s, z\_ / \space s].
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double s = 2.0;;
+ * p /= s; // equivalent to p = p / s
+ * std::cout << p << std::endl; //[0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ Quat<_Tp>& operator/=(const _Tp s);
+
+ /**
+ * @brief Addition operator of a scalar and a dual quaternions.
+ * Adds right hand operand from left hand operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double scalar = 2.0;
+ * std::cout << scalar + p << std::endl; //[3.0, 2, 3, 4, 5, 6, 7, 8]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator+(const T s, const DualQuat<T>&);
+
+ /**
+ * @brief Addition operator of a dual quaternions and a scalar.
+ * Adds right hand operand from left hand operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double scalar = 2.0;
+ * std::cout << p + scalar << std::endl; //[3.0, 2, 3, 4, 5, 6, 7, 8]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator+(const DualQuat<T>&, const T s);
+
+ /**
+ * @brief Multiplication operator of a scalar and a dual quaternions.
+ * It multiplies right operand with the left operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion multiplication with a scalar:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p * s &= [w, x, y, z, w\_, x\_, y\_, z\_] * s\\
+ * &=[w s, x s, y s, z s, w\_ \space s, x\_ \space s, y\_ \space s, z\_ \space s].
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double s = 2.0;
+ * std::cout << s * p << std::endl; //[2, 4, 6, 8, 10, 12, 14, 16]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator*(const T s, const DualQuat<T>&);
+
+ /**
+ * @brief Subtraction operator of a dual quaternion and a scalar.
+ * Subtracts right hand operand from left hand operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double scalar = 2.0;
+ * std::cout << p - scalar << std::endl; //[-1, 2, 3, 4, 5, 6, 7, 8]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator-(const DualQuat<T>&, const T s);
+
+ /**
+ * @brief Subtraction operator of a scalar and a dual quaternions.
+ * Subtracts right hand operand from left hand operand.
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double scalar = 2.0;
+ * std::cout << scalar - p << std::endl; //[1.0, -2, -3, -4, -5, -6, -7, -8]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator-(const T s, const DualQuat<T>&);
+
+ /**
+ * @brief Multiplication operator of a dual quaternions and a scalar.
+ * It multiplies right operand with the left operand and assign the result to left operand.
+ *
+ * Rule of dual quaternion multiplication with a scalar:
+ * \f[
+ * \begin{equation}
+ * \begin{split}
+ * p * s &= [w, x, y, z, w\_, x\_, y\_, z\_] * s\\
+ * &=[w s, x s, y s, z s, w\_ \space s, x\_ \space s, y\_ \space s, z\_ \space s].
+ * \end{split}
+ * \end{equation}
+ * \f]
+ *
+ * For example
+ * ```
+ * DualQuatd p{1, 2, 3, 4, 5, 6, 7, 8};
+ * double s = 2.0;
+ * std::cout << p * s << std::endl; //[2, 4, 6, 8, 10, 12, 14, 16]
+ * ```
+ * @note the type of scalar should be equal to the dual quaternion.
+ */
+ template <typename T>
+ friend DualQuat<T> cv::operator*(const DualQuat<T>&, const T s);
+
+ template <typename S>
+ friend std::ostream& cv::operator<<(std::ostream&, const DualQuat<S>&);
+
+};
+
+using DualQuatd = DualQuat<double>;
+using DualQuatf = DualQuat<float>;
+
+//! @} core
+}//namespace
+
+#include "dualquaternion.inl.hpp"
+
+#endif /* OPENCV_CORE_QUATERNION_HPP */
--- /dev/null
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2020, Huawei Technologies Co., Ltd. All rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Author: Liangqian Kong <kongliangqian@huawei.com>
+// Longbu Wang <wanglongbu@huawei.com>
+
+#ifndef OPENCV_CORE_DUALQUATERNION_INL_HPP
+#define OPENCV_CORE_DUALQUATERNION_INL_HPP
+
+#ifndef OPENCV_CORE_DUALQUATERNION_HPP
+#error This is not a standalone header. Include dualquaternion.hpp instead.
+#endif
+
+///////////////////////////////////////////////////////////////////////////////////////
+//Implementation
+namespace cv {
+
+template <typename T>
+DualQuat<T>::DualQuat():w(0), x(0), y(0), z(0), w_(0), x_(0), y_(0), z_(0){};
+
+template <typename T>
+DualQuat<T>::DualQuat(const T vw, const T vx, const T vy, const T vz, const T _w, const T _x, const T _y, const T _z):
+ w(vw), x(vx), y(vy), z(vz), w_(_w), x_(_x), y_(_y), z_(_z){};
+
+template <typename T>
+DualQuat<T>::DualQuat(const Vec<T, 8> &q):w(q[0]), x(q[1]), y(q[2]), z(q[3]),
+ w_(q[4]), x_(q[5]), y_(q[6]), z_(q[7]){};
+
+template <typename T>
+DualQuat<T> DualQuat<T>::createFromQuat(const Quat<T> &realPart, const Quat<T> &dualPart)
+{
+ T w = realPart.w;
+ T x = realPart.x;
+ T y = realPart.y;
+ T z = realPart.z;
+ T w_ = dualPart.w;
+ T x_ = dualPart.x;
+ T y_ = dualPart.y;
+ T z_ = dualPart.z;
+ return DualQuat<T>(w, x, y, z, w_, x_, y_, z_);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::createFromAngleAxisTrans(const T angle, const Vec<T, 3> &axis, const Vec<T, 3> &trans)
+{
+ Quat<T> r = Quat<T>::createFromAngleAxis(angle, axis);
+ Quat<T> t{0, trans[0], trans[1], trans[2]};
+ return createFromQuat(r, t * r / 2);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::createFromMat(InputArray _R)
+{
+ CV_CheckTypeEQ(_R.type(), cv::traits::Type<T>::value, "");
+ if (_R.size() != Size(4, 4))
+ {
+ CV_Error(Error::StsBadArg, "The input matrix must have 4 columns and 4 rows");
+ }
+ Mat R = _R.getMat();
+ Quat<T> r = Quat<T>::createFromRotMat(R.colRange(0, 3).rowRange(0, 3));
+ Quat<T> trans(0, R.at<T>(0, 3), R.at<T>(1, 3), R.at<T>(2, 3));
+ return createFromQuat(r, trans * r / 2);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::createFromAffine3(const Affine3<T> &R)
+{
+ return createFromMat(R.matrix);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::createFromPitch(const T angle, const T d, const Vec<T, 3> &axis, const Vec<T, 3> &moment)
+{
+ T half_angle = angle / 2, half_d = d / 2;
+ Quat<T> qaxis = Quat<T>(0, axis[0], axis[1], axis[2]).normalize();
+ Quat<T> qmoment = Quat<T>(0, moment[0], moment[1], moment[2]);
+ qmoment -= qaxis * axis.dot(moment);
+ Quat<T> dual = -half_d * std::sin(half_angle) + std::sin(half_angle) * qmoment +
+ half_d * std::cos(half_angle) * qaxis;
+ return createFromQuat(Quat<T>::createFromAngleAxis(angle, axis), dual);
+}
+
+template <typename T>
+inline bool DualQuat<T>::operator==(const DualQuat<T> &q) const
+{
+ return (abs(w - q.w) < CV_DUAL_QUAT_EPS && abs(x - q.x) < CV_DUAL_QUAT_EPS &&
+ abs(y - q.y) < CV_DUAL_QUAT_EPS && abs(z - q.z) < CV_DUAL_QUAT_EPS &&
+ abs(w_ - q.w_) < CV_DUAL_QUAT_EPS && abs(x_ - q.x_) < CV_DUAL_QUAT_EPS &&
+ abs(y_ - q.y_) < CV_DUAL_QUAT_EPS && abs(z_ - q.z_) < CV_DUAL_QUAT_EPS);
+}
+
+template <typename T>
+inline Quat<T> DualQuat<T>::getRealPart() const
+{
+ return Quat<T>(w, x, y, z);
+}
+
+template <typename T>
+inline Quat<T> DualQuat<T>::getDualPart() const
+{
+ return Quat<T>(w_, x_, y_, z_);
+}
+
+template <typename T>
+inline DualQuat<T> conjugate(const DualQuat<T> &dq)
+{
+ return dq.conjugate();
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::conjugate() const
+{
+ return DualQuat<T>(w, -x, -y, -z, w_, -x_, -y_, -z_);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::norm() const
+{
+ Quat<T> real = getRealPart();
+ T realNorm = real.norm();
+ Quat<T> dual = getDualPart();
+ if (realNorm < CV_DUAL_QUAT_EPS){
+ return DualQuat<T>(0, 0, 0, 0, 0, 0, 0, 0);
+ }
+ return DualQuat<T>(realNorm, 0, 0, 0, real.dot(dual) / realNorm, 0, 0, 0);
+}
+
+template <typename T>
+inline Quat<T> DualQuat<T>::getRotation(QuatAssumeType assumeUnit) const
+{
+ if (assumeUnit)
+ {
+ return getRealPart();
+ }
+ return getRealPart().normalize();
+}
+
+template <typename T>
+inline Vec<T, 3> DualQuat<T>::getTranslation(QuatAssumeType assumeUnit) const
+{
+ Quat<T> trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit));
+ return Vec<T, 3>{trans[1], trans[2], trans[3]};
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::normalize() const
+{
+ Quat<T> p = getRealPart();
+ Quat<T> q = getDualPart();
+ T p_norm = p.norm();
+ if (p_norm < CV_DUAL_QUAT_EPS)
+ {
+ CV_Error(Error::StsBadArg, "Cannot normalize this dual quaternion: the norm is too small.");
+ }
+ Quat<T> p_nr = p / p_norm;
+ Quat<T> q_nr = q / p_norm;
+ return createFromQuat(p_nr, q_nr - p_nr * p_nr.dot(q_nr));
+}
+
+template <typename T>
+inline T DualQuat<T>::dot(DualQuat<T> q) const
+{
+ return q.w * w + q.x * x + q.y * y + q.z * z + q.w_ * w_ + q.x_ * x_ + q.y_ * y_ + q.z_ * z_;
+}
+
+template <typename T>
+inline DualQuat<T> inv(const DualQuat<T> &dq, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
+{
+ return dq.inv(assumeUnit);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::inv(QuatAssumeType assumeUnit) const
+{
+ Quat<T> real = getRealPart();
+ Quat<T> dual = getDualPart();
+ return createFromQuat(real.inv(assumeUnit), -real.inv(assumeUnit) * dual * real.inv(assumeUnit));
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator-(const DualQuat<T> &q) const
+{
+ return DualQuat<T>(w - q.w, x - q.x, y - q.y, z - q.z, w_ - q.w_, x_ - q.x_, y_ - q.y_, z_ - q.z_);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator-() const
+{
+ return DualQuat<T>(-w, -x, -y, -z, -w_, -x_, -y_, -z_);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator+(const DualQuat<T> &q) const
+{
+ return DualQuat<T>(w + q.w, x + q.x, y + q.y, z + q.z, w_ + q.w_, x_ + q.x_, y_ + q.y_, z_ + q.z_);
+}
+
+template <typename T>
+inline DualQuat<T>& DualQuat<T>::operator+=(const DualQuat<T> &q)
+{
+ *this = *this + q;
+ return *this;
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator*(const DualQuat<T> &q) const
+{
+ Quat<T> A = getRealPart();
+ Quat<T> B = getDualPart();
+ Quat<T> C = q.getRealPart();
+ Quat<T> D = q.getDualPart();
+ return DualQuat<T>::createFromQuat(A * C, A * D + B * C);
+}
+
+template <typename T>
+inline DualQuat<T>& DualQuat<T>::operator*=(const DualQuat<T> &q)
+{
+ *this = *this * q;
+ return *this;
+}
+
+template <typename T>
+inline DualQuat<T> operator+(const T a, const DualQuat<T> &q)
+{
+ return DualQuat<T>(a + q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
+}
+
+template <typename T>
+inline DualQuat<T> operator+(const DualQuat<T> &q, const T a)
+{
+ return DualQuat<T>(a + q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
+}
+
+template <typename T>
+inline DualQuat<T> operator-(const DualQuat<T> &q, const T a)
+{
+ return DualQuat<T>(q.w - a, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
+}
+
+template <typename T>
+inline DualQuat<T>& DualQuat<T>::operator-=(const DualQuat<T> &q)
+{
+ *this = *this - q;
+ return *this;
+}
+
+template <typename T>
+inline DualQuat<T> operator-(const T a, const DualQuat<T> &q)
+{
+ return DualQuat<T>(a - q.w, -q.x, -q.y, -q.z, -q.w_, -q.x_, -q.y_, -q.z_);
+}
+
+template <typename T>
+inline DualQuat<T> operator*(const T a, const DualQuat<T> &q)
+{
+ return DualQuat<T>(q.w * a, q.x * a, q.y * a, q.z * a, q.w_ * a, q.x_ * a, q.y_ * a, q.z_ * a);
+}
+
+template <typename T>
+inline DualQuat<T> operator*(const DualQuat<T> &q, const T a)
+{
+ return DualQuat<T>(q.w * a, q.x * a, q.y * a, q.z * a, q.w_ * a, q.x_ * a, q.y_ * a, q.z_ * a);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator/(const T a) const
+{
+ return DualQuat<T>(w / a, x / a, y / a, z / a, w_ / a, x_ / a, y_ / a, z_ / a);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::operator/(const DualQuat<T> &q) const
+{
+ return *this * q.inv();
+}
+
+template <typename T>
+inline DualQuat<T>& DualQuat<T>::operator/=(const DualQuat<T> &q)
+{
+ *this = *this / q;
+ return *this;
+}
+
+template <typename T>
+std::ostream & operator<<(std::ostream &os, const DualQuat<T> &q)
+{
+ os << "DualQuat " << Vec<T, 8>{q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_};
+ return os;
+}
+
+template <typename T>
+inline DualQuat<T> exp(const DualQuat<T> &dq)
+{
+ return dq.exp();
+}
+
+namespace detail {
+
+template <typename _Tp>
+Matx<_Tp, 4, 4> jacob_exp(const Quat<_Tp> &q)
+{
+ _Tp nv = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
+ _Tp sinc_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? 1 - nv * nv / 6 : std::sin(nv) / nv;
+ _Tp csiii_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? -(_Tp)1.0 / 3 : (std::cos(nv) - sinc_nv) / nv / nv;
+ Matx<_Tp, 4, 4> J_exp_quat {
+ std::cos(nv), -sinc_nv * q.x, -sinc_nv * q.y, -sinc_nv * q.z,
+ sinc_nv * q.x, csiii_nv * q.x * q.x + sinc_nv, csiii_nv * q.x * q.y, csiii_nv * q.x * q.z,
+ sinc_nv * q.y, csiii_nv * q.y * q.x, csiii_nv * q.y * q.y + sinc_nv, csiii_nv * q.y * q.z,
+ sinc_nv * q.z, csiii_nv * q.z * q.x, csiii_nv * q.z * q.y, csiii_nv * q.z * q.z + sinc_nv
+ };
+ return std::exp(q.w) * J_exp_quat;
+}
+
+} // namespace detail
+
+template <typename T>
+DualQuat<T> DualQuat<T>::exp() const
+{
+ Quat<T> real = getRealPart();
+ return createFromQuat(real.exp(), Quat<T>(detail::jacob_exp(real) * getDualPart().toVec()));
+}
+
+template <typename T>
+DualQuat<T> log(const DualQuat<T> &dq, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
+{
+ return dq.log(assumeUnit);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::log(QuatAssumeType assumeUnit) const
+{
+ Quat<T> plog = getRealPart().log(assumeUnit);
+ Matx<T, 4, 4> jacob = detail::jacob_exp(plog);
+ return createFromQuat(plog, Quat<T>(jacob.inv() * getDualPart().toVec()));
+}
+
+template <typename T>
+inline DualQuat<T> power(const DualQuat<T> &dq, const T t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
+{
+ return dq.power(t, assumeUnit);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::power(const T t, QuatAssumeType assumeUnit) const
+{
+ return (t * log(assumeUnit)).exp();
+}
+
+template <typename T>
+inline DualQuat<T> power(const DualQuat<T> &p, const DualQuat<T> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
+{
+ return p.power(q, assumeUnit);
+}
+
+template <typename T>
+inline DualQuat<T> DualQuat<T>::power(const DualQuat<T> &q, QuatAssumeType assumeUnit) const
+{
+ return (q * log(assumeUnit)).exp();
+}
+
+template <typename T>
+inline Vec<T, 8> DualQuat<T>::toVec() const
+{
+ return Vec<T, 8>(w, x, y, z, w_, x_, y_, z_);
+}
+
+template <typename T>
+Affine3<T> DualQuat<T>::toAffine3(QuatAssumeType assumeUnit) const
+{
+ return Affine3<T>(toMat(assumeUnit));
+}
+
+template <typename T>
+Matx<T, 4, 4> DualQuat<T>::toMat(QuatAssumeType assumeUnit) const
+{
+ Matx<T, 4, 4> rot44 = getRotation(assumeUnit).toRotMat4x4();
+ Vec<T, 3> translation = getTranslation(assumeUnit);
+ rot44(0, 3) = translation[0];
+ rot44(1, 3) = translation[1];
+ rot44(2, 3) = translation[2];
+ return rot44;
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::sclerp(const DualQuat<T> &q0, const DualQuat<T> &q1, const T t, bool directChange, QuatAssumeType assumeUnit)
+{
+ DualQuat<T> v0(q0), v1(q1);
+ if (!assumeUnit)
+ {
+ v0 = v0.normalize();
+ v1 = v1.normalize();
+ }
+ Quat<T> v0Real = v0.getRealPart();
+ Quat<T> v1Real = v1.getRealPart();
+ if (directChange && v1Real.dot(v0Real) < 0)
+ {
+ v0 = -v0;
+ }
+ DualQuat<T> v0inv1 = v0.inv() * v1;
+ return v0 * v0inv1.power(t, QUAT_ASSUME_UNIT);
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::dqblend(const DualQuat<T> &q1, const DualQuat<T> &q2, const T t, QuatAssumeType assumeUnit)
+{
+ DualQuat<T> v1(q1), v2(q2);
+ if (!assumeUnit)
+ {
+ v1 = v1.normalize();
+ v2 = v2.normalize();
+ }
+ if (v1.getRotation(assumeUnit).dot(v2.getRotation(assumeUnit)) < 0)
+ {
+ return ((1 - t) * v1 - t * v2).normalize();
+ }
+ return ((1 - t) * v1 + t * v2).normalize();
+}
+
+template <typename T>
+DualQuat<T> DualQuat<T>::gdqblend(InputArray _dualquat, InputArray _weight, QuatAssumeType assumeUnit)
+{
+ CV_CheckTypeEQ(_weight.type(), cv::traits::Type<T>::value, "");
+ CV_CheckTypeEQ(_dualquat.type(), CV_MAKETYPE(CV_MAT_DEPTH(cv::traits::Type<T>::value), 8), "");
+ Size dq_s = _dualquat.size();
+ if (dq_s != _weight.size() || (dq_s.height != 1 && dq_s.width != 1))
+ {
+ CV_Error(Error::StsBadArg, "The size of weight must be the same as dualquat, both of them should be (1, n) or (n, 1)");
+ }
+ Mat dualquat = _dualquat.getMat(), weight = _weight.getMat();
+ const int cn = std::max(dq_s.width, dq_s.height);
+ if (!assumeUnit)
+ {
+ for (int i = 0; i < cn; ++i)
+ {
+ dualquat.at<Vec<T, 8>>(i) = DualQuat<T>{dualquat.at<Vec<T, 8>>(i)}.normalize().toVec();
+ }
+ }
+ Vec<T, 8> dq_blend = dualquat.at<Vec<T, 8>>(0) * weight.at<T>(0);
+ Quat<T> q0 = DualQuat<T> {dualquat.at<Vec<T, 8>>(0)}.getRotation(assumeUnit);
+ for (int i = 1; i < cn; ++i)
+ {
+ T k = q0.dot(DualQuat<T>{dualquat.at<Vec<T, 8>>(i)}.getRotation(assumeUnit)) < 0 ? -1: 1;
+ dq_blend = dq_blend + dualquat.at<Vec<T, 8>>(i) * k * weight.at<T>(i);
+ }
+ return DualQuat<T>{dq_blend}.normalize();
+}
+
+template <typename T>
+template <int cn>
+DualQuat<T> DualQuat<T>::gdqblend(const Vec<DualQuat<T>, cn> &_dualquat, InputArray _weight, QuatAssumeType assumeUnit)
+{
+ Vec<DualQuat<T>, cn> dualquat(_dualquat);
+ if (cn == 0)
+ {
+ return DualQuat<T>(1, 0, 0, 0, 0, 0, 0, 0);
+ }
+ Mat dualquat_mat(cn, 1, CV_64FC(8));
+ for (int i = 0; i < cn ; ++i)
+ {
+ dualquat_mat.at<Vec<T, 8>>(i) = dualquat[i].toVec();
+ }
+ return gdqblend(dualquat_mat, _weight, assumeUnit);
+}
+
+} //namespace cv
+
+#endif /*OPENCV_CORE_DUALQUATERNION_INL_HPP*/
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
+#include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
+
#include <opencv2/core/quaternion.hpp>
-#include <opencv2/ts/cuda_test.hpp>
-using namespace cv;
+#include <opencv2/core/dualquaternion.hpp>
+
namespace opencv_test{ namespace {
-class QuatTest: public ::testing::Test {
+
+class QuatTest: public ::testing::Test
+{
protected:
void SetUp() override
{
};
-TEST_F(QuatTest, constructor){
+TEST_F(QuatTest, constructor)
+{
Vec<double, 4> coeff{1, 2, 3, 4};
EXPECT_EQ(Quat<double> (coeff), q1);
EXPECT_EQ(q3, q3UnitAxis);
EXPECT_EQ(Quatd::createFromRvec(Vec3d(0, 0, 0)), qIdentity);
}
-TEST_F(QuatTest, basicfuns){
+TEST_F(QuatTest, basicfuns)
+{
Quat<double> q1Conj{1, -2, -3, -4};
EXPECT_EQ(q3Norm2.normalize(), q3);
EXPECT_EQ(q1.norm(), sqrt(30));
EXPECT_EQ(tan(atan(q1)), q1);
}
-TEST_F(QuatTest, operator){
+TEST_F(QuatTest, test_operator)
+{
Quatd minusQ{-1, -2, -3, -4};
Quatd qAdd{3.5, 0, 6.5, 8};
Quatd qMinus{-1.5, 4, -0.5, 0};
EXPECT_ANY_THROW(q1.at(4));
}
-TEST_F(QuatTest, quatAttrs){
+TEST_F(QuatTest, quatAttrs)
+{
double angleQ1 = 2 * acos(1.0 / sqrt(30));
Vec3d axis1{0.3713906763541037, 0.557086014, 0.742781352};
Vec<double, 3> q1axis1 = q1.getAxis();
EXPECT_NEAR(axis1[2], axis1[2], 1e-6);
}
-TEST_F(QuatTest, interpolation){
+TEST_F(QuatTest, interpolation)
+{
Quatd qNoRot = Quatd::createFromAngleAxis(0, axis);
Quatd qLerpInter(1.0 / 2, sqrt(3) / 6, sqrt(3) / 6, sqrt(3) / 6);
EXPECT_EQ(Quatd::lerp(qNoRot, q3, 0), qNoRot);
Quatd(0.653285, -0.0990435, 0.369641, 0.65328) //EXT_ZYZ
};
-TEST_F(QuatTest, EulerAngles){
+TEST_F(QuatTest, EulerAngles)
+{
Vec3d test_angle = {0.523598, 0.78539, 1.04719};
for (QuatEnum::EulerAnglesType i = QuatEnum::EulerAnglesType::INT_XYZ; i <= QuatEnum::EulerAnglesType::EXT_ZYZ; i = (QuatEnum::EulerAnglesType)(i + 1))
{
EXPECT_EQ(Quatd::createFromEulerAngles(test_angle6, QuatEnum::INT_ZXY), Quatd::createFromEulerAngles(test_angle7, QuatEnum::INT_ZXY));
}
-} // namespace
-}// opencv_test
+
+class DualQuatTest: public ::testing::Test
+{
+protected:
+ double scalar = 2.5;
+ double angle = CV_PI;
+ Vec<double, 3> axis{1, 1, 1};
+ Vec<double, 3> unAxis{0, 0, 0};
+ Vec<double, 3> unitAxis{1.0 / sqrt(3), 1.0 / sqrt(3), 1.0 / sqrt(3)};
+ DualQuatd dq1{1, 2, 3, 4, 5, 6, 7, 8};
+ Vec3d trans{0, 0, 5};
+ double rotation_angle = 2.0 / 3 * CV_PI;
+ DualQuatd dq2 = DualQuatd::createFromAngleAxisTrans(rotation_angle, axis, trans);
+ DualQuatd dqAllOne{1, 1, 1, 1, 1, 1, 1, 1};
+ DualQuatd dqAllZero{0, 0, 0, 0, 0, 0, 0, 0};
+ DualQuatd dqIdentity{1, 0, 0, 0, 0, 0, 0, 0};
+ DualQuatd dqTrans{1, 0, 0, 0, 0, 2, 3, 4};
+ DualQuatd dqOnlyTrans{0, 0, 0, 0, 0, 2, 3, 4};
+ DualQuatd dualNumber1{-3,0,0,0,-31.1,0,0,0};
+ DualQuatd dualNumber2{4,0,0,0,5.1,0,0,0};
+};
+
+TEST_F(DualQuatTest, constructor)
+{
+ EXPECT_EQ(dq1, DualQuatd::createFromQuat(Quatd(1, 2, 3, 4), Quatd(5, 6, 7, 8)));
+ EXPECT_EQ(dq2 * dq2.conjugate(), dqIdentity);
+ EXPECT_NEAR(dq2.getRotation(QUAT_ASSUME_UNIT).norm(), 1, 1e-6);
+ EXPECT_NEAR(dq2.getRealPart().dot(dq2.getDualPart()), 0, 1e-6);
+ EXPECT_MAT_NEAR(dq2.getTranslation(QUAT_ASSUME_UNIT), trans, 1e-6);
+ DualQuatd q_conj = DualQuatd::createFromQuat(dq2.getRealPart().conjugate(), -dq2.getDualPart().conjugate());
+ DualQuatd q{1,0,0,0,0,3,0,0};
+ EXPECT_EQ(dq2 * q * q_conj, DualQuatd(1,0,0,0,0,0,3,5));
+ Matx44d R1 = dq2.toMat();
+ DualQuatd dq3 = DualQuatd::createFromMat(R1);
+ EXPECT_EQ(dq3, dq2);
+ axis = axis / std::sqrt(axis.dot(axis));
+ Vec3d moment = 1.0 / 2 * (trans.cross(axis) + axis.cross(trans.cross(axis)) *
+ std::cos(rotation_angle / 2) / std::sin(rotation_angle / 2));
+ double d = trans.dot(axis);
+ DualQuatd dq4 = DualQuatd::createFromPitch(rotation_angle, d, axis, moment);
+ EXPECT_EQ(dq4, dq3);
+ EXPECT_EQ(dq2, DualQuatd::createFromAffine3(dq2.toAffine3()));
+ EXPECT_EQ(dq1.normalize(), DualQuatd::createFromAffine3(dq1.toAffine3()));
+}
+
+TEST_F(DualQuatTest, test_operator)
+{
+ DualQuatd dq_origin{1, 2, 3, 4, 5, 6, 7, 8};
+ EXPECT_EQ(dq1 - dqAllOne, DualQuatd(0, 1, 2, 3, 4, 5, 6, 7));
+ EXPECT_EQ(-dq1, DualQuatd(-1, -2, -3, -4, -5, -6, -7, -8));
+ EXPECT_EQ(dq1 + dqAllOne, DualQuatd(2, 3, 4, 5, 6, 7, 8, 9));
+ EXPECT_EQ(dq1 / dq1, dqIdentity);
+ DualQuatd dq3{-4, 1, 3, 2, -15.5, 0, -3, 8.5};
+ EXPECT_EQ(dq1 * dq2, dq3);
+ EXPECT_EQ(dq3 / dq2, dq1);
+ DualQuatd dq12{2, 4, 6, 8, 10, 12, 14, 16};
+ EXPECT_EQ(dq1 * 2.0, dq12);
+ EXPECT_EQ(2.0 * dq1, dq12);
+ EXPECT_EQ(dq1 - 1.0, DualQuatd(0, 2, 3, 4, 5, 6, 7, 8));
+ EXPECT_EQ(1.0 - dq1, DualQuatd(0, -2, -3, -4, -5, -6, -7, -8));
+ EXPECT_EQ(dq1 + 1.0, DualQuatd(2, 2, 3, 4, 5, 6, 7, 8));
+ EXPECT_EQ(1.0 + dq1, DualQuatd(2, 2, 3, 4, 5, 6, 7, 8));
+ dq1 += dq2;
+ EXPECT_EQ(dq1, dq_origin + dq2);
+ dq1 -= dq2;
+ EXPECT_EQ(dq1, dq_origin);
+ dq1 *= dq2;
+ EXPECT_EQ(dq1, dq_origin * dq2);
+ dq1 /= dq2;
+ EXPECT_EQ(dq1, dq_origin);
+}
+
+TEST_F(DualQuatTest, basic_ops)
+{
+ EXPECT_EQ(dq1.getRealPart(), Quatd(1, 2, 3, 4));
+ EXPECT_EQ(dq1.getDualPart(), Quatd(5, 6, 7, 8));
+ EXPECT_EQ((dq1 * dq2).conjugate(), conjugate(dq1 * dq2));
+ EXPECT_EQ(dq1.conjugate(), DualQuatd::createFromQuat(dq1.getRealPart().conjugate(), dq1.getDualPart().conjugate()));
+ EXPECT_EQ((dq2 * dq1).conjugate(), dq1.conjugate() * dq2.conjugate());
+ EXPECT_EQ(dq1.conjugate() * dq1, dq1.norm() * dq1.norm());
+ EXPECT_EQ(dq1.conjugate() * dq1, dq1.norm().power(2.0));
+ EXPECT_EQ(dualNumber2.power(2.0), DualQuatd(16, 0, 0, 0, 40.8, 0, 0, 0));
+ EXPECT_EQ(dq1.power(2.0), (2.0 * dq1.log()).exp());
+ EXPECT_EQ(power(dq1, 2.0), (exp(2.0 * log(dq1))));
+ EXPECT_EQ(dq2.power(3.0 / 2, QUAT_ASSUME_UNIT).power(4.0 / 3, QUAT_ASSUME_UNIT), dq2 * dq2);
+ EXPECT_EQ(dq2.power(-0.5).power(2.0), dq2.inv());
+ EXPECT_EQ(power(dq1, dq2), exp(dq2 * log(dq1)));
+ EXPECT_EQ(power(dq2, dq1, QUAT_ASSUME_UNIT), exp(dq1 * log(dq2)));
+ EXPECT_EQ((dq2.norm() * dq1).power(2.0), dq1.power(2.0) * dq2.norm().power(2.0));
+ DualQuatd q1norm = dq1.normalize();
+ EXPECT_EQ(dq2.norm(), dqIdentity);
+ EXPECT_NEAR(q1norm.getRealPart().norm(), 1, 1e-6);
+ EXPECT_NEAR(q1norm.getRealPart().dot(q1norm.getDualPart()), 0, 1e-6);
+ EXPECT_NEAR(dq1.getRotation().norm(), 1, 1e-6);
+ EXPECT_NEAR(dq2.getRotation(QUAT_ASSUME_UNIT).norm(), 1, 1e-6);
+ EXPECT_NEAR(dq2.getRotation(QUAT_ASSUME_UNIT).norm(), 1, 1e-6);
+ EXPECT_MAT_NEAR(Mat(dq2.getTranslation()), Mat(trans), 1e-6);
+ EXPECT_MAT_NEAR(Mat(q1norm.getTranslation(QUAT_ASSUME_UNIT)), Mat(dq1.getTranslation()), 1e-6);
+ EXPECT_EQ(dq2.getTranslation(), dq2.getTranslation(QUAT_ASSUME_UNIT));
+ EXPECT_EQ(dq1.inv() * dq1, dqIdentity);
+ EXPECT_EQ(inv(dq1) * dq1, dqIdentity);
+ EXPECT_EQ(dq2.inv(QUAT_ASSUME_UNIT) * dq2, dqIdentity);
+ EXPECT_EQ(inv(dq2, QUAT_ASSUME_UNIT) * dq2, dqIdentity);
+ EXPECT_EQ(dq2.inv(), dq2.conjugate());
+ EXPECT_EQ(dqIdentity.inv(), dqIdentity);
+ EXPECT_ANY_THROW(dqAllZero.inv());
+ EXPECT_EQ(dqAllZero.exp(), dqIdentity);
+ EXPECT_EQ(exp(dqAllZero), dqIdentity);
+ EXPECT_ANY_THROW(log(dqAllZero));
+ EXPECT_EQ(log(dqIdentity), dqAllZero);
+ EXPECT_EQ(dqIdentity.log(), dqAllZero);
+ EXPECT_EQ(dualNumber1 * dualNumber2, dualNumber2 * dualNumber1);
+ EXPECT_EQ(dualNumber2.exp().log(), dualNumber2);
+ EXPECT_EQ(dq2.log(QUAT_ASSUME_UNIT).exp(), dq2);
+ EXPECT_EQ(exp(log(dq2, QUAT_ASSUME_UNIT)), dq2);
+ EXPECT_EQ(dqIdentity.log(QUAT_ASSUME_UNIT).exp(), dqIdentity);
+ EXPECT_EQ(dq1.log().exp(), dq1);
+ EXPECT_EQ(dqTrans.log().exp(), dqTrans);
+ EXPECT_MAT_NEAR(q1norm.toMat(QUAT_ASSUME_UNIT), dq1.toMat(), 1e-6);
+ Matx44d R1 = dq2.toMat();
+ Mat point = (Mat_<double>(4, 1) << 3, 0, 0, 1);
+ Mat new_point = R1 * point;
+ Mat after = (Mat_<double>(4, 1) << 0, 3, 5 ,1);
+ EXPECT_MAT_NEAR(new_point, after, 1e-6);
+ Vec<double, 8> vec = dq1.toVec();
+ EXPECT_EQ(DualQuatd(vec), dq1);
+ Affine3d afd = q1norm.toAffine3(QUAT_ASSUME_UNIT);
+ EXPECT_MAT_NEAR(Mat(afd.translation()), Mat(q1norm.getTranslation(QUAT_ASSUME_UNIT)), 1e-6);
+ Affine3d dq1_afd = dq1.toAffine3();
+ EXPECT_MAT_NEAR(dq1_afd.matrix, afd.matrix, 1e-6);
+ EXPECT_ANY_THROW(dqAllZero.toAffine3());
+}
+
+TEST_F(DualQuatTest, interpolation)
+{
+ DualQuatd dq = DualQuatd::createFromAngleAxisTrans(8 * CV_PI / 5, Vec3d{0, 0, 1}, Vec3d{0, 0, 10});
+ EXPECT_EQ(DualQuatd::sclerp(dqIdentity, dq, 0.5), DualQuatd::sclerp(-dqIdentity, dq, 0.5, false));
+ EXPECT_EQ(DualQuatd::sclerp(dqIdentity, dq, 0), -dqIdentity);
+ EXPECT_EQ(DualQuatd::sclerp(dqIdentity, dq2, 1), dq2);
+ EXPECT_EQ(DualQuatd::sclerp(dqIdentity, dq2, 0.4, false, QUAT_ASSUME_UNIT), DualQuatd(0.91354546, 0.23482951, 0.23482951, 0.23482951, -0.23482951, -0.47824988, 0.69589767, 0.69589767));
+ EXPECT_EQ(DualQuatd::dqblend(dqIdentity, dq1.normalize(), 0.2, QUAT_ASSUME_UNIT), DualQuatd::dqblend(dqIdentity, -dq1, 0.2));
+ EXPECT_EQ(DualQuatd::dqblend(dqIdentity, dq2, 0.4), DualQuatd(0.91766294, 0.22941573, 0.22941573, 0.22941573, -0.21130397, -0.48298049, 0.66409818, 0.66409818));
+ DualQuatd gdb = DualQuatd::gdqblend(Vec<DualQuatd, 3>{dqIdentity, dq, dq2}, Vec3d{0.4, 0, 0.6}, QUAT_ASSUME_UNIT);
+ EXPECT_EQ(gdb, DualQuatd::dqblend(dqIdentity, dq2, 0.6));
+ EXPECT_ANY_THROW(DualQuatd::gdqblend(Vec<DualQuatd, 1>{dq2}, Vec2d{0.5, 0.5}));
+ Mat gdqb_d(1, 2, CV_64FC(7));
+ gdqb_d.at<Vec<double, 7>>(0, 0) = Vec<double, 7>{1,2,3,4,5,6,7};
+ gdqb_d.at<Vec<double, 7>>(0, 1) = Vec<double, 7>{1,2,3,4,5,6,7};
+ EXPECT_ANY_THROW(DualQuatd::gdqblend(gdqb_d, Vec2d{0.5, 0.5}));
+ Mat gdqb_f(1, 2, CV_32FC(8));
+ gdqb_f.at<Vec<float, 8>>(0, 0) = Vec<float, 8>{1.f,2.f,3.f,4.f,5.f,6.f,7.f,8.f};
+ gdqb_f.at<Vec<float, 8>>(0, 1) = Vec<float, 8>{1.f,2.f,3.f,4.f,5.f,6.f,7.f,8.f};
+ EXPECT_ANY_THROW(DualQuatd::gdqblend(gdqb_f, Vec2d{0.5, 0.5}));
+ EXPECT_ANY_THROW(DualQuatd::gdqblend(Vec<DualQuatd, 3>{dqIdentity, dq, dq2}, Vec3f{0.4f, 0.f, 0.6f}, QUAT_ASSUME_UNIT));
+ EXPECT_EQ(gdb, DualQuatd::gdqblend(Vec<DualQuatd, 3>{dqIdentity, dq * dualNumber1, -dq2}, Vec3d{0.4, 0, 0.6}));
+}
+
+
+}} // namespace
projects / platform / upstream / opencv.git / commitdiff