affine.hpp
Go to the documentation of this file.
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/core.hpp>
50 
51 namespace cv
52 {
53  template<typename T>
54  class Affine3
55  {
56  public:
57  typedef T float_type;
61 
62  Affine3();
63 
64  //Augmented affine matrix
65  Affine3(const Mat4& affine);
66 
67  //Rotation matrix
68  Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
69 
70  //Rodrigues vector
71  Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
72 
73  //Combines all contructors above. Supports 4x4, 4x3, 3x3, 1x3, 3x1 sizes of data matrix
74  explicit Affine3(const Mat& data, const Vec3& t = Vec3::all(0));
75 
76  //From 16th element array
77  explicit Affine3(const float_type* vals);
78 
79  static Affine3 Identity();
80 
81  //Rotation matrix
82  void rotation(const Mat3& R);
83 
84  //Rodrigues vector
85  void rotation(const Vec3& rvec);
86 
87  //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
88  void rotation(const Mat& data);
89 
90  void linear(const Mat3& L);
91  void translation(const Vec3& t);
92 
93  Mat3 rotation() const;
94  Mat3 linear() const;
95  Vec3 translation() const;
96 
97  //Rodrigues vector
98  Vec3 rvec() const;
99 
100  Affine3 inv(int method = cv::DECOMP_SVD) const;
101 
102  // a.rotate(R) is equivalent to Affine(R, 0) * a;
103  Affine3 rotate(const Mat3& R) const;
104 
105  // a.rotate(R) is equivalent to Affine(rvec, 0) * a;
106  Affine3 rotate(const Vec3& rvec) const;
107 
108  // a.translate(t) is equivalent to Affine(E, t) * a;
109  Affine3 translate(const Vec3& t) const;
110 
111  // a.concatenate(affine) is equivalent to affine * a;
112  Affine3 concatenate(const Affine3& affine) const;
113 
114  template <typename Y> operator Affine3<Y>() const;
115 
116  template <typename Y> Affine3<Y> cast() const;
117 
119 
120 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
121  Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
122  Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
123  operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
124  operator Eigen::Transform<T, 3, Eigen::Affine>() const;
125 #endif
126  };
127 
128  template<typename T> static
129  Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
130 
131  template<typename T, typename V> static
132  V operator*(const Affine3<T>& affine, const V& vector);
133 
136 
137  static Vec3f operator*(const Affine3f& affine, const Vec3f& vector);
138  static Vec3d operator*(const Affine3d& affine, const Vec3d& vector);
139 
140  template<typename _Tp> class DataType< Affine3<_Tp> >
141  {
142  public:
145  typedef _Tp channel_type;
146 
147  enum { generic_type = 0,
149  channels = 16,
151  type = CV_MAKETYPE(depth, channels)
152  };
153 
155  };
156 }
157 
158 
161 
162 template<typename T> inline
164  : matrix(Mat4::eye())
165 {}
166 
167 template<typename T> inline
169  : matrix(affine)
170 {}
171 
172 template<typename T> inline
174 {
175  rotation(R);
176  translation(t);
177  matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
178  matrix.val[15] = 1;
179 }
180 
181 template<typename T> inline
182 cv::Affine3<T>::Affine3(const Vec3& _rvec, const Vec3& t)
183 {
184  rotation(_rvec);
185  translation(t);
186  matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
187  matrix.val[15] = 1;
188 }
189 
190 template<typename T> inline
192 {
193  CV_Assert(data.type() == cv::DataType<T>::type);
194 
195  if (data.cols == 4 && data.rows == 4)
196  {
197  data.copyTo(matrix);
198  return;
199  }
200  else if (data.cols == 4 && data.rows == 3)
201  {
202  rotation(data(Rect(0, 0, 3, 3)));
203  translation(data(Rect(3, 0, 1, 3)));
204  return;
205  }
206 
207  rotation(data);
208  translation(t);
209  matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
210  matrix.val[15] = 1;
211 }
212 
213 template<typename T> inline
215 {}
216 
217 template<typename T> inline
219 {
221 }
222 
223 template<typename T> inline
225 {
226  linear(R);
227 }
228 
229 template<typename T> inline
230 void cv::Affine3<T>::rotation(const Vec3& _rvec)
231 {
232  double rx = _rvec[0], ry = _rvec[1], rz = _rvec[2];
233  double theta = std::sqrt(rx*rx + ry*ry + rz*rz);
234 
235  if (theta < DBL_EPSILON)
236  rotation(Mat3::eye());
237  else
238  {
239  const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
240 
241  double c = std::cos(theta);
242  double s = std::sin(theta);
243  double c1 = 1. - c;
244  double itheta = theta ? 1./theta : 0.;
245 
246  rx *= itheta; ry *= itheta; rz *= itheta;
247 
248  double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
249  double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
250  Mat3 R;
251 
252  // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
253  // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
254  for(int k = 0; k < 9; ++k)
255  R.val[k] = static_cast<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
256 
257  rotation(R);
258  }
259 }
260 
261 //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
262 template<typename T> inline
264 {
265  CV_Assert(data.type() == cv::DataType<T>::type);
266 
267  if (data.cols == 3 && data.rows == 3)
268  {
269  Mat3 R;
270  data.copyTo(R);
271  rotation(R);
272  }
273  else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
274  {
275  Vec3 _rvec;
276  data.reshape(1, 3).copyTo(_rvec);
277  rotation(_rvec);
278  }
279  else
280  CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1");
281 }
282 
283 template<typename T> inline
285 {
286  matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2];
287  matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5];
288  matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8];
289 }
290 
291 template<typename T> inline
293 {
294  matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
295 }
296 
297 template<typename T> inline
299 {
300  return linear();
301 }
302 
303 template<typename T> inline
305 {
306  typename cv::Affine3<T>::Mat3 R;
307  R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2];
308  R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6];
309  R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10];
310  return R;
311 }
312 
313 template<typename T> inline
315 {
316  return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
317 }
318 
319 template<typename T> inline
321 {
322  cv::Vec3d w;
323  cv::Matx33d u, vt, R = rotation();
325  R = u * vt;
326 
327  double rx = R.val[7] - R.val[5];
328  double ry = R.val[2] - R.val[6];
329  double rz = R.val[3] - R.val[1];
330 
331  double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25);
332  double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5;
333  c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c;
334  double theta = acos(c);
335 
336  if( s < 1e-5 )
337  {
338  if( c > 0 )
339  rx = ry = rz = 0;
340  else
341  {
342  double t;
343  t = (R.val[0] + 1) * 0.5;
344  rx = std::sqrt(std::max(t, 0.0));
345  t = (R.val[4] + 1) * 0.5;
346  ry = std::sqrt(std::max(t, 0.0)) * (R.val[1] < 0 ? -1.0 : 1.0);
347  t = (R.val[8] + 1) * 0.5;
348  rz = std::sqrt(std::max(t, 0.0)) * (R.val[2] < 0 ? -1.0 : 1.0);
349 
350  if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R.val[5] > 0) != (ry*rz > 0) )
351  rz = -rz;
352  theta /= std::sqrt(rx*rx + ry*ry + rz*rz);
353  rx *= theta;
354  ry *= theta;
355  rz *= theta;
356  }
357  }
358  else
359  {
360  double vth = 1/(2*s);
361  vth *= theta;
362  rx *= vth; ry *= vth; rz *= vth;
363  }
364 
365  return cv::Vec3d(rx, ry, rz);
366 }
367 
368 template<typename T> inline
370 {
371  return matrix.inv(method);
372 }
373 
374 template<typename T> inline
376 {
377  Mat3 Lc = linear();
378  Vec3 tc = translation();
379  Mat4 result;
380  result.val[12] = result.val[13] = result.val[14] = 0;
381  result.val[15] = 1;
382 
383  for(int j = 0; j < 3; ++j)
384  {
385  for(int i = 0; i < 3; ++i)
386  {
387  float_type value = 0;
388  for(int k = 0; k < 3; ++k)
389  value += R(j, k) * Lc(k, i);
390  result(j, i) = value;
391  }
392 
393  result(j, 3) = R.row(j).dot(tc.t());
394  }
395  return result;
396 }
397 
398 template<typename T> inline
400 {
401  return rotate(Affine3f(_rvec).rotation());
402 }
403 
404 template<typename T> inline
406 {
407  Mat4 m = matrix;
408  m.val[ 3] += t[0];
409  m.val[ 7] += t[1];
410  m.val[11] += t[2];
411  return m;
412 }
413 
414 template<typename T> inline
416 {
417  return (*this).rotate(affine.rotation()).translate(affine.translation());
418 }
419 
420 template<typename T> template <typename Y> inline
422 {
423  return Affine3<Y>(matrix);
424 }
425 
426 template<typename T> template <typename Y> inline
428 {
429  return Affine3<Y>(matrix);
430 }
431 
432 template<typename T> inline
433 cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
434 {
435  return affine2.concatenate(affine1);
436 }
437 
438 template<typename T, typename V> inline
439 V cv::operator*(const cv::Affine3<T>& affine, const V& v)
440 {
441  const typename Affine3<T>::Mat4& m = affine.matrix;
442 
443  V r;
444  r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
445  r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
446  r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
447  return r;
448 }
449 
450 static inline
451 cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
452 {
453  const cv::Matx44f& m = affine.matrix;
454  cv::Vec3f r;
455  r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
456  r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
457  r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
458  return r;
459 }
460 
461 static inline
462 cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
463 {
464  const cv::Matx44d& m = affine.matrix;
465  cv::Vec3d r;
466  r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
467  r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
468  r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
469  return r;
470 }
471 
472 
473 
474 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
475 
476 template<typename T> inline
477 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
478 {
479  cv::Mat(4, 4, cv::DataType<T>::type, affine.matrix().data()).copyTo(matrix);
480 }
481 
482 template<typename T> inline
483 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
484 {
485  Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
486  cv::Mat(4, 4, cv::DataType<T>::type, a.matrix().data()).copyTo(matrix);
487 }
488 
489 template<typename T> inline
490 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
491 {
492  Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
493  cv::Mat hdr(4, 4, cv::DataType<T>::type, r.matrix().data());
494  cv::Mat(matrix, false).copyTo(hdr);
495  return r;
496 }
497 
498 template<typename T> inline
499 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
500 {
501  return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
502 }
503 
504 #endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
505 
506 
507 #endif /* __cplusplus */
508 
509 #endif /* __OPENCV_CORE_AFFINE3_HPP__ */
GLuint GLenum matrix
CV_EXPORTS_W void sqrt(InputArray src, OutputArray dst)
computes square root of each matrix element (dst = src**0.5)
int rows
the number of rows and columns or (-1, -1) when the matrix has more than 2 dimensions ...
Definition: core.hpp:1962
Affine3 translate(const Vec3 &t) const
Definition: affine.hpp:405
Mat reshape(int cn, int rows=0) const
creates alternative matrix header for the same data, with different
Affine3< _Tp > value_type
Definition: affine.hpp:143
Affine3< double > Affine3d
Definition: affine.hpp:135
Definition: core.hpp:1013
int CvScalar value
Definition: core_c.h:340
A short numerical vector.
Definition: core.hpp:84
const GLfloat * tc
Definition: core.hpp:2499
Mat4 matrix
Definition: affine.hpp:118
void copyTo(OutputArray m) const
copies the matrix content to "m".
Definition: core.hpp:85
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: core_c.h:403
CvRect r
Definition: core_c.h:1282
const CvMat const CvMat const CvMat CvMat CvMat CvMat CvMat CvSize CvMat CvMat * T
Definition: calib3d.hpp:270
_Tp channel_type
Definition: affine.hpp:145
Vec< float_type, 3 > Vec3
Definition: affine.hpp:60
Affine3< float > Affine3f
Definition: affine.hpp:134
Affine3 concatenate(const Affine3 &affine) const
Definition: affine.hpp:415
const CvPoint2D32f double matrix[3][3]
Definition: legacy.hpp:1070
const CvArr CvArr int method
Definition: imgproc_c.h:281
ColorT< T > operator*(Y s, const ColorT< T > &c)
const CvArr const CvArr CvArr * result
Definition: core_c.h:805
Matx< _Tp, 1, n > row(int i) const
extract the matrix row
Definition: operations.hpp:427
int cols
Definition: core.hpp:1962
Vec3 translation() const
Definition: affine.hpp:314
int type() const
returns element type, similar to CV_MAT_TYPE(cvmat->type)
Definition: mat.hpp:399
Definition: core.hpp:1013
T float_type
Definition: affine.hpp:57
Definition: core.hpp:2499
Vec< double, 3 > Vec3d
Definition: core.hpp:684
static Vec all(_Tp alpha)
Definition: operations.hpp:1210
Mat3 rotation() const
Definition: affine.hpp:298
const CvMat const CvMat const CvMat CvMat CvMat CvMat CvMat CvSize CvMat * R
Definition: calib3d.hpp:270
Vec3 rvec() const
Definition: affine.hpp:320
Vec< channel_type, channels > vec_type
Definition: affine.hpp:154
const CvMat CvMat CvMat int k
Definition: legacy.hpp:3052
Affine3 inv(int method=cv::DECOMP_SVD) const
Definition: affine.hpp:369
CvPoint2D32f float float float c
Definition: legacy.hpp:578
Informative template class for OpenCV "scalars".
Definition: core.hpp:1006
void int double double theta
Definition: imgproc_c.h:603
void * data
Definition: core_c.h:206
const GLdouble * v
The Core Functionality.
The n-dimensional matrix class.
Definition: core.hpp:1688
static void compute(InputArray src, OutputArray w, OutputArray u, OutputArray vt, int flags=0)
decomposes matrix and stores the results to user-provided matrices
static Affine3 Identity()
Definition: affine.hpp:218
const CvArr const CvArr * V
Definition: core_c.h:733
GLsizei const GLfloat * value
Definition: core_c.h:341
void rotation(const Mat3 &R)
Definition: affine.hpp:224
_Tp val[m *n]
Definition: core.hpp:544
GLuint GLuint GLsizei GLenum type
Definition: core_c.h:114
const GLubyte * c
Definition: legacy.hpp:633
Definition: core.hpp:1013
GLboolean GLboolean GLboolean GLboolean a
Definition: legacy.hpp:633
Definition: affine.hpp:54
Matx< float_type, 4, 4 > Mat4
Definition: affine.hpp:59
Affine3()
Implementaiton.
Definition: affine.hpp:163
GLubyte GLubyte GLubyte GLubyte w
Affine3 rotate(const Mat3 &R) const
Definition: affine.hpp:375
const GLfloat * m
Matx< float_type, 3, 3 > Mat3
Definition: affine.hpp:58
int x
Definition: types_c.h:953
GLint GLint GLsizei GLsizei GLsizei depth
Definition: core_c.h:76
Mat3 linear() const
Definition: affine.hpp:304
Definition: core.hpp:131
GLdouble GLdouble t
GLdouble s
void translate(const Vec2f &pos)
CV_EXPORTS void rotate(const GpuMat &src, GpuMat &dst, Size dsize, double angle, double xShift=0, double yShift=0, int interpolation=INTER_LINEAR, Stream &stream=Stream::Null())
Affine3< typename DataType< _Tp >::work_type > work_type
Definition: affine.hpp:144
Rect_< int > Rect
Definition: core.hpp:897
void translation(const Vec3 &t)
Definition: affine.hpp:292
Matx< _Tp, n, m > t() const
transpose the matrix
Definition: operations.hpp:777
Affine3< Y > cast() const