From 8e32566583c5249249aa044b93e497aeb35ae1aa Mon Sep 17 00:00:00 2001 From: Kong Liangqian <chargerKong@126.com> Date: Tue, 24 Nov 2020 18:35:39 +0800 Subject: [PATCH] Add adding and subtraction operations between a number and a quaternion; fix a typo; Add documentation of quaternion operators; Restrict the type of scalar: the same as quaternion; --- .../core/include/opencv2/core/quaternion.hpp | 365 ++++++++++++++++-- .../include/opencv2/core/quaternion.inl.hpp | 45 ++- modules/core/test/test_quaternion.cpp | 16 +- 3 files changed, 389 insertions(+), 37 deletions(-) diff --git a/modules/core/include/opencv2/core/quaternion.hpp b/modules/core/include/opencv2/core/quaternion.hpp index c72ee8c37f..7bc51e6c6d 100644 --- a/modules/core/include/opencv2/core/quaternion.hpp +++ b/modules/core/include/opencv2/core/quaternion.hpp @@ -277,17 +277,18 @@ public: * For example * ``` * Quatd q(1,2,3,4); - * power(q, 2); + * power(q, 2.0); * * 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. + * power(q1, 2.0, assumeUnit);//This assumeUnit means q1 is a unit quaternion. * ``` + * @note the type of the index should be the same as the quaternion. */ - template <typename T, typename _T> - friend Quat<T> power(const Quat<T> &q, _T x, QuatAssumeType assumeUnit); + template <typename T> + friend Quat<T> power(const Quat<T> &q, const T x, QuatAssumeType assumeUnit); /** * @brief return the value of power function with index \f$x\f$. @@ -298,17 +299,16 @@ public: * For example * ``` * Quatd q(1,2,3,4); - * q.power(2); + * q.power(2.0); * * 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 + * q1.power(2.0, assumeUnit); //This assumeUnt means q1 is a unit quaternion * ``` */ - template <typename _T> - Quat<_Tp> power(_T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; + Quat<_Tp> power(const _Tp x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; /** * @brief return \f$\sqrt{q}\f$. @@ -859,6 +859,7 @@ public: * * @sa toRotMat3x3 */ + Matx<_Tp, 4, 4> toRotMat4x4(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const; /** @@ -1073,42 +1074,362 @@ public: const Quat<_Tp> &q2, const Quat<_Tp> &q3, const _Tp t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); - + /** + * @brief Return opposite quaternion \f$-p\f$ + * which satisfies \f$p + (-p) = 0.\f$ + * + * For example + * ``` + * Quatd q{1, 2, 3, 4}; + * std::cout << -q << std::endl; // [-1, -2, -3, -4] + * ``` + */ Quat<_Tp> operator-() const; + /** + * @brief return true if two 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_QUAT_EPS. + */ bool operator==(const Quat<_Tp>&) const; + /** + * @brief Addition operator of two quaternions p and q. + * It returns a new quaternion that each value is the sum of \f$p_i\f$ and \f$q_i\f$. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * std::cout << p + q << std::endl; //[6, 8, 10, 12] + * ``` + */ Quat<_Tp> operator+(const Quat<_Tp>&) const; + /** + * @brief Addition assignment operator of two quaternions p and q. + * It adds right operand to the left operand and assign the result to left operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * p += q; // equivalent to p = p + q + * std::cout << p << std::endl; //[6, 8, 10, 12] + * + * ``` + */ Quat<_Tp>& operator+=(const Quat<_Tp>&); + /** + * @brief Subtraction operator of two quaternions p and q. + * It returns a new quaternion that each value is the sum of \f$p_i\f$ and \f$-q_i\f$. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * std::cout << p - q << std::endl; //[-4, -4, -4, -4] + * ``` + */ Quat<_Tp> operator-(const Quat<_Tp>&) const; + /** + * @brief Subtraction assignment operator of two quaternions p and q. + * It subtracts right operand from the left operand and assign the result to left operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * p -= q; // equivalent to p = p - q + * std::cout << p << std::endl; //[-4, -4, -4, -4] + * + * ``` + */ Quat<_Tp>& operator-=(const Quat<_Tp>&); + /** + * @brief Multiplication assignment operator of two quaternions q and p. + * It multiplies right operand with the left operand and assign the result to left operand. + * + * Rule of quaternion multiplication: + * \f[ + * \begin{equation} + * \begin{split} + * p * q &= [p_0, \boldsymbol{u}]*[q_0, \boldsymbol{v}]\\ + * &=[p_0q_0 - \boldsymbol{u}\cdot \boldsymbol{v}, p_0\boldsymbol{v} + q_0\boldsymbol{u}+ \boldsymbol{u}\times \boldsymbol{v}]. + * \end{split} + * \end{equation} + * \f] + * where \f$\cdot\f$ means dot product and \f$\times \f$ means cross product. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * p *= q; // equivalent to p = p * q + * std::cout << p << std::endl; //[-60, 12, 30, 24] + * ``` + */ Quat<_Tp>& operator*=(const Quat<_Tp>&); - Quat<_Tp>& operator*=(const _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 quaternion multiplication with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p * s &= [w, x, y, z] * s\\ + * &=[w * s, x * s, y * s, z * s]. + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double s = 2.0; + * p *= s; // equivalent to p = p * s + * std::cout << p << std::endl; //[2.0, 4.0, 6.0, 8.0] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + Quat<_Tp>& operator*=(const _Tp s); + /** + * @brief Multiplication operator of two quaternions q and p. + * Multiplies values on either side of the operator. + * + * Rule of quaternion multiplication: + * \f[ + * \begin{equation} + * \begin{split} + * p * q &= [p_0, \boldsymbol{u}]*[q_0, \boldsymbol{v}]\\ + * &=[p_0q_0 - \boldsymbol{u}\cdot \boldsymbol{v}, p_0\boldsymbol{v} + q_0\boldsymbol{u}+ \boldsymbol{u}\times \boldsymbol{v}]. + * \end{split} + * \end{equation} + * \f] + * where \f$\cdot\f$ means dot product and \f$\times \f$ means cross product. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * std::cout << p * q << std::endl; //[-60, 12, 30, 24] + * ``` + */ Quat<_Tp> operator*(const Quat<_Tp>&) const; - Quat<_Tp> operator/(const _Tp&) const; + /** + * @brief Division operator of a quaternions and a scalar. + * It divides left operand with the right operand and assign the result to left operand. + * + * Rule of quaternion division with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p / s &= [w, x, y, z] / s\\ + * &=[w/s, x/s, y/s, z/s]. + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double s = 2.0; + * p /= s; // equivalent to p = p / s + * std::cout << p << std::endl; //[0.5, 1, 1.5, 2] + * ``` + * @note the type of scalar should be equal to this quaternion. + */ + Quat<_Tp> operator/(const _Tp s) const; + /** + * @brief Division operator of two quaternions p and q. + * Divides left hand operand by right hand operand. + * + * Rule of quaternion division with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p / q &= p * q.inv()\\ + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * std::cout << p / q << std::endl; // equivalent to p * q.inv() + * ``` + */ Quat<_Tp> operator/(const Quat<_Tp>&) const; - Quat<_Tp>& operator/=(const _Tp&); + /** + * @brief Division assignment operator of a quaternions and a scalar. + * It divides left operand with the right operand and assign the result to left operand. + * + * Rule of quaternion division with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p / s &= [w, x, y, z] / s\\ + * &=[w / s, x / s, y / s, z / s]. + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double s = 2.0;; + * p /= s; // equivalent to p = p / s + * std::cout << p << std::endl; //[0.5, 1.0, 1.5, 2.0] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + Quat<_Tp>& operator/=(const _Tp s); + /** + * @brief Division assignment operator of two quaternions p and q; + * It divides left operand with the right operand and assign the result to left operand. + * + * Rule of quaternion division with a quaternion: + * \f[ + * \begin{equation} + * \begin{split} + * p / q&= p * q.inv()\\ + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * Quatd q{5, 6, 7, 8}; + * p /= q; // equivalent to p = p * q.inv() + * std::cout << p << std::endl; + * ``` + */ Quat<_Tp>& operator/=(const Quat<_Tp>&); _Tp& operator[](std::size_t n); const _Tp& operator[](std::size_t n) const; - template <typename S, typename T> - friend Quat<S> cv::operator*(const T, const Quat<S>&); + /** + * @brief Subtraction operator of a scalar and a quaternions. + * Subtracts right hand operand from left hand operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double scalar = 2.0; + * std::cout << scalar - p << std::endl; //[1.0, -2, -3, -4] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator-(const T s, const Quat<T>&); - template <typename S, typename T> - friend Quat<S> cv::operator*(const Quat<S>&, const T); + /** + * @brief Subtraction operator of a quaternions and a scalar. + * Subtracts right hand operand from left hand operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double scalar = 2.0; + * std::cout << p - scalar << std::endl; //[-1.0, 2, 3, 4] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator-(const Quat<T>&, const T s); + + /** + * @brief Addition operator of a quaternions and a scalar. + * Adds right hand operand from left hand operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double scalar = 2.0; + * std::cout << scalar + p << std::endl; //[3.0, 2, 3, 4] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator+(const T s, const Quat<T>&); + + /** + * @brief Addition operator of a quaternions and a scalar. + * Adds right hand operand from left hand operand. + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double scalar = 2.0; + * std::cout << p + scalar << std::endl; //[3.0, 2, 3, 4] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator+(const Quat<T>&, const T s); + + /** + * @brief Multiplication operator of a scalar and a quaternions. + * It multiplies right operand with the left operand and assign the result to left operand. + * + * Rule of quaternion multiplication with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p * s &= [w, x, y, z] * s\\ + * &=[w * s, x * s, y * s, z * s]. + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double s = 2.0; + * std::cout << s * p << std::endl; //[2.0, 4.0, 6.0, 8.0] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator*(const T s, const Quat<T>&); + + /** + * @brief Multiplication operator of a quaternion and a scalar. + * It multiplies right operand with the left operand and assign the result to left operand. + * + * Rule of quaternion multiplication with a scalar: + * \f[ + * \begin{equation} + * \begin{split} + * p * s &= [w, x, y, z] * s\\ + * &=[w * s, x * s, y * s, z * s]. + * \end{split} + * \end{equation} + * \f] + * + * For example + * ``` + * Quatd p{1, 2, 3, 4}; + * double s = 2.0; + * std::cout << p * s << std::endl; //[2.0, 4.0, 6.0, 8.0] + * ``` + * @note the type of scalar should be equal to the quaternion. + */ + template <typename T> + friend Quat<T> cv::operator*(const Quat<T>&, const T s); template <typename S> friend std::ostream& cv::operator<<(std::ostream&, const Quat<S>&); @@ -1165,8 +1486,8 @@ Quat<T> exp(const Quat<T> &q); template <typename T> Quat<T> log(const Quat<T> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); -template <typename T, typename _T> -Quat<T> power(const Quat<T>& q, _T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); +template <typename T> +Quat<T> power(const Quat<T>& q, const T x, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); template <typename T> Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q); @@ -1174,11 +1495,11 @@ Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q); template <typename S> Quat<S> sqrt(const Quat<S> &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT); -template <typename S, typename T> -Quat<S> operator*(const T, const Quat<S>&); +template <typename T> +Quat<T> operator*(const T, const Quat<T>&); -template <typename S, typename T> -Quat<S> operator*(const Quat<S>&, const T); +template <typename T> +Quat<T> operator*(const Quat<T>&, const T); template <typename S> std::ostream& operator<<(std::ostream&, const Quat<S>&); diff --git a/modules/core/include/opencv2/core/quaternion.inl.hpp b/modules/core/include/opencv2/core/quaternion.inl.hpp index 769f53ed4b..f665dbe6c8 100644 --- a/modules/core/include/opencv2/core/quaternion.inl.hpp +++ b/modules/core/include/opencv2/core/quaternion.inl.hpp @@ -148,6 +148,30 @@ inline Quat<T> Quat<T>::operator+(const Quat<T> &q1) const return Quat<T>(w + q1.w, x + q1.x, y + q1.y, z + q1.z); } +template <typename T> +inline Quat<T> operator+(const T a, const Quat<T>& q) +{ + return Quat<T>(q.w + a, q.x, q.y, q.z); +} + +template <typename T> +inline Quat<T> operator+(const Quat<T>& q, const T a) +{ + return Quat<T>(q.w + a, q.x, q.y, q.z); +} + +template <typename T> +inline Quat<T> operator-(const T a, const Quat<T>& q) +{ + return Quat<T>(a - q.w, -q.x, -q.y, -q.z); +} + +template <typename T> +inline Quat<T> operator-(const Quat<T>& q, const T a) +{ + return Quat<T>(q.w - a, q.x, q.y, q.z); +} + template <typename T> inline Quat<T> Quat<T>::operator-(const Quat<T> &q1) const { @@ -183,14 +207,14 @@ inline Quat<T> Quat<T>::operator*(const Quat<T> &q1) const } -template <typename T, typename S> -Quat<T> operator*(const Quat<T> &q1, const S a) +template <typename T> +Quat<T> operator*(const Quat<T> &q1, const T a) { return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z); } -template <typename T, typename S> -Quat<T> operator*(const S a, const Quat<T> &q1) +template <typename T> +Quat<T> operator*(const T a, const Quat<T> &q1) { return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z); } @@ -221,7 +245,7 @@ inline Quat<T>& Quat<T>::operator/=(const Quat<T> &q1) return *this; } template <typename T> -Quat<T>& Quat<T>::operator*=(const T &q1) +Quat<T>& Quat<T>::operator*=(const T q1) { w *= q1; x *= q1; @@ -231,7 +255,7 @@ Quat<T>& Quat<T>::operator*=(const T &q1) } template <typename T> -inline Quat<T>& Quat<T>::operator/=(const T &a) +inline Quat<T>& Quat<T>::operator/=(const T a) { const T a_inv = 1.0 / a; w *= a_inv; @@ -242,7 +266,7 @@ inline Quat<T>& Quat<T>::operator/=(const T &a) } template <typename T> -inline Quat<T> Quat<T>::operator/(const T &a) const +inline Quat<T> Quat<T>::operator/(const T a) const { const T a_inv = 1.0 / a; return Quat<T>(w * a_inv, x * a_inv, y * a_inv, z * a_inv); @@ -353,15 +377,14 @@ Quat<T> Quat<T>::log(QuatAssumeType assumeUnit) const return Quat<T>(std::log(qNorm), v[0] * k, v[1] * k, v[2] *k); } -template <typename T, typename _T> -inline Quat<T> power(const Quat<T> &q1, _T alpha, QuatAssumeType assumeUnit) +template <typename T> +inline Quat<T> power(const Quat<T> &q1, const T alpha, QuatAssumeType assumeUnit) { return q1.power(alpha, assumeUnit); } template <typename T> -template <typename _T> -inline Quat<T> Quat<T>::power(_T alpha, QuatAssumeType assumeUnit) const +inline Quat<T> Quat<T>::power(const T alpha, QuatAssumeType assumeUnit) const { if (x * x + y * y + z * z > CV_QUAT_EPS) { diff --git a/modules/core/test/test_quaternion.cpp b/modules/core/test/test_quaternion.cpp index 0025674ec7..324d535bff 100644 --- a/modules/core/test/test_quaternion.cpp +++ b/modules/core/test/test_quaternion.cpp @@ -18,7 +18,7 @@ protected: } double scalar = 2.5; double angle = CV_PI; - int qNorm2 = 2; + double qNorm2 = 2; Vec<double, 3> axis{1, 1, 1}; Vec<double, 3> unAxis{0, 0, 0}; Vec<double, 3> unitAxis{1.0 / sqrt(3), 1.0 / sqrt(3), 1.0 / sqrt(3)}; @@ -124,7 +124,7 @@ TEST_F(QuatTest, basicfuns){ 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(q3, 2.0), 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)); @@ -160,7 +160,7 @@ TEST_F(QuatTest, basicfuns){ EXPECT_EQ(tan(atan(q1)), q1); } -TEST_F(QuatTest, opeartor){ +TEST_F(QuatTest, operator){ Quatd minusQ{-1, -2, -3, -4}; Quatd qAdd{3.5, 0, 6.5, 8}; Quatd qMinus{-1.5, 4, -0.5, 0}; @@ -171,7 +171,15 @@ TEST_F(QuatTest, opeartor){ EXPECT_EQ(-q1, minusQ); EXPECT_EQ(q1 + q2, qAdd); + EXPECT_EQ(q1 + scalar, Quatd(3.5, 2, 3, 4)); + EXPECT_EQ(scalar + q1, Quatd(3.5, 2, 3, 4)); + EXPECT_EQ(q1 + 2.0, Quatd(3, 2, 3, 4)); + EXPECT_EQ(2.0 + q1, Quatd(3, 2, 3, 4)); EXPECT_EQ(q1 - q2, qMinus); + EXPECT_EQ(q1 - scalar, Quatd(-1.5, 2, 3, 4)); + EXPECT_EQ(scalar - q1, Quatd(1.5, -2, -3, -4)); + EXPECT_EQ(q1 - 2.0, Quatd(-1, 2, 3, 4)); + EXPECT_EQ(2.0 - q1, Quatd(1, -2, -3, -4)); EXPECT_EQ(q1 * q2, qMultq); EXPECT_EQ(q1 * scalar, qMults); EXPECT_EQ(scalar * q1, qMults); @@ -252,4 +260,4 @@ TEST_F(QuatTest, interpolation){ } // namespace -}// opencv_test \ No newline at end of file +}// opencv_test