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
54 //! @addtogroup core
55 //! @{
56
57 /** @brief Affine transform
58 @todo document
59 */
60 template<typename T>
61 class Affine3
62 {
63 public:
64 typedef T float_type;
65 typedef Matx<float_type, 3, 3> Mat3;
66 typedef Matx<float_type, 4, 4> Mat4;
67 typedef Vec<float_type, 3> Vec3;
68
69 Affine3();
70
71 //! Augmented affine matrix
72 Affine3(const Mat4& affine);
73
74 //! Rotation matrix
75 Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
76
77 //! Rodrigues vector
78 Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
79
80 //! Combines all contructors above. Supports 4x4, 4x3, 3x3, 1x3, 3x1 sizes of data matrix
81 explicit Affine3(const Mat& data, const Vec3& t = Vec3::all(0));
82
83 //! From 16th element array
84 explicit Affine3(const float_type* vals);
85
86 //! Create identity transform
87 static Affine3 Identity();
88
89 //! Rotation matrix
90 void rotation(const Mat3& R);
91
92 //! Rodrigues vector
93 void rotation(const Vec3& rvec);
94
95 //! Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
96 void rotation(const Mat& data);
97
98 void linear(const Mat3& L);
99 void translation(const Vec3& t);
100
101 Mat3 rotation() const;
102 Mat3 linear() const;
103 Vec3 translation() const;
104
105 //! Rodrigues vector
106 Vec3 rvec() const;
107
108 Affine3 inv(int method = cv::DECOMP_SVD) const;
109
110 //! a.rotate(R) is equivalent to Affine(R, 0) * a;
111 Affine3 rotate(const Mat3& R) const;
112
113 //! a.rotate(R) is equivalent to Affine(rvec, 0) * a;
114 Affine3 rotate(const Vec3& rvec) const;
115
116 //! a.translate(t) is equivalent to Affine(E, t) * a;
117 Affine3 translate(const Vec3& t) const;
118
119 //! a.concatenate(affine) is equivalent to affine * a;
120 Affine3 concatenate(const Affine3& affine) const;
121
122 template <typename Y> operator Affine3<Y>() const;
123
124 template <typename Y> Affine3<Y> cast() const;
125
126 Mat4 matrix;
127
128 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
129 Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
130 Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
131 operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
132 operator Eigen::Transform<T, 3, Eigen::Affine>() const;
133 #endif
134 };
135
136 template<typename T> static
137 Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
138
139 template<typename T, typename V> static
140 V operator*(const Affine3<T>& affine, const V& vector);
141
142 typedef Affine3<float> Affine3f;
143 typedef Affine3<double> Affine3d;
144
145 static Vec3f operator*(const Affine3f& affine, const Vec3f& vector);
146 static Vec3d operator*(const Affine3d& affine, const Vec3d& vector);
147
148 template<typename _Tp> class DataType< Affine3<_Tp> >
149 {
150 public:
151 typedef Affine3<_Tp> value_type;
152 typedef Affine3<typename DataType<_Tp>::work_type> work_type;
153 typedef _Tp channel_type;
154
155 enum { generic_type = 0,
156 depth = DataType<channel_type>::depth,
157 channels = 16,
158 fmt = DataType<channel_type>::fmt + ((channels - 1) << 8),
159 type = CV_MAKETYPE(depth, channels)
160 };
161
162 typedef Vec<channel_type, channels> vec_type;
163 };
164
165 //! @} core
166
167 }
168
169 //! @cond IGNORED
170
171 ///////////////////////////////////////////////////////////////////////////////////
172 // Implementaiton
173
174 template<typename T> inline
Affine3()175 cv::Affine3<T>::Affine3()
176 : matrix(Mat4::eye())
177 {}
178
179 template<typename T> inline
Affine3(const Mat4 & affine)180 cv::Affine3<T>::Affine3(const Mat4& affine)
181 : matrix(affine)
182 {}
183
184 template<typename T> inline
Affine3(const Mat3 & R,const Vec3 & t)185 cv::Affine3<T>::Affine3(const Mat3& R, const Vec3& t)
186 {
187 rotation(R);
188 translation(t);
189 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
190 matrix.val[15] = 1;
191 }
192
193 template<typename T> inline
Affine3(const Vec3 & _rvec,const Vec3 & t)194 cv::Affine3<T>::Affine3(const Vec3& _rvec, const Vec3& t)
195 {
196 rotation(_rvec);
197 translation(t);
198 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
199 matrix.val[15] = 1;
200 }
201
202 template<typename T> inline
Affine3(const cv::Mat & data,const Vec3 & t)203 cv::Affine3<T>::Affine3(const cv::Mat& data, const Vec3& t)
204 {
205 CV_Assert(data.type() == cv::DataType<T>::type);
206
207 if (data.cols == 4 && data.rows == 4)
208 {
209 data.copyTo(matrix);
210 return;
211 }
212 else if (data.cols == 4 && data.rows == 3)
213 {
214 rotation(data(Rect(0, 0, 3, 3)));
215 translation(data(Rect(3, 0, 1, 3)));
216 return;
217 }
218
219 rotation(data);
220 translation(t);
221 matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
222 matrix.val[15] = 1;
223 }
224
225 template<typename T> inline
Affine3(const float_type * vals)226 cv::Affine3<T>::Affine3(const float_type* vals) : matrix(vals)
227 {}
228
229 template<typename T> inline
Identity()230 cv::Affine3<T> cv::Affine3<T>::Identity()
231 {
232 return Affine3<T>(cv::Affine3<T>::Mat4::eye());
233 }
234
235 template<typename T> inline
rotation(const Mat3 & R)236 void cv::Affine3<T>::rotation(const Mat3& R)
237 {
238 linear(R);
239 }
240
241 template<typename T> inline
rotation(const Vec3 & _rvec)242 void cv::Affine3<T>::rotation(const Vec3& _rvec)
243 {
244 double rx = _rvec[0], ry = _rvec[1], rz = _rvec[2];
245 double theta = std::sqrt(rx*rx + ry*ry + rz*rz);
246
247 if (theta < DBL_EPSILON)
248 rotation(Mat3::eye());
249 else
250 {
251 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
252
253 double c = std::cos(theta);
254 double s = std::sin(theta);
255 double c1 = 1. - c;
256 double itheta = (theta != 0) ? 1./theta : 0.;
257
258 rx *= itheta; ry *= itheta; rz *= itheta;
259
260 double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
261 double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
262 Mat3 R;
263
264 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
265 // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
266 for(int k = 0; k < 9; ++k)
267 R.val[k] = static_cast<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
268
269 rotation(R);
270 }
271 }
272
273 //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
274 template<typename T> inline
rotation(const cv::Mat & data)275 void cv::Affine3<T>::rotation(const cv::Mat& data)
276 {
277 CV_Assert(data.type() == cv::DataType<T>::type);
278
279 if (data.cols == 3 && data.rows == 3)
280 {
281 Mat3 R;
282 data.copyTo(R);
283 rotation(R);
284 }
285 else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
286 {
287 Vec3 _rvec;
288 data.reshape(1, 3).copyTo(_rvec);
289 rotation(_rvec);
290 }
291 else
292 CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1");
293 }
294
295 template<typename T> inline
linear(const Mat3 & L)296 void cv::Affine3<T>::linear(const Mat3& L)
297 {
298 matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1]; matrix.val[ 2] = L.val[2];
299 matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4]; matrix.val[ 6] = L.val[5];
300 matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7]; matrix.val[10] = L.val[8];
301 }
302
303 template<typename T> inline
translation(const Vec3 & t)304 void cv::Affine3<T>::translation(const Vec3& t)
305 {
306 matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
307 }
308
309 template<typename T> inline
rotation() const310 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::rotation() const
311 {
312 return linear();
313 }
314
315 template<typename T> inline
linear() const316 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::linear() const
317 {
318 typename cv::Affine3<T>::Mat3 R;
319 R.val[0] = matrix.val[0]; R.val[1] = matrix.val[1]; R.val[2] = matrix.val[ 2];
320 R.val[3] = matrix.val[4]; R.val[4] = matrix.val[5]; R.val[5] = matrix.val[ 6];
321 R.val[6] = matrix.val[8]; R.val[7] = matrix.val[9]; R.val[8] = matrix.val[10];
322 return R;
323 }
324
325 template<typename T> inline
translation() const326 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::translation() const
327 {
328 return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
329 }
330
331 template<typename T> inline
rvec() const332 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::rvec() const
333 {
334 cv::Vec3d w;
335 cv::Matx33d u, vt, R = rotation();
336 cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
337 R = u * vt;
338
339 double rx = R.val[7] - R.val[5];
340 double ry = R.val[2] - R.val[6];
341 double rz = R.val[3] - R.val[1];
342
343 double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25);
344 double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5;
345 c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c;
346 double theta = acos(c);
347
348 if( s < 1e-5 )
349 {
350 if( c > 0 )
351 rx = ry = rz = 0;
352 else
353 {
354 double t;
355 t = (R.val[0] + 1) * 0.5;
356 rx = std::sqrt(std::max(t, 0.0));
357 t = (R.val[4] + 1) * 0.5;
358 ry = std::sqrt(std::max(t, 0.0)) * (R.val[1] < 0 ? -1.0 : 1.0);
359 t = (R.val[8] + 1) * 0.5;
360 rz = std::sqrt(std::max(t, 0.0)) * (R.val[2] < 0 ? -1.0 : 1.0);
361
362 if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R.val[5] > 0) != (ry*rz > 0) )
363 rz = -rz;
364 theta /= std::sqrt(rx*rx + ry*ry + rz*rz);
365 rx *= theta;
366 ry *= theta;
367 rz *= theta;
368 }
369 }
370 else
371 {
372 double vth = 1/(2*s);
373 vth *= theta;
374 rx *= vth; ry *= vth; rz *= vth;
375 }
376
377 return cv::Vec3d(rx, ry, rz);
378 }
379
380 template<typename T> inline
inv(int method) const381 cv::Affine3<T> cv::Affine3<T>::inv(int method) const
382 {
383 return matrix.inv(method);
384 }
385
386 template<typename T> inline
rotate(const Mat3 & R) const387 cv::Affine3<T> cv::Affine3<T>::rotate(const Mat3& R) const
388 {
389 Mat3 Lc = linear();
390 Vec3 tc = translation();
391 Mat4 result;
392 result.val[12] = result.val[13] = result.val[14] = 0;
393 result.val[15] = 1;
394
395 for(int j = 0; j < 3; ++j)
396 {
397 for(int i = 0; i < 3; ++i)
398 {
399 float_type value = 0;
400 for(int k = 0; k < 3; ++k)
401 value += R(j, k) * Lc(k, i);
402 result(j, i) = value;
403 }
404
405 result(j, 3) = R.row(j).dot(tc.t());
406 }
407 return result;
408 }
409
410 template<typename T> inline
rotate(const Vec3 & _rvec) const411 cv::Affine3<T> cv::Affine3<T>::rotate(const Vec3& _rvec) const
412 {
413 return rotate(Affine3f(_rvec).rotation());
414 }
415
416 template<typename T> inline
translate(const Vec3 & t) const417 cv::Affine3<T> cv::Affine3<T>::translate(const Vec3& t) const
418 {
419 Mat4 m = matrix;
420 m.val[ 3] += t[0];
421 m.val[ 7] += t[1];
422 m.val[11] += t[2];
423 return m;
424 }
425
426 template<typename T> inline
concatenate(const Affine3<T> & affine) const427 cv::Affine3<T> cv::Affine3<T>::concatenate(const Affine3<T>& affine) const
428 {
429 return (*this).rotate(affine.rotation()).translate(affine.translation());
430 }
431
432 template<typename T> template <typename Y> inline
operator Affine3<Y>() const433 cv::Affine3<T>::operator Affine3<Y>() const
434 {
435 return Affine3<Y>(matrix);
436 }
437
438 template<typename T> template <typename Y> inline
cast() const439 cv::Affine3<Y> cv::Affine3<T>::cast() const
440 {
441 return Affine3<Y>(matrix);
442 }
443
444 template<typename T> inline
operator *(const cv::Affine3<T> & affine1,const cv::Affine3<T> & affine2)445 cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
446 {
447 return affine2.concatenate(affine1);
448 }
449
450 template<typename T, typename V> inline
operator *(const cv::Affine3<T> & affine,const V & v)451 V cv::operator*(const cv::Affine3<T>& affine, const V& v)
452 {
453 const typename Affine3<T>::Mat4& m = affine.matrix;
454
455 V r;
456 r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
457 r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
458 r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
459 return r;
460 }
461
462 static inline
operator *(const cv::Affine3f & affine,const cv::Vec3f & v)463 cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
464 {
465 const cv::Matx44f& m = affine.matrix;
466 cv::Vec3f r;
467 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
468 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
469 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
470 return r;
471 }
472
473 static inline
operator *(const cv::Affine3d & affine,const cv::Vec3d & v)474 cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
475 {
476 const cv::Matx44d& m = affine.matrix;
477 cv::Vec3d r;
478 r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
479 r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
480 r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
481 return r;
482 }
483
484
485
486 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
487
488 template<typename T> inline
Affine3(const Eigen::Transform<T,3,Eigen::Affine,(Eigen::RowMajor)> & affine)489 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
490 {
491 cv::Mat(4, 4, cv::DataType<T>::type, affine.matrix().data()).copyTo(matrix);
492 }
493
494 template<typename T> inline
Affine3(const Eigen::Transform<T,3,Eigen::Affine> & affine)495 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
496 {
497 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
498 cv::Mat(4, 4, cv::DataType<T>::type, a.matrix().data()).copyTo(matrix);
499 }
500
501 template<typename T> inline
operator Eigen::Transform<T,3,Eigen::Affine,(Eigen::RowMajor)>() const502 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
503 {
504 Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
505 cv::Mat hdr(4, 4, cv::DataType<T>::type, r.matrix().data());
506 cv::Mat(matrix, false).copyTo(hdr);
507 return r;
508 }
509
510 template<typename T> inline
operator Eigen::Transform<T,3,Eigen::Affine>() const511 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
512 {
513 return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
514 }
515
516 #endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
517
518 //! @endcond
519
520 #endif /* __cplusplus */
521
522 #endif /* __OPENCV_CORE_AFFINE3_HPP__ */
523