OpenCV 4.5.3(日本語機械翻訳)
quaternion.inl.hpp
1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4 //
5 //
6 // License Agreement
7 // For Open Source Computer Vision Library
8 //
9 // Copyright (C) 2020, Huawei Technologies Co., Ltd. All rights reserved.
10 // Third party copyrights are property of their respective owners.
11 //
12 // Licensed under the Apache License, Version 2.0 (the "License");
13 // you may not use this file except in compliance with the License.
14 // You may obtain a copy of the License at
15 //
16 // http://www.apache.org/licenses/LICENSE-2.0
17 //
18 // Unless required by applicable law or agreed to in writing, software
19 // distributed under the License is distributed on an "AS IS" BASIS,
20 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
21 // See the License for the specific language governing permissions and
22 // limitations under the License.
23 //
24 // Author: Liangqian Kong <chargerKong@126.com>
25 // Longbu Wang <riskiest@gmail.com>
26
27 #ifndef OPENCV_CORE_QUATERNION_INL_HPP
28 #define OPENCV_CORE_QUATERNION_INL_HPP
29
30 #ifndef OPENCV_CORE_QUATERNION_HPP
31 #erorr This is not a standalone header. Include quaternion.hpp instead.
32 #endif
33
34 //@cond IGNORE
36 //Implementation
37 namespace cv {
38
39 template <typename T>
40Quat<T>::Quat() : w(0), x(0), y(0), z(0) {}
41
42 template <typename T>
43Quat<T>::Quat(const Vec<T, 4> &coeff):w(coeff[0]), x(coeff[1]), y(coeff[2]), z(coeff[3]){}
44
45 template <typename T>
46Quat<T>::Quat(const T qw, const T qx, const T qy, const T qz):w(qw), x(qx), y(qy), z(qz){}
47
48 template <typename T>
49Quat<T> Quat<T>::createFromAngleAxis(const T angle, const Vec<T, 3> &axis)
50{
51 T w, x, y, z;
52 T vNorm = std::sqrt(axis.dot(axis));
53 if (vNorm < CV_QUAT_EPS)
54 {
55 CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation");
56 }
57 const T angle_half = angle * 0.5;
58 w = std::cos(angle_half);
59 const T sin_v = std::sin(angle_half);
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);
65}
66
67 template <typename T>
68Quat<T> Quat<T>::createFromRotMat(InputArray _R)
69{
70 CV_CheckTypeEQ(_R.type(), cv::traits::Type<T>::value, "");
71 if (_R.rows() != 3 || _R.cols() != 3)
72 {
73 CV_Error(Error::StsBadArg, "Cannot convert matrix to quaternion: rotation matrix should be a 3x3 matrix");
74 }
75 Matx<T, 3, 3> R;
76 _R.copyTo(R);
77
78 T S, w, x, y, z;
79 T trace = R(0, 0) + R(1, 1) + R(2, 2);
80 if (trace > 0)
81 {
82 S = std::sqrt(trace + 1) * 2;
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;
86 w = -0.25 * S;
87 }
88 else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2))
89 {
90
91 S = std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2;
92 x = -0.25 * S;
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;
96 }
97 else if (R(1, 1) > R(2, 2))
98 {
99 S = std::sqrt(1.0 - R(0, 0) + R(1, 1) - R(2, 2)) * 2;
100 x = (R(0, 1) + R(1, 0)) / S;
101 y = 0.25 * S;
102 z = (R(1, 2) + R(2, 1)) / S;
103 w = (R(0, 2) - R(2, 0)) / S;
104 }
105 else
106 {
107 S = std::sqrt(1.0 - R(0, 0) - R(1, 1) + R(2, 2)) * 2;
108 x = (R(0, 2) + R(2, 0)) / S;
109 y = (R(1, 2) + R(2, 1)) / S;
110 z = 0.25 * S;
111 w = -(R(0, 1) - R(1, 0)) / S;
112 }
113 return Quat<T> (w, x, y, z);
114}
115
116 template <typename T>
117Quat<T> Quat<T>::createFromRvec(InputArray _rvec)
118{
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");
121 }
122 Vec<T, 3> rvec;
123 _rvec.copyTo(rvec);
124 T psi = std::sqrt(rvec.dot(rvec));
125 if (abs(psi) < CV_QUAT_EPS) {
126 return Quat<T> (1, 0, 0, 0);
127 }
128 Vec<T, 3> axis = rvec / psi;
129 return createFromAngleAxis(psi, axis);
130}
131
132 template <typename T>
133 inline Quat<T> Quat<T>::operator-() const
134 {
135 return Quat<T>(-w, -x, -y, -z);
136}
137
138
139 template <typename T>
140 inline bool Quat<T>::operator==(const Quat<T> &q) const
141 {
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);
143}
144
145 template <typename T>
146 inline Quat<T> Quat<T>::operator+(const Quat<T> &q1) const
147 {
148 return Quat<T>(w + q1.w, x + q1.x, y + q1.y, z + q1.z);
149}
150
151 template <typename T>
152 inline Quat<T> operator+(const T a, const Quat<T>& q)
153{
154 return Quat<T>(q.w + a, q.x, q.y, q.z);
155}
156
157 template <typename T>
158 inline Quat<T> operator+(const Quat<T>& q, const T a)
159{
160 return Quat<T>(q.w + a, q.x, q.y, q.z);
161}
162
163 template <typename T>
164 inline Quat<T> operator-(const T a, const Quat<T>& q)
165{
166 return Quat<T>(a - q.w, -q.x, -q.y, -q.z);
167}
168
169 template <typename T>
170 inline Quat<T> operator-(const Quat<T>& q, const T a)
171{
172 return Quat<T>(q.w - a, q.x, q.y, q.z);
173}
174
175 template <typename T>
176 inline Quat<T> Quat<T>::operator-(const Quat<T> &q1) const
177 {
178 return Quat<T>(w - q1.w, x - q1.x, y - q1.y, z - q1.z);
179}
180
181 template <typename T>
182 inline Quat<T>& Quat<T>::operator+=(const Quat<T> &q1)
183{
184 w += q1.w;
185 x += q1.x;
186 y += q1.y;
187 z += q1.z;
188 return *this;
189}
190
191 template <typename T>
192 inline Quat<T>& Quat<T>::operator-=(const Quat<T> &q1)
193{
194 w -= q1.w;
195 x -= q1.x;
196 y -= q1.y;
197 z -= q1.z;
198 return *this;
199}
200
201 template <typename T>
202 inline Quat<T> Quat<T>::operator*(const Quat<T> &q1) const
203 {
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);
207}
208
209
210 template <typename T>
211Quat<T> operator*(const Quat<T> &q1, const T a)
212{
213 return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z);
214}
215
216 template <typename T>
217Quat<T> operator*(const T a, const Quat<T> &q1)
218{
219 return Quat<T>(a * q1.w, a * q1.x, a * q1.y, a * q1.z);
220}
221
222 template <typename T>
223 inline Quat<T>& Quat<T>::operator*=(const Quat<T> &q1)
224{
225 T qw, qx, qy, qz;
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;
230 w = qw;
231 x = qx;
232 y = qy;
233 z = qz;
234 return *this;
235}
236
237 template <typename T>
238 inline Quat<T>& Quat<T>::operator/=(const Quat<T> &q1)
239{
240 Quat<T> q(*this * q1.inv());
241 w = q.w;
242 x = q.x;
243 y = q.y;
244 z = q.z;
245 return *this;
246}
247 template <typename T>
248Quat<T>& Quat<T>::operator*=(const T q1)
249{
250 w *= q1;
251 x *= q1;
252 y *= q1;
253 z *= q1;
254 return *this;
255}
256
257 template <typename T>
258 inline Quat<T>& Quat<T>::operator/=(const T a)
259{
260 const T a_inv = 1.0 / a;
261 w *= a_inv;
262 x *= a_inv;
263 y *= a_inv;
264 z *= a_inv;
265 return *this;
266}
267
268 template <typename T>
269 inline Quat<T> Quat<T>::operator/(const T a) const
270 {
271 const T a_inv = 1.0 / a;
272 return Quat<T>(w * a_inv, x * a_inv, y * a_inv, z * a_inv);
273}
274
275 template <typename T>
276 inline Quat<T> Quat<T>::operator/(const Quat<T> &q) const
277 {
278 return *this * q.inv();
279}
280
281 template <typename T>
282 inline const T& Quat<T>::operator[](std::size_t n) const
283 {
284 switch (n) {
285 case 0:
286 return w;
287 case 1:
288 return x;
289 case 2:
290 return y;
291 case 3:
292 return z;
293 default:
294 CV_Error(Error::StsOutOfRange, "subscript exceeds the index range");
295 }
296}
297
298 template <typename T>
299 inline T& Quat<T>::operator[](std::size_t n)
300{
301 switch (n) {
302 case 0:
303 return w;
304 case 1:
305 return x;
306 case 2:
307 return y;
308 case 3:
309 return z;
310 default:
311 CV_Error(Error::StsOutOfRange, "subscript exceeds the index range");
312 }
313}
314
315 template <typename T>
316std::ostream & operator<<(std::ostream &os, const Quat<T> &q)
317{
318 os << "Quat " << Vec<T, 4>{q.w, q.x, q.y, q.z};
319 return os;
320}
321
322 template <typename T>
323 inline T Quat<T>::at(size_t index) const
324 {
325 return (*this)[index];
326}
327
328 template <typename T>
329 inline Quat<T> Quat<T>::conjugate() const
330 {
331 return Quat<T>(w, -x, -y, -z);
332}
333
334 template <typename T>
335 inline T Quat<T>::norm() const
336 {
337 return std::sqrt(dot(*this));
338}
339
340 template <typename T>
341Quat<T> exp(const Quat<T> &q)
342{
343 return q.exp();
344}
345
346 template <typename T>
347Quat<T> Quat<T>::exp() const
348 {
349 Vec<T, 3> v{x, y, z};
350 T normV = std::sqrt(v.dot(v));
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);
353}
354
355 template <typename T>
356Quat<T> log(const Quat<T> &q, QuatAssumeType assumeUnit)
357{
358 return q.log(assumeUnit);
359}
360
361 template <typename T>
362Quat<T> Quat<T>::log(QuatAssumeType assumeUnit) const
363 {
364 Vec<T, 3> v{x, y, z};
365 T vNorm = std::sqrt(v.dot(v));
366 if (assumeUnit)
367 {
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);
370 }
371 T qNorm = norm();
372 if (qNorm < CV_QUAT_EPS)
373 {
374 CV_Error(Error::StsBadArg, "Cannot apply this quaternion to log function: undefined");
375 }
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);
378}
379
380 template <typename T>
381 inline Quat<T> power(const Quat<T> &q1, const T alpha, QuatAssumeType assumeUnit)
382{
383 return q1.power(alpha, assumeUnit);
384}
385
386 template <typename T>
387 inline Quat<T> Quat<T>::power(const T alpha, QuatAssumeType assumeUnit) const
388 {
389 if (x * x + y * y + z * z > CV_QUAT_EPS)
390 {
391 T angle = getAngle(assumeUnit);
392 Vec<T, 3> axis = getAxis(assumeUnit);
393 if (assumeUnit)
394 {
395 return createFromAngleAxis(alpha * angle, axis);
396 }
397 return std::pow(norm(), alpha) * createFromAngleAxis(alpha * angle, axis);
398 }
399 else
400 {
401 return std::pow(norm(), alpha) * Quat<T>(w, x, y, z);
402 }
403}
404
405
406 template <typename T>
407 inline Quat<T> sqrt(const Quat<T> &q, QuatAssumeType assumeUnit)
408{
409 return q.sqrt(assumeUnit);
410}
411
412 template <typename T>
413 inline Quat<T> Quat<T>::sqrt(QuatAssumeType assumeUnit) const
414 {
415 return power(0.5, assumeUnit);
416}
417
418
419 template <typename T>
420 inline Quat<T> power(const Quat<T> &p, const Quat<T> &q, QuatAssumeType assumeUnit)
421{
422 return p.power(q, assumeUnit);
423}
424
425
426 template <typename T>
427 inline Quat<T> Quat<T>::power(const Quat<T> &q, QuatAssumeType assumeUnit) const
428 {
429 return cv::exp(q * log(assumeUnit));
430}
431
432 template <typename T>
433 inline T Quat<T>::dot(Quat<T> q1) const
434 {
435 return w * q1.w + x * q1.x + y * q1.y + z * q1.z;
436}
437
438
439 template <typename T>
440 inline Quat<T> crossProduct(const Quat<T> &p, const Quat<T> &q)
441{
442 return p.crossProduct(q);
443}
444
445
446 template <typename T>
447 inline Quat<T> Quat<T>::crossProduct(const Quat<T> &q) const
448 {
449 return Quat<T> (0, y * q.z - z * q.y, z * q.x - x * q.z, x * q.y - q.x * y);
450}
451
452 template <typename T>
453 inline Quat<T> Quat<T>::normalize() const
454 {
455 T normVal = norm();
456 if (normVal < CV_QUAT_EPS)
457 {
458 CV_Error(Error::StsBadArg, "Cannot normalize this quaternion: the norm is too small.");
459 }
460 return Quat<T>(w / normVal, x / normVal, y / normVal, z / normVal) ;
461}
462
463 template <typename T>
464 inline Quat<T> inv(const Quat<T> &q, QuatAssumeType assumeUnit)
465{
466 return q.inv(assumeUnit);
467}
468
469
470 template <typename T>
471 inline Quat<T> Quat<T>::inv(QuatAssumeType assumeUnit) const
472 {
473 if (assumeUnit)
474 {
475 return conjugate();
476 }
477 T norm2 = dot(*this);
478 if (norm2 < CV_QUAT_EPS)
479 {
480 CV_Error(Error::StsBadArg, "This quaternion do not have inverse quaternion");
481 }
482 return conjugate() / norm2;
483}
484
485 template <typename T>
486 inline Quat<T> sinh(const Quat<T> &q)
487{
488 return q.sinh();
489}
490
491
492 template <typename T>
493 inline Quat<T> Quat<T>::sinh() const
494 {
495 Vec<T, 3> v{x, y ,z};
496 T vNorm = std::sqrt(v.dot(v));
497 T k = vNorm < CV_QUAT_EPS ? 1 : std::cosh(w) * std::sin(vNorm) / vNorm;
498 return Quat<T>(std::sinh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k);
499}
500
501
502 template <typename T>
503 inline Quat<T> cosh(const Quat<T> &q)
504{
505 return q.cosh();
506}
507
508
509 template <typename T>
510 inline Quat<T> Quat<T>::cosh() const
511 {
512 Vec<T, 3> v{x, y ,z};
513 T vNorm = std::sqrt(v.dot(v));
514 T k = vNorm < CV_QUAT_EPS ? 1 : std::sinh(w) * std::sin(vNorm) / vNorm;
515 return Quat<T>(std::cosh(w) * std::cos(vNorm), v[0] * k, v[1] * k, v[2] * k);
516}
517
518 template <typename T>
519 inline Quat<T> tanh(const Quat<T> &q)
520{
521 return q.tanh();
522}
523
524 template <typename T>
525 inline Quat<T> Quat<T>::tanh() const
526 {
527 return sinh() * cosh().inv();
528}
529
530
531 template <typename T>
532 inline Quat<T> sin(const Quat<T> &q)
533{
534 return q.sin();
535}
536
537
538 template <typename T>
539 inline Quat<T> Quat<T>::sin() const
540 {
541 Vec<T, 3> v{x, y ,z};
542 T vNorm = std::sqrt(v.dot(v));
543 T k = vNorm < CV_QUAT_EPS ? 1 : std::cos(w) * std::sinh(vNorm) / vNorm;
544 return Quat<T>(std::sin(w) * std::cosh(vNorm), v[0] * k, v[1] * k, v[2] * k);
545}
546
547 template <typename T>
548 inline Quat<T> cos(const Quat<T> &q)
549{
550 return q.cos();
551}
552
553 template <typename T>
554 inline Quat<T> Quat<T>::cos() const
555 {
556 Vec<T, 3> v{x, y ,z};
557 T vNorm = std::sqrt(v.dot(v));
558 T k = vNorm < CV_QUAT_EPS ? 1 : std::sin(w) * std::sinh(vNorm) / vNorm;
559 return Quat<T>(std::cos(w) * std::cosh(vNorm), -v[0] * k, -v[1] * k, -v[2] * k);
560}
561
562 template <typename T>
563 inline Quat<T> tan(const Quat<T> &q)
564{
565 return q.tan();
566}
567
568 template <typename T>
569 inline Quat<T> Quat<T>::tan() const
570 {
571 return sin() * cos().inv();
572}
573
574 template <typename T>
575 inline Quat<T> asinh(const Quat<T> &q)
576{
577 return q.asinh();
578}
579
580 template <typename T>
581 inline Quat<T> Quat<T>::asinh() const
582 {
583 return cv::log(*this + cv::power(*this * *this + Quat<T>(1, 0, 0, 0), 0.5));
584}
585
586 template <typename T>
587 inline Quat<T> acosh(const Quat<T> &q)
588{
589 return q.acosh();
590}
591
592 template <typename T>
593 inline Quat<T> Quat<T>::acosh() const
594 {
595 return cv::log(*this + cv::power(*this * *this - Quat<T>(1,0,0,0), 0.5));
596}
597
598 template <typename T>
599 inline Quat<T> atanh(const Quat<T> &q)
600{
601 return q.atanh();
602}
603
604 template <typename T>
605 inline Quat<T> Quat<T>::atanh() const
606 {
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);
611}
612
613 template <typename T>
614 inline Quat<T> asin(const Quat<T> &q)
615{
616 return q.asin();
617}
618
619 template <typename T>
620 inline Quat<T> Quat<T>::asin() const
621 {
622 Quat<T> v(0, x, y, z);
623 T vNorm = v.norm();
624 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
625 return -v / k * (*this * v / k).asinh();
626}
627
628 template <typename T>
629 inline Quat<T> acos(const Quat<T> &q)
630{
631 return q.acos();
632}
633
634 template <typename T>
635 inline Quat<T> Quat<T>::acos() const
636 {
637 Quat<T> v(0, x, y, z);
638 T vNorm = v.norm();
639 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
640 return -v / k * acosh();
641}
642
643 template <typename T>
644 inline Quat<T> atan(const Quat<T> &q)
645{
646 return q.atan();
647}
648
649 template <typename T>
650 inline Quat<T> Quat<T>::atan() const
651 {
652 Quat<T> v(0, x, y, z);
653 T vNorm = v.norm();
654 T k = vNorm < CV_QUAT_EPS ? 1 : vNorm;
655 return -v / k * (*this * v / k).atanh();
656}
657
658 template <typename T>
659 inline T Quat<T>::getAngle(QuatAssumeType assumeUnit) const
660 {
661 if (assumeUnit)
662 {
663 return 2 * std::acos(w);
664 }
665 if (norm() < CV_QUAT_EPS)
666 {
667 CV_Error(Error::StsBadArg, "This quaternion does not represent a rotation");
668 }
669 return 2 * std::acos(w / norm());
670}
671
672 template <typename T>
673 inline Vec<T, 3> Quat<T>::getAxis(QuatAssumeType assumeUnit) const
674 {
675 T angle = getAngle(assumeUnit);
676 const T sin_v = std::sin(angle * 0.5);
677 if (assumeUnit)
678 {
679 return Vec<T, 3>{x, y, z} / sin_v;
680 }
681 return Vec<T, 3> {x, y, z} / (norm() * sin_v);
682}
683
684 template <typename T>
685Matx<T, 4, 4> Quat<T>::toRotMat4x4(QuatAssumeType assumeUnit) const
686 {
687 T a = w, b = x, c = y, d = z;
688 if (!assumeUnit)
689 {
690 Quat<T> qTemp = normalize();
691 a = qTemp.w;
692 b = qTemp.x;
693 c = qTemp.y;
694 d = qTemp.z;
695 }
696 Matx<T, 4, 4> R{
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,
700 0 , 0 , 0 , 1,
701 };
702 return R;
703}
704
705 template <typename T>
706Matx<T, 3, 3> Quat<T>::toRotMat3x3(QuatAssumeType assumeUnit) const
707 {
708 T a = w, b = x, c = y, d = z;
709 if (!assumeUnit)
710 {
711 Quat<T> qTemp = normalize();
712 a = qTemp.w;
713 b = qTemp.x;
714 c = qTemp.y;
715 d = qTemp.z;
716 }
717 Matx<T, 3, 3> R{
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)
721 };
722 return R;
723}
724
725 template <typename T>
726Vec<T, 3> Quat<T>::toRotVec(QuatAssumeType assumeUnit) const
727 {
728 T angle = getAngle(assumeUnit);
729 Vec<T, 3> axis = getAxis(assumeUnit);
730 return angle * axis;
731}
732
733 template <typename T>
734Vec<T, 4> Quat<T>::toVec() const
735 {
736 return Vec<T, 4>{w, x, y, z};
737}
738
739 template <typename T>
740Quat<T> Quat<T>::lerp(const Quat<T> &q0, const Quat<T> &q1, const T t)
741{
742 return (1 - t) * q0 + t * q1;
743}
744
745 template <typename T>
746Quat<T> Quat<T>::slerp(const Quat<T> &q0, const Quat<T> &q1, const T t, QuatAssumeType assumeUnit, bool directChange)
747{
748 Quatd v0(q0);
749 Quatd v1(q1);
750 if (!assumeUnit)
751 {
752 v0 = v0.normalize();
753 v1 = v1.normalize();
754 }
755 T cosTheta = v0.dot(v1);
756 constexpr T DOT_THRESHOLD = 0.995;
757 if (cosTheta > DOT_THRESHOLD)
758 {
759 return nlerp(v0, v1, t, QUAT_ASSUME_UNIT);
760 }
761
762 if (directChange && cosTheta < 0)
763 {
764 v0 = -v0;
765 cosTheta = -cosTheta;
766 }
767 T sinTheta = std::sqrt(1 - cosTheta * cosTheta);
768 T angle = atan2(sinTheta, cosTheta);
769 return (std::sin((1 - t) * angle) / (sinTheta) * v0 + std::sin(t * angle) / (sinTheta) * v1).normalize();
770}
771
772
773 template <typename T>
774 inline Quat<T> Quat<T>::nlerp(const Quat<T> &q0, const Quat<T> &q1, const T t, QuatAssumeType assumeUnit)
775{
776 Quat<T> v0(q0), v1(q1);
777 if (v1.dot(v0) < 0)
778 {
779 v0 = -v0;
780 }
781 if (assumeUnit)
782 {
783 return ((1 - t) * v0 + t * v1).normalize();
784 }
785 v0 = v0.normalize();
786 v1 = v1.normalize();
787 return ((1 - t) * v0 + t * v1).normalize();
788}
789
790
791 template <typename T>
792 inline bool Quat<T>::isNormal(T eps) const
793 {
794
795 double normVar = norm();
796 if ((normVar > 1 - eps) && (normVar < 1 + eps))
797 return true;
798 return false;
799}
800
801 template <typename T>
802 inline void Quat<T>::assertNormal(T eps) const
803 {
804 if (!isNormal(eps))
805 CV_Error(Error::StsBadArg, "Quaternion should be normalized");
806}
807
808
809 template <typename T>
810 inline 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,
813 bool directChange)
814{
815 Quat<T> v0(q0), v1(q1), v2(q2), v3(q3);
816 if (!assumeUnit)
817 {
818 v0 = v0.normalize();
819 v1 = v1.normalize();
820 v2 = v2.normalize();
821 v3 = v3.normalize();
822 }
823
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);
827}
828
829 template <typename T>
830Quat<T> Quat<T>::interPoint(const Quat<T> &q0, const Quat<T> &q1,
831 const Quat<T> &q2, QuatAssumeType assumeUnit)
832{
833 Quat<T> v0(q0), v1(q1), v2(q2);
834 if (!assumeUnit)
835 {
836 v0 = v0.normalize();
837 v1 = v1.normalize();
838 v2 = v2.normalize();
839 }
840 return v1 * cv::exp(-(cv::log(v1.conjugate() * v0, assumeUnit) + (cv::log(v1.conjugate() * v2, assumeUnit))) / 4);
841}
842
843 template <typename T>
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)
845{
846 Quatd v0(q0), v1(q1), v2(q2), v3(q3);
847 if (!assumeUnit)
848 {
849 v0 = v0.normalize();
850 v1 = v1.normalize();
851 v2 = v2.normalize();
852 v3 = v3.normalize();
853 }
854 T cosTheta;
855 std::vector<Quat<T>> vec{v0, v1, v2, v3};
856 for (size_t i = 0; i < 3; ++i)
857 {
858 cosTheta = vec[i].dot(vec[i + 1]);
859 if (cosTheta < 0)
860 {
861 vec[i + 1] = -vec[i + 1];
862 }
863 }
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);
867}
868
869 namespace detail {
870
871 template <typename T> static
872Quat<T> createFromAxisRot(int axis, const T theta)
873{
874 if (axis == 0)
875 return Quat<T>::createFromXRot(theta);
876 if (axis == 1)
877 return Quat<T>::createFromYRot(theta);
878 if (axis == 2)
879 return Quat<T>::createFromZRot(theta);
880 CV_Assert(0);
881}
882
883 inline bool isIntAngleType(QuatEnum::EulerAnglesType eulerAnglesType)
884{
885 return eulerAnglesType < QuatEnum::EXT_XYZ;
886}
887
888 inline bool isTaitBryan(QuatEnum::EulerAnglesType eulerAnglesType)
889{
890 return eulerAnglesType/6 == 1 || eulerAnglesType/6 == 3;
891}
892} // namespace detail
893
894 template <typename T>
895Quat<T> Quat<T>::createFromYRot(const T theta)
896{
897 return Quat<T>{std::cos(theta * 0.5f), 0, std::sin(theta * 0.5f), 0};
898}
899
900 template <typename T>
901Quat<T> Quat<T>::createFromXRot(const T theta){
902 return Quat<T>{std::cos(theta * 0.5f), std::sin(theta * 0.5f), 0, 0};
903}
904
905 template <typename T>
906Quat<T> Quat<T>::createFromZRot(const T theta){
907 return Quat<T>{std::cos(theta * 0.5f), 0, 0, std::sin(theta * 0.5f)};
908}
909
910
911 template <typename T>
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] = {
915 {0, 1, 2},
916 {0, 2, 1},
917 {1, 0, 2},
918 {1, 2, 0},
919 {2, 0, 1},
920 {2, 1, 0},
921 {0, 1, 0},
922 {0, 2, 0},
923 {1, 0, 1},
924 {1, 2, 1},
925 {2, 0, 2},
926 {2, 1, 2},
927 {0, 1, 2},
928 {0, 2, 1},
929 {1, 0, 2},
930 {1, 2, 0},
931 {2, 0, 1},
932 {2, 1, 0},
933 {0, 1, 0},
934 {0, 2, 0},
935 {1, 0, 1},
936 {1, 2, 1},
937 {2, 0, 2},
938 {2, 1, 2}
939 };
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))
944 {
945 return q1 * q2 * q3;
946 }
947 else // (!detail::isIntAngleType<T>(eulerAnglesType))
948 {
949 return q3 * q2 * q1;
950 }
951}
952
953 template <typename T>
954Vec<T, 3> Quat<T>::toEulerAngles(QuatEnum::EulerAnglesType eulerAnglesType){
955 CV_Assert(eulerAnglesType < QuatEnum::EulerAnglesType::EULER_ANGLES_MAX_VALUE);
956 Matx33d R = toRotMat3x3();
957 enum {
958 C_ZERO,
959 C_PI,
960 C_PI_2,
961 N_CONSTANTS,
962 R_0_0 = N_CONSTANTS, R_0_1, R_0_2,
963 R_1_0, R_1_1, R_1_2,
964 R_2_0, R_2_1, R_2_2
965 };
966 static const T constants_[N_CONSTANTS] = {
967 0, // C_ZERO
968 (T)CV_PI, // C_PI
969 (T)(CV_PI * 0.5) // C_PI_2, -C_PI_2
970 };
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}, // INT_XYZ
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}, // INT_XZY
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}, // INT_YXZ
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}, // INT_YZX
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}, // INT_ZXY
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}, // INT_ZYX
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}, // INT_XYX
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}, // INT_XZX
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}, // INT_YXY
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}, // INT_YZY
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}, // INT_ZXZ
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}, // INT_ZYZ
984
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}, // EXT_XYZ
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}, // EXT_XZY
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}, // EXT_YXZ
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}, // EXT_YZX
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}, // EXT_ZXY
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}, // EXT_ZYX
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}, // EXT_XYX
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}, // EXT_XZX
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}, // EXT_YXY
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}, // EXT_YZY
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}, // EXT_ZXZ
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}, // EXT_ZYZ
997 };
998 T rotationR[12];
999 for (int i = 0; i < 12; i++)
1000 {
1001 int id = rotationR_[eulerAnglesType][i];
1002 unsigned idx = std::abs(id);
1003 T value = 0.0f;
1004 if (idx < N_CONSTANTS)
1005 {
1006 value = constants_[idx];
1007 }
1008 else
1009 {
1010 unsigned r_idx = idx - N_CONSTANTS;
1011 CV_DbgAssert(r_idx < 9);
1012 value = R.val[r_idx];
1013 }
1014 bool isNegative = id < 0;
1015 if (isNegative)
1016 value = -value;
1017 rotationR[i] = value;
1018 }
1019 Vec<T, 3> angles;
1020 if (detail::isIntAngleType(eulerAnglesType))
1021 {
1022 if (abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
1023 {
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};
1026 return angles;
1027 }
1028 else if(abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
1029 {
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};
1032 return angles;
1033 }
1034 }
1035 else // (!detail::isIntAngleType<T>(eulerAnglesType))
1036 {
1037 if (abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
1038 {
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])};
1041 return angles;
1042 }
1043 else if (abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
1044 {
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])};
1047 return angles;
1048 }
1049 }
1050
1051 angles(0) = std::atan2(rotationR[7], rotationR[8]);
1052 if (detail::isTaitBryan(eulerAnglesType))
1053 angles(1) = std::acos(rotationR[9]);
1054 else
1055 angles(1) = std::asin(rotationR[9]);
1056 angles(2) = std::atan2(rotationR[10], rotationR[11]);
1057 return angles;
1058}
1059
1060} // namepsace
1062
1063 #endif /*OPENCV_CORE_QUATERNION_INL_HPP*/
cv::FileStorage::operator<<
CV_EXPORTS FileStorage & operator<<(FileStorage &fs, const String &str)
Writes string to a file storage.
cv::MatExpr::abs
CV_EXPORTS MatExpr abs(const Mat &m)
Calculates an absolute value of each matrix element.
cv::sqrt
CV_EXPORTS_W void sqrt(InputArray src, OutputArray dst)
Calculates a square root of array elements.
cv::exp
CV_EXPORTS_W void exp(InputArray src, OutputArray dst)
Calculates the exponent of every array element.
cv::norm
CV_EXPORTS_W double norm(InputArray src1, int normType=NORM_L2, InputArray mask=noArray())
Calculates the absolute norm of an array.
cv::pow
CV_EXPORTS_W void pow(InputArray src, double power, OutputArray dst)
Raises every array element to a power.
cv::trace
CV_EXPORTS_W Scalar trace(InputArray mtx)
Returns the trace of a matrix.
cv::log
CV_EXPORTS_W void log(InputArray src, OutputArray dst)
Calculates the natural logarithm of every array element.
cv::normalize
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::operator*
CV_INLINE v_reg< _Tp, n > operator*(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Multiply values
cv::operator+
CV_INLINE v_reg< _Tp, n > operator+(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Add values
cv::operator/
CV_INLINE v_reg< _Tp, n > operator/(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Divide values
cv::operator-
CV_INLINE v_reg< _Tp, n > operator-(const v_reg< _Tp, n > &a, const v_reg< _Tp, n > &b)
Subtract values
cv::abs
softfloat abs(softfloat a)
Absolute value
Definition: softfloat.hpp:444
CV_Error
#define CV_Error(code, msg)
Call the error handler.
Definition: base.hpp:320
CV_Assert
#define CV_Assert(expr)
Checks a condition at runtime and throws exception if it fails
Definition: base.hpp:342
CV_DbgAssert
#define CV_DbgAssert(expr)
Definition: base.hpp:375
cv::tan
Quat< T > tan(const Quat< T > &q)
cv::asinh
Quat< T > asinh(const Quat< T > &q)
cv::asin
Quat< T > asin(const Quat< T > &q)
cv::tanh
Quat< T > tanh(const Quat< T > &q)
cv::crossProduct
Quat< T > crossProduct(const Quat< T > &p, const Quat< T > &q)
cv::inv
Quat< T > inv(const Quat< T > &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
cv::acosh
Quat< T > acosh(const Quat< T > &q)
cv::atanh
Quat< T > atanh(const Quat< T > &q)
cv::cos
Quat< T > cos(const Quat< T > &q)
cv::sinh
Quat< T > sinh(const Quat< T > &q)
cv::QuatAssumeType
QuatAssumeType
Unit quaternion flag
Definition: quaternion.hpp:39
cv::cosh
Quat< T > cosh(const Quat< T > &q)
cv::atan
Quat< T > atan(const Quat< T > &q)
cv::sin
Quat< T > sin(const Quat< T > &q)
cv::acos
Quat< T > acos(const Quat< T > &q)
cv::QUAT_ASSUME_NOT_UNIT
@ QUAT_ASSUME_NOT_UNIT
Definition: quaternion.hpp:46
cv::QUAT_ASSUME_UNIT
@ QUAT_ASSUME_UNIT
Definition: quaternion.hpp:52
cv
"black box" representation of the file storage associated with a file on disk.
Definition: aruco.hpp:75
cv::conjugate
DualQuat< T > conjugate(const DualQuat< T > &dq)
Definition: dualquaternion.inl.hpp:125
cv::power
DualQuat< T > power(const DualQuat< T > &dq, const T t, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
Definition: dualquaternion.inl.hpp:358
cv::traits::Type
Definition: traits.hpp:386
構築: doxygen 1.9.2