diff --git a/modules/core/include/opencv2/core/dualquaternion.inl.hpp b/modules/core/include/opencv2/core/dualquaternion.inl.hpp index 4aec961dd2..6abb15924b 100644 --- a/modules/core/include/opencv2/core/dualquaternion.inl.hpp +++ b/modules/core/include/opencv2/core/dualquaternion.inl.hpp @@ -65,7 +65,7 @@ DualQuat DualQuat::createFromAngleAxisTrans(const T angle, const Vec { Quat r = Quat::createFromAngleAxis(angle, axis); Quat t{0, trans[0], trans[1], trans[2]}; - return createFromQuat(r, t * r / 2); + return createFromQuat(r, t * r * T(0.5)); } template @@ -79,7 +79,7 @@ DualQuat DualQuat::createFromMat(InputArray _R) Mat R = _R.getMat(); Quat r = Quat::createFromRotMat(R.colRange(0, 3).rowRange(0, 3)); Quat trans(0, R.at(0, 3), R.at(1, 3), R.at(2, 3)); - return createFromQuat(r, trans * r / 2); + return createFromQuat(r, trans * r * T(0.5)); } template @@ -91,7 +91,7 @@ DualQuat DualQuat::createFromAffine3(const Affine3 &R) template DualQuat DualQuat::createFromPitch(const T angle, const T d, const Vec &axis, const Vec &moment) { - T half_angle = angle / 2, half_d = d / 2; + T half_angle = angle * T(0.5), half_d = d * T(0.5); Quat qaxis = Quat(0, axis[0], axis[1], axis[2]).normalize(); Quat qmoment = Quat(0, moment[0], moment[1], moment[2]); qmoment -= qaxis * axis.dot(moment); @@ -158,7 +158,7 @@ inline Quat DualQuat::getRotation(QuatAssumeType assumeUnit) const template inline Vec DualQuat::getTranslation(QuatAssumeType assumeUnit) const { - Quat trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit)); + Quat trans = T(2.0) * (getDualPart() * getRealPart().inv(assumeUnit)); return Vec{trans[1], trans[2], trans[3]}; } @@ -320,8 +320,8 @@ template Matx<_Tp, 4, 4> jacob_exp(const Quat<_Tp> &q) { _Tp nv = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z); - _Tp sinc_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? 1 - nv * nv / 6 : std::sin(nv) / nv; - _Tp csiii_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? -(_Tp)1.0 / 3 : (std::cos(nv) - sinc_nv) / nv / nv; + _Tp sinc_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? _Tp(1.0) - nv * nv * _Tp(1.0/6.0) : std::sin(nv) / nv; + _Tp csiii_nv = abs(nv) < cv::DualQuat<_Tp>::CV_DUAL_QUAT_EPS ? -_Tp(1.0/3.0) : (std::cos(nv) - sinc_nv) / nv / nv; Matx<_Tp, 4, 4> J_exp_quat { std::cos(nv), -sinc_nv * q.x, -sinc_nv * q.y, -sinc_nv * q.z, sinc_nv * q.x, csiii_nv * q.x * q.x + sinc_nv, csiii_nv * q.x * q.y, csiii_nv * q.x * q.z, diff --git a/modules/core/include/opencv2/core/quaternion.inl.hpp b/modules/core/include/opencv2/core/quaternion.inl.hpp index 3c2fce10af..29a16d9f7d 100644 --- a/modules/core/include/opencv2/core/quaternion.inl.hpp +++ b/modules/core/include/opencv2/core/quaternion.inl.hpp @@ -54,7 +54,7 @@ Quat Quat::createFromAngleAxis(const T angle, const Vec &axis) { CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation"); } - const T angle_half = angle * 0.5; + const T angle_half = angle * T(0.5); w = std::cos(angle_half); const T sin_v = std::sin(angle_half); const T sin_norm = sin_v / vNorm; @@ -79,35 +79,35 @@ Quat Quat::createFromRotMat(InputArray _R) T trace = R(0, 0) + R(1, 1) + R(2, 2); if (trace > 0) { - S = std::sqrt(trace + 1) * 2; + S = std::sqrt(trace + 1) * T(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; + w = -T(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; + S = std::sqrt(T(1.0) + R(0, 0) - R(1, 1) - R(2, 2)) * T(2); + x = -T(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; + S = std::sqrt(T(1.0) - R(0, 0) + R(1, 1) - R(2, 2)) * T(2); x = (R(0, 1) + R(1, 0)) / S; - y = 0.25 * S; + y = T(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; + S = std::sqrt(T(1.0) - R(0, 0) - R(1, 1) + R(2, 2)) * T(2); x = (R(0, 2) + R(2, 0)) / S; y = (R(1, 2) + R(2, 1)) / S; - z = 0.25 * S; + z = T(0.25) * S; w = -(R(0, 1) - R(1, 0)) / S; } return Quat (w, x, y, z); @@ -268,7 +268,7 @@ inline Quat& Quat::operator/=(const T a) template inline Quat Quat::operator/(const T a) const { - const T a_inv = 1.0 / a; + const T a_inv = T(1.0) / a; return Quat(w * a_inv, x * a_inv, y * a_inv, z * a_inv); }