OpenCV 4.5.3(日本語機械翻訳)
affine.hpp
1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
16 // Third party copyrights are property of their respective owners.
17 //
18 // Redistribution and use in source and binary forms, with or without modification,
19 // are permitted provided that the following conditions are met:
20 //
21 // * Redistribution's of source code must retain the above copyright notice,
22 // this list of conditions and the following disclaimer.
23 //
24 // * Redistribution's in binary form must reproduce the above copyright notice,
25 // this list of conditions and the following disclaimer in the documentation
26 // and/or other materials provided with the distribution.
27 //
28 // * The name of the copyright holders may not be used to endorse or promote products
29 // derived from this software without specific prior written permission.
30 //
31 // This software is provided by the copyright holders and contributors "as is" and
32 // any express or implied warranties, including, but not limited to, the implied
33 // warranties of merchantability and fitness for a particular purpose are disclaimed.
34 // In no event shall the Intel Corporation or contributors be liable for any direct,
35 // indirect, incidental, special, exemplary, or consequential damages
36 // (including, but not limited to, procurement of substitute goods or services;
37 // loss of use, data, or profits; or business interruption) however caused
38 // and on any theory of liability, whether in contract, strict liability,
39 // or tort (including negligence or otherwise) arising in any way out of
40 // the use of this software, even if advised of the possibility of such damage.
41 //
42 //M*/
43
44 #ifndef OPENCV_CORE_AFFINE3_HPP
45 #define OPENCV_CORE_AFFINE3_HPP
46
47 #ifdef __cplusplus
48
49 #include <opencv2/core.hpp>
50
51 namespace cv
52{
53
56
125 template<typename T>
126 class Affine3
127 {
128 public:
129 typedef T float_type;
130 typedef Matx<float_type, 3, 3> Mat3;
131 typedef Matx<float_type, 4, 4> Mat4;
132 typedef Vec<float_type, 3> Vec3;
133
135 Affine3();
136
138 Affine3(const Mat4& affine);
139
153 Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
154
164 Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
165
179 explicit Affine3(const Mat& data, const Vec3& t = Vec3::all(0));
180
182 explicit Affine3(const float_type* vals);
183
185 static Affine3 Identity();
186
196 void rotation(const Mat3& R);
197
206 void rotation(const Vec3& rvec);
207
218 void rotation(const Mat& data);
219
227 void linear(const Mat3& L);
228
236 void translation(const Vec3& t);
237
239 Mat3 rotation() const;
240
242 Mat3 linear() const;
243
245 Vec3 translation() const;
246
252 Vec3 rvec() const;
253
255 Affine3 inv(int method = cv::DECOMP_SVD) const;
256
258 Affine3 rotate(const Mat3& R) const;
259
261 Affine3 rotate(const Vec3& rvec) const;
262
264 Affine3 translate(const Vec3& t) const;
265
267 Affine3 concatenate(const Affine3& affine) const;
268
269 template <typename Y> operator Affine3<Y>() const;
270
271 template <typename Y> Affine3<Y> cast() const;
272
273 Mat4 matrix;
274
275 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
276 Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
277 Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
278 operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
279 operator Eigen::Transform<T, 3, Eigen::Affine>() const;
280 #endif
281 };
282
283 template<typename T> static
284 Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
285
287 template<typename T, typename V> static
288 V operator*(const Affine3<T>& affine, const V& vector);
289
290 typedef Affine3<float> Affine3f;
291 typedef Affine3<double> Affine3d;
292
293 static Vec3f operator*(const Affine3f& affine, const Vec3f& vector);
294 static Vec3d operator*(const Affine3d& affine, const Vec3d& vector);
295
296 template<typename _Tp> class DataType< Affine3<_Tp> >
297 {
298 public:
299 typedef Affine3<_Tp> value_type;
300 typedef Affine3<typename DataType<_Tp>::work_type> work_type;
301 typedef _Tp channel_type;
302
303 enum { generic_type = 0,
304 channels = 16,
305 fmt = traits::SafeFmt<channel_type>::fmt + ((channels - 1) << 8)
306 #ifdef OPENCV_TRAITS_ENABLE_DEPRECATED
307 ,depth = DataType<channel_type>::depth
308 ,type = CV_MAKETYPE(depth, channels)
309 #endif
310 };
311
312 typedef Vec<channel_type, channels> vec_type;
313 };
314
315 namespace traits {
316 template<typename _Tp>
317 struct Depth< Affine3<_Tp> > { enum { value = Depth<_Tp>::value }; };
318 template<typename _Tp>
319 struct Type< Affine3<_Tp> > { enum { value = CV_MAKETYPE(Depth<_Tp>::value, 16) }; };
320 } // namespace
321
323
324}
325
327
329 // Implementation
330
331 template<typename T> inline
332cv::Affine3<T>::Affine3()
333 : matrix(Mat4::eye())
334{}
335
336 template<typename T> inline
337cv::Affine3<T>::Affine3(const Mat4& affine)
338 : matrix(affine)
339{}
340
341 template<typename T> inline
342cv::Affine3<T>::Affine3(const Mat3& R, const Vec3& t)
343{
344 rotation(R);
345 translation(t);
346 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
347 matrix.val[15] = 1;
348}
349
350 template<typename T> inline
351cv::Affine3<T>::Affine3(const Vec3& _rvec, const Vec3& t)
352{
353 rotation(_rvec);
354 translation(t);
355 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
356 matrix.val[15] = 1;
357}
358
359 template<typename T> inline
360cv::Affine3<T>::Affine3(const cv::Mat& data, const Vec3& t)
361{
362 CV_Assert(data.type() == cv::traits::Type<T>::value);
363 CV_Assert(data.channels() == 1);
364
365 if (data.cols == 4 && data.rows == 4)
366 {
367 data.copyTo(matrix);
368 return;
369 }
370 else if (data.cols == 4 && data.rows == 3)
371 {
372 rotation(data(Rect(0, 0, 3, 3)));
373 translation(data(Rect(3, 0, 1, 3)));
374 }
375 else
376 {
377 rotation(data);
378 translation(t);
379 }
380
381 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
382 matrix.val[15] = 1;
383}
384
385 template<typename T> inline
386cv::Affine3<T>::Affine3(const float_type* vals) : matrix(vals)
387{}
388
389 template<typename T> inline
390cv::Affine3<T> cv::Affine3<T>::Identity()
391{
392 return Affine3<T>(cv::Affine3<T>::Mat4::eye());
393}
394
395 template<typename T> inline
396 void cv::Affine3<T>::rotation(const Mat3& R)
397{
398 linear(R);
399}
400
401 template<typename T> inline
402 void cv::Affine3<T>::rotation(const Vec3& _rvec)
403{
404 double theta = norm(_rvec);
405
406 if (theta < DBL_EPSILON)
407 rotation(Mat3::eye());
408 else
409 {
410 double c = std::cos(theta);
411 double s = std::sin(theta);
412 double c1 = 1. - c;
413 double itheta = (theta != 0) ? 1./theta : 0.;
414
415 Point3_<T> r = _rvec*itheta;
416
417 Mat3 rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );
418 Mat3 r_x( 0, -r.z, r.y, r.z, 0, -r.x, -r.y, r.x, 0 );
419
420 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
421 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
422 Mat3 R = c*Mat3::eye() + c1*rrt + s*r_x;
423
424 rotation(R);
425 }
426}
427
428 //Combines rotation methods above. Supports 3x3, 1x3, 3x1 sizes of data matrix;
429 template<typename T> inline
430 void cv::Affine3<T>::rotation(const cv::Mat& data)
431{
432 CV_Assert(data.type() == cv::traits::Type<T>::value);
433 CV_Assert(data.channels() == 1);
434
435 if (data.cols == 3 && data.rows == 3)
436 {
437 Mat3 R;
438 data.copyTo(R);
439 rotation(R);
440 }
441 else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
442 {
443 Vec3 _rvec;
444 data.reshape(1, 3).copyTo(_rvec);
445 rotation(_rvec);
446 }
447 else
448 CV_Error(Error::StsError, "Input matrix can only be 3x3, 1x3 or 3x1");
449}
450
451 template<typename T> inline
452 void cv::Affine3<T>::linear(const Mat3& L)
453{
454 matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2];
455 matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5];
456 matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8];
457}
458
459 template<typename T> inline
460 void cv::Affine3<T>::translation(const Vec3& t)
461{
462 matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
463}
464
465 template<typename T> inline
466 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::rotation() const
467 {
468 return linear();
469}
470
471 template<typename T> inline
472 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::linear() const
473 {
474 typename cv::Affine3<T>::Mat3 R;
475 R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2];
476 R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6];
477 R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10];
478 return R;
479}
480
481 template<typename T> inline
482 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::translation() const
483 {
484 return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
485}
486
487 template<typename T> inline
488 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::rvec() const
489 {
490 cv::Vec3d w;
491 cv::Matx33d u, vt, R = rotation();
492 cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
493 R = u * vt;
494
495 double rx = R.val[7] - R.val[5];
496 double ry = R.val[2] - R.val[6];
497 double rz = R.val[3] - R.val[1];
498
499 double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25);
500 double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5;
501 c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c;
502 double theta = std::acos(c);
503
504 if( s < 1e-5 )
505 {
506 if( c > 0 )
507 rx = ry = rz = 0;
508 else
509 {
510 double t;
511 t = (R.val[0] + 1) * 0.5;
512 rx = std::sqrt(std::max(t, 0.0));
513 t = (R.val[4] + 1) * 0.5;
514 ry = std::sqrt(std::max(t, 0.0)) * (R.val[1] < 0 ? -1.0 : 1.0);
515 t = (R.val[8] + 1) * 0.5;
516 rz = std::sqrt(std::max(t, 0.0)) * (R.val[2] < 0 ? -1.0 : 1.0);
517
518 if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R.val[5] > 0) != (ry*rz > 0) )
519 rz = -rz;
520 theta /= std::sqrt(rx*rx + ry*ry + rz*rz);
521 rx *= theta;
522 ry *= theta;
523 rz *= theta;
524 }
525 }
526 else
527 {
528 double vth = 1/(2*s);
529 vth *= theta;
530 rx *= vth; ry *= vth; rz *= vth;
531 }
532
533 return cv::Vec3d(rx, ry, rz);
534}
535
536 template<typename T> inline
537cv::Affine3<T> cv::Affine3<T>::inv(int method) const
538 {
539 return matrix.inv(method);
540}
541
542 template<typename T> inline
543cv::Affine3<T> cv::Affine3<T>::rotate(const Mat3& R) const
544 {
545 Mat3 Lc = linear();
546 Vec3 tc = translation();
547 Mat4 result;
548 result.val[12] = result.val[13] = result.val[14] = 0;
549 result.val[15] = 1;
550
551 for(int j = 0; j < 3; ++j)
552 {
553 for(int i = 0; i < 3; ++i)
554 {
555 float_type value = 0;
556 for(int k = 0; k < 3; ++k)
557 value += R(j, k) * Lc(k, i);
558 result(j, i) = value;
559 }
560
561 result(j, 3) = R.row(j).dot(tc.t());
562 }
563 return result;
564}
565
566 template<typename T> inline
567cv::Affine3<T> cv::Affine3<T>::rotate(const Vec3& _rvec) const
568 {
569 return rotate(Affine3f(_rvec).rotation());
570}
571
572 template<typename T> inline
573cv::Affine3<T> cv::Affine3<T>::translate(const Vec3& t) const
574 {
575 Mat4 m = matrix;
576 m.val[ 3] += t[0];
577 m.val[ 7] += t[1];
578 m.val[11] += t[2];
579 return m;
580}
581
582 template<typename T> inline
583cv::Affine3<T> cv::Affine3<T>::concatenate(const Affine3<T>& affine) const
584 {
585 return (*this).rotate(affine.rotation()).translate(affine.translation());
586}
587
588 template<typename T> template <typename Y> inline
589cv::Affine3<T>::operator Affine3<Y>() const
590 {
591 return Affine3<Y>(matrix);
592}
593
594 template<typename T> template <typename Y> inline
595cv::Affine3<Y> cv::Affine3<T>::cast() const
596 {
597 return Affine3<Y>(matrix);
598}
599
600 template<typename T> inline
601cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
602{
603 return affine2.concatenate(affine1);
604}
605
606 template<typename T, typename V> inline
607V cv::operator*(const cv::Affine3<T>& affine, const V& v)
608{
609 const typename Affine3<T>::Mat4& m = affine.matrix;
610
611 V r;
612 r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
613 r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
614 r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
615 return r;
616}
617
618 static inline
619 cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
620{
621 const cv::Matx44f& m = affine.matrix;
622 cv::Vec3f r;
623 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
624 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
625 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
626 return r;
627}
628
629 static inline
630 cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
631{
632 const cv::Matx44d& m = affine.matrix;
633 cv::Vec3d r;
634 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
635 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
636 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
637 return r;
638}
639
640
641
642 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
643
644 template<typename T> inline
645cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
646{
647 cv::Mat(4, 4, cv::traits::Type<T>::value, affine.matrix().data()).copyTo(matrix);
648}
649
650 template<typename T> inline
651cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
652{
653 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
654 cv::Mat(4, 4, cv::traits::Type<T>::value, a.matrix().data()).copyTo(matrix);
655}
656
657 template<typename T> inline
658cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
659 {
660 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
661 cv::Mat hdr(4, 4, cv::traits::Type<T>::value, r.matrix().data());
662 cv::Mat(matrix, false).copyTo(hdr);
663 return r;
664}
665
666 template<typename T> inline
667cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
668 {
669 return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
670}
671
672 #endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
673
675
676 #endif /* __cplusplus */
677
678 #endif /* OPENCV_CORE_AFFINE3_HPP */
cv::Mat
n-dimensional dense array class
Definition: mat.hpp:802
cv::Mat::copyTo
void copyTo(OutputArray m) const
Copies the matrix to another one.
cv::Matx
Template class for small matrices whose type and size are known at compilation time
Definition: matx.hpp:100
cv::SVD::FULL_UV
@ FULL_UV
Definition: core.hpp:2656
cv::SVD::MODIFY_A
@ MODIFY_A
Definition: core.hpp:2649
cv::SVD::compute
static void compute(InputArray src, OutputArray w, OutputArray u, OutputArray vt, int flags=0)
decomposes matrix and stores the results to user-provided matrices
cv::Vec
Template class for short numerical vectors, a partial case of Matx
Definition: matx.hpp:342
cv::max
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::sqrt
CV_EXPORTS_W void sqrt(InputArray src, OutputArray dst)
Calculates a square root of array elements.
cv::norm
CV_EXPORTS_W double norm(InputArray src1, int normType=NORM_L2, InputArray mask=noArray())
Calculates the absolute norm of an array.
cv::rotate
CV_EXPORTS_W void rotate(InputArray src, OutputArray dst, int rotateCode)
Rotates a 2D array in multiples of 90 degrees. The function cv::rotate rotates the array in one of th...
cv::DECOMP_SVD
@ DECOMP_SVD
Definition: base.hpp:138
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::inv
Quat< T > inv(const Quat< T > &q, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
cv::cos
Quat< T > cos(const Quat< T > &q)
cv::sin
Quat< T > sin(const Quat< T > &q)
cv::acos
Quat< T > acos(const Quat< T > &q)
cv
"black box" representation of the file storage associated with a file on disk.
Definition: aruco.hpp:75
cv::inv
DualQuat< T > inv(const DualQuat< T > &dq, QuatAssumeType assumeUnit=QUAT_ASSUME_NOT_UNIT)
Definition: dualquaternion.inl.hpp:187
cv::traits::Type
Definition: traits.hpp:386
構築: doxygen 1.9.2