Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #19026 from chargerKong:dualquat

Browse Source 
Dual quaternion

* create dual quaternion;
basic operations, functions(exp,log,norm,inv), to/from mat, sclerp.

* add dqb, dqs, gdqb, to/from affine3;
change algorithm of norm, inv, getTranslation, createFromPitch, normalize；
change type translation to Vec3;
comment improve;

* try fix warning: unreferenced local function

* change exp calculation;
add func(obj) operations;

* Change the algorithm of log function;
add assumeUnit in getRotation;
remove dqs;
change std::vector to InputArray

* fix warning: doxygen and Vec<double, 0>

* fix warning: doxygen and Vec<double, 0>

* add inputarray param for gdqb

* change int to size_t

* win cl warning fix

* replace size_t by int at using Mat.at() function

* replace double by float

* interpolation fix

* replace (i, 0) to (i)

* core(quat): exclude ABI, test_dualquaternion=>test_quaternion.cpp

Co-authored-by: arsaratovtsev <arsaratovtsev@intel.com>
Co-authored-by: Alexander Alekhin <alexander.a.alekhin@gmail.com>
pull/19561/head
Liangqian 4 years ago committed by GitHub
parent b50c1ba9e6
commit 0aca3fb5af
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 1646 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cmake/templates/opencv_abi.xml.in
  2. 979
      modules/core/include/opencv2/core/dualquaternion.hpp
  3. 487
      modules/core/include/opencv2/core/dualquaternion.inl.hpp
  4. 2
      modules/core/include/opencv2/core/quaternion.inl.hpp
  5. 189
      modules/core/test/test_quaternion.cpp

2
cmake/templates/opencv_abi.xml.in
Unescape View File

@ -28,7 +28,7 @@
opencv2/core/opencl*
opencv2/core/parallel/backend/*
opencv2/core/private*
opencv2/core/quaternion*
opencv2/core/*quaternion*
opencv/cxeigen.hpp
opencv2/core/eigen.hpp
opencv2/flann/hdf5.h

979
modules/core/include/opencv2/core/dualquaternion.hpp
Unescape View File

@ -0,0 +1,979 @@
// 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 */

487
modules/core/include/opencv2/core/dualquaternion.inl.hpp
Unescape View File

@ -0,0 +1,487 @@
// 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*/

2
modules/core/include/opencv2/core/quaternion.inl.hpp
Unescape View File

@ -880,7 +880,7 @@ Quat<T> createFromAxisRot(int axis, const T theta)
CV_Assert(0);
}
static bool isIntAngleType(QuatEnum::EulerAnglesType eulerAnglesType)
inline bool isIntAngleType(QuatEnum::EulerAnglesType eulerAnglesType)
{
return eulerAnglesType < QuatEnum::EXT_XYZ;
}

189
modules/core/test/test_quaternion.cpp
Unescape View File

@ -3,11 +3,15 @@
// 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
{
@ -37,7 +41,8 @@ protected:
};
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);
@ -78,7 +83,8 @@ TEST_F(QuatTest, constructor){
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));
@ -160,7 +166,8 @@ TEST_F(QuatTest, basicfuns){
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};
@ -203,7 +210,8 @@ TEST_F(QuatTest, operator){
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();
@ -223,7 +231,8 @@ TEST_F(QuatTest, quatAttrs){
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);
@ -286,7 +295,8 @@ static const Quatd qEuler[24] = {
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))
{
@ -320,6 +330,163 @@ TEST_F(QuatTest, EulerAngles){
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

Write Preview
Loading…
Cancel
Save