27#ifndef OPENCV_CORE_DUALQUATERNION_INL_HPP
28#define OPENCV_CORE_DUALQUATERNION_INL_HPP
30#ifndef OPENCV_CORE_DUALQUATERNION_HPP
31#error This is not a standalone header. Include dualquaternion.hpp instead.
39DualQuat<T>::DualQuat():w(0), x(0), y(0), z(0), w_(0), x_(0), y_(0), z_(0){};
42DualQuat<T>::DualQuat(
const T vw,
const T vx,
const T vy,
const T vz,
const T _w,
const T _x,
const T _y,
const T _z):
43 w(vw), x(vx), y(vy), z(vz), w_(_w), x_(_x), y_(_y), z_(_z){};
46DualQuat<T>::DualQuat(
const Vec<T, 8> &q):w(q[0]), x(q[1]), y(q[2]), z(q[3]),
47 w_(q[4]), x_(q[5]), y_(q[6]), z_(q[7]){};
67 Quat<T> t{0, trans[0], trans[1], trans[2]};
68 return createFromQuat(r, t * r / 2);
75 if (_R.size() !=
Size(4, 4))
77 CV_Error(Error::StsBadArg,
"The input matrix must have 4 columns and 4 rows");
81 Quat<T> trans(0, R.at<T>(0, 3), R.at<T>(1, 3), R.at<T>(2, 3));
82 return createFromQuat(r, trans * r / 2);
88 return createFromMat(R.matrix);
94 T half_angle = angle / 2, half_d = d / 2;
97 qmoment -= qaxis * axis.
dot(moment);
99 half_d *
std::cos(half_angle) * qaxis;
106 return (
abs(w - q.w) < CV_DUAL_QUAT_EPS &&
abs(x - q.x) < CV_DUAL_QUAT_EPS &&
107 abs(y - q.y) < CV_DUAL_QUAT_EPS &&
abs(z - q.z) < CV_DUAL_QUAT_EPS &&
108 abs(w_ - q.w_) < CV_DUAL_QUAT_EPS &&
abs(x_ - q.x_) < CV_DUAL_QUAT_EPS &&
109 abs(y_ - q.y_) < CV_DUAL_QUAT_EPS &&
abs(z_ - q.z_) < CV_DUAL_QUAT_EPS);
121 return Quat<T>(w_, x_, y_, z_);
133 return DualQuat<T>(w, -x, -y, -z, w_, -x_, -y_, -z_);
140 T realNorm = real.
norm();
142 if (realNorm < CV_DUAL_QUAT_EPS){
145 return DualQuat<T>(realNorm, 0, 0, 0, real.
dot(dual) / realNorm, 0, 0, 0);
153 return getRealPart();
155 return getRealPart().normalize();
161 Quat<T> trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit));
162 return Vec<T, 3>{trans[1], trans[2], trans[3]};
171 if (p_norm < CV_DUAL_QUAT_EPS)
173 CV_Error(Error::StsBadArg,
"Cannot normalize this dual quaternion: the norm is too small.");
177 return createFromQuat(p_nr, q_nr - p_nr * p_nr.
dot(q_nr));
183 return q.w * w + q.x * x + q.y * y + q.z * z + q.w_ * w_ + q.x_ * x_ + q.y_ * y_ + q.z_ * z_;
189 return dq.
inv(assumeUnit);
197 return createFromQuat(real.
inv(assumeUnit), -real.
inv(assumeUnit) * dual * real.
inv(assumeUnit));
203 return DualQuat<T>(w - q.w, x - q.x, y - q.y, z - q.z, w_ - q.w_, x_ - q.x_, y_ - q.y_, z_ - q.z_);
209 return DualQuat<T>(-w, -x, -y, -z, -w_, -x_, -y_, -z_);
215 return DualQuat<T>(w + q.w, x + q.x, y + q.y, z + q.z, w_ + q.w_, x_ + q.x_, y_ + q.y_, z_ + q.z_);
243inline DualQuat<T>
operator+(
const T a,
const DualQuat<T> &q)
245 return DualQuat<T>(a + q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
249inline DualQuat<T>
operator+(
const DualQuat<T> &q,
const T a)
251 return DualQuat<T>(a + q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
255inline DualQuat<T>
operator-(
const DualQuat<T> &q,
const T a)
257 return DualQuat<T>(q.w - a, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_);
270 return DualQuat<T>(a - q.w, -q.x, -q.y, -q.z, -q.w_, -q.x_, -q.y_, -q.z_);
274inline DualQuat<T>
operator*(
const T a,
const DualQuat<T> &q)
276 return DualQuat<T>(q.w * a, q.x * a, q.y * a, q.z * a, q.w_ * a, q.x_ * a, q.y_ * a, q.z_ * a);
280inline DualQuat<T>
operator*(
const DualQuat<T> &q,
const T a)
282 return DualQuat<T>(q.w * a, q.x * a, q.y * a, q.z * a, q.w_ * a, q.x_ * a, q.y_ * a, q.z_ * a);
288 return DualQuat<T>(w / a, x / a, y / a, z / a, w_ / a, x_ / a, y_ / a, z_ / a);
294 return *
this * q.inv();
298inline DualQuat<T>& DualQuat<T>::operator/=(
const DualQuat<T> &q)
305std::ostream &
operator<<(std::ostream &os,
const DualQuat<T> &q)
307 os <<
"DualQuat " << Vec<T, 8>{q.w, q.x, q.y, q.z, q.w_, q.x_, q.y_, q.z_};
319template <
typename _Tp>
322 _Tp nv =
std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
326 std::cos(nv), -sinc_nv * q.x, -sinc_nv * q.y, -sinc_nv * q.z,
327 sinc_nv * q.x, csiii_nv * q.x * q.x + sinc_nv, csiii_nv * q.x * q.y, csiii_nv * q.x * q.z,
328 sinc_nv * q.y, csiii_nv * q.y * q.x, csiii_nv * q.y * q.y + sinc_nv, csiii_nv * q.y * q.z,
329 sinc_nv * q.z, csiii_nv * q.z * q.x, csiii_nv * q.z * q.y, csiii_nv * q.z * q.z + sinc_nv
340 return createFromQuat(real.
exp(),
Quat<T>(detail::jacob_exp(real) * getDualPart().toVec()));
346 return dq.
log(assumeUnit);
354 return createFromQuat(plog,
Quat<T>(jacob.
inv() * getDualPart().toVec()));
360 return dq.
power(t, assumeUnit);
366 return (t *
log(assumeUnit)).
exp();
372 return p.
power(q, assumeUnit);
378 return (q *
log(assumeUnit)).
exp();
384 return Vec<T, 8>(w, x, y, z, w_, x_, y_, z_);
390 return Affine3<T>(toMat(assumeUnit));
397 Vec<T, 3> translation = getTranslation(assumeUnit);
398 rot44(0, 3) = translation[0];
399 rot44(1, 3) = translation[1];
400 rot44(2, 3) = translation[2];
413 Quat<T> v0Real = v0.getRealPart();
415 if (directChange && v1Real.
dot(v0Real) < 0)
432 if (v1.getRotation(assumeUnit).dot(v2.
getRotation(assumeUnit)) < 0)
434 return ((1 - t) * v1 - t * v2).normalize();
436 return ((1 - t) * v1 + t * v2).normalize();
444 Size dq_s = _dualquat.size();
445 if (dq_s != _weight.size() || (dq_s.
height != 1 && dq_s.
width != 1))
447 CV_Error(Error::StsBadArg,
"The size of weight must be the same as dualquat, both of them should be (1, n) or (n, 1)");
449 Mat dualquat = _dualquat.getMat(), weight = _weight.getMat();
453 for (
int i = 0; i < cn; ++i)
460 for (
int i = 1; i < cn; ++i)
463 dq_blend = dq_blend + dualquat.
at<
Vec<T, 8>>(i) * k * weight.at<T>(i);
477 Mat dualquat_mat(cn, 1, CV_64FC(8));
478 for (
int i = 0; i < cn ; ++i)
480 dualquat_mat.at<Vec<T, 8>>(i) = dualquat[i].toVec();
482 return gdqblend(dualquat_mat, _weight, assumeUnit);
Definition: dualquaternion.hpp:146
Quat< _Tp > getRealPart() const
return a quaternion which represent the real part of dual quaternion. The definition of real part is ...
Definition: dualquaternion.inl.hpp:113
friend DualQuat< T > power(const DualQuat< T > &dq, const T t, QuatAssumeType assumeUnit)
return the value of where p is a dual quaternion. This could be calculated as:
Definition: dualquaternion.inl.hpp:358
friend DualQuat< T > exp(const DualQuat< T > &dq)
return the value of exponential function value
Definition: dualquaternion.inl.hpp:312
friend DualQuat< T > log(const DualQuat< T > &dq, QuatAssumeType assumeUnit)
return the value of logarithm function value
Definition: dualquaternion.inl.hpp:344
Quat< _Tp > getDualPart() const
return a quaternion which represent the dual part of dual quaternion. The definition of dual part is ...
Definition: dualquaternion.inl.hpp:119
friend DualQuat< T > inv(const DualQuat< T > &dq, QuatAssumeType assumeUnit)
if is a dual quaternion, p is not zero, the inverse dual quaternion is
Definition: dualquaternion.inl.hpp:187
friend DualQuat< T > conjugate(const DualQuat< T > &dq)
return the conjugate of a dual quaternion.
Definition: dualquaternion.inl.hpp:125
Quat< _Tp > getRotation(QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT) const
return the rotation in quaternion form.
Definition: dualquaternion.inl.hpp:149
DualQuat< _Tp > normalize() const
return a normalized dual quaternion. A dual quaternion can be expressed as
Definition: dualquaternion.inl.hpp:166
CV_EXPORTS FileStorage & operator<<(FileStorage &fs, const String &str)
Writes string to a file storage.
CV_EXPORTS MatExpr abs(const Mat &m)
Calculates an absolute value of each matrix element.
n-dimensional dense array class
Definition: mat.hpp:802
_Tp & at(int i0=0)
Returns a reference to the specified array element.
Template class for small matrices whose type and size are known at compilation time
Definition: matx.hpp:100
Matx< _Tp, n, m > inv(int method=DECOMP_LU, bool *p_is_ok=NULL) const
invert the matrix
_Tp dot(const Matx< _Tp, m, n > &v) const
dot product computed with the default precision
Definition: quaternion.hpp:211
_Tp dot(Quat< _Tp > q) const
return the dot between quaternion and this quaternion.
friend Quat< T > log(const Quat< T > &q, QuatAssumeType assumeUnit)
return the value of logarithm function.
Quat< _Tp > normalize() const
return a normalized .
friend Quat< T > inv(const Quat< T > &q, QuatAssumeType assumeUnit)
return which is an inverse of which satisfies .
friend Quat< T > exp(const Quat< T > &q)
return the value of exponential value.
_Tp norm() const
return the norm of quaternion.
Template class for specifying the size of an image or rectangle.
Definition: core/types.hpp:316
_Tp height
the height
Definition: core/types.hpp:340
_Tp width
the width
Definition: core/types.hpp:339
Template class for short numerical vectors, a partial case of Matx
Definition: matx.hpp:342
CV_EXPORTS_W void max(InputArray src1, InputArray src2, OutputArray dst)
Calculates per-element maximum of two arrays or an array and a scalar.
CV_EXPORTS_W void sqrt(InputArray src, OutputArray dst)
Calculates a square root of array elements.
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 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.
CV_INLINE v_reg< _Tp, n > operator*(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Multiply values
CV_INLINE v_reg< _Tp, n > operator+(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Add values
CV_INLINE v_reg< _Tp, n > operator/(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Divide values
CV_INLINE v_reg< _Tp, n > operator-(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Subtract values
softfloat abs(softfloat a)
Absolute value
Definition: softfloat.hpp:444
#define CV_Error(code, msg)
Call the error handler.
Definition: base.hpp:320
Quat< T > inv(const Quat< T > &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
Quat< T > cos(const Quat< T > &q)
QuatAssumeType
Unit quaternion flag
Definition: quaternion.hpp:39
Quat< T > sin(const Quat< T > &q)
@ QUAT_ASSUME_NOT_UNIT
Definition: quaternion.hpp:46
@ QUAT_ASSUME_UNIT
Definition: quaternion.hpp:52
"black box" representation of the file storage associated with a file on disk.
Definition: aruco.hpp:75
DualQuat< T > conjugate(const DualQuat< T > &dq)
Definition: dualquaternion.inl.hpp:125
Definition: traits.hpp:386