27#ifndef OPENCV_CORE_QUATERNION_INL_HPP
28#define OPENCV_CORE_QUATERNION_INL_HPP
30#ifndef OPENCV_CORE_QUATERNION_HPP
31#error This is not a standalone header. Include quaternion.hpp instead.
43Quat<T>::Quat(
const Vec<T, 4> &coeff):w(coeff[0]),
x(coeff[1]),
y(coeff[2]), z(coeff[3]){}
46Quat<T>::Quat(
const T qw,
const T qx,
const T qy,
const T qz):w(qw),
x(qx),
y(qy), z(qz){}
49Quat<T> Quat<T>::createFromAngleAxis(
const T
angle,
const Vec<T, 3> &axis)
53 if (vNorm < CV_QUAT_EPS)
55 CV_Error(Error::StsBadArg,
"this quaternion does not represent a rotation");
57 const T angle_half =
angle *
T(0.5);
60 const T sin_norm = sin_v / vNorm;
61 x = sin_norm * axis[0];
62 y = sin_norm * axis[1];
63 z = sin_norm * axis[2];
64 return Quat<T>(w,
x,
y, z);
68Quat<T> Quat<T>::createFromRotMat(InputArray _R)
71 if (_R.rows() != 3 || _R.cols() != 3)
73 CV_Error(Error::StsBadArg,
"Cannot convert matrix to quaternion: rotation matrix should be a 3x3 matrix");
83 x = (
R(1, 2) -
R(2, 1)) / S;
84 y = (
R(2, 0) -
R(0, 2)) / S;
85 z = (
R(0, 1) -
R(1, 0)) / S;
88 else if (
R(0, 0) >
R(1, 1) &&
R(0, 0) >
R(2, 2))
93 y = -(
R(1, 0) +
R(0, 1)) / S;
94 z = -(
R(0, 2) +
R(2, 0)) / S;
95 w = (
R(1, 2) -
R(2, 1)) / S;
97 else if (
R(1, 1) >
R(2, 2))
100 x = (
R(0, 1) +
R(1, 0)) / S;
102 z = (
R(1, 2) +
R(2, 1)) / S;
103 w = (
R(0, 2) -
R(2, 0)) / S;
108 x = (
R(0, 2) +
R(2, 0)) / S;
109 y = (
R(1, 2) +
R(2, 1)) / S;
111 w = -(
R(0, 1) -
R(1, 0)) / S;
113 return Quat<T> (w,
x,
y, z);
117Quat<T> Quat<T>::createFromRvec(InputArray _rvec)
119 if (!((_rvec.cols() == 1 && _rvec.rows() == 3) || (_rvec.cols() == 3 && _rvec.rows() == 1))) {
120 CV_Error(Error::StsBadArg,
"Cannot convert rotation vector to quaternion: The length of rotation vector should be 3");
125 if (
abs(psi) < CV_QUAT_EPS) {
126 return Quat<T> (1, 0, 0, 0);
128 Vec<T, 3> axis = rvec / psi;
129 return createFromAngleAxis(psi, axis);
133inline Quat<T> Quat<T>::operator-()
const
135 return Quat<T>(-w, -
x, -
y, -z);
140inline bool Quat<T>::operator==(
const Quat<T> &q)
const
142 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);
146inline Quat<T> Quat<T>::operator+(
const Quat<T> &q1)
const
148 return Quat<T>(w + q1.w,
x + q1.x,
y + q1.y, z + q1.z);
152inline Quat<T>
operator+(
const T a,
const Quat<T>& q)
154 return Quat<T>(q.w + a, q.x, q.y, q.z);
158inline Quat<T>
operator+(
const Quat<T>& q,
const T a)
160 return Quat<T>(q.w + a, q.x, q.y, q.z);
164inline Quat<T>
operator-(
const T a,
const Quat<T>& q)
166 return Quat<T>(a - q.w, -q.x, -q.y, -q.z);
170inline Quat<T>
operator-(
const Quat<T>& q,
const T a)
172 return Quat<T>(q.w - a, q.x, q.y, q.z);
176inline Quat<T> Quat<T>::operator-(
const Quat<T> &q1)
const
178 return Quat<T>(w - q1.w,
x - q1.x,
y - q1.y, z - q1.z);
182inline Quat<T>& Quat<T>::operator+=(
const Quat<T> &q1)
192inline Quat<T>& Quat<T>::operator-=(
const Quat<T> &q1)
202inline Quat<T> Quat<T>::operator*(
const Quat<T> &q1)
const
204 Vec<T, 4> q{w,
x,
y, z};
205 Vec<T, 4> q2{q1.w, q1.x, q1.y, q1.z};
206 return Quat<T>(q * q2);
211Quat<T>
operator*(
const Quat<T> &q1,
const T a)
213 return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z);
217Quat<T>
operator*(
const T a,
const Quat<T> &q1)
219 return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z);
223inline Quat<T>& Quat<T>::operator*=(
const Quat<T> &q1)
226 qw = w * q1.w -
x * q1.x -
y * q1.y - z * q1.z;
227 qx =
x * q1.w + w * q1.x +
y * q1.z - z * q1.y;
228 qy =
y * q1.w + w * q1.y + z * q1.x -
x * q1.z;
229 qz = z * q1.w + w * q1.z +
x * q1.y -
y * q1.x;
238inline Quat<T>& Quat<T>::operator/=(
const Quat<T> &q1)
240 Quat<T> q(*
this * q1.inv());
248Quat<T>& Quat<T>::operator*=(
const T q1)
258inline Quat<T>& Quat<T>::operator/=(
const T a)
260 const T a_inv = 1.0 / a;
269inline Quat<T> Quat<T>::operator/(
const T a)
const
271 const T a_inv =
T(1.0) / a;
272 return Quat<T>(w * a_inv,
x * a_inv,
y * a_inv, z * a_inv);
276inline Quat<T> Quat<T>::operator/(
const Quat<T> &q)
const
278 return *
this * q.inv();
294 CV_Error(Error::StsOutOfRange,
"subscript exceeds the index range");
311 CV_Error(Error::StsOutOfRange,
"subscript exceeds the index range");
318 os <<
"Quat " << Vec<T, 4>{q.w, q.x, q.y, q.z};
323inline T Quat<T>::at(
size_t index)
const
325 return (*
this)[
index];
329inline Quat<T> Quat<T>::conjugate()
const
331 return Quat<T>(w, -
x, -
y, -z);
335inline T Quat<T>::norm()
const
341Quat<T>
exp(
const Quat<T> &q)
347Quat<T> Quat<T>::exp()
const
349 Vec<T, 3> v{
x,
y, z};
351 T k = normV < CV_QUAT_EPS ? 1 :
std::sin(normV) / normV;
352 return std::exp(w) * Quat<T>(
std::cos(normV), v[0] * k, v[1] * k, v[2] * k);
356Quat<T>
log(
const Quat<T> &q, QuatAssumeType assumeUnit)
358 return q.log(assumeUnit);
362Quat<T> Quat<T>::log(QuatAssumeType assumeUnit)
const
364 Vec<T, 3> v{
x,
y, z};
368 T k = vNorm < CV_QUAT_EPS ? 1 :
std::acos(w) / vNorm;
369 return Quat<T>(0, v[0] * k, v[1] * k, v[2] * k);
372 if (qNorm < CV_QUAT_EPS)
374 CV_Error(Error::StsBadArg,
"Cannot apply this quaternion to log function: undefined");
376 T k = vNorm < CV_QUAT_EPS ? 1 :
std::acos(w / qNorm) / vNorm;
377 return Quat<T>(
std::log(qNorm), v[0] * k, v[1] * k, v[2] *k);
381inline Quat<T>
power(
const Quat<T> &q1,
const T
alpha, QuatAssumeType assumeUnit)
383 return q1.power(
alpha, assumeUnit);
387inline Quat<T> Quat<T>::power(
const T
alpha, QuatAssumeType assumeUnit)
const
389 if (
x *
x +
y *
y + z * z > CV_QUAT_EPS)
391 T angle = getAngle(assumeUnit);
392 Vec<T, 3> axis = getAxis(assumeUnit);
395 return createFromAngleAxis(
alpha *
angle, axis);
407inline Quat<T>
sqrt(
const Quat<T> &q, QuatAssumeType assumeUnit)
409 return q.sqrt(assumeUnit);
413inline Quat<T> Quat<T>::sqrt(QuatAssumeType assumeUnit)
const
415 return power(0.5, assumeUnit);
420inline Quat<T>
power(
const Quat<T> &p,
const Quat<T> &q, QuatAssumeType assumeUnit)
422 return p.power(q, assumeUnit);
427inline Quat<T> Quat<T>::power(
const Quat<T> &q, QuatAssumeType assumeUnit)
const
433inline T Quat<T>::dot(Quat<T> q1)
const
435 return w * q1.w +
x * q1.x +
y * q1.y + z * q1.z;
440inline Quat<T>
crossProduct(
const Quat<T> &p,
const Quat<T> &q)
442 return p.crossProduct(q);
447inline Quat<T> Quat<T>::crossProduct(
const Quat<T> &q)
const
449 return Quat<T> (0,
y * q.z - z * q.y, z * q.x -
x * q.z,
x * q.y - q.x *
y);
453inline Quat<T> Quat<T>::normalize()
const
456 if (normVal < CV_QUAT_EPS)
458 CV_Error(Error::StsBadArg,
"Cannot normalize this quaternion: the norm is too small.");
460 return Quat<T>(w / normVal,
x / normVal,
y / normVal, z / normVal) ;
464inline Quat<T>
inv(
const Quat<T> &q, QuatAssumeType assumeUnit)
466 return q.inv(assumeUnit);
471inline Quat<T> Quat<T>::inv(QuatAssumeType assumeUnit)
const
477 T norm2 = dot(*
this);
478 if (norm2 < CV_QUAT_EPS)
480 CV_Error(Error::StsBadArg,
"This quaternion do not have inverse quaternion");
486inline Quat<T>
sinh(
const Quat<T> &q)
493inline Quat<T> Quat<T>::sinh()
const
495 Vec<T, 3> v{
x,
y ,z};
503inline Quat<T>
cosh(
const Quat<T> &q)
510inline Quat<T> Quat<T>::cosh()
const
512 Vec<T, 3> v{
x,
y ,z};
519inline Quat<T>
tanh(
const Quat<T> &q)
525inline Quat<T> Quat<T>::tanh()
const
532inline Quat<T>
sin(
const Quat<T> &q)
539inline Quat<T> Quat<T>::sin()
const
541 Vec<T, 3> v{
x,
y ,z};
548inline Quat<T>
cos(
const Quat<T> &q)
554inline Quat<T> Quat<T>::cos()
const
556 Vec<T, 3> v{
x,
y ,z};
563inline Quat<T>
tan(
const Quat<T> &q)
569inline Quat<T> Quat<T>::tan()
const
571 return sin() *
cos().inv();
575inline Quat<T>
asinh(
const Quat<T> &q)
581inline Quat<T> Quat<T>::asinh()
const
583 return cv::log(*
this +
cv::power(*
this * *
this + Quat<T>(1, 0, 0, 0), 0.5));
587inline Quat<T>
acosh(
const Quat<T> &q)
593inline Quat<T> Quat<T>::acosh()
const
599inline Quat<T>
atanh(
const Quat<T> &q)
605inline Quat<T> Quat<T>::atanh()
const
607 Quat<T> ident(1, 0, 0, 0);
608 Quat<T> c1 = (ident + *
this).
log();
609 Quat<T> c2 = (ident - *
this).
log();
610 return 0.5 * (c1 - c2);
614inline Quat<T>
asin(
const Quat<T> &q)
620inline Quat<T> Quat<T>::asin()
const
622 Quat<T> v(0,
x,
y, z);
624 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
625 return -v /
k * (*
this * v /
k).
asinh();
629inline Quat<T>
acos(
const Quat<T> &q)
635inline Quat<T> Quat<T>::acos()
const
637 Quat<T> v(0,
x,
y, z);
639 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
644inline Quat<T>
atan(
const Quat<T> &q)
650inline Quat<T> Quat<T>::atan()
const
652 Quat<T> v(0,
x,
y, z);
654 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
655 return -v /
k * (*
this * v /
k).
atanh();
659inline T Quat<T>::getAngle(QuatAssumeType assumeUnit)
const
665 if (
norm() < CV_QUAT_EPS)
667 CV_Error(Error::StsBadArg,
"This quaternion does not represent a rotation");
673inline Vec<T, 3> Quat<T>::getAxis(QuatAssumeType assumeUnit)
const
675 T angle = getAngle(assumeUnit);
679 return Vec<T, 3>{
x,
y, z} / sin_v;
681 return Vec<T, 3> {
x,
y, z} / (
norm() * sin_v);
685Matx<T, 4, 4> Quat<T>::toRotMat4x4(QuatAssumeType assumeUnit)
const
687 T a = w, b =
x, c =
y, d = z;
697 1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c) , 0,
698 2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b) , 0,
699 2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c), 0,
706Matx<T, 3, 3> Quat<T>::toRotMat3x3(QuatAssumeType assumeUnit)
const
708 T a = w, b =
x, c =
y, d = z;
718 1 - 2 * (c * c + d * d), 2 * (b * c - a * d) , 2 * (b * d + a * c),
719 2 * (b * c + a * d) , 1 - 2 * (b * b + d * d), 2 * (c * d - a * b),
720 2 * (b * d - a * c) , 2 * (c * d + a * b) , 1 - 2 * (b * b + c * c)
726Vec<T, 3> Quat<T>::toRotVec(QuatAssumeType assumeUnit)
const
728 T angle = getAngle(assumeUnit);
729 Vec<T, 3> axis = getAxis(assumeUnit);
734Vec<T, 4> Quat<T>::toVec()
const
736 return Vec<T, 4>{w,
x,
y, z};
740Quat<T> Quat<T>::lerp(
const Quat<T> &q0,
const Quat<T> &q1,
const T t)
742 return (1 - t) * q0 + t * q1;
746Quat<T> Quat<T>::slerp(
const Quat<T> &q0,
const Quat<T> &q1,
const T t, QuatAssumeType assumeUnit,
bool directChange)
755 T cosTheta = v0.dot(v1);
756 constexpr T DOT_THRESHOLD = 0.995;
757 if (std::abs(cosTheta) > DOT_THRESHOLD)
759 return nlerp(v0, v1, t, QUAT_ASSUME_UNIT);
762 if (directChange && cosTheta < 0)
765 cosTheta = -cosTheta;
767 T sinTheta =
std::sqrt(1 - cosTheta * cosTheta);
774inline Quat<T> Quat<T>::nlerp(
const Quat<T> &q0,
const Quat<T> &q1,
const T t, QuatAssumeType assumeUnit)
776 Quat<T> v0(q0), v1(q1);
783 return ((1 - t) * v0 + t * v1).normalize();
787 return ((1 - t) * v0 + t * v1).normalize();
792inline bool Quat<T>::isNormal(T
eps)
const
795 double normVar =
norm();
796 if ((normVar > 1 -
eps) && (normVar < 1 +
eps))
802inline void Quat<T>::assertNormal(T
eps)
const
805 CV_Error(Error::StsBadArg,
"Quaternion should be normalized");
810inline Quat<T> Quat<T>::squad(
const Quat<T> &q0,
const Quat<T> &q1,
811 const Quat<T> &q2,
const Quat<T> &q3,
812 const T t, QuatAssumeType assumeUnit,
815 Quat<T> v0(q0), v1(q1), v2(q2), v3(q3);
824 Quat<T> c0 = slerp(v0, v3, t, assumeUnit, directChange);
825 Quat<T> c1 = slerp(v1, v2, t, assumeUnit, directChange);
826 return slerp(c0, c1, 2 * t * (1 - t), assumeUnit, directChange);
830Quat<T> Quat<T>::interPoint(
const Quat<T> &q0,
const Quat<T> &q1,
831 const Quat<T> &q2, QuatAssumeType assumeUnit)
833 Quat<T> v0(q0), v1(q1), v2(q2);
840 return v1 *
cv::exp(-(cv::log(v1.conjugate() * v0, assumeUnit) + (cv::log(v1.conjugate() * v2, assumeUnit))) / 4);
844Quat<T> Quat<T>::spline(
const Quat<T> &q0,
const Quat<T> &q1,
const Quat<T> &q2,
const Quat<T> &q3,
const T t, QuatAssumeType assumeUnit)
846 Quat<T> v0(q0), v1(q1), v2(q2), v3(q3);
856 for (
size_t i = 0; i < 3; ++i)
858 cosTheta = vec[i].dot(vec[i + 1]);
861 vec[i + 1] = -vec[i + 1];
864 Quat<T> s1 = interPoint(vec[0], vec[1], vec[2], QUAT_ASSUME_UNIT);
865 Quat<T> s2 = interPoint(vec[1], vec[2], vec[3], QUAT_ASSUME_UNIT);
866 return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT);
871template <
typename T>
static
872Quat<T> createFromAxisRot(
int axis,
const T
theta)
875 return Quat<T>::createFromXRot(
theta);
877 return Quat<T>::createFromYRot(
theta);
879 return Quat<T>::createFromZRot(
theta);
883inline bool isIntAngleType(QuatEnum::EulerAnglesType eulerAnglesType)
885 return eulerAnglesType < QuatEnum::EXT_XYZ;
888inline bool isTaitBryan(QuatEnum::EulerAnglesType eulerAnglesType)
890 return eulerAnglesType/6 == 1 || eulerAnglesType/6 == 3;
895Quat<T> Quat<T>::createFromYRot(
const T
theta)
901Quat<T> Quat<T>::createFromXRot(
const T
theta){
906Quat<T> Quat<T>::createFromZRot(
const T
theta){
912Quat<T> Quat<T>::createFromEulerAngles(
const Vec<T, 3> &angles, QuatEnum::EulerAnglesType eulerAnglesType) {
913 CV_Assert(eulerAnglesType < QuatEnum::EulerAnglesType::EULER_ANGLES_MAX_VALUE);
914 static const int rotationAxis[24][3] = {
940 Quat<T> q1 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][0], angles(0));
941 Quat<T> q2 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][1], angles(1));
942 Quat<T> q3 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][2], angles(2));
943 if (detail::isIntAngleType(eulerAnglesType))
954Vec<T, 3> Quat<T>::toEulerAngles(QuatEnum::EulerAnglesType eulerAnglesType){
955 CV_Assert(eulerAnglesType < QuatEnum::EulerAnglesType::EULER_ANGLES_MAX_VALUE);
962 R_0_0 = N_CONSTANTS, R_0_1, R_0_2,
966 static const T constants_[N_CONSTANTS] = {
971 static const int rotationR_[24][12] = {
972 {+R_0_2, +R_1_0, +R_1_1, C_PI_2, +R_2_1, +R_1_1, -C_PI_2, -R_1_2, +R_2_2, +R_0_2, -R_0_1, +R_0_0},
973 {+R_0_1, -R_1_2, +R_2_2, -C_PI_2, +R_2_0, +R_2_2, C_PI_2, +R_2_1, +R_1_1, -R_0_1, +R_0_2, +R_0_0},
974 {+R_1_2, -R_0_1, +R_0_0, -C_PI_2, +R_0_1, +R_0_0, C_PI_2, +R_0_2, +R_2_2, -R_1_2, +R_1_0, +R_1_1},
975 {+R_1_0, +R_0_2, +R_2_2, C_PI_2, +R_0_2, +R_0_1, -C_PI_2, -R_2_0, +R_0_0, +R_1_0, -R_1_2, +R_1_1},
976 {+R_2_1, +R_1_0, +R_0_0, C_PI_2, +R_1_0, +R_0_0, -C_PI_2, -R_0_1, +R_1_1, +R_2_1, -R_2_0, +R_2_2},
977 {+R_2_0, -R_0_1, +R_1_1, -C_PI_2, +R_1_2, +R_1_1, C_PI_2, +R_1_0, +R_0_0, -R_2_0, +R_2_1, +R_2_2},
978 {+R_0_0, +R_2_1, +R_2_2, C_ZERO, +R_1_2, +R_1_1, C_PI, +R_1_0, -R_2_0, +R_0_0, +R_0_1, +R_0_2},
979 {+R_0_0, +R_2_1, +R_2_2, C_ZERO, -R_2_1, +R_2_2, C_PI, +R_2_0, +R_1_0, +R_0_0, +R_0_2, -R_0_1},
980 {+R_1_1, +R_0_2, +R_0_0, C_ZERO, -R_2_0, +R_0_0, C_PI, +R_0_1, +R_2_1, +R_1_1, +R_1_0, -R_1_2},
981 {+R_1_1, +R_0_2, +R_0_0, C_ZERO, +R_0_2, -R_0_0, C_PI, +R_2_1, -R_0_1, +R_1_1, +R_1_2, +R_1_0},
982 {+R_2_2, +R_1_0, +R_1_1, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_0_2, -R_1_2, +R_2_2, +R_2_0, +R_2_1},
983 {+R_2_2, +R_1_0, +R_0_0, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_1_2, +R_0_2, +R_2_2, +R_2_1, -R_2_0},
985 {+R_2_0, -C_PI_2, -R_0_1, +R_1_1, C_PI_2, +R_1_2, +R_1_1, +R_2_1, +R_2_2, -R_2_0, +R_1_0, +R_0_0},
986 {+R_1_0, C_PI_2, +R_0_2, +R_2_2, -C_PI_2, +R_0_2, +R_0_1, -R_1_2, +R_1_1, +R_1_0, -R_2_0, +R_0_0},
987 {+R_2_1, C_PI_2, +R_1_0, +R_0_0, -C_PI_2, +R_1_0, +R_0_0, -R_2_0, +R_2_2, +R_2_1, -R_0_1, +R_1_1},
988 {+R_0_2, -C_PI_2, -R_1_2, +R_2_2, C_PI_2, +R_2_0, +R_2_2, +R_0_2, +R_0_0, -R_0_1, +R_2_1, +R_1_1},
989 {+R_1_2, -C_PI_2, -R_0_1, +R_0_0, C_PI_2, +R_0_1, +R_0_0, +R_1_0, +R_1_1, -R_1_2, +R_0_2, +R_2_2},
990 {+R_0_2, C_PI_2, +R_1_0, +R_1_1, -C_PI_2, +R_2_1, +R_1_1, -R_0_1, +R_0_0, +R_0_2, -R_1_2, +R_2_2},
991 {+R_0_0, C_ZERO, +R_2_1, +R_2_2, C_PI, +R_1_2, +R_1_1, +R_0_1, +R_0_2, +R_0_0, +R_1_0, -R_2_0},
992 {+R_0_0, C_ZERO, +R_2_1, +R_2_2, C_PI, +R_2_1, +R_2_2, +R_0_2, -R_0_1, +R_0_0, +R_2_0, +R_1_0},
993 {+R_1_1, C_ZERO, +R_0_2, +R_0_0, C_PI, -R_2_0, +R_0_0, +R_1_0, -R_1_2, +R_1_1, +R_0_1, +R_2_1},
994 {+R_1_1, C_ZERO, +R_0_2, +R_0_0, C_PI, +R_0_2, -R_0_0, +R_1_2, +R_1_0, +R_1_1, +R_2_1, -R_0_1},
995 {+R_2_2, C_ZERO, +R_1_0, +R_1_1, C_PI, +R_1_0, +R_0_0, +R_2_0, +R_2_1, +R_2_2, +R_0_2, -R_1_2},
996 {+R_2_2, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_1_0, +R_0_0, +R_2_1, -R_2_0, +R_2_2, +R_1_2, +R_0_2},
999 for (
int i = 0; i < 12; i++)
1001 int id = rotationR_[eulerAnglesType][i];
1002 unsigned idx = std::abs(
id);
1004 if (
idx < N_CONSTANTS)
1010 unsigned r_idx =
idx - N_CONSTANTS;
1014 bool isNegative =
id < 0;
1017 rotationR[i] =
value;
1020 if (detail::isIntAngleType(eulerAnglesType))
1022 if (
abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
1024 CV_LOG_WARNING(NULL,
"Gimbal Lock occurs. Euler angles are non-unique, we set the third angle to 0");
1025 angles = {
std::atan2(rotationR[1], rotationR[2]), rotationR[3], 0};
1028 else if(
abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
1030 CV_LOG_WARNING(NULL,
"Gimbal Lock occurs. Euler angles are non-unique, we set the third angle to 0");
1031 angles = {
std::atan2(rotationR[4], rotationR[5]), rotationR[6], 0};
1037 if (
abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
1039 CV_LOG_WARNING(NULL,
"Gimbal Lock occurs. Euler angles are non-unique, we set the first angle to 0");
1040 angles = {0, rotationR[1],
std::atan2(rotationR[2], rotationR[3])};
1043 else if (
abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
1045 CV_LOG_WARNING(NULL,
"Gimbal Lock occurs. Euler angles are non-unique, we set the first angle to 0");
1046 angles = {0, rotationR[4],
std::atan2(rotationR[5], rotationR[6])};
1051 angles(0) =
std::atan2(rotationR[7], rotationR[8]);
1052 if (detail::isTaitBryan(eulerAnglesType))
1056 angles(2) =
std::atan2(rotationR[10], rotationR[11]);
InputArrayOfArrays InputArrayOfArrays InputOutputArray InputOutputArray InputOutputArray InputOutputArray Size InputOutputArray InputOutputArray T
Definition calib3d.hpp:1867
InputArrayOfArrays InputArrayOfArrays InputOutputArray InputOutputArray InputOutputArray InputOutputArray Size InputOutputArray R
Definition calib3d.hpp:1867
CV_EXPORTS_W void exp(InputArray src, OutputArray dst)
Calculates the exponent of every array element.
CV_EXPORTS_W double norm(InputArray src1, int normType=NORM_L2, InputArray mask=noArray())
Calculates the absolute norm of an array.
CV_EXPORTS_W Scalar trace(InputArray mtx)
Returns the trace of a matrix.
CV_EXPORTS_W void log(InputArray src, OutputArray dst)
Calculates the natural logarithm of every array element.
CV_EXPORTS_W void normalize(InputArray src, InputOutputArray dst, double alpha=1, double beta=0, int norm_type=NORM_L2, int dtype=-1, InputArray mask=noArray())
Normalizes the norm or value range of an array.
Matx< double, 3, 3 > Matx33d
Definition matx.hpp:247
int CvScalar value
Definition core_c.h:720
const int * idx
Definition core_c.h:668
int index
Definition core_c.h:634
const CvArr * angle
Definition core_c.h:1194
CvArr double power
Definition core_c.h:1199
const CvArr CvArr * x
Definition core_c.h:1195
double alpha
Definition core_c.h:1093
const CvArr * y
Definition core_c.h:1187
#define CV_LOG_WARNING(tag,...)
Definition logger.hpp:131
softfloat abs(softfloat a)
Absolute value.
Definition softfloat.hpp:444
#define CV_Error(code, msg)
Call the error handler.
Definition base.hpp:320
#define CV_PI
Definition cvdef.h:380
#define CV_Assert(expr)
Checks a condition at runtime and throws exception if it fails.
Definition base.hpp:342
#define CV_DbgAssert(expr)
Definition base.hpp:375
Quat< T > crossProduct(const Quat< T > &p, const Quat< T > &q)
std::ostream & operator<<(std::ostream &, const DualQuat< _Tp > &)
void int double double theta
Definition imgproc_c.h:926
int CvMemStorage int double eps
Definition imgproc_c.h:493
CV_EXPORTS OutputArray int double double InputArray OutputArray int int bool double k
Definition imgproc.hpp:2133
"black box" representation of the file storage associated with a file on disk.
Definition calib3d.hpp:441
DualQuat< T > inv(const DualQuat< T > &dq, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
Definition dualquaternion.inl.hpp:187
DualQuat< T > operator-(const DualQuat< T > &q, const T a)
Definition dualquaternion.inl.hpp:255
DualQuat< T > conjugate(const DualQuat< T > &dq)
Definition dualquaternion.inl.hpp:125
DualQuat< T > power(const DualQuat< T > &dq, const T t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
Definition dualquaternion.inl.hpp:358
DualQuat< T > operator*(const T a, const DualQuat< T > &q)
Definition dualquaternion.inl.hpp:274
QKeyCombination operator+(Qt::Key key, Qt::KeyboardModifier modifier)
Definition traits.hpp:386