mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-01-18 22:44:02 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #18911 from chargerKong:quat

Browse Source
This commit is contained in:
Vadim Pisarevsky 2020-12-02 19:14:47 +00:00
commit b023fcd264
3 changed files with 389 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

365
modules/core/include/opencv2/core/quaternion.hpp
View File

@ -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>&);

45
modules/core/include/opencv2/core/quaternion.inl.hpp
View File

@ -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)
{

16
modules/core/test/test_quaternion.cpp
View File

@ -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
}// opencv_test