mirror of https://github.com/opencv/opencv.git
Merge pull request #19026 from chargerKong:dualquat
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
5 changed files with 1646 additions and 13 deletions
@ -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 */ |
||||||
@ -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*/ |
||||||
Loading…
Reference in new issue