From 30c3abbc114245692f7d43d42381609baf276d9b Mon Sep 17 00:00:00 2001 From: chargerKong <49099366+chargerKong@users.noreply.github.com> Date: Fri, 20 Nov 2020 00:59:33 +0800 Subject: [PATCH] Merge pull request #18335 from chargerKong:master Ordinary quaternion * version 1.0 * add assumeUnit; add UnitTest; check boundary value; fix the func using method: func(obj); fix 4x4; add rodrigues vector transformation; fix mat to quat; * fix blank and tab * fix blank and tab modify test;cpp to hpp * mainly improve comment; add rvec2Quat;fix toRodrigues; fix throw to CV_Error * fix bug of quatd * int; combine hpp and cpp; fix << overload error in win system; modify include in test file; * move implementation to quaternion.ini.hpp; change some constructor to createFrom* function; change Rodrigues vector to rotation vector; change the matexpr to mat of 3x3 return type; improve comments; * try fix log function error in win * add enums for assumeUnit; improve docs; add using std::cos funcs * remove using std::* from header; add std::* in affine.hpp,warpers_inl.hpp; * quat: coding style * quat: AssumeType => QuatAssumeType --- modules/core/include/opencv2/core/affine.hpp | 2 +- .../core/include/opencv2/core/quaternion.hpp | 1194 +++++++++++++++++ .../include/opencv2/core/quaternion.inl.hpp | 849 ++++++++++++ modules/core/test/test_quaternion.cpp | 255 ++++ .../opencv2/stitching/detail/warpers_inl.hpp | 6 +- 5 files changed, 2302 insertions(+), 4 deletions(-) create mode 100644 modules/core/include/opencv2/core/quaternion.hpp create mode 100644 modules/core/include/opencv2/core/quaternion.inl.hpp create mode 100644 modules/core/test/test_quaternion.cpp diff --git a/modules/core/include/opencv2/core/affine.hpp b/modules/core/include/opencv2/core/affine.hpp index 7e2ed3078583..1806382e99ae 100644 --- a/modules/core/include/opencv2/core/affine.hpp +++ b/modules/core/include/opencv2/core/affine.hpp @@ -499,7 +499,7 @@ typename cv::Affine3::Vec3 cv::Affine3::rvec() const double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25); double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5; c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c; - double theta = acos(c); + double theta = std::acos(c); if( s < 1e-5 ) { diff --git a/modules/core/include/opencv2/core/quaternion.hpp b/modules/core/include/opencv2/core/quaternion.hpp new file mode 100644 index 000000000000..c72ee8c37fe7 --- /dev/null +++ b/modules/core/include/opencv2/core/quaternion.hpp @@ -0,0 +1,1194 @@ +// 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 +// Longbu Wang +#ifndef OPENCV_CORE_QUATERNION_HPP +#define OPENCV_CORE_QUATERNION_HPP + +#include +#include +namespace cv +{ +//! @addtogroup core +//! @{ + +//! Unit quaternion flag +enum QuatAssumeType +{ + /** + * This flag is specified by default. + * If this flag is specified, the input quaternions are assumed to be not unit quaternions. + * It can guarantee the correctness of the calculations, + * although the calculation speed will be slower than the flag QUAT_ASSUME_UNIT. + */ + QUAT_ASSUME_NOT_UNIT, + /** + * If this flag is specified, the input quaternions are assumed to be unit quaternions which + * will save some computations. However, if this flag is specified without unit quaternion, + * the program correctness of the result will not be guaranteed. + */ + QUAT_ASSUME_UNIT +}; + +template class Quat; +template std::ostream& operator<<(std::ostream&, const Quat<_Tp>&); + +/** + * Quaternion is a number system that extends the complex numbers. It can be expressed as a + * rotation in three-dimensional space. + * A quaternion is generally represented in the form: + * \f[q = w + x\boldsymbol{i} + y\boldsymbol{j} + z\boldsymbol{k}\f] + * \f[q = [w, x, y, z]\f] + * \f[q = [w, \boldsymbol{v}] \f] + * \f[q = ||q||[\cos\psi, u_x\sin\psi,u_y\sin\psi, u_z\sin\psi].\f] + * \f[q = ||q||[\cos\psi, \boldsymbol{u}\sin\psi]\f] + * where \f$\psi = \frac{\theta}{2}\f$, \f$\theta\f$ represents rotation angle, + * \f$\boldsymbol{u} = [u_x, u_y, u_z]\f$ represents normalized rotation axis, + * and \f$||q||\f$ represents the norm of \f$q\f$. + * + * A unit quaternion is usually represents rotation, which has the form: + * \f[q = [\cos\psi, u_x\sin\psi,u_y\sin\psi, u_z\sin\psi].\f] + * + * To create a quaternion representing the rotation around the axis \f$\boldsymbol{u}\f$ + * with angle \f$\theta\f$, you can use + * ``` + * using namespace cv; + * double angle = CV_PI; + * Vec3d axis = {0, 0, 1}; + * Quatd q = Quatd::createFromAngleAxis(angle, axis); + * ``` + * + * You can simply use four same type number to create a quaternion + * ``` + * Quatd q(1, 2, 3, 4); + * ``` + * Or use a Vec4d or Vec4f vector. + * ``` + * Vec4d vec{1, 2, 3, 4}; + * Quatd q(vec); + * ``` + * + * ``` + * Vec4f vec{1, 2, 3, 4}; + * Quatf q(vec); + * ``` + * + * If you already have a 3x3 rotation matrix R, then you can use + * ``` + * Quatd q = Quatd::createFromRotMat(R); + * ``` + * + * If you already have a rotation vector rvec which has the form of `angle * axis`, then you can use + * ``` + * Quatd q = Quatd::createFromRvec(rvec); + * ``` + * + * To extract the rotation matrix from quaternion, see toRotMat3x3() + * + * To extract the Vec4d or Vec4f, see toVec() + * + * To extract the rotation vector, see toRotVec() + * + * If there are two quaternions \f$q_0, q_1\f$ are needed to interpolate, you can use nlerp(), slerp() or spline() + * ``` + * Quatd::nlerp(q0, q1, t) + * + * Quatd::slerp(q0, q1, t) + * + * Quatd::spline(q0, q0, q1, q1, t) + * ``` + * spline can smoothly connect rotations of multiple quaternions + * + * Three ways to get an element in Quaternion + * ``` + * Quatf q(1,2,3,4); + * std::cout << q.w << std::endl; // w=1, x=2, y=3, z=4 + * std::cout << q[0] << std::endl; // q[0]=1, q[1]=2, q[2]=3, q[3]=4 + * std::cout << q.at(0) << std::endl; + * ``` + */ +template +class Quat +{ + static_assert(std::is_floating_point<_Tp>::value, "Quaternion only make sense with type of float or double"); + using value_type = _Tp; + +public: + static constexpr _Tp CV_QUAT_EPS = (_Tp)1.e-6; + + Quat(); + + /** + * @brief From Vec4d or Vec4f. + */ + explicit Quat(const Vec<_Tp, 4> &coeff); + + /** + * @brief from four numbers. + */ + Quat(_Tp w, _Tp x, _Tp y, _Tp z); + + /** + * @brief from an angle, axis. Axis will be normalized in this function. And + * it generates + * \f[q = [\cos\psi, u_x\sin\psi,u_y\sin\psi, u_z\sin\psi].\f] + * where \f$\psi = \frac{\theta}{2}\f$, \f$\theta\f$ is the rotation angle. + */ + static Quat<_Tp> createFromAngleAxis(const _Tp angle, const Vec<_Tp, 3> &axis); + + /** + * @brief from a 3x3 rotation matrix. + */ + static Quat<_Tp> createFromRotMat(InputArray R); + + /** + * @brief from a rotation vector + * \f$r\f$ has the form \f$\theta \cdot \boldsymbol{u}\f$, where \f$\theta\f$ + * represents rotation angle and \f$\boldsymbol{u}\f$ represents normalized rotation axis. + * + * Angle and axis could be easily derived as: + * \f[ + * \begin{equation} + * \begin{split} + * \psi &= ||r||\\ + * \boldsymbol{u} &= \frac{r}{\theta} + * \end{split} + * \end{equation} + * \f] + * Then a quaternion can be calculated by + * \f[q = [\cos\psi, \boldsymbol{u}\sin\psi]\f] + * where \f$\psi = \theta / 2 \f$ + */ + static Quat<_Tp> createFromRvec(InputArray rvec); + + /** + * @brief a way to get element. + * @param index over a range [0, 3]. + * + * A quaternion q + * + * q.at(0) is equivalent to q.w, + * + * q.at(1) is equivalent to q.x, + * + * q.at(2) is equivalent to q.y, + * + * q.at(3) is equivalent to q.z. + */ + _Tp at(size_t index) const; + + /** + * @brief return the conjugate of this quaternion. + * \f[q.conjugate() = (w, -x, -y, -z).\f] + */ + Quat<_Tp> conjugate() const; + + /** + * + * @brief return the value of exponential value. + * \f[\exp(q) = e^w (\cos||\boldsymbol{v}||+ \frac{v}{||\boldsymbol{v}||})\sin||\boldsymbol{v}||\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example: + * ``` + * Quatd q{1,2,3,4}; + * cout << exp(q) << endl; + * ``` + */ + template + friend Quat exp(const Quat &q); + + /** + * @brief return the value of exponential value. + * \f[\exp(q) = e^w (\cos||\boldsymbol{v}||+ \frac{v}{||\boldsymbol{v}||}\sin||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q{1,2,3,4}; + * cout << q.exp() << endl; + * ``` + */ + Quat<_Tp> exp() const; + + /** + * @brief return the value of logarithm function. + * \f[\ln(q) = \ln||q|| + \frac{\boldsymbol{v}}{||\boldsymbol{v}||}\arccos\frac{w}{||q||}.\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * @param assumeUnit if QUAT_ASSUME_UNIT, q assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q1{1,2,3,4}; + * cout << log(q1) << endl; + * ``` + */ + template + friend Quat log(const Quat &q, QuatAssumeType assumeUnit); + + /** + * @brief return the value of logarithm function. + * \f[\ln(q) = \ln||q|| + \frac{\boldsymbol{v}}{||\boldsymbol{v}||}\arccos\frac{w}{||q||}\f]. + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.log(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * Quatd q1(1,2,3,4); + * q1.normalize().log(assumeUnit); + * ``` + */ + Quat<_Tp> log(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return the value of power function with index \f$x\f$. + * \f[q^x = ||q||(cos(x\theta) + \boldsymbol{u}sin(x\theta))).\f] + * @param q a quaternion. + * @param x index of exponentiation. + * @param assumeUnit if QUAT_ASSUME_UNIT, quaternion q assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * power(q, 2); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * double angle = CV_PI; + * Vec3d axis{0, 0, 1}; + * Quatd q1 = Quatd::createFromAngleAxis(angle, axis); //generate a unit quat by axis and angle + * power(q1, 2, assumeUnit);//This assumeUnit means q1 is a unit quaternion. + * ``` + */ + template + friend Quat power(const Quat &q, _T x, QuatAssumeType assumeUnit); + + /** + * @brief return the value of power function with index \f$x\f$. + * \f[q^x = ||q||(\cos(x\theta) + \boldsymbol{u}\sin(x\theta))).\f] + * @param x index of exponentiation. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.power(2); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * double angle = CV_PI; + * Vec3d axis{0, 0, 1}; + * Quatd q1 = Quatd::createFromAngleAxis(angle, axis); //generate a unit quat by axis and angle + * q1.power(2, assumeUnit); //This assumeUnt means q1 is a unit quaternion + * ``` + */ + template + Quat<_Tp> power(_T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return \f$\sqrt{q}\f$. + * @param q a quaternion. + * @param assumeUnit if QUAT_ASSUME_UNIT, quaternion q assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatf q(1,2,3,4); + * sqrt(q); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q = {1,0,0,0}; + * sqrt(q, assumeUnit); //This assumeUnit means q is a unit quaternion. + * ``` + */ + template + friend Quat sqrt(const Quat &q, QuatAssumeType assumeUnit); + + /** + * @brief return \f$\sqrt{q}\f$. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatf q(1,2,3,4); + * q.sqrt(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q = {1,0,0,0}; + * q.sqrt(assumeUnit); //This assumeUnit means q is a unit quaternion + * ``` + */ + Quat<_Tp> sqrt(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return the value of power function with quaternion \f$q\f$. + * \f[p^q = e^{q\ln(p)}.\f] + * @param p base quaternion of power function. + * @param q index quaternion of power function. + * @param assumeUnit if QUAT_ASSUME_UNIT, quaternion \f$p\f$ assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd p(1,2,3,4); + * Quatd q(5,6,7,8); + * power(p, q); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * p = p.normalize(); + * power(p, q, assumeUnit); //This assumeUnit means p is a unit quaternion + * ``` + */ + template + friend Quat power(const Quat &p, const Quat &q, QuatAssumeType assumeUnit); + + /** + * @brief return the value of power function with quaternion \f$q\f$. + * \f[p^q = e^{q\ln(p)}.\f] + * @param q index quaternion of power function. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd p(1,2,3,4); + * Quatd q(5,6,7,8); + * p.power(q); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * p = p.normalize(); + * p.power(q, assumeUnit); //This assumeUnit means p is a unit quaternion + * ``` + */ + Quat<_Tp> power(const Quat<_Tp> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return the crossProduct between \f$p = (a, b, c, d) = (a, \boldsymbol{u})\f$ and \f$q = (w, x, y, z) = (w, \boldsymbol{v})\f$. + * \f[p \times q = \frac{pq- qp}{2}\f] + * \f[p \times q = \boldsymbol{u} \times \boldsymbol{v}\f] + * \f[p \times q = (cz-dy)i + (dx-bz)j + (by-xc)k \f] + * + * For example + * ``` + * Quatd q{1,2,3,4}; + * Quatd p{5,6,7,8}; + * crossProduct(p, q); + * ``` + */ + template + friend Quat crossProduct(const Quat &p, const Quat &q); + + /** + * @brief return the crossProduct between \f$p = (a, b, c, d) = (a, \boldsymbol{u})\f$ and \f$q = (w, x, y, z) = (w, \boldsymbol{v})\f$. + * \f[p \times q = \frac{pq- qp}{2}.\f] + * \f[p \times q = \boldsymbol{u} \times \boldsymbol{v}.\f] + * \f[p \times q = (cz-dy)i + (dx-bz)j + (by-xc)k. \f] + * + * For example + * ``` + * Quatd q{1,2,3,4}; + * Quatd p{5,6,7,8}; + * p.crossProduct(q) + * ``` + */ + Quat<_Tp> crossProduct(const Quat<_Tp> &q) const; + + /** + * @brief return the norm of quaternion. + * \f[||q|| = \sqrt{w^2 + x^2 + y^2 + z^2}.\f] + */ + _Tp norm() const; + + /** + * @brief return a normalized \f$p\f$. + * \f[p = \frac{q}{||q||}\f] + * where \f$p\f$ satisfies \f$(p.x)^2 + (p.y)^2 + (p.z)^2 + (p.w)^2 = 1.\f$ + */ + Quat<_Tp> normalize() const; + + /** + * @brief return \f$q^{-1}\f$ which is an inverse of \f$q\f$ + * which satisfies \f$q * q^{-1} = 1\f$. + * @param q a quaternion. + * @param assumeUnit if QUAT_ASSUME_UNIT, quaternion q assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * inv(q); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q = q.normalize(); + * inv(q, assumeUnit);//This assumeUnit means p is a unit quaternion + * ``` + */ + template + friend Quat inv(const Quat &q, QuatAssumeType assumeUnit); + + /** + * @brief return \f$q^{-1}\f$ which is an inverse of \f$q\f$ + * satisfying \f$q * q^{-1} = 1\f$. + * @param assumeUnit if QUAT_ASSUME_UNIT, quaternion q assume to be a unit quaternion and this function will save some computations. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.inv(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q = q.normalize(); + * q.inv(assumeUnit); //assumeUnit means p is a unit quaternion + * ``` + */ + Quat<_Tp> inv(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return sinh value of quaternion q, sinh could be calculated as: + * \f[\sinh(p) = \sin(w)\cos(||\boldsymbol{v}||) + \cosh(w)\frac{v}{||\boldsymbol{v}||}\sin||\boldsymbol{v}||\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * sinh(q); + * ``` + */ + template + friend Quat sinh(const Quat &q); + + /** + * @brief return sinh value of this quaternion, sinh could be calculated as: + * \f$\sinh(p) = \sin(w)\cos(||\boldsymbol{v}||) + \cosh(w)\frac{v}{||\boldsymbol{v}||}\sin||\boldsymbol{v}||\f$ + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.sinh(); + * ``` + */ + Quat<_Tp> sinh() const; + + /** + * @brief return cosh value of quaternion q, cosh could be calculated as: + * \f[\cosh(p) = \cosh(w) * \cos(||\boldsymbol{v}||) + \sinh(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}\sin(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * cosh(q); + * ``` + */ + template + friend Quat cosh(const Quat &q); + + /** + * @brief return cosh value of this quaternion, cosh could be calculated as: + * \f[\cosh(p) = \cosh(w) * \cos(||\boldsymbol{v}||) + \sinh(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}sin(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.cosh(); + * ``` + */ + Quat<_Tp> cosh() const; + + /** + * @brief return tanh value of quaternion q, tanh could be calculated as: + * \f[ \tanh(q) = \frac{\sinh(q)}{\cosh(q)}.\f] + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * tanh(q); + * ``` + * @sa sinh, cosh + */ + template + friend Quat tanh(const Quat &q); + + /** + * @brief return tanh value of this quaternion, tanh could be calculated as: + * \f[ \tanh(q) = \frac{\sinh(q)}{\cosh(q)}.\f] + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.tanh(); + * ``` + * @sa sinh, cosh + */ + Quat<_Tp> tanh() const; + + /** + * @brief return tanh value of quaternion q, sin could be calculated as: + * \f[\sin(p) = \sin(w) * \cosh(||\boldsymbol{v}||) + \cos(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}\sinh(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * sin(q); + * ``` + */ + template + friend Quat sin(const Quat &q); + + /** + * @brief return sin value of this quaternion, sin could be calculated as: + * \f[\sin(p) = \sin(w) * \cosh(||\boldsymbol{v}||) + \cos(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}\sinh(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.sin(); + * ``` + */ + Quat<_Tp> sin() const; + + /** + * @brief return sin value of quaternion q, cos could be calculated as: + * \f[\cos(p) = \cos(w) * \cosh(||\boldsymbol{v}||) - \sin(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}\sinh(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * cos(q); + * ``` + */ + template + friend Quat cos(const Quat &q); + + /** + * @brief return cos value of this quaternion, cos could be calculated as: + * \f[\cos(p) = \cos(w) * \cosh(||\boldsymbol{v}||) - \sin(w)\frac{\boldsymbol{v}}{||\boldsymbol{v}||}\sinh(||\boldsymbol{v}||)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.cos(); + * ``` + */ + Quat<_Tp> cos() const; + + /** + * @brief return tan value of quaternion q, tan could be calculated as: + * \f[\tan(q) = \frac{\sin(q)}{\cos(q)}.\f] + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * tan(q); + * ``` + */ + template + friend Quat tan(const Quat &q); + + /** + * @brief return tan value of this quaternion, tan could be calculated as: + * \f[\tan(q) = \frac{\sin(q)}{\cos(q)}.\f] + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.tan(); + * ``` + */ + Quat<_Tp> tan() const; + + /** + * @brief return arcsin value of quaternion q, arcsin could be calculated as: + * \f[\arcsin(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arcsinh(q\frac{\boldsymbol{v}}{||\boldsymbol{v}||})\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * asin(q); + * ``` + */ + template + friend Quat asin(const Quat &q); + + /** + * @brief return arcsin value of this quaternion, arcsin could be calculated as: + * \f[\arcsin(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arcsinh(q\frac{\boldsymbol{v}}{||\boldsymbol{v}||})\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.asin(); + * ``` + */ + Quat<_Tp> asin() const; + + /** + * @brief return arccos value of quaternion q, arccos could be calculated as: + * \f[\arccos(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arccosh(q)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * acos(q); + * ``` + */ + template + friend Quat acos(const Quat &q); + + /** + * @brief return arccos value of this quaternion, arccos could be calculated as: + * \f[\arccos(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arccosh(q)\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.acos(); + * ``` + */ + Quat<_Tp> acos() const; + + /** + * @brief return arctan value of quaternion q, arctan could be calculated as: + * \f[\arctan(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arctanh(q\frac{\boldsymbol{v}}{||\boldsymbol{v}||})\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * atan(q); + * ``` + */ + template + friend Quat atan(const Quat &q); + + /** + * @brief return arctan value of this quaternion, arctan could be calculated as: + * \f[\arctan(q) = -\frac{\boldsymbol{v}}{||\boldsymbol{v}||}arctanh(q\frac{\boldsymbol{v}}{||\boldsymbol{v}||})\f] + * where \f$\boldsymbol{v} = [x, y, z].\f$ + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.atan(); + * ``` + */ + Quat<_Tp> atan() const; + + /** + * @brief return arcsinh value of quaternion q, arcsinh could be calculated as: + * \f[arcsinh(q) = \ln(q + \sqrt{q^2 + 1})\f]. + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * asinh(q); + * ``` + */ + template + friend Quat asinh(const Quat &q); + + /** + * @brief return arcsinh value of this quaternion, arcsinh could be calculated as: + * \f[arcsinh(q) = \ln(q + \sqrt{q^2 + 1})\f]. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.asinh(); + * ``` + */ + Quat<_Tp> asinh() const; + + /** + * @brief return arccosh value of quaternion q, arccosh could be calculated as: + * \f[arccosh(q) = \ln(q + \sqrt{q^2 - 1})\f]. + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * acosh(q); + * ``` + */ + template + friend Quat acosh(const Quat &q); + + /** + * @brief return arccosh value of this quaternion, arccosh could be calculated as: + * \f[arcosh(q) = \ln(q + \sqrt{q^2 - 1})\f]. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.acosh(); + * ``` + */ + Quat<_Tp> acosh() const; + + /** + * @brief return arctanh value of quaternion q, arctanh could be calculated as: + * \f[arctanh(q) = \frac{\ln(q + 1) - \ln(1 - q)}{2}\f]. + * @param q a quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * atanh(q); + * ``` + */ + template + friend Quat atanh(const Quat &q); + + /** + * @brief return arctanh value of this quaternion, arctanh could be calculated as: + * \f[arcsinh(q) = \frac{\ln(q + 1) - \ln(1 - q)}{2}\f]. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.atanh(); + * ``` + */ + Quat<_Tp> atanh() const; + + /** + * @brief return true if this quaternion is a unit quaternion. + * @param eps tolerance scope of normalization. The eps could be defined as + * + * \f[eps = |1 - dotValue|\f] where \f[dotValue = (this.w^2 + this.x^2 + this,y^2 + this.z^2).\f] + * And this function will consider it is normalized when the dotValue over a range \f$[1-eps, 1+eps]\f$. + */ + bool isNormal(_Tp eps=CV_QUAT_EPS) const; + + /** + * @brief to throw an error if this quaternion is not a unit quaternion. + * @param eps tolerance scope of normalization. + * @sa isNormal + */ + void assertNormal(_Tp eps=CV_QUAT_EPS) const; + + /** + * @brief transform a quaternion to a 3x3 rotation matrix. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and + * this function will save some computations. Otherwise, this function will normalized this + * quaternion at first then to do the transformation. + * + * @note Matrix A which is to be rotated should have 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 + * \end{bmatrix}\f] + * where the same subscript represents a point. The shape of A assume to be [3, n] + * The points matrix A can be rotated by toRotMat3x3() * A. + * The result has 3 rows and n columns too. + + * For example + * ``` + * double angle = CV_PI; + * Vec3d axis{0,0,1}; + * Quatd q_unit = Quatd::createFromAngleAxis(angle, axis); //quaternion could also be get by interpolation by two or more quaternions. + * + * //assume there is two points (1,0,0) and (1,0,1) to be rotated + * Mat pointsA = (Mat_(2, 3) << 1,0,0,1,0,1); + * //change the shape + * pointsA = pointsA.t(); + * // rotate 180 degrees around the z axis + * Mat new_point = q_unit.toRotMat3x3() * pointsA; + * // print two points + * cout << new_point << endl; + * ``` + */ + Matx<_Tp, 3, 3> toRotMat3x3(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief transform a quaternion to a 4x4 rotation matrix. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and + * this function will save some computations. Otherwise, this function will normalized this + * quaternion at first then to do the transformation. + * + * The operations is similar as toRotMat3x3 + * except that the points matrix should have 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\\ + * 0&0&0&...&0 + * \end{bmatrix}\f] + * + * @sa toRotMat3x3 + */ + Matx<_Tp, 4, 4> toRotMat4x4(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief transform the this quaternion to a Vec. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.toVec(); + * ``` + */ + Vec<_Tp, 4> toVec() const; + + /** + * @brief transform this quaternion to a Rotation vector. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and + * this function will save some computations. + * Rotation vector rVec is defined as: + * \f[ rVec = [\theta v_x, \theta v_y, \theta v_z]\f] + * where \f$\theta\f$ represents rotation angle, and \f$\boldsymbol{v}\f$ represents the normalized rotation axis. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.toRotVec(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q.normalize().toRotVec(assumeUnit); //answer is same as q.toRotVec(). + * ``` + */ + Vec<_Tp, 3> toRotVec(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief get the angle of quaternion, it returns the rotation angle. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and + * this function will save some computations. + * \f[\psi = 2 *arccos(\frac{w}{||q||})\f] + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.getAngle(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q.normalize().getAngle(assumeUnit);//same as q.getAngle(). + * ``` + * @note It always return the value between \f$[0, 2\pi]\f$. + */ + _Tp getAngle(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief get the axis of quaternion, it returns a vector of length 3. + * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and + * this function will save some computations. + * + * the unit axis \f$\boldsymbol{u}\f$ is defined by + * \f[\begin{equation} + * \begin{split} + * \boldsymbol{v} + * &= \boldsymbol{u} ||\boldsymbol{v}||\\ + * &= \boldsymbol{u}||q||sin(\frac{\theta}{2}) + * \end{split} + * \end{equation}\f] + * where \f$v=[x, y ,z]\f$ and \f$\theta\f$ represents rotation angle. + * + * + * For example + * ``` + * Quatd q(1,2,3,4); + * q.getAxis(); + * + * QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + * q.normalize().getAxis(assumeUnit);//same as q.getAxis() + * ``` + */ + Vec<_Tp, 3> getAxis(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + + /** + * @brief return the dot between quaternion \f$q\f$ and this quaternion. + * + * dot(p, q) is a good metric of how close the quaternions are. + * Indeed, consider the unit quaternion difference \f$p^{-1} * q\f$, its real part is dot(p, q). + * At the same time its real part is equal to \f$\cos(\beta/2)\f$ where \f$\beta\f$ is + * an angle of rotation between p and q, i.e., + * Therefore, the closer dot(p, q) to 1, + * the smaller rotation between them. + * \f[p \cdot q = p.w \cdot q.w + p.x \cdot q.x + p.y \cdot q.y + p.z \cdot q.z\f] + * @param q the other quaternion. + * + * For example + * ``` + * Quatd q(1,2,3,4); + * Quatd p(5,6,7,8); + * p.dot(q); + * ``` + */ + _Tp dot(Quat<_Tp> q) const; + + /** + * @brief To calculate the interpolation from \f$q_0\f$ to \f$q_1\f$ by Linear Interpolation(Nlerp) + * For two quaternions, this interpolation curve can be displayed as: + * \f[Lerp(q_0, q_1, t) = (1 - t)q_0 + tq_1.\f] + * Obviously, the lerp will interpolate along a straight line if we think of \f$q_0\f$ and \f$q_1\f$ as a vector + * in a two-dimensional space. When \f$t = 0\f$, it returns \f$q_0\f$ and when \f$t= 1\f$, it returns \f$q_1\f$. + * \f$t\f$ should to be ranged in \f$[0, 1]\f$ normally. + * @param q0 a quaternion used in linear interpolation. + * @param q1 a quaternion used in linear interpolation. + * @param t percent of vector \f$\overrightarrow{q_0q_1}\f$ over a range [0, 1]. + * @note it returns a non-unit quaternion. + */ + static Quat<_Tp> lerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t); + + /** + * @brief To calculate the interpolation from \f$q_0\f$ to \f$q_1\f$ by Normalized Linear Interpolation(Nlerp). + * it returns a normalized quaternion of Linear Interpolation(Lerp). + * \f[ Nlerp(q_0, q_1, t) = \frac{(1 - t)q_0 + tq_1}{||(1 - t)q_0 + tq_1||}.\f] + * The interpolation will always choose the shortest path but the constant speed is not guaranteed. + * @param q0 a quaternion used in normalized linear interpolation. + * @param q1 a quaternion used in normalized linear interpolation. + * @param t percent of vector \f$\overrightarrow{q_0q_1}\f$ over a range [0, 1]. + * @param assumeUnit if QUAT_ASSUME_UNIT, all input quaternions assume to be unit quaternion. Otherwise, all inputs + quaternion will be normalized inside the function. + * @sa lerp + */ + static Quat<_Tp> nlerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + + /** + @brief To calculate the interpolation between \f$q_0\f$ and \f$q_1\f$ by Spherical Linear + Interpolation(Slerp), which can be defined as: + \f[ Slerp(q_0, q_1, t) = \frac{\sin((1-t)\theta)}{\sin(\theta)}q_0 + \frac{\sin(t\theta)}{\sin(\theta)}q_1\f] + where \f$\theta\f$ can be calculated as: + \f[\theta=cos^{-1}(q_0\cdot q_1)\f] + resulting from the both of their norm is unit. + @param q0 a quaternion used in Slerp. + @param q1 a quaternion used in Slerp. + @param t percent of angle between \f$q_0\f$ and \f$q_1\f$ over a range [0, 1]. + @param assumeUnit if QUAT_ASSUME_UNIT, all input quaternions assume to be unit quaternions. Otherwise, all input + quaternions will be normalized inside the function. + @param directChange if QUAT_ASSUME_UNIT, the interpolation will choose the nearest path. + @note If the interpolation angle is small, the error between Nlerp and Slerp is not so large. To improve efficiency and + avoid zero division error, we use Nlerp instead of Slerp. + */ + static Quat<_Tp> slerp(const Quat<_Tp> &q0, const Quat &q1, const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT, bool directChange=true); + + /** + * @brief To calculate the interpolation between \f$q_0\f$,\f$q_1\f$,\f$q_2\f$,\f$q_3\f$ by Spherical and quadrangle(Squad). This could be defined as: + * \f[Squad(q_i, s_i, s_{i+1}, q_{i+1}, t) = Slerp(Slerp(q_i, q_{i+1}, t), Slerp(s_i, s_{i+1}, t), 2t(1-t))\f] + * where + * \f[s_i = q_i\exp(-\frac{\log(q^*_iq_{i+1}) + \log(q^*_iq_{i-1})}{4})\f] + * + * The Squad expression is analogous to the \f$B\acute{e}zier\f$ curve, but involves spherical linear + * interpolation instead of simple linear interpolation. Each \f$s_i\f$ needs to be calculated by three + * quaternions. + * + * @param q0 the first quaternion. + * @param s0 the second quaternion. + * @param s1 the third quaternion. + * @param q1 thr fourth quaternion. + * @param t interpolation parameter of quadratic and linear interpolation over a range \f$[0, 1]\f$. + * @param assumeUnit if QUAT_ASSUME_UNIT, all input quaternions assume to be unit quaternion. Otherwise, all input + * quaternions will be normalized inside the function. + * @param directChange if QUAT_ASSUME_UNIT, squad will find the nearest path to interpolate. + * @sa interPoint, spline + */ + static Quat<_Tp> squad(const Quat<_Tp> &q0, const Quat<_Tp> &s0, + const Quat<_Tp> &s1, const Quat<_Tp> &q1, + const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT, + bool directChange=true); + + /** + * @brief This is the part calculation of squad. + * To calculate the intermedia quaternion \f$s_i\f$ between each three quaternion + * \f[s_i = q_i\exp(-\frac{\log(q^*_iq_{i+1}) + \log(q^*_iq_{i-1})}{4}).\f] + * @param q0 the first quaternion. + * @param q1 the second quaternion. + * @param q2 the third quaternion. + * @param assumeUnit if QUAT_ASSUME_UNIT, all input quaternions assume to be unit quaternion. Otherwise, all input + * quaternions will be normalized inside the function. + * @sa squad + */ + static Quat<_Tp> interPoint(const Quat<_Tp> &q0, const Quat<_Tp> &q1, + const Quat<_Tp> &q2, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + + /** + * @brief to calculate a quaternion which is the result of a \f$C^1\f$ continuous + * spline curve constructed by squad at the ratio t. Here, the interpolation values are + * between \f$q_1\f$ and \f$q_2\f$. \f$q_0\f$ and \f$q_2\f$ are used to ensure the \f$C^1\f$ + * continuity. if t = 0, it returns \f$q_1\f$, if t = 1, it returns \f$q_2\f$. + * @param q0 the first input quaternion to ensure \f$C^1\f$ continuity. + * @param q1 the second input quaternion. + * @param q2 the third input quaternion. + * @param q3 the fourth input quaternion the same use of \f$q1\f$. + * @param t ratio over a range [0, 1]. + * @param assumeUnit if QUAT_ASSUME_UNIT, \f$q_0, q_1, q_2, q_3\f$ assume to be unit quaternion. Otherwise, all input + * quaternions will be normalized inside the function. + * + * For example: + * + * If there are three double quaternions \f$v_0, v_1, v_2\f$ waiting to be interpolated. + * + * Interpolation between \f$v_0\f$ and \f$v_1\f$ with a ratio \f$t_0\f$ could be calculated as + * ``` + * Quatd::spline(v0, v0, v1, v2, t0); + * ``` + * Interpolation between \f$v_1\f$ and \f$v_2\f$ with a ratio \f$t_0\f$ could be calculated as + * ``` + * Quatd::spline(v0, v1, v2, v2, t0); + * ``` + * @sa squad, slerp + */ + static Quat<_Tp> spline(const Quat<_Tp> &q0, const Quat<_Tp> &q1, + const Quat<_Tp> &q2, const Quat<_Tp> &q3, + const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + + + Quat<_Tp> operator-() const; + + bool operator==(const Quat<_Tp>&) const; + + Quat<_Tp> operator+(const Quat<_Tp>&) const; + + Quat<_Tp>& operator+=(const Quat<_Tp>&); + + Quat<_Tp> operator-(const Quat<_Tp>&) const; + + Quat<_Tp>& operator-=(const Quat<_Tp>&); + + Quat<_Tp>& operator*=(const Quat<_Tp>&); + + Quat<_Tp>& operator*=(const _Tp&); + + Quat<_Tp> operator*(const Quat<_Tp>&) const; + + Quat<_Tp> operator/(const _Tp&) const; + + Quat<_Tp> operator/(const Quat<_Tp>&) const; + + Quat<_Tp>& operator/=(const _Tp&); + + Quat<_Tp>& operator/=(const Quat<_Tp>&); + + _Tp& operator[](std::size_t n); + + const _Tp& operator[](std::size_t n) const; + + template + friend Quat cv::operator*(const T, const Quat&); + + template + friend Quat cv::operator*(const Quat&, const T); + + template + friend std::ostream& cv::operator<<(std::ostream&, const Quat&); + + _Tp w, x, y, z; + +}; + +template +Quat inv(const Quat &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + +template +Quat sinh(const Quat &q); + +template +Quat cosh(const Quat &q); + +template +Quat tanh(const Quat &q); + +template +Quat sin(const Quat &q); + +template +Quat cos(const Quat &q); + +template +Quat tan(const Quat &q); + +template +Quat asinh(const Quat &q); + +template +Quat acosh(const Quat &q); + +template +Quat atanh(const Quat &q); + +template +Quat asin(const Quat &q); + +template +Quat acos(const Quat &q); + +template +Quat atan(const Quat &q); + +template +Quat power(const Quat &q, const Quat &p, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + +template +Quat exp(const Quat &q); + +template +Quat log(const Quat &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + +template +Quat power(const Quat& q, _T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + +template +Quat crossProduct(const Quat &p, const Quat &q); + +template +Quat sqrt(const Quat &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); + +template +Quat operator*(const T, const Quat&); + +template +Quat operator*(const Quat&, const T); + +template +std::ostream& operator<<(std::ostream&, const Quat&); + +using Quatd = Quat; +using Quatf = Quat; + +//! @} core +} + +#include "opencv2/core/quaternion.inl.hpp" + +#endif /* OPENCV_CORE_QUATERNION_HPP */ diff --git a/modules/core/include/opencv2/core/quaternion.inl.hpp b/modules/core/include/opencv2/core/quaternion.inl.hpp new file mode 100644 index 000000000000..769f53ed4b39 --- /dev/null +++ b/modules/core/include/opencv2/core/quaternion.inl.hpp @@ -0,0 +1,849 @@ +// 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 +// Longbu Wang + +#ifndef OPENCV_CORE_QUATERNION_INL_HPP +#define OPENCV_CORE_QUATERNION_INL_HPP + +#ifndef OPENCV_CORE_QUATERNION_HPP +#erorr This is not a standalone header. Include quaternion.hpp instead. +#endif + +//@cond IGNORE +/////////////////////////////////////////////////////////////////////////////////////// +//Implementation +namespace cv { + +template +Quat::Quat() : w(0), x(0), y(0), z(0) {} + +template +Quat::Quat(const Vec &coeff):w(coeff[0]), x(coeff[1]), y(coeff[2]), z(coeff[3]){} + +template +Quat::Quat(const T qw, const T qx, const T qy, const T qz):w(qw), x(qx), y(qy), z(qz){} + +template +Quat Quat::createFromAngleAxis(const T angle, const Vec &axis) +{ + T w, x, y, z; + T vNorm = std::sqrt(axis.dot(axis)); + if (vNorm < CV_QUAT_EPS) + { + CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation"); + } + const T angle_half = angle * 0.5; + w = std::cos(angle_half); + const T sin_v = std::sin(angle_half); + const T sin_norm = sin_v / vNorm; + x = sin_norm * axis[0]; + y = sin_norm * axis[1]; + z = sin_norm * axis[2]; + return Quat(w, x, y, z); +} + +template +Quat Quat::createFromRotMat(InputArray _R) +{ + CV_CheckTypeEQ(_R.type(), cv::traits::Type::value, ""); + if (_R.rows() != 3 || _R.cols() != 3) + { + CV_Error(Error::StsBadArg, "Cannot convert matrix to quaternion: rotation matrix should be a 3x3 matrix"); + } + Matx R; + _R.copyTo(R); + + T S, w, x, y, z; + T trace = R(0, 0) + R(1, 1) + R(2, 2); + if (trace > 0) + { + S = std::sqrt(trace + 1) * 2; + x = (R(1, 2) - R(2, 1)) / S; + y = (R(2, 0) - R(0, 2)) / S; + z = (R(0, 1) - R(1, 0)) / S; + w = -0.25 * S; + } + else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) + { + + S = std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2; + x = -0.25 * S; + y = -(R(1, 0) + R(0, 1)) / S; + z = -(R(0, 2) + R(2, 0)) / S; + w = (R(1, 2) - R(2, 1)) / S; + } + else if (R(1, 1) > R(2, 2)) + { + S = std::sqrt(1.0 - R(0, 0) + R(1, 1) - R(2, 2)) * 2; + x = (R(0, 1) + R(1, 0)) / S; + y = 0.25 * S; + z = (R(1, 2) + R(2, 1)) / S; + w = (R(0, 2) - R(2, 0)) / S; + } + else + { + S = std::sqrt(1.0 - R(0, 0) - R(1, 1) + R(2, 2)) * 2; + x = (R(0, 2) + R(2, 0)) / S; + y = (R(1, 2) + R(2, 1)) / S; + z = 0.25 * S; + w = -(R(0, 1) - R(1, 0)) / S; + } + return Quat (w, x, y, z); +} + +template +Quat Quat::createFromRvec(InputArray _rvec) +{ + if (!((_rvec.cols() == 1 && _rvec.rows() == 3) || (_rvec.cols() == 3 && _rvec.rows() == 1))) { + CV_Error(Error::StsBadArg, "Cannot convert rotation vector to quaternion: The length of rotation vector should be 3"); + } + Vec rvec; + _rvec.copyTo(rvec); + T psi = std::sqrt(rvec.dot(rvec)); + if (abs(psi) < CV_QUAT_EPS) { + return Quat (1, 0, 0, 0); + } + Vec axis = rvec / psi; + return createFromAngleAxis(psi, axis); +} + +template +inline Quat Quat::operator-() const +{ + return Quat(-w, -x, -y, -z); +} + + +template +inline bool Quat::operator==(const Quat &q) const +{ + return (abs(w - q.w) < CV_QUAT_EPS && abs(x - q.x) < CV_QUAT_EPS && abs(y - q.y) < CV_QUAT_EPS && abs(z - q.z) < CV_QUAT_EPS); +} + +template +inline Quat Quat::operator+(const Quat &q1) const +{ + return Quat(w + q1.w, x + q1.x, y + q1.y, z + q1.z); +} + +template +inline Quat Quat::operator-(const Quat &q1) const +{ + return Quat(w - q1.w, x - q1.x, y - q1.y, z - q1.z); +} + +template +inline Quat& Quat::operator+=(const Quat &q1) +{ + w += q1.w; + x += q1.x; + y += q1.y; + z += q1.z; + return *this; +} + +template +inline Quat& Quat::operator-=(const Quat &q1) +{ + w -= q1.w; + x -= q1.x; + y -= q1.y; + z -= q1.z; + return *this; +} + +template +inline Quat Quat::operator*(const Quat &q1) const +{ + Vec q{w, x, y, z}; + Vec q2{q1.w, q1.x, q1.y, q1.z}; + return Quat(q * q2); +} + + +template +Quat operator*(const Quat &q1, const S a) +{ + return Quat(a * q1.w, a * q1.x, a * q1.y, a * q1.z); +} + +template +Quat operator*(const S a, const Quat &q1) +{ + return Quat(a * q1.w, a * q1.x, a * q1.y, a * q1.z); +} + +template +inline Quat& Quat::operator*=(const Quat &q1) +{ + T qw, qx, qy, qz; + qw = w * q1.w - x * q1.x - y * q1.y - z * q1.z; + qx = x * q1.w + w * q1.x + y * q1.z - z * q1.y; + qy = y * q1.w + w * q1.y + z * q1.x - x * q1.z; + qz = z * q1.w + w * q1.z + x * q1.y - y * q1.x; + w = qw; + x = qx; + y = qy; + z = qz; + return *this; +} + +template +inline Quat& Quat::operator/=(const Quat &q1) +{ + Quat q(*this * q1.inv()); + w = q.w; + x = q.x; + y = q.y; + z = q.z; + return *this; +} +template +Quat& Quat::operator*=(const T &q1) +{ + w *= q1; + x *= q1; + y *= q1; + z *= q1; + return *this; +} + +template +inline Quat& Quat::operator/=(const T &a) +{ + const T a_inv = 1.0 / a; + w *= a_inv; + x *= a_inv; + y *= a_inv; + z *= a_inv; + return *this; +} + +template +inline Quat Quat::operator/(const T &a) const +{ + const T a_inv = 1.0 / a; + return Quat(w * a_inv, x * a_inv, y * a_inv, z * a_inv); +} + +template +inline Quat Quat::operator/(const Quat &q) const +{ + return *this * q.inv(); +} + +template +inline const T& Quat::operator[](std::size_t n) const +{ + switch (n) { + case 0: + return w; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + CV_Error(Error::StsOutOfRange, "subscript exceeds the index range"); + } +} + +template +inline T& Quat::operator[](std::size_t n) +{ + switch (n) { + case 0: + return w; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + CV_Error(Error::StsOutOfRange, "subscript exceeds the index range"); + } +} + +template +std::ostream & operator<<(std::ostream &os, const Quat &q) +{ + os << "Quat " << Vec{q.w, q.x, q.y, q.z}; + return os; +} + +template +inline T Quat::at(size_t index) const +{ + return (*this)[index]; +} + +template +inline Quat Quat::conjugate() const +{ + return Quat(w, -x, -y, -z); +} + +template +inline T Quat::norm() const +{ + return std::sqrt(dot(*this)); +} + +template +Quat exp(const Quat &q) +{ + return q.exp(); +} + +template +Quat Quat::exp() const +{ + Vec v{x, y, z}; + T normV = std::sqrt(v.dot(v)); + T k = normV < CV_QUAT_EPS ? 1 : std::sin(normV) / normV; + return std::exp(w) * Quat(std::cos(normV), v[0] * k, v[1] * k, v[2] * k); +} + +template +Quat log(const Quat &q, QuatAssumeType assumeUnit) +{ + return q.log(assumeUnit); +} + +template +Quat Quat::log(QuatAssumeType assumeUnit) const +{ + Vec v{x, y, z}; + T vNorm = std::sqrt(v.dot(v)); + if (assumeUnit) + { + T k = vNorm < CV_QUAT_EPS ? 1 : std::acos(w) / vNorm; + return Quat(0, v[0] * k, v[1] * k, v[2] * k); + } + T qNorm = norm(); + if (qNorm < CV_QUAT_EPS) + { + CV_Error(Error::StsBadArg, "Cannot apply this quaternion to log function: undefined"); + } + T k = vNorm < CV_QUAT_EPS ? 1 : std::acos(w / qNorm) / vNorm; + return Quat(std::log(qNorm), v[0] * k, v[1] * k, v[2] *k); +} + +template +inline Quat power(const Quat &q1, _T alpha, QuatAssumeType assumeUnit) +{ + return q1.power(alpha, assumeUnit); +} + +template +template +inline Quat Quat::power(_T alpha, QuatAssumeType assumeUnit) const +{ + if (x * x + y * y + z * z > CV_QUAT_EPS) + { + T angle = getAngle(assumeUnit); + Vec axis = getAxis(assumeUnit); + if (assumeUnit) + { + return createFromAngleAxis(alpha * angle, axis); + } + return std::pow(norm(), alpha) * createFromAngleAxis(alpha * angle, axis); + } + else + { + return std::pow(norm(), alpha) * Quat(w, x, y, z); + } +} + + +template +inline Quat sqrt(const Quat &q, QuatAssumeType assumeUnit) +{ + return q.sqrt(assumeUnit); +} + +template +inline Quat Quat::sqrt(QuatAssumeType assumeUnit) const +{ + return power(0.5, assumeUnit); +} + + +template +inline Quat power(const Quat &p, const Quat &q, QuatAssumeType assumeUnit) +{ + return p.power(q, assumeUnit); +} + + +template +inline Quat Quat::power(const Quat &q, QuatAssumeType assumeUnit) const +{ + return cv::exp(q * log(assumeUnit)); +} + +template +inline T Quat::dot(Quat q1) const +{ + return w * q1.w + x * q1.x + y * q1.y + z * q1.z; +} + + +template +inline Quat crossProduct(const Quat &p, const Quat &q) +{ + return p.crossProduct(q); +} + + +template +inline Quat Quat::crossProduct(const Quat &q) const +{ + return Quat (0, y * q.z - z * q.y, z * q.x - x * q.z, x * q.y - q.x * y); +} + +template +inline Quat Quat::normalize() const +{ + T normVal = norm(); + if (normVal < CV_QUAT_EPS) + { + CV_Error(Error::StsBadArg, "Cannot normalize this quaternion: the norm is too small."); + } + return Quat(w / normVal, x / normVal, y / normVal, z / normVal) ; +} + +template +inline Quat inv(const Quat &q, QuatAssumeType assumeUnit) +{ + return q.inv(assumeUnit); +} + + +template +inline Quat Quat::inv(QuatAssumeType assumeUnit) const +{ + if (assumeUnit) + { + return conjugate(); + } + T norm2 = dot(*this); + if (norm2 < CV_QUAT_EPS) + { + CV_Error(Error::StsBadArg, "This quaternion do not have inverse quaternion"); + } + return conjugate() / norm2; +} + +template +inline Quat sinh(const Quat &q) +{ + return q.sinh(); +} + + +template +inline Quat Quat::sinh() const +{ + Vec v{x, y ,z}; + T vNorm = std::sqrt(v.dot(v)); + T k = vNorm < CV_QUAT_EPS ? 1 : std::cosh(w) * std::sin(vNorm) / vNorm; + return Quat(std::sinh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k); +} + + +template +inline Quat cosh(const Quat &q) +{ + return q.cosh(); +} + + +template +inline Quat Quat::cosh() const +{ + Vec v{x, y ,z}; + T vNorm = std::sqrt(v.dot(v)); + T k = vNorm < CV_QUAT_EPS ? 1 : std::sinh(w) * std::sin(vNorm) / vNorm; + return Quat(std::cosh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k); +} + +template +inline Quat tanh(const Quat &q) +{ + return q.tanh(); +} + +template +inline Quat Quat::tanh() const +{ + return sinh() * cosh().inv(); +} + + +template +inline Quat sin(const Quat &q) +{ + return q.sin(); +} + + +template +inline Quat Quat::sin() const +{ + Vec v{x, y ,z}; + T vNorm = std::sqrt(v.dot(v)); + T k = vNorm < CV_QUAT_EPS ? 1 : std::cos(w) * std::sinh(vNorm) / vNorm; + return Quat(std::sin(w) * std::cosh(vNorm), v[0] * k, v[1] * k, v[2] * k); +} + +template +inline Quat cos(const Quat &q) +{ + return q.cos(); +} + +template +inline Quat Quat::cos() const +{ + Vec v{x, y ,z}; + T vNorm = std::sqrt(v.dot(v)); + T k = vNorm < CV_QUAT_EPS ? 1 : std::sin(w) * std::sinh(vNorm) / vNorm; + return Quat(std::cos(w) * std::cosh(vNorm), -v[0] * k, -v[1] * k, -v[2] * k); +} + +template +inline Quat tan(const Quat &q) +{ + return q.tan(); +} + +template +inline Quat Quat::tan() const +{ + return sin() * cos().inv(); +} + +template +inline Quat asinh(const Quat &q) +{ + return q.asinh(); +} + +template +inline Quat Quat::asinh() const +{ + return cv::log(*this + cv::power(*this * *this + Quat(1, 0, 0, 0), 0.5)); +} + +template +inline Quat acosh(const Quat &q) +{ + return q.acosh(); +} + +template +inline Quat Quat::acosh() const +{ + return cv::log(*this + cv::power(*this * *this - Quat(1,0,0,0), 0.5)); +} + +template +inline Quat atanh(const Quat &q) +{ + return q.atanh(); +} + +template +inline Quat Quat::atanh() const +{ + Quat ident(1, 0, 0, 0); + Quat c1 = (ident + *this).log(); + Quat c2 = (ident - *this).log(); + return 0.5 * (c1 - c2); +} + +template +inline Quat asin(const Quat &q) +{ + return q.asin(); +} + +template +inline Quat Quat::asin() const +{ + Quat v(0, x, y, z); + T vNorm = v.norm(); + T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; + return -v / k * (*this * v / k).asinh(); +} + +template +inline Quat acos(const Quat &q) +{ + return q.acos(); +} + +template +inline Quat Quat::acos() const +{ + Quat v(0, x, y, z); + T vNorm = v.norm(); + T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; + return -v / k * acosh(); +} + +template +inline Quat atan(const Quat &q) +{ + return q.atan(); +} + +template +inline Quat Quat::atan() const +{ + Quat v(0, x, y, z); + T vNorm = v.norm(); + T k = vNorm < CV_QUAT_EPS ? 1 : vNorm; + return -v / k * (*this * v / k).atanh(); +} + +template +inline T Quat::getAngle(QuatAssumeType assumeUnit) const +{ + if (assumeUnit) + { + return 2 * std::acos(w); + } + if (norm() < CV_QUAT_EPS) + { + CV_Error(Error::StsBadArg, "This quaternion does not represent a rotation"); + } + return 2 * std::acos(w / norm()); +} + +template +inline Vec Quat::getAxis(QuatAssumeType assumeUnit) const +{ + T angle = getAngle(assumeUnit); + const T sin_v = std::sin(angle * 0.5); + if (assumeUnit) + { + return Vec{x, y, z} / sin_v; + } + return Vec {x, y, z} / (norm() * sin_v); +} + +template +Matx Quat::toRotMat4x4(QuatAssumeType assumeUnit) const +{ + T a = w, b = x, c = y, d = z; + if (!assumeUnit) + { + Quat qTemp = normalize(); + a = qTemp.w; + b = qTemp.x; + c = qTemp.y; + d = qTemp.z; + } + Matx R{ + 1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c) , 0, + 2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b) , 0, + 2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c), 0, + 0 , 0 , 0 , 1, + }; + return R; +} + +template +Matx Quat::toRotMat3x3(QuatAssumeType assumeUnit) const +{ + T a = w, b = x, c = y, d = z; + if (!assumeUnit) + { + Quat qTemp = normalize(); + a = qTemp.w; + b = qTemp.x; + c = qTemp.y; + d = qTemp.z; + } + Matx R{ + 1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c), + 2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b), + 2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c) + }; + return R; +} + +template +Vec Quat::toRotVec(QuatAssumeType assumeUnit) const +{ + T angle = getAngle(assumeUnit); + Vec axis = getAxis(assumeUnit); + return angle * axis; +} + +template +Vec Quat::toVec() const +{ + return Vec{w, x, y, z}; +} + +template +Quat Quat::lerp(const Quat &q0, const Quat &q1, const T t) +{ + return (1 - t) * q0 + t * q1; +} + +template +Quat Quat::slerp(const Quat &q0, const Quat &q1, const T t, QuatAssumeType assumeUnit, bool directChange) +{ + Quatd v0(q0); + Quatd v1(q1); + if (!assumeUnit) + { + v0 = v0.normalize(); + v1 = v1.normalize(); + } + T cosTheta = v0.dot(v1); + constexpr T DOT_THRESHOLD = 0.995; + if (cosTheta > DOT_THRESHOLD) + { + return nlerp(v0, v1, t, QUAT_ASSUME_UNIT); + } + + if (directChange && cosTheta < 0) + { + v0 = -v0; + cosTheta = -cosTheta; + } + T sinTheta = std::sqrt(1 - cosTheta * cosTheta); + T angle = atan2(sinTheta, cosTheta); + return (std::sin((1 - t) * angle) / (sinTheta) * v0 + std::sin(t * angle) / (sinTheta) * v1).normalize(); +} + + +template +inline Quat Quat::nlerp(const Quat &q0, const Quat &q1, const T t, QuatAssumeType assumeUnit) +{ + Quat v0(q0), v1(q1); + if (v1.dot(v0) < 0) + { + v0 = -v0; + } + if (assumeUnit) + { + return ((1 - t) * v0 + t * v1).normalize(); + } + v0 = v0.normalize(); + v1 = v1.normalize(); + return ((1 - t) * v0 + t * v1).normalize(); +} + + +template +inline bool Quat::isNormal(T eps) const +{ + + double normVar = norm(); + if ((normVar > 1 - eps) && (normVar < 1 + eps)) + return true; + return false; +} + +template +inline void Quat::assertNormal(T eps) const +{ + if (!isNormal(eps)) + CV_Error(Error::StsBadArg, "Quaternion should be normalized"); +} + + +template +inline Quat Quat::squad(const Quat &q0, const Quat &q1, + const Quat &q2, const Quat &q3, + const T t, QuatAssumeType assumeUnit, + bool directChange) +{ + Quat v0(q0), v1(q1), v2(q2), v3(q3); + if (!assumeUnit) + { + v0 = v0.normalize(); + v1 = v1.normalize(); + v2 = v2.normalize(); + v3 = v3.normalize(); + } + + Quat c0 = slerp(v0, v3, t, assumeUnit, directChange); + Quat c1 = slerp(v1, v2, t, assumeUnit, directChange); + return slerp(c0, c1, 2 * t * (1 - t), assumeUnit, directChange); +} + +template +Quat Quat::interPoint(const Quat &q0, const Quat &q1, + const Quat &q2, QuatAssumeType assumeUnit) +{ + Quat v0(q0), v1(q1), v2(q2); + if (!assumeUnit) + { + v0 = v0.normalize(); + v1 = v1.normalize(); + v2 = v2.normalize(); + } + return v1 * cv::exp(-(cv::log(v1.conjugate() * v0, assumeUnit) + (cv::log(v1.conjugate() * v2, assumeUnit))) / 4); +} + +template +Quat Quat::spline(const Quat &q0, const Quat &q1, const Quat &q2, const Quat &q3, const T t, QuatAssumeType assumeUnit) +{ + Quatd v0(q0), v1(q1), v2(q2), v3(q3); + if (!assumeUnit) + { + v0 = v0.normalize(); + v1 = v1.normalize(); + v2 = v2.normalize(); + v3 = v3.normalize(); + } + T cosTheta; + std::vector> vec{v0, v1, v2, v3}; + for (size_t i = 0; i < 3; ++i) + { + cosTheta = vec[i].dot(vec[i + 1]); + if (cosTheta < 0) + { + vec[i + 1] = -vec[i + 1]; + } + } + Quat s1 = interPoint(vec[0], vec[1], vec[2], QUAT_ASSUME_UNIT); + Quat s2 = interPoint(vec[1], vec[2], vec[3], QUAT_ASSUME_UNIT); + return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT); +} + +} // namepsace +//! @endcond + +#endif /*OPENCV_CORE_QUATERNION_INL_HPP*/ diff --git a/modules/core/test/test_quaternion.cpp b/modules/core/test/test_quaternion.cpp new file mode 100644 index 000000000000..0025674ec700 --- /dev/null +++ b/modules/core/test/test_quaternion.cpp @@ -0,0 +1,255 @@ +// 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. + +#include "test_precomp.hpp" +#include +#include +using namespace cv; +namespace opencv_test{ namespace { +class QuatTest: public ::testing::Test { +protected: + void SetUp() override + { + q1 = {1,2,3,4}; + q2 = {2.5,-2,3.5,4}; + q1Unit = {1 / sqrt(30), sqrt(2) /sqrt(15), sqrt(3) / sqrt(10), 2 * sqrt(2) / sqrt(15)}; + q1Inv = {1.0 / 30, -1.0 / 15, -1.0 / 10, -2.0 / 15}; + } + double scalar = 2.5; + double angle = CV_PI; + int qNorm2 = 2; + Vec axis{1, 1, 1}; + Vec unAxis{0, 0, 0}; + Vec unitAxis{1.0 / sqrt(3), 1.0 / sqrt(3), 1.0 / sqrt(3)}; + Quatd q3 = Quatd::createFromAngleAxis(angle, axis); + Quatd q3UnitAxis = Quatd::createFromAngleAxis(angle, unitAxis); + Quat q3Norm2 = q3 * qNorm2; + + Quat q1Inv; + Quat q1; + Quat q2; + Quat q1Unit; + + Quatd qNull{0, 0, 0, 0}; + Quatd qIdentity{1, 0, 0, 0}; + QuatAssumeType assumeUnit = QUAT_ASSUME_UNIT; + +}; + +TEST_F(QuatTest, constructor){ + Vec coeff{1, 2, 3, 4}; + EXPECT_EQ(Quat (coeff), q1); + EXPECT_EQ(q3, q3UnitAxis); + EXPECT_ANY_THROW(Quatd::createFromAngleAxis(angle, unAxis)); + Matx33d R1{ + -1.0 / 3, 2.0 / 3 , 2.0 / 3, + 2.0 / 3 , -1.0 / 3, 2.0 / 3, + 2.0 / 3 , 2.0 / 3 , -1.0 / 3 + }; + Matx33d R2{ + -2.0 / 3, -2.0 / 3, -1.0 / 3, + -2.0 / 3, 1.0 / 3, 2.0 / 3, + -1.0 / 3, 2.0 / 3, -2.0 / 3 + }; + Matx33d R3{ + 0.818181818181, 0.181818181818, 0.54545455454, + 0.545454545545, -0.54545454545, -0.6363636364, + 0.181818181818, 0.818181818182, -0.5454545455 + }; + Matx33d R4{ + 0.818181818181, -0.181818181818, 0.54545455454, + 0.545454545545, 0.54545454545, -0.6363636364, + -0.181818181818, 0.818181818182, 0.5454545455 + }; + Quatd qMat = Quatd::createFromRotMat(R1); + Quatd qMat2 = Quatd::createFromRotMat(R2); + Quatd qMat3 = Quatd::createFromRotMat(R3); + Quatd qMat4 = Quatd::createFromRotMat(R4); + EXPECT_EQ(qMat2, Quatd(0, -0.408248290463, 0.816496580927, 0.408248904638)); + EXPECT_EQ(qMat3, Quatd(-0.426401432711,-0.852802865422, -0.213200716355, -0.2132007163)); + EXPECT_EQ(qMat, q3); + EXPECT_EQ(qMat4, -Quatd(0.852802865422, 0.426401432711221, 0.2132007163556, 0.2132007163)); + + Vec3d rot{angle / sqrt(3),angle / sqrt(3), angle / sqrt(3)}; + Quatd rotQuad{0, 1.0 / sqrt(3), 1. / sqrt(3), 1. / sqrt(3)}; + Quatd qRot = Quatd::createFromRvec(rot); + EXPECT_EQ(qRot, rotQuad); + EXPECT_EQ(Quatd::createFromRvec(Vec3d(0, 0, 0)), qIdentity); +} + +TEST_F(QuatTest, basicfuns){ + Quat q1Conj{1, -2, -3, -4}; + EXPECT_EQ(q3Norm2.normalize(), q3); + EXPECT_EQ(q1.norm(), sqrt(30)); + EXPECT_EQ(q1.normalize(), q1Unit); + EXPECT_ANY_THROW(qNull.normalize()); + EXPECT_EQ(q1.conjugate(), q1Conj); + EXPECT_EQ(q1.inv(), q1Inv); + EXPECT_EQ(inv(q1), q1Inv); + EXPECT_EQ(q3.inv(assumeUnit) * q3, qIdentity); + EXPECT_EQ(q1.inv() * q1, qIdentity); + EXPECT_ANY_THROW(inv(qNull)); + EXPECT_NO_THROW(q1.at(0)); + EXPECT_ANY_THROW(q1.at(4)); + + Matx33d R{ + -2.0 / 3, 2.0 / 15 , 11.0 / 15, + 2.0 / 3 , -1.0 / 3 , 2.0 / 3 , + 1.0 / 3 , 14.0 / 15, 2.0 / 15 + }; + Matx33d q1RotMat = q1.toRotMat3x3(); + EXPECT_MAT_NEAR(q1RotMat, R, 1e-6); + Vec3d z_axis{0,0,1}; + Quatd q_unit1 = Quatd::createFromAngleAxis(angle, z_axis); + Mat pointsA = (Mat_(2, 3) << 1,0,0,1,0,1); + pointsA = pointsA.t(); + Mat new_point = q_unit1.toRotMat3x3() * pointsA; + Mat afterRo = (Mat_(3, 2) << -1,-1,0,0,0,1); + EXPECT_MAT_NEAR(afterRo, new_point, 1e-6); + EXPECT_ANY_THROW(qNull.toRotVec()); + Vec3d rodVec{CV_PI/sqrt(3), CV_PI/sqrt(3), CV_PI/sqrt(3)}; + Vec3d q3Rod = q3.toRotVec(); + EXPECT_NEAR(q3Rod[0], rodVec[0], 1e-6); + EXPECT_NEAR(q3Rod[1], rodVec[1], 1e-6); + EXPECT_NEAR(q3Rod[2], rodVec[2], 1e-6); + + EXPECT_EQ(log(q1Unit, assumeUnit), log(q1Unit)); + EXPECT_EQ(log(qIdentity, assumeUnit), qNull); + EXPECT_EQ(log(q3), Quatd(0, angle * unitAxis[0] / 2, angle * unitAxis[1] / 2, angle * unitAxis[2] / 2)); + EXPECT_ANY_THROW(log(qNull)); + EXPECT_EQ(log(Quatd(exp(1), 0, 0, 0)), qIdentity); + + EXPECT_EQ(exp(qIdentity), Quatd(exp(1), 0, 0, 0)); + EXPECT_EQ(exp(qNull), qIdentity); + EXPECT_EQ(exp(Quatd(0, angle * unitAxis[0] / 2, angle * unitAxis[1] / 2, angle * unitAxis[2] / 2)), q3); + + EXPECT_EQ(power(q3, 2), Quatd::createFromAngleAxis(2*angle, axis)); + EXPECT_EQ(power(Quatd(0.5, 0.5, 0.5, 0.5), 2.0, assumeUnit), Quatd(-0.5,0.5,0.5,0.5)); + EXPECT_EQ(power(Quatd(0.5, 0.5, 0.5, 0.5), -2.0), Quatd(-0.5,-0.5,-0.5,-0.5)); + EXPECT_EQ(sqrt(q1), power(q1, 0.5)); + EXPECT_EQ(exp(q3 * log(q1)), power(q1, q3)); + EXPECT_EQ(exp(q1 * log(q3)), power(q3, q1, assumeUnit)); + EXPECT_EQ(crossProduct(q1, q3), (q1 * q3 - q3 * q1) / 2); + EXPECT_EQ(sinh(qNull), qNull); + EXPECT_EQ(sinh(q1), (exp(q1) - exp(-q1)) / 2); + EXPECT_EQ(sinh(qIdentity), Quatd(sinh(1), 0, 0, 0)); + EXPECT_EQ(sinh(q1), Quatd(0.73233760604, -0.44820744998, -0.67231117497, -0.8964148999610843)); + EXPECT_EQ(cosh(qNull), qIdentity); + EXPECT_EQ(cosh(q1), Quatd(0.961585117636, -0.34135217456, -0.51202826184, -0.682704349122)); + EXPECT_EQ(tanh(q1), sinh(q1) * inv(cosh(q1))); + EXPECT_EQ(sin(qNull), qNull); + EXPECT_EQ(sin(q1), Quatd(91.78371578403, 21.88648685303, 32.829730279543, 43.772973706058)); + EXPECT_EQ(cos(qNull), qIdentity); + EXPECT_EQ(cos(q1), Quatd(58.9336461679, -34.0861836904, -51.12927553569, -68.17236738093)); + EXPECT_EQ(tan(q1), sin(q1)/cos(q1)); + EXPECT_EQ(sinh(asinh(q1)), q1); + Quatd c1 = asinh(sinh(q1)); + EXPECT_EQ(sinh(c1), sinh(q1)); + EXPECT_EQ(cosh(acosh(q1)), q1); + c1 = acosh(cosh(q1)); + EXPECT_EQ(cosh(c1), cosh(q1)); + EXPECT_EQ(tanh(atanh(q1)), q1); + c1 = atanh(tanh(q1)); + EXPECT_EQ(tanh(q1), tanh(c1)); + EXPECT_EQ(asin(sin(q1)), q1); + EXPECT_EQ(sin(asin(q1)), q1); + EXPECT_EQ(acos(cos(q1)), q1); + EXPECT_EQ(cos(acos(q1)), q1); + EXPECT_EQ(atan(tan(q3)), q3); + EXPECT_EQ(tan(atan(q1)), q1); +} + +TEST_F(QuatTest, opeartor){ + Quatd minusQ{-1, -2, -3, -4}; + Quatd qAdd{3.5, 0, 6.5, 8}; + Quatd qMinus{-1.5, 4, -0.5, 0}; + Quatd qMultq{-20, 1, -5, 27}; + Quatd qMults{2.5, 5.0, 7.5, 10.0}; + Quatd qDvss{1.0 / 2.5, 2.0 / 2.5, 3.0 / 2.5, 4.0 / 2.5}; + Quatd qOrigin(q1); + + EXPECT_EQ(-q1, minusQ); + EXPECT_EQ(q1 + q2, qAdd); + EXPECT_EQ(q1 - q2, qMinus); + EXPECT_EQ(q1 * q2, qMultq); + EXPECT_EQ(q1 * scalar, qMults); + EXPECT_EQ(scalar * q1, qMults); + EXPECT_EQ(q1 / q1, qIdentity); + EXPECT_EQ(q1 / scalar, qDvss); + q1 += q2; + EXPECT_EQ(q1, qAdd); + q1 -= q2; + EXPECT_EQ(q1, qOrigin); + q1 *= q2; + EXPECT_EQ(q1, qMultq); + q1 /= q2; + EXPECT_EQ(q1, qOrigin); + q1 *= scalar; + EXPECT_EQ(q1, qMults); + q1 /= scalar; + EXPECT_EQ(q1, qOrigin); + EXPECT_NO_THROW(q1[0]); + EXPECT_NO_THROW(q1.at(0)); + EXPECT_ANY_THROW(q1[4]); + EXPECT_ANY_THROW(q1.at(4)); +} + +TEST_F(QuatTest, quatAttrs){ + double angleQ1 = 2 * acos(1.0 / sqrt(30)); + Vec3d axis1{0.3713906763541037, 0.557086014, 0.742781352}; + Vec q1axis1 = q1.getAxis(); + + EXPECT_EQ(angleQ1, q1.getAngle()); + EXPECT_EQ(angleQ1, q1Unit.getAngle()); + EXPECT_EQ(angleQ1, q1Unit.getAngle(assumeUnit)); + EXPECT_EQ(0, qIdentity.getAngle()); + EXPECT_ANY_THROW(qNull.getAxis()); + EXPECT_NEAR(axis1[0], q1axis1[0], 1e-6); + EXPECT_NEAR(axis1[1], q1axis1[1], 1e-6); + EXPECT_NEAR(axis1[2], q1axis1[2], 1e-6); + EXPECT_NEAR(q3Norm2.norm(), qNorm2, 1e-6); + EXPECT_EQ(q3Norm2.getAngle(), angle); + EXPECT_NEAR(axis1[0], axis1[0], 1e-6); + EXPECT_NEAR(axis1[1], axis1[1], 1e-6); + EXPECT_NEAR(axis1[2], axis1[2], 1e-6); +} + +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); + EXPECT_EQ(Quatd::lerp(qNoRot, q3, 1), q3); + EXPECT_EQ(Quatd::lerp(qNoRot, q3, 0.5), qLerpInter); + Quatd q3NrNn2 = qNoRot * qNorm2; + EXPECT_EQ(Quatd::nlerp(q3NrNn2, q3Norm2, 0), qNoRot); + EXPECT_EQ(Quatd::nlerp(q3NrNn2, q3Norm2, 1), q3); + EXPECT_EQ(Quatd::nlerp(q3NrNn2, q3Norm2, 0.5), qLerpInter.normalize()); + EXPECT_EQ(Quatd::nlerp(qNoRot, q3, 0, assumeUnit), qNoRot); + EXPECT_EQ(Quatd::nlerp(qNoRot, q3, 1, assumeUnit), q3); + EXPECT_EQ(Quatd::nlerp(qNoRot, q3, 0.5, assumeUnit), qLerpInter.normalize()); + Quatd q3Minus(-q3); + EXPECT_EQ(Quatd::nlerp(qNoRot, q3, 0.4), -Quatd::nlerp(qNoRot, q3Minus, 0.4)); + EXPECT_EQ(Quatd::slerp(qNoRot, q3, 0, assumeUnit), qNoRot); + EXPECT_EQ(Quatd::slerp(qNoRot, q3, 1, assumeUnit), q3); + EXPECT_EQ(Quatd::slerp(qNoRot, q3, 0.5, assumeUnit), -Quatd::nlerp(qNoRot, -q3, 0.5, assumeUnit)); + EXPECT_EQ(Quatd::slerp(qNoRot, q1, 0.5), Quatd(0.76895194, 0.2374325, 0.35614876, 0.47486501)); + EXPECT_EQ(Quatd::slerp(-qNoRot, q1, 0.5), Quatd(0.76895194, 0.2374325, 0.35614876, 0.47486501)); + EXPECT_EQ(Quatd::slerp(qNoRot, -q1, 0.5), -Quatd::slerp(-qNoRot, q1, 0.5)); + + Quat tr1 = Quatd::createFromAngleAxis(0, axis); + Quat tr2 = Quatd::createFromAngleAxis(angle / 2, axis); + Quat tr3 = Quatd::createFromAngleAxis(angle, axis); + Quat tr4 = Quatd::createFromAngleAxis(angle, Vec3d{-1/sqrt(2),0,1/(sqrt(2))}); + EXPECT_ANY_THROW(Quatd::spline(qNull, tr1, tr2, tr3, 0)); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr4, 0), tr2); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr4, 1), tr3); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr4, 0.6, assumeUnit), Quatd::spline(tr1, tr2, tr3, tr4, 0.6)); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr3, 0.5), Quatd::spline(tr1, -tr2, tr3, tr3, 0.5)); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr3, 0.5), -Quatd::spline(-tr1, -tr2, -tr3, tr3, 0.5)); + EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr3, 0.5), Quatd(0.336889853392, 0.543600719487, 0.543600719487, 0.543600719487)); +} + +} // namespace + +}// opencv_test \ No newline at end of file diff --git a/modules/stitching/include/opencv2/stitching/detail/warpers_inl.hpp b/modules/stitching/include/opencv2/stitching/detail/warpers_inl.hpp index 5e2375621e79..72b5c086725b 100644 --- a/modules/stitching/include/opencv2/stitching/detail/warpers_inl.hpp +++ b/modules/stitching/include/opencv2/stitching/detail/warpers_inl.hpp @@ -363,8 +363,8 @@ void StereographicProjector::mapForward(float x, float y, float &u, float &v) float r = sinf(v_) / (1 - cosf(v_)); - u = scale * r * cos(u_); - v = scale * r * sin(u_); + u = scale * r * std::cos(u_); + v = scale * r * std::sin(u_); } inline @@ -625,7 +625,7 @@ void TransverseMercatorProjector::mapBackward(float u, float v, float &x, float v /= scale; float v_ = asinf( sinf(v) / coshf(u) ); - float u_ = atan2f( sinhf(u), cos(v) ); + float u_ = atan2f( sinhf(u), std::cos(v) ); float cosv = cosf(v_); float x_ = cosv * sinf(u_);