affine.hpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                          License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
00014 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
00015 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00016 // Third party copyrights are property of their respective owners.
00017 //
00018 // Redistribution and use in source and binary forms, with or without modification,
00019 // are permitted provided that the following conditions are met:
00020 //
00021 //   * Redistribution's of source code must retain the above copyright notice,
00022 //     this list of conditions and the following disclaimer.
00023 //
00024 //   * Redistribution's in binary form must reproduce the above copyright notice,
00025 //     this list of conditions and the following disclaimer in the documentation
00026 //     and/or other materials provided with the distribution.
00027 //
00028 //   * The name of the copyright holders may not be used to endorse or promote products
00029 //     derived from this software without specific prior written permission.
00030 //
00031 // This software is provided by the copyright holders and contributors "as is" and
00032 // any express or implied warranties, including, but not limited to, the implied
00033 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00034 // In no event shall the Intel Corporation or contributors be liable for any direct,
00035 // indirect, incidental, special, exemplary, or consequential damages
00036 // (including, but not limited to, procurement of substitute goods or services;
00037 // loss of use, data, or profits; or business interruption) however caused
00038 // and on any theory of liability, whether in contract, strict liability,
00039 // or tort (including negligence or otherwise) arising in any way out of
00040 // the use of this software, even if advised of the possibility of such damage.
00041 //
00042 //M*/
00043 
00044 #ifndef __OPENCV_CORE_AFFINE3_HPP__
00045 #define __OPENCV_CORE_AFFINE3_HPP__
00046 
00047 #ifdef __cplusplus
00048 
00049 #include <opencv2/core.hpp>
00050 
00051 namespace cv
00052 {
00053 
00054 //! @addtogroup core
00055 //! @{
00056 
00057     /** @brief Affine transform
00058       @todo document
00059      */
00060     template<typename T>
00061     class Affine3
00062     {
00063     public:
00064         typedef T float_type;
00065         typedef Matx<float_type, 3, 3> Mat3;
00066         typedef Matx<float_type, 4, 4>  Mat4 ;
00067         typedef Vec<float_type, 3> Vec3;
00068 
00069         Affine3();
00070 
00071         //! Augmented affine matrix
00072         Affine3(const Mat4 & affine);
00073 
00074         //! Rotation matrix
00075         Affine3(const Mat3& R, const Vec3& t = Vec3::all(0));
00076 
00077         //! Rodrigues vector
00078         Affine3(const Vec3& rvec, const Vec3& t = Vec3::all(0));
00079 
00080         //! Combines all contructors above. Supports 4x4, 4x3, 3x3, 1x3, 3x1 sizes of data matrix
00081         explicit Affine3(const Mat& data, const Vec3& t = Vec3::all(0));
00082 
00083         //! From 16th element array
00084         explicit Affine3(const float_type* vals);
00085 
00086         //! Create identity transform
00087         static Affine3 Identity();
00088 
00089         //! Rotation matrix
00090         void rotation(const Mat3& R);
00091 
00092         //! Rodrigues vector
00093         void rotation(const Vec3& rvec);
00094 
00095         //! Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
00096         void rotation(const Mat& data);
00097 
00098         void linear(const Mat3& L);
00099         void translation(const Vec3& t);
00100 
00101         Mat3 rotation() const;
00102         Mat3 linear() const;
00103         Vec3 translation() const;
00104 
00105         //! Rodrigues vector
00106         Vec3 rvec() const;
00107 
00108         Affine3 inv(int method = cv::DECOMP_SVD) const;
00109 
00110         //! a.rotate(R) is equivalent to Affine(R, 0) * a;
00111         Affine3 rotate(const Mat3& R) const;
00112 
00113         //! a.rotate(rvec) is equivalent to Affine(rvec, 0) * a;
00114         Affine3 rotate(const Vec3& rvec) const;
00115 
00116         //! a.translate(t) is equivalent to Affine(E, t) * a;
00117         Affine3 translate(const Vec3& t) const;
00118 
00119         //! a.concatenate(affine) is equivalent to affine * a;
00120         Affine3 concatenate(const Affine3& affine) const;
00121 
00122         template <typename Y> operator Affine3<Y>() const;
00123 
00124         template <typename Y> Affine3<Y> cast() const;
00125 
00126         Mat4  matrix;
00127 
00128 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
00129         Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine);
00130         Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine);
00131         operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const;
00132         operator Eigen::Transform<T, 3, Eigen::Affine>() const;
00133 #endif
00134     };
00135 
00136     template<typename T> static
00137     Affine3<T> operator*(const Affine3<T>& affine1, const Affine3<T>& affine2);
00138 
00139     template<typename T, typename V> static
00140     V operator*(const Affine3<T>& affine, const V& vector);
00141 
00142     typedef Affine3<float> Affine3f;
00143     typedef Affine3<double> Affine3d;
00144 
00145     static Vec3f operator*(const Affine3f& affine, const Vec3f& vector);
00146     static Vec3d operator*(const Affine3d& affine, const Vec3d& vector);
00147 
00148     template<typename _Tp> class DataType< Affine3<_Tp> >
00149     {
00150     public:
00151         typedef Affine3<_Tp>                               value_type;
00152         typedef Affine3<typename DataType<_Tp>::work_type> work_type;
00153         typedef _Tp                                        channel_type;
00154 
00155         enum { generic_type = 0,
00156                depth        = DataType<channel_type>::depth,
00157                channels     = 16,
00158                fmt          = DataType<channel_type>::fmt + ((channels - 1) << 8),
00159                type         = CV_MAKETYPE(depth, channels)
00160              };
00161 
00162         typedef Vec<channel_type, channels> vec_type;
00163     };
00164 
00165 //! @} core
00166 
00167 }
00168 
00169 //! @cond IGNORED
00170 
00171 ///////////////////////////////////////////////////////////////////////////////////
00172 // Implementaiton
00173 
00174 template<typename T> inline
00175 cv::Affine3<T>::Affine3()
00176     : matrix(Mat4::eye())
00177 {}
00178 
00179 template<typename T> inline
00180 cv::Affine3<T>::Affine3(const Mat4& affine)
00181     : matrix(affine)
00182 {}
00183 
00184 template<typename T> inline
00185 cv::Affine3<T>::Affine3(const Mat3& R, const Vec3& t)
00186 {
00187     rotation(R);
00188     translation(t);
00189     matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
00190     matrix.val[15] = 1;
00191 }
00192 
00193 template<typename T> inline
00194 cv::Affine3<T>::Affine3(const Vec3& _rvec, const Vec3& t)
00195 {
00196     rotation(_rvec);
00197     translation(t);
00198     matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
00199     matrix.val[15] = 1;
00200 }
00201 
00202 template<typename T> inline
00203 cv::Affine3<T>::Affine3(const cv::Mat& data, const Vec3& t)
00204 {
00205     CV_Assert(data.type() == cv::DataType<T>::type);
00206 
00207     if (data.cols == 4 && data.rows == 4)
00208     {
00209         data.copyTo(matrix);
00210         return;
00211     }
00212     else if (data.cols == 4 && data.rows == 3)
00213     {
00214         rotation(data(Rect(0, 0, 3, 3)));
00215         translation(data(Rect(3, 0, 1, 3)));
00216         return;
00217     }
00218 
00219     rotation(data);
00220     translation(t);
00221     matrix.val[12] = matrix.val[13] = matrix.val[14] = 0;
00222     matrix.val[15] = 1;
00223 }
00224 
00225 template<typename T> inline
00226 cv::Affine3<T>::Affine3(const float_type* vals) : matrix(vals)
00227 {}
00228 
00229 template<typename T> inline
00230 cv::Affine3<T> cv::Affine3<T>::Identity()
00231 {
00232     return Affine3<T>(cv::Affine3<T>::Mat4::eye());
00233 }
00234 
00235 template<typename T> inline
00236 void cv::Affine3<T>::rotation(const Mat3& R)
00237 {
00238     linear(R);
00239 }
00240 
00241 template<typename T> inline
00242 void cv::Affine3<T>::rotation(const Vec3& _rvec)
00243 {
00244     double rx = _rvec[0], ry = _rvec[1], rz = _rvec[2];
00245     double theta = std::sqrt(rx*rx + ry*ry + rz*rz);
00246 
00247     if (theta < DBL_EPSILON)
00248         rotation(Mat3::eye());
00249     else
00250     {
00251         const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
00252 
00253         double c = std::cos(theta);
00254         double s = std::sin(theta);
00255         double c1 = 1. - c;
00256         double itheta = (theta != 0) ? 1./theta : 0.;
00257 
00258         rx *= itheta; ry *= itheta; rz *= itheta;
00259 
00260         double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz };
00261         double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 };
00262         Mat3 R;
00263 
00264         // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
00265         // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0]
00266         for(int k = 0; k < 9; ++k)
00267             R.val[k] = static_cast<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
00268 
00269         rotation(R);
00270     }
00271 }
00272 
00273 //Combines rotation methods above. Suports 3x3, 1x3, 3x1 sizes of data matrix;
00274 template<typename T> inline
00275 void cv::Affine3<T>::rotation(const cv::Mat& data)
00276 {
00277     CV_Assert(data.type() == cv::DataType<T>::type);
00278 
00279     if (data.cols == 3 && data.rows == 3)
00280     {
00281         Mat3 R;
00282         data.copyTo(R);
00283         rotation(R);
00284     }
00285     else if ((data.cols == 3 && data.rows == 1) || (data.cols == 1 && data.rows == 3))
00286     {
00287         Vec3 _rvec;
00288         data.reshape(1, 3).copyTo(_rvec);
00289         rotation(_rvec);
00290     }
00291     else
00292         CV_Assert(!"Input marix can be 3x3, 1x3 or 3x1");
00293 }
00294 
00295 template<typename T> inline
00296 void cv::Affine3<T>::linear(const Mat3& L)
00297 {
00298     matrix.val[0] = L.val[0]; matrix.val[1] = L.val[1];  matrix.val[ 2] = L.val[2];
00299     matrix.val[4] = L.val[3]; matrix.val[5] = L.val[4];  matrix.val[ 6] = L.val[5];
00300     matrix.val[8] = L.val[6]; matrix.val[9] = L.val[7];  matrix.val[10] = L.val[8];
00301 }
00302 
00303 template<typename T> inline
00304 void cv::Affine3<T>::translation(const Vec3& t)
00305 {
00306     matrix.val[3] = t[0]; matrix.val[7] = t[1]; matrix.val[11] = t[2];
00307 }
00308 
00309 template<typename T> inline
00310 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::rotation() const
00311 {
00312     return linear();
00313 }
00314 
00315 template<typename T> inline
00316 typename cv::Affine3<T>::Mat3 cv::Affine3<T>::linear() const
00317 {
00318     typename cv::Affine3<T>::Mat3 R;
00319     R.val[0] = matrix.val[0];  R.val[1] = matrix.val[1];  R.val[2] = matrix.val[ 2];
00320     R.val[3] = matrix.val[4];  R.val[4] = matrix.val[5];  R.val[5] = matrix.val[ 6];
00321     R.val[6] = matrix.val[8];  R.val[7] = matrix.val[9];  R.val[8] = matrix.val[10];
00322     return R;
00323 }
00324 
00325 template<typename T> inline
00326 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::translation() const
00327 {
00328     return Vec3(matrix.val[3], matrix.val[7], matrix.val[11]);
00329 }
00330 
00331 template<typename T> inline
00332 typename cv::Affine3<T>::Vec3 cv::Affine3<T>::rvec() const
00333 {
00334     cv::Vec3d w;
00335     cv::Matx33d u, vt, R = rotation();
00336     cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
00337     R = u * vt;
00338 
00339     double rx = R.val[7] - R.val[5];
00340     double ry = R.val[2] - R.val[6];
00341     double rz = R.val[3] - R.val[1];
00342 
00343     double s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25);
00344     double c = (R.val[0] + R.val[4] + R.val[8] - 1) * 0.5;
00345     c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c;
00346     double theta = acos(c);
00347 
00348     if( s < 1e-5 )
00349     {
00350         if( c > 0 )
00351             rx = ry = rz = 0;
00352         else
00353         {
00354             double t;
00355             t = (R.val[0] + 1) * 0.5;
00356             rx = std::sqrt(std::max(t, 0.0));
00357             t = (R.val[4] + 1) * 0.5;
00358             ry = std::sqrt(std::max(t, 0.0)) * (R.val[1] < 0 ? -1.0 : 1.0);
00359             t = (R.val[8] + 1) * 0.5;
00360             rz = std::sqrt(std::max(t, 0.0)) * (R.val[2] < 0 ? -1.0 : 1.0);
00361 
00362             if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R.val[5] > 0) != (ry*rz > 0) )
00363                 rz = -rz;
00364             theta /= std::sqrt(rx*rx + ry*ry + rz*rz);
00365             rx *= theta;
00366             ry *= theta;
00367             rz *= theta;
00368         }
00369     }
00370     else
00371     {
00372         double vth = 1/(2*s);
00373         vth *= theta;
00374         rx *= vth; ry *= vth; rz *= vth;
00375     }
00376 
00377     return cv::Vec3d(rx, ry, rz);
00378 }
00379 
00380 template<typename T> inline
00381 cv::Affine3<T> cv::Affine3<T>::inv(int method) const
00382 {
00383     return matrix.inv(method);
00384 }
00385 
00386 template<typename T> inline
00387 cv::Affine3<T> cv::Affine3<T>::rotate(const Mat3& R) const
00388 {
00389     Mat3 Lc = linear();
00390     Vec3 tc = translation();
00391     Mat4 result;
00392     result.val[12] = result.val[13] = result.val[14] = 0;
00393     result.val[15] = 1;
00394 
00395     for(int j = 0; j < 3; ++j)
00396     {
00397         for(int i = 0; i < 3; ++i)
00398         {
00399             float_type value = 0;
00400             for(int k = 0; k < 3; ++k)
00401                 value += R(j, k) * Lc(k, i);
00402             result(j, i) = value;
00403         }
00404 
00405         result(j, 3) = R.row(j).dot(tc.t());
00406     }
00407     return result;
00408 }
00409 
00410 template<typename T> inline
00411 cv::Affine3<T> cv::Affine3<T>::rotate(const Vec3& _rvec) const
00412 {
00413     return rotate(Affine3f(_rvec).rotation());
00414 }
00415 
00416 template<typename T> inline
00417 cv::Affine3<T> cv::Affine3<T>::translate(const Vec3& t) const
00418 {
00419     Mat4 m = matrix;
00420     m.val[ 3] += t[0];
00421     m.val[ 7] += t[1];
00422     m.val[11] += t[2];
00423     return m;
00424 }
00425 
00426 template<typename T> inline
00427 cv::Affine3<T> cv::Affine3<T>::concatenate(const Affine3<T>& affine) const
00428 {
00429     return (*this).rotate(affine.rotation()).translate(affine.translation());
00430 }
00431 
00432 template<typename T> template <typename Y> inline
00433 cv::Affine3<T>::operator Affine3<Y>() const
00434 {
00435     return Affine3<Y>(matrix);
00436 }
00437 
00438 template<typename T> template <typename Y> inline
00439 cv::Affine3<Y> cv::Affine3<T>::cast() const
00440 {
00441     return Affine3<Y>(matrix);
00442 }
00443 
00444 template<typename T> inline
00445 cv::Affine3<T> cv::operator*(const cv::Affine3<T>& affine1, const cv::Affine3<T>& affine2)
00446 {
00447     return affine2.concatenate(affine1);
00448 }
00449 
00450 template<typename T, typename V> inline
00451 V cv::operator*(const cv::Affine3<T>& affine, const V& v)
00452 {
00453     const typename Affine3<T>::Mat4& m = affine.matrix;
00454 
00455     V r;
00456     r.x = m.val[0] * v.x + m.val[1] * v.y + m.val[ 2] * v.z + m.val[ 3];
00457     r.y = m.val[4] * v.x + m.val[5] * v.y + m.val[ 6] * v.z + m.val[ 7];
00458     r.z = m.val[8] * v.x + m.val[9] * v.y + m.val[10] * v.z + m.val[11];
00459     return r;
00460 }
00461 
00462 static inline
00463 cv::Vec3f cv::operator*(const cv::Affine3f& affine, const cv::Vec3f& v)
00464 {
00465     const cv::Matx44f& m = affine.matrix;
00466     cv::Vec3f r;
00467     r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
00468     r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
00469     r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
00470     return r;
00471 }
00472 
00473 static inline
00474 cv::Vec3d cv::operator*(const cv::Affine3d& affine, const cv::Vec3d& v)
00475 {
00476     const cv::Matx44d& m = affine.matrix;
00477     cv::Vec3d r;
00478     r.val[0] = m.val[0] * v[0] + m.val[1] * v[1] + m.val[ 2] * v[2] + m.val[ 3];
00479     r.val[1] = m.val[4] * v[0] + m.val[5] * v[1] + m.val[ 6] * v[2] + m.val[ 7];
00480     r.val[2] = m.val[8] * v[0] + m.val[9] * v[1] + m.val[10] * v[2] + m.val[11];
00481     return r;
00482 }
00483 
00484 
00485 
00486 #if defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H
00487 
00488 template<typename T> inline
00489 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>& affine)
00490 {
00491     cv::Mat(4, 4, cv::DataType<T>::type, affine.matrix().data()).copyTo(matrix);
00492 }
00493 
00494 template<typename T> inline
00495 cv::Affine3<T>::Affine3(const Eigen::Transform<T, 3, Eigen::Affine>& affine)
00496 {
00497     Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> a = affine;
00498     cv::Mat(4, 4, cv::DataType<T>::type, a.matrix().data()).copyTo(matrix);
00499 }
00500 
00501 template<typename T> inline
00502 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>() const
00503 {
00504     Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)> r;
00505     cv::Mat hdr(4, 4, cv::DataType<T>::type, r.matrix().data());
00506     cv::Mat(matrix, false).copyTo(hdr);
00507     return r;
00508 }
00509 
00510 template<typename T> inline
00511 cv::Affine3<T>::operator Eigen::Transform<T, 3, Eigen::Affine>() const
00512 {
00513     return this->operator Eigen::Transform<T, 3, Eigen::Affine, (Eigen::RowMajor)>();
00514 }
00515 
00516 #endif /* defined EIGEN_WORLD_VERSION && defined EIGEN_GEOMETRY_MODULE_H */
00517 
00518 //! @endcond
00519 
00520 #endif /* __cplusplus */
00521 
00522 #endif /* __OPENCV_CORE_AFFINE3_HPP__ */
00523