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

Merge pull request #21319 from savuor:backport_levmarqfromscratch

Browse Source 
Warning fixes for quaternion headers

* warning fixes for quaternion headers

* a/T(x) => a * T(1/x)
This commit is contained in:
Rostislav Vasilikhin 2021-12-25 02:00:21 +03:00 committed by GitHub
parent 547bb3b053
commit 3048188b5b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
modules/core/include/opencv2/core/dualquaternion.inl.hpp
View File

@ -65,7 +65,7 @@ DualQuat<T> DualQuat<T>::createFromAngleAxisTrans(const T angle, const Vec<T, 3>
{
Quat<T> r = Quat<T>::createFromAngleAxis(angle, axis);
Quat<T> t{0, trans[0], trans[1], trans[2]};
return createFromQuat(r, t * r / 2);
return createFromQuat(r, t * r * T(0.5));
}
template <typename T>
@ -79,7 +79,7 @@ DualQuat<T> DualQuat<T>::createFromMat(InputArray _R)
Mat R = _R.getMat();
Quat<T> r = Quat<T>::createFromRotMat(R.colRange(0, 3).rowRange(0, 3));
Quat<T> trans(0, R.at<T>(0, 3), R.at<T>(1, 3), R.at<T>(2, 3));
return createFromQuat(r, trans * r / 2);
return createFromQuat(r, trans * r * T(0.5));
}
template <typename T>
@ -91,7 +91,7 @@ DualQuat<T> DualQuat<T>::createFromAffine3(const Affine3<T> &R)
template <typename T>
DualQuat<T> DualQuat<T>::createFromPitch(const T angle, const T d, const Vec<T, 3> &axis, const Vec<T, 3> &moment)
{
T half_angle = angle / 2, half_d = d / 2;
T half_angle = angle * T(0.5), half_d = d * T(0.5);
Quat<T> qaxis = Quat<T>(0, axis[0], axis[1], axis[2]).normalize();
Quat<T> qmoment = Quat<T>(0, moment[0], moment[1], moment[2]);
qmoment -= qaxis * axis.dot(moment);
@ -158,7 +158,7 @@ inline Quat<T> DualQuat<T>::getRotation(QuatAssumeType assumeUnit) const
template <typename T>
inline Vec<T, 3> DualQuat<T>::getTranslation(QuatAssumeType assumeUnit) const
{
Quat<T> trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit));
Quat<T> trans = T(2.0) * (getDualPart() * getRealPart().inv(assumeUnit));
return Vec<T, 3>{trans[1], trans[2], trans[3]};
}
@ -320,8 +320,8 @@ template <typename _Tp>
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,

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

@ -54,7 +54,7 @@ Quat<T> Quat<T>::createFromAngleAxis(const T angle, const Vec<T, 3> &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<T> Quat<T>::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<T> (w, x, y, z);
@ -268,7 +268,7 @@ inline Quat<T>& Quat<T>::operator/=(const T a)
template <typename T>
inline Quat<T> Quat<T>::operator/(const T a) const
{
const T a_inv = 1.0 / a;
const T a_inv = T(1.0) / a;
return Quat<T>(w * a_inv, x * a_inv, y * a_inv, z * a_inv);
}